Respond to ICMP Echo Requests (aka pings)

src/main.cpp

@@ -1941,7 +1941,6 @@ extern "C"
 			MsDelay(1000);
 		}
 
-		const auto ipAddress = 0x0A00000B;
 		uint8_t ipBuffer[USPI_FRAME_BUFFER_SIZE];
 
 		if (USPiEthernetAvailable()) {
@@ -1952,14 +1951,15 @@ extern "C"
 			screen.PrintText(false, 0, y_pos+=16, tempBuffer, COLOUR_WHITE, COLOUR_BLACK);
 
 			// Send an ARP announcement
-			SendArpAnnouncement(macAddress, ipAddress);
+			SendArpAnnouncement(macAddress, Ipv4Address);
 
 			while (true)
 			{
 				size_t size;
 				if (!USPiReceiveFrame(ipBuffer, &size))
 				{
-					const auto targetIp = 0x0A00000A;
+					/*
+					const auto targetIp = 0xC0A80128;
 					const auto targetMacIter = ArpTable.find(targetIp);
 
 					if (targetMacIter != ArpTable.end())
@@ -1970,10 +1970,11 @@ extern "C"
 					else
 					{
 						// Send an ARP request to find the MAC address belonging to this IP.
-						SendArpRequest(MacBroadcast, macAddress, targetIp, ipAddress);
+						SendArpRequest(MacBroadcast, macAddress, targetIp, Ipv4Address);
 					}
+					*/
 
-					MsDelay(1000);
+					MsDelay(100);
 					continue;
 				}
 
@@ -1991,6 +1992,12 @@ extern "C"
 				{
 					HandleArpFrame(ipBuffer);
 				}
+				else if (header.type == ETHERTYPE_IPV4)
+				{
+					uint64_t debug = HandleIpv4Frame(ipBuffer);
+					snprintf(tempBuffer, tempBufferSize, "Debug: %016llx", debug);
+					screen.PrintText(false, 0, y_pos+=16, tempBuffer, COLOUR_WHITE, COLOUR_BLACK);
+				}
 			}
 		}
 

src/net-arp.h

@@ -51,7 +51,7 @@ struct Ipv4ArpPacket
 		return 28;
 	}
 
