6 files changed, 122 insertions, 18 deletions

diff --git a/src/user/app/ethdump/ether.c b/src/user/app/ethdump/ether.c
index da21b49..32646e2 100644
--- a/src/user/app/ethdump/ether.c
+++ b/src/user/app/ethdump/ether.c
@@ -24,9 +24,16 @@ void ether_parse(const uint8_t *buf, size_t len) {
 
 	uint16_t ethertype = nget16(buf + EtherType);
 	printf("ethertype %u\n", ethertype);
+
+	struct ethernet ether = (struct ethernet){
+		.src = &smac,
+		.dst = &dmac,
+		.type = ethertype,
+	};
+
 	switch (ethertype) {
 		case ET_IPv4:
-			ipv4_parse(buf + Payload, len - Payload);
+			ipv4_parse(buf + Payload, len - Payload, ether);
 			break;
 		case ET_ARP:
 			arp_parse(buf + Payload, len - Payload);
diff --git a/src/user/app/ethdump/icmp.c b/src/user/app/ethdump/icmp.c
index 9c80a2d..3f10ae8 100644
--- a/src/user/app/ethdump/icmp.c
+++ b/src/user/app/ethdump/icmp.c
@@ -1,8 +1,37 @@
 #include "proto.h"
 #include "util.h"
+#include <string.h>
 
-void icmp_parse(const uint8_t *buf, size_t len) {
-	if (len < 4) return;
-	uint8_t type = buf[0];
+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];
 	printf("ICMP type %u\n", type);
+	if (type == 8 && ip.dst == state.ip) {
+		uint8_t *pkt = icmp_start(len - Payload, (struct icmp){
+			.type = 0,
+			.ip.dst = ip.src,
+			.ip.e.dst = ip.e.src,
+		});
+		memcpy(pkt, buf + Payload, len - Payload);
+		icmp_finish(pkt);
+	}
+}
+
+uint8_t *icmp_start(size_t len, struct icmp i) {
+	i.ip.proto = 1;
+	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);
 }
diff --git a/src/user/app/ethdump/ipv4.c b/src/user/app/ethdump/ipv4.c
index 408e1df..c9b2df4 100644
--- a/src/user/app/ethdump/ipv4.c
+++ b/src/user/app/ethdump/ipv4.c
@@ -1,20 +1,23 @@
 #include "proto.h"
 #include "util.h"
+#include <stdlib.h>
 
 enum {
-	Version = 0,
-	HdrLen  = 0,
-	PktLen  = 2,
-	Id      = 4,
-	Proto   = 9,
-	SrcIP   = 12,
-	DestIP  = 16,
+	Version  = 0,
+	HdrLen   = 0,
+	PktLen   = 2,
+	Id       = 4,
+	TTL      = 8,
+	Proto    = 9,
+	Checksum = 10,
+	SrcIP    = 12,
+	DstIP    = 16,
 };
 
-void ipv4_parse(const uint8_t *buf, size_t len) {
+void ipv4_parse(const uint8_t *buf, size_t len, struct ethernet ether) {
 	uint8_t version, headerlen, proto;
 	uint16_t packetlen, id;
-	uint32_t dest, src;
+	uint32_t dst, src;
 
 	version   = buf[Version] >> 4;
 	if (version != 4) {
@@ -26,25 +29,57 @@ void ipv4_parse(const uint8_t *buf, size_t len) {
 	id        = nget16(buf + Id);
 	proto     = buf[Proto];
 	src  = nget32(buf + SrcIP);
-	dest = nget32(buf + DestIP);
+	dst = nget32(buf + DstIP);
 
 	// 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("from %x to %x\n", src, dst);
 	printf("id %u\n", id);
 	if (packetlen < headerlen) {
 		printf("headerlen too big\n");
 		return;
 	}
+
+	struct ipv4 ip = (struct ipv4){
+		.e = ether,
+		.src = src,
+		.dst = dst,
+		.proto = proto,
+	};
+
 	switch (proto) {
 		case 1:
 			printf("proto %u - icmp\n", proto);
-			icmp_parse(buf + headerlen, packetlen - headerlen);
+			icmp_parse(buf + headerlen, packetlen - headerlen, ip);
 			break;
 		default:
 			printf("proto %u - unknown\n", proto);
 			break;
 	}
 }
+
+uint8_t *ipv4_start(size_t len, struct ipv4 ip) {
+	ip.e.type = ET_IPv4;
+	if (!ip.src) ip.src = state.ip;
+	if (!ip.e.dst && ip.dst == 0xFFFFFFFF)
+		ip.e.dst = &MAC_BROADCAST;
+
+	size_t hdrlen = 20;
+	uint8_t *pkt = ether_start(len + hdrlen, ip.e);
+	pkt[Version] = 0x40;
+	pkt[HdrLen] |= hdrlen / 4;
+	nput16(pkt + PktLen, len + hdrlen);
+	pkt[TTL] = 0xFF;
+	pkt[Proto] = ip.proto;
+	nput32(pkt + SrcIP, ip.src);
+	nput32(pkt + DstIP, ip.dst);
+
+	nput16(pkt + Checksum, ip_checksum(pkt, hdrlen));
+
+	return pkt + hdrlen;
+}
+void ipv4_finish(uint8_t *pkt) {
+	ether_finish(pkt - 20);
+}
diff --git a/src/user/app/ethdump/proto.h b/src/user/app/ethdump/proto.h
index f47ca53..df2000e 100644
--- a/src/user/app/ethdump/proto.h
+++ b/src/user/app/ethdump/proto.h
@@ -3,6 +3,7 @@
 #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;
@@ -21,10 +22,27 @@ struct ethernet {
 	uint16_t type;
 };
 
+struct ipv4 {
+	struct ethernet e;
+	uint32_t src, dst;
+	uint8_t proto;
+};
+
+struct icmp {
+	struct ipv4 ip;
+	uint8_t type, code;
+};
+
 
 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 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 ipv4_parse(const uint8_t *buf, size_t len, struct ethernet ether);
+uint8_t *ipv4_start(size_t len, struct ipv4 ip);
+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);
diff --git a/src/user/app/ethdump/util.c b/src/user/app/ethdump/util.c
index b0d8785..8c29340 100644
--- a/src/user/app/ethdump/util.c
+++ b/src/user/app/ethdump/util.c
@@ -31,3 +31,15 @@ uint32_t crc32(const uint8_t *buf, size_t len) {
 		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/ethdump/util.h b/src/user/app/ethdump/util.h
index 536f270..ca7e99b 100644
--- a/src/user/app/ethdump/util.h
+++ b/src/user/app/ethdump/util.h
@@ -4,9 +4,12 @@
 
 #define eprintf(fmt, ...) fprintf(stderr, "ethdump: "fmt"\n" __VA_OPT__(,) __VA_ARGS__)
 
+// #define printf(...)
+
 void hexdump(const void *vbuf, size_t len);
 
 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) {