Initialize more stuff

ffvisd/ffvisd.c

@@ -26,6 +26,7 @@
 
 #define _GNU_SOURCE
 
+#include <errno.h>
 #include <libgen.h>
 #include <stdarg.h>
 #include <stdbool.h>
@@ -33,18 +34,25 @@
 #include <stdint.h>
 #include <stdlib.h>
 #include <string.h>
+#include <time.h>
 #include <unistd.h>
 
+#include <arpa/inet.h>
+
+#include <linux/rtnetlink.h>
+
 #include <net/ethernet.h>
 #include <net/if.h>
 #include <net/if_arp.h>
 
+#include <netinet/in.h>
+
 #include <sys/types.h>
 #include <sys/stat.h>
 
 
-#define ANNOUNCE_INTERVAL 20
+#define ANNOUNCE_INTERVAL 10000
-#define ANNOUNCE_TIMEOUT 60
+#define ANNOUNCE_TIMEOUT 30000
 
 #define SYSFS_PATH_MAX 256
 #define SYSFS_CLASS_NET "/sys/class/net"
@@ -52,32 +60,33 @@
 #define FFD_MAGIC 0xffd
 #define FFVISD_VERSION 0
 
-
+#define FFD_PORT 0xffd
-//static char *mesh = "bat0";
 
 
-typedef struct _ffvisd_eth_address_t {
+static const struct in6_addr ffd_addr = { .s6_addr = {0xff, 0x12, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+						      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;
 
-typedef struct _ffvisd_ipv6_host_address_t {
+typedef struct __attribute__((__packed__)) _ffvisd_ipv6_host_address_t {
 	uint8_t address[8];
 } ffvisd_ipv6_host_address_t;
 
-typedef struct _ffvisd_server_announce_t {
+typedef struct __attribute__((__packed__)) _ffvisd_packet_server_announce_t {
+	uint32_t seqno;
 	ffvisd_eth_address_t eth_address;
-	uint16_t reserved;
 	ffvisd_ipv6_host_address_t address;
-	uint32_t age;
+} ffvisd_packet_server_announce_t;
-} ffvisd_server_announce_t;
 
-typedef struct _ffvisd_announce_t {
+typedef struct __attribute__((__packed__)) _ffvisd_packet_announce_t {
 	uint16_t magic;
 	uint16_t version;
 	uint32_t n_servers;
-	uint32_t reserved;
+	ffvisd_packet_server_announce_t servers[];
-	ffvisd_server_announce_t servers[];
+} ffvisd_packet_announce_t;
-} ffvisd_announce_t;
 
 typedef enum _ffvisd_iftype_t {
 	IF_UNKNOWN = 0,
@@ -97,6 +106,19 @@ typedef struct _ffvisd_iface_t {
 	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;
+
+	struct timeval received;
+	ffvisd_packet_server_announce_t announce;
+} ffvisd_announce_t;
+
 static const char *ffvisd_iftype_names[IF_MAX] = {
 	[IF_UNKNOWN] = "unknown",
 	[IF_WIRED] = "wired",
@@ -106,13 +128,28 @@ static const char *ffvisd_iftype_names[IF_MAX] = {
 	[IF_MESH] = "mesh",
 };
 
+
 typedef void (*iface_cb)(const ffvisd_iface_t *iface, void *arg);
 
 
-void ffvisd_iface_free(ffvisd_iface_t *iface);
+static int sockfd;
+static char *mesh = "bat0";
 
 
-bool file_readv(const char *file, const char *format, va_list ap) {
+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;
+}
+
+
+static bool file_readv(const char *file, const char *format, va_list ap) {
 	FILE *f = fopen(file, "r");
 	if (!f)
 		return false;
@@ -124,7 +161,7 @@ bool file_readv(const char *file, const char *format, va_list ap) {
 	return (ret > 0);
 }
 
-bool iface_file_read(const char *ifname, const char *file, const char *format, ...) {
+static bool iface_file_read(const char *ifname, const char *file, const char *format, ...) {
 	char filename[SYSFS_PATH_MAX];
 
 	snprintf(filename, SYSFS_PATH_MAX, SYSFS_CLASS_NET"/%s/%s", ifname, file);
@@ -137,7 +174,7 @@ bool iface_file_read(const char *ifname, const char *file, const char *format, .
 	return ret;
 }
 
-bool iface_file_exists(const char *ifname, const char *file) {
+static bool iface_file_exists(const char *ifname, const char *file) {
 	char filename[SYSFS_PATH_MAX];
 
 	snprintf(filename, SYSFS_PATH_MAX, SYSFS_CLASS_NET"/%s/%s", ifname, file);
@@ -146,7 +183,11 @@ bool iface_file_exists(const char *ifname, const char *file) {
 	return (stat(filename, &st) == 0);
 }
 
-char* get_bridge(const char *ifname) {
+static bool iface_is_mesh(const char *ifname) {
+	return iface_file_exists(ifname, "mesh");
+}
+
+static char* iface_get_bridge(const char *ifname) {
 	char filename[SYSFS_PATH_MAX], filename2[SYSFS_PATH_MAX] = {0};
 
 	snprintf(filename, SYSFS_PATH_MAX, SYSFS_CLASS_NET"/%s/brport/bridge", ifname);
@@ -156,7 +197,74 @@ char* get_bridge(const char *ifname) {
 	return strdup(basename(filename2));
 }
 
-ffvisd_iface_t* ffvisd_iface_new(const char *ifname, unsigned ifindex) {
+static struct in6_addr* iface_get_address(ffvisd_iface_t *iface, int scope) {
+	struct msg {
+		struct nlmsghdr nh;
+		struct ifaddrmsg addr;
+		uint8_t attrbuf[16384];
+	} msg;
+
+	memset(&msg, 0, sizeof(msg));
+
+	int rtnetlink_sk = socket(AF_NETLINK, SOCK_DGRAM, NETLINK_ROUTE);
+
+	msg.nh.nlmsg_len = NLMSG_LENGTH(sizeof(struct ifaddrmsg));
+	msg.nh.nlmsg_flags = NLM_F_REQUEST | NLM_F_ROOT;
+	msg.nh.nlmsg_type = RTM_GETADDR;
+
+	msg.addr.ifa_family = AF_INET6;
+
+	if(write(rtnetlink_sk, &msg, msg.nh.nlmsg_len) <= 0) {
+		fprintf(stderr, "error: write: %m\n");
+		close(rtnetlink_sk);
+		return NULL;
+	}
+
+	int readlen = read(rtnetlink_sk, &msg, sizeof(msg));
+
+	if (readlen <= 0) {
+		fprintf(stderr, "error: read: %m\n");
+		close(rtnetlink_sk);
+		return NULL;
+	}
+
+	close(rtnetlink_sk);
+
+	struct msg *chunk;
+
+	for (chunk = &msg; readlen > sizeof(sizeof(struct nlmsghdr));) {
+		int len = chunk->nh.nlmsg_len - sizeof(struct nlmsghdr);
+
+		if (len < sizeof(struct ifaddrmsg) || readlen < len)
+			return NULL;
+
+		if (!NLMSG_OK(&chunk->nh, readlen))
+			return NULL;
+
+		if (chunk->nh.nlmsg_type == RTM_NEWADDR && chunk->addr.ifa_scope == scope && chunk->addr.ifa_index == iface->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));
+					memcpy(ret->s6_addr, RTA_DATA(rta), 16);
+					return ret;
+				}
+			}
+		}
+
+		readlen -= NLMSG_ALIGN(chunk->nh.nlmsg_len);
+		chunk = (struct msg*)((uint8_t*)chunk + NLMSG_ALIGN(chunk->nh.nlmsg_len));
+	}
+
+	return NULL;
+}
+
+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);
@@ -167,7 +275,7 @@ ffvisd_iface_t* ffvisd_iface_new(const char *ifname, unsigned ifindex) {
 		iface->mesh_iface = NULL;
 	}
 
-	iface->bridge_iface = get_bridge(ifname);
+	iface->bridge_iface = iface_get_bridge(ifname);
 
 	int type;
 	if (iface_file_read(ifname, "type", "%i", &type) && type == ARPHRD_ETHER) {
@@ -186,7 +294,7 @@ ffvisd_iface_t* ffvisd_iface_new(const char *ifname, unsigned ifindex) {
 	return iface;
 }
 
-void ffvisd_iface_free(ffvisd_iface_t *iface) {
+static void ffvisd_iface_free(ffvisd_iface_t *iface) {
 	if (iface) {
 		free(iface->ifname);
 		free(iface->bridge_iface);
@@ -195,11 +303,11 @@ void ffvisd_iface_free(ffvisd_iface_t *iface) {
 	}
 }
 
-void ffvisd_iface_foreach(iface_cb cb, void *arg) {
+static void ffvisd_iface_foreach(iface_cb cb, void *arg) {
 	struct if_nameindex *ifaces = if_nameindex();
 
 	if (!ifaces) {
-		fprintf(stderr, "error: if_nameindex: %m");
+		fprintf(stderr, "error: if_nameindex: %m\n");
 		return;
 	}
 
@@ -217,13 +325,131 @@ void ffvisd_iface_foreach(iface_cb cb, void *arg) {
 }
 
 
-void pr_iface_info(const ffvisd_iface_t *iface, void *arg) {
+static void pr_iface_info(const ffvisd_iface_t *iface, 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]);
 }
 
 
+static void join_mcast(const ffvisd_iface_t *iface, void *arg) {
+	if (!iface->mesh_iface || strcmp(iface->mesh_iface, mesh))
+		return;
+
+	struct ipv6_mreq imr;
+	imr.ipv6mr_multiaddr = ffd_addr;
+	imr.ipv6mr_interface = iface->ifindex;
+	if (setsockopt(sockfd, IPPROTO_IPV6, IPV6_ADD_MEMBERSHIP, &imr, sizeof(struct ipv6_mreq)) && errno != EADDRINUSE)
+		fprintf(stderr, "warning: setsockopt: %m\n");
+}
+
+static bool check_config() {
+	if (!iface_is_mesh(mesh)) {
+		fprintf(stderr, "error: configured interface is no mesh\n");
+		return false;
+	}
+
+	return true;
+}
+
+static bool init_socket() {
+	sockfd = socket(AF_INET6, SOCK_DGRAM, 0);
+	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) {
+		iface = ffvisd_iface_new(ifname, if_nametoindex(ifname));
+		free(ifname);
+	}
+	else {
+		iface = ffvisd_iface_new(mesh, if_nametoindex(mesh));
+	}
+
+	if (!iface)
+		return false;
+
+	ffvisd_announce_t *announce = announcements = calloc(1, sizeof(ffvisd_announce_t));
+
+	uint8_t *a = announce->announce.eth_address.address;
+	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]))
+		return false;
+
+	struct in6_addr *addr = iface_get_address(iface, RT_SCOPE_LINK);
+	if (!addr) {
+		ffvisd_iface_free(iface);
+		return false;
+	}
+
+	char buf[INET6_ADDRSTRLEN];
+	inet_ntop(AF_INET6, addr, buf, sizeof(buf));
+	puts(buf);
+
+	memcpy(announce->announce.address.address, addr->s6_addr+8, 8);
+	free(addr);
+
+	ffvisd_iface_free(iface);
+
+	return true;
+}
+
+static void maintenance() {
+	ffvisd_announce_t **announce;
+
+	for(announce = &announcements; *announce; announce = &(*announce)->next) {
+		if (!(*announce)->received.tv_sec) /* announcement is local */
+			(*announce)->announce.seqno++;
+	}
+}
+
 int main() {
+	if (!check_config())
+		return 1;
+
 	ffvisd_iface_foreach(pr_iface_info, NULL);
 
+	if (!init_socket())
+		return 1;
+
+	if (!init_announcements())
+		return 1;
+
+	while (true) {
+		ffvisd_iface_foreach(join_mcast, NULL);
+
+		maintenance();
+
+		usleep(ANNOUNCE_INTERVAL*1000);
+	}
+
 	return 0;
 }