-	static Ipv4ArpPacket Deserialize(uint8_t* buffer)
+	static Ipv4ArpPacket Deserialize(const uint8_t* buffer)
 	{
 		Ipv4ArpPacket self;
 

src/net-ethernet.h

@@ -7,14 +7,18 @@ struct EthernetFrameHeader
 	MacAddress macSource;
 	std::uint16_t type;
 
+	EthernetFrameHeader() {}
+
 	EthernetFrameHeader(std::uint16_t type) :
 		macDestination{255, 255, 255, 255, 255, 255},
 		macSource{0, 0, 0, 0, 0, 0},
 		type(type)
-	{
+	{}
-	}
+
+	EthernetFrameHeader(MacAddress macDestination, MacAddress macSource, uint16_t type) :
+		macDestination(macDestination), macSource(macSource), type(type)
+	{}
 
-	EthernetFrameHeader() : EthernetFrameHeader(0) {}
 
 	std::size_t Serialize(uint8_t* buffer)
 	{
@@ -32,7 +36,7 @@ struct EthernetFrameHeader
 		return i;
 	}
 
-	static EthernetFrameHeader Deserialize(uint8_t* buffer)
+	static EthernetFrameHeader Deserialize(const uint8_t* buffer)
 	{
 		EthernetFrameHeader self;
 		memcpy(self.macDestination.data(), buffer + 0, self.macDestination.size());
@@ -52,9 +56,16 @@ struct EthernetFrame
 
 	EthernetFrame() {}
 
-	EthernetFrame(std::uint16_t type, T payload) : header(type), payload(payload), crc(0)
+	EthernetFrame(std::uint16_t type, T payload) : header(type), payload(payload)
-	{
+	{}
-	}
+
+	EthernetFrame(
+		MacAddress macDestination,
+		MacAddress macSource,
+		std::uint16_t type,
+		T payload
+	) : header(macDestination, macSource, type), payload(payload)
+	{}
 
 	std::size_t Serialize(uint8_t* buffer)
 	{
@@ -79,7 +90,7 @@ struct EthernetFrame
 		return i;
 	}
 
-	static EthernetFrame<T> Deserialize(uint8_t* buffer)
+	static EthernetFrame<T> Deserialize(const uint8_t* buffer)
 	{
 		EthernetFrame<T> self;
 

src/net-icmp.h

@@ -1,18 +1,26 @@
 #pragma once
 #include "net.h"
 
+enum IcmpType
+{
+	ICMP_ECHO_REPLY = 0,
+	ICMP_ECHO_REQUEST = 8,
+};
+
 struct IcmpPacketHeader
 {
 	std::uint8_t type;
 	std::uint8_t code;
 	std::uint16_t checksum;
 
+	IcmpPacketHeader() {}
+
 	IcmpPacketHeader(std::uint8_t type, std::uint8_t code) :
 		type(type), code(code), checksum(0)
-	{
+	{}
-	}
 
-	std::size_t Serialize(uint8_t* buffer) {
+	std::size_t Serialize(uint8_t* buffer) const
+	{
 		size_t i = 0;
 		buffer[i++] = type;
 		buffer[i++] = code;
@@ -20,6 +28,15 @@ struct IcmpPacketHeader
 		buffer[i++] = checksum >> 8;
 		return i;
 	}
+
+	static IcmpPacketHeader Deserialize(const uint8_t* buffer)
+	{
+		IcmpPacketHeader self;
+		self.type = buffer[0];
+		self.code = buffer[1];
+		self.checksum = buffer[2] << 8 | buffer[3];
+		return self;
+	}
 } __attribute__((packed));
 
 
@@ -29,10 +46,15 @@ struct IcmpPacket
 	IcmpPacketHeader header;
 	T payload;
 
+	IcmpPacket() {}
+
+	IcmpPacket(std::uint8_t type, std::uint8_t code) :
+		header(type, code)
+	{}
+
 	IcmpPacket(std::uint8_t type, std::uint8_t code, T payload) :
 		header(type, code), payload(payload)
-	{
+	{}
-	}
 
 	std::size_t Serialize(uint8_t* buffer)
 	{
@@ -48,15 +70,25 @@ struct IcmpPacket
 
 		return i;
 	}
+
+	static IcmpPacket<T> Deserialize(const uint8_t* buffer)
+	{
+		IcmpPacket<T> self;
+		self.header = IcmpPacketHeader::Deserialize(buffer);
+		self.payload = T::Deserialize(buffer + sizeof(IcmpPacketHeader));
+		return self;
+	}
 } __attribute__((packed));
 
 template <class T>
-struct IcmpEchoRequest 
+struct IcmpEchoRequest
 {
 	uint16_t identifier;
 	uint16_t sequenceNumber;
 	T data;
 
+	IcmpEchoRequest() {}
+
 	IcmpEchoRequest(T data) : identifier(0), sequenceNumber(0), data(data)
 	{}
 
@@ -73,4 +105,77 @@ struct IcmpEchoRequest
 
 		return i;
 	}
+
+	static IcmpEchoRequest<T> Deserialize(const uint8_t* buffer)
+	{
+		IcmpEchoRequest self;
+		self.identifier = buffer[0] << 8 | buffer[1];
+		self.sequenceNumber = buffer[2] << 8 | buffer[3];
+		memcpy(self.data, buffer + 4, sizeof(T));
+		return self;
+	}
+} __attribute__((packed));
+
+template <>
+struct IcmpEchoRequest<void>
+{
+	uint16_t identifier;
+	uint16_t sequenceNumber;
+
+	IcmpEchoRequest() : identifier(0), sequenceNumber(0)
+	{}
+
+	size_t Serialize(uint8_t* buffer)
+	{
+		size_t i = 0;
+		buffer[i++] = identifier >> 8;
+		buffer[i++] = identifier;
+		buffer[i++] = sequenceNumber >> 8;
+		buffer[i++] = sequenceNumber;
+		return i;
+	}
+
+	static IcmpEchoRequest Deserialize(const uint8_t* buffer)
+	{
+		IcmpEchoRequest self;
+		self.identifier = buffer[0] << 8 | buffer[1];
+		self.sequenceNumber = buffer[2] << 8 | buffer[3];
+		return self;
+	}
+} __attribute__((packed));
+
+template <class T>
+struct IcmpEchoResponse
+{
+	uint16_t identifier;
+	uint16_t sequenceNumber;
+	T data;
+
+	IcmpEchoResponse() {}
+
+	IcmpEchoResponse(T data) : identifier(0), sequenceNumber(0), data(data)
+	{}
+
+	size_t Serialize(uint8_t* buffer)
+	{
+		size_t i = 0;
+		buffer[i++] = identifier >> 8;
+		buffer[i++] = identifier;
+		buffer[i++] = sequenceNumber >> 8;
+		buffer[i++] = sequenceNumber;
+
+		memcpy(buffer + i, &data, sizeof(T));
+		i += sizeof(T);
+
+		return i;
+	}
+
+	static IcmpEchoResponse<T> Deserialize(const uint8_t* buffer)
+	{
+		IcmpEchoResponse self;
+		self.identifier = buffer[0] << 8 | buffer[1];
+		self.sequenceNumber = buffer[2] << 8 | buffer[3];
+		memcpy(self.data, buffer + 4, sizeof(T));
+		return self;
+	}
 } __attribute__((packed));

src/net-ipv4.h

@@ -1,6 +1,11 @@
 #pragma once
 #include "net.h"
 
+enum IpProtocols
+{
+	IP_PROTO_ICMP = 1,
+};
+
 struct Ipv4Header
 {
 	unsigned int version : 4;
@@ -17,6 +22,8 @@ struct Ipv4Header
 	uint32_t sourceIp;
 	uint32_t destinationIp;
 
+	Ipv4Header() {}
+
 	Ipv4Header(
 		uint8_t protocol, uint32_t sourceIp, uint32_t destinationIp, uint16_t totalLength
 	) :
@@ -70,6 +77,31 @@ struct Ipv4Header
 
 		return i;
 	}
+
+	static Ipv4Header Deserialize(const uint8_t* buffer)
+	{
+		Ipv4Header self;
+		self.version = buffer[0] >> 4;
+		self.ihl = buffer[0] & 0x0F;
+
+		self.dscp = buffer[1] >> 2;
+		self.ecn = buffer[1] & 0x03;
+
+		self.totalLength = buffer[2] << 8 | buffer[3];
+		self.identification = buffer[4] << 8 | buffer[5];
+
+		self.flags = buffer[6] >> 5;
+		self.fragmentOffset = (buffer[6] & 0x1F) << 8 | buffer[7];
+
+		self.ttl = buffer[8];
+		self.protocol = buffer[9];
+		self.headerChecksum = buffer[10] << 8 | buffer[11];
+
+		self.sourceIp = buffer[12] << 24 | buffer[13] << 16 | buffer[14] << 8 | buffer[15];
+		self.destinationIp = buffer[16] << 24 | buffer[17] << 16 | buffer[18] << 8 | buffer[19];
+
+		return self;
+	}
 } __attribute__((packed));
 
 template<class T>
@@ -78,6 +110,8 @@ struct Ipv4Packet
 	Ipv4Header header;
 	T payload;
 
+	Ipv4Packet() {}
+
 	Ipv4Packet(uint8_t protocol, uint32_t sourceIp, uint32_t destinationIp, T payload) :
 		header(protocol, sourceIp, destinationIp, sizeof(Ipv4Packet<T>)),
 		payload(payload)
@@ -91,4 +125,13 @@ struct Ipv4Packet
 		i += payload.Serialize(buffer + i);
 		return i;
 	}
