Keep track of timestamps on incoming Hello messages

message.c

@@ -166,6 +166,48 @@ parse_route_attributes(const unsigned char *a, int alen,
     }
 }
 
+static int
+parse_hello_subtlv(const unsigned char *a, int alen, struct neighbour *neigh)
+{
+    int type, len, i = 0, ret = 0;
+
+    while(i < alen) {
+        type = a[0];
+        if(type == SUBTLV_PAD1) {
+            i++;
+            continue;
+        }
+
+        if(i + 1 > alen) {
+            fprintf(stderr, "Received truncated sub-TLV on Hello message.\n");
+            return -1;
+        }
+        len = a[i + 1];
+        if(i + len > alen) {
+            fprintf(stderr, "Received truncated sub-TLV on Hello message.\n");
+            return -1;
+        }
+
+        if(type == SUBTLV_PADN) {
+            /* Nothing to do. */
+        } else if(type == SUBTLV_TIMESTAMP) {
+            if(len >= 4) {
+                DO_NTOHL(neigh->hello_send_us, a + i + 2);
+                neigh->hello_rtt_receive_time = now;
+                ret = 1;
+            } else {
+                fprintf(stderr,
+                        "Received incorrect RTT sub-TLV on Hello message.\n");
+            }
+        } else {
+            fprintf(stderr, "Received unknown Hello sub-TLV type %d.\n", type);
+        }
+
+        i += len + 2;
+    }
+    return ret;
+}
+
 static int
 network_address(int ae, const unsigned char *a, unsigned int len,
                 unsigned char *a_r)
@@ -194,6 +236,9 @@ parse_packet(const unsigned char *from, struct interface *ifp,
     unsigned char router_id[8], v4_prefix[16], v6_prefix[16],
         v4_nh[16], v6_nh[16];
 
+    /* We want to track exactly when we received this packet. */
+    gettime(&now);
+
     if(!linklocal(from)) {
         fprintf(stderr, "Received packet from non-local address %s.\n",
                 format_address(from));
@@ -276,6 +321,9 @@ parse_packet(const unsigned char *from, struct interface *ifp,
             if(interval > 0)
                 /* Multiply by 3/2 to allow hellos to expire. */
                 schedule_neighbours_check(interval * 15, 0);
+            /* Sub-TLV handling. */
+            if(len > 8)
+                parse_hello_subtlv(message + 8, len - 6, neigh);
         } else if(type == MESSAGE_IHU) {
             unsigned short txcost, interval;
             unsigned char address[16];

neighbour.c

@@ -97,6 +97,8 @@ find_neighbour(const unsigned char *address, struct interface *ifp)
     neigh->hello_time = zero;
     neigh->hello_interval = 0;
     neigh->ihu_interval = 0;
+    neigh->hello_send_us = 0;
+    neigh->hello_rtt_receive_time = zero;
     neigh->ifp = ifp;
     neigh->next = neighs;
     neighs = neigh;

neighbour.h

@@ -31,6 +31,11 @@ struct neighbour {
     struct timeval ihu_time;
     unsigned short hello_interval; /* in centiseconds */
     unsigned short ihu_interval;   /* in centiseconds */
+    /* Used for RTT estimation. */
+    /* Absolute time (modulo 2^32) at which the Hello was sent,
+       according to remote clock. */
+    unsigned int hello_send_us;
+    struct timeval hello_rtt_receive_time;
     struct interface *ifp;
 };