diff --git a/src/net-arp.cpp b/src/net-arp.cpp index 232ee1e..6edb8e4 100644 --- a/src/net-arp.cpp +++ b/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; diff --git a/src/net-dhcp.cpp b/src/net-dhcp.cpp index 0f148aa..cbc2e40 100644 --- a/src/net-dhcp.cpp +++ b/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; diff --git a/src/net-icmp.cpp b/src/net-icmp.cpp index ea9ea85..20851d9 100644 --- a/src/net-icmp.cpp +++ b/src/net-icmp.cpp @@ -1,7 +1,9 @@ #include +#include #include #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(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(type); buffer[i++] = code; - buffer[i++] = checksum; - buffer[i++] = checksum >> 8; + buffer[i++] = 0; + 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 auto reqEchoHeaderSize = - 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 Ethernet::Header respEthernetHeader( + reqEthernetHeader.macSource, Utils::GetMacAddress(), Ethernet::EtherType::Ipv4); const Ipv4::Header respIpv4Header( Ipv4::Protocol::Icmp, Utils::Ipv4Address, reqIpv4Header.sourceIp, reqIpv4Header.totalLength); - const Ethernet::Header respEthernetHeader( - reqEthernetHeader.macSource, Utils::GetMacAddress(), Ethernet::EtherType::Ipv4); + Header respIcmpHeader(Type::EchoReply, 0); const auto payloadSize = reqIpv4Header.totalLength - reqIpv4Header.SerializedLength() - - reqIcmpHeader.SerializedLength() - reqEchoHeaderSize; + reqIcmpHeader.SerializedLength(); + + DEBUG_LOG("payloadSize: %u\r\n", payloadSize); std::array respBuffer; @@ -145,10 +148,8 @@ namespace Net::Icmp respBuffer.data() + respSize, respBuffer.size() - respSize); respSize += respIpv4Header.Serialize(respBuffer.data() + respSize, respBuffer.size() - respSize); - respSize += - respIcmpHeader.Serialize(respBuffer.data() + respSize, respBuffer.size() - respSize); - std::memcpy(respBuffer.data() + respSize, reqBuffer + reqEchoHeaderSize, payloadSize); - respSize += payloadSize; + respSize += respIcmpHeader.Serialize( + respBuffer.data() + respSize, respBuffer.size() - respSize, reqBuffer, 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 += - Icmp::Header::Deserialize(icmpHeader, buffer + headerSize, bufferSize - headerSize); - - const auto expectedHeaderSize = ethernetHeader.SerializedLength() + - ipv4Header.SerializedLength() + icmpHeader.SerializedLength(); - if (headerSize != expectedHeaderSize) + size_t headerSize = Icmp::Header::Deserialize(icmpHeader, buffer, bufferSize); + if (headerSize != Icmp::Header::SerializedLength()) { 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(icmpHeader.type)); + if (icmpHeader.type == Type::EchoRequest) { handleEchoRequest( diff --git a/src/net-icmp.h b/src/net-icmp.h index e162a89..b2d057f 100644 --- a/src/net-icmp.h +++ b/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 diff --git a/src/net-ipv4.cpp b/src/net-ipv4.cpp index 9beee28..9dc1fd2 100644 --- a/src/net-ipv4.cpp +++ b/src/net-ipv4.cpp @@ -9,6 +9,10 @@ #include "debug.h" +#include "types.h" +#include +#include + 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); } } diff --git a/src/net-tftp.cpp b/src/net-tftp.cpp index e42fdd7..f3fe6c0 100644 --- a/src/net-tftp.cpp +++ b/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); } diff --git a/src/net-udp.cpp b/src/net-udp.cpp index 9861f3e..fb60895 100644 --- a/src/net-udp.cpp +++ b/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; diff --git a/src/net.cpp b/src/net.cpp index 141c5e8..49ef71b 100644 --- a/src/net.cpp +++ b/src/net.cpp @@ -6,14 +6,23 @@ #include #include +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(&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(parameter); if (options->GetDHCPEnable()) { + DEBUG_LOG("DHCP enabled, trying to obtain IP\r\n"); std::function callback = ipObtained; Dhcp::ObtainIp(callback); } else { // Try parsing the IP address in the options. - uint8_t ip[4]; - int scanned = sscanf( - options->GetIPAddress(), "%hhu.%hhu.%hhu.%hhu", &ip[3], &ip[2], &ip[1], &ip[0]); + unsigned int ip[4]; + char dot; + 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]); - Utils::Ipv4Address = *reinterpret_cast(ip); - } + DEBUG_LOG("Setting IP address %u.%u.%u.%u\r\n", ip[0], ip[1], ip[2], ip[3]); + 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 - uint8_t* ip = reinterpret_cast(Utils::Ipv4Address); - DEBUG_LOG("Obtained IP address %d.%d.%d.%d\r\n", ip[3], ip[2], ip[1], ip[0]); -#endif - + DEBUG_LOG( + "Obtained IP address %ld.%ld.%ld.%ld\r\n", + Utils::Ipv4Address >> 24, + (Utils::Ipv4Address >> 16) & 0xFF, + (Utils::Ipv4Address >> 8) & 0xFF, + Utils::Ipv4Address & 0xFF); Arp::SendAnnouncement(Utils::GetMacAddress(), Utils::Ipv4Address); } } // namespace Net