+
+	static Ipv4Packet<T> Deserialize(const uint8_t* buffer)
+	{
+		Ipv4Packet<T> self;
+		self.header = Ipv4Header::Deserialize(buffer);
+		self.payload = T::Deserialize(buffer + sizeof(Ipv4Header));
+		return self;
+	}
+
 } __attribute__((packed));

src/net.cpp

@@ -7,8 +7,6 @@
 #include "types.h"
 #include <uspi.h>
 
-const uint32_t Ipv4Address = 0x0A00000B;
-
 //
 // ARP
 //
@@ -57,7 +55,6 @@ void SendArpAnnouncement(MacAddress mac, uint32_t ip)
 void HandleArpFrame(uint8_t* buffer)
 {
 	const auto macAddress = GetMacAddress();
-	const auto ipAddress = 0x0A00000B;
 
 	const auto frame = EthernetFrame<Ipv4ArpPacket>::Deserialize(buffer);
 	const auto arp = frame.payload;
@@ -65,20 +62,40 @@ void HandleArpFrame(uint8_t* buffer)
 	if (arp.hardwareType == 1 &&
 		arp.protocolType == ETHERTYPE_IPV4 &&
 		arp.operation == ARP_OPERATION_REQUEST &&
-		arp.targetIp == ipAddress)
+		arp.targetIp == Ipv4Address)
 	{
-		SendArpReply(arp.senderMac, macAddress, arp.senderIp, ipAddress);
+		SendArpReply(arp.senderMac, macAddress, arp.senderIp, Ipv4Address);
 	}
 	else if (arp.hardwareType == 1 &&
 			arp.protocolType == ETHERTYPE_IPV4 &&
 			arp.operation == ARP_OPERATION_REPLY &&
-			arp.targetIp == ipAddress &&
+			arp.targetIp == Ipv4Address &&
 			arp.targetMac == macAddress)
 	{
 		ArpTable.insert(std::make_pair(arp.senderIp, arp.senderMac));
 	}
 }
 
