Add checksum calculation and verification

1 files changed, 23 insertions, 0 deletions

diff --git a/ffvisd/ffvisd.c b/ffvisd/ffvisd.c
index 93e9b1b..d8ecf3d 100644
--- a/ffvisd/ffvisd.c
+++ b/ffvisd/ffvisd.c
@@ -133,6 +133,12 @@ static struct timespec now;
 static ffvisd_announce_t *announcements = NULL;
 
 
+/* ones' complement add */
+static inline u_int16_t add16(u_int16_t a, u_int16_t b) {
+        a += b;
+        return a + (a < b);
+}
+
 static inline bool is_eth_addr_unspec(const eth_addr_t *address) {
 	const uint8_t *a = address->address;
 
@@ -447,6 +453,16 @@ static void maintenance() {
 	}
 }
 
+static uint16_t do_checksum(void *data, unsigned n_words) {
+	uint16_t ret = 0;
+	uint16_t *words = data;
+
+	while(n_words)
+		ret = add16(ret, words[--n_words]);
+
+	return ~ret;
+}
+
 static void send_announcement(const char *ifname, unsigned ifindex, void *arg) {
 	char *if_mesh = iface_get_mesh(ifname);
 
@@ -488,6 +504,8 @@ static void send_announcements() {
 
 	packet->n_servers = htons(n_servers);
 
+	packet->csum = do_checksum(packet, (sizeof(ffvisd_packet_announce_t) + n_servers*sizeof(ffvisd_packet_server_announce_t))/2);
+
 	ffvisd_iface_foreach(send_announcement, packet);
 }
 
@@ -513,6 +531,11 @@ static void receive_announcement() {
 		return;
 	}
 
+	if (do_checksum(packet, (sizeof(ffvisd_packet_announce_t) + n_servers*sizeof(ffvisd_packet_server_announce_t))/2)) {
+		fprintf(stderr, "warning: invalid checksum");
+		return;
+	}
+
 	puts("Received announcements:");
 	int i;
 	for(i = 0; i < n_servers; i++)