Send and receive announcement, rewrite a lot of code

ffvisd/ffvisd.c

@@ -26,8 +26,10 @@
 
 #define _GNU_SOURCE
 
+#include <alloca.h>
 #include <errno.h>
 #include <libgen.h>
+#include <poll.h>
 #include <stdarg.h>
 #include <stdbool.h>
 #include <stdio.h>
@@ -47,12 +49,15 @@
 
 #include <netinet/in.h>
 
+#include <netpacket/packet.h>
+
 #include <sys/types.h>
 #include <sys/stat.h>
 
 
-#define ANNOUNCE_INTERVAL 10000
+#define ANNOUNCE_INTERVAL 10
-#define ANNOUNCE_TIMEOUT 30000
+#define ANNOUNCE_TIMEOUT (3*ANNOUNCE_INTERVAL)
+#define ANNOUNCE_PRUNE (3*ANNOUNCE_TIMEOUT)
 
 #define SYSFS_PATH_MAX 256
 #define SYSFS_CLASS_NET "/sys/class/net"
@@ -60,31 +65,29 @@
 #define FFD_MAGIC 0xffd
 #define FFVISD_VERSION 0
 
+#define FFD_PROTO 0xffd
 #define FFD_PORT 0xffd
 
 
-static const struct in6_addr ffd_addr = { .s6_addr = {0xff, 0x12, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+typedef struct __attribute__((__packed__)) _eth_addr_t {
-						      0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x0f, 0xfd} };
-
-
-typedef struct __attribute__((__packed__)) _ffvisd_eth_address_t {
 	uint8_t address[ETH_ALEN];
-} ffvisd_eth_address_t;
+} eth_addr_t;
 
-typedef struct __attribute__((__packed__)) _ffvisd_ipv6_host_address_t {
+typedef struct __attribute__((__packed__)) _ffvisd_ipv6_host_addr_t {
 	uint8_t address[8];
-} ffvisd_ipv6_host_address_t;
+} ffvisd_ipv6_host_addr_t;
 
 typedef struct __attribute__((__packed__)) _ffvisd_packet_server_announce_t {
-	uint32_t seqno;
+	uint16_t seqno;
-	ffvisd_eth_address_t eth_address;
+	eth_addr_t eth_address;
-	ffvisd_ipv6_host_address_t address;
+	ffvisd_ipv6_host_addr_t address;
 } ffvisd_packet_server_announce_t;
 
 typedef struct __attribute__((__packed__)) _ffvisd_packet_announce_t {
 	uint16_t magic;
 	uint16_t version;
-	uint32_t n_servers;
+	uint16_t csum;
+	uint16_t n_servers;
 	ffvisd_packet_server_announce_t servers[];
 } ffvisd_packet_announce_t;
 
@@ -98,20 +101,6 @@ typedef enum _ffvisd_iftype_t {
 	IF_MAX
 } ffvisd_iftype_t;
 