+//
+// IPv4
+//
+uint64_t HandleIpv4Frame(const uint8_t* buffer)
+{
+	const auto frame = EthernetFrame<Ipv4Header>::Deserialize(buffer);
+	const auto header = frame.payload;
+
+	if (header.version != 4) return 0x4;
+	if (header.ihl != 5) return 0x8; // Not supported
+	if (header.destinationIp != Ipv4Address) return 0x10 | std::uint64_t{header.destinationIp} << 32;
+	if (header.fragmentOffset != 0) return 0x20; // TODO Support this
+
+	if (header.protocol == IP_PROTO_ICMP)
+	{
+		return HandleIcmpFrame(buffer) | 0x2;
+	}
+	return 0x0;
+}
+
 
 //
 // ICMP
@@ -98,6 +115,51 @@ void SendIcmpEchoRequest(MacAddress mac, uint32_t ip)
 	USPiSendFrame(buffer, size);
 }
 
+uint64_t HandleIcmpFrame(const uint8_t* buffer)
+{
+	const auto frame = EthernetFrame<Ipv4Packet<IcmpPacketHeader>>::Deserialize(buffer);
+	const auto packetHeader = frame.payload.payload;
+
+	if (packetHeader.type == ICMP_ECHO_REQUEST)
+	{
+		// TODO This should not be hardcoded lol
+		typedef EthernetFrame<Ipv4Packet<IcmpPacket<IcmpEchoRequest<uint8_t[56]>>>> Frame;
+		auto frameReq = Frame::Deserialize(buffer);
+		
+		auto echoReq = frameReq.payload.payload.payload;
+
+		IcmpEchoResponse<uint8_t[56]> echoResp;
+		echoResp.identifier = echoReq.identifier;
+		echoResp.sequenceNumber = echoReq.sequenceNumber;
+		memcpy(echoResp.data, echoReq.data, 56);
+
+		const auto sourceIp = frame.payload.header.sourceIp;
+
+		IcmpPacket<decltype(echoResp)> icmpResp(ICMP_ECHO_REPLY, 0, echoResp);
+
+		Ipv4Packet<decltype(icmpResp)> ipv4Resp(
+			IP_PROTO_ICMP,
+			Ipv4Address,
+			sourceIp,
+			icmpResp
+		);
+
+		EthernetFrame<decltype(ipv4Resp)> frameResp(
+			frame.header.macSource,
+			GetMacAddress(),
+			ETHERTYPE_IPV4,
+			ipv4Resp
+		);
+
+		uint8_t bufferResp[USPI_FRAME_BUFFER_SIZE];
+		const auto size = frameResp.Serialize(bufferResp);
+		USPiSendFrame(bufferResp, size);
+
+		return 0x1;
+	}
+	return 0x0 | std::uint64_t{packetHeader.type} << 32;
+}
+
 //
 // Helpers
 //
@@ -213,5 +275,7 @@ MacAddress GetMacAddress()
 	return macAddress;
 }
 
-MacAddress MacBroadcast{0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF};
+const uint32_t Ipv4Address = 0xC0A80164;
+const MacAddress MacBroadcast{0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF};
+
 std::unordered_map<std::uint32_t, MacAddress> ArpTable;

src/net.h

@@ -15,10 +15,9 @@ enum ArpOperation {
 	ARP_OPERATION_REPLY = 2,
 };
 
-typedef std::array<std::uint8_t, 6> MacAddress;
+typedef std::array<uint8_t, 6> MacAddress;
 
-
+void HandleArpFrame(uint8_t* buffer);
-void HandleArpFrame(std::uint8_t* buffer);
 void SendArpPacket(ArpOperation operation,
 					MacAddress targetMac,
 					MacAddress senderMac,
@@ -34,13 +33,18 @@ void SendArpReply(MacAddress targetMac,
 					uint32_t targetIp);
 void SendArpAnnouncement(MacAddress mac, uint32_t ip);
 
+uint64_t HandleIpv4Frame(const uint8_t* buffer);
+
 void SendIcmpEchoRequest(MacAddress mac, uint32_t ip);
+uint64_t HandleIcmpFrame(const uint8_t* buffer);
 
 std::uint32_t Crc32(const std::uint8_t* buffer, std::size_t size);
 std::uint16_t InternetChecksum(const void* data, std::size_t size);
 MacAddress GetMacAddress();
 
-extern MacAddress MacBroadcast;
+extern const MacAddress MacBroadcast;
+extern const uint32_t Ipv4Address;
+
 extern std::unordered_map<std::uint32_t, MacAddress> ArpTable;
 
 struct UdpDatagramHeader