Send and handle IHUs

3 files changed, 75 insertions, 5 deletions

diff --git a/ffd/ffd.c b/ffd/ffd.c
index 14d770c..02e2df2 100644
--- a/ffd/ffd.c
+++ b/ffd/ffd.c
@@ -47,7 +47,8 @@
 
 static const eth_addr_t ffd_addr = {{0x03, 0x00, 0x00, 0x00, 0x0f, 0xfd}};
 
-#define HELLO_INTERVAL 10
+#define HELLO_INTERVAL 1000
+#define IHU_INTERVAL (3*HELLO_INTERVAL)
 #define PACKET_MAX 1000
 
 
@@ -152,6 +153,7 @@ static void update_netif(const char *ifname, unsigned ifindex, void *arg) {
 	iface->ifindex = ifindex;
 	strncpy(iface->name, ifname, IF_NAMESIZE);
 	iface->type = netif_get_type(ifname);
+	iface->addr = netif_get_eth_addr(ifname);
 }
 
 static inline ffd_neigh_t* get_neigh(const ffd_iface_t *iface, const eth_addr_t *addr) {
@@ -186,7 +188,7 @@ static void free_neighs(ffd_neigh_t *neigh) {
 	}
 }
 
-static uint16_t neigh_get_rxcost(ffd_neigh_t *neigh) {
+static uint16_t get_neigh_rxcost(const ffd_neigh_t *neigh) {
 	int timediff = timespec_diff(&now, &neigh->last_hello)/10;
 	int shift = (timediff - neigh->hello_interval/2)/neigh->hello_interval;
 	int received = __builtin_popcount(neigh->hello_log << shift);
@@ -253,6 +255,21 @@ static bool send_eth(const eth_addr_t *addr, unsigned ifindex, void *buf, size_t
 	return true;
 }
 
+static void add_ihus(ffd_packet_t *packet, size_t max_len, const ffd_iface_t *iface) {
+	const ffd_neigh_t *neigh;
+
+	for (neigh = iface->neigh_list; neigh; neigh = neigh->next) {
+		ffd_tlv_ihu_t *ihu = ffd_tlv_add(packet, PACKET_MAX, TLV_IHU, sizeof(ffd_tlv_ihu_t)+sizeof(eth_addr_t));
+		if (!ihu)
+			return;
+
+		ihu->ae = ADDR_ENC_ETH;
+		ihu->reserved = 0;
+		ihu->rxcost = htons(get_neigh_rxcost(neigh));
+		ihu->interval = htons(IHU_INTERVAL);
+		memcpy(ihu->address, &neigh->addr, sizeof(eth_addr_t));
+	}
+}
 
 static void send_hellos() {
 	ffd_packet_t *packet = alloca(sizeof(ffd_packet_t)+PACKET_MAX);
@@ -265,12 +282,18 @@ static void send_hellos() {
 		return;
 
 	hello->reserved = 0;
-	hello->interval = htons(HELLO_INTERVAL*100);
+	hello->interval = htons(HELLO_INTERVAL);
+
+	uint16_t len = packet->len;
 
 	ffd_iface_t *iface;
 	for (iface = iface_list; iface; iface = iface->next) {
 		hello->seqno = htons(iface->seqno++);
 
+		packet->len = len;
+
+		add_ihus(packet, PACKET_MAX, iface);
+
 		if (!send_eth(&ffd_addr, iface->ifindex, packet, sizeof(ffd_packet_t)+ntohs(packet->len)))
 			fprintf(stderr, "send_eth: %m\n");
 	}
@@ -316,7 +339,33 @@ static void handle_tlv_hello(const ffd_tlv_hello_t *tlv_hello, size_t len, const
 	neigh->last_seqno = seqno;
 	neigh->last_hello = now;
 
-	fprintf(stderr, "debug: accepted hello, log %04x, rxcost is %u now.\n", neigh->hello_log, neigh_get_rxcost(neigh));
+	fprintf(stderr, "debug: accepted hello, log %04x, rxcost is %u now.\n", neigh->hello_log, get_neigh_rxcost(neigh));
+}
+
+static void handle_tlv_ihu(const ffd_tlv_ihu_t *tlv_ihu, size_t len, const eth_addr_t *addr, ffd_iface_t *iface) {
+	if (len < sizeof(ffd_tlv_ihu_t)) {
+		fprintf(stderr, "warn: received short IHU TLV.\n");
+		return;
+	}
+
+	if (tlv_ihu->ae == ADDR_ENC_ETH) {
+		if (len < sizeof(ffd_tlv_ihu_t)+sizeof(eth_addr_t)) {
+			fprintf(stderr, "warn: received short IHU TLV.\n");
+			return;
+		}
+
+		if (memcmp(tlv_ihu->address, &iface->addr, sizeof(eth_addr_t)) != 0)
+			return;
+	}
+	else if (tlv_ihu->ae != ADDR_ENC_UNSPEC) {
+		return;
+	}
+
+	ffd_neigh_t *neigh = get_neigh_create(iface, addr);
+	neigh->last_ihu = now;
+	neigh->txcost = ntohs(tlv_ihu->rxcost);
+
+	fprintf(stderr, "debug: accepted IHU, txcost is %u now.\n", neigh->txcost);
 }
 
 static void handle_tlv(ffd_tlv_type_t type, const void *data, size_t len, void *arg) {
@@ -332,6 +381,10 @@ static void handle_tlv(ffd_tlv_type_t type, const void *data, size_t len, void *
 		handle_tlv_hello(data, len, addr, iface);
 		return;
 
+	case TLV_IHU:
+		handle_tlv_ihu(data, len, addr, iface);
+		return;
+
 	default:
 		fprintf(stderr, "debug: received unknown TLV %u on %s.\n", type, iface->name);
 		return;
@@ -365,6 +418,13 @@ static void receive_packet() {
 	}
 }
 
+void add_interval(struct timespec *time, int interval) {
+	int cs = time->tv_nsec/1e7 + interval;
+
+	time->tv_sec += cs/100;
+	time->tv_nsec = time->tv_nsec%(long)1e7 + (cs%100)*1e7;
+}
+
 int main() {
 	if (!check_config())
 		return 1;
@@ -387,7 +447,7 @@ int main() {
 		if (timeout <= 0) {
 			send_hellos();
 
-			next_hello.tv_sec += HELLO_INTERVAL;
+			add_interval(&next_hello, HELLO_INTERVAL);
 			continue;
 		}
 
diff --git a/ffd/ffd.h b/ffd/ffd.h
index 1ef5494..81bccfc 100644
--- a/ffd/ffd.h
+++ b/ffd/ffd.h
@@ -50,6 +50,7 @@ typedef struct _ffd_neigh_t {
 	uint16_t hello_interval;
 	uint16_t last_seqno;
 	struct timespec last_hello;
+	struct timespec last_ihu;
 
 	uint16_t txcost;
 } ffd_neigh_t;
@@ -59,6 +60,7 @@ typedef struct _ffd_iface_t {
 
 	unsigned ifindex;
 	char name[IF_NAMESIZE];
+	eth_addr_t addr;
 
 	netif_type_t type;
 	uint16_t seqno;
diff --git a/ffd/tlv_types.h b/ffd/tlv_types.h
index 39efc13..221240e 100644
--- a/ffd/tlv_types.h
+++ b/ffd/tlv_types.h
@@ -36,4 +36,12 @@ typedef struct __attribute__((packed)) _ffd_tlv_hello_t {
 	uint16_t interval;
 } ffd_tlv_hello_t;
 
+typedef struct __attribute__((packed)) _ffd_tlv_ihu_t {
+	uint8_t ae;
+	uint8_t reserved;
+	uint16_t rxcost;
+	uint16_t interval;
+	uint8_t address[0];
+} ffd_tlv_ihu_t;
+
 #endif /* _FFD_TLV_TYPES_H_ */