Fix network initialization, swapped ARP MACs and ICMP responses

src/net-arp.cpp

@@ -89,7 +89,7 @@ namespace Net::Arp
 		arpPacket.targetIp = targetIp;
 		arpPacket.senderIp = senderIp;
 
-		Ethernet::Header ethernetHeader(senderMac, targetMac, Ethernet::EtherType::Arp);
+		Ethernet::Header ethernetHeader(targetMac, senderMac, Ethernet::EtherType::Arp);
 
 		uint8_t buffer[USPI_FRAME_BUFFER_SIZE];
 		size_t size = 0;
@@ -136,7 +136,7 @@ namespace Net::Arp
 		if (arpSize == 0 || arpSize != arpPacket.SerializedLength())
 		{
 			DEBUG_LOG(
-				"Dropped ARP packet (invalid buffer size %lu, expected %lu)\r\n",
+				"Dropped ARP packet (invalid buffer size %u, expected %u)\r\n",
 				bufferSize,
 				arpPacket.SerializedLength());
 			return;

src/net-dhcp.cpp

@@ -241,7 +241,7 @@ namespace Net::Dhcp
 		if (dhcpSize != Header::SerializedLength())
 		{
 			DEBUG_LOG(
-				"Dropped DHCP packet (invalid buffer size %lu, expected %lu)\r\n",
+				"Dropped DHCP packet (invalid buffer size %u, expected %u)\r\n",
 				size,
 				Header::SerializedLength());
 			return;

src/net-icmp.cpp

@@ -1,7 +1,9 @@
 #include <cassert>
+#include <cstdio>
 #include <cstring>
 
 #include "net-icmp.h"
+#include "net-utils.h"
 
 #include "debug.h"
 #include "types.h"
@@ -15,7 +17,8 @@ namespace Net::Icmp
 	Header::Header() : type(static_cast<Type>(0)), code(0), checksum(0) {}
 	Header::Header(Type type, uint8_t code) : type(type), code(code), checksum(0) {}
 
-	size_t Header::Serialize(uint8_t* buffer, const size_t bufferSize) const
+	size_t Header::Serialize(
+		uint8_t* buffer, const size_t bufferSize, const uint8_t* data, const size_t dataSize) const
 	{
 		if (bufferSize < SerializedLength())
 		{
@@ -25,8 +28,16 @@ namespace Net::Icmp
 		size_t i = 0;
 		buffer[i++] = static_cast<uint8_t>(type);
 		buffer[i++] = code;
-		buffer[i++] = checksum;
+		buffer[i++] = 0;
-		buffer[i++] = checksum >> 8;
+		buffer[i++] = 0 >> 8;
+
+		std::memcpy(buffer + i, data, dataSize);
+		i += dataSize;
+
+		uint16_t checksum = Utils::InternetChecksum(buffer, i);
+		buffer[2] = checksum;
+		buffer[3] = checksum >> 8;
+
 		return i;
 	}
 
@@ -93,10 +104,12 @@ namespace Net::Icmp
 		uint8_t buffer[USPI_FRAME_BUFFER_SIZE];
 		size_t size = 0;
 
+		const uint8_t emptyBuffer[1] = {};
+
 		size += ethernetHeader.Serialize(buffer + size, sizeof(buffer) - size);
 		size += ipv4Header.Serialize(buffer + size, sizeof(buffer) - size);
 		size += pingHeader.Serialize(buffer + size, sizeof(buffer) - size);
-		size += icmpHeader.Serialize(buffer + size, sizeof(buffer) - size);
+		size += icmpHeader.Serialize(buffer + size, sizeof(buffer) - size, emptyBuffer, 0);
 
 		const auto expectedSize = ethernetHeader.SerializedLength() +
 			ipv4Header.SerializedLength() + pingHeader.SerializedLength() +
@@ -114,29 +127,19 @@ namespace Net::Icmp
 		const uint8_t* reqBuffer,
 		const size_t reqBufferSize)
 	{
-		EchoHeader reqEchoHeader;
+		const Ethernet::Header respEthernetHeader(
-		const auto reqEchoHeaderSize =
+			reqEthernetHeader.macSource, Utils::GetMacAddress(), Ethernet::EtherType::Ipv4);
-			Icmp::EchoHeader::Deserialize(reqEchoHeader, reqBuffer, reqBufferSize);
-		if (reqEchoHeaderSize == 0 || reqBufferSize < reqEchoHeaderSize)
-		{
-			DEBUG_LOG(
-				"Dropped ICMP packet (invalid buffer size %ul, expected at least %ul)\r\n",
-				reqBufferSize,
-				EchoHeader::SerializedLength());
-			return;
-		}
-
-		const Icmp::Header respIcmpHeader(Icmp::Type::EchoReply, 0);
 		const Ipv4::Header respIpv4Header(
 			Ipv4::Protocol::Icmp,
 			Utils::Ipv4Address,
 			reqIpv4Header.sourceIp,
 			reqIpv4Header.totalLength);
-		const Ethernet::Header respEthernetHeader(
+		Header respIcmpHeader(Type::EchoReply, 0);
-			reqEthernetHeader.macSource, Utils::GetMacAddress(), Ethernet::EtherType::Ipv4);
 
 		const auto payloadSize = reqIpv4Header.totalLength - reqIpv4Header.SerializedLength() -
-			reqIcmpHeader.SerializedLength() - reqEchoHeaderSize;
+			reqIcmpHeader.SerializedLength();
+
+		DEBUG_LOG("payloadSize: %u\r\n", payloadSize);
 
 		std::array<uint8_t, USPI_FRAME_BUFFER_SIZE> respBuffer;
 
@@ -145,10 +148,8 @@ namespace Net::Icmp
 			respBuffer.data() + respSize, respBuffer.size() - respSize);
 		respSize +=
 			respIpv4Header.Serialize(respBuffer.data() + respSize, respBuffer.size() - respSize);
-		respSize +=
+		respSize += respIcmpHeader.Serialize(
-			respIcmpHeader.Serialize(respBuffer.data() + respSize, respBuffer.size() - respSize);
+			respBuffer.data() + respSize, respBuffer.size() - respSize, reqBuffer, payloadSize);
-		std::memcpy(respBuffer.data() + respSize, reqBuffer + reqEchoHeaderSize, payloadSize);
-		respSize += payloadSize;
 
 		const auto expectedRespSize = respEthernetHeader.SerializedLength() +
 			respIpv4Header.SerializedLength() + respIcmpHeader.SerializedLength() + payloadSize;
@@ -158,33 +159,24 @@ namespace Net::Icmp
 		USPiSendFrame(respBuffer.data(), respSize);
 	}
 
-	void HandlePacket(const uint8_t* buffer, const size_t bufferSize)
+	void HandlePacket(
+		Ethernet::Header ethernetHeader,
+		Ipv4::Header ipv4Header,
+		const uint8_t* buffer,
+		const size_t bufferSize)
 	{
-		// TODO Don't re-parse the upper layers
-		size_t headerSize = 0;
-
-		Ethernet::Header ethernetHeader;
-		headerSize += Ethernet::Header::Deserialize(
-			ethernetHeader, buffer + headerSize, bufferSize - headerSize);
-
-		Ipv4::Header ipv4Header;
-		headerSize +=
-			Ipv4::Header::Deserialize(ipv4Header, buffer + headerSize, bufferSize - headerSize);
-
 		Header icmpHeader;
-		headerSize +=
+		size_t headerSize = Icmp::Header::Deserialize(icmpHeader, buffer, bufferSize);
-			Icmp::Header::Deserialize(icmpHeader, buffer + headerSize, bufferSize - headerSize);
+		if (headerSize != Icmp::Header::SerializedLength())
-
-		const auto expectedHeaderSize = ethernetHeader.SerializedLength() +
-			ipv4Header.SerializedLength() + icmpHeader.SerializedLength();
-		if (headerSize != expectedHeaderSize)
 		{
 			DEBUG_LOG(
-				"Dropped ICMP packet (invalid buffer size %ul, expected at least %ul)\r\n",
+				"Dropped ICMP packet (invalid buffer size %u, expected at least %u)\r\n",
 				bufferSize,
-				expectedHeaderSize);
+				Icmp::Header::SerializedLength());
 		}
 
+		DEBUG_LOG("Got ICMP header type %u\r\n", static_cast<uint8_t>(icmpHeader.type));
+
 		if (icmpHeader.type == Type::EchoRequest)
 		{
 			handleEchoRequest(

src/net-icmp.h

@@ -23,7 +23,12 @@ namespace Net::Icmp
 			return sizeof(type) + sizeof(code) + sizeof(checksum);
 		}
 
-		size_t Serialize(uint8_t* buffer, const size_t bufferSize) const;
+		void UpdateChecksum(const uint8_t* data, const size_t dataSize);
+		size_t Serialize(
+			uint8_t* buffer,
+			const size_t bufferSize,
+			const uint8_t* data,
+			const size_t dataSize) const;
 		static size_t Deserialize(Header& out, const uint8_t* buffer, const size_t bufferSize);
 	};
 
@@ -45,5 +50,9 @@ namespace Net::Icmp
 	};
 
 	void SendEchoRequest(const Utils::MacAddress mac, const uint32_t ip);