-typedef struct _ffvisd_iface_t {
-	unsigned ifindex;
-	ffvisd_iftype_t iftype;
-	char *ifname;
-	char *bridge_iface;
-	char *mesh_iface;
-} ffvisd_iface_t;
-
-typedef struct _ffvisd_orig_t {
-	struct _ffvisd_orig_t *next;
-
-	ffvisd_eth_address_t eth_address;
-} ffvisd_orig_t;
-
 typedef struct _ffvisd_announce_t {
 	struct _ffvisd_announce_t *next;
 
@@ -119,17 +108,35 @@ typedef struct _ffvisd_announce_t {
 	ffvisd_packet_server_announce_t announce;
 } ffvisd_announce_t;
 
-static const char *ffvisd_iftype_names[IF_MAX] = {
+/*static const char *ffvisd_iftype_names[IF_MAX] = {
 	[IF_UNKNOWN] = "unknown",
 	[IF_WIRED] = "wired",
 	[IF_WIRELESS] = "wireless",
 	[IF_VIRTUAL] = "virtual",
 	[IF_BRIDGE] = "bridge",
 	[IF_MESH] = "mesh",
-};
+	};*/
 
 
-typedef void (*iface_cb)(const ffvisd_iface_t *iface, void *arg);
+typedef void (*iface_cb)(const char *ifname, unsigned ifindex, void *arg);
+
+
+static const eth_addr_t eth_addr_unspec = {{0x00, 0x00, 0x00, 0x00, 0x00, 0x00}};
+
+static const eth_addr_t ffd_addr = {{0x03, 0x00, 0x00, 0x00, 0x0f, 0xfd}};
+
+
+static inline bool is_eth_addr_unspec(const eth_addr_t *address) {
+	const uint8_t *a = address->address;
+
+	if (a[0]||a[1]||a[2]||a[3]||a[4]||a[5])
+		return false;
+	else
+		return true;
+}
+
+
+#define ETH_ADDR_IS_UNSPEC(a) (!((a).address[0]||(a).address[0]||(a).address[0]||(a).address[0]||(a).address[0]||(a).address[0]))
 
 
 static int sockfd;
@@ -139,10 +146,6 @@ static char *mesh = "bat0";
 ffvisd_announce_t *announcements = NULL;
 
 
-static void ffvisd_iface_free(ffvisd_iface_t *iface);
-
-
-
 /* returns (tp1 - tp2) in milliseconds  */
 static inline int timespec_diff(const struct timespec *tp1, const struct timespec *tp2) {
 	return ((tp1->tv_sec - tp2->tv_sec))*1000 + (tp1->tv_nsec - tp2->tv_nsec)/1e6;
@@ -187,6 +190,17 @@ static bool iface_is_mesh(const char *ifname) {
 	return iface_file_exists(ifname, "mesh");
 }
 
+static char* iface_get_mesh(const char *ifname) {
+	char *mesh = NULL;
+
+	if (!iface_file_read(ifname, "batman_adv/mesh_iface", "%as", &mesh) || !strcmp(mesh, "none")) {
+		free(mesh);
+		return NULL;
+	}
+
+	return mesh;
+}
+
 static char* iface_get_bridge(const char *ifname) {
 	char filename[SYSFS_PATH_MAX], filename2[SYSFS_PATH_MAX] = {0};
 
@@ -197,7 +211,17 @@ static char* iface_get_bridge(const char *ifname) {
 	return strdup(basename(filename2));
 }
 
-static struct in6_addr* iface_get_address(ffvisd_iface_t *iface, int scope) {
+static eth_addr_t iface_get_eth_address(const char *ifname) {
+	eth_addr_t ret;
+	uint8_t *a = ret.address;
+
+	if (!iface_file_read(ifname, "address", "%x:%x:%x:%x:%x:%x", &a[0], &a[1], &a[2], &a[3], &a[4], &a[5]))
+		return eth_addr_unspec;
+
+	return ret;
+}
+
+static struct in6_addr iface_get_address(unsigned ifindex, int scope) {
 	struct msg {
 		struct nlmsghdr nh;
 		struct ifaddrmsg addr;
@@ -217,7 +241,7 @@ static struct in6_addr* iface_get_address(ffvisd_iface_t *iface, int scope) {
 	if(write(rtnetlink_sk, &msg, msg.nh.nlmsg_len) <= 0) {
 		fprintf(stderr, "error: write: %m\n");
 		close(rtnetlink_sk);
-		return NULL;
+		return in6addr_any;
 	}
 
 	int readlen = read(rtnetlink_sk, &msg, sizeof(msg));
@@ -225,7 +249,7 @@ static struct in6_addr* iface_get_address(ffvisd_iface_t *iface, int scope) {
 	if (readlen <= 0) {
 		fprintf(stderr, "error: read: %m\n");
 		close(rtnetlink_sk);
-		return NULL;
+		return in6addr_any;
 	}
 
 	close(rtnetlink_sk);
@@ -236,19 +260,19 @@ static struct in6_addr* iface_get_address(ffvisd_iface_t *iface, int scope) {
 		int len = chunk->nh.nlmsg_len - sizeof(struct nlmsghdr);
 
 		if (len < sizeof(struct ifaddrmsg) || readlen < len)
-			return NULL;
+			return in6addr_any;
 
 		if (!NLMSG_OK(&chunk->nh, readlen))
-			return NULL;
+			return in6addr_any;
 
-		if (chunk->nh.nlmsg_type == RTM_NEWADDR && chunk->addr.ifa_scope == scope && chunk->addr.ifa_index == iface->ifindex) {
+		if (chunk->nh.nlmsg_type == RTM_NEWADDR && chunk->addr.ifa_scope == scope && chunk->addr.ifa_index == ifindex) {
 			struct rtattr *rta = (struct rtattr *)IFA_RTA(&chunk->addr);
 			int rtattrlen = IFA_PAYLOAD(&chunk->nh);
 
 			for (; RTA_OK(rta, rtattrlen); rta = RTA_NEXT(rta, rtattrlen)) {
 				if(rta->rta_type == IFA_ADDRESS && rta->rta_len == RTA_LENGTH(16)) {
-					struct in6_addr *ret = malloc(sizeof(struct in6_addr));
+					struct in6_addr ret;
-					memcpy(ret->s6_addr, RTA_DATA(rta), 16);
+					memcpy(ret.s6_addr, RTA_DATA(rta), 16);
 					return ret;
 				}
 			}
@@ -258,49 +282,7 @@ static struct in6_addr* iface_get_address(ffvisd_iface_t *iface, int scope) {
 		chunk = (struct msg*)((uint8_t*)chunk + NLMSG_ALIGN(chunk->nh.nlmsg_len));
 	}
 
-	return NULL;
+	return in6addr_any;
-}
-
-static ffvisd_iface_t* ffvisd_iface_new(const char *ifname, unsigned ifindex) {
-	if (!ifname || !ifindex)
-		return false;
-
-	ffvisd_iface_t *iface = calloc(1, sizeof(ffvisd_iface_t));
-
-	iface->ifname = strdup(ifname);
-	iface->ifindex = ifindex;
-
-	if (!iface_file_read(ifname, "batman_adv/mesh_iface", "%as", &iface->mesh_iface) || !strcmp(iface->mesh_iface, "none")) {
-		free(iface->mesh_iface);
-		iface->mesh_iface = NULL;
-	}
-
-	iface->bridge_iface = iface_get_bridge(ifname);
-
-	int type;
-	if (iface_file_read(ifname, "type", "%i", &type) && type == ARPHRD_ETHER) {
-		if(iface_file_exists(ifname, "mesh"))
-			iface->iftype = IF_MESH;
-		else if(iface_file_exists(ifname, "bridge"))
-			iface->iftype = IF_BRIDGE;
-		else if(iface_file_exists(ifname, "tun_flags"))
-			iface->iftype = IF_VIRTUAL;
-		else if(iface_file_exists(ifname, "wireless"))
-			iface->iftype = IF_WIRELESS;
-		else
-			iface->iftype = IF_WIRED;
-	}
-
-	return iface;
-}
-
-static void ffvisd_iface_free(ffvisd_iface_t *iface) {
-	if (iface) {
-		free(iface->ifname);
-		free(iface->bridge_iface);
-		free(iface->mesh_iface);
-		free(iface);
-	}
 }
 
 static void ffvisd_iface_foreach(iface_cb cb, void *arg) {
@@ -312,33 +294,37 @@ static void ffvisd_iface_foreach(iface_cb cb, void *arg) {
 	}
 
 	int i;
-	for (i = 0; ifaces[i].if_name; i++) {
+	for (i = 0; ifaces[i].if_name; i++)
-		ffvisd_iface_t *iface = ffvisd_iface_new(ifaces[i].if_name, ifaces[i].if_index);
+		cb(ifaces[i].if_name, ifaces[i].if_index, arg);
-
-		if (iface)
-			cb(iface, arg);
-
-		ffvisd_iface_free(iface);
-	}
 
 	if_freenameindex(ifaces);
 }
 
 
-static void pr_iface_info(const ffvisd_iface_t *iface, void *arg) {
+static void join_mcast(const char *ifname, unsigned ifindex, void *arg) {
-	printf("Interface %s: ifindex=%i bridge_iface=%s mesh_iface=%s iftype=%s\n", iface->ifname, iface->ifindex, iface->bridge_iface, iface->mesh_iface, ffvisd_iftype_names[iface->iftype]);
+	char *if_mesh = iface_get_mesh(ifname);
+
+	if (if_mesh && !strcmp(if_mesh, mesh)) {
+		struct packet_mreq mr;
+		memset(&mr, 0, sizeof(mr));
+		mr.mr_ifindex = ifindex;
+		mr.mr_type = PACKET_MR_MULTICAST;
+		mr.mr_alen = ETH_ALEN;
+		memcpy(mr.mr_address, ffd_addr.address, ETH_ALEN);
+		if (setsockopt(sockfd, SOL_PACKET, PACKET_ADD_MEMBERSHIP, &mr, sizeof(mr)) && errno != EADDRINUSE)
+			fprintf(stderr, "warning: setsockopt: %m\n");
 	}
 
+	free(if_mesh);
+}
 
-static void join_mcast(const ffvisd_iface_t *iface, void *arg) {
+static void pr_announcement_packet(const ffvisd_packet_server_announce_t *announce) {
-	if (!iface->mesh_iface || strcmp(iface->mesh_iface, mesh))
+	const uint8_t *e = announce->eth_address.address, *a = announce->address.address;
-		return;
 
-	struct ipv6_mreq imr;
+	printf("seqno: %u, ether: %02x:%02x:%02x:%02x:%02x:%02x, address: fe80::%02x%02x:%02x%02x:%02x%02x:%02x%02x\n",
-	imr.ipv6mr_multiaddr = ffd_addr;
+	       announce->seqno,
-	imr.ipv6mr_interface = iface->ifindex;
+	       e[0], e[1], e[2], e[3], e[4], e[5],
-	if (setsockopt(sockfd, IPPROTO_IPV6, IPV6_ADD_MEMBERSHIP, &imr, sizeof(struct ipv6_mreq)) && errno != EADDRINUSE)
+	       a[0], a[1], a[2], a[3], a[4], a[5], a[6], a[7]);
-		fprintf(stderr, "warning: setsockopt: %m\n");
 }
 
 static void pr_announcement(const ffvisd_announce_t *announce) {
@@ -351,12 +337,7 @@ static void pr_announcement(const ffvisd_announce_t *announce) {
 		printf("age: %.1fs, ", timespec_diff(&tv, &announce->received)/1000.0);
 	}
 
-	const uint8_t *e = announce->announce.eth_address.address, *a = announce->announce.address.address;
+	pr_announcement_packet(&announce->announce);
-
-	printf("seqno: %u, ether: %02x:%02x:%02x:%02x:%02x:%02x, address: fe80::%02x%02x:%02x%02x:%02x%02x:%02x%02x\n",
-	       announce->announce.seqno,
-	       e[0], e[1], e[2], e[3], e[4], e[5],
-	       a[0], a[1], a[2], a[3], a[4], a[5], a[6], a[7]);
 }
 
 static bool check_config() {
@@ -369,69 +350,45 @@ static bool check_config() {
 }
 
 static bool init_socket() {
-	sockfd = socket(AF_INET6, SOCK_DGRAM, 0);
+	sockfd = socket(AF_PACKET, SOCK_DGRAM, htons(FFD_PROTO));
 	if (sockfd < 0) {
 		fprintf(stderr, "error: socket: %m\n");
 		return false;
 	}
 
-	int val = 1;
-	if (setsockopt(sockfd, IPPROTO_IPV6, IPV6_V6ONLY, &val, sizeof(val))) {
-		fprintf(stderr, "error: setsockopt: %m\n");
-		return false;
-	}
-
-	val = 0;
-	if (setsockopt(sockfd, IPPROTO_IPV6, IPV6_MULTICAST_LOOP, &val, sizeof(val))) {
-		fprintf(stderr, "error: setsockopt: %m\n");
-		return false;
-	}
-
-	struct sockaddr_in6 sa6;
-	memset(&sa6, 0, sizeof(sa6));
-	sa6.sin6_family = AF_INET6;
-	sa6.sin6_port = htons(FFD_PORT);
-
-	if (bind(sockfd, (struct sockaddr*)&sa6, sizeof(struct sockaddr_in6))) {
-		fprintf(stderr, "error: bind: %m\n");
-		return false;
-	}
-
 	return true;
 }
 
 static bool init_announcements() {
-	ffvisd_iface_t *iface;
-
 	char *ifname = iface_get_bridge(mesh);
 
-	if (ifname) {
+	if (!ifname)
-		iface = ffvisd_iface_new(ifname, if_nametoindex(ifname));
+		ifname = strdup(mesh);
-		free(ifname);
-	}
-	else {
-		iface = ffvisd_iface_new(mesh, if_nametoindex(mesh));
-	}
 
-	if (!iface)
+	unsigned ifindex = if_nametoindex(ifname);
+
+	if (!ifindex) {
+		free(ifname);
 		return false;
+	}
 
 	ffvisd_announce_t *announce = announcements = calloc(1, sizeof(ffvisd_announce_t));
 
-	uint8_t *a = announce->announce.eth_address.address;
+	announce->announce.eth_address = iface_get_eth_address(ifname);
-	if (!iface_file_read(iface->ifname, "address", "%x:%x:%x:%x:%x:%x", &a[0], &a[1], &a[2], &a[3], &a[4], &a[5]))
+	if (is_eth_addr_unspec(&announce->announce.eth_address)) {
-		return false;
+		free(ifname);
-
-	struct in6_addr *addr = iface_get_address(iface, RT_SCOPE_LINK);
-	if (!addr) {
-		ffvisd_iface_free(iface);
 		return false;
 	}
 
-	memcpy(announce->announce.address.address, addr->s6_addr+8, 8);
+	struct in6_addr addr = iface_get_address(ifindex, RT_SCOPE_LINK);
-	free(addr);
+	if (IN6_IS_ADDR_UNSPECIFIED(&addr)) {
+		free(ifname);
+		return false;
+	}
 
-	ffvisd_iface_free(iface);
+	memcpy(announce->announce.address.address, addr.s6_addr+8, 8);
+
+	free(ifname);
 
 	return true;
 }
@@ -445,19 +402,81 @@ static void maintenance() {
 	}
 }
 
+static void send_announcement(const char *ifname, unsigned ifindex, void *arg) {
+	ffvisd_packet_announce_t *packet = arg;
+
+	struct sockaddr_ll sa;
+	memset(&sa, 0, sizeof(sa));
+
+	sa.sll_family = AF_PACKET;
+	sa.sll_protocol = htons(FFD_PROTO);
+	sa.sll_ifindex = ifindex;
+	sa.sll_halen = ETH_ALEN;
+	memcpy(sa.sll_addr, ffd_addr.address, ETH_ALEN);
+
+	sendto(sockfd, packet, sizeof(ffvisd_packet_announce_t) + ntohs(packet->n_servers)*sizeof(ffvisd_packet_server_announce_t), 0, (struct sockaddr*)&sa, sizeof(sa));
+}
+
+static void send_announcements() {
+	ffvisd_packet_announce_t *packet = alloca(sizeof(ffvisd_packet_announce_t) + 75*sizeof(ffvisd_packet_server_announce_t));
+
+	packet->magic = htons(FFD_MAGIC);
+	packet->version = htons(FFVISD_VERSION);
+	packet->csum = 0;
+
+	uint16_t n_servers = 0;
+	ffvisd_announce_t *announce;
+	for(announce = announcements; announce; announce = announce->next) {
+		packet->servers[n_servers++] = announce->announce;
+	}
+
+	packet->n_servers = htons(n_servers);
+
+	ffvisd_iface_foreach(send_announcement, packet);
+}
+
+static void receive_announcement() {
+	ffvisd_packet_announce_t *packet = alloca(sizeof(ffvisd_packet_announce_t) + 75*sizeof(ffvisd_packet_server_announce_t));
+
+	int readlen = read(sockfd, packet, sizeof(ffvisd_packet_announce_t) + 75*sizeof(ffvisd_packet_server_announce_t));
+
+	if (readlen < 0) {
+		fprintf(stderr, "error: read: %m\n");
+		return;
+	}
+
+	if (readlen < sizeof(ffvisd_packet_announce_t)) {
+		fprintf(stderr, "warning: short read");
+		return;
+	}
+
+	int n_servers = ntohs(packet->n_servers);
+
+	if (readlen < sizeof(ffvisd_packet_announce_t) + n_servers*sizeof(ffvisd_packet_server_announce_t)) {
+		fprintf(stderr, "warning: short read");
+		return;
+	}
+
+	puts("Received announcements:");
+	int i;
+	for(i = 0; i < n_servers; i++)
+		pr_announcement_packet(&packet->servers[i]);
+	puts("");
+}
+
 int main() {
 	if (!check_config())
 		return 1;
 
-	ffvisd_iface_foreach(pr_iface_info, NULL);
-	puts("");
-
 	if (!init_socket())
 		return 1;
 
 	if (!init_announcements())
 		return 1;
 
+	struct timespec next_announce;
+	clock_gettime(CLOCK_MONOTONIC, &next_announce);
+
 	while (true) {
 		ffvisd_iface_foreach(join_mcast, NULL);
 
@@ -469,7 +488,28 @@ int main() {
 			pr_announcement(announce);
 		puts("");
 
-		usleep(ANNOUNCE_INTERVAL*1000);
+		send_announcements();
+
+		next_announce.tv_sec += ANNOUNCE_INTERVAL;
+
+		while (true) {
+			struct timespec now;
+			clock_gettime(CLOCK_MONOTONIC, &now);
+			int timeout = timespec_diff(&next_announce, &now);
+
+			if (timeout <= 0)
+				break;
+
+			struct pollfd fds[1];
+
+			fds[0].fd = sockfd;
+			fds[0].events = POLLIN;
+
+			poll(fds, 1, timeout);
+
+			if (fds[0].revents & POLLIN)
+				receive_announcement();
+		}
 	}
 
 	return 0;