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

Fix network initialization, swapped ARP MACs and ICMP responses

This commit is contained in:
Sijmen 2021-01-06 05:31:48 +01:00
parent 24002b225a
commit f9376089e0
Signed by: vijfhoek
GPG Key ID: DAF7821E067D9C48
8 changed files with 136 additions and 79 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
src/net-arp.cpp
View File

@ -89,7 +89,7 @@ namespace Net::Arp
arpPacket.targetIp = targetIp;
arpPacket.senderIp = senderIp;
Ethernet::Header ethernetHeader(senderMac, targetMac, Ethernet::EtherType::Arp);
Ethernet::Header ethernetHeader(targetMac, senderMac, Ethernet::EtherType::Arp);
uint8_t buffer[USPI_FRAME_BUFFER_SIZE];
size_t size = 0;
@ -136,7 +136,7 @@ namespace Net::Arp
if (arpSize == 0 || arpSize != arpPacket.SerializedLength())
{
DEBUG_LOG(
"Dropped ARP packet (invalid buffer size %lu, expected %lu)\r\n",
"Dropped ARP packet (invalid buffer size %u, expected %u)\r\n",
bufferSize,
arpPacket.SerializedLength());
return;

2
src/net-dhcp.cpp
View File

@ -241,7 +241,7 @@ namespace Net::Dhcp
if (dhcpSize != Header::SerializedLength())
{
DEBUG_LOG(
"Dropped DHCP packet (invalid buffer size %lu, expected %lu)\r\n",
"Dropped DHCP packet (invalid buffer size %u, expected %u)\r\n",
size,
Header::SerializedLength());
return;

80
src/net-icmp.cpp
View File

@ -1,7 +1,9 @@
#include <cassert>
#include <cstdio>
#include <cstring>
#include "net-icmp.h"
#include "net-utils.h"
#include "debug.h"
#include "types.h"
@ -15,7 +17,8 @@ namespace Net::Icmp
Header::Header() : type(static_cast<Type>(0)), code(0), checksum(0) {}
Header::Header(Type type, uint8_t code) : type(type), code(code), checksum(0) {}
size_t Header::Serialize(uint8_t* buffer, const size_t bufferSize) const
size_t Header::Serialize(
uint8_t* buffer, const size_t bufferSize, const uint8_t* data, const size_t dataSize) const
{
if (bufferSize < SerializedLength())
{
@ -25,8 +28,16 @@ namespace Net::Icmp
size_t i = 0;
buffer[i++] = static_cast<uint8_t>(type);
buffer[i++] = code;
buffer[i++] = checksum;
buffer[i++] = checksum >> 8;
buffer[i++] = 0;
buffer[i++] = 0 >> 8;
std::memcpy(buffer + i, data, dataSize);
i += dataSize;
uint16_t checksum = Utils::InternetChecksum(buffer, i);
buffer[2] = checksum;
buffer[3] = checksum >> 8;
return i;
}
@ -93,10 +104,12 @@ namespace Net::Icmp
uint8_t buffer[USPI_FRAME_BUFFER_SIZE];
size_t size = 0;
const uint8_t emptyBuffer[1] = {};
size += ethernetHeader.Serialize(buffer + size, sizeof(buffer) - size);
size += ipv4Header.Serialize(buffer + size, sizeof(buffer) - size);
size += pingHeader.Serialize(buffer + size, sizeof(buffer) - size);
size += icmpHeader.Serialize(buffer + size, sizeof(buffer) - size);
size += icmpHeader.Serialize(buffer + size, sizeof(buffer) - size, emptyBuffer, 0);
const auto expectedSize = ethernetHeader.SerializedLength() +
ipv4Header.SerializedLength() + pingHeader.SerializedLength() +
@ -114,29 +127,19 @@ namespace Net::Icmp
const uint8_t* reqBuffer,
const size_t reqBufferSize)
{
EchoHeader reqEchoHeader;
const auto reqEchoHeaderSize =
Icmp::EchoHeader::Deserialize(reqEchoHeader, reqBuffer, reqBufferSize);
if (reqEchoHeaderSize == 0 || reqBufferSize < reqEchoHeaderSize)
{
DEBUG_LOG(
"Dropped ICMP packet (invalid buffer size %ul, expected at least %ul)\r\n",
reqBufferSize,
EchoHeader::SerializedLength());
return;
}
const Icmp::Header respIcmpHeader(Icmp::Type::EchoReply, 0);
const Ethernet::Header respEthernetHeader(
reqEthernetHeader.macSource, Utils::GetMacAddress(), Ethernet::EtherType::Ipv4);
const Ipv4::Header respIpv4Header(
Ipv4::Protocol::Icmp,
Utils::Ipv4Address,
reqIpv4Header.sourceIp,
reqIpv4Header.totalLength);
const Ethernet::Header respEthernetHeader(
reqEthernetHeader.macSource, Utils::GetMacAddress(), Ethernet::EtherType::Ipv4);
Header respIcmpHeader(Type::EchoReply, 0);
const auto payloadSize = reqIpv4Header.totalLength - reqIpv4Header.SerializedLength() -
reqIcmpHeader.SerializedLength() - reqEchoHeaderSize;
reqIcmpHeader.SerializedLength();
DEBUG_LOG("payloadSize: %u\r\n", payloadSize);
std::array<uint8_t, USPI_FRAME_BUFFER_SIZE> respBuffer;
@ -145,10 +148,8 @@ namespace Net::Icmp
respBuffer.data() + respSize, respBuffer.size() - respSize);
respSize +=
respIpv4Header.Serialize(respBuffer.data() + respSize, respBuffer.size() - respSize);
respSize +=
respIcmpHeader.Serialize(respBuffer.data() + respSize, respBuffer.size() - respSize);
std::memcpy(respBuffer.data() + respSize, reqBuffer + reqEchoHeaderSize, payloadSize);
respSize += payloadSize;
respSize += respIcmpHeader.Serialize(
respBuffer.data() + respSize, respBuffer.size() - respSize, reqBuffer, payloadSize);
const auto expectedRespSize = respEthernetHeader.SerializedLength() +
respIpv4Header.SerializedLength() + respIcmpHeader.SerializedLength() + payloadSize;
@ -158,33 +159,24 @@ namespace Net::Icmp
USPiSendFrame(respBuffer.data(), respSize);
}
void HandlePacket(const uint8_t* buffer, const size_t bufferSize)
void HandlePacket(
Ethernet::Header ethernetHeader,
Ipv4::Header ipv4Header,
const uint8_t* buffer,
const size_t bufferSize)
{
// TODO Don't re-parse the upper layers
size_t headerSize = 0;
Ethernet::Header ethernetHeader;
headerSize += Ethernet::Header::Deserialize(
ethernetHeader, buffer + headerSize, bufferSize - headerSize);
Ipv4::Header ipv4Header;
headerSize +=
Ipv4::Header::Deserialize(ipv4Header, buffer + headerSize, bufferSize - headerSize);
Header icmpHeader;
headerSize +=
Icmp::Header::Deserialize(icmpHeader, buffer + headerSize, bufferSize - headerSize);
const auto expectedHeaderSize = ethernetHeader.SerializedLength() +
ipv4Header.SerializedLength() + icmpHeader.SerializedLength();
if (headerSize != expectedHeaderSize)
size_t headerSize = Icmp::Header::Deserialize(icmpHeader, buffer, bufferSize);
if (headerSize != Icmp::Header::SerializedLength())
{
DEBUG_LOG(
"Dropped ICMP packet (invalid buffer size %ul, expected at least %ul)\r\n",
"Dropped ICMP packet (invalid buffer size %u, expected at least %u)\r\n",
bufferSize,
expectedHeaderSize);
Icmp::Header::SerializedLength());
}
DEBUG_LOG("Got ICMP header type %u\r\n", static_cast<uint8_t>(icmpHeader.type));
if (icmpHeader.type == Type::EchoRequest)
{
handleEchoRequest(

13
src/net-icmp.h
View File

@ -23,7 +23,12 @@ namespace Net::Icmp
return sizeof(type) + sizeof(code) + sizeof(checksum);
}
size_t Serialize(uint8_t* buffer, const size_t bufferSize) const;
void UpdateChecksum(const uint8_t* data, const size_t dataSize);
size_t Serialize(
uint8_t* buffer,
const size_t bufferSize,
const uint8_t* data,
const size_t dataSize) const;
static size_t Deserialize(Header& out, const uint8_t* buffer, const size_t bufferSize);
};
@ -45,5 +50,9 @@ namespace Net::Icmp
};
void SendEchoRequest(const Utils::MacAddress mac, const uint32_t ip);
void HandlePacket(const uint8_t* buffer, const size_t bufferSize);
void HandlePacket(
Ethernet::Header ethernetHeader,
Ipv4::Header ipv4Header,
const uint8_t* buffer,
const size_t bufferSize);
} // namespace Net::Icmp

38
src/net-ipv4.cpp
View File

@ -9,6 +9,10 @@
#include "debug.h"
#include "types.h"
#include <cstring>
#include <uspi.h>
namespace Net::Ipv4
{
Header::Header() {}
@ -102,12 +106,14 @@ namespace Net::Ipv4
void HandlePacket(
const Ethernet::Header& ethernetHeader, const uint8_t* buffer, const size_t bufferSize)
{
char printBuffer[USPI_FRAME_BUFFER_SIZE] = {};
Header header;
const auto headerSize = Header::Deserialize(header, buffer, bufferSize);
if (headerSize != Header::SerializedLength())
{
DEBUG_LOG(
"Dropped IPv4 packet (invalid buffer size %lu, expected at least %lu)\r\n",
"Dropped IPv4 packet (invalid buffer size %u, expected at least %u)\r\n",
bufferSize,
headerSize);
return;
@ -117,20 +123,42 @@ namespace Net::Ipv4
Arp::ArpTable.insert(std::make_pair(header.sourceIp, ethernetHeader.macSource));
if (header.version != 4)
{
DEBUG_LOG(
"Dropped IPv4 packet (invalid header version %u, expected 4)\r\n", header.version);
return;
}
if (header.ihl != 5)
return; // Not supported
if (header.destinationIp != Utils::Ipv4Address)
{
// Not supported
DEBUG_LOG("Dropped IPv4 packet (unsupported IHL %u, expected 5)\r\n", header.ihl);
return;
}
if (header.destinationIp != Utils::Ipv4Address)
{
DEBUG_LOG(
"Dropped IPv4 packet (invalid destination IP address %08lx)\r\n",
header.destinationIp);
return;
}
if (header.fragmentOffset != 0)
return; // TODO Support this
{
// TODO Support this
DEBUG_LOG(
"Dropped IPv4 packet (unexpected fragment offset %u, expected 0)\r\n",
header.fragmentOffset);
return;
}
if (header.protocol == Ipv4::Protocol::Icmp)
{
Icmp::HandlePacket(buffer, bufferSize - headerSize);
DEBUG_LOG("Ethernet -> IPv4 -> ICMP\r\n");
Icmp::HandlePacket(
ethernetHeader, header, buffer + headerSize, bufferSize - headerSize);
}
else if (header.protocol == Ipv4::Protocol::Udp)
{
DEBUG_LOG("Ethernet -> IPv4 -> UDP\r\n");
Udp::HandlePacket(ethernetHeader, header, buffer + headerSize, bufferSize - headerSize);
}
}

10
src/net-tftp.cpp
View File

@ -32,9 +32,9 @@ namespace Net::Tftp
if (size == 0)
{
DEBUG_LOG(
"Dropped TFTP packet (invalid buffer size %lu, expected at least %lu)\r\n",
"Dropped TFTP packet (invalid buffer size %u, expected at least %u)\r\n",
dataSize,
sizeof(WriteReadRequestPacket::opcode) + 2)
sizeof(WriteReadRequestPacket::opcode) + 2);
return nullptr;
}
@ -93,9 +93,9 @@ namespace Net::Tftp
if (tftpSize == 0)
{
DEBUG_LOG(
"Dropped TFTP data packet (invalid buffer size %lu, expected at least %lu)\r\n",
"Dropped TFTP data packet (invalid buffer size %u, expected at least %u)\r\n",
size,
sizeof(packet.opcode) + sizeof(packet.blockNumber))
sizeof(packet.opcode) + sizeof(packet.blockNumber));
return nullptr;
}
@ -140,7 +140,7 @@ namespace Net::Tftp
if (reqBufferSize < payloadSize)
{
DEBUG_LOG(
"Dropped TFTP packet (invalid buffer size %lu, expected at least %lu)\r\n",
"Dropped TFTP packet (invalid buffer size %u, expected at least %u)\r\n",
reqBufferSize,
payloadSize);
}

4
src/net-udp.cpp
View File

@ -57,7 +57,7 @@ namespace Net::Udp
if (headerSize == 0 || headerSize != udpHeader.SerializedLength())
{
DEBUG_LOG(
"Dropped UDP header (invalid buffer size %lu, expected at least %lu)\r\n",
"Dropped UDP header (invalid buffer size %u, expected at least %u)\r\n",
bufferSize,
Header::SerializedLength());
return;
@ -65,7 +65,7 @@ namespace Net::Udp
if (udpHeader.length <= bufferSize)
{
DEBUG_LOG(
"Dropped UDP packet (invalid buffer size %lu, expected at least %lu)\r\n",
"Dropped UDP packet (invalid buffer size %u, expected at least %u)\r\n",
bufferSize,
udpHeader.length);
return;

60
src/net.cpp
View File

@ -6,14 +6,23 @@
#include <uspi.h>
#include <uspios.h>
extern "C"
{
#include "rpiHardware.h"
}
namespace Net
{
static void postInitialize(unsigned int, void* parameter, void*);
static uint32_t postInitializeTime = 0;
static Options* options;
static void postInitialize();
static void ipObtained();
void Initialize(Options& options)
{
// Wait for ethernet to become available.
DEBUG_LOG("Waiting for ethernet\r\n");
while (!USPiEthernetAvailable())
{
MsDelay(500);
@ -21,14 +30,22 @@ namespace Net
// Wait 3 seconds, then run postInitialize
const auto optionsVoid = static_cast<void*>(&options);
StartKernelTimer(3 * HZ, postInitialize, optionsVoid, nullptr);
DEBUG_LOG("Scheduled post-init\r\n");
Net::options = &options;
postInitializeTime = read32(ARM_SYSTIMER_CLO) + 30000;
}
void Update()
{
if (postInitializeTime && read32(ARM_SYSTIMER_CLO) > postInitializeTime)
{
postInitialize();
postInitializeTime = 0;
}
unsigned int bufferSize = 0;
uint8_t buffer[USPI_FRAME_BUFFER_SIZE];
if (!USPiEthernetAvailable() || !USPiReceiveFrame(buffer, &bufferSize))
if (!USPiReceiveFrame(buffer, &bufferSize))
{
return;
}
@ -38,7 +55,7 @@ namespace Net
if (headerSize == 0 || headerSize != Ethernet::Header::SerializedLength())
{
DEBUG_LOG(
"Dropped ethernet packet (invalid buffer size %lu, expected at least %lu)\r\n",
"Dropped ethernet packet (invalid buffer size %u, expected at least %u)\r\n",
headerSize,
Ethernet::Header::SerializedLength());
return;
@ -55,40 +72,51 @@ namespace Net
}
}
static void postInitialize(unsigned int, void* parameter, void*)
static void postInitialize()
{
DEBUG_LOG("Running network post-init\r\n");
const auto options = static_cast<const Options*>(parameter);
if (options->GetDHCPEnable())
{
DEBUG_LOG("DHCP enabled, trying to obtain IP\r\n");
std::function<void()> callback = ipObtained;
Dhcp::ObtainIp(callback);
}
else
{
// Try parsing the IP address in the options.
uint8_t ip[4];
int scanned = sscanf(
options->GetIPAddress(), "%hhu.%hhu.%hhu.%hhu", &ip[3], &ip[2], &ip[1], &ip[0]);
unsigned int ip[4];
char dot;
int scanned =
sscanf(options->GetIPAddress(), "%u.%u.%u.%u", &ip[0], &ip[1], &ip[2], &ip[3]);
if (scanned == 4)
{
DEBUG_LOG("Setting IP address %d.%d.%d.%d\r\n", ip[3], ip[2], ip[1], ip[0]);
Utils::Ipv4Address = *reinterpret_cast<uint32_t*>(ip);
DEBUG_LOG("Setting IP address %u.%u.%u.%u\r\n", ip[0], ip[1], ip[2], ip[3]);
Utils::Ipv4Address = 0;
for (int i = 0; i < 4; i++)
{
Utils::Ipv4Address <<= 8;
Utils::Ipv4Address |= ip[i];
}
ipObtained();
}
else
{
DEBUG_LOG("Invalid IP address '%s'\r\n", options->GetIPAddress());
}
}
}
static void ipObtained()
{
#ifdef DEBUG
uint8_t* ip = reinterpret_cast<uint8_t*>(Utils::Ipv4Address);
DEBUG_LOG("Obtained IP address %d.%d.%d.%d\r\n", ip[3], ip[2], ip[1], ip[0]);
#endif
DEBUG_LOG(
"Obtained IP address %ld.%ld.%ld.%ld\r\n",
Utils::Ipv4Address >> 24,
(Utils::Ipv4Address >> 16) & 0xFF,
(Utils::Ipv4Address >> 8) & 0xFF,
Utils::Ipv4Address & 0xFF);
Arp::SendAnnouncement(Utils::GetMacAddress(), Utils::Ipv4Address);
}
} // namespace Net