From cff82bbad3e20a3bee7fdb77ecfa63957a8dc8c3 Mon Sep 17 00:00:00 2001 From: Sijmen Schoon Date: Thu, 10 Dec 2020 23:53:56 +0100 Subject: [PATCH] Hackily implement ARP and echo request support --- Makefile | 3 +- src/main.cpp | 412 +++++++++++++-------------------------------- src/net-arp.h | 74 ++++++++ src/net-ethernet.h | 102 +++++++++++ src/net-icmp.h | 76 +++++++++ src/net-ipv4.h | 94 +++++++++++ src/net.cpp | 105 ++++++++++++ src/net.h | 28 +++ 8 files changed, 599 insertions(+), 295 deletions(-) create mode 100644 src/net-arp.h create mode 100644 src/net-ethernet.h create mode 100644 src/net-icmp.h create mode 100644 src/net-ipv4.h create mode 100644 src/net.cpp create mode 100644 src/net.h diff --git a/Makefile b/Makefile index fa62367..b3e31b6 100644 --- a/Makefile +++ b/Makefile @@ -3,7 +3,8 @@ OBJS = armc-start.o armc-cstartup.o armc-cstubs.o armc-cppstubs.o \ rpi-gpio.o rpi-interrupts.o dmRotary.o cache.o ff.o interrupt.o Keyboard.o performance.o \ Drive.o Pi1541.o DiskImage.o iec_bus.o iec_commands.o m6502.o m6522.o \ gcr.o prot.o lz.o emmc.o diskio.o options.o Screen.o SSD1306.o ScreenLCD.o \ - Timer.o FileBrowser.o DiskCaddy.o ROMs.o InputMappings.o xga_font_data.o m8520.o wd177x.o Pi1581.o SpinLock.o + Timer.o FileBrowser.o DiskCaddy.o ROMs.o InputMappings.o xga_font_data.o m8520.o wd177x.o Pi1581.o SpinLock.o \ + net.o SRCDIR = src OBJS := $(addprefix $(SRCDIR)/, $(OBJS)) diff --git a/src/main.cpp b/src/main.cpp index 312120b..a7fe109 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -19,6 +19,7 @@ #include #include #include +#include #include "defs.h" #include @@ -52,6 +53,12 @@ extern "C" #include "sample.h" #include "ssd_logo.h" +#include "net.h" +#include "net-ethernet.h" +#include "net-icmp.h" +#include "net-arp.h" +#include "net-ipv4.h" + unsigned versionMajor = 1; unsigned versionMinor = 23; @@ -137,290 +144,6 @@ unsigned int screenHeight = 768; const char* termainalTextRed = "\E[31m"; const char* termainalTextNormal = "\E[0m"; - -const uint32_t crc_tab32[256] = { - 0x00000000ul, 0x77073096ul, 0xEE0E612Cul, 0x990951BAul, - 0x076DC419ul, 0x706AF48Ful, 0xE963A535ul, 0x9E6495A3ul, - 0x0EDB8832ul, 0x79DCB8A4ul, 0xE0D5E91Eul, 0x97D2D988ul, - 0x09B64C2Bul, 0x7EB17CBDul, 0xE7B82D07ul, 0x90BF1D91ul, - 0x1DB71064ul, 0x6AB020F2ul, 0xF3B97148ul, 0x84BE41DEul, - 0x1ADAD47Dul, 0x6DDDE4EBul, 0xF4D4B551ul, 0x83D385C7ul, - 0x136C9856ul, 0x646BA8C0ul, 0xFD62F97Aul, 0x8A65C9ECul, - 0x14015C4Ful, 0x63066CD9ul, 0xFA0F3D63ul, 0x8D080DF5ul, - 0x3B6E20C8ul, 0x4C69105Eul, 0xD56041E4ul, 0xA2677172ul, - 0x3C03E4D1ul, 0x4B04D447ul, 0xD20D85FDul, 0xA50AB56Bul, - 0x35B5A8FAul, 0x42B2986Cul, 0xDBBBC9D6ul, 0xACBCF940ul, - 0x32D86CE3ul, 0x45DF5C75ul, 0xDCD60DCFul, 0xABD13D59ul, - 0x26D930ACul, 0x51DE003Aul, 0xC8D75180ul, 0xBFD06116ul, - 0x21B4F4B5ul, 0x56B3C423ul, 0xCFBA9599ul, 0xB8BDA50Ful, - 0x2802B89Eul, 0x5F058808ul, 0xC60CD9B2ul, 0xB10BE924ul, - 0x2F6F7C87ul, 0x58684C11ul, 0xC1611DABul, 0xB6662D3Dul, - 0x76DC4190ul, 0x01DB7106ul, 0x98D220BCul, 0xEFD5102Aul, - 0x71B18589ul, 0x06B6B51Ful, 0x9FBFE4A5ul, 0xE8B8D433ul, - 0x7807C9A2ul, 0x0F00F934ul, 0x9609A88Eul, 0xE10E9818ul, - 0x7F6A0DBBul, 0x086D3D2Dul, 0x91646C97ul, 0xE6635C01ul, - 0x6B6B51F4ul, 0x1C6C6162ul, 0x856530D8ul, 0xF262004Eul, - 0x6C0695EDul, 0x1B01A57Bul, 0x8208F4C1ul, 0xF50FC457ul, - 0x65B0D9C6ul, 0x12B7E950ul, 0x8BBEB8EAul, 0xFCB9887Cul, - 0x62DD1DDFul, 0x15DA2D49ul, 0x8CD37CF3ul, 0xFBD44C65ul, - 0x4DB26158ul, 0x3AB551CEul, 0xA3BC0074ul, 0xD4BB30E2ul, - 0x4ADFA541ul, 0x3DD895D7ul, 0xA4D1C46Dul, 0xD3D6F4FBul, - 0x4369E96Aul, 0x346ED9FCul, 0xAD678846ul, 0xDA60B8D0ul, - 0x44042D73ul, 0x33031DE5ul, 0xAA0A4C5Ful, 0xDD0D7CC9ul, - 0x5005713Cul, 0x270241AAul, 0xBE0B1010ul, 0xC90C2086ul, - 0x5768B525ul, 0x206F85B3ul, 0xB966D409ul, 0xCE61E49Ful, - 0x5EDEF90Eul, 0x29D9C998ul, 0xB0D09822ul, 0xC7D7A8B4ul, - 0x59B33D17ul, 0x2EB40D81ul, 0xB7BD5C3Bul, 0xC0BA6CADul, - 0xEDB88320ul, 0x9ABFB3B6ul, 0x03B6E20Cul, 0x74B1D29Aul, - 0xEAD54739ul, 0x9DD277AFul, 0x04DB2615ul, 0x73DC1683ul, - 0xE3630B12ul, 0x94643B84ul, 0x0D6D6A3Eul, 0x7A6A5AA8ul, - 0xE40ECF0Bul, 0x9309FF9Dul, 0x0A00AE27ul, 0x7D079EB1ul, - 0xF00F9344ul, 0x8708A3D2ul, 0x1E01F268ul, 0x6906C2FEul, - 0xF762575Dul, 0x806567CBul, 0x196C3671ul, 0x6E6B06E7ul, - 0xFED41B76ul, 0x89D32BE0ul, 0x10DA7A5Aul, 0x67DD4ACCul, - 0xF9B9DF6Ful, 0x8EBEEFF9ul, 0x17B7BE43ul, 0x60B08ED5ul, - 0xD6D6A3E8ul, 0xA1D1937Eul, 0x38D8C2C4ul, 0x4FDFF252ul, - 0xD1BB67F1ul, 0xA6BC5767ul, 0x3FB506DDul, 0x48B2364Bul, - 0xD80D2BDAul, 0xAF0A1B4Cul, 0x36034AF6ul, 0x41047A60ul, - 0xDF60EFC3ul, 0xA867DF55ul, 0x316E8EEFul, 0x4669BE79ul, - 0xCB61B38Cul, 0xBC66831Aul, 0x256FD2A0ul, 0x5268E236ul, - 0xCC0C7795ul, 0xBB0B4703ul, 0x220216B9ul, 0x5505262Ful, - 0xC5BA3BBEul, 0xB2BD0B28ul, 0x2BB45A92ul, 0x5CB36A04ul, - 0xC2D7FFA7ul, 0xB5D0CF31ul, 0x2CD99E8Bul, 0x5BDEAE1Dul, - 0x9B64C2B0ul, 0xEC63F226ul, 0x756AA39Cul, 0x026D930Aul, - 0x9C0906A9ul, 0xEB0E363Ful, 0x72076785ul, 0x05005713ul, - 0x95BF4A82ul, 0xE2B87A14ul, 0x7BB12BAEul, 0x0CB61B38ul, - 0x92D28E9Bul, 0xE5D5BE0Dul, 0x7CDCEFB7ul, 0x0BDBDF21ul, - 0x86D3D2D4ul, 0xF1D4E242ul, 0x68DDB3F8ul, 0x1FDA836Eul, - 0x81BE16CDul, 0xF6B9265Bul, 0x6FB077E1ul, 0x18B74777ul, - 0x88085AE6ul, 0xFF0F6A70ul, 0x66063BCAul, 0x11010B5Cul, - 0x8F659EFFul, 0xF862AE69ul, 0x616BFFD3ul, 0x166CCF45ul, - 0xA00AE278ul, 0xD70DD2EEul, 0x4E048354ul, 0x3903B3C2ul, - 0xA7672661ul, 0xD06016F7ul, 0x4969474Dul, 0x3E6E77DBul, - 0xAED16A4Aul, 0xD9D65ADCul, 0x40DF0B66ul, 0x37D83BF0ul, - 0xA9BCAE53ul, 0xDEBB9EC5ul, 0x47B2CF7Ful, 0x30B5FFE9ul, - 0xBDBDF21Cul, 0xCABAC28Aul, 0x53B39330ul, 0x24B4A3A6ul, - 0xBAD03605ul, 0xCDD70693ul, 0x54DE5729ul, 0x23D967BFul, - 0xB3667A2Eul, 0xC4614AB8ul, 0x5D681B02ul, 0x2A6F2B94ul, - 0xB40BBE37ul, 0xC30C8EA1ul, 0x5A05DF1Bul, 0x2D02EF8Dul -}; - - -uint32_t crc_32( const char *input_str, size_t num_bytes ) { - - uint32_t crc; - const char *ptr; - size_t a; - - crc = 0xFFFFFFFFul; - ptr = input_str; - - if ( ptr != NULL ) for (a=0; a> 8) ^ crc_tab32[ (crc ^ (uint32_t) *ptr++) & 0x000000FFul ]; - } - - return (crc ^ 0xFFFFFFFFul); - -} /* crc_32 */ - - -uint16_t InternetChecksum(const void* data, size_t size) -{ - uint32_t sum = 0; - while (size > 1) { - sum += *(uint16_t*)data; - data = (uint16_t*)data + 1; - - size -= 2; - } - - if (size > 0) { - sum += *(uint16_t*)data; - } - - while (sum >> 16) { - sum = (sum & 0xFFFF) + (sum >> 16); - } - - return ~sum; -} - - -template -struct IcmpPacket -{ - uint8_t type; - uint8_t code; - uint16_t checksum; - T payload; - - IcmpPacket(uint8_t type, uint8_t code, T payload) - : type(type), code(code), checksum(0), payload(payload) - {} - - size_t Serialize(char *buffer) { - size_t i = 0; - buffer[i++] = type; - buffer[i++] = code; - buffer[i++] = 0; - buffer[i++] = 0; - i += payload.Serialize(buffer + i); - - checksum = InternetChecksum(buffer, i); - buffer[2] = checksum; - buffer[3] = checksum >> 8; - - return i; - } -} __attribute__((packed)); - -template -struct Ipv4Packet -{ - unsigned int version : 4; - unsigned int ihl : 4; - unsigned int dscp : 6; - unsigned int ecn : 2; - uint16_t totalLength; - uint16_t identification; - unsigned int flags : 3; - unsigned int fragmentOffset : 13; - uint8_t ttl; - uint8_t protocol; - uint16_t headerChecksum; - uint32_t sourceIp; - uint32_t destinationIp; - T payload; - - Ipv4Packet( - uint8_t protocol, uint32_t sourceIp, uint32_t destinationIp, T payload - ) : - version(4), ihl(5), dscp(0), ecn(0), totalLength(sizeof(Ipv4Packet)), - identification(0), flags(0), fragmentOffset(0), ttl(64), - protocol(protocol), headerChecksum(0), sourceIp(sourceIp), - destinationIp(destinationIp), payload(payload) - {} - - size_t Serialize(char *buffer) - { - size_t i = 0; - buffer[i++] = version << 4 | ihl; - buffer[i++] = dscp << 2 | ecn; - buffer[i++] = totalLength >> 8; - buffer[i++] = totalLength; - buffer[i++] = identification >> 8; - buffer[i++] = identification; - buffer[i++] = (flags << 13 | fragmentOffset) >> 8; - buffer[i++] = flags << 13 | fragmentOffset; - buffer[i++] = ttl; - buffer[i++] = protocol; - buffer[i++] = 0; - buffer[i++] = 0; - buffer[i++] = sourceIp >> 24; - buffer[i++] = sourceIp >> 16; - buffer[i++] = sourceIp >> 8; - buffer[i++] = sourceIp; - buffer[i++] = destinationIp >> 24; - buffer[i++] = destinationIp >> 16; - buffer[i++] = destinationIp >> 8; - buffer[i++] = destinationIp; - i += payload.Serialize(buffer + i); - - headerChecksum = InternetChecksum(buffer, i); - buffer[10] = headerChecksum; - buffer[11] = headerChecksum >> 8; - - return i; - } -} __attribute__((packed)); - -template -struct IcmpEchoRequest -{ - uint16_t identifier; - uint16_t sequenceNumber; - T data; - - IcmpEchoRequest(T data) : identifier(0), sequenceNumber(0), data(data) - {} - - size_t Serialize(char *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; - } -} __attribute__((packed)); - - -struct UdpDatagramHeader -{ - uint16_t sourcePort; - uint16_t destinationPort; - uint16_t length; - uint16_t checksum; -}; - - -template -struct EthernetFrame -{ - uint8_t macDestination[6]; - uint8_t macSource[6]; - uint16_t type; - T payload; - uint32_t crc; - - EthernetFrame(uint16_t type, T payload) : - macDestination{255, 255, 255, 255, 255, 255}, - macSource{0, 0, 0, 0, 0, 0}, - type(type), - payload(payload), - crc(0) - {} - - size_t Serialize(char* buffer) - { - crc = 0; - - size_t i = 0; - memcpy(buffer + i, macDestination, sizeof(macDestination)); - i += sizeof(macDestination); - memcpy(buffer + i, macSource, sizeof(macSource)); - i += sizeof(macSource); - - buffer[i++] = type >> 8; - buffer[i++] = type; - - 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 = crc_32(buffer, i); - - buffer[i++] = crc; - buffer[i++] = crc >> 8; - buffer[i++] = crc >> 16; - buffer[i++] = crc >> 24; - - return i; - } -} __attribute__((packed)); - - - // Hooks required for USPi library extern "C" { @@ -2219,24 +1942,125 @@ extern "C" } const auto ipAddress = 0x0A00000B; - char ipBuffer[USPI_FRAME_BUFFER_SIZE]; + uint8_t ipBuffer[USPI_FRAME_BUFFER_SIZE]; + std::unordered_map> arpTable; + if (USPiEthernetAvailable()) { + std::array macAddress; + USPiGetMACAddress(macAddress.data()); + snprintf(tempBuffer, tempBufferSize, "Ethernet found"); screen.PrintText(false, 0, y_pos+=16, tempBuffer, COLOUR_WHITE, COLOUR_BLACK); - int value = 1337; - IcmpEchoRequest ping(value); - IcmpPacket icmp(8, 0, ping); - Ipv4Packet ip(1, ipAddress, 0x0A00000A, icmp); - EthernetFrame frame(0x0800, ip); - USPiGetMACAddress(frame.macSource); + // Send an ARP announcement + { + Ipv4ArpPacket arp(ARP_OPERATION_REPLY); - size_t size = frame.Serialize(ipBuffer); + arp.senderMac = macAddress; + arp.senderIp = ipAddress; - while (true) { + arp.targetMac.fill(0); + arp.targetIp = ipAddress; + + EthernetFrame frame(ETHERTYPE_ARP, arp); + frame.header.macDestination.fill(0xFF); // Broadcast + frame.header.macSource = macAddress; + + size_t size = frame.Serialize(ipBuffer); USPiSendFrame(ipBuffer, size); - MsDelay(1000); + } + + while (true) + { + size_t size; + if (!USPiReceiveFrame(ipBuffer, &size)) + { + const auto targetIp = 0x0A00000A; + const auto targetMacIter = arpTable.find(targetIp); + + if (targetMacIter != arpTable.end()) + { + const auto targetMac = targetMacIter->second; + + int value = 1337; + IcmpEchoRequest ping(value); + IcmpPacket icmp(8, 0, ping); + Ipv4Packet ip(1, ipAddress, targetIp, icmp); + + EthernetFrame frame(ETHERTYPE_IPV4, ip); + frame.header.macDestination = targetMac; + frame.header.macSource = macAddress; + + size_t size = frame.Serialize(ipBuffer); + USPiSendFrame(ipBuffer, size); + } + else + { + // Send ARP request + Ipv4ArpPacket arp(ARP_OPERATION_REQUEST); + + arp.senderMac = macAddress; + arp.senderIp = ipAddress; + + arp.targetMac.fill(0); + arp.targetIp = 0x0A00000A; + + EthernetFrame frame(ETHERTYPE_ARP, arp); + frame.header.macDestination.fill(0xFF); // Broadcast + frame.header.macSource = macAddress; + + size_t size = frame.Serialize(ipBuffer); + USPiSendFrame(ipBuffer, size); + } + + MsDelay(1000); + continue; + } + + auto header = EthernetFrameHeader::Deserialize(ipBuffer); + + snprintf( + tempBuffer, + tempBufferSize, + "Received frame of ethertype %04x", + header.type + ); + screen.PrintText(false, 0, y_pos+=16, tempBuffer, COLOUR_WHITE, COLOUR_BLACK); + + if (header.type == ETHERTYPE_ARP) + { + auto frame = EthernetFrame::Deserialize(ipBuffer); + const auto arp = frame.payload; + + if (arp.hardwareType == 1 && + arp.protocolType == ETHERTYPE_IPV4 && + arp.operation == ARP_OPERATION_REQUEST && + arp.targetIp == ipAddress) + { + Ipv4ArpPacket arpReply(ARP_OPERATION_REPLY); + + arpReply.targetMac = arp.senderMac; + arpReply.senderMac = macAddress; + + arpReply.targetIp = arp.senderIp; + arpReply.senderIp = ipAddress; + + EthernetFrame frame(ETHERTYPE_ARP, arpReply); + frame.header.macSource = macAddress; + + size_t size = frame.Serialize(ipBuffer); + USPiSendFrame(ipBuffer, size); + } + else if (arp.hardwareType == 1 && + arp.protocolType == ETHERTYPE_IPV4 && + arp.operation == ARP_OPERATION_REPLY && + arp.targetIp == ipAddress && + arp.targetMac == macAddress) + { + arpTable.insert(std::make_pair(arp.senderIp, arp.senderMac)); + } + } } } diff --git a/src/net-arp.h b/src/net-arp.h new file mode 100644 index 0000000..449bc2e --- /dev/null +++ b/src/net-arp.h @@ -0,0 +1,74 @@ +#pragma once + +struct Ipv4ArpPacket +{ + std::uint16_t hardwareType; + std::uint16_t protocolType; + std::uint8_t hardwareAddressLength; + std::uint8_t protocolAddressLength; + std::uint16_t operation; + + MacAddress senderMac; + std::uint32_t senderIp; + MacAddress targetMac; + std::uint32_t targetIp; + + Ipv4ArpPacket() {} + + Ipv4ArpPacket(std::uint16_t operation) : + hardwareType(1), // Ethernet + protocolType(ETHERTYPE_IPV4), // IPv4 + hardwareAddressLength(6), + protocolAddressLength(4), + operation(operation) + {} + + std::size_t Serialize(std::uint8_t* buffer) + { + buffer[0] = hardwareType >> 8; + buffer[1] = hardwareType; + buffer[2] = protocolType >> 8; + buffer[3] = protocolType; + buffer[4] = hardwareAddressLength; + buffer[5] = protocolAddressLength; + buffer[6] = operation >> 8; + buffer[7] = operation; + + memcpy(buffer + 8, senderMac.data(), 6); + + buffer[14] = senderIp >> 24; + buffer[15] = senderIp >> 16; + buffer[16] = senderIp >> 8; + buffer[17] = senderIp; + + memcpy(buffer + 18, targetMac.data(), 6); + + buffer[24] = targetIp >> 24; + buffer[25] = targetIp >> 16; + buffer[26] = targetIp >> 8; + buffer[27] = targetIp; + + return 28; + } + + static Ipv4ArpPacket Deserialize(uint8_t* buffer) + { + Ipv4ArpPacket self; + + self.hardwareType = buffer[0] << 8 | buffer[1]; + self.protocolType = buffer[2] << 8 | buffer[3]; + self.hardwareAddressLength = buffer[4]; + self.protocolAddressLength = buffer[5]; + self.operation = buffer[6] << 8 | buffer[7]; + + memcpy(self.senderMac.data(), buffer + 8, 6); + self.senderIp = + buffer[14] << 24 | buffer[15] << 16 | buffer[16] << 8 | buffer[17]; + memcpy(self.targetMac.data(), buffer + 18, 6); + self.targetIp = + buffer[24] << 24 | buffer[25] << 16 | buffer[26] << 8 | buffer[27]; + + return self; + } +} __attribute__((packed)); + diff --git a/src/net-ethernet.h b/src/net-ethernet.h new file mode 100644 index 0000000..8a19874 --- /dev/null +++ b/src/net-ethernet.h @@ -0,0 +1,102 @@ +#pragma once +#include "net.h" + +struct EthernetFrameHeader +{ + MacAddress macDestination; + MacAddress macSource; + std::uint16_t type; + + EthernetFrameHeader(std::uint16_t type) : + macDestination{255, 255, 255, 255, 255, 255}, + macSource{0, 0, 0, 0, 0, 0}, + type(type) + { + } + + EthernetFrameHeader() : EthernetFrameHeader(0) {} + + std::size_t Serialize(uint8_t* buffer) + { + std::size_t i = 0; + + std::memcpy(buffer + i, macDestination.data(), macDestination.size()); + i += sizeof(macDestination); + + std::memcpy(buffer + i, macSource.data(), macSource.size()); + i += sizeof(macSource); + + buffer[i++] = type >> 8; + buffer[i++] = type; + + return i; + } + + static EthernetFrameHeader Deserialize(uint8_t* buffer) + { + EthernetFrameHeader self; + memcpy(self.macDestination.data(), buffer + 0, self.macDestination.size()); + memcpy(self.macSource.data(), buffer + 6, self.macSource.size()); + self.type = buffer[12] << 8 | buffer[13]; + return self; + } +} __attribute__((packed)); + + +template +struct EthernetFrame +{ + EthernetFrameHeader header; + T payload; + std::uint32_t crc; + + EthernetFrame() {} + + EthernetFrame(std::uint16_t type, T payload) : header(type), payload(payload), crc(0) + { + } + + 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 Deserialize(uint8_t* buffer) + { + EthernetFrame 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)); diff --git a/src/net-icmp.h b/src/net-icmp.h new file mode 100644 index 0000000..d42f28d --- /dev/null +++ b/src/net-icmp.h @@ -0,0 +1,76 @@ +#pragma once +#include "net.h" + +struct IcmpPacketHeader +{ + std::uint8_t type; + std::uint8_t code; + std::uint16_t checksum; + + IcmpPacketHeader(std::uint8_t type, std::uint8_t code) : + type(type), code(code), checksum(0) + { + } + + std::size_t Serialize(uint8_t* buffer) { + size_t i = 0; + buffer[i++] = type; + buffer[i++] = code; + buffer[i++] = checksum; + buffer[i++] = checksum >> 8; + return i; + } +} __attribute__((packed)); + + +template +struct IcmpPacket +{ + IcmpPacketHeader header; + T payload; + + 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; + } +} __attribute__((packed)); + +template +struct IcmpEchoRequest +{ + uint16_t identifier; + uint16_t sequenceNumber; + T data; + + 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; + } +} __attribute__((packed)); diff --git a/src/net-ipv4.h b/src/net-ipv4.h new file mode 100644 index 0000000..7d11518 --- /dev/null +++ b/src/net-ipv4.h @@ -0,0 +1,94 @@ +#pragma once +#include "net.h" + +struct Ipv4Header +{ + unsigned int version : 4; + unsigned int ihl : 4; + unsigned int dscp : 6; + unsigned int ecn : 2; + uint16_t totalLength; + uint16_t identification; + unsigned int flags : 3; + unsigned int fragmentOffset : 13; + uint8_t ttl; + uint8_t protocol; + uint16_t headerChecksum; + uint32_t sourceIp; + uint32_t destinationIp; + + Ipv4Header( + uint8_t protocol, uint32_t sourceIp, uint32_t destinationIp, uint16_t totalLength + ) : + version(4), + ihl(5), + dscp(0), + ecn(0), + totalLength(totalLength), + identification(0), + flags(0), + fragmentOffset(0), + ttl(64), + protocol(protocol), + headerChecksum(0), + sourceIp(sourceIp), + destinationIp(destinationIp) + { + } + + size_t Serialize(uint8_t* buffer) + { + size_t i = 0; + + buffer[i++] = version << 4 | ihl; + buffer[i++] = dscp << 2 | ecn; + buffer[i++] = totalLength >> 8; + buffer[i++] = totalLength; + buffer[i++] = identification >> 8; + buffer[i++] = identification; + buffer[i++] = (flags << 13 | fragmentOffset) >> 8; + buffer[i++] = flags << 13 | fragmentOffset; + buffer[i++] = ttl; + buffer[i++] = protocol; + + headerChecksum = 0; + buffer[i++] = headerChecksum; + buffer[i++] = headerChecksum >> 8; + + buffer[i++] = sourceIp >> 24; + buffer[i++] = sourceIp >> 16; + buffer[i++] = sourceIp >> 8; + buffer[i++] = sourceIp; + buffer[i++] = destinationIp >> 24; + buffer[i++] = destinationIp >> 16; + buffer[i++] = destinationIp >> 8; + buffer[i++] = destinationIp; + + headerChecksum = internetChecksum(buffer, i); + buffer[10] = headerChecksum; + buffer[11] = headerChecksum >> 8; + + return i; + } +} __attribute__((packed)); + +template +struct Ipv4Packet +{ + Ipv4Header header; + T payload; + + Ipv4Packet(uint8_t protocol, uint32_t sourceIp, uint32_t destinationIp, T payload) : + header(protocol, sourceIp, destinationIp, sizeof(Ipv4Packet)), + payload(payload) + { + } + + size_t Serialize(uint8_t* buffer) + { + size_t i = 0; + i += header.Serialize(buffer); + i += payload.Serialize(buffer + i); + return i; + } +} __attribute__((packed)); diff --git a/src/net.cpp b/src/net.cpp new file mode 100644 index 0000000..ec0583c --- /dev/null +++ b/src/net.cpp @@ -0,0 +1,105 @@ +#include "net.h" +#include + +// +// Helpers +// +static const std::uint32_t crcTab32[256] = { + 0x00000000ul, 0x77073096ul, 0xEE0E612Cul, 0x990951BAul, + 0x076DC419ul, 0x706AF48Ful, 0xE963A535ul, 0x9E6495A3ul, + 0x0EDB8832ul, 0x79DCB8A4ul, 0xE0D5E91Eul, 0x97D2D988ul, + 0x09B64C2Bul, 0x7EB17CBDul, 0xE7B82D07ul, 0x90BF1D91ul, + 0x1DB71064ul, 0x6AB020F2ul, 0xF3B97148ul, 0x84BE41DEul, + 0x1ADAD47Dul, 0x6DDDE4EBul, 0xF4D4B551ul, 0x83D385C7ul, + 0x136C9856ul, 0x646BA8C0ul, 0xFD62F97Aul, 0x8A65C9ECul, + 0x14015C4Ful, 0x63066CD9ul, 0xFA0F3D63ul, 0x8D080DF5ul, + 0x3B6E20C8ul, 0x4C69105Eul, 0xD56041E4ul, 0xA2677172ul, + 0x3C03E4D1ul, 0x4B04D447ul, 0xD20D85FDul, 0xA50AB56Bul, + 0x35B5A8FAul, 0x42B2986Cul, 0xDBBBC9D6ul, 0xACBCF940ul, + 0x32D86CE3ul, 0x45DF5C75ul, 0xDCD60DCFul, 0xABD13D59ul, + 0x26D930ACul, 0x51DE003Aul, 0xC8D75180ul, 0xBFD06116ul, + 0x21B4F4B5ul, 0x56B3C423ul, 0xCFBA9599ul, 0xB8BDA50Ful, + 0x2802B89Eul, 0x5F058808ul, 0xC60CD9B2ul, 0xB10BE924ul, + 0x2F6F7C87ul, 0x58684C11ul, 0xC1611DABul, 0xB6662D3Dul, + 0x76DC4190ul, 0x01DB7106ul, 0x98D220BCul, 0xEFD5102Aul, + 0x71B18589ul, 0x06B6B51Ful, 0x9FBFE4A5ul, 0xE8B8D433ul, + 0x7807C9A2ul, 0x0F00F934ul, 0x9609A88Eul, 0xE10E9818ul, + 0x7F6A0DBBul, 0x086D3D2Dul, 0x91646C97ul, 0xE6635C01ul, + 0x6B6B51F4ul, 0x1C6C6162ul, 0x856530D8ul, 0xF262004Eul, + 0x6C0695EDul, 0x1B01A57Bul, 0x8208F4C1ul, 0xF50FC457ul, + 0x65B0D9C6ul, 0x12B7E950ul, 0x8BBEB8EAul, 0xFCB9887Cul, + 0x62DD1DDFul, 0x15DA2D49ul, 0x8CD37CF3ul, 0xFBD44C65ul, + 0x4DB26158ul, 0x3AB551CEul, 0xA3BC0074ul, 0xD4BB30E2ul, + 0x4ADFA541ul, 0x3DD895D7ul, 0xA4D1C46Dul, 0xD3D6F4FBul, + 0x4369E96Aul, 0x346ED9FCul, 0xAD678846ul, 0xDA60B8D0ul, + 0x44042D73ul, 0x33031DE5ul, 0xAA0A4C5Ful, 0xDD0D7CC9ul, + 0x5005713Cul, 0x270241AAul, 0xBE0B1010ul, 0xC90C2086ul, + 0x5768B525ul, 0x206F85B3ul, 0xB966D409ul, 0xCE61E49Ful, + 0x5EDEF90Eul, 0x29D9C998ul, 0xB0D09822ul, 0xC7D7A8B4ul, + 0x59B33D17ul, 0x2EB40D81ul, 0xB7BD5C3Bul, 0xC0BA6CADul, + 0xEDB88320ul, 0x9ABFB3B6ul, 0x03B6E20Cul, 0x74B1D29Aul, + 0xEAD54739ul, 0x9DD277AFul, 0x04DB2615ul, 0x73DC1683ul, + 0xE3630B12ul, 0x94643B84ul, 0x0D6D6A3Eul, 0x7A6A5AA8ul, + 0xE40ECF0Bul, 0x9309FF9Dul, 0x0A00AE27ul, 0x7D079EB1ul, + 0xF00F9344ul, 0x8708A3D2ul, 0x1E01F268ul, 0x6906C2FEul, + 0xF762575Dul, 0x806567CBul, 0x196C3671ul, 0x6E6B06E7ul, + 0xFED41B76ul, 0x89D32BE0ul, 0x10DA7A5Aul, 0x67DD4ACCul, + 0xF9B9DF6Ful, 0x8EBEEFF9ul, 0x17B7BE43ul, 0x60B08ED5ul, + 0xD6D6A3E8ul, 0xA1D1937Eul, 0x38D8C2C4ul, 0x4FDFF252ul, + 0xD1BB67F1ul, 0xA6BC5767ul, 0x3FB506DDul, 0x48B2364Bul, + 0xD80D2BDAul, 0xAF0A1B4Cul, 0x36034AF6ul, 0x41047A60ul, + 0xDF60EFC3ul, 0xA867DF55ul, 0x316E8EEFul, 0x4669BE79ul, + 0xCB61B38Cul, 0xBC66831Aul, 0x256FD2A0ul, 0x5268E236ul, + 0xCC0C7795ul, 0xBB0B4703ul, 0x220216B9ul, 0x5505262Ful, + 0xC5BA3BBEul, 0xB2BD0B28ul, 0x2BB45A92ul, 0x5CB36A04ul, + 0xC2D7FFA7ul, 0xB5D0CF31ul, 0x2CD99E8Bul, 0x5BDEAE1Dul, + 0x9B64C2B0ul, 0xEC63F226ul, 0x756AA39Cul, 0x026D930Aul, + 0x9C0906A9ul, 0xEB0E363Ful, 0x72076785ul, 0x05005713ul, + 0x95BF4A82ul, 0xE2B87A14ul, 0x7BB12BAEul, 0x0CB61B38ul, + 0x92D28E9Bul, 0xE5D5BE0Dul, 0x7CDCEFB7ul, 0x0BDBDF21ul, + 0x86D3D2D4ul, 0xF1D4E242ul, 0x68DDB3F8ul, 0x1FDA836Eul, + 0x81BE16CDul, 0xF6B9265Bul, 0x6FB077E1ul, 0x18B74777ul, + 0x88085AE6ul, 0xFF0F6A70ul, 0x66063BCAul, 0x11010B5Cul, + 0x8F659EFFul, 0xF862AE69ul, 0x616BFFD3ul, 0x166CCF45ul, + 0xA00AE278ul, 0xD70DD2EEul, 0x4E048354ul, 0x3903B3C2ul, + 0xA7672661ul, 0xD06016F7ul, 0x4969474Dul, 0x3E6E77DBul, + 0xAED16A4Aul, 0xD9D65ADCul, 0x40DF0B66ul, 0x37D83BF0ul, + 0xA9BCAE53ul, 0xDEBB9EC5ul, 0x47B2CF7Ful, 0x30B5FFE9ul, + 0xBDBDF21Cul, 0xCABAC28Aul, 0x53B39330ul, 0x24B4A3A6ul, + 0xBAD03605ul, 0xCDD70693ul, 0x54DE5729ul, 0x23D967BFul, + 0xB3667A2Eul, 0xC4614AB8ul, 0x5D681B02ul, 0x2A6F2B94ul, + 0xB40BBE37ul, 0xC30C8EA1ul, 0x5A05DF1Bul, 0x2D02EF8Dul +}; + +std::uint32_t crc32(const std::uint8_t *buffer, std::size_t size) { + std::uint32_t crc = 0xFFFFFFFFul; + + for (std::size_t a = 0; a < size; a++) + { + auto index = (crc ^ (std::uint32_t)*buffer++) & 0x000000FFul; + crc = (crc >> 8) ^ crcTab32[index]; + } + + return crc ^ 0xFFFFFFFFul; +} + +std::uint16_t internetChecksum(const void* data, std::size_t size) +{ + std::uint32_t sum = 0; + while (size > 1) { + sum += *(std::uint16_t*)data; + data = (std::uint16_t*)data + 1; + + size -= 2; + } + + if (size > 0) { + sum += *(std::uint16_t*)data; + } + + while (sum >> 16) { + sum = (sum & 0xFFFF) + (sum >> 16); + } + + return ~sum; +} diff --git a/src/net.h b/src/net.h new file mode 100644 index 0000000..3b506a0 --- /dev/null +++ b/src/net.h @@ -0,0 +1,28 @@ +#pragma once +#include +#include +#include +#include + +enum EtherType { + ETHERTYPE_IPV4 = 0x0800, + ETHERTYPE_ARP = 0x0806, +}; + +enum ArpOperation { + ARP_OPERATION_REQUEST = 1, + ARP_OPERATION_REPLY = 2, +}; + +typedef std::array MacAddress; + +std::uint32_t crc32(const std::uint8_t* buffer, std::size_t size); +std::uint16_t internetChecksum(const void* data, std::size_t size); + +struct UdpDatagramHeader +{ + std::uint16_t sourcePort; + std::uint16_t destinationPort; + std::uint16_t length; + std::uint16_t checksum; +};