diff --git a/src/main.cpp b/src/main.cpp index e3f9375..0a1b4cf 100644 --- a/src/main.cpp +++ b/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); + } } } diff --git a/src/net-arp.h b/src/net-arp.h index 449bc2e..2244e95 100644 --- a/src/net-arp.h +++ b/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; diff --git a/src/net-ethernet.h b/src/net-ethernet.h index c950597..53f09fd 100644 --- a/src/net-ethernet.h +++ b/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 Deserialize(uint8_t* buffer) + static EthernetFrame Deserialize(const uint8_t* buffer) { EthernetFrame self; diff --git a/src/net-icmp.h b/src/net-icmp.h index 4308d1a..36befcb 100644 --- a/src/net-icmp.h +++ b/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 Deserialize(const uint8_t* buffer) + { + IcmpPacket self; + self.header = IcmpPacketHeader::Deserialize(buffer); + self.payload = T::Deserialize(buffer + sizeof(IcmpPacketHeader)); + return self; + } } __attribute__((packed)); template -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 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 +{ + 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 +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 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)); diff --git a/src/net-ipv4.h b/src/net-ipv4.h index d93b53e..12e8540 100644 --- a/src/net-ipv4.h +++ b/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 @@ -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)), payload(payload) @@ -91,4 +125,13 @@ struct Ipv4Packet i += payload.Serialize(buffer + i); return i; } + + static Ipv4Packet Deserialize(const uint8_t* buffer) + { + Ipv4Packet self; + self.header = Ipv4Header::Deserialize(buffer); + self.payload = T::Deserialize(buffer + sizeof(Ipv4Header)); + return self; + } + } __attribute__((packed)); diff --git a/src/net.cpp b/src/net.cpp index d126479..62ade8e 100644 --- a/src/net.cpp +++ b/src/net.cpp @@ -7,8 +7,6 @@ #include "types.h" #include -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::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::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>::Deserialize(buffer); + const auto packetHeader = frame.payload.payload; + + if (packetHeader.type == ICMP_ECHO_REQUEST) + { + // TODO This should not be hardcoded lol + typedef EthernetFrame>>> Frame; + auto frameReq = Frame::Deserialize(buffer); + + auto echoReq = frameReq.payload.payload.payload; + + IcmpEchoResponse echoResp; + echoResp.identifier = echoReq.identifier; + echoResp.sequenceNumber = echoReq.sequenceNumber; + memcpy(echoResp.data, echoReq.data, 56); + + const auto sourceIp = frame.payload.header.sourceIp; + + IcmpPacket icmpResp(ICMP_ECHO_REPLY, 0, echoResp); + + Ipv4Packet ipv4Resp( + IP_PROTO_ICMP, + Ipv4Address, + sourceIp, + icmpResp + ); + + EthernetFrame 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 ArpTable; diff --git a/src/net.h b/src/net.h index 061618f..bb9c69a 100644 --- a/src/net.h +++ b/src/net.h @@ -15,10 +15,9 @@ enum ArpOperation { ARP_OPERATION_REPLY = 2, }; -typedef std::array MacAddress; +typedef std::array MacAddress; - -void HandleArpFrame(std::uint8_t* buffer); +void HandleArpFrame(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 ArpTable; struct UdpDatagramHeader