Get rid of the template stuff that didn't work well for this situation.
This commit is contained in:
parent
41ee2d9777
commit
74a92bc9d5
GPG key ID:
DAF7821E067D9C48
9 changed files with 186 additions and 379 deletions
10
src/main.cpp
10
src/main.cpp
|
|
@ -369,14 +369,14 @@ void InitialiseLCD()
|
|||
|
||||
void updateNetwork()
|
||||
{
|
||||
size_t size;
|
||||
unsigned int size = 0;
|
||||
uint8_t ipBuffer[USPI_FRAME_BUFFER_SIZE];
|
||||
if (!USPiEthernetAvailable() || !USPiReceiveFrame(ipBuffer, &size))
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
auto header = EthernetFrameHeader::Deserialize(ipBuffer);
|
||||
auto ethernetHeader = EthernetFrameHeader::Deserialize(ipBuffer);
|
||||
|
||||
static bool announcementSent = false;
|
||||
if (!announcementSent)
|
||||
|
|
@ -385,13 +385,13 @@ void updateNetwork()
|
|||
announcementSent = true;
|
||||
}
|
||||
|
||||
switch (header.type)
|
||||
switch (ethernetHeader.type)
|
||||
{
|
||||
case ETHERTYPE_ARP:
|
||||
HandleArpFrame(ipBuffer);
|
||||
HandleArpFrame(ethernetHeader, ipBuffer + ethernetHeader.SerializedLength());
|
||||
break;
|
||||
case ETHERTYPE_IPV4:
|
||||
HandleIpv4Frame(ipBuffer);
|
||||
HandleIpv4Packet(ethernetHeader, ipBuffer + ethernetHeader.SerializedLength());
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -24,6 +24,20 @@ struct Ipv4ArpPacket
|
|||
operation(operation)
|
||||
{}
|
||||
|
||||
constexpr std::size_t SerializedLength() const
|
||||
{
|
||||
return
|
||||
sizeof(hardwareType) +
|
||||
sizeof(protocolType) +
|
||||
sizeof(hardwareAddressLength) +
|
||||
sizeof(protocolAddressLength) +
|
||||
sizeof(operation) +
|
||||
senderMac.size() +
|
||||
sizeof(senderIp) +
|
||||
targetMac.size() +
|
||||
sizeof(targetIp);
|
||||
}
|
||||
|
||||
std::size_t Serialize(std::uint8_t* buffer)
|
||||
{
|
||||
buffer[0] = hardwareType >> 8;
|
||||
|
|
@ -71,5 +85,4 @@ struct Ipv4ArpPacket
|
|||
|
||||
return self;
|
||||
}
|
||||
} __attribute__((packed));
|
||||
|
||||
};
|
||||
|
|
|
|||
|
|
@ -21,10 +21,10 @@ struct EthernetFrameHeader
|
|||
|
||||
constexpr static std::size_t SerializedLength()
|
||||
{
|
||||
return sizeof(EthernetFrameHeader);
|
||||
return sizeof(macDestination) + sizeof(macSource) + sizeof(type);
|
||||
}
|
||||
|
||||
std::size_t Serialize(uint8_t* buffer)
|
||||
std::size_t Serialize(uint8_t* buffer) const
|
||||
{
|
||||
std::size_t i = 0;
|
||||
|
||||
|
|
@ -43,75 +43,9 @@ struct EthernetFrameHeader
|
|||
static EthernetFrameHeader Deserialize(const uint8_t* buffer)
|
||||
{
|
||||
EthernetFrameHeader self;
|
||||
memcpy(self.macDestination.data(), buffer + 0, self.macDestination.size());
|
||||
memcpy(self.macSource.data(), buffer + 6, self.macSource.size());
|
||||
std::memcpy(self.macDestination.data(), buffer + 0, self.macDestination.size());
|
||||
std::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)
|
||||
{}
|
||||
|
||||
EthernetFrame(
|
||||
MacAddress macDestination,
|
||||
MacAddress macSource,
|
||||
std::uint16_t type,
|
||||
T payload
|
||||
) : header(macDestination, macSource, type), payload(payload)
|
||||
{}
|
||||
|
||||
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(const 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));
|
||||
};
|
||||
|
|
|
|||
139
src/net-icmp.h
139
src/net-icmp.h
|
|
@ -19,6 +19,11 @@ struct IcmpPacketHeader
|
|||
type(type), code(code), checksum(0)
|
||||
{}
|
||||
|
||||
constexpr static std::size_t SerializedLength()
|
||||
{
|
||||
return sizeof(type) + sizeof(code) + sizeof(checksum);
|
||||
}
|
||||
|
||||
std::size_t Serialize(uint8_t* buffer) const
|
||||
{
|
||||
size_t i = 0;
|
||||
|
|
@ -37,93 +42,21 @@ struct IcmpPacketHeader
|
|||
self.checksum = buffer[2] << 8 | buffer[3];
|
||||
return self;
|
||||
}
|
||||
} __attribute__((packed));
|
||||
};
|
||||
|
||||
|
||||
template <class T>
|
||||
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)
|
||||
{
|
||||
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;
|
||||
}
|
||||
|
||||
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>
|
||||
struct IcmpEchoRequest
|
||||
{
|
||||
uint16_t identifier;
|
||||
uint16_t sequenceNumber;
|
||||
T data;
|
||||
|
||||
IcmpEchoRequest() {}
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
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>
|
||||
struct IcmpEchoHeader
|
||||
{
|
||||
uint16_t identifier;
|
||||
uint16_t sequenceNumber;
|
||||
|
||||
IcmpEchoRequest() : identifier(0), sequenceNumber(0)
|
||||
{}
|
||||
IcmpEchoHeader() : IcmpEchoHeader(0, 0) {}
|
||||
IcmpEchoHeader(uint16_t identifier, uint16_t sequenceNumber) :
|
||||
identifier(identifier), sequenceNumber(sequenceNumber) {}
|
||||
|
||||
constexpr static size_t SerializedLength()
|
||||
{
|
||||
return sizeof(identifier) + sizeof(sequenceNumber);
|
||||
}
|
||||
|
||||
size_t Serialize(uint8_t* buffer)
|
||||
{
|
||||
|
|
@ -135,47 +68,11 @@ struct IcmpEchoRequest<void>
|
|||
return i;
|
||||
}
|
||||
|
||||
static IcmpEchoRequest Deserialize(const uint8_t* buffer)
|
||||
static IcmpEchoHeader Deserialize(const uint8_t* buffer)
|
||||
{
|
||||
IcmpEchoRequest self;
|
||||
IcmpEchoHeader 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));
|
||||
};
|
||||
|
|
|
|||
|
|
@ -10,14 +10,14 @@ enum IpProtocols
|
|||
|
||||
struct Ipv4Header
|
||||
{
|
||||
unsigned int version : 4;
|
||||
unsigned int ihl : 4;
|
||||
unsigned int dscp : 6;
|
||||
unsigned int ecn : 2;
|
||||
uint8_t version;
|
||||
uint8_t ihl;
|
||||
uint8_t dscp;
|
||||
uint8_t ecn;
|
||||
uint16_t totalLength;
|
||||
uint16_t identification;
|
||||
unsigned int flags : 3;
|
||||
unsigned int fragmentOffset : 13;
|
||||
uint8_t flags;
|
||||
uint16_t fragmentOffset;
|
||||
uint8_t ttl;
|
||||
uint8_t protocol;
|
||||
uint16_t headerChecksum;
|
||||
|
|
@ -47,10 +47,11 @@ struct Ipv4Header
|
|||
|
||||
static constexpr size_t SerializedLength()
|
||||
{
|
||||
return sizeof(Ipv4Header);
|
||||
// Hardcoded because of bitfields.
|
||||
return 20;
|
||||
}
|
||||
|
||||
size_t Serialize(uint8_t* buffer)
|
||||
size_t Serialize(uint8_t* buffer) const
|
||||
{
|
||||
size_t i = 0;
|
||||
|
||||
|
|
@ -65,9 +66,9 @@ struct Ipv4Header
|
|||
buffer[i++] = ttl;
|
||||
buffer[i++] = protocol;
|
||||
|
||||
headerChecksum = 0;
|
||||
buffer[i++] = headerChecksum;
|
||||
buffer[i++] = headerChecksum >> 8;
|
||||
// Zero the checksum before calculating it
|
||||
buffer[i++] = 0;
|
||||
buffer[i++] = 0 >> 8;
|
||||
|
||||
buffer[i++] = sourceIp >> 24;
|
||||
buffer[i++] = sourceIp >> 16;
|
||||
|
|
@ -78,9 +79,9 @@ struct Ipv4Header
|
|||
buffer[i++] = destinationIp >> 8;
|
||||
buffer[i++] = destinationIp;
|
||||
|
||||
headerChecksum = InternetChecksum(buffer, i);
|
||||
buffer[10] = headerChecksum;
|
||||
buffer[11] = headerChecksum >> 8;
|
||||
uint16_t checksum = InternetChecksum(buffer, i);
|
||||
buffer[10] = checksum;
|
||||
buffer[11] = checksum >> 8;
|
||||
|
||||
return i;
|
||||
}
|
||||
|
|
@ -109,36 +110,4 @@ struct Ipv4Header
|
|||
|
||||
return self;
|
||||
}
|
||||
} __attribute__((packed));
|
||||
|
||||
template<class T>
|
||||
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)
|
||||
{
|
||||
}
|
||||
|
||||
size_t Serialize(uint8_t* buffer)
|
||||
{
|
||||
size_t i = 0;
|
||||
i += header.Serialize(buffer);
|
||||
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));
|
||||
};
|
||||
|
|
|
|||
|
|
@ -18,7 +18,10 @@ struct TftpPacket
|
|||
|
||||
TftpPacket(uint16_t opcode) : opcode(opcode) {}
|
||||
|
||||
virtual size_t SerializedLength() const = 0;
|
||||
virtual size_t SerializedLength() const {
|
||||
return sizeof(opcode);
|
||||
}
|
||||
|
||||
virtual size_t Serialize(uint8_t* buffer) const = 0;
|
||||
};
|
||||
|
||||
|
|
@ -29,8 +32,8 @@ struct TftpWriteReadRequestPacket : public TftpPacket
|
|||
|
||||
TftpWriteReadRequestPacket(uint16_t opcode) : TftpPacket(opcode) {}
|
||||
|
||||
constexpr size_t SerializedLength() override {
|
||||
return sizeof(opcode) + filename.size() + 1 + mode.size() + 1;
|
||||
size_t SerializedLength() const override {
|
||||
return TftpPacket::SerializedLength() + filename.size() + 1 + mode.size() + 1;
|
||||
}
|
||||
|
||||
size_t Serialize(uint8_t* buffer) const override {
|
||||
|
|
@ -73,9 +76,9 @@ struct TftpErrorPacket : public TftpPacket
|
|||
TftpPacket(TFTP_OP_ERROR), errorCode(errorCode), message(message)
|
||||
{}
|
||||
|
||||
constexpr size_t SerializedLength() const override
|
||||
size_t SerializedLength() const override
|
||||
{
|
||||
return sizeof(opcode) + sizeof(errorCode) + message.size() + 1;
|
||||
return TftpPacket::SerializedLength() + sizeof(errorCode) + message.size() + 1;
|
||||
}
|
||||
|
||||
size_t Serialize(uint8_t* buffer) const
|
||||
|
|
@ -103,9 +106,9 @@ struct TftpAcknowledgementPacket : public TftpPacket
|
|||
TftpPacket(TFTP_OP_ACKNOWLEDGEMENT), blockNumber(blockNumber)
|
||||
{}
|
||||
|
||||
constexpr size_t SerializedLength() override
|
||||
size_t SerializedLength() const override
|
||||
{
|
||||
return sizeof(opcode) + sizeof(blockNumber);
|
||||
return TftpPacket::SerializedLength() + sizeof(blockNumber);
|
||||
}
|
||||
|
||||
size_t Serialize(uint8_t* buffer) const override
|
||||
|
|
|
|||
|
|
@ -51,18 +51,4 @@ struct UdpDatagramHeader
|
|||
self.checksum = buffer[6] << 8 | buffer[7];
|
||||
return self;
|
||||
}
|
||||
} __attribute__((packed));
|
||||
|
||||
template <class T>
|
||||
struct UdpDatagram
|
||||
{
|
||||
UdpDatagramHeader header;
|
||||
T payload;
|
||||
|
||||
UdpDatagram() {}
|
||||
|
||||
UdpDatagram(uint16_t sourcePort, uint16_t destinationPort, T payload) :
|
||||
header(sourcePort, destinationPort, sizeof(UdpDatagram<T>)),
|
||||
payload(payload)
|
||||
{}
|
||||
} __attribute__((packed));
|
||||
};
|
||||
|
|
|
|||
179
src/net.cpp
179
src/net.cpp
|
|
@ -22,18 +22,18 @@ void SendArpPacket(
|
|||
uint32_t targetIp,
|
||||
uint32_t senderIp)
|
||||
{
|
||||
Ipv4ArpPacket arp(operation);
|
||||
arp.targetMac = targetMac;
|
||||
arp.senderMac = senderMac;
|
||||
arp.targetIp = targetIp;
|
||||
arp.senderIp = senderIp;
|
||||
Ipv4ArpPacket arpPacket(operation);
|
||||
arpPacket.targetMac = targetMac;
|
||||
arpPacket.senderMac = senderMac;
|
||||
arpPacket.targetIp = targetIp;
|
||||
arpPacket.senderIp = senderIp;
|
||||
|
||||
EthernetFrame<decltype(arp)> frame(ETHERTYPE_ARP, arp);
|
||||
frame.header.macDestination = targetMac;
|
||||
frame.header.macSource = senderMac;
|
||||
EthernetFrameHeader ethernetHeader(senderMac, targetMac, ETHERTYPE_ARP);
|
||||
|
||||
uint8_t buffer[USPI_FRAME_BUFFER_SIZE];
|
||||
const auto size = frame.Serialize(buffer);
|
||||
size_t size = 0;
|
||||
size += ethernetHeader.Serialize(buffer + size);
|
||||
size += arpPacket.Serialize(buffer + size);
|
||||
USPiSendFrame(buffer, size);
|
||||
}
|
||||
|
||||
|
|
@ -54,79 +54,75 @@ void SendArpAnnouncement(MacAddress mac, uint32_t ip)
|
|||
SendArpReply(MacBroadcast, mac, ip, ip);
|
||||
}
|
||||
|
||||
void HandleArpFrame(uint8_t* buffer)
|
||||
void HandleArpFrame(const EthernetFrameHeader ethernetHeader, uint8_t* buffer)
|
||||
{
|
||||
const auto macAddress = GetMacAddress();
|
||||
const auto arpPacket = Ipv4ArpPacket::Deserialize(buffer);
|
||||
|
||||
const auto frame = EthernetFrame<Ipv4ArpPacket>::Deserialize(buffer);
|
||||
const auto arp = frame.payload;
|
||||
|
||||
if (arp.hardwareType == 1 &&
|
||||
arp.protocolType == ETHERTYPE_IPV4 &&
|
||||
arp.operation == ARP_OPERATION_REQUEST &&
|
||||
arp.targetIp == Ipv4Address)
|
||||
if (
|
||||
arpPacket.hardwareType == 1 &&
|
||||
arpPacket.protocolType == ETHERTYPE_IPV4 &&
|
||||
arpPacket.operation == ARP_OPERATION_REQUEST &&
|
||||
arpPacket.targetIp == Ipv4Address)
|
||||
{
|
||||
SendArpReply(arp.senderMac, macAddress, arp.senderIp, Ipv4Address);
|
||||
SendArpReply(arpPacket.senderMac, macAddress, arpPacket.senderIp, Ipv4Address);
|
||||
}
|
||||
else if (arp.hardwareType == 1 &&
|
||||
arp.protocolType == ETHERTYPE_IPV4 &&
|
||||
arp.operation == ARP_OPERATION_REPLY &&
|
||||
arp.targetIp == Ipv4Address &&
|
||||
arp.targetMac == macAddress)
|
||||
|
||||
else if (
|
||||
arpPacket.hardwareType == 1 &&
|
||||
arpPacket.protocolType == ETHERTYPE_IPV4 &&
|
||||
arpPacket.operation == ARP_OPERATION_REPLY &&
|
||||
arpPacket.targetIp == Ipv4Address &&
|
||||
arpPacket.targetMac == macAddress)
|
||||
{
|
||||
ArpTable.insert(std::make_pair(arp.senderIp, arp.senderMac));
|
||||
ArpTable.insert(std::make_pair(arpPacket.senderIp, arpPacket.senderMac));
|
||||
}
|
||||
}
|
||||
|
||||
//
|
||||
// IPv4
|
||||
//
|
||||
void HandleIpv4Frame(const uint8_t* buffer)
|
||||
void HandleIpv4Packet(const EthernetFrameHeader ethernetHeader, const uint8_t* buffer)
|
||||
{
|
||||
const auto frame = EthernetFrame<Ipv4Header>::Deserialize(buffer);
|
||||
const auto header = frame.payload;
|
||||
const auto ipv4Header = Ipv4Header::Deserialize(buffer);
|
||||
|
||||
// Update ARP table
|
||||
ArpTable.insert(std::make_pair(frame.payload.sourceIp, frame.header.macSource));
|
||||
ArpTable.insert(std::make_pair(ipv4Header.sourceIp, ethernetHeader.macSource));
|
||||
|
||||
if (header.version != 4) return;
|
||||
if (header.ihl != 5) return; // Not supported
|
||||
if (header.destinationIp != Ipv4Address) return;
|
||||
if (header.fragmentOffset != 0) return; // TODO Support this
|
||||
if (ipv4Header.version != 4) return;
|
||||
if (ipv4Header.ihl != 5) return; // Not supported
|
||||
if (ipv4Header.destinationIp != Ipv4Address) return;
|
||||
if (ipv4Header.fragmentOffset != 0) return; // TODO Support this
|
||||
|
||||
if (header.protocol == IP_PROTO_ICMP)
|
||||
if (ipv4Header.protocol == IP_PROTO_ICMP)
|
||||
{
|
||||
HandleIcmpFrame(buffer);
|
||||
}
|
||||
else if (header.protocol == IP_PROTO_UDP)
|
||||
else if (ipv4Header.protocol == IP_PROTO_UDP)
|
||||
{
|
||||
return HandleUdpFrame(buffer);
|
||||
HandleUdpDatagram(
|
||||
ethernetHeader, ipv4Header, buffer + ipv4Header.SerializedLength());
|
||||
}
|
||||
}
|
||||
|
||||
//
|
||||
// UDP
|
||||
//
|
||||
void HandleUdpFrame(const uint8_t* buffer)
|
||||
void HandleUdpDatagram(
|
||||
const EthernetFrameHeader ethernetHeader,
|
||||
const Ipv4Header ipv4Header,
|
||||
const uint8_t* buffer
|
||||
)
|
||||
{
|
||||
const auto frame = EthernetFrame<Ipv4Packet<UdpDatagramHeader>>::Deserialize(buffer);
|
||||
const auto header = frame.payload.payload;
|
||||
const auto udpHeader = UdpDatagramHeader::Deserialize(buffer);
|
||||
|
||||
uint8_t* data = (uint8_t*)malloc(header.length);
|
||||
const auto size =
|
||||
buffer +
|
||||
EthernetFrameHeader::SerializedLength() +
|
||||
Ipv4Header::SerializedLength() +
|
||||
UdpDatagramHeader::SerializedLength();
|
||||
memcpy(data, size, header.length);
|
||||
|
||||
if (header.destinationPort == 69) // nice
|
||||
if (udpHeader.destinationPort == 69) // nice
|
||||
{
|
||||
HandleTftpDatagram(
|
||||
frame.header,
|
||||
frame.payload.header,
|
||||
frame.payload.payload,
|
||||
data
|
||||
ethernetHeader,
|
||||
ipv4Header,
|
||||
udpHeader,
|
||||
buffer + udpHeader.SerializedLength()
|
||||
);
|
||||
}
|
||||
}
|
||||
|
|
@ -136,58 +132,65 @@ void HandleUdpFrame(const uint8_t* buffer)
|
|||
//
|
||||
void SendIcmpEchoRequest(MacAddress mac, uint32_t ip)
|
||||
{
|
||||
IcmpEchoRequest<int> pingPacket(0);
|
||||
IcmpPacket<decltype(pingPacket)> icmpPacket(8, 0, pingPacket);
|
||||
Ipv4Packet<decltype(icmpPacket)> ipv4Packet(1, Ipv4Address, ip, icmpPacket);
|
||||
IcmpPacketHeader icmpHeader(8, 0);
|
||||
IcmpEchoHeader pingHeader(0, 0);
|
||||
|
||||
EthernetFrame<decltype(ipv4Packet)> frame(ETHERTYPE_IPV4, ipv4Packet);
|
||||
frame.header.macDestination = mac;
|
||||
frame.header.macSource = GetMacAddress();
|
||||
size_t ipv4TotalSize = IcmpPacketHeader::SerializedLength() +
|
||||
IcmpEchoHeader::SerializedLength() +
|
||||
Ipv4Header::SerializedLength();
|
||||
Ipv4Header ipv4Header(1, Ipv4Address, ip, ipv4TotalSize);
|
||||
|
||||
EthernetFrameHeader ethernetHeader(mac, GetMacAddress(), ETHERTYPE_IPV4);
|
||||
|
||||
uint8_t buffer[USPI_FRAME_BUFFER_SIZE];
|
||||
size_t size = frame.Serialize(buffer);
|
||||
USPiSendFrame(buffer, size);
|
||||
size_t i = 0;
|
||||
|
||||
i += ethernetHeader.Serialize(buffer + i);
|
||||
i += ipv4Header.Serialize(buffer + i);
|
||||
i += pingHeader.Serialize(buffer + i);
|
||||
i += icmpHeader.Serialize(buffer + 1);
|
||||
|
||||
USPiSendFrame(buffer, i);
|
||||
}
|
||||
|
||||
void HandleIcmpFrame(const uint8_t* buffer)
|
||||
{
|
||||
const auto frame = EthernetFrame<Ipv4Packet<IcmpPacketHeader>>::Deserialize(buffer);
|
||||
const auto packetHeader = frame.payload.payload;
|
||||
size_t requestSize = 0;
|
||||
const auto requestEthernetHeader = EthernetFrameHeader::Deserialize(buffer + requestSize);
|
||||
requestSize += requestEthernetHeader.SerializedLength();
|
||||
const auto requestIpv4Header = Ipv4Header::Deserialize(buffer + requestSize);
|
||||
requestSize += requestIpv4Header.SerializedLength();
|
||||
const auto requestIcmpHeader = IcmpPacketHeader::Deserialize(buffer + requestSize);
|
||||
requestSize += requestIcmpHeader.SerializedLength();
|
||||
|
||||
if (packetHeader.type == ICMP_ECHO_REQUEST)
|
||||
if (requestIcmpHeader.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);
|
||||
const auto requestEchoHeader = IcmpEchoHeader::Deserialize(buffer + requestSize);
|
||||
requestSize += requestEchoHeader.SerializedLength();
|
||||
|
||||
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(
|
||||
const IcmpPacketHeader responseIcmpHeader(ICMP_ECHO_REPLY, 0);
|
||||
const Ipv4Header responseIpv4Header(
|
||||
IP_PROTO_ICMP,
|
||||
Ipv4Address,
|
||||
sourceIp,
|
||||
icmpResp
|
||||
requestIpv4Header.sourceIp,
|
||||
requestIpv4Header.totalLength
|
||||
);
|
||||
const EthernetFrameHeader responseEthernetHeader(
|
||||
requestEthernetHeader.macSource, GetMacAddress(), ETHERTYPE_IPV4);
|
||||
|
||||
EthernetFrame<decltype(ipv4Resp)> frameResp(
|
||||
frame.header.macSource,
|
||||
GetMacAddress(),
|
||||
ETHERTYPE_IPV4,
|
||||
ipv4Resp
|
||||
);
|
||||
const auto payloadLength = requestIpv4Header.totalLength -
|
||||
requestIpv4Header.SerializedLength() -
|
||||
requestIcmpHeader.SerializedLength() -
|
||||
requestEchoHeader.SerializedLength();
|
||||
|
||||
uint8_t bufferResp[USPI_FRAME_BUFFER_SIZE];
|
||||
const auto size = frameResp.Serialize(bufferResp);
|
||||
USPiSendFrame(bufferResp, size);
|
||||
std::array<uint8_t, USPI_FRAME_BUFFER_SIZE> bufferResp;
|
||||
size_t respSize = 0;
|
||||
respSize += responseEthernetHeader.Serialize(bufferResp.data() + respSize);
|
||||
respSize += responseIpv4Header.Serialize(bufferResp.data() + respSize);
|
||||
respSize += responseIcmpHeader.Serialize(bufferResp.data() + respSize);
|
||||
memcpy(bufferResp.data() + respSize, buffer + requestSize, payloadLength);
|
||||
respSize += payloadLength;
|
||||
USPiSendFrame(bufferResp.data(), respSize);
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
|||
36
src/net.h
36
src/net.h
|
|
@ -17,38 +17,40 @@ enum ArpOperation {
|
|||
|
||||
typedef std::array<uint8_t, 6> MacAddress;
|
||||
|
||||
struct EthernetFrameHeader;
|
||||
struct UdpDatagramHeader;
|
||||
struct Ipv4Header;
|
||||
|
||||
//
|
||||
// ARP
|
||||
//
|
||||
void HandleArpFrame(uint8_t* buffer);
|
||||
void SendArpPacket(ArpOperation operation,
|
||||
void HandleArpFrame(EthernetFrameHeader header, uint8_t* buffer);
|
||||
void SendArpPacket(
|
||||
ArpOperation operation,
|
||||
MacAddress targetMac,
|
||||
MacAddress senderMac,
|
||||
uint32_t senderIp,
|
||||
uint32_t targetIp);
|
||||
void SendArpRequest(MacAddress targetMac,
|
||||
MacAddress senderMac,
|
||||
uint32_t senderIp,
|
||||
uint32_t targetIp);
|
||||
void SendArpReply(MacAddress targetMac,
|
||||
MacAddress senderMac,
|
||||
uint32_t senderIp,
|
||||
uint32_t targetIp);
|
||||
uint32_t targetIp
|
||||
);
|
||||
void SendArpRequest(
|
||||
MacAddress targetMac, MacAddress senderMac, uint32_t senderIp, uint32_t targetIp);
|
||||
void SendArpReply(
|
||||
MacAddress targetMac, MacAddress senderMac, uint32_t senderIp, uint32_t targetIp);
|
||||
void SendArpAnnouncement(MacAddress mac, uint32_t ip);
|
||||
|
||||
//
|
||||
// IPv4
|
||||
//
|
||||
void HandleIpv4Frame(const uint8_t* buffer);
|
||||
void HandleIpv4Packet(const EthernetFrameHeader ethernetHeader, const uint8_t* buffer);
|
||||
|
||||
//
|
||||
// UDP
|
||||
//
|
||||
struct EthernetFrameHeader;
|
||||
struct UdpDatagramHeader;
|
||||
struct Ipv4Header;
|
||||
|
||||
void HandleUdpFrame(const uint8_t* buffer);
|
||||
void HandleUdpDatagram(
|
||||
const EthernetFrameHeader ethernetHeader,
|
||||
const Ipv4Header ipv4Header,
|
||||
const uint8_t* buffer
|
||||
);
|
||||
|
||||
void HandleTftpDatagram(
|
||||
const EthernetFrameHeader ethernetReqHeader,
|
||||
|
|
|
|||
Loading…
Reference in a new issue