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

Respond to ICMP Echo Requests (aka pings)

This commit is contained in:
Sijmen 2020-12-11 03:39:21 +01:00
parent 18c5bbd29d
commit 9cfcb0ddf7
Signed by: vijfhoek
GPG Key ID: DAF7821E067D9C48
7 changed files with 265 additions and 31 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
src/main.cpp
View File

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

2
src/net-arp.h
View File

@ -51,7 +51,7 @@ struct Ipv4ArpPacket
return 28;
}
static Ipv4ArpPacket Deserialize(uint8_t* buffer)
static Ipv4ArpPacket Deserialize(const uint8_t* buffer)
{
Ipv4ArpPacket self;

27
src/net-ethernet.h
View File

@ -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<T> Deserialize(uint8_t* buffer)
static EthernetFrame<T> Deserialize(const uint8_t* buffer)
{
EthernetFrame<T> self;

115
src/net-icmp.h
View File

@ -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,6 +70,14 @@ struct IcmpPacket
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>
@ -57,6 +87,8 @@ struct IcmpEchoRequest
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<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)
{}
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 <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));

43
src/net-ipv4.h
View File

@ -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<class T>
@ -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<T>)),
payload(payload)
@ -91,4 +125,13 @@ struct Ipv4Packet
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));

78
src/net.cpp
View File

@ -7,8 +7,6 @@
#include "types.h"
#include <uspi.h>
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<Ipv4ArpPacket>::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<Ipv4Header>::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<Ipv4Packet<IcmpPacketHeader>>::Deserialize(buffer);
const auto packetHeader = frame.payload.payload;
if (packetHeader.type == ICMP_ECHO_REQUEST)
{
// TODO This should not be hardcoded lol
typedef EthernetFrame<Ipv4Packet<IcmpPacket<IcmpEchoRequest<uint8_t[56]>>>> Frame;
auto frameReq = Frame::Deserialize(buffer);
auto echoReq = frameReq.payload.payload.payload;
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,
icmpResp
);
EthernetFrame<decltype(ipv4Resp)> 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<std::uint32_t, MacAddress> ArpTable;

12
src/net.h
View File

@ -15,10 +15,9 @@ enum ArpOperation {
ARP_OPERATION_REPLY = 2,
};
typedef std::array<std::uint8_t, 6> MacAddress;
typedef std::array<uint8_t, 6> 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<std::uint32_t, MacAddress> ArpTable;
struct UdpDatagramHeader