Get rid of the template stuff that didn't work well for this situation.

src/main.cpp

@@ -369,14 +369,14 @@ void InitialiseLCD()
 
 void updateNetwork()
 {
-	size_t size;
+	unsigned int size = 0;
 	uint8_t ipBuffer[USPI_FRAME_BUFFER_SIZE];
 	if (!USPiEthernetAvailable() || !USPiReceiveFrame(ipBuffer, &size))
 	{
 		return;
 	}
 
-	auto header = EthernetFrameHeader::Deserialize(ipBuffer);
+	auto ethernetHeader = EthernetFrameHeader::Deserialize(ipBuffer);
 
 	static bool announcementSent = false;
 	if (!announcementSent)
@@ -385,13 +385,13 @@ void updateNetwork()
 		announcementSent = true;
 	}
 
-	switch (header.type)
+	switch (ethernetHeader.type)
 	{
 	case ETHERTYPE_ARP:
-		HandleArpFrame(ipBuffer);
+		HandleArpFrame(ethernetHeader, ipBuffer + ethernetHeader.SerializedLength());
 		break;
 	case ETHERTYPE_IPV4:
-		HandleIpv4Frame(ipBuffer);
+		HandleIpv4Packet(ethernetHeader, ipBuffer + ethernetHeader.SerializedLength());
 		break;
 	}
 }

src/net-arp.h

@@ -24,6 +24,20 @@ struct Ipv4ArpPacket
 		operation(operation)
 	{}
 
+	constexpr std::size_t SerializedLength() const
+	{
+		return
+			sizeof(hardwareType) +
+			sizeof(protocolType) +
+			sizeof(hardwareAddressLength) +
+			sizeof(protocolAddressLength) +
+			sizeof(operation) +
+			senderMac.size() +
+			sizeof(senderIp) +
+			targetMac.size() +
+			sizeof(targetIp);
+	}
+
 	std::size_t Serialize(std::uint8_t* buffer)
 	{
 		buffer[0] = hardwareType >> 8;
@@ -34,7 +48,7 @@ struct Ipv4ArpPacket
 		buffer[5] = protocolAddressLength;
 		buffer[6] = operation >> 8;
 		buffer[7] = operation;
-		
+
 		memcpy(buffer + 8, senderMac.data(), 6);
 
 		buffer[14] = senderIp >> 24;
@@ -71,5 +85,4 @@ struct Ipv4ArpPacket
 
 		return self;
 	}
-} __attribute__((packed));
+};
-

src/net-ethernet.h

@@ -21,10 +21,10 @@ struct EthernetFrameHeader
 
 	constexpr static std::size_t SerializedLength()
 	{
-		return sizeof(EthernetFrameHeader);
+		return sizeof(macDestination) + sizeof(macSource) + sizeof(type);
 	}
 
-	std::size_t Serialize(uint8_t* buffer)
+	std::size_t Serialize(uint8_t* buffer) const
 	{
 		std::size_t i = 0;
 
@@ -43,75 +43,9 @@ struct EthernetFrameHeader
 	static EthernetFrameHeader Deserialize(const uint8_t* buffer)
 	{
 		EthernetFrameHeader self;
-		memcpy(self.macDestination.data(), buffer + 0, self.macDestination.size());
+		std::memcpy(self.macDestination.data(), buffer + 0, self.macDestination.size());
-		memcpy(self.macSource.data(), buffer + 6, self.macSource.size());
+		std::memcpy(self.macSource.data(), buffer + 6, self.macSource.size());
 		self.type = buffer[12] << 8 | buffer[13];
 		return self;
 	}
-} __attribute__((packed));
+};
-
-
-template <class T>
-struct EthernetFrame
-{
-	EthernetFrameHeader header;
-	T payload;
-	std::uint32_t crc;
-
-	EthernetFrame() {}
-
-	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)
-	{
-		std::size_t i = 0;
-		i += header.Serialize(buffer);
-
-		std::size_t payload_size = payload.Serialize(buffer + i);
-		i += payload_size;
-
-		// Pad data to 46 bytes
-		for (; payload_size < 46; payload_size++) {
-			buffer[i++] = 0;
-		}
-
-		crc = Crc32(buffer, i);
-
-		buffer[i++] = crc;
-		buffer[i++] = crc >> 8;
-		buffer[i++] = crc >> 16;
-		buffer[i++] = crc >> 24;
-
-		return i;
-	}
-
-	static EthernetFrame<T> Deserialize(const uint8_t* buffer)
-	{
-		EthernetFrame<T> self;
-
-		self.header = EthernetFrameHeader::Deserialize(buffer);
-		size_t i = sizeof(EthernetFrameHeader);
-
-		// XXX Might want to base this on actual deserialized data, might not match.
-		std::size_t payloadSize = sizeof(T);
-		self.payload = T::Deserialize(buffer + i);
-
-		// Skip the padding
-		i += std::max(payloadSize, std::size_t{46});
-
-		self.crc =
-			buffer[i] << 24 | buffer[i + 1] << 16 | buffer[i + 2] << 8 | buffer[i + 3];
-		i += 4;
-
-		return self;
-	}
-} __attribute__((packed));

src/net-icmp.h

@@ -19,6 +19,11 @@ struct IcmpPacketHeader
 		type(type), code(code), checksum(0)
 	{}
 
+	constexpr static std::size_t SerializedLength()
+	{
+		return sizeof(type) + sizeof(code) + sizeof(checksum);
+	}
+
 	std::size_t Serialize(uint8_t* buffer) const
 	{
 		size_t i = 0;
@@ -37,93 +42,21 @@ struct IcmpPacketHeader
 		self.checksum = buffer[2] << 8 | buffer[3];
 		return self;
 	}
-} __attribute__((packed));
+};
 
-
+struct IcmpEchoHeader
-template <class T>
-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)
-	{
-		std::size_t i = 0;
-
-		header.checksum = 0;
-		i += header.Serialize(buffer);
-		i += payload.Serialize(buffer + i);
-
-		uint16_t checksum = InternetChecksum(buffer, i);
-		buffer[2] = checksum;
-		buffer[3] = checksum >> 8;
-
-		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
-{
-	uint16_t identifier;
-	uint16_t sequenceNumber;
-	T data;
-
-	IcmpEchoRequest() {}
-
-	IcmpEchoRequest(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 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)
+	IcmpEchoHeader() : IcmpEchoHeader(0, 0) {}
-	{}
+	IcmpEchoHeader(uint16_t identifier, uint16_t sequenceNumber) :
+		identifier(identifier), sequenceNumber(sequenceNumber) {}
+
+	constexpr static size_t SerializedLength()
+	{
+		return sizeof(identifier) + sizeof(sequenceNumber);
+	}
 
 	size_t Serialize(uint8_t* buffer)
 	{
@@ -135,47 +68,11 @@ struct IcmpEchoRequest<void>
 		return i;
 	}
 
-	static IcmpEchoRequest Deserialize(const uint8_t* buffer)
+	static IcmpEchoHeader Deserialize(const uint8_t* buffer)
 	{
-		IcmpEchoRequest self;
+		IcmpEchoHeader 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

@@ -10,14 +10,14 @@ enum IpProtocols
 
 struct Ipv4Header
 {
-	unsigned int version : 4;
+	uint8_t version;
-	unsigned int ihl : 4;
+	uint8_t ihl;
-	unsigned int dscp : 6;
+	uint8_t dscp;
-	unsigned int ecn : 2;
+	uint8_t ecn;
 	uint16_t totalLength;
 	uint16_t identification;
-	unsigned int flags : 3;
+	uint8_t flags;
-	unsigned int fragmentOffset : 13;
+	uint16_t fragmentOffset;
 	uint8_t ttl;
 	uint8_t protocol;
 	uint16_t headerChecksum;
@@ -47,10 +47,11 @@ struct Ipv4Header
 
 	static constexpr size_t SerializedLength()
 	{
-		return sizeof(Ipv4Header);
+		// Hardcoded because of bitfields.
+		return 20;
 	}
-	
+
-	size_t Serialize(uint8_t* buffer)
+	size_t Serialize(uint8_t* buffer) const
 	{
 		size_t i = 0;
 
@@ -65,9 +66,9 @@ struct Ipv4Header
 		buffer[i++] = ttl;
 		buffer[i++] = protocol;
 
-		headerChecksum = 0;
+		// Zero the checksum before calculating it
-		buffer[i++] = headerChecksum;
+		buffer[i++] = 0;
-		buffer[i++] = headerChecksum >> 8;
+		buffer[i++] = 0 >> 8;
 
 		buffer[i++] = sourceIp >> 24;
 		buffer[i++] = sourceIp >> 16;
@@ -78,9 +79,9 @@ struct Ipv4Header
 		buffer[i++] = destinationIp >> 8;
 		buffer[i++] = destinationIp;
 
-		headerChecksum = InternetChecksum(buffer, i);
+		uint16_t checksum = InternetChecksum(buffer, i);
-		buffer[10] = headerChecksum;
+		buffer[10] = checksum;
-		buffer[11] = headerChecksum >> 8;
+		buffer[11] = checksum >> 8;
 
 		return i;
 	}
@@ -109,36 +110,4 @@ struct Ipv4Header
 
 		return self;
 	}
-} __attribute__((packed));
+};
-
-template<class T>
-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)
-	{
-	}
-
-	size_t Serialize(uint8_t* buffer)
-	{
-		size_t i = 0;
-		i += header.Serialize(buffer);
-		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-tftp.h

@@ -18,7 +18,10 @@ struct TftpPacket
 
 	TftpPacket(uint16_t opcode) : opcode(opcode) {}
 
-	virtual size_t SerializedLength() const = 0;
+	virtual size_t SerializedLength() const {
+		return sizeof(opcode);
+	}
+
 	virtual size_t Serialize(uint8_t* buffer) const = 0;
 };
 
@@ -29,8 +32,8 @@ struct TftpWriteReadRequestPacket : public TftpPacket
 
 	TftpWriteReadRequestPacket(uint16_t opcode) : TftpPacket(opcode) {}
 
-	constexpr size_t SerializedLength() override {
+	size_t SerializedLength() const override {
-		return sizeof(opcode) + filename.size() + 1 + mode.size() + 1;
+		return TftpPacket::SerializedLength() + filename.size() + 1 + mode.size() + 1;
 	}
 
 	size_t Serialize(uint8_t* buffer) const override {
@@ -73,9 +76,9 @@ struct TftpErrorPacket : public TftpPacket
 		TftpPacket(TFTP_OP_ERROR), errorCode(errorCode), message(message)
 	{}
 
-	constexpr size_t SerializedLength() const override
+	size_t SerializedLength() const override
 	{
-		return sizeof(opcode) + sizeof(errorCode) + message.size() + 1;
+		return TftpPacket::SerializedLength() + sizeof(errorCode) + message.size() + 1;
 	}
 
 	size_t Serialize(uint8_t* buffer) const
@@ -85,7 +88,7 @@ struct TftpErrorPacket : public TftpPacket
 		buffer[i++] = opcode;
 		buffer[i++] = errorCode >> 8;
 		buffer[i++] = errorCode;
-		
+
 		i += message.copy(reinterpret_cast<char*>(buffer + i), message.size());
 		buffer[i++] = 0;
 
@@ -103,9 +106,9 @@ struct TftpAcknowledgementPacket : public TftpPacket
 		TftpPacket(TFTP_OP_ACKNOWLEDGEMENT), blockNumber(blockNumber)
 	{}
 
-	constexpr size_t SerializedLength() override
+	size_t SerializedLength() const override
 	{
-		return sizeof(opcode) + sizeof(blockNumber);
+		return TftpPacket::SerializedLength() + sizeof(blockNumber);
 	}
 
 	size_t Serialize(uint8_t* buffer) const override

src/net-udp.h

@@ -51,18 +51,4 @@ struct UdpDatagramHeader
 		self.checksum = buffer[6] << 8 | buffer[7];
 		return self;
 	}
-} __attribute__((packed));
+};
-
-template <class T>
-struct UdpDatagram
-{
-	UdpDatagramHeader header;
-	T payload;
-
-	UdpDatagram() {}
-
-	UdpDatagram(uint16_t sourcePort, uint16_t destinationPort, T payload) :
-		header(sourcePort, destinationPort, sizeof(UdpDatagram<T>)),
-		payload(payload)
-	{}
-} __attribute__((packed));

src/net.cpp

@@ -22,18 +22,18 @@ void SendArpPacket(
 	uint32_t targetIp,
 	uint32_t senderIp)
 {
-	Ipv4ArpPacket arp(operation);
+	Ipv4ArpPacket arpPacket(operation);
-	arp.targetMac = targetMac;
+	arpPacket.targetMac = targetMac;
-	arp.senderMac = senderMac;
+	arpPacket.senderMac = senderMac;
-	arp.targetIp = targetIp;
+	arpPacket.targetIp = targetIp;
-	arp.senderIp = senderIp;
+	arpPacket.senderIp = senderIp;
 
-	EthernetFrame<decltype(arp)> frame(ETHERTYPE_ARP, arp);
+	EthernetFrameHeader ethernetHeader(senderMac, targetMac, ETHERTYPE_ARP);
-	frame.header.macDestination = targetMac;
-	frame.header.macSource = senderMac;
 
 	uint8_t buffer[USPI_FRAME_BUFFER_SIZE];
-	const auto size = frame.Serialize(buffer);
+	size_t size = 0;
+	size += ethernetHeader.Serialize(buffer + size);
+	size += arpPacket.Serialize(buffer + size);
 	USPiSendFrame(buffer, size);
 }
 
@@ -54,79 +54,75 @@ void SendArpAnnouncement(MacAddress mac, uint32_t ip)
 	SendArpReply(MacBroadcast, mac, ip, ip);
 }
 
-void HandleArpFrame(uint8_t* buffer)
+void HandleArpFrame(const EthernetFrameHeader ethernetHeader, uint8_t* buffer)
 {
 	const auto macAddress = GetMacAddress();
+	const auto arpPacket = Ipv4ArpPacket::Deserialize(buffer);
 
-	const auto frame = EthernetFrame<Ipv4ArpPacket>::Deserialize(buffer);
+	if (
-	const auto arp = frame.payload;
+		arpPacket.hardwareType == 1 &&
-
+		arpPacket.protocolType == ETHERTYPE_IPV4 &&
-	if (arp.hardwareType == 1 &&
+		arpPacket.operation == ARP_OPERATION_REQUEST &&
-		arp.protocolType == ETHERTYPE_IPV4 &&
+		arpPacket.targetIp == Ipv4Address)
-		arp.operation == ARP_OPERATION_REQUEST &&
-		arp.targetIp == Ipv4Address)
 	{
-		SendArpReply(arp.senderMac, macAddress, arp.senderIp, Ipv4Address);
+		SendArpReply(arpPacket.senderMac, macAddress, arpPacket.senderIp, Ipv4Address);
 	}
-	else if (arp.hardwareType == 1 &&
+
-			arp.protocolType == ETHERTYPE_IPV4 &&
+	else if (
-			arp.operation == ARP_OPERATION_REPLY &&
+		arpPacket.hardwareType == 1 &&
-			arp.targetIp == Ipv4Address &&
+		arpPacket.protocolType == ETHERTYPE_IPV4 &&
-			arp.targetMac == macAddress)
+		arpPacket.operation == ARP_OPERATION_REPLY &&
+		arpPacket.targetIp == Ipv4Address &&
+		arpPacket.targetMac == macAddress)
 	{
-		ArpTable.insert(std::make_pair(arp.senderIp, arp.senderMac));
+		ArpTable.insert(std::make_pair(arpPacket.senderIp, arpPacket.senderMac));
 	}
 }
 
 //
 // IPv4
 //
-void HandleIpv4Frame(const uint8_t* buffer)
+void HandleIpv4Packet(const EthernetFrameHeader ethernetHeader, const uint8_t* buffer)
 {
-	const auto frame = EthernetFrame<Ipv4Header>::Deserialize(buffer);
+	const auto ipv4Header = Ipv4Header::Deserialize(buffer);
-	const auto header = frame.payload;
 
 	// Update ARP table
-	ArpTable.insert(std::make_pair(frame.payload.sourceIp, frame.header.macSource));
+	ArpTable.insert(std::make_pair(ipv4Header.sourceIp, ethernetHeader.macSource));
 
-	if (header.version != 4) return;
+	if (ipv4Header.version != 4) return;
-	if (header.ihl != 5) return; // Not supported
+	if (ipv4Header.ihl != 5) return; // Not supported
-	if (header.destinationIp != Ipv4Address) return;
+	if (ipv4Header.destinationIp != Ipv4Address) return;
-	if (header.fragmentOffset != 0) return; // TODO Support this
+	if (ipv4Header.fragmentOffset != 0) return; // TODO Support this
 
-	if (header.protocol == IP_PROTO_ICMP)
+	if (ipv4Header.protocol == IP_PROTO_ICMP)
 	{
 		HandleIcmpFrame(buffer);
 	}
-	else if (header.protocol == IP_PROTO_UDP)
+	else if (ipv4Header.protocol == IP_PROTO_UDP)
 	{
-		return HandleUdpFrame(buffer);
+		HandleUdpDatagram(
+			ethernetHeader, ipv4Header, buffer + ipv4Header.SerializedLength());
 	}
 }
 
 //
 // UDP
 //
-void HandleUdpFrame(const uint8_t* buffer)
+void HandleUdpDatagram(
+	const EthernetFrameHeader ethernetHeader,
+	const Ipv4Header ipv4Header,
+	const uint8_t* buffer
+)
 {
-	const auto frame = EthernetFrame<Ipv4Packet<UdpDatagramHeader>>::Deserialize(buffer);
+	const auto udpHeader = UdpDatagramHeader::Deserialize(buffer);
-	const auto header = frame.payload.payload;
 
-	uint8_t* data = (uint8_t*)malloc(header.length);
+	if (udpHeader.destinationPort == 69) // nice
-	const auto size =
-		buffer +
-		EthernetFrameHeader::SerializedLength() +
-		Ipv4Header::SerializedLength() +
-		UdpDatagramHeader::SerializedLength();
-	memcpy(data, size, header.length);
-
-	if (header.destinationPort == 69) // nice
 	{
 		HandleTftpDatagram(
-			frame.header,
+			ethernetHeader,
-			frame.payload.header,
+			ipv4Header,
-			frame.payload.payload,
+			udpHeader,
-			data
+			buffer + udpHeader.SerializedLength()
 		);
 	}
 }
@@ -136,58 +132,65 @@ void HandleUdpFrame(const uint8_t* buffer)
 //
 void SendIcmpEchoRequest(MacAddress mac, uint32_t ip)
 {
-	IcmpEchoRequest<int> pingPacket(0);
+	IcmpPacketHeader icmpHeader(8, 0);
-	IcmpPacket<decltype(pingPacket)> icmpPacket(8, 0, pingPacket);
+	IcmpEchoHeader pingHeader(0, 0);
-	Ipv4Packet<decltype(icmpPacket)> ipv4Packet(1, Ipv4Address, ip, icmpPacket);
 
-	EthernetFrame<decltype(ipv4Packet)> frame(ETHERTYPE_IPV4, ipv4Packet);
+	size_t ipv4TotalSize = IcmpPacketHeader::SerializedLength() +
-	frame.header.macDestination = mac;
+		IcmpEchoHeader::SerializedLength() +
-	frame.header.macSource = GetMacAddress();
+		Ipv4Header::SerializedLength();
+	Ipv4Header ipv4Header(1, Ipv4Address, ip, ipv4TotalSize);
+
+	EthernetFrameHeader ethernetHeader(mac, GetMacAddress(), ETHERTYPE_IPV4);
 
 	uint8_t buffer[USPI_FRAME_BUFFER_SIZE];
-	size_t size = frame.Serialize(buffer);
+	size_t i = 0;
-	USPiSendFrame(buffer, size);
+
+	i += ethernetHeader.Serialize(buffer + i);
+	i += ipv4Header.Serialize(buffer + i);
+	i += pingHeader.Serialize(buffer + i);
+	i += icmpHeader.Serialize(buffer + 1);
+
+	USPiSendFrame(buffer, i);
 }
 
 void HandleIcmpFrame(const uint8_t* buffer)
 {
-	const auto frame = EthernetFrame<Ipv4Packet<IcmpPacketHeader>>::Deserialize(buffer);
+	size_t requestSize = 0;
-	const auto packetHeader = frame.payload.payload;
+	const auto requestEthernetHeader = EthernetFrameHeader::Deserialize(buffer + requestSize);
+	requestSize += requestEthernetHeader.SerializedLength();
+	const auto requestIpv4Header = Ipv4Header::Deserialize(buffer + requestSize);
+	requestSize += requestIpv4Header.SerializedLength();
+	const auto requestIcmpHeader = IcmpPacketHeader::Deserialize(buffer + requestSize);
+	requestSize += requestIcmpHeader.SerializedLength();
 
-	if (packetHeader.type == ICMP_ECHO_REQUEST)
+	if (requestIcmpHeader.type == ICMP_ECHO_REQUEST)
 	{
-		// TODO This should not be hardcoded lol
+		const auto requestEchoHeader = IcmpEchoHeader::Deserialize(buffer + requestSize);
-		typedef EthernetFrame<Ipv4Packet<IcmpPacket<IcmpEchoRequest<uint8_t[56]>>>> Frame;
+		requestSize += requestEchoHeader.SerializedLength();
-		auto frameReq = Frame::Deserialize(buffer);
 
-		auto echoReq = frameReq.payload.payload.payload;
+		const IcmpPacketHeader responseIcmpHeader(ICMP_ECHO_REPLY, 0);
-
+		const Ipv4Header responseIpv4Header(
-		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,
+			requestIpv4Header.sourceIp,
-			icmpResp
+			requestIpv4Header.totalLength
 		);
+		const EthernetFrameHeader responseEthernetHeader(
+			requestEthernetHeader.macSource, GetMacAddress(), ETHERTYPE_IPV4);
 
-		EthernetFrame<decltype(ipv4Resp)> frameResp(
+		const auto payloadLength = requestIpv4Header.totalLength -
-			frame.header.macSource,
+			requestIpv4Header.SerializedLength() -
-			GetMacAddress(),
+			requestIcmpHeader.SerializedLength() -
-			ETHERTYPE_IPV4,
+			requestEchoHeader.SerializedLength();
-			ipv4Resp
-		);
 
-		uint8_t bufferResp[USPI_FRAME_BUFFER_SIZE];
+		std::array<uint8_t, USPI_FRAME_BUFFER_SIZE> bufferResp;
-		const auto size = frameResp.Serialize(bufferResp);
+		size_t respSize = 0;
-		USPiSendFrame(bufferResp, size);
+		respSize += responseEthernetHeader.Serialize(bufferResp.data() + respSize);
+		respSize += responseIpv4Header.Serialize(bufferResp.data() + respSize);
+		respSize += responseIcmpHeader.Serialize(bufferResp.data() + respSize);
+		memcpy(bufferResp.data() + respSize, buffer + requestSize, payloadLength);
+		respSize += payloadLength;
+		USPiSendFrame(bufferResp.data(), respSize);
 	}
 }
 

src/net.h

@@ -17,38 +17,40 @@ enum ArpOperation {
 
 typedef std::array<uint8_t, 6> MacAddress;
 
+struct EthernetFrameHeader;
+struct UdpDatagramHeader;
+struct Ipv4Header;
+
 //
 // ARP
 //
-void HandleArpFrame(uint8_t* buffer);
+void HandleArpFrame(EthernetFrameHeader header, uint8_t* buffer);
-void SendArpPacket(ArpOperation operation,
+void SendArpPacket(
-					MacAddress targetMac,
+	ArpOperation operation,
-					MacAddress senderMac,
+	MacAddress targetMac,
-					uint32_t senderIp,
+	MacAddress senderMac,
-					uint32_t targetIp);
+	uint32_t senderIp,
-void SendArpRequest(MacAddress targetMac,
+	uint32_t targetIp
-					MacAddress senderMac,
+);
-					uint32_t senderIp,
+void SendArpRequest(
-					uint32_t targetIp);
+	MacAddress targetMac, MacAddress senderMac, uint32_t senderIp, uint32_t targetIp);
-void SendArpReply(MacAddress targetMac,
+void SendArpReply(
-					MacAddress senderMac,
+	MacAddress targetMac, MacAddress senderMac, uint32_t senderIp, uint32_t targetIp);
-					uint32_t senderIp,
-					uint32_t targetIp);
 void SendArpAnnouncement(MacAddress mac, uint32_t ip);
 
 //
 // IPv4
 //
-void HandleIpv4Frame(const uint8_t* buffer);
+void HandleIpv4Packet(const EthernetFrameHeader ethernetHeader, const uint8_t* buffer);
 
 //
 // UDP
 //
-struct EthernetFrameHeader;
+void HandleUdpDatagram(
-struct UdpDatagramHeader;
+	const EthernetFrameHeader ethernetHeader,
-struct Ipv4Header;
+	const Ipv4Header ipv4Header,
-
+	const uint8_t* buffer
-void HandleUdpFrame(const uint8_t* buffer);
+);
 
 void HandleTftpDatagram(
 	const EthernetFrameHeader ethernetReqHeader,