-	void HandlePacket(const uint8_t* buffer, const size_t bufferSize);
+	void HandlePacket(
+		Ethernet::Header ethernetHeader,
+		Ipv4::Header ipv4Header,
+		const uint8_t* buffer,
+		const size_t bufferSize);
 } // namespace Net::Icmp

src/net-ipv4.cpp

@@ -9,6 +9,10 @@
 
 #include "debug.h"
 
+#include "types.h"
+#include <cstring>
+#include <uspi.h>
+
 namespace Net::Ipv4
 {
 	Header::Header() {}
@@ -102,12 +106,14 @@ namespace Net::Ipv4
 	void HandlePacket(
 		const Ethernet::Header& ethernetHeader, const uint8_t* buffer, const size_t bufferSize)
 	{
+		char printBuffer[USPI_FRAME_BUFFER_SIZE] = {};
+
 		Header header;
 		const auto headerSize = Header::Deserialize(header, buffer, bufferSize);
 		if (headerSize != Header::SerializedLength())
 		{
 			DEBUG_LOG(
-				"Dropped IPv4 packet (invalid buffer size %lu, expected at least %lu)\r\n",
+				"Dropped IPv4 packet (invalid buffer size %u, expected at least %u)\r\n",
 				bufferSize,
 				headerSize);
 			return;
@@ -117,20 +123,42 @@ namespace Net::Ipv4
 		Arp::ArpTable.insert(std::make_pair(header.sourceIp, ethernetHeader.macSource));
 
 		if (header.version != 4)
+		{
+			DEBUG_LOG(
+				"Dropped IPv4 packet (invalid header version %u, expected 4)\r\n", header.version);
 			return;
+		}
 		if (header.ihl != 5)
-			return; // Not supported
+		{
-		if (header.destinationIp != Utils::Ipv4Address)
+			// Not supported
+			DEBUG_LOG("Dropped IPv4 packet (unsupported IHL %u, expected 5)\r\n", header.ihl);
 			return;
+		}
+		if (header.destinationIp != Utils::Ipv4Address)
+		{
+			DEBUG_LOG(
+				"Dropped IPv4 packet (invalid destination IP address %08lx)\r\n",
+				header.destinationIp);
+			return;
+		}
 		if (header.fragmentOffset != 0)
-			return; // TODO Support this
+		{
+			// TODO Support this
+			DEBUG_LOG(
+				"Dropped IPv4 packet (unexpected fragment offset %u, expected 0)\r\n",
+				header.fragmentOffset);
+			return;
+		}
 
 		if (header.protocol == Ipv4::Protocol::Icmp)
 		{
-			Icmp::HandlePacket(buffer, bufferSize - headerSize);
+			DEBUG_LOG("Ethernet -> IPv4 -> ICMP\r\n");
+			Icmp::HandlePacket(
+				ethernetHeader, header, buffer + headerSize, bufferSize - headerSize);
 		}
 		else if (header.protocol == Ipv4::Protocol::Udp)
 		{
+			DEBUG_LOG("Ethernet -> IPv4 -> UDP\r\n");
 			Udp::HandlePacket(ethernetHeader, header, buffer + headerSize, bufferSize - headerSize);
 		}
 	}

src/net-tftp.cpp

@@ -32,9 +32,9 @@ namespace Net::Tftp
 		if (size == 0)
 		{
 			DEBUG_LOG(
-				"Dropped TFTP packet (invalid buffer size %lu, expected at least %lu)\r\n",
+				"Dropped TFTP packet (invalid buffer size %u, expected at least %u)\r\n",
 				dataSize,
-				sizeof(WriteReadRequestPacket::opcode) + 2)
+				sizeof(WriteReadRequestPacket::opcode) + 2);
 			return nullptr;
 		}
 
@@ -93,9 +93,9 @@ namespace Net::Tftp
 		if (tftpSize == 0)
 		{
 			DEBUG_LOG(
-				"Dropped TFTP data packet (invalid buffer size %lu, expected at least %lu)\r\n",
+				"Dropped TFTP data packet (invalid buffer size %u, expected at least %u)\r\n",
 				size,
-				sizeof(packet.opcode) + sizeof(packet.blockNumber))
+				sizeof(packet.opcode) + sizeof(packet.blockNumber));
 			return nullptr;
 		}
 
@@ -140,7 +140,7 @@ namespace Net::Tftp
 		if (reqBufferSize < payloadSize)
 		{
 			DEBUG_LOG(
-				"Dropped TFTP packet (invalid buffer size %lu, expected at least %lu)\r\n",
+				"Dropped TFTP packet (invalid buffer size %u, expected at least %u)\r\n",
 				reqBufferSize,
 				payloadSize);
 		}

src/net-udp.cpp

@@ -57,7 +57,7 @@ namespace Net::Udp
 		if (headerSize == 0 || headerSize != udpHeader.SerializedLength())
 		{
 			DEBUG_LOG(
-				"Dropped UDP header (invalid buffer size %lu, expected at least %lu)\r\n",
+				"Dropped UDP header (invalid buffer size %u, expected at least %u)\r\n",
 				bufferSize,
 				Header::SerializedLength());
 			return;
@@ -65,7 +65,7 @@ namespace Net::Udp
 		if (udpHeader.length <= bufferSize)
 		{
 			DEBUG_LOG(
-				"Dropped UDP packet (invalid buffer size %lu, expected at least %lu)\r\n",
+				"Dropped UDP packet (invalid buffer size %u, expected at least %u)\r\n",
 				bufferSize,
 				udpHeader.length);
 			return;

src/net.cpp

@@ -6,14 +6,23 @@
 #include <uspi.h>
 #include <uspios.h>
 
+extern "C"
+{
+#include "rpiHardware.h"
+}
+
 namespace Net
 {
-	static void postInitialize(unsigned int, void* parameter, void*);
+	static uint32_t postInitializeTime = 0;
+	static Options* options;
+
+	static void postInitialize();
 	static void ipObtained();
 
 	void Initialize(Options& options)
 	{
 		// Wait for ethernet to become available.
+		DEBUG_LOG("Waiting for ethernet\r\n");
 		while (!USPiEthernetAvailable())
 		{
 			MsDelay(500);
@@ -21,14 +30,22 @@ namespace Net
 
 		// Wait 3 seconds, then run postInitialize
 		const auto optionsVoid = static_cast<void*>(&options);
-		StartKernelTimer(3 * HZ, postInitialize, optionsVoid, nullptr);
+		DEBUG_LOG("Scheduled post-init\r\n");
+		Net::options = &options;
+		postInitializeTime = read32(ARM_SYSTIMER_CLO) + 30000;
 	}
 
 	void Update()
 	{
+		if (postInitializeTime && read32(ARM_SYSTIMER_CLO) > postInitializeTime)
+		{
+			postInitialize();
+			postInitializeTime = 0;
+		}
+
 		unsigned int bufferSize = 0;
 		uint8_t buffer[USPI_FRAME_BUFFER_SIZE];
-		if (!USPiEthernetAvailable() || !USPiReceiveFrame(buffer, &bufferSize))
+		if (!USPiReceiveFrame(buffer, &bufferSize))
 		{
 			return;
 		}
@@ -38,7 +55,7 @@ namespace Net
 		if (headerSize == 0 || headerSize != Ethernet::Header::SerializedLength())
 		{
 			DEBUG_LOG(
-				"Dropped ethernet packet (invalid buffer size %lu, expected at least %lu)\r\n",
+				"Dropped ethernet packet (invalid buffer size %u, expected at least %u)\r\n",
 				headerSize,
 				Ethernet::Header::SerializedLength());
 			return;
@@ -55,40 +72,51 @@ namespace Net
 		}
 	}
 
-	static void postInitialize(unsigned int, void* parameter, void*)
+	static void postInitialize()
 	{
 		DEBUG_LOG("Running network post-init\r\n");
-		const auto options = static_cast<const Options*>(parameter);
 
 		if (options->GetDHCPEnable())
 		{
+			DEBUG_LOG("DHCP enabled, trying to obtain IP\r\n");
 			std::function<void()> callback = ipObtained;
 			Dhcp::ObtainIp(callback);
 		}
 		else
 		{
 			// Try parsing the IP address in the options.
-			uint8_t ip[4];
+			unsigned int ip[4];
-			int scanned = sscanf(
+			char dot;
-				options->GetIPAddress(), "%hhu.%hhu.%hhu.%hhu", &ip[3], &ip[2], &ip[1], &ip[0]);
+			int scanned =
+				sscanf(options->GetIPAddress(), "%u.%u.%u.%u", &ip[0], &ip[1], &ip[2], &ip[3]);
 
 			if (scanned == 4)
 			{
-				DEBUG_LOG("Setting IP address %d.%d.%d.%d\r\n", ip[3], ip[2], ip[1], ip[0]);
+				DEBUG_LOG("Setting IP address %u.%u.%u.%u\r\n", ip[0], ip[1], ip[2], ip[3]);
-				Utils::Ipv4Address = *reinterpret_cast<uint32_t*>(ip);
+				Utils::Ipv4Address = 0;
-			}
+				for (int i = 0; i < 4; i++)
+				{
+					Utils::Ipv4Address <<= 8;
+					Utils::Ipv4Address |= ip[i];
+				}
 
-			ipObtained();
+				ipObtained();
+			}
+			else
+			{
+				DEBUG_LOG("Invalid IP address '%s'\r\n", options->GetIPAddress());
+			}
 		}
 	}
 
 	static void ipObtained()
 	{
-#ifdef DEBUG
+		DEBUG_LOG(
-		uint8_t* ip = reinterpret_cast<uint8_t*>(Utils::Ipv4Address);
+			"Obtained IP address %ld.%ld.%ld.%ld\r\n",
-		DEBUG_LOG("Obtained IP address %d.%d.%d.%d\r\n", ip[3], ip[2], ip[1], ip[0]);
+			Utils::Ipv4Address >> 24,
-#endif
+			(Utils::Ipv4Address >> 16) & 0xFF,
-
+			(Utils::Ipv4Address >> 8) & 0xFF,
+			Utils::Ipv4Address & 0xFF);
 		Arp::SendAnnouncement(Utils::GetMacAddress(), Utils::Ipv4Address);
 	}
 } // namespace Net