1 files changed, 100 insertions, 2 deletions

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);
 	}
 }