1 files changed, 85 insertions, 1 deletions

diff --git a/ffd/send.c b/ffd/send.c
index d020aca..3478e80 100644
--- a/ffd/send.c
+++ b/ffd/send.c
@@ -116,6 +116,90 @@ void ffd_send_hellos() {
 	}
 }
 
-void ffd_send_announce_request(ffd_iface_t *iface, ffd_neigh_t *neigh, void *announce) {
+static bool add_node_id(ffd_packet_t *packet, size_t max_len, ffd_node_id_t node_id) {
+	ffd_tlv_node_id_t *tlv = ffd_tlv_add(packet, FFD_PACKET_MAX, TLV_NODE_ID, sizeof(ffd_tlv_node_id_t));
+	if (!tlv)
+		return false;
 
+	tlv->id = node_id;
+
+	return true;
+}
+
+static bool add_update(ffd_packet_t *packet, size_t max_len, ffd_node_id_t *node_id, const ffd_announce_t *announce) {
+	uint16_t len = packet->len;
+
+	if (!node_id || !ffd_are_node_ids_equal(node_id, &announce->node)) {
+		if (!add_node_id(packet, max_len, announce->node))
+			return false;
+
+		if (node_id)
+			*node_id = announce->node;
+	}
+
+	ffd_tlv_update_t *update = ffd_tlv_add(packet, FFD_PACKET_MAX, TLV_UPDATE, sizeof(ffd_tlv_update_t)+announce->len);
+	if (!update) {
+		packet->len = len;
+		return false;
+	}
+
+	ffd_metric_seqno_t metric = ffd_announce_get_metric(announce);
+
+	update->interval = htons(announce->interval);
+	update->seqno = htons(metric.seqno);
+	update->metric = htons(metric.metric);
+	update->type = htons(announce->type);
+	update->key = htons(announce->key);
+
+	memcpy(update->data, announce->data, announce->len);
+
+	return true;
+}
+
+void ffd_send_update(ffd_iface_t *iface, ffd_neigh_t *neigh, const ffd_announce_t *announce) {
+	ffd_packet_t *packet = alloca(sizeof(ffd_packet_t)+FFD_PACKET_MAX);
+
+	packet->version_magic = htons(FFD_VERSION_MAGIC);
+	packet->len = 0;
+
+	if (announce) {
+		add_update(packet, FFD_PACKET_MAX, NULL, announce);
+	}
+	else {
+		ffd_node_id_t node_id = FFD_NODE_ID_UNSPEC;
+
+		ffd_announce_t *a;
+		for (a = announce_list; a; a = a->next) {
+			if (!add_update(packet, FFD_PACKET_MAX, &node_id, a)) {
+				if (!send_eth(&ffd_addr, iface->ifindex, packet, sizeof(ffd_packet_t)+ntohs(packet->len)))
+					fprintf(stderr, "send_eth: %m\n");
+
+				node_id = FFD_NODE_ID_UNSPEC;
+				packet->len = 0;
+			}
+		}
+
+		if (packet->len) {
+			if (!send_eth(&ffd_addr, iface->ifindex, packet, sizeof(ffd_packet_t)+ntohs(packet->len)))
+				fprintf(stderr, "send_eth: %m\n");
+		}
+	}
+}
+
+void ffd_send_announce_request(ffd_iface_t *iface, ffd_neigh_t *neigh, ffd_node_id_t node, uint16_t type, uint16_t key) {
+	ffd_packet_t *packet = alloca(sizeof(ffd_packet_t)+FFD_PACKET_MAX);
+
+	packet->version_magic = htons(FFD_VERSION_MAGIC);
+	packet->len = 0;
+
+	ffd_tlv_announce_req_t *req = ffd_tlv_add(packet, FFD_PACKET_MAX, TLV_ANNOUNCE_REQ, sizeof(ffd_tlv_announce_req_t));
+	if (!req)
+		return;
+
+	req->node = node;
+	req->type = htons(type);
+	req->key = htons(key);
+
+	if (!send_eth(&ffd_addr, iface->ifindex, packet, sizeof(ffd_packet_t)+ntohs(packet->len)))
+		fprintf(stderr, "send_eth: %m\n");
 }