Implement hello and IHU sending and receiving

1 files changed, 230 insertions, 0 deletions

diff --git a/src/babel.c b/src/babel.c
new file mode 100644
index 0000000..39cd6c8
--- /dev/null
+++ b/src/babel.c
@@ -0,0 +1,230 @@
+/*
+  Copyright (c) 2013, Matthias Schiffer <mschiffer@universe-factory.net>
+  All rights reserved.
+
+  Redistribution and use in source and binary forms, with or without
+  modification, are permitted provided that the following conditions are met:
+
+    1. Redistributions of source code must retain the above copyright notice,
+       this list of conditions and the following disclaimer.
+    2. Redistributions in binary form must reproduce the above copyright notice,
+       this list of conditions and the following disclaimer in the documentation
+       and/or other materials provided with the distribution.
+
+  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+  DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+  FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+  DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+  SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+  OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+  OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+
+#include "babel.h"
+#include "neigh.h"
+#include "packet.h"
+#include "tlv.h"
+#include "tlv_types.h"
+
+#include <stdlib.h>
+
+#include <arpa/inet.h>
+
+
+const char *gmrf_protocol_name = "babel";
+const char *gmrf_protocol_version = "experimental";
+
+
+static void add_ihus(gmrf_t *gmrf, gp_babel_packet_t *packet, size_t max_len, const gp_babel_iface_t *iface) {
+	const gp_babel_neigh_t *neigh;
+
+	for (neigh = iface->neighbours; neigh; neigh = neigh->next) {
+		gp_babel_tlv_ihu_t *ihu = gp_babel_tlv_add(packet, GP_BABEL_PACKET_MAX, TLV_IHU, sizeof(gp_babel_tlv_ihu_t)+sizeof(gmrf_addr_t));
+		if (!ihu)
+			return;
+
+		ihu->ae = ADDR_ENC_GMRF;
+		ihu->reserved = 0;
+		ihu->rxcost = htons(gp_babel_neigh_get_rxcost(gmrf, neigh));
+		ihu->interval = htons(GP_BABEL_IHU_INTERVAL);
+		memcpy(ihu->address, &neigh->addr, sizeof(gmrf_addr_t));
+	}
+}
+
+static void send_hellos(gmrf_t *gmrf, gmrf_context_t *ctx, void *arg) {
+	gmrf_schedule(gmrf, send_hellos, NULL, GP_BABEL_HELLO_INTERVAL*10);
+
+	gmrf_logf(gmrf, LOG_DEBUG, "sending hellos...");
+
+	gp_babel_packet_t *packet = alloca(sizeof(gp_babel_packet_t)+GP_BABEL_PACKET_MAX);
+
+	packet->version = htons(GP_BABEL_VERSION);
+	packet->len = 0;
+
+	gp_babel_tlv_hello_t *hello = gp_babel_tlv_add(packet, GP_BABEL_PACKET_MAX, TLV_HELLO, sizeof(gp_babel_tlv_hello_t));
+	if (!hello)
+		return;
+
+	hello->reserved = 0;
+	hello->interval = htons(GP_BABEL_HELLO_INTERVAL);
+
+	uint16_t len = packet->len;
+
+	gp_babel_iface_t *iface;
+	for (iface = ctx->interfaces; iface; iface = iface->next) {
+		hello->seqno = htons(iface->seqno++);
+
+		packet->len = len;
+
+		add_ihus(gmrf, packet, GP_BABEL_PACKET_MAX, iface);
+
+		gmrf_iface_send_bc(gmrf, iface->gmrf_iface, packet, sizeof(gp_babel_packet_t)+ntohs(packet->len));
+	}
+}
+
+gmrf_context_t* gmrf_protocol_init(gmrf_t *gmrf) {
+	gmrf_logf(gmrf, LOG_INFO, "initalizing...");
+
+	gmrf_schedule(gmrf, send_hellos, NULL, GP_BABEL_HELLO_INTERVAL*10);
+
+	gmrf_context_t *ctx = calloc(1, sizeof(gmrf_context_t));
+	gmrf_random_bytes(gmrf, &ctx->self, sizeof(gp_babel_node_id_t));
+
+	return ctx;
+}
+
+void gmrf_protocol_add_iface(gmrf_t *gmrf, gmrf_context_t *ctx, gmrf_iface_t *iface) {
+	gmrf_logf(gmrf, LOG_INFO, "interface `%s' added.", gmrf_iface_get_name(gmrf, iface));
+
+	gp_babel_iface_t *b_iface = calloc(1, sizeof(gp_babel_iface_t));
+	b_iface->gmrf_iface = iface;
+
+	b_iface->next = ctx->interfaces;
+	ctx->interfaces = b_iface;
+}
+
+typedef struct handle_tlv_arg {
+	gp_babel_iface_t *iface;
+	const gmrf_addr_t *source;
+
+	gp_babel_node_id_t node_id;
+} handle_tlv_arg_t;
+
+static inline gp_babel_neigh_t* get_tlv_neigh(handle_tlv_arg_t *arg) {
+	return gp_babel_neigh_get(arg->iface, arg->source);
+}
+
+static void handle_tlv_hello(gmrf_t *gmrf, gmrf_context_t *ctx, const gp_babel_tlv_hello_t *tlv_hello, size_t len, handle_tlv_arg_t *arg) {
+	if (len < sizeof(gp_babel_tlv_hello_t)) {
+		gmrf_logf(gmrf, LOG_WARNING, "received short hello TLV.");
+		return;
+	}
+
+	gmrf_logf(gmrf, LOG_DEBUG, "received hello with seqno %u.", ntohs(tlv_hello->seqno));
+
+	gp_babel_neigh_t *neigh = get_tlv_neigh(arg);
+
+	uint16_t seqno = ntohs(tlv_hello->seqno);
+
+	if (neigh->last_hello != gmrf_time_unspec) {
+		int timediff = (gmrf_now(gmrf) - 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) {
+			gmrf_logf(gmrf, LOG_INFO, "neighbour was reset.");
+			neigh->hello_log = 0;
+			neigh->txcost = 0xffff;
+		}
+		else {
+			int16_t seqdiff = seqno - neigh->last_seqno;
+			if (seqdiff <= 0) {
+				gmrf_logf(gmrf, LOG_DEBUG, "seqno already seen, ignoring.");
+				return;
+			}
+
+			neigh->hello_log <<= seqdiff;
+		}
+	}
+	else {
+		gmrf_logf(gmrf, LOG_DEBUG, "received hello from new neighbour.");
+	}
+
+	neigh->hello_log |= 1;
+	neigh->hello_interval = ntohs(tlv_hello->interval);
+	neigh->last_seqno = seqno;
+	neigh->last_hello = gmrf_now(gmrf);
+
+	if (neigh->hello_log == 1) /* new or reset neighbour */
+		gp_babel_neigh_reset(arg->iface, neigh);
+
+	gmrf_logf(gmrf, LOG_DEBUG, "accepted hello, log %04x, rxcost is %u, cost is %u now.", neigh->hello_log, gp_babel_neigh_get_rxcost(gmrf, neigh), gp_babel_neigh_get_cost(gmrf, neigh));
+}
+
+static void handle_tlv_ihu(gmrf_t *gmrf, gmrf_context_t *ctx, const gp_babel_tlv_ihu_t *tlv_ihu, size_t len, handle_tlv_arg_t *arg) {
+	if (len < sizeof(gp_babel_tlv_ihu_t)) {
+		gmrf_logf(gmrf, LOG_WARNING, "received short IHU TLV.");
+		return;
+	}
+
+	if (tlv_ihu->ae == ADDR_ENC_GMRF) {
+		if (len < sizeof(gp_babel_tlv_ihu_t)+sizeof(gmrf_addr_t)) {
+			gmrf_logf(gmrf, LOG_WARNING, "received short IHU TLV.");
+			return;
+		}
+
+		gmrf_addr_t iface_addr = gmrf_iface_get_addr(gmrf, arg->iface->gmrf_iface);
+		if (memcmp(tlv_ihu->address, &iface_addr, sizeof(gmrf_addr_t)) != 0)
+			return;
+	}
+	else if (tlv_ihu->ae != ADDR_ENC_UNSPEC) {
+		return;
+	}
+
+	gp_babel_neigh_t *neigh = get_tlv_neigh(arg);
+	neigh->ihu_interval = ntohs(tlv_ihu->interval);
+	neigh->last_ihu = gmrf_now(gmrf);
+	neigh->txcost = ntohs(tlv_ihu->rxcost);
+
+	gmrf_logf(gmrf, LOG_DEBUG, "accepted IHU, txcost is %u, cost is %u now.", gp_babel_neigh_get_txcost(gmrf, neigh), gp_babel_neigh_get_cost(gmrf, neigh));
+}
+
+static void handle_tlv(gmrf_t *gmrf, gmrf_context_t *ctx, gp_babel_tlv_type_t type, const void *data, size_t len, void *arg) {
+	handle_tlv_arg_t *tlv_arg = arg;
+
+	switch (type) {
+	case TLV_HELLO:
+		handle_tlv_hello(gmrf, ctx, data, len, tlv_arg);
+		return;
+
+	case TLV_IHU:
+		handle_tlv_ihu(gmrf, ctx, data, len, tlv_arg);
+		return;
+
+	default:
+		gmrf_logf(gmrf, LOG_DEBUG, "received unknown TLV %u on %s.", type, gmrf_iface_get_name(gmrf, tlv_arg->iface->gmrf_iface));
+		return;
+	}
+}
+
+void gmrf_protocol_handle_packet(gmrf_t *gmrf, gmrf_context_t *ctx, gmrf_iface_t *iface, const gmrf_addr_t *source, const void *data, size_t len) {
+	const gp_babel_packet_t *packet = data;
+
+	if (len < sizeof(gp_babel_packet_t) || len < sizeof(gp_babel_packet_t)+ntohs(packet->len)) {
+		gmrf_logf(gmrf, LOG_DEBUG, "received short packet.");
+		return;
+	}
+
+	handle_tlv_arg_t arg = { .iface = gp_babel_get_iface(ctx, iface), .source = source };
+
+	if (!arg.iface)
+		return;
+
+	if (!gp_babel_tlv_parse(gmrf, ctx, packet, handle_tlv, &arg)) {
+		gmrf_logf(gmrf, LOG_DEBUG, "received invalid packet.");
+	}
+}