#include "netlib.h"
/* 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. */

static unsigned char pkt[256];

/* 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;
}

/* 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;
    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, 255);
        if (len < 20 || (pkt[0] >> 4) != 4) continue;
        ihl = (pkt[0] & 0x0f) << 2;
        pkt[8] = 64;                                  /* TTL for any reply */

        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);

        } 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);
        }
    }
}