user/ethdump: UDP support

8 files changed, 289 insertions, 41 deletions

diff --git a/src/user/app/ethdump/ether.c b/src/user/app/ethdump/ether.c
index 3f6a40d..5893632 100644
--- a/src/user/app/ethdump/ether.c
+++ b/src/user/app/ethdump/ether.c
@@ -1,6 +1,4 @@
 #include <camellia/syscalls.h>
-#include <stdlib.h>
-#include <string.h>
 #include "proto.h"
 #include "util.h"
 
@@ -10,7 +8,7 @@ enum {
 	EtherType = 12,
 	Payload   = 14,
 };
-struct queue_entry *ether_queue;
+struct ethq *ether_queue;
 
 void ether_parse(const uint8_t *buf, size_t len) {
 	struct ethernet ether = (struct ethernet){
@@ -19,9 +17,8 @@ void ether_parse(const uint8_t *buf, size_t len) {
 		.type = nget16(buf + EtherType),
 	};
 
-	struct queue_entry **iter = &ether_queue;
-	while (iter && *iter) {
-		struct queue_entry *qe = *iter;
+	for (struct ethq **iter = &ether_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 */
diff --git a/src/user/app/ethdump/fs.c b/src/user/app/ethdump/fs.c
index 5078a13..b8c2f7b 100644
--- a/src/user/app/ethdump/fs.c
+++ b/src/user/app/ethdump/fs.c
@@ -1,63 +1,152 @@
 #include "proto.h"
 #include <camellia/syscalls.h>
+#include <stdio.h>
 #include <stdlib.h>
 #include <string.h>
 #include <user/lib/fs/dir.h>
 
-enum {
+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 h = _syscall_fs_wait(buf, buflen, &res);
-		if (h < 0) break;
+		handle_t reqh = _syscall_fs_wait(buf, buflen, &res);
+		if (reqh < 0) break;
+		struct handle *h = res.id;
+		long ret;
 		switch (res.op) {
-			long ret;
 			case VFSOP_OPEN:
-				ret = -1;
-				if (res.len < buflen) {
-					buf[res.len] = '\0';
-					if (!strcmp("/", buf)) ret = H_ROOT;
-					else if (!strcmp("/raw", buf)) ret = H_ETHER;
+				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);
 				}
-				_syscall_fs_respond(h, (void*)ret, ret, 0);
 				break;
 			case VFSOP_READ:
-				switch ((long)res.id) {
-					struct dirbuild db;
-					struct queue_entry *qe;
-					case H_ROOT:
+				switch (h->type) {
+					case H_ROOT: {
+						struct dirbuild db;
 						dir_start(&db, res.offset, buf, sizeof buf);
 						dir_append(&db, "raw");
-						_syscall_fs_respond(h, buf, dir_finish(&db), 0);
-						break;
-					case H_ETHER:
+						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 = h;
+						qe->h = reqh;
 						qe->next = ether_queue;
 						ether_queue = qe;
+						break;}
+					case H_UDP:
+						udp_recv_enqueue(h, reqh);
 						break;
 					default:
-						_syscall_fs_respond(h, NULL, -1, 0);
+						_syscall_fs_respond(reqh, NULL, -1, 0);
 				}
 				break;
 			case VFSOP_WRITE:
-				switch ((long)res.id) {
+				switch (h->type) {
 					case H_ETHER:
 						ret = _syscall_write(state.raw_h, buf, res.len, 0, 0);
-						_syscall_fs_respond(h, NULL, ret, 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(h, NULL, -1, 0);
+						_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(h, NULL, -1, 0);
+				_syscall_fs_respond(reqh, NULL, -1, 0);
 				break;
 		}
 	}
diff --git a/src/user/app/ethdump/icmp.c b/src/user/app/ethdump/icmp.c
index b3eff93..94f718e 100644
--- a/src/user/app/ethdump/icmp.c
+++ b/src/user/app/ethdump/icmp.c
@@ -1,6 +1,5 @@
 #include "proto.h"
 #include "util.h"
-#include <string.h>
 
 enum {
 	Type     = 0,
@@ -20,7 +19,7 @@ void icmp_parse(const uint8_t *buf, size_t len, struct ipv4 ip) {
 			.ip.e.dst = ip.e.src,
 		});
 		memcpy(pkt, buf + Payload, len - Payload);
-		icmp_finish(pkt);
+		icmp_finish(pkt, len - Payload);
 	}
 }
 
@@ -29,9 +28,10 @@ uint8_t *icmp_start(size_t len, struct icmp i) {
 	uint8_t *pkt = ipv4_start(Payload + len, i.ip);
 	pkt[Type] = i.type;
 	pkt[Code] = i.code;
-	nput16(pkt + Checksum, ip_checksum(pkt, Payload + len));
 	return pkt + Payload;
 }
-void icmp_finish(uint8_t *pkt) {
-	ipv4_finish(pkt - Payload);
+void icmp_finish(uint8_t *pkt, size_t len) {
+	pkt -= Payload;
+	nput16(pkt + Checksum, ip_checksum(pkt, Payload + len));
+	ipv4_finish(pkt);
 }
diff --git a/src/user/app/ethdump/ipv4.c b/src/user/app/ethdump/ipv4.c
index e8ab7ea..8b181f6 100644
--- a/src/user/app/ethdump/ipv4.c
+++ b/src/user/app/ethdump/ipv4.c
@@ -33,12 +33,17 @@ void ipv4_parse(const uint8_t *buf, size_t len, struct ethernet ether) {
 		.src = nget32(buf + SrcIP),
 		.dst = nget32(buf + DstIP),
 		.proto = buf[Proto],
+		.header = buf,
+		.hlen = headerlen,
 	};
 
 	switch (ip.proto) {
-		case 1:
+		case 0x01:
 			icmp_parse(buf + headerlen, packetlen - headerlen, ip);
 			break;
+		case 0x11:
+			udp_parse(buf + headerlen, packetlen - headerlen, ip);
+			break;
 	}
 }
 
diff --git a/src/user/app/ethdump/proto.h b/src/user/app/ethdump/proto.h
index 21057a2..9bd6936 100644
--- a/src/user/app/ethdump/proto.h
+++ b/src/user/app/ethdump/proto.h
@@ -1,5 +1,6 @@
 #pragma once
 #include <camellia/types.h>
+#include <stdbool.h>
 #include <stdint.h>
 
 typedef uint8_t mac_t[6];
@@ -26,6 +27,7 @@ struct ipv4 {
 	struct ethernet e;
 	uint32_t src, dst;
 	uint8_t proto;
+	uint8_t *header; size_t hlen;
 };
 
 struct icmp {
@@ -34,20 +36,19 @@ struct icmp {
 };
 
 
-/* NOT THREADSAFE, YET USED FROM THREADS
+/* NOT THREADSAFE, YET USED FROM CONCURRENT THREADS
  * will break if i implement a scheduler*/
-struct queue_entry {
+struct ethq {
+	struct ethq *next;
 	handle_t h;
-	struct queue_entry *next;
 };
-extern struct queue_entry *ether_queue;
-
+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);
 uint8_t *icmp_start(size_t len, struct icmp i);
-void icmp_finish(uint8_t *pkt);
+void icmp_finish(uint8_t *pkt, size_t len);
 
 void ipv4_parse(const uint8_t *buf, size_t len, struct ethernet ether);
 uint8_t *ipv4_start(size_t len, struct ipv4 ip);
@@ -56,3 +57,15 @@ void ipv4_finish(uint8_t *pkt);
 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/ethdump/udp.c b/src/user/app/ethdump/udp.c
new file mode 100644
index 0000000..328f12b
--- /dev/null
+++ b/src/user/app/ethdump/udp.c
@@ -0,0 +1,95 @@
+#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 = ipv4_start(Payload + len, (struct ipv4){
+		.proto = 0x11,
+		.src = c->lip,
+		.dst = c->rip,
+		.e.dst = c->rmac,
+	});
+	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_finish(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 = icmp_start(4 + ip.hlen + 8, (struct icmp){
+			.type = 3, /* destination unreachable */
+			.code = 3, /* port unreachable */
+			.ip.dst = ip.src,
+			.ip.e.dst = ip.e.src,
+		});
+		memcpy(pkt + 4, ip.header, ip.hlen + 8);
+		icmp_finish(pkt, 4 + ip.hlen + 8);
+	}
+}
diff --git a/src/user/app/ethdump/util.h b/src/user/app/ethdump/util.h
index 6f58b82..bfff6e3 100644
--- a/src/user/app/ethdump/util.h
+++ b/src/user/app/ethdump/util.h
@@ -1,6 +1,8 @@
 #pragma once
 #include <stdint.h>
 #include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
 
 #define eprintf(fmt, ...) fprintf(stderr, "ethdump: "fmt"\n" __VA_OPT__(,) __VA_ARGS__)
 
diff --git a/src/user/app/netdog/nd.c b/src/user/app/netdog/nd.c
new file mode 100644
index 0000000..363c6c6
--- /dev/null
+++ b/src/user/app/netdog/nd.c
@@ -0,0 +1,47 @@
+#include <camellia/syscalls.h>
+#include <stdio.h>
+#include <string.h>
+#include <user/lib/thread.h>
+
+#define eprintf(fmt, ...) fprintf(stderr, "netdog: "fmt"\n" __VA_OPT__(,) __VA_ARGS__)
+
+handle_t conn;
+
+void send_stdin(void *arg) { (void)arg;
+	static char buf[4096];
+	for (;;) {
+		// TODO define STDIN_FILENO
+		long ret = _syscall_read(0, buf, sizeof buf, -1);
+		if (ret <= 0) return; /* instead of sending an empty packet, quit. */
+		ret = _syscall_write(conn, buf, ret, -1, 0);
+		if (ret < 0) return;
+	}
+}
+
+void recv_stdout(void *arg) { (void)arg;
+	static char buf[4096];
+	for (;;) {
+		long ret = _syscall_read(conn, buf, sizeof buf, -1);
+		if (ret < 0) return;
+		ret = _syscall_write(1, buf, ret, -1, 0);
+		if (ret < 0) return;
+	}
+}
+
+int main(int argc, char **argv) {
+	if (argc < 2) {
+		eprintf("no argument");
+		return 1;
+	}
+
+	conn = _syscall_open(argv[1], strlen(argv[1]), 0);
+	if (conn < 0) {
+		eprintf("couldn't open '%s', err %u", argv[1], -conn);
+		return -conn;
+	}
+
+	thread_create(0, send_stdin, NULL);
+	thread_create(0, recv_stdout, NULL);
+	_syscall_await();
+	return 0;
+}