net: IP stack milestone B - UDP echo (pseudo-header checksum)

Refactor netd into a protocol dispatcher (IP -> ICMP/UDP) and add UDP echo:
swap addresses + ports and recompute the UDP checksum over the pseudo-header
(src/dst IP + proto + length) plus the datagram. This is the same pseudo-
header TCP uses, so it de-risks the next milestone.

tools/gateway.py --mode udp sends a datagram and verifies the echo.
Verified: 'hello udp gbos' echoes back with cksum=ok.

1 files changed, 61 insertions, 30 deletions

diff --git a/c/netd.c b/c/netd.c
index 36435d4..bce6ea7 100644
--- a/c/netd.c
+++ b/c/netd.c
@@ -1,47 +1,78 @@
 #include "netlib.h"
-/* netd - a minimal IP stack over SLIP. Milestone A: answer ICMP echo (ping).
-   Our address is 10.0.0.2; the SLIP peer (gateway) is 10.0.0.1.
+/* netd - a minimal IP stack over SLIP. Our address is 10.0.0.2, peer 10.0.0.1.
+   Milestones: A) ICMP echo, B) UDP echo. Raw byte arrays, network byte order
+   (big-endian) handled explicitly. */
 
-   Everything is done on raw byte arrays so endianness is explicit (network
-   byte order = big-endian): field[hi], field[lo]. */
+static unsigned char pkt[256];
 
-/* Internet checksum (RFC 1071): 16-bit one's-complement sum with carry fold. */
-static unsigned int cksum(unsigned char *d, unsigned char len) {
-    unsigned int sum = 0, w;
-    unsigned char i = 0;
-    while ((unsigned char)(i + 1) < len) {
+/* one's-complement sum (RFC 1071) with a running seed, carry-folded each add */
+static unsigned int sum16(unsigned char *d, unsigned int len, unsigned int sum) {
+    unsigned int w, i = 0;
+    while (i + 1 < len) {
         w = ((unsigned int)d[i] << 8) | d[i + 1];
         sum += w; if (sum < w) sum++;
         i += 2;
     }
     if (len & 1) { w = (unsigned int)d[len - 1] << 8; sum += w; if (sum < w) sum++; }
-    return ~sum;
+    return sum;
 }
 
-void main(void) {
-    unsigned char pkt[128];
-    unsigned char len, ihl, i, t;
+/* pseudo-header sum for UDP/TCP: src+dst IP, protocol, segment length */
+static unsigned int pseudo(unsigned char proto, unsigned int seglen) {
+    unsigned int sum = sum16(pkt + 12, 8, 0), w;
+    w = proto;  sum += w; if (sum < w) sum++;
+    w = seglen; sum += w; if (sum < w) sum++;
+    return sum;
+}
+
+static void swap4(unsigned char a, unsigned char b) {
+    unsigned char i, t;
+    for (i = 0; i < 4; i++) { t = pkt[a + i]; pkt[a + i] = pkt[b + i]; pkt[b + i] = t; }
+}
+static void swap2(unsigned char a, unsigned char b) {
+    unsigned char t;
+    t = pkt[a]; pkt[a] = pkt[b]; pkt[b] = t;
+    t = pkt[a + 1]; pkt[a + 1] = pkt[b + 1]; pkt[b + 1] = t;
+}
+
+/* recompute the IP header checksum over ihl bytes, then send 'len' total */
+static void ip_send(unsigned char ihl, unsigned int len) {
     unsigned int c;
-    puts("netd: IP/ICMP up (10.0.0.2)"); nl();
+    pkt[10] = 0; pkt[11] = 0;
+    c = ~sum16(pkt, ihl, 0);
+    pkt[10] = (unsigned char)(c >> 8); pkt[11] = (unsigned char)c;
+    slip_send((char *)pkt, (unsigned char)len);
+}
+
+void main(void) {
+    unsigned char len, ihl;
+    unsigned int c, seglen;
+    puts("netd: IP up (10.0.0.2)"); nl();
     for (;;) {
-        len = slip_recv((char *)pkt, 127);
-        if (len < 28) continue;                 /* IP(20) + ICMP(8) minimum */
-        if ((pkt[0] >> 4) != 4) continue;       /* IPv4 only */
+        len = slip_recv((char *)pkt, 255);
+        if (len < 20 || (pkt[0] >> 4) != 4) continue;
         ihl = (pkt[0] & 0x0f) << 2;
-        if (pkt[9] != 1) continue;              /* protocol 1 = ICMP */
-        if (pkt[ihl] != 8) continue;            /* ICMP type 8 = echo request */
+        pkt[8] = 64;                                  /* TTL for any reply */
 
-        /* turn the request into a reply, in place */
-        for (i = 0; i < 4; i++) { t = pkt[12 + i]; pkt[12 + i] = pkt[16 + i]; pkt[16 + i] = t; }
-        pkt[8] = 64;                            /* reset TTL */
-        pkt[ihl] = 0;                           /* ICMP type -> echo reply */
-        pkt[ihl + 2] = 0; pkt[ihl + 3] = 0;     /* recompute ICMP checksum */
-        c = cksum(pkt + ihl, (unsigned char)(len - ihl));
-        pkt[ihl + 2] = (unsigned char)(c >> 8); pkt[ihl + 3] = (unsigned char)c;
-        pkt[10] = 0; pkt[11] = 0;               /* recompute IP header checksum */
-        c = cksum(pkt, ihl);
-        pkt[10] = (unsigned char)(c >> 8); pkt[11] = (unsigned char)c;
+        if (pkt[9] == 1) {                            /* ICMP */
+            if (len < (unsigned char)(ihl + 8) || pkt[ihl] != 8) continue;   /* echo request */
+            swap4(12, 16);
+            pkt[ihl] = 0;                             /* -> echo reply */
+            pkt[ihl + 2] = 0; pkt[ihl + 3] = 0;
+            c = ~sum16(pkt + ihl, (unsigned int)(len - ihl), 0);
+            pkt[ihl + 2] = (unsigned char)(c >> 8); pkt[ihl + 3] = (unsigned char)c;
+            ip_send(ihl, len);
 
-        slip_send((char *)pkt, len);
+        } else if (pkt[9] == 17) {                    /* UDP echo */
+            if (len < (unsigned char)(ihl + 8)) continue;
+            seglen = (unsigned int)(len - ihl);
+            swap4(12, 16);                            /* swap IP addresses */
+            swap2(ihl, ihl + 2);                      /* swap UDP ports */
+            pkt[ihl + 6] = 0; pkt[ihl + 7] = 0;
+            c = ~sum16(pkt + ihl, seglen, pseudo(17, seglen));
+            if (c == 0) c = 0xffff;                   /* UDP: 0 means "none" */
+            pkt[ihl + 6] = (unsigned char)(c >> 8); pkt[ihl + 7] = (unsigned char)c;
+            ip_send(ihl, len);
+        }
     }
 }