Add sending of updates

4 files changed, 151 insertions, 5 deletions

diff --git a/src/babel.c b/src/babel.c
index 56d3ba0..4177100 100644
--- a/src/babel.c
+++ b/src/babel.c
@@ -41,6 +41,17 @@ static void send_hellos(gmrf_t *gmrf, gmrf_context_t *ctx, void *arg) {
 	gp_babel_send_hellos(gmrf, ctx);
 }
 
+static void send_updates(gmrf_t *gmrf, gmrf_context_t *ctx, void *arg) {
+	gmrf_schedule(gmrf, send_updates, NULL, GP_BABEL_UPDATE_INTERVAL*10);
+
+	gmrf_logf(gmrf, LOG_DEBUG, "sending periodic updates.");
+
+	gp_babel_iface_t *iface;
+	for (iface = ctx->interfaces; iface; iface = iface->next) {
+		gp_babel_send_update(gmrf, ctx, iface, NULL, NULL, false);
+	}
+}
+
 static void maintenance(gmrf_t *gmrf, gmrf_context_t *ctx, void *arg) {
 	gmrf_schedule(gmrf, maintenance, NULL, GP_BABEL_MAINTENANCE_INTERVAL*10);
 
@@ -72,6 +83,7 @@ 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_schedule(gmrf, send_updates, NULL, GP_BABEL_UPDATE_INTERVAL*10);
 	gmrf_schedule(gmrf, maintenance, NULL, GP_BABEL_MAINTENANCE_INTERVAL*10);
 
 	gmrf_context_t *ctx = calloc(1, sizeof(gmrf_context_t));
diff --git a/src/babel.h b/src/babel.h
index 3385ec1..5870364 100644
--- a/src/babel.h
+++ b/src/babel.h
@@ -29,11 +29,16 @@
 
 #include "types.h"
 
+
+#define GP_BABEL_INFINITY 0xffff
+
 #define GP_BABEL_PACKET_MAX 1000
 
 #define GP_BABEL_HELLO_INTERVAL 400
 #define GP_BABEL_IHU_INTERVAL (3*GP_BABEL_HELLO_INTERVAL)
 
+#define GP_BABEL_UPDATE_INTERVAL 3000
+
 #define GP_BABEL_HELLO_TIMEOUT(interval) (16*(interval))
 #define GP_BABEL_IHU_TIMEOUT(interval) ((interval)*7/2)
 
@@ -110,11 +115,39 @@ static inline gp_babel_iface_t* gp_babel_get_iface(gmrf_context_t *ctx, gmrf_ifa
 }
 
 
+static inline bool gp_babel_is_metric_better(gp_babel_metric_seqno_t ms1, gp_babel_metric_seqno_t ms2) {
+	if (ms1.metric == GP_BABEL_INFINITY)
+		return false;
+
+	if (ms2.metric == GP_BABEL_INFINITY)
+		return true;
+
+	int16_t seqno_diff = ms2.seqno - ms1.seqno;
+
+	if (seqno_diff < 0)
+		return true;
+	if (seqno_diff == 0 && ms1.metric < ms2.metric)
+		return true;
+
+	return false;
+}
+
+static inline bool gp_babel_is_feasible(const gp_babel_announce_t *announce, gp_babel_metric_seqno_t ms) {
+	if (ms.metric == GP_BABEL_INFINITY)
+		return true;
+
+	return gp_babel_is_metric_better(ms, announce->feasibility_distance);
+}
+
+
 void gp_babel_handle_packet(gmrf_t *gmrf, gmrf_context_t *ctx, gp_babel_iface_t *iface, const gmrf_addr_t *source, const gp_babel_packet_t *packet);
 
 void gp_babel_send_ack(gmrf_t *gmrf, gmrf_context_t *ctx, gp_babel_neigh_t *neigh, uint16_t nonce);
 void gp_babel_send_hellos(gmrf_t *gmrf, gmrf_context_t *ctx);
 
+void gp_babel_send_update(gmrf_t *gmrf, gmrf_context_t *ctx, gp_babel_iface_t *iface, gp_babel_neigh_t *neigh, gp_babel_announce_t *announce, bool with_data);
+
+
 gp_babel_announce_t* gp_babel_announce_new(gmrf_t *gmrf, gmrf_context_t *ctx);
 gp_babel_announce_t* gp_babel_announce_find(gmrf_t *gmrf, gmrf_context_t *ctx, const gp_babel_node_id_t *node, uint16_t type, uint16_t key);
 gp_babel_announce_t* gp_babel_announce_get(gmrf_t *gmrf, gmrf_context_t *ctx, const gp_babel_node_id_t *node, uint16_t type, uint16_t key);
diff --git a/src/send.c b/src/send.c
index 56685c3..ed23075 100644
--- a/src/send.c
+++ b/src/send.c
@@ -43,6 +43,15 @@ static inline bool send_neigh(gmrf_t *gmrf, const gp_babel_neigh_t *neigh, const
 	return true;
 }
 
+static inline bool send_iface(gmrf_t *gmrf, const gp_babel_iface_t *iface, const gp_babel_packet_t *packet) {
+	if (!gmrf_iface_send_bc(gmrf, iface->gmrf_iface, packet, gp_babel_packet_size(packet))) {
+		gmrf_logf(gmrf, LOG_WARNING, "gmrf_iface_send_bc: %m");
+		return false;
+	}
+
+	return true;
+}
+
 void gp_babel_send_ack(gmrf_t *gmrf, gmrf_context_t *ctx, gp_babel_neigh_t *neigh, uint16_t nonce) {
 	gp_babel_packet_buf_t *buf = gp_babel_packet_alloca(GP_BABEL_PACKET_MAX);
 
@@ -90,9 +99,98 @@ void gp_babel_send_hellos(gmrf_t *gmrf, gmrf_context_t *ctx) {
 		hello->seqno = htons(iface->seqno++);
 
 		buf->packet.len = len;
-
 		add_ihus(gmrf, buf, iface);
 
-		gmrf_iface_send_bc(gmrf, iface->gmrf_iface, &buf->packet, gp_babel_packet_size(&buf->packet));
+		send_iface(gmrf, iface, &buf->packet);
+	}
+}
+
+static inline bool add_node_id(gp_babel_packet_buf_t *buf, const gp_babel_node_id_t *node_id) {
+	gp_babel_tlv_node_id_t *tlv = gp_babel_tlv_add(buf, TLV_NODE_ID, sizeof(gp_babel_tlv_node_id_t));
+	if (!tlv)
+		return false;
+
+	tlv->id = *node_id;
+
+	return true;
+}
+
+static bool add_update(gp_babel_packet_buf_t *buf, gp_babel_node_id_t *node_id, gp_babel_announce_t *announce, bool with_data, bool targetted) {
+	if (announce->len && !announce->data) {
+		/* incomplete announce, handle like non-existent announce */
+		return true;
+	}
+
+	uint16_t len = buf->packet.len;
+
+	if (!node_id || !gp_babel_node_id_equal(node_id, &announce->node)) {
+		if (!add_node_id(buf, &announce->node))
+			return false;
+
+		if (node_id)
+			*node_id = announce->node;
+	}
+
+	uint8_t data_len = (with_data && announce->metric.metric != GP_BABEL_INFINITY) ? announce->len : 0;
+	gp_babel_tlv_update_t *update = gp_babel_tlv_add(buf, TLV_UPDATE, sizeof(gp_babel_tlv_update_t)+data_len);
+	if (!update) {
+		/* reset length to remove possibly added node ID record */
+		buf->packet.len = len;
+		return false;
+	}
+
+	update->flags = 0;
+	update->reserved = 0;
+	update->interval = htons(GP_BABEL_UPDATE_INTERVAL);
+	update->seqno = htons(announce->metric.seqno);
+	update->metric = htons(announce->metric.metric);
+	update->type = htons(announce->type);
+	update->key = htons(announce->key);
+
+	if (data_len)
+		memcpy(update->data, announce->data, data_len);
+
+	if (gp_babel_is_metric_better(announce->metric, announce->feasibility_distance))
+		announce->feasibility_distance = announce->metric;
+
+	if (!targetted)
+		announce->last_metric = announce->metric.metric;
+
+	return true;
+}
+
+void gp_babel_send_update(gmrf_t *gmrf, gmrf_context_t *ctx, gp_babel_iface_t *iface, gp_babel_neigh_t *neigh, gp_babel_announce_t *announce, bool with_data) {
+	gp_babel_packet_buf_t *buf = gp_babel_packet_alloca(GP_BABEL_PACKET_MAX);
+
+	if (announce) {
+		add_update(buf, NULL, announce, with_data, iface || neigh);
+	}
+	else {
+		gp_babel_node_id_t node_id = gp_babel_node_id_unspec;
+
+		gp_babel_announce_t *a;
+		for (a = ctx->announces; a; a = a->next) {
+			if (!add_update(buf, &node_id, a, with_data, iface || neigh)) {
+				if (neigh)
+					send_neigh(gmrf, neigh, &buf->packet);
+				else
+					send_iface(gmrf, iface, &buf->packet);
+
+				node_id = gp_babel_node_id_unspec;
+				buf->packet.len = 0;
+
+				if (!add_update(buf, &node_id, a, with_data, iface || neigh)) {
+					gmrf_logf(gmrf, LOG_ERR, "add_update failed\n");
+					return;
+				}
+			}
+		}
+	}
+
+	if (buf->packet.len) {
+		if (neigh)
+			send_neigh(gmrf, neigh, &buf->packet);
+		else
+			send_iface(gmrf, iface, &buf->packet);
 	}
 }
diff --git a/src/types.h b/src/types.h
index 58b3721..a659230 100644
--- a/src/types.h
+++ b/src/types.h
@@ -33,12 +33,15 @@
 #define GP_BABEL_NODE_ID_LENGTH 8
 
 typedef struct __attribute__((packed)) gp_gabel_node_id {
-        uint8_t id[GP_BABEL_NODE_ID_LENGTH];
+	uint8_t id[GP_BABEL_NODE_ID_LENGTH];
 } gp_babel_node_id_t;
 
+static const gp_babel_node_id_t gp_babel_node_id_unspec = {};
+
+
 typedef struct gp_babel_metric_seqno {
-        uint16_t metric;
-        uint16_t seqno;
+	uint16_t metric;
+	uint16_t seqno;
 } gp_babel_metric_seqno_t;