From e35d6a4fde9a0671bc7d2527ff6b55b0ce1b4b1e Mon Sep 17 00:00:00 2001
From: dzwdz
Date: Sun, 21 Aug 2022 22:33:09 +0200
Subject: user: rename ethdump to netstack
---
src/user/app/netstack/arp.c | 48 +++++++++
src/user/app/netstack/ether.c | 59 +++++++++++
src/user/app/netstack/fs.c | 154 ++++++++++++++++++++++++++++
src/user/app/netstack/icmp.c | 34 +++++++
src/user/app/netstack/ipv4.c | 215 +++++++++++++++++++++++++++++++++++++++
src/user/app/netstack/netstack.c | 43 ++++++++
src/user/app/netstack/proto.h | 70 +++++++++++++
src/user/app/netstack/udp.c | 98 ++++++++++++++++++
src/user/app/netstack/util.c | 31 ++++++
src/user/app/netstack/util.h | 38 +++++++
10 files changed, 790 insertions(+)
create mode 100644 src/user/app/netstack/arp.c
create mode 100644 src/user/app/netstack/ether.c
create mode 100644 src/user/app/netstack/fs.c
create mode 100644 src/user/app/netstack/icmp.c
create mode 100644 src/user/app/netstack/ipv4.c
create mode 100644 src/user/app/netstack/netstack.c
create mode 100644 src/user/app/netstack/proto.h
create mode 100644 src/user/app/netstack/udp.c
create mode 100644 src/user/app/netstack/util.c
create mode 100644 src/user/app/netstack/util.h
(limited to 'src/user/app/netstack')
diff --git a/src/user/app/netstack/arp.c b/src/user/app/netstack/arp.c
new file mode 100644
index 0000000..cfb1e04
--- /dev/null
+++ b/src/user/app/netstack/arp.c
@@ -0,0 +1,48 @@
+#include "proto.h"
+#include "util.h"
+#include <string.h>
+
+enum {
+ HdrType = 0,
+ HdrTypeEther = 1,
+ ProtoType = 2,
+ HdrALen = 4,
+ ProtoALen = 5,
+ Operation = 6,
+ OpReq = 1,
+ OpReply = 2,
+};
+
+void arp_parse(const uint8_t *buf, size_t len) {
+ // TODO no bound checks
+ uint16_t htype = nget16(buf + HdrType);
+ uint16_t ptype = nget16(buf + ProtoType);
+ uint16_t op = nget16(buf + Operation);
+
+ if (!(htype == HdrTypeEther && ptype == ET_IPv4)) return;
+ enum { /* only valid for this combination of header + proto */
+ SrcMAC = 8,
+ SrcIP = 14,
+ DstMAC = 18,
+ DstIP = 24,
+ };
+
+ if (op == OpReq) {
+ uint32_t daddr = nget32(buf + DstIP);
+ if (daddr == state.ip) {
+ uint8_t *pkt = ether_start(30, (struct ethernet){
+ .dst = buf + SrcMAC,
+ .type = ET_ARP,
+ });
+ nput16(pkt + HdrType, 1);
+ nput16(pkt + ProtoType, ET_IPv4);
+ pkt[HdrALen] = 6;
+ pkt[ProtoALen] = 4;
+ nput16(pkt + Operation, OpReply);
+ memcpy(pkt + SrcMAC, state.mac, 6);
+ nput32(pkt + SrcIP, state.ip);
+ memcpy(pkt + DstMAC, buf + SrcMAC, 10); /* sender's MAC and IP */
+ ether_finish(pkt);
+ }
+ }
+}
diff --git a/src/user/app/netstack/ether.c b/src/user/app/netstack/ether.c
new file mode 100644
index 0000000..5893632
--- /dev/null
+++ b/src/user/app/netstack/ether.c
@@ -0,0 +1,59 @@
+#include <camellia/syscalls.h>
+#include "proto.h"
+#include "util.h"
+
+enum {
+ DstMAC = 0,
+ SrcMAC = 6,
+ EtherType = 12,
+ Payload = 14,
+};
+struct ethq *ether_queue;
+
+void ether_parse(const uint8_t *buf, size_t len) {
+ struct ethernet ether = (struct ethernet){
+ .src = buf + SrcMAC,
+ .dst = buf + DstMAC,
+ .type = nget16(buf + EtherType),
+ };
+
+ for (struct ethq **iter = ðer_queue; iter && *iter; ) {
+ struct ethq *qe = *iter;
+ _syscall_fs_respond(qe->h, buf, len, 0);
+ /* remove entry */
+ /* yes, doing it this way here doesn't make sense. i'm preparing for filtering */
+ *iter = qe->next;
+ free(qe);
+ }
+
+ switch (ether.type) {
+ case ET_IPv4:
+ ipv4_parse(buf + Payload, len - Payload, ether);
+ break;
+ case ET_ARP:
+ arp_parse(buf + Payload, len - Payload);
+ break;
+ }
+}
+
+static const size_t fhoff = sizeof(size_t);
+uint8_t *ether_start(size_t len, struct ethernet ether) {
+ if (len < 60 - Payload) len = 60 - Payload;
+
+ if (!ether.dst) eprintf("NULL ether.dst!"); // TODO arp? i guess?
+ if (!ether.src) ether.src = &state.mac;
+
+ uint8_t *buf = malloc(fhoff + Payload + len);
+ memset(buf, 0, fhoff + Payload + len);
+ *(size_t*)buf = len + Payload;
+ memcpy(buf + fhoff + DstMAC, ether.dst, 6);
+ memcpy(buf + fhoff + SrcMAC, ether.src, 6);
+ nput16(buf + fhoff + EtherType, ether.type);
+ return buf + fhoff + Payload;
+}
+void ether_finish(uint8_t *pkt) {
+ uint8_t *buf = pkt - Payload - fhoff;
+ size_t len = *(size_t*)buf;
+ _syscall_write(state.raw_h, buf + fhoff, len, 0, 0);
+ free(buf);
+}
diff --git a/src/user/app/netstack/fs.c b/src/user/app/netstack/fs.c
new file mode 100644
index 0000000..b8c2f7b
--- /dev/null
+++ b/src/user/app/netstack/fs.c
@@ -0,0 +1,154 @@
+#include "proto.h"
+#include <camellia/syscalls.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <user/lib/fs/dir.h>
+
+enum handle_type {
+ H_ROOT,
+ H_ETHER,
+ H_UDP,
+};
+
+struct strqueue {
+ struct strqueue *next;
+ size_t len;
+ char buf[];
+};
+
+struct handle {
+ enum handle_type type;
+ struct {
+ struct udp_conn *c;
+ struct strqueue *rx, *rxlast;
+ } udp;
+ handle_t reqh;
+};
+
+
+static void udp_listen_callback(struct udp_conn *c, void *arg) {
+ struct handle *h = arg;
+ h->udp.c = c;
+ h->udp.rx = NULL;
+ h->udp.rxlast = NULL;
+ _syscall_fs_respond(h->reqh, h, 0, 0);
+ h->reqh = -1;
+}
+
+static void udp_recv_callback(const void *buf, size_t len, void *arg) {
+ struct handle *h = arg;
+ if (h->reqh >= 0) {
+ _syscall_fs_respond(h->reqh, buf, len, 0);
+ h->reqh = -1;
+ return;
+ }
+ // TODO don't malloc on the network thread, dumbass
+ struct strqueue *sq = malloc(sizeof(*sq) + len);
+ sq->next = NULL;
+ sq->len = len;
+ memcpy(sq->buf, buf, len);
+ if (h->udp.rx) {
+ h->udp.rxlast->next = sq;
+ h->udp.rxlast = sq;
+ } else {
+ h->udp.rx = sq;
+ h->udp.rxlast = sq;
+ }
+}
+
+static void udp_recv_enqueue(struct handle *h, handle_t reqh) {
+ if (h->reqh > 0) {
+ // TODO queue
+ _syscall_fs_respond(reqh, NULL, -1, 0);
+ } else if (h->udp.rx) {
+ _syscall_fs_respond(reqh, h->udp.rx->buf, h->udp.rx->len, 0);
+ h->udp.rx = h->udp.rx->next;
+ free(h->udp.rx);
+ } else {
+ h->reqh = reqh;
+ }
+}
+
+void fs_thread(void *arg) { (void)arg;
+ const size_t buflen = 4096;
+ char *buf = malloc(buflen);
+ for (;;) {
+ struct fs_wait_response res;
+ handle_t reqh = _syscall_fs_wait(buf, buflen, &res);
+ if (reqh < 0) break;
+ struct handle *h = res.id;
+ long ret;
+ switch (res.op) {
+ case VFSOP_OPEN:
+ if (res.len == 1 && !memcmp("/", buf, 1)) {
+ h = malloc(sizeof *h);
+ h->type = H_ROOT;
+ _syscall_fs_respond(reqh, h, 0, 0);
+ } else if (res.len == 4 && !memcmp("/raw", buf, 4)) {
+ h = malloc(sizeof *h);
+ h->type = H_ETHER;
+ _syscall_fs_respond(reqh, h, 0, 0);
+ } else if (res.len > 6 && !memcmp("/udpl/", buf, 6)) {
+ uint16_t port = strtol(buf + 6, NULL, 0);
+ h = malloc(sizeof *h);
+ h->type = H_UDP;
+ h->udp.c = NULL;
+ h->reqh = reqh;
+ udp_listen(port, udp_listen_callback, udp_recv_callback, h);
+ } else {
+ _syscall_fs_respond(reqh, NULL, -1, 0);
+ }
+ break;
+ case VFSOP_READ:
+ switch (h->type) {
+ case H_ROOT: {
+ struct dirbuild db;
+ dir_start(&db, res.offset, buf, sizeof buf);
+ dir_append(&db, "raw");
+ dir_append(&db, "udpl/");
+ _syscall_fs_respond(reqh, buf, dir_finish(&db), 0);
+ break;}
+ case H_ETHER: {
+ struct ethq *qe;
+ qe = malloc(sizeof *qe);
+ qe->h = reqh;
+ qe->next = ether_queue;
+ ether_queue = qe;
+ break;}
+ case H_UDP:
+ udp_recv_enqueue(h, reqh);
+ break;
+ default:
+ _syscall_fs_respond(reqh, NULL, -1, 0);
+ }
+ break;
+ case VFSOP_WRITE:
+ switch (h->type) {
+ case H_ETHER:
+ ret = _syscall_write(state.raw_h, buf, res.len, 0, 0);
+ _syscall_fs_respond(reqh, NULL, ret, 0);
+ break;
+ case H_UDP:
+ udpc_send(h->udp.c, buf, res.len);
+ _syscall_fs_respond(reqh, NULL, res.len, 0);
+ break;
+ default:
+ _syscall_fs_respond(reqh, NULL, -1, 0);
+ }
+ break;
+ case VFSOP_CLOSE:
+ // TODO remove entries in queue
+ // TODO why does close even have _syscall_fs_respond?
+ if (h->type == H_UDP)
+ udpc_close(h->udp.c);
+ free(h);
+ _syscall_fs_respond(reqh, NULL, -1, 0);
+ break;
+ default:
+ _syscall_fs_respond(reqh, NULL, -1, 0);
+ break;
+ }
+ }
+ free(buf);
+}
diff --git a/src/user/app/netstack/icmp.c b/src/user/app/netstack/icmp.c
new file mode 100644
index 0000000..0c6a502
--- /dev/null
+++ b/src/user/app/netstack/icmp.c
@@ -0,0 +1,34 @@
+#include "proto.h"
+#include "util.h"
+
+enum {
+ Type = 0,
+ Code = 1,
+ Checksum = 2,
+ Payload = 4,
+};
+
+void icmp_parse(const uint8_t *buf, size_t len, struct ipv4 ip) {
+ if (len < Payload) return;
+ uint8_t type = buf[Type];
+ if (type == 8 && ip.dst == state.ip) {
+ /* echo reply */
+ icmp_send(buf + Payload, len - Payload, (struct icmp){
+ .type = 0,
+ .ip.dst = ip.src,
+ .ip.e.dst = ip.e.src,
+ });
+ }
+}
+
+void icmp_send(const void *payload, size_t len, struct icmp i) {
+ i.ip.proto = 1;
+ uint8_t *pkt = malloc(Payload + len);
+ pkt[Type] = i.type;
+ pkt[Code] = i.code;
+ memcpy(pkt + Payload, payload, len);
+ nput16(pkt + Checksum, 0);
+ nput16(pkt + Checksum, ip_checksum(pkt, Payload + len));
+ ipv4_send(pkt, Payload + len, i.ip);
+ free(pkt);
+}
diff --git a/src/user/app/netstack/ipv4.c b/src/user/app/netstack/ipv4.c
new file mode 100644
index 0000000..3243d3c
--- /dev/null
+++ b/src/user/app/netstack/ipv4.c
@@ -0,0 +1,215 @@
+#include "proto.h"
+#include "util.h"
+#include <assert.h>
+#include <stdlib.h>
+
+enum {
+ Version = 0,
+ HdrLen = 0,
+ TotalLen = 2,
+ Id = 4,
+ FragInfo = 6,
+ EvilBit = 0x8000,
+ DontFrag = 0x4000,
+ MoreFrags = 0x2000,
+ FragOff = 0x1FFF,
+ TTL = 8,
+ Proto = 9,
+ Checksum = 10,
+ SrcIP = 12,
+ DstIP = 16,
+};
+static void ipv4_dispatch(const uint8_t *buf, size_t len, struct ipv4 ip);
+
+struct fragment {
+ struct fragment *next; /* sorted */
+ size_t len, offset;
+ bool last;
+ uint8_t buf[];
+};
+struct fragmented {
+ /* src, dst, proto, id come from the first arrived packet
+ * and are used to tell fragmenteds apart.
+ * the rest comes from the sequentially first packet.
+ * ip.h.header points to a malloc'd buffer*/
+ struct ipv4 h;
+
+ struct fragment *first;
+ struct fragmented *next, **prev; /* *(inc->prev) == inc */
+ // TODO timer
+};
+struct fragmented *fragmenteds;
+static struct fragmented *fragmented_find(struct ipv4 fraghdr);
+static void fragmented_tryinsert(const uint8_t *payload, size_t plen, struct ipv4 ip);
+static void fragmented_tryfinish(struct fragmented *inc);
+static void fragmented_free(struct fragmented *inc);
+
+static struct fragmented *fragmented_find(struct ipv4 fraghdr) {
+ struct fragmented *inc;
+ for (inc = fragmenteds; inc; inc = inc->next) {
+ if (inc->h.src == fraghdr.src &&
+ inc->h.dst == fraghdr.dst &&
+ inc->h.proto == fraghdr.proto &&
+ inc->h.id == fraghdr.id)
+ {
+ return inc;
+ }
+ }
+ inc = malloc(sizeof *inc);
+ memset(inc, 0, sizeof *inc);
+ inc->h.src = fraghdr.src;
+ inc->h.dst = fraghdr.dst;
+ inc->h.proto = fraghdr.proto;
+ inc->h.id = fraghdr.id;
+
+ inc->next = fragmenteds;
+ if (inc->next) inc->next->prev = &inc->next;
+ inc->prev = &fragmenteds;
+ *inc->prev = inc;
+ return inc;
+}
+
+static void fragmented_tryinsert(const uint8_t *payload, size_t plen, struct ipv4 ip) {
+ struct fragmented *inc = fragmented_find(ip);
+ size_t off = (ip.fraginfo & FragOff) * 8;
+ bool last = !(ip.fraginfo & MoreFrags);
+ // eprintf("fragmented packet, %u + %u, part of 0x%x", off, plen, inc);
+
+ /* find the first fragment at a bigger offset, and insert before it */
+ struct fragment **insert = &inc->first;
+ for (; *insert; insert = &(*insert)->next) {
+ if ((*insert)->offset > off) break;
+ if ((*insert)->offset == off) return; /* duplicate packet */
+ }
+ /* later on: frag->next = *insert;
+ * if we're the last fragment, frag->next must == NULL */
+ if (last && *insert != NULL) return;
+
+ struct fragment *frag = malloc(sizeof(struct fragment) + plen);
+ frag->next = *insert;
+ *insert = frag;
+ frag->len = plen;
+ frag->offset = off;
+ frag->last = last;
+ memcpy(frag->buf, payload, plen);
+
+ if (off == 0) { /* save header */
+ assert(!inc->h.header);
+ void *headercpy = malloc(ip.hlen);
+ memcpy(headercpy, ip.header, ip.hlen);
+ inc->h = ip;
+ inc->h.header = headercpy;
+ }
+
+ fragmented_tryfinish(inc);
+}
+
+static void fragmented_tryfinish(struct fragmented *inc) {
+ if (inc->first->offset != 0) return;
+ for (struct fragment *iter = inc->first; iter; iter = iter->next) {
+ size_t iterend = iter->offset + iter->len;
+ struct fragment *next = iter->next;
+ if (next) {
+ if (iterend < next->offset) return; /* incomplete */
+ if (iterend > next->offset) {
+ fragmented_free(inc);
+ return;
+ }
+ } else if (iter->last) {
+ void *buf = malloc(iterend);
+ for (struct fragment *iter = inc->first; iter; iter = iter->next) {
+ assert(iter->offset + iter->len <= iterend);
+ memcpy(buf + iter->offset, iter->buf, iter->len);
+ }
+ ipv4_dispatch(buf, iterend, inc->h);
+ free(buf);
+ fragmented_free(inc);
+ }
+ }
+}
+
+static void fragmented_free(struct fragmented *inc) {
+ if (inc->next) {
+ inc->next->prev = inc->prev;
+ *inc->next->prev = inc->next;
+ } else {
+ *inc->prev = NULL;
+ }
+
+ for (struct fragment *next, *iter = inc->first; iter; iter = next) {
+ next = iter->next;
+ free(iter);
+ }
+ free((void*)inc->h.header);
+ free(inc);
+}
+
+
+static void ipv4_dispatch(const uint8_t *buf, size_t len, struct ipv4 ip) {
+ switch (ip.proto) {
+ case 0x01: icmp_parse(buf, len, ip); break;
+ case 0x11: udp_parse(buf, len, ip); break;
+ }
+}
+
+void ipv4_parse(const uint8_t *buf, size_t len, struct ethernet ether) {
+ uint8_t version, headerlen;
+ uint16_t totallen;
+
+ version = buf[Version] >> 4;
+ if (version != 4) return;
+ headerlen = (buf[HdrLen] & 0xf) * 4;
+ totallen = nget16(buf + TotalLen);
+ if (totallen < headerlen) return;
+
+ /* ignores checksum. TODO? */
+
+ struct ipv4 ip = (struct ipv4){
+ .e = ether,
+ .src = nget32(buf + SrcIP),
+ .dst = nget32(buf + DstIP),
+ .id = nget16(buf + Id),
+ .fraginfo = nget16(buf + FragInfo),
+ .proto = buf[Proto],
+ .header = buf,
+ .hlen = headerlen,
+ };
+
+ if (ip.fraginfo & ~(EvilBit | DontFrag)) {
+ fragmented_tryinsert(buf + headerlen, totallen - headerlen, ip);
+ } else {
+ if (totallen > len) return;
+ ipv4_dispatch(buf + headerlen, totallen - headerlen, ip);
+ }
+}
+
+static uint16_t next_id = 0;
+void ipv4_send(const void *payload, size_t len, struct ipv4 ip) {
+ const size_t mtu = 1500;
+ const size_t hdrlen = 20;
+
+ ip.e.type = ET_IPv4;
+ if (!ip.src) ip.src = state.ip;
+ if (!ip.e.dst && ip.dst == 0xFFFFFFFF)
+ ip.e.dst = &MAC_BROADCAST;
+
+ uint16_t id = next_id++;
+ for (size_t off = 0, fraglen = mtu - hdrlen; off < len; off += fraglen) {
+ if (fraglen > len - off)
+ fraglen = len - off;
+ bool last = off + fraglen >= len;
+ uint8_t *pkt = ether_start(hdrlen + fraglen, ip.e);
+ pkt[Version] = 0x40;
+ pkt[HdrLen] |= hdrlen / 4;
+ nput16(pkt + TotalLen, hdrlen + fraglen);
+ nput16(pkt + Id, id);
+ nput16(pkt + FragInfo, (off >> 3) | (last ? 0 : MoreFrags));
+ pkt[TTL] = 0xFF;
+ pkt[Proto] = ip.proto;
+ nput32(pkt + SrcIP, ip.src);
+ nput32(pkt + DstIP, ip.dst);
+ nput16(pkt + Checksum, ip_checksum(pkt, hdrlen));
+ memcpy(pkt + hdrlen, payload + off, fraglen);
+ ether_finish(pkt);
+ }
+}
diff --git a/src/user/app/netstack/netstack.c b/src/user/app/netstack/netstack.c
new file mode 100644
index 0000000..0451511
--- /dev/null
+++ b/src/user/app/netstack/netstack.c
@@ -0,0 +1,43 @@
+#include "proto.h"
+#include "util.h"
+#include <camellia/syscalls.h>
+#include <stdbool.h>
+#include <stddef.h>
+#include <stdlib.h>
+#include <string.h>
+#include <user/lib/thread.h>
+
+struct net_state state = {
+ // TODO dynamically get mac
+ .mac = {0x52, 0x54, 0x00, 0xCA, 0x77, 0x1A},
+ .ip = (192 << 24) + (168 << 16) + 11,
+};
+
+void network_thread(void *arg) { (void)arg;
+ const size_t buflen = 4096;
+ char *buf = malloc(buflen);
+ for (;;) {
+ long ret = _syscall_read(state.raw_h, buf, buflen, -1);
+ if (ret < 0) break;
+ ether_parse((void*)buf, ret);
+ }
+ free(buf);
+}
+
+void fs_thread(void *arg);
+
+int main(int argc, char **argv) {
+ if (argc < 2) {
+ eprintf("no argument");
+ return 1;
+ }
+ state.raw_h = _syscall_open(argv[1], strlen(argv[1]), 0);
+ if (state.raw_h < 0) {
+ eprintf("couldn't open %s", argv[1]);
+ return 1;
+ }
+ thread_create(0, network_thread, NULL);
+ thread_create(0, fs_thread, NULL);
+ _syscall_await();
+ return 0;
+}
diff --git a/src/user/app/netstack/proto.h b/src/user/app/netstack/proto.h
new file mode 100644
index 0000000..106f286
--- /dev/null
+++ b/src/user/app/netstack/proto.h
@@ -0,0 +1,70 @@
+#pragma once
+#include <camellia/types.h>
+#include <stdbool.h>
+#include <stdint.h>
+
+typedef uint8_t mac_t[6];
+static const mac_t MAC_BROADCAST = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF};
+
+extern struct net_state {
+ mac_t mac;
+ uint32_t ip;
+
+ handle_t raw_h;
+} state;
+
+enum { /* ethertype */
+ ET_IPv4 = 0x800,
+ ET_ARP = 0x806,
+};
+
+struct ethernet {
+ const mac_t *src, *dst;
+ uint16_t type;
+};
+
+struct ipv4 {
+ struct ethernet e;
+ uint32_t src, dst;
+ uint16_t id, fraginfo;
+ uint8_t proto;
+ const uint8_t *header; size_t hlen;
+};
+
+struct icmp {
+ struct ipv4 ip;
+ uint8_t type, code;
+};
+
+
+/* NOT THREADSAFE, YET USED FROM CONCURRENT THREADS
+ * will break if i implement a scheduler*/
+struct ethq {
+ struct ethq *next;
+ handle_t h;
+};
+extern struct ethq *ether_queue;
+
+void arp_parse(const uint8_t *buf, size_t len);
+
+void icmp_parse(const uint8_t *buf, size_t len, struct ipv4 ip);
+void icmp_send(const void *payload, size_t len, struct icmp i);
+
+void ipv4_parse(const uint8_t *buf, size_t len, struct ethernet ether);
+void ipv4_send(const void *payload, size_t len, struct ipv4 ip);
+
+void ether_parse(const uint8_t *buf, size_t len);
+uint8_t *ether_start(size_t len, struct ethernet ether);
+void ether_finish(uint8_t *pkt);
+
+struct udp_conn;
+void udp_parse(const uint8_t *buf, size_t len, struct ipv4 ip);
+/* calls callback once, after a client connects. */
+void udp_listen(uint16_t port,
+ void (*on_conn)(struct udp_conn *, void *carg),
+ void (*on_recv)(const void *, size_t, void *carg),
+ void *carg);
+// TODO udp_onclosed
+void udpc_send(struct udp_conn *, const void *buf, size_t len);
+/* frees */
+void udpc_close(struct udp_conn *);
diff --git a/src/user/app/netstack/udp.c b/src/user/app/netstack/udp.c
new file mode 100644
index 0000000..8129a48
--- /dev/null
+++ b/src/user/app/netstack/udp.c
@@ -0,0 +1,98 @@
+#include "proto.h"
+#include "util.h"
+
+enum {
+ SrcPort = 0,
+ DstPort = 2,
+ Length = 4,
+ Checksum = 6,
+ Payload = 8,
+};
+
+
+struct udp_conn {
+ uint32_t lip, rip;
+ uint16_t lport, rport;
+ mac_t rmac;
+ void (*on_conn)(struct udp_conn *, void *); /* if non-NULL - listening, if NULL - estabilished */
+ void (*on_recv)(const void *, size_t, void *);
+ void *carg;
+ struct udp_conn *next, **prev;
+};
+struct udp_conn *conns;
+void udp_listen(uint16_t port,
+ void (*on_conn)(struct udp_conn *, void *carg),
+ void (*on_recv)(const void *, size_t, void *carg),
+ void *carg)
+{
+ if (!on_conn) return;
+ struct udp_conn *c = malloc(sizeof *c);
+ c->lport = port;
+ c->lip = state.ip;
+ c->on_conn = on_conn;
+ c->on_recv = on_recv;
+ c->carg = carg;
+
+ c->next = conns;
+ if (c->next) *(c->next->prev) = c;
+ c->prev = &conns;
+ *c->prev = c;
+}
+void udpc_close(struct udp_conn *c) {
+ if (c->next) c->next->prev = c->prev;
+ *(c->prev) = c->next;
+ free(c);
+}
+void udpc_send(struct udp_conn *c, const void *buf, size_t len) {
+ uint8_t *pkt = malloc(Payload + len);
+ nput16(pkt + SrcPort, c->lport);
+ nput16(pkt + DstPort, c->rport);
+ nput16(pkt + Length, Payload + len);
+ nput16(pkt + Checksum, 0);
+ memcpy(pkt + Payload, buf, len);
+ ipv4_send(pkt, Payload + len, (struct ipv4){
+ .proto = 0x11,
+ .src = c->lip,
+ .dst = c->rip,
+ .e.dst = c->rmac,
+ });
+ free(pkt);
+}
+
+
+void udp_parse(const uint8_t *buf, size_t len, struct ipv4 ip) {
+ uint16_t srcport = nget16(buf + SrcPort);
+ uint16_t dstport = nget16(buf + DstPort);
+ bool active = false;
+
+ for (struct udp_conn *iter = conns; iter; iter = iter->next) {
+ if (iter->on_conn && dstport == iter->lport) {
+ iter->on_conn(iter, iter->carg);
+ iter->on_conn = NULL;
+ iter->rport = srcport;
+ memcpy(&iter->rmac, ip.e.src, sizeof(mac_t));
+ iter->rip = ip.src;
+ }
+ if (iter->rip == ip.src
+ && iter->rport == srcport
+ && iter->lport == dstport
+ && iter->on_recv)
+ {
+ active = true;
+ iter->on_recv(buf + Payload, len - Payload, iter->carg);
+ }
+ }
+
+ if (!active) {
+ uint8_t *pkt = malloc(4 + ip.hlen + 8);
+ nput32(pkt, 0);
+ memcpy(pkt + 4, ip.header, ip.hlen + 8);
+ icmp_send(pkt, 4 + ip.hlen + 8, (struct icmp){
+ .type = 3, /* destination unreachable */
+ .code = 3, /* port unreachable */
+ .ip.dst = ip.src,
+ .ip.e.dst = ip.e.src,
+ });
+ free(pkt);
+ }
+}
diff --git a/src/user/app/netstack/util.c b/src/user/app/netstack/util.c
new file mode 100644
index 0000000..7c484f3
--- /dev/null
+++ b/src/user/app/netstack/util.c
@@ -0,0 +1,31 @@
+#include "util.h"
+
+/* https://www.w3.org/TR/PNG/#D-CRCAppendix */
+static uint32_t crc_table[256];
+uint32_t crc32(const uint8_t *buf, size_t len) {
+ if (!crc_table[1]) {
+ for (int i = 0; i < 256; i++) {
+ uint32_t c = i;
+ for (int j = 0; j < 8; j++)
+ c = ((c&1) ? 0xedb88320 : 0) ^ (c >> 1);
+ crc_table[i] = c;
+ }
+ }
+
+ uint32_t c = 0xFFFFFFFF;
+ for (size_t i = 0; i < len; i++)
+ c = crc_table[(c ^ buf[i]) & 0xff] ^ (c >> 8);
+ return ~c;
+}
+
+uint16_t ip_checksum(const uint8_t *buf, size_t len) {
+ uint32_t c = 0;
+ while (len >= 2) {
+ c += nget16(buf);
+ buf += 2; len -= 2;
+ }
+ if (len) c += (*buf) << 8;
+ while (c >= 0xFFFF)
+ c = (c & 0xFFFF) + (c >> 16);
+ return ~c;
+}
diff --git a/src/user/app/netstack/util.h b/src/user/app/netstack/util.h
new file mode 100644
index 0000000..5472b05
--- /dev/null
+++ b/src/user/app/netstack/util.h
@@ -0,0 +1,38 @@
+#pragma once
+#include <stdint.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+
+#define eprintf(fmt, ...) fprintf(stderr, "netstack: "fmt"\n" __VA_OPT__(,) __VA_ARGS__)
+
+uint32_t crc32(const uint8_t *buf, size_t len);
+uint16_t ip_checksum(const uint8_t *buf, size_t len);
+
+static inline void nput16(void *vbuf, uint16_t n) {
+ uint8_t *b = vbuf;
+ b[0] = n >> 8;
+ b[1] = n >> 0;
+}
+
+static inline void nput32(void *vbuf, uint32_t n) {
+ uint8_t *b = vbuf;
+ b[0] = n >> 24;
+ b[1] = n >> 16;
+ b[2] = n >> 8;
+ b[3] = n >> 0;
+}
+
+static inline uint16_t nget16(const void *vbuf) {
+ const uint8_t *b = vbuf;
+ return (b[0] << 8)
+ | (b[1] << 0);
+}
+
+static inline uint32_t nget32(const void *vbuf) {
+ const uint8_t *b = vbuf;
+ return (b[0] << 24)
+ | (b[1] << 16)
+ | (b[2] << 8)
+ | (b[3] << 0);
+}
--
cgit v1.2.3