Handle hello packets

3 files changed, 133 insertions, 12 deletions

diff --git a/ffd/ffd.c b/ffd/ffd.c
index e729a43..1fc9eed 100644
--- a/ffd/ffd.c
+++ b/ffd/ffd.c
@@ -52,7 +52,6 @@ static const eth_addr_t ffd_addr = {{0x03, 0x00, 0x00, 0x00, 0x0f, 0xfd}};
 
 
 static char *mesh = "bat0";
-static unsigned mtu = 1528;
 
 static int sockfd;
 static struct timespec now;
@@ -114,6 +113,16 @@ static bool init_socket() {
 	return true;
 }
 
+static inline ffd_iface_t* get_iface(unsigned ifindex) {
+	ffd_iface_t *iface;
+	for (iface = iface_list; iface; iface = iface->next) {
+		if (iface->ifindex == ifindex)
+			return iface;
+	}
+
+	return NULL;
+}
+
 static void update_netif(const char *ifname, unsigned ifindex, void *arg) {
 	if (!use_netif(ifname))
 		return;
@@ -128,11 +137,7 @@ static void update_netif(const char *ifname, unsigned ifindex, void *arg) {
 	if (setsockopt(sockfd, SOL_PACKET, PACKET_ADD_MEMBERSHIP, &mr, sizeof(mr)) && errno != EADDRINUSE)
 		fprintf(stderr, "warning: setsockopt: %m\n");
 
-	ffd_iface_t *iface;
-	for (iface = iface_list; iface; iface = iface->next) {
-		if (iface->ifindex == ifindex)
-			break;
-	}
+	ffd_iface_t *iface = get_iface(ifindex);
 
 	if (!iface) {
 		/* new iface */
@@ -141,7 +146,7 @@ static void update_netif(const char *ifname, unsigned ifindex, void *arg) {
 		iface_list = iface;
 
 		iface->seqno = 0;
-		iface->neighs = NULL;
+		iface->neigh_list = NULL;
 	}
 
 	iface->ifindex = ifindex;
@@ -149,6 +154,41 @@ static void update_netif(const char *ifname, unsigned ifindex, void *arg) {
 	iface->type = netif_get_type(ifname);
 }
 
+static inline ffd_neigh_t* get_neigh(const ffd_iface_t *iface, const eth_addr_t *addr) {
+	ffd_neigh_t *neigh;
+	for (neigh = iface->neigh_list; neigh; neigh = neigh->next) {
+		if (are_eth_addrs_equal(addr, &neigh->addr))
+			return neigh;
+	}
+
+	return neigh;
+}
+
+static ffd_neigh_t* get_neigh_create(ffd_iface_t *iface, const eth_addr_t *addr) {
+	ffd_neigh_t *neigh = get_neigh(iface, addr);
+	if (!neigh) {
+		neigh = malloc(sizeof(ffd_neigh_t));
+		neigh->next = iface->neigh_list;
+		iface->neigh_list = neigh;
+
+		neigh->addr = *addr;
+		neigh->hello_log = 0;
+		neigh->hello_interval = 0;
+		neigh->last_seqno = 0;
+		neigh->last_hello = (struct timespec){};
+	}
+
+	return neigh;
+}
+
+static void free_neighs(ffd_neigh_t *neigh) {
+	ffd_neigh_t *next;
+	for (; neigh; neigh = next) {
+		next = neigh->next;
+		free(neigh);
+	}
+}
+
 static void update_netifs() {
 	ffd_iface_t *iface, **cur;
 
@@ -163,6 +203,8 @@ static void update_netifs() {
 
 		if (iface->type == IF_UNSPEC) {
 			*cur = iface->next;
+
+			free_neighs(iface->neigh_list);
 			free(iface);
 		}
 		else {
@@ -226,19 +268,93 @@ static void send_hellos() {
 	}
 }
 
+static void handle_tlv_hello(const ffd_tlv_hello_t *tlv_hello, size_t len, const eth_addr_t *addr, ffd_iface_t *iface) {
+	if (len < sizeof(ffd_tlv_hello_t)) {
+		fprintf(stderr, "warn: received short hello TLV.\n");
+		return;
+	}
+
+	fprintf(stderr, "debug: received hello with seqno %u.\n", ntohs(tlv_hello->seqno));
+
+	ffd_neigh_t *neigh = get_neigh_create(iface, addr);
+
+	uint16_t seqno = ntohs(tlv_hello->seqno);
+
+	if (neigh->last_hello.tv_sec) {
+		int16_t seqdiff = seqno - neigh->last_seqno;
+		if (seqdiff <= 0) {
+			fprintf(stderr, "debug: seqno already seen, ignorin.\n");
+			return;
+		}
+
+		int timediff = timespec_diff(&now, &neigh->last_hello)/10;
+		uint16_t seqexp = neigh->last_seqno + (timediff - neigh->hello_interval/2)/neigh->hello_interval;
+
+		/* cast to int16_t to ensure correct handling of seqno wrapping */
+		if (abs((int16_t)(seqno - seqexp)) > 16) {
+			fprintf(stderr, "info: neighbor was reset.\n");
+			neigh->hello_log = 0;
+		}
+		else {
+			neigh->hello_log <<= seqdiff;
+		}
+	}
+	else {
+		fprintf(stderr, "info: received hello from new neighbor.\n");
+	}
+
+	neigh->hello_log |= 1;
+	neigh->hello_interval = ntohs(tlv_hello->interval);
+	neigh->last_seqno = seqno;
+	neigh->last_hello = now;
+
+	fprintf(stderr, "debug: accepted hello, log %04x.\n", neigh->hello_log);
+}
+
+static void handle_tlv(ffd_tlv_type_t type, const void *data, size_t len, void *arg) {
+	const struct sockaddr_ll *from = arg;
+	const eth_addr_t *addr = (const eth_addr_t*)from->sll_addr;
+	ffd_iface_t *iface = get_iface(from->sll_ifindex);
+
+	if (!iface)
+		return;
+
+	switch (type) {
+	case TLV_HELLO:
+		handle_tlv_hello(data, len, addr, iface);
+		return;
+
+	default:
+		fprintf(stderr, "debug: received unknown TLV %u on %s.\n", type, iface->name);
+		return;
+	}
+}
+
 static void receive_packet() {
-	uint8_t buf[mtu];
+	ffd_packet_t *packet = alloca(sizeof(ffd_packet_t)+PACKET_MAX);
 	struct sockaddr_ll from;
 	socklen_t fromlen = sizeof(from);
 
-	ssize_t readlen = recvfrom(sockfd, buf, sizeof(buf), 0, (struct sockaddr*)&from, &fromlen);
+	ssize_t readlen = recvfrom(sockfd, packet, sizeof(ffd_packet_t)+PACKET_MAX, 0, (struct sockaddr*)&from, &fromlen);
 
 	if (readlen < 0) {
 		fprintf(stderr, "error: recvfrom: %m\n");
 		return;
 	}
 
-	fprintf(stderr, "debug: read %zi bytes.\n", readlen);
+	if (from.sll_halen != ETH_ALEN) {
+		fprintf(stderr, "error: receive_packet: strange address length\n");
+		return;
+	}
+
+	if (readlen < sizeof(ffd_packet_t) || readlen < sizeof(ffd_packet_t)+ntohs(packet->len)) {
+		fprintf(stderr, "debug: received short packet.\n");
+		return;
+	}
+
+	if (!ffd_tlv_parse(packet, handle_tlv, &from)) {
+		fprintf(stderr, "debug: received invalid packet.\n");
+	}
 }
 
 int main() {
diff --git a/ffd/ffd.h b/ffd/ffd.h
index c78c4a4..e29fce6 100644
--- a/ffd/ffd.h
+++ b/ffd/ffd.h
@@ -45,6 +45,11 @@ typedef struct _ffd_neigh_t {
 	struct _ffd_neigh_t *next;
 
 	eth_addr_t addr;
+
+	uint16_t hello_log;
+	uint16_t hello_interval;
+	uint16_t last_seqno;
+	struct timespec last_hello;
 } ffd_neigh_t;
 
 typedef struct _ffd_iface_t {
@@ -56,7 +61,7 @@ typedef struct _ffd_iface_t {
 	netif_type_t type;
 	uint16_t seqno;
 
-	ffd_neigh_t *neighs;
+	ffd_neigh_t *neigh_list;
 } ffd_iface_t;
 
 #endif /* _FFD_FFD_H_ */
diff --git a/ffd/tlv.c b/ffd/tlv.c
index 0a1950d..30e56b9 100644
--- a/ffd/tlv.c
+++ b/ffd/tlv.c
@@ -36,7 +36,7 @@ bool ffd_tlv_parse(const ffd_packet_t *packet, ffd_tlv_cb cb, void *arg) {
 	if (packet->version_magic != htons(FFD_VERSION_MAGIC))
 		return false;
 
-	const uint8_t *data = packet->tlv + 2, *end = packet->tlv + packet->len;
+	const uint8_t *data = packet->tlv + 2, *end = packet->tlv + ntohs(packet->len);
 
 	while (data <= end) {
 		if (data[-2] == TLV_PAD1) {