c64
/
pi1541
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Hackily implement ARP and echo request support

This commit is contained in:
Sijmen 2020-12-10 23:53:56 +01:00
parent 1cabaf2b04
commit cff82bbad3
Signed by: vijfhoek
GPG Key ID: DAF7821E067D9C48
8 changed files with 599 additions and 295 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
Makefile
View File

@ -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))

404
src/main.cpp
View File

@ -19,6 +19,7 @@
#include <algorithm>
#include <array>
#include <numeric>
#include <unordered_map>
#include "defs.h"
#include <string.h>
@ -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<num_bytes; a++) {
crc = (crc >> 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<class T>
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<class T>
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<T>)),
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 <class T>
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<class T>
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<std::uint32_t, std::array<std::uint8_t, 6>> arpTable;
if (USPiEthernetAvailable()) {
std::array<std::uint8_t, 6> macAddress;
USPiGetMACAddress(macAddress.data());
snprintf(tempBuffer, tempBufferSize, "Ethernet found");
screen.PrintText(false, 0, y_pos+=16, tempBuffer, COLOUR_WHITE, COLOUR_BLACK);
// Send an ARP announcement
{
Ipv4ArpPacket arp(ARP_OPERATION_REPLY);
arp.senderMac = macAddress;
arp.senderIp = ipAddress;
arp.targetMac.fill(0);
arp.targetIp = ipAddress;
EthernetFrame<decltype(arp)> frame(ETHERTYPE_ARP, arp);
frame.header.macDestination.fill(0xFF); // Broadcast
frame.header.macSource = macAddress;
size_t size = frame.Serialize(ipBuffer);
USPiSendFrame(ipBuffer, size);
}
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<int> ping(value);
IcmpPacket<decltype(ping)> icmp(8, 0, ping);
Ipv4Packet<decltype(icmp)> ip(1, ipAddress, 0x0A00000A, icmp);
Ipv4Packet<decltype(icmp)> ip(1, ipAddress, targetIp, icmp);
EthernetFrame<decltype(ip)> frame(0x0800, ip);
USPiGetMACAddress(frame.macSource);
EthernetFrame<decltype(ip)> frame(ETHERTYPE_IPV4, ip);
frame.header.macDestination = targetMac;
frame.header.macSource = macAddress;
size_t size = frame.Serialize(ipBuffer);
while (true) {
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<decltype(arp)> 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<Ipv4ArpPacket>::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<decltype(arpReply)> 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));
}
}
}
}

74
src/net-arp.h Normal file
View File

@ -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));

102
src/net-ethernet.h Normal file
View File

@ -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 <class T>
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<T> Deserialize(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));

76
src/net-icmp.h Normal file
View File

@ -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 <class T>
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 <class T>
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));

94
src/net-ipv4.h Normal file
View File

@ -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<class T>
struct Ipv4Packet
{
Ipv4Header header;
T payload;
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;
}
} __attribute__((packed));

105
src/net.cpp Normal file
View File

@ -0,0 +1,105 @@
#include "net.h"
#include <unordered_map>
//
// 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;
}

28
src/net.h Normal file
View File

@ -0,0 +1,28 @@
#pragma once
#include <cstdint>
#include <cstring>
#include <algorithm>
#include <array>
enum EtherType {
ETHERTYPE_IPV4 = 0x0800,
ETHERTYPE_ARP = 0x0806,
};
enum ArpOperation {
ARP_OPERATION_REQUEST = 1,
ARP_OPERATION_REPLY = 2,
};
typedef std::array<std::uint8_t, 6> 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;
};