user/ethdump: file per protocol, ethernet frame functions

8 files changed, 289 insertions, 196 deletions

diff --git a/src/user/app/ethdump/arp.c b/src/user/app/ethdump/arp.c
new file mode 100644
index 0000000..a022b12
--- /dev/null
+++ b/src/user/app/ethdump/arp.c
@@ -0,0 +1,56 @@
+#include "proto.h"
+#include "util.h"
+#include <stdlib.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);
+
+	const char *ops = "bad operation";
+	if (op == 1) ops = "request";
+	if (op == 2) ops = "reply";
+
+	printf("ARP htype 0x%x, ptype 0x%x, %s\n", htype, ptype, ops);
+
+	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);
+		printf("IPv4 request for %08x\n", daddr);
+		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/ethdump/ethdump.c b/src/user/app/ethdump/ethdump.c
index 8d5be34..4047962 100644
--- a/src/user/app/ethdump/ethdump.c
+++ b/src/user/app/ethdump/ethdump.c
@@ -1,206 +1,21 @@
+#include "proto.h"
+#include "util.h"
 #include <camellia/syscalls.h>
 #include <stdbool.h>
 #include <stddef.h>
-#include <stdio.h>
 #include <stdlib.h>
 #include <string.h>
 
-#define eprintf(fmt, ...) fprintf(stderr, "ethdump: "fmt"\n" __VA_OPT__(,) __VA_ARGS__)
-
-handle_t eth_h;
-
-// TODO dynamically get mac
-uint8_t my_mac[] = {0x52, 0x54, 0x00, 0xCA, 0x77, 0x1A};
-uint32_t my_ip = (192 << 24) + (168 << 16) + 11;
-
-static void hexdump(const void *vbuf, size_t len) {
-	const uint8_t *buf = vbuf;
-	for (size_t i = 0; i < len; i += 16) {
-		printf("%08x  ", i);
-
-		for (size_t j = i; j < i + 8 && j < len; j++)
-			printf("%02x ", buf[j]);
-		printf(" ");
-		for (size_t j = i + 8; j < i + 16 && j < len; j++)
-			printf("%02x ", buf[j]);
-		printf("\n");
-	}
-}
-
-static void nput16(void *vbuf, uint16_t n) {
-	uint8_t *b = vbuf;
-	b[0] = n >> 8;
-	b[1] = n >> 0;
-}
-
-static 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 uint16_t nget16(const void *vbuf) {
-	const uint8_t *b = vbuf;
-	return (b[0] << 8)
-	     | (b[1] << 0);
-}
-
-static uint32_t nget32(const void *vbuf) {
-	const uint8_t *b = vbuf;
-	return (b[0] << 24)
-	     | (b[1] << 16)
-	     | (b[2] <<  8)
-	     | (b[3] <<  0);
-}
-
-
-static void parse_icmp(const uint8_t *buf, size_t len) {
-	if (len < 4) return;
-	uint8_t type = buf[0];
-	printf("ICMP type %u\n", type);
-}
-
-
-static void parse_ipv4(const uint8_t *buf, size_t len) {
-	uint8_t version, headerlen, proto;
-	uint16_t packetlen, id, fragoffset;
-	uint32_t dest, src;
-
-	version   = buf[0] >> 4;
-	if (version != 4) {
-		printf("bad IPv4 version %u\n", version);
-		return;
-	}
-	headerlen = (buf[0] & 0xf) * 4;
-	packetlen = nget16(buf + 2);
-	id        = nget16(buf + 4);
-	proto     = buf[9];
-	src  = nget32(buf + 12);
-	dest = nget32(buf + 16);
-
-	// TODO checksum
-	// TODO fragmentation
-
-	printf("headerlen %u, packetlen %u (real %u), id %u\n", headerlen, packetlen, len, id);
-	printf("from %x to %x\n", src, dest);
-	printf("id %u\n", id);
-	if (packetlen < headerlen) {
-		printf("headerlen too big\n");
-		return;
-	}
-	switch (proto) {
-		case 1:
-			printf("proto %u - icmp\n", proto);
-			parse_icmp(buf + headerlen, packetlen - headerlen);
-			break;
-		default:
-			printf("proto %u - unknown\n", proto);
-			break;
-	}
-}
-
-static void parse_arp(const uint8_t *buf, size_t len) {
-	// TODO no bound checks
-	uint16_t htype = nget16(buf + 0);
-	uint16_t ptype = nget16(buf + 2);
-	uint16_t op = nget16(buf + 6);
-
-	const char *ops = "bad operation";
-	if (op == 1) ops = "request";
-	if (op == 2) ops = "reply";
-
-	printf("ARP htype 0x%x, ptype 0x%x, %s\n", htype, ptype, ops);
-
-	if (!(htype == 1 && ptype == 0x800)) return;
-	/* IPv4 over Ethernet */
-
-	if (op != 1) return;
-	/* a request */
-
-	uint32_t daddr = nget32(buf + 24);
-	printf("IPv4 request for %08x\n", daddr);
-
-	if (daddr == my_ip) {/* send a response */
-		char res[64];
-		memset(res, 0, 64);
-		/* Ethernet */
-		memcpy(res + 0, buf + 8, 6); /* from ARP sender MAC */
-		memcpy(res + 6, my_mac,  6);
-		nput16(res + 12, 0x0806); /* ethertype == ARP */
-
-		/* ARP */
-		nput16(res + 14, 1);      /* Ethernet */
-		nput16(res + 16, 0x800);  /* IPv4 */
-		res[18] = 6; res[19] = 4; /* address lengths */
-		nput16(res + 20, 2);      /* operation == reply */
-		memcpy(res + 22, my_mac, 6);
-		nput32(res + 28, my_ip);
-		memcpy(res + 32, buf + 8, 10); /* sender's MAC and IP */
-		printf("sending ARP response:\n");
-		hexdump(res, sizeof res);
-		_syscall_write(eth_h, res, sizeof res, 0, 0);
-	}
-}
-
-/* https://www.w3.org/TR/PNG/#D-CRCAppendix */
-uint32_t crc_table[256];
-static 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;
-}
-
-static void parse_ethernet(const uint8_t *buf, size_t len) {
-	uint8_t dmac[6], smac[6];
-
-	if (len < 60) return;
-	for (int i = 0; i < 6; i++) dmac[i] = buf[i];
-	for (int i = 0; i < 6; i++) smac[i] = buf[i + 6];
-	printf("from %02x:%02x:%02x:%02x:%02x:%02x\n",
-		smac[0], smac[1], smac[2], smac[3], smac[4], smac[5]);
-	printf("to   %02x:%02x:%02x:%02x:%02x:%02x\n",
-		dmac[0], dmac[1], dmac[2], dmac[3], dmac[4], dmac[5]);
-
-	if (false) {
-		/* byte order switched on purpose */
-		uint32_t crc = (buf[len - 1] << 24)
-		             | (buf[len - 2] << 16)
-		             | (buf[len - 3] <<  8)
-		             | (buf[len - 4] <<  0);
-		printf("fcf %x, crc %x\n", crc, crc32(buf, len - 4));
-	}
-
-	uint16_t ethertype = nget16(buf + 12);
-	printf("ethertype %u\n", ethertype);
-	switch (ethertype) {
-		case 0x800:
-			parse_ipv4(buf + 14, len - 14);
-			break;
-		case 0x806:
-			parse_arp(buf + 14, len - 14);
-			break;
-		default:
-			printf("(unknown)\n");
-			break;
-	}
-}
+struct net_state state = {
+	// TODO dynamically get mac
+	.mac = {0x52, 0x54, 0x00, 0xCA, 0x77, 0x1A},
+	.ip = (192 << 24) + (168 << 16) + 11,
+};
 
 int main(void) {
 	const char *path = "/kdev/eth";
-	eth_h = _syscall_open(path, strlen(path), 0);
-	if (eth_h < 0) {
+	state.raw_h = _syscall_open(path, strlen(path), 0);
+	if (state.raw_h < 0) {
 		eprintf("couldn't open %s", path);
 		return 1;
 	}
@@ -208,11 +23,11 @@ int main(void) {
 	const size_t buflen = 4096;
 	char *buf = malloc(buflen);
 	for (;;) {
-		long ret = _syscall_read(eth_h, buf, buflen, -1);
+		long ret = _syscall_read(state.raw_h, buf, buflen, -1);
 		if (ret < 0) break;
 		printf("\npacket of length %u\n", ret);
 		hexdump(buf, ret);
-		parse_ethernet((void*)buf, ret);
+		ether_parse((void*)buf, ret);
 	}
 	free(buf);
 	return 0;
diff --git a/src/user/app/ethdump/ether.c b/src/user/app/ethdump/ether.c
new file mode 100644
index 0000000..da21b49
--- /dev/null
+++ b/src/user/app/ethdump/ether.c
@@ -0,0 +1,62 @@
+#include <camellia/syscalls.h>
+#include <stdlib.h>
+#include <string.h>
+#include "proto.h"
+#include "util.h"
+
+enum {
+	DstMAC    =  0,
+	SrcMAC    =  6,
+	EtherType = 12,
+	Payload   = 14,
+};
+
+void ether_parse(const uint8_t *buf, size_t len) {
+	uint8_t dmac[6], smac[6];
+
+	if (len < 60) return;
+	for (int i = 0; i < 6; i++) dmac[i] = buf[i + DstMAC];
+	for (int i = 0; i < 6; i++) smac[i] = buf[i + SrcMAC];
+	printf("from %02x:%02x:%02x:%02x:%02x:%02x\n",
+		smac[0], smac[1], smac[2], smac[3], smac[4], smac[5]);
+	printf("to   %02x:%02x:%02x:%02x:%02x:%02x\n",
+		dmac[0], dmac[1], dmac[2], dmac[3], dmac[4], dmac[5]);
+
+	uint16_t ethertype = nget16(buf + EtherType);
+	printf("ethertype %u\n", ethertype);
+	switch (ethertype) {
+		case ET_IPv4:
+			ipv4_parse(buf + Payload, len - Payload);
+			break;
+		case ET_ARP:
+			arp_parse(buf + Payload, len - Payload);
+			break;
+		default:
+			printf("(unknown)\n");
+			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;
+	printf("sending:\n");
+	hexdump(buf + fhoff, len);
+	_syscall_write(state.raw_h, buf + fhoff, len, 0, 0);
+	free(buf);
+}
diff --git a/src/user/app/ethdump/icmp.c b/src/user/app/ethdump/icmp.c
new file mode 100644
index 0000000..9c80a2d
--- /dev/null
+++ b/src/user/app/ethdump/icmp.c
@@ -0,0 +1,8 @@
+#include "proto.h"
+#include "util.h"
+
+void icmp_parse(const uint8_t *buf, size_t len) {
+	if (len < 4) return;
+	uint8_t type = buf[0];
+	printf("ICMP type %u\n", type);
+}
diff --git a/src/user/app/ethdump/ipv4.c b/src/user/app/ethdump/ipv4.c
new file mode 100644
index 0000000..408e1df
--- /dev/null
+++ b/src/user/app/ethdump/ipv4.c
@@ -0,0 +1,50 @@
+#include "proto.h"
+#include "util.h"
+
+enum {
+	Version = 0,
+	HdrLen  = 0,
+	PktLen  = 2,
+	Id      = 4,
+	Proto   = 9,
+	SrcIP   = 12,
+	DestIP  = 16,
+};
+
+void ipv4_parse(const uint8_t *buf, size_t len) {
+	uint8_t version, headerlen, proto;
+	uint16_t packetlen, id;
+	uint32_t dest, src;
+
+	version   = buf[Version] >> 4;
+	if (version != 4) {
+		printf("bad IPv4 version %u\n", version);
+		return;
+	}
+	headerlen = (buf[HdrLen] & 0xf) * 4;
+	packetlen = nget16(buf + PktLen);
+	id        = nget16(buf + Id);
+	proto     = buf[Proto];
+	src  = nget32(buf + SrcIP);
+	dest = nget32(buf + DestIP);
+
+	// TODO checksum
+	// TODO fragmentation
+
+	printf("headerlen %u, packetlen %u (real %u), id %u\n", headerlen, packetlen, len, id);
+	printf("from %x to %x\n", src, dest);
+	printf("id %u\n", id);
+	if (packetlen < headerlen) {
+		printf("headerlen too big\n");
+		return;
+	}
+	switch (proto) {
+		case 1:
+			printf("proto %u - icmp\n", proto);
+			icmp_parse(buf + headerlen, packetlen - headerlen);
+			break;
+		default:
+			printf("proto %u - unknown\n", proto);
+			break;
+	}
+}
diff --git a/src/user/app/ethdump/proto.h b/src/user/app/ethdump/proto.h
new file mode 100644
index 0000000..f47ca53
--- /dev/null
+++ b/src/user/app/ethdump/proto.h
@@ -0,0 +1,31 @@
+#pragma once
+#include <camellia/types.h>
+#include <stdint.h>
+
+typedef uint8_t mac_t[6];
+
+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;
+};
+
+
+void arp_parse(const uint8_t *buf, size_t len);
+void icmp_parse(const uint8_t *buf, size_t len);
+void ipv4_parse(const uint8_t *buf, size_t len);
+
+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);
diff --git a/src/user/app/ethdump/util.c b/src/user/app/ethdump/util.c
new file mode 100644
index 0000000..b0d8785
--- /dev/null
+++ b/src/user/app/ethdump/util.c
@@ -0,0 +1,33 @@
+#include "util.h"
+
+void hexdump(const void *vbuf, size_t len) {
+	const uint8_t *buf = vbuf;
+	for (size_t i = 0; i < len; i += 16) {
+		printf("%08x  ", i);
+
+		for (size_t j = i; j < i + 8 && j < len; j++)
+			printf("%02x ", buf[j]);
+		printf(" ");
+		for (size_t j = i + 8; j < i + 16 && j < len; j++)
+			printf("%02x ", buf[j]);
+		printf("\n");
+	}
+}
+
+/* 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;
+}
diff --git a/src/user/app/ethdump/util.h b/src/user/app/ethdump/util.h
new file mode 100644
index 0000000..536f270
--- /dev/null
+++ b/src/user/app/ethdump/util.h
@@ -0,0 +1,38 @@
+#pragma once
+#include <stdint.h>
+#include <stdio.h>
+
+#define eprintf(fmt, ...) fprintf(stderr, "ethdump: "fmt"\n" __VA_OPT__(,) __VA_ARGS__)
+
+void hexdump(const void *vbuf, size_t len);
+
+uint32_t crc32(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);
+}