ipv4: routing table

kernel-src/io/net/ipv4.c

@@ -2,11 +2,76 @@
 #include <kernel/eth.h>
 #include <arch/cpu.h>
 #include <logging.h>
+#include <mutex.h>
+#include <kernel/alloc.h>
 
-int ipv4_allocdesc(netdev_t *netdev, size_t requestedsize, netdesc_t *desc) {
-	int e = netdev->allocdesc(requestedsize + sizeof(ipv4frame_t), desc);
-	desc->curroffset += sizeof(ipv4frame_t);
-	return e;
+typedef struct {
+	netdev_t *netdev;
+	uint32_t addr;
+	uint32_t gateway;
+	uint32_t mask;
+	int weight;
+} routingentry_t;
+
+static routingentry_t *routingtable;
+static mutex_t routingtablelock;
+static size_t routingtablesize;
+
+static routingentry_t getroute(uint32_t ip) {
+	routingentry_t selected = {0};
+
+	MUTEX_ACQUIRE(&routingtablelock, false);
+
+	for (int i = 0; i < routingtablesize; ++i) {
+		if (routingtable[i].netdev == NULL || (routingtable[i].mask & ip) != (routingtable[i].addr & routingtable[i].mask))
+			continue;
+
+		selected = routingtable[i].weight > selected.weight ? routingtable[i] : selected;
+	}
+
+	MUTEX_RELEASE(&routingtablelock);
+
+	return selected;
+}
+
+int ipv4_addroute(netdev_t *netdev, uint32_t addr, uint32_t gateway, uint32_t mask, int weight) {
+	routingentry_t entry = {
+		.netdev = netdev,
+		.addr = addr,
+		.gateway = gateway,
+		.mask = mask,
+		.weight = weight
+	};
+
+	MUTEX_ACQUIRE(&routingtablelock, false);
+
+	int error = 0;
+	int i;
+
+	for (i = 0; i < routingtablesize; ++i) {
+		if (routingtable[i].netdev != NULL)
+			continue;
+
+		routingtable[i] = entry;
+		break;
+	}
+
+	if (i == routingtablesize) {
+		// insert to the end
+		size_t newsize = routingtablesize + 1;
+		void *tmp = realloc(routingtable, newsize * sizeof(routingentry_t));
+		if (tmp) {
+			routingtable = tmp;
+			routingtablesize = newsize;
+			routingtable[i] = entry;
+		} else {
+			error = ENOMEM;
+		}
+	}
+
+	MUTEX_RELEASE(&routingtablelock);
+
+	return error;
 }
 
 #define VERSION_LENGTH(v, l) (((v) << 4) | l)
@@ -15,43 +80,73 @@ int ipv4_allocdesc(netdev_t *netdev, size_t requestedsize, netdesc_t *desc) {
 // offset has to be a multiple of 8
 #define FLAGS_FRAGOFFSET(flg, offset) (((flg) << 13) | ((offset) >> 3))
 
-static int dispatch_fragment(netdev_t *netdev, netdesc_t originaldesc, netdesc_t fragdesc, 
-	uintmax_t fragmentoffset, size_t fragmentlen, int id, uint32_t ip, int proto) {
+static int dispatch_fragment(netdev_t *netdev, netdesc_t fragdesc, 
+	uintmax_t fragmentoffset, size_t fragmentlen, int id, uint32_t ip, mac_t mac, int proto, bool last) {
 	ipv4frame_t frame = {
 		.version_length = VERSION_LENGTH(4, 5),
 		.servicetype_ecn = 0,
 		.packetlen = cpu_to_be_w(fragmentlen + sizeof(ipv4frame_t)),
 		.id = cpu_to_be_w(0x1234), // TODO proper id allocation per netdev
-		.flags_fragoffset = cpu_to_be_w(FLAGS_FRAGOFFSET((fragmentoffset + fragmentlen < originaldesc.size - originaldesc.curroffset - sizeof(ipv4frame_t)) ? FLAG_MF : 0, fragmentoffset)),
+		.flags_fragoffset = cpu_to_be_w(FLAGS_FRAGOFFSET(!last ? FLAG_MF : 0, fragmentoffset)),
 		.timetolive = 0xff,
 		.protocol = proto,
 		.framechecksum = 0xfefe, // TODO checksum
-		.srcaddr = 0xffffffff, // TODO src addr
+		.srcaddr = netdev->ip, // TODO src addr
 		.dstaddr = cpu_to_be_d(ip)
 	};
 
 	memcpy((void *)((uintptr_t)fragdesc.address + fragdesc.curroffset), &frame, sizeof(ipv4frame_t));
-	memcpy((void *)((uintptr_t)fragdesc.address + fragdesc.curroffset + sizeof(ipv4frame_t)), originaldesc.address + originaldesc.curroffset + sizeof(ipv4frame_t) + fragmentoffset, fragmentlen);
-	return netdev->sendpacket(netdev, fragdesc, NET_BROADCAST_MAC, ETH_PROTO_IP);
+	return netdev->sendpacket(netdev, fragdesc, mac, ETH_PROTO_IP);
 }
 
-int ipv4_sendpacket(netdev_t *netdev, netdesc_t desc, uint32_t ip, int proto) {
-	size_t datasize = desc.size - desc.curroffset;
-	desc.curroffset -= sizeof(ipv4frame_t);
-	__assert(desc.size - desc.curroffset <= 65535);
-	
-	size_t mtuheader = netdev->mtu - sizeof(ipv4frame_t);
-	size_t devfragmentsize = mtuheader - (mtuheader % 8);
-	size_t fragmentcount = datasize / devfragmentsize + 1;
+int ipv4_sendpacket(void *buffer, size_t packetsize, uint32_t ip, int proto, netdev_t *broadcastdev) {
+	if ((packetsize + sizeof(ipv4frame_t)) > 65535)
+		return EMSGSIZE;
 
-	for (int i = 0; i < fragmentcount; ++i) {
-		size_t fragmentlen = (i == fragmentcount - 1) ? datasize - devfragmentsize * (fragmentcount - 1) : devfragmentsize;
-		netdesc_t fragdesc;
-		__assert(ipv4_allocdesc(netdev, fragmentlen + sizeof(ipv4frame_t), &fragdesc) == 0);
-		fragdesc.curroffset -= sizeof(ipv4frame_t);
-		dispatch_fragment(netdev, desc, fragdesc, i * devfragmentsize, fragmentlen, 0x1234, ip, proto);
-		// TODO unallocate desc
+	mac_t mac;
+	netdev_t *netdev;
+
+	if (ip != IPV4_BROADCAST_ADDRESS) {
+		routingentry_t entry = getroute(ip);
+		if (entry.netdev == NULL)
+			return 0; // if a packet can't be routed to anywhere just pretend it was sent somewhere
+
+		int error = arp_lookup(entry.netdev, entry.gateway ? entry.gateway : ip, &mac);
+		if (error)
+			return error;
+
+		netdev = entry.netdev;
+	} else {
+		__assert(broadcastdev);
+		mac = NET_BROADCAST_MAC;
+		netdev = broadcastdev;
 	}
 
-	return 0; // TODO error handling
+
+	size_t mtuheader = netdev->mtu - sizeof(ipv4frame_t);
+	size_t devfragmentsize = mtuheader - (mtuheader % 8);
+	size_t fragmentcount = packetsize / devfragmentsize + 1;
+
+	for (int i = 0; i < fragmentcount; ++i) {
+		size_t fragmentlen = (i == fragmentcount - 1) ? packetsize - devfragmentsize * (fragmentcount - 1) : devfragmentsize;
+		netdesc_t fragdesc;
+		int e = netdev->allocdesc(fragmentlen + sizeof(ipv4frame_t), &fragdesc);
+		if (e)
+			return e;
+
+		memcpy((void *)((uintptr_t)fragdesc.address + sizeof(ipv4frame_t)), (void *)((uintptr_t)buffer + i * devfragmentsize), fragmentlen);
+		e = dispatch_fragment(netdev, fragdesc, i * devfragmentsize, fragmentlen, 0x1234, ip, mac, proto, i == fragmentcount - 1);
+		// TODO unallocate desc
+		if (e)
+			return e;
+	}
+
+	 return 0;
+}
+
+void ipv4_init() {
+	routingtablesize = 1;
+	MUTEX_INIT(&routingtablelock);
+	routingtable = alloc(sizeof(routingentry_t));
+	__assert(routingtable);
 }