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.

2 files changed, 88 insertions, 31 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);
+        }
     }
 }
diff --git a/tools/gateway.py b/tools/gateway.py
index 590526e..f5fc701 100644
--- a/tools/gateway.py
+++ b/tools/gateway.py
@@ -126,9 +126,19 @@ def icmp_echo(seq, payload=b"gbos-ping-0123456789"):
     ip = ip[:10] + struct.pack("!H", inet_cksum(ip)) + ip[12:]
     return ip + icmp
 
+def udp_packet(sport, dport, payload):
+    ulen = 8 + len(payload)
+    udp = struct.pack("!HHHH", sport, dport, ulen, 0) + payload
+    pseudo = GW_IP + GB_IP + bytes([0, 17]) + struct.pack("!H", ulen)
+    ck = inet_cksum(pseudo + udp) or 0xffff
+    udp = udp[:6] + struct.pack("!H", ck) + udp[8:]
+    ip = struct.pack("!BBHHHBBH", 0x45, 0, 20+ulen, 7, 0, 64, 17, 0) + GW_IP + GB_IP
+    ip = ip[:10] + struct.pack("!H", inet_cksum(ip)) + ip[12:]
+    return ip + udp
+
 def main():
     ap = argparse.ArgumentParser()
-    ap.add_argument("--mode", default="echo", choices=["echo", "http", "chat", "irc", "ping"])
+    ap.add_argument("--mode", default="echo", choices=["echo", "http", "chat", "irc", "ping", "udp"])
     ap.add_argument("--cmd", default="necho")
     ap.add_argument("--nick", default="gameboy")
     ap.add_argument("--channel", default="#sl0p")
@@ -159,6 +169,17 @@ def main():
             m = bytes(payload).decode("latin1", "replace")
             sys.stderr.write("\n[gw] GB says: %s\n" % m)
             return [("<bot> " + m).encode()]
+    elif a.mode == "udp":
+        def on_frame(payload):
+            p = bytes(payload)
+            if len(p) >= 28 and p[9] == 17:
+                ihl = (p[0] & 0x0f) * 4
+                sport, dport, ulen = struct.unpack("!HHH", p[ihl:ihl+6])
+                data = p[ihl+8:ihl+8+ulen-8]
+                ck_ok = inet_cksum(GB_IP + GW_IP + bytes([0,17]) + p[ihl+4:ihl+6] + p[ihl:]) == 0
+                sys.stderr.write("\n[gw] UDP echo %d->%d: %r cksum=%s\n"
+                                 % (sport, dport, data, "ok" if ck_ok else "BAD"))
+            return []
     elif a.mode == "ping":
         def on_frame(payload):
             p = bytes(payload)
@@ -183,6 +204,11 @@ def main():
             except Exception: pass
         irc_holder[0] = IRC(a.host, a.port, a.nick, a.channel, to_gb)
         threading.Thread(target=irc_holder[0].loop, daemon=True).start()
+    if a.mode == "udp":
+        time.sleep(1.0)
+        p.stdin.write(slip_encode(udp_packet(5000, 7, b"hello udp gbos"))); p.stdin.flush()
+        sys.stderr.write("[gw] sent UDP :5000->:7 'hello udp gbos'\n")
+        time.sleep(2.0)
     if a.mode == "ping":
         time.sleep(1.0)
         for seq in range(1, 5):