Move ARP and TFTP to their own namespaces
This commit is contained in:
parent
fc39c7a99f
commit
9638a0dc3d
GPG Key ID:
DAF7821E067D9C48
|
|
@ -382,14 +382,14 @@ void updateNetwork()
|
|||
static bool announcementSent = false;
|
||||
if (!announcementSent)
|
||||
{
|
||||
SendArpAnnouncement(GetMacAddress(), Ipv4Address);
|
||||
Net::Arp::SendAnnouncement(GetMacAddress(), Ipv4Address);
|
||||
announcementSent = true;
|
||||
}
|
||||
|
||||
switch (ethernetHeader.type)
|
||||
{
|
||||
case ETHERTYPE_ARP:
|
||||
HandleArpFrame(ethernetHeader, ipBuffer + offset);
|
||||
Net::Arp::HandlePacket(ethernetHeader, ipBuffer + offset);
|
||||
break;
|
||||
case ETHERTYPE_IPV4:
|
||||
HandleIpv4Packet(ethernetHeader, ipBuffer + offset, sizeof(ipBuffer) - offset);
|
||||
|
|
|
|||
169
src/net-arp.cpp
169
src/net-arp.cpp
|
|
@ -1,61 +1,134 @@
|
|||
#include "net-arp.h"
|
||||
#include "net-ethernet.h"
|
||||
|
||||
Ipv4ArpPacket::Ipv4ArpPacket()
|
||||
{}
|
||||
#include "types.h"
|
||||
#include <uspi.h>
|
||||
|
||||
Ipv4ArpPacket::Ipv4ArpPacket(std::uint16_t operation) :
|
||||
hardwareType(1), // Ethernet
|
||||
protocolType(ETHERTYPE_IPV4), // IPv4
|
||||
hardwareAddressLength(6),
|
||||
protocolAddressLength(4),
|
||||
operation(operation)
|
||||
{}
|
||||
|
||||
std::size_t Ipv4ArpPacket::Serialize(std::uint8_t* buffer)
|
||||
namespace Net::Arp
|
||||
{
|
||||
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;
|
||||
Ipv4ArpPacket::Ipv4ArpPacket()
|
||||
{}
|
||||
|
||||
memcpy(buffer + 8, senderMac.data(), 6);
|
||||
Ipv4ArpPacket::Ipv4ArpPacket(std::uint16_t operation) :
|
||||
hardwareType(1), // Ethernet
|
||||
protocolType(ETHERTYPE_IPV4), // IPv4
|
||||
hardwareAddressLength(6),
|
||||
protocolAddressLength(4),
|
||||
operation(operation)
|
||||
{}
|
||||
|
||||
buffer[14] = senderIp >> 24;
|
||||
buffer[15] = senderIp >> 16;
|
||||
buffer[16] = senderIp >> 8;
|
||||
buffer[17] = senderIp;
|
||||
std::size_t Ipv4ArpPacket::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 + 18, targetMac.data(), 6);
|
||||
memcpy(buffer + 8, senderMac.data(), 6);
|
||||
|
||||
buffer[24] = targetIp >> 24;
|
||||
buffer[25] = targetIp >> 16;
|
||||
buffer[26] = targetIp >> 8;
|
||||
buffer[27] = targetIp;
|
||||
buffer[14] = senderIp >> 24;
|
||||
buffer[15] = senderIp >> 16;
|
||||
buffer[16] = senderIp >> 8;
|
||||
buffer[17] = senderIp;
|
||||
|
||||
return 28;
|
||||
}
|
||||
memcpy(buffer + 18, targetMac.data(), 6);
|
||||
|
||||
// Static
|
||||
Ipv4ArpPacket Ipv4ArpPacket::Deserialize(const uint8_t* buffer)
|
||||
{
|
||||
Ipv4ArpPacket self;
|
||||
buffer[24] = targetIp >> 24;
|
||||
buffer[25] = targetIp >> 16;
|
||||
buffer[26] = targetIp >> 8;
|
||||
buffer[27] = targetIp;
|
||||
|
||||
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];
|
||||
return 28;
|
||||
}
|
||||
|
||||
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];
|
||||
// Static
|
||||
Ipv4ArpPacket Ipv4ArpPacket::Deserialize(const uint8_t* buffer)
|
||||
{
|
||||
Ipv4ArpPacket self;
|
||||
|
||||
return 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;
|
||||
}
|
||||
|
||||
void SendPacket(
|
||||
ArpOperation operation,
|
||||
MacAddress targetMac,
|
||||
MacAddress senderMac,
|
||||
uint32_t targetIp,
|
||||
uint32_t senderIp)
|
||||
{
|
||||
Ipv4ArpPacket arpPacket(operation);
|
||||
arpPacket.targetMac = targetMac;
|
||||
arpPacket.senderMac = senderMac;
|
||||
arpPacket.targetIp = targetIp;
|
||||
arpPacket.senderIp = senderIp;
|
||||
|
||||
EthernetFrameHeader ethernetHeader(senderMac, targetMac, ETHERTYPE_ARP);
|
||||
|
||||
uint8_t buffer[USPI_FRAME_BUFFER_SIZE];
|
||||
size_t size = 0;
|
||||
size += ethernetHeader.Serialize(buffer + size);
|
||||
size += arpPacket.Serialize(buffer + size);
|
||||
USPiSendFrame(buffer, size);
|
||||
}
|
||||
|
||||
void SendRequest(
|
||||
MacAddress targetMac, MacAddress senderMac, uint32_t targetIp, uint32_t senderIp)
|
||||
{
|
||||
SendPacket(ARP_OPERATION_REQUEST, targetMac, senderMac, targetIp, senderIp);
|
||||
}
|
||||
|
||||
void SendReply(
|
||||
MacAddress targetMac, MacAddress senderMac, uint32_t targetIp, uint32_t senderIp)
|
||||
{
|
||||
SendPacket(ARP_OPERATION_REPLY, targetMac, senderMac, targetIp, senderIp);
|
||||
}
|
||||
|
||||
void SendAnnouncement(MacAddress mac, uint32_t ip)
|
||||
{
|
||||
SendReply(MacBroadcast, mac, ip, ip);
|
||||
}
|
||||
|
||||
void HandlePacket(const EthernetFrameHeader ethernetHeader, uint8_t* buffer)
|
||||
{
|
||||
const auto macAddress = GetMacAddress();
|
||||
const auto arpPacket = Ipv4ArpPacket::Deserialize(buffer);
|
||||
|
||||
if (
|
||||
arpPacket.hardwareType == 1 &&
|
||||
arpPacket.protocolType == ETHERTYPE_IPV4 &&
|
||||
arpPacket.operation == ARP_OPERATION_REQUEST &&
|
||||
arpPacket.targetIp == Ipv4Address)
|
||||
{
|
||||
SendReply(arpPacket.senderMac, macAddress, arpPacket.senderIp, Ipv4Address);
|
||||
}
|
||||
|
||||
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(arpPacket.senderIp, arpPacket.senderMac));
|
||||
}
|
||||
}
|
||||
|
||||
std::unordered_map<std::uint32_t, MacAddress> ArpTable;
|
||||
}; // namespace Net::Arp
|
||||
|
|
|
|||
|
|
@ -1,37 +1,68 @@
|
|||
#pragma once
|
||||
#include "net.h"
|
||||
|
||||
struct Ipv4ArpPacket
|
||||
namespace Net::Arp
|
||||
{
|
||||
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);
|
||||
|
||||
constexpr std::size_t SerializedLength() const
|
||||
struct Ipv4ArpPacket
|
||||
{
|
||||
return
|
||||
sizeof(hardwareType) +
|
||||
sizeof(protocolType) +
|
||||
sizeof(hardwareAddressLength) +
|
||||
sizeof(protocolAddressLength) +
|
||||
sizeof(operation) +
|
||||
senderMac.size() +
|
||||
sizeof(senderIp) +
|
||||
targetMac.size() +
|
||||
sizeof(targetIp);
|
||||
}
|
||||
std::uint16_t hardwareType;
|
||||
std::uint16_t protocolType;
|
||||
std::uint8_t hardwareAddressLength;
|
||||
std::uint8_t protocolAddressLength;
|
||||
std::uint16_t operation;
|
||||
|
||||
std::size_t Serialize(std::uint8_t* buffer);
|
||||
MacAddress senderMac;
|
||||
std::uint32_t senderIp;
|
||||
MacAddress targetMac;
|
||||
std::uint32_t targetIp;
|
||||
|
||||
static Ipv4ArpPacket Deserialize(const uint8_t* buffer);
|
||||
};
|
||||
Ipv4ArpPacket();
|
||||
Ipv4ArpPacket(std::uint16_t 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);
|
||||
|
||||
static Ipv4ArpPacket Deserialize(const uint8_t* buffer);
|
||||
};
|
||||
|
||||
void HandlePacket(EthernetFrameHeader header, uint8_t* buffer);
|
||||
|
||||
void SendPacket(
|
||||
ArpOperation operation,
|
||||
MacAddress targetMac,
|
||||
MacAddress senderMac,
|
||||
uint32_t senderIp,
|
||||
uint32_t targetIp
|
||||
);
|
||||
|
||||
void SendRequest(
|
||||
MacAddress targetMac,
|
||||
MacAddress senderMac,
|
||||
uint32_t senderIp,
|
||||
uint32_t targetIp
|
||||
);
|
||||
|
||||
void SendReply(
|
||||
MacAddress targetMac,
|
||||
MacAddress senderMac,
|
||||
uint32_t senderIp,
|
||||
uint32_t targetIp
|
||||
);
|
||||
|
||||
void SendAnnouncement(MacAddress mac, uint32_t ip);
|
||||
|
||||
extern std::unordered_map<std::uint32_t, MacAddress> ArpTable;
|
||||
}; // namespace Net::Arp
|
||||
|
|
|
|||
512
src/net-tftp.cpp
512
src/net-tftp.cpp
|
|
@ -1,288 +1,308 @@
|
|||
#include <memory>
|
||||
|
||||
#include "ff.h"
|
||||
#include "net-arp.h"
|
||||
#include "net-ethernet.h"
|
||||
#include "net-ipv4.h"
|
||||
#include "net-tftp.h"
|
||||
#include "net-udp.h"
|
||||
#include "net.h"
|
||||
#include "types.h"
|
||||
|
||||
#include "types.h"
|
||||
#include <uspi.h>
|
||||
|
||||
// TODO Allow multiple files open
|
||||
static FIL outFile;
|
||||
static bool shouldReboot = false;
|
||||
static uint32_t currentBlockNumber = -1;
|
||||
|
||||
static std::unique_ptr<TftpPacket> handleTftpWriteRequest(const uint8_t* data)
|
||||
namespace Net::Tftp
|
||||
{
|
||||
auto packet = TftpWriteReadRequestPacket::Deserialize(data);
|
||||
// TODO Allow multiple files open
|
||||
static FIL outFile;
|
||||
static bool shouldReboot = false;
|
||||
static uint32_t currentBlockNumber = -1;
|
||||
|
||||
// TODO Implement netscii, maybe
|
||||
if (packet.mode != "octet")
|
||||
static std::unique_ptr<TftpPacket> handleTftpWriteRequest(const uint8_t* data)
|
||||
{
|
||||
return std::unique_ptr<TftpErrorPacket>(
|
||||
new TftpErrorPacket(0, "please use mode octet")
|
||||
);
|
||||
auto packet = TftpWriteReadRequestPacket::Deserialize(data);
|
||||
|
||||
// TODO Implement netscii, maybe
|
||||
if (packet.mode != "octet")
|
||||
{
|
||||
return std::unique_ptr<TftpErrorPacket>(
|
||||
new TftpErrorPacket(0, "please use mode octet")
|
||||
);
|
||||
}
|
||||
|
||||
currentBlockNumber = 0;
|
||||
|
||||
// TODO Return to the original working directory.
|
||||
char workingDirectory[256];
|
||||
f_getcwd(workingDirectory, sizeof(workingDirectory));
|
||||
|
||||
// Try opening the file
|
||||
auto separator = packet.filename.rfind('/', packet.filename.size());
|
||||
if (separator != std::string::npos)
|
||||
{
|
||||
auto path = "/" + packet.filename.substr(0, separator);
|
||||
f_chdir(path.c_str());
|
||||
}
|
||||
else
|
||||
{
|
||||
f_chdir("/");
|
||||
separator = 0;
|
||||
}
|
||||
|
||||
// Open the output file.
|
||||
auto filename = packet.filename.substr(separator + 1);
|
||||
const auto result = f_open(&outFile, filename.c_str(), FA_CREATE_ALWAYS | FA_WRITE);
|
||||
|
||||
std::unique_ptr<TftpPacket> response;
|
||||
if (result != FR_OK)
|
||||
{
|
||||
response = std::unique_ptr<TftpErrorPacket>(
|
||||
new TftpErrorPacket(0, "error opening target file")
|
||||
);
|
||||
}
|
||||
else
|
||||
{
|
||||
shouldReboot =
|
||||
packet.filename == "kernel.img" || packet.filename == "options.txt";
|
||||
response = std::unique_ptr<TftpAcknowledgementPacket>(
|
||||
new TftpAcknowledgementPacket(currentBlockNumber)
|
||||
);
|
||||
}
|
||||
|
||||
// TODO Return to the original working directory here
|
||||
|
||||
return response;
|
||||
}
|
||||
|
||||
currentBlockNumber = 0;
|
||||
|
||||
// TODO Return to the original working directory.
|
||||
char workingDirectory[256];
|
||||
f_getcwd(workingDirectory, sizeof(workingDirectory));
|
||||
|
||||
// Try opening the file
|
||||
auto separator = packet.filename.rfind('/', packet.filename.size());
|
||||
if (separator != std::string::npos)
|
||||
static std::unique_ptr<TftpPacket> handleTftpData(const uint8_t* buffer, size_t size)
|
||||
{
|
||||
auto path = "/" + packet.filename.substr(0, separator);
|
||||
f_chdir(path.c_str());
|
||||
}
|
||||
else
|
||||
{
|
||||
f_chdir("/");
|
||||
separator = 0;
|
||||
}
|
||||
TftpDataPacket packet;
|
||||
const auto tftpSize = TftpDataPacket::Deserialize(packet, buffer, size);
|
||||
if (size == 0)
|
||||
{
|
||||
// TODO log
|
||||
return nullptr;
|
||||
}
|
||||
|
||||
// Open the output file.
|
||||
auto filename = packet.filename.substr(separator + 1);
|
||||
const auto result = f_open(&outFile, filename.c_str(), FA_CREATE_ALWAYS | FA_WRITE);
|
||||
if (packet.blockNumber != currentBlockNumber + 1)
|
||||
{
|
||||
f_close(&outFile);
|
||||
return std::unique_ptr<TftpErrorPacket>(
|
||||
new TftpErrorPacket(0, "invalid block number")
|
||||
);
|
||||
}
|
||||
currentBlockNumber = packet.blockNumber;
|
||||
|
||||
std::unique_ptr<TftpPacket> response;
|
||||
if (result != FR_OK)
|
||||
{
|
||||
response = std::unique_ptr<TftpErrorPacket>(
|
||||
new TftpErrorPacket(0, "error opening target file")
|
||||
);
|
||||
}
|
||||
else
|
||||
{
|
||||
shouldReboot =
|
||||
packet.filename == "kernel.img" || packet.filename == "options.txt";
|
||||
response = std::unique_ptr<TftpAcknowledgementPacket>(
|
||||
unsigned int bytesWritten;
|
||||
const auto result =
|
||||
f_write(&outFile, packet.data.data(), packet.data.size(), &bytesWritten);
|
||||
|
||||
if (result != FR_OK || bytesWritten != packet.data.size())
|
||||
{
|
||||
f_close(&outFile);
|
||||
return std::unique_ptr<TftpErrorPacket>(new TftpErrorPacket(0, "io error"));
|
||||
}
|
||||
|
||||
if (packet.data.size() < TFTP_BLOCK_SIZE)
|
||||
{
|
||||
// Close the file for the last packet.
|
||||
f_close(&outFile);
|
||||
}
|
||||
|
||||
return std::unique_ptr<TftpAcknowledgementPacket>(
|
||||
new TftpAcknowledgementPacket(currentBlockNumber)
|
||||
);
|
||||
}
|
||||
|
||||
// TODO Return to the original working directory here
|
||||
void HandleTftpDatagram(
|
||||
const EthernetFrameHeader ethernetReqHeader,
|
||||
const Ipv4Header ipv4ReqHeader,
|
||||
const UdpDatagramHeader udpReqHeader,
|
||||
const uint8_t* data
|
||||
) {
|
||||
const auto opcode = static_cast<Opcode>(data[0] << 8 | data[1]);
|
||||
std::unique_ptr<TftpPacket> response;
|
||||
bool last = false;
|
||||
|
||||
return response;
|
||||
}
|
||||
if (opcode == Opcode::WriteRequest)
|
||||
{
|
||||
response = handleTftpWriteRequest(data);
|
||||
}
|
||||
else if (opcode == Opcode::Data)
|
||||
{
|
||||
const auto length = udpReqHeader.length - UdpDatagramHeader::SerializedLength();
|
||||
response = handleTftpData(data, length);
|
||||
}
|
||||
else
|
||||
{
|
||||
response = std::unique_ptr<TftpErrorPacket>(
|
||||
new TftpErrorPacket(4, "not implemented yet")
|
||||
);
|
||||
}
|
||||
|
||||
static std::unique_ptr<TftpPacket> handleTftpData(const uint8_t* buffer, size_t size)
|
||||
{
|
||||
TftpDataPacket packet;
|
||||
const auto tftpSize = TftpDataPacket::Deserialize(packet, buffer, size);
|
||||
if (size == 0)
|
||||
{
|
||||
// TODO log
|
||||
return nullptr;
|
||||
if (response != nullptr)
|
||||
{
|
||||
UdpDatagramHeader udpRespHeader(
|
||||
udpReqHeader.destinationPort,
|
||||
udpReqHeader.sourcePort,
|
||||
response->SerializedLength() + UdpDatagramHeader::SerializedLength()
|
||||
);
|
||||
Ipv4Header ipv4RespHeader(
|
||||
IP_PROTO_UDP,
|
||||
Ipv4Address,
|
||||
ipv4ReqHeader.sourceIp,
|
||||
udpRespHeader.length + Ipv4Header::SerializedLength()
|
||||
);
|
||||
EthernetFrameHeader ethernetRespHeader(
|
||||
Net::Arp::ArpTable[ipv4RespHeader.destinationIp],
|
||||
GetMacAddress(),
|
||||
ETHERTYPE_IPV4
|
||||
);
|
||||
|
||||
size_t i = 0;
|
||||
uint8_t buffer[USPI_FRAME_BUFFER_SIZE];
|
||||
i += ethernetRespHeader.Serialize(buffer + i);
|
||||
i += ipv4RespHeader.Serialize(buffer + i);
|
||||
i += udpRespHeader.Serialize(buffer + i);
|
||||
i += response->Serialize(buffer + i);
|
||||
USPiSendFrame(buffer, i);
|
||||
}
|
||||
|
||||
if (last && shouldReboot)
|
||||
{
|
||||
// TODO eww
|
||||
extern void Reboot_Pi();
|
||||
Reboot_Pi();
|
||||
}
|
||||
}
|
||||
|
||||
if (packet.blockNumber != currentBlockNumber + 1)
|
||||
{
|
||||
f_close(&outFile);
|
||||
return std::unique_ptr<TftpErrorPacket>(
|
||||
new TftpErrorPacket(0, "invalid block number")
|
||||
);
|
||||
}
|
||||
currentBlockNumber = packet.blockNumber;
|
||||
//
|
||||
// TftpWriteReadRequestPacket
|
||||
//
|
||||
TftpWriteReadRequestPacket::TftpWriteReadRequestPacket(const Opcode opcode) :
|
||||
TftpPacket(opcode)
|
||||
{}
|
||||
|
||||
unsigned int bytesWritten;
|
||||
const auto result =
|
||||
f_write(&outFile, packet.data.data(), packet.data.size(), &bytesWritten);
|
||||
|
||||
if (result != FR_OK || bytesWritten != packet.data.size())
|
||||
size_t TftpWriteReadRequestPacket::SerializedLength() const
|
||||
{
|
||||
f_close(&outFile);
|
||||
return std::unique_ptr<TftpErrorPacket>(new TftpErrorPacket(0, "io error"));
|
||||
return TftpPacket::SerializedLength() + filename.size() + 1 + mode.size() + 1;
|
||||
}
|
||||
|
||||
if (packet.data.size() < TFTP_BLOCK_SIZE)
|
||||
size_t TftpWriteReadRequestPacket::Serialize(uint8_t* buffer) const
|
||||
{
|
||||
// Close the file for the last packet.
|
||||
f_close(&outFile);
|
||||
}
|
||||
|
||||
return std::unique_ptr<TftpAcknowledgementPacket>(
|
||||
new TftpAcknowledgementPacket(currentBlockNumber)
|
||||
);
|
||||
}
|
||||
|
||||
void HandleTftpDatagram(
|
||||
const EthernetFrameHeader ethernetReqHeader,
|
||||
const Ipv4Header ipv4ReqHeader,
|
||||
const UdpDatagramHeader udpReqHeader,
|
||||
const uint8_t* data
|
||||
) {
|
||||
const auto opcode = data[0] << 8 | data[1];
|
||||
std::unique_ptr<TftpPacket> response;
|
||||
bool last = false;
|
||||
|
||||
if (opcode == TFTP_OP_WRITE_REQUEST)
|
||||
{
|
||||
response = handleTftpWriteRequest(data);
|
||||
}
|
||||
else if (opcode == TFTP_OP_DATA)
|
||||
{
|
||||
const auto length = udpReqHeader.length - UdpDatagramHeader::SerializedLength();
|
||||
response = handleTftpData(data, length);
|
||||
}
|
||||
else
|
||||
{
|
||||
response = std::unique_ptr<TftpErrorPacket>(
|
||||
new TftpErrorPacket(4, "not implemented yet")
|
||||
);
|
||||
}
|
||||
|
||||
if (response != nullptr)
|
||||
{
|
||||
UdpDatagramHeader udpRespHeader(
|
||||
udpReqHeader.destinationPort,
|
||||
udpReqHeader.sourcePort,
|
||||
response->SerializedLength() + UdpDatagramHeader::SerializedLength()
|
||||
);
|
||||
Ipv4Header ipv4RespHeader(
|
||||
IP_PROTO_UDP,
|
||||
Ipv4Address,
|
||||
ipv4ReqHeader.sourceIp,
|
||||
udpRespHeader.length + Ipv4Header::SerializedLength()
|
||||
);
|
||||
EthernetFrameHeader ethernetRespHeader(
|
||||
ArpTable[ipv4RespHeader.destinationIp],
|
||||
GetMacAddress(),
|
||||
ETHERTYPE_IPV4
|
||||
);
|
||||
|
||||
size_t i = 0;
|
||||
uint8_t buffer[USPI_FRAME_BUFFER_SIZE];
|
||||
i += ethernetRespHeader.Serialize(buffer + i);
|
||||
i += ipv4RespHeader.Serialize(buffer + i);
|
||||
i += udpRespHeader.Serialize(buffer + i);
|
||||
i += response->Serialize(buffer + i);
|
||||
USPiSendFrame(buffer, i);
|
||||
buffer[i++] = static_cast<uint16_t>(opcode) >> 8;
|
||||
buffer[i++] = static_cast<uint16_t>(opcode);
|
||||
|
||||
i += filename.copy(reinterpret_cast<char*>(buffer + i), filename.size());
|
||||
buffer[i++] = 0;
|
||||
|
||||
i += mode.copy(reinterpret_cast<char*>(buffer + i), mode.size());
|
||||
buffer[i++] = 0;
|
||||
|
||||
return i;
|
||||
}
|
||||
|
||||
if (last && shouldReboot)
|
||||
TftpWriteReadRequestPacket TftpWriteReadRequestPacket::Deserialize(const uint8_t* buffer)
|
||||
{
|
||||
// TODO eww
|
||||
extern void Reboot_Pi();
|
||||
Reboot_Pi();
|
||||
}
|
||||
}
|
||||
size_t i = 0;
|
||||
|
||||
//
|
||||
// TftpWriteReadRequestPacket
|
||||
//
|
||||
TftpWriteReadRequestPacket::TftpWriteReadRequestPacket(uint16_t opcode) :
|
||||
TftpPacket(opcode)
|
||||
{}
|
||||
const auto opcode = static_cast<Opcode>(buffer[i] << 8 | buffer[i + 1]);
|
||||
TftpWriteReadRequestPacket self(opcode);
|
||||
i += 2;
|
||||
|
||||
size_t TftpWriteReadRequestPacket::SerializedLength() const
|
||||
{
|
||||
return TftpPacket::SerializedLength() + filename.size() + 1 + mode.size() + 1;
|
||||
}
|
||||
self.filename = reinterpret_cast<const char*>(buffer + i);
|
||||
i += self.filename.size() + 1;
|
||||
|
||||
size_t TftpWriteReadRequestPacket::Serialize(uint8_t* buffer) const
|
||||
{
|
||||
size_t i = 0;
|
||||
buffer[i++] = opcode >> 8;
|
||||
buffer[i++] = opcode;
|
||||
self.mode = reinterpret_cast<const char*>(buffer + i);
|
||||
i += self.mode.size() + 1;
|
||||
|
||||
i += filename.copy(reinterpret_cast<char*>(buffer + i), filename.size());
|
||||
buffer[i++] = 0;
|
||||
|
||||
i += mode.copy(reinterpret_cast<char*>(buffer + i), mode.size());
|
||||
buffer[i++] = 0;
|
||||
|
||||
return i;
|
||||
}
|
||||
|
||||
TftpWriteReadRequestPacket TftpWriteReadRequestPacket::Deserialize(const uint8_t* buffer)
|
||||
{
|
||||
size_t i = 0;
|
||||
|
||||
TftpWriteReadRequestPacket self(buffer[i] << 8 | buffer[i + 1]);
|
||||
i += 2;
|
||||
|
||||
self.filename = reinterpret_cast<const char*>(buffer + i);
|
||||
i += self.filename.size() + 1;
|
||||
|
||||
self.mode = reinterpret_cast<const char*>(buffer + i);
|
||||
i += self.mode.size() + 1;
|
||||
|
||||
return self;
|
||||
}
|
||||
|
||||
//
|
||||
// TftpErrorPacket
|
||||
//
|
||||
TftpErrorPacket::TftpErrorPacket() : TftpPacket(TFTP_OP_ERROR) {}
|
||||
TftpErrorPacket::TftpErrorPacket(uint16_t errorCode, std::string message) :
|
||||
TftpPacket(TFTP_OP_ERROR), errorCode(errorCode), message(message)
|
||||
{}
|
||||
|
||||
size_t TftpErrorPacket::SerializedLength() const
|
||||
{
|
||||
return TftpPacket::SerializedLength() + sizeof(errorCode) + message.size() + 1;
|
||||
}
|
||||
|
||||
size_t TftpErrorPacket::Serialize(uint8_t* buffer) const
|
||||
{
|
||||
size_t i = 0;
|
||||
buffer[i++] = opcode >> 8;
|
||||
buffer[i++] = opcode;
|
||||
buffer[i++] = errorCode >> 8;
|
||||
buffer[i++] = errorCode;
|
||||
|
||||
i += message.copy(reinterpret_cast<char*>(buffer + i), message.size());
|
||||
buffer[i++] = 0;
|
||||
|
||||
return i;
|
||||
}
|
||||
|
||||
//
|
||||
// TftpAcknowledgementPacket
|
||||
//
|
||||
TftpAcknowledgementPacket::TftpAcknowledgementPacket() :
|
||||
TftpPacket(TFTP_OP_ACKNOWLEDGEMENT)
|
||||
{}
|
||||
|
||||
TftpAcknowledgementPacket::TftpAcknowledgementPacket(uint16_t blockNumber) :
|
||||
TftpPacket(TFTP_OP_ACKNOWLEDGEMENT), blockNumber(blockNumber)
|
||||
{}
|
||||
|
||||
size_t TftpAcknowledgementPacket::SerializedLength() const
|
||||
{
|
||||
return TftpPacket::SerializedLength() + sizeof(blockNumber);
|
||||
}
|
||||
|
||||
size_t TftpAcknowledgementPacket::Serialize(uint8_t* buffer) const
|
||||
{
|
||||
size_t i = 0;
|
||||
buffer[i++] = opcode >> 8;
|
||||
buffer[i++] = opcode;
|
||||
buffer[i++] = blockNumber >> 8;
|
||||
buffer[i++] = blockNumber;
|
||||
return i;
|
||||
}
|
||||
|
||||
//
|
||||
// TftpDataPacket
|
||||
//
|
||||
TftpDataPacket::TftpDataPacket() : opcode(TFTP_OP_DATA) {}
|
||||
|
||||
size_t TftpDataPacket::Deserialize(
|
||||
TftpDataPacket& out, const uint8_t* buffer, size_t size
|
||||
) {
|
||||
if (size < sizeof(opcode) + sizeof(blockNumber)) {
|
||||
return 0;
|
||||
return self;
|
||||
}
|
||||
|
||||
out.opcode = buffer[0] << 8 | buffer[1];
|
||||
out.blockNumber = buffer[2] << 8 | buffer[3];
|
||||
out.data = std::vector<uint8_t>(buffer + 4, buffer + size);
|
||||
return size;
|
||||
}
|
||||
//
|
||||
// TftpErrorPacket
|
||||
//
|
||||
TftpErrorPacket::TftpErrorPacket() : TftpPacket(Opcode::Error) {}
|
||||
TftpErrorPacket::TftpErrorPacket(uint16_t errorCode, std::string message) :
|
||||
TftpPacket(Opcode::Error), errorCode(errorCode), message(message)
|
||||
{}
|
||||
|
||||
size_t TftpErrorPacket::SerializedLength() const
|
||||
{
|
||||
return TftpPacket::SerializedLength() + sizeof(errorCode) + message.size() + 1;
|
||||
}
|
||||
|
||||
size_t TftpErrorPacket::Serialize(uint8_t* buffer) const
|
||||
{
|
||||
size_t i = 0;
|
||||
buffer[i++] = static_cast<uint16_t>(opcode) >> 8;
|
||||
buffer[i++] = static_cast<uint16_t>(opcode);
|
||||
buffer[i++] = errorCode >> 8;
|
||||
buffer[i++] = errorCode;
|
||||
|
||||
i += message.copy(reinterpret_cast<char*>(buffer + i), message.size());
|
||||
buffer[i++] = 0;
|
||||
|
||||
return i;
|
||||
}
|
||||
|
||||
//
|
||||
// TftpAcknowledgementPacket
|
||||
//
|
||||
TftpAcknowledgementPacket::TftpAcknowledgementPacket() :
|
||||
TftpPacket(Opcode::Acknowledgement)
|
||||
{}
|
||||
|
||||
TftpAcknowledgementPacket::TftpAcknowledgementPacket(uint16_t blockNumber) :
|
||||
TftpPacket(Opcode::Acknowledgement), blockNumber(blockNumber)
|
||||
{}
|
||||
|
||||
size_t TftpAcknowledgementPacket::SerializedLength() const
|
||||
{
|
||||
return TftpPacket::SerializedLength() + sizeof(blockNumber);
|
||||
}
|
||||
|
||||
size_t TftpAcknowledgementPacket::Serialize(uint8_t* buffer) const
|
||||
{
|
||||
size_t i = 0;
|
||||
buffer[i++] = static_cast<uint16_t>(opcode) >> 8;
|
||||
buffer[i++] = static_cast<uint16_t>(opcode);
|
||||
buffer[i++] = blockNumber >> 8;
|
||||
buffer[i++] = blockNumber;
|
||||
return i;
|
||||
}
|
||||
|
||||
//
|
||||
// TftpDataPacket
|
||||
//
|
||||
TftpDataPacket::TftpDataPacket() : TftpPacket(Opcode::Data), blockNumber(0)
|
||||
{}
|
||||
|
||||
size_t TftpDataPacket::Serialize(uint8_t* buffer) const
|
||||
{
|
||||
size_t i = 0;
|
||||
buffer[i++] = static_cast<uint16_t>(opcode) >> 8;
|
||||
buffer[i++] = static_cast<uint16_t>(opcode);
|
||||
buffer[i++] = blockNumber >> 8;
|
||||
buffer[i++] = blockNumber;
|
||||
|
||||
std::memcpy(buffer + i, data.data(), data.size());
|
||||
i += data.size();
|
||||
|
||||
return i;
|
||||
}
|
||||
|
||||
size_t TftpDataPacket::Deserialize(
|
||||
TftpDataPacket& out, const uint8_t* buffer, size_t size
|
||||
) {
|
||||
if (size < sizeof(opcode) + sizeof(blockNumber)) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
out.opcode = static_cast<Opcode>(buffer[0] << 8 | buffer[1]);
|
||||
out.blockNumber = buffer[2] << 8 | buffer[3];
|
||||
out.data = std::vector<uint8_t>(buffer + 4, buffer + size);
|
||||
return size;
|
||||
}
|
||||
}; // namespace Net::Tftp
|
||||
|
|
|
|||
115
src/net-tftp.h
115
src/net-tftp.h
|
|
@ -1,69 +1,78 @@
|
|||
#pragma once
|
||||
#include <vector>
|
||||
|
||||
const size_t TFTP_BLOCK_SIZE = 512;
|
||||
namespace Net::Tftp {
|
||||
const size_t TFTP_BLOCK_SIZE = 512;
|
||||
|
||||
enum TftpOperation
|
||||
{
|
||||
TFTP_OP_READ_REQUEST = 1,
|
||||
TFTP_OP_WRITE_REQUEST = 2,
|
||||
TFTP_OP_DATA = 3,
|
||||
TFTP_OP_ACKNOWLEDGEMENT = 4,
|
||||
TFTP_OP_ERROR = 5,
|
||||
};
|
||||
enum class Opcode : uint16_t
|
||||
{
|
||||
ReadRequest = 1,
|
||||
WriteRequest = 2,
|
||||
Data = 3,
|
||||
Acknowledgement = 4,
|
||||
Error = 5,
|
||||
};
|
||||
|
||||
struct TftpPacket
|
||||
{
|
||||
uint16_t opcode;
|
||||
struct TftpPacket
|
||||
{
|
||||
Opcode opcode;
|
||||
|
||||
TftpPacket(uint16_t opcode) : opcode(opcode) {}
|
||||
TftpPacket(Opcode opcode) : opcode(opcode) {}
|
||||
|
||||
virtual size_t SerializedLength() const {
|
||||
return sizeof(opcode);
|
||||
}
|
||||
virtual size_t SerializedLength() const {
|
||||
return sizeof(opcode);
|
||||
}
|
||||
|
||||
virtual size_t Serialize(uint8_t* buffer) const = 0;
|
||||
};
|
||||
virtual size_t Serialize(uint8_t* buffer) const = 0;
|
||||
};
|
||||
|
||||
struct TftpWriteReadRequestPacket : public TftpPacket
|
||||
{
|
||||
std::string filename;
|
||||
std::string mode;
|
||||
struct TftpWriteReadRequestPacket : public TftpPacket
|
||||
{
|
||||
std::string filename;
|
||||
std::string mode;
|
||||
|
||||
TftpWriteReadRequestPacket(uint16_t opcode);
|
||||
size_t SerializedLength() const override;
|
||||
size_t Serialize(uint8_t* buffer) const override;
|
||||
static TftpWriteReadRequestPacket Deserialize(const uint8_t* buffer);
|
||||
};
|
||||
TftpWriteReadRequestPacket(const Opcode opcode);
|
||||
size_t SerializedLength() const override;
|
||||
size_t Serialize(uint8_t* buffer) const override;
|
||||
static TftpWriteReadRequestPacket Deserialize(const uint8_t* buffer);
|
||||
};
|
||||
|
||||
struct TftpErrorPacket : public TftpPacket
|
||||
{
|
||||
uint16_t errorCode;
|
||||
std::string message;
|
||||
struct TftpErrorPacket : public TftpPacket
|
||||
{
|
||||
uint16_t errorCode;
|
||||
std::string message;
|
||||
|
||||
TftpErrorPacket();
|
||||
TftpErrorPacket(uint16_t errorCode, std::string message);
|
||||
size_t SerializedLength() const override;
|
||||
size_t Serialize(uint8_t* buffer) const override;
|
||||
};
|
||||
TftpErrorPacket();
|
||||
TftpErrorPacket(uint16_t errorCode, std::string message);
|
||||
size_t SerializedLength() const override;
|
||||
size_t Serialize(uint8_t* buffer) const override;
|
||||
};
|
||||
|
||||
struct TftpAcknowledgementPacket : public TftpPacket
|
||||
{
|
||||
uint16_t blockNumber;
|
||||
struct TftpAcknowledgementPacket : public TftpPacket
|
||||
{
|
||||
uint16_t blockNumber;
|
||||
|
||||
TftpAcknowledgementPacket();
|
||||
TftpAcknowledgementPacket(uint16_t blockNumber);
|
||||
size_t SerializedLength() const override;
|
||||
size_t Serialize(uint8_t* buffer) const override;
|
||||
};
|
||||
TftpAcknowledgementPacket();
|
||||
TftpAcknowledgementPacket(uint16_t blockNumber);
|
||||
size_t SerializedLength() const override;
|
||||
size_t Serialize(uint8_t* buffer) const override;
|
||||
};
|
||||
|
||||
struct TftpDataPacket
|
||||
{
|
||||
uint16_t opcode;
|
||||
uint16_t blockNumber;
|
||||
std::vector<uint8_t> data;
|
||||
struct TftpDataPacket : public TftpPacket
|
||||
{
|
||||
uint16_t blockNumber;
|
||||
std::vector<uint8_t> data;
|
||||
|
||||
TftpDataPacket();
|
||||
static size_t Deserialize(
|
||||
TftpDataPacket& out, const uint8_t* buffer, size_t length);
|
||||
};
|
||||
TftpDataPacket();
|
||||
size_t Serialize(uint8_t* buffer) const override;
|
||||
static size_t Deserialize(
|
||||
TftpDataPacket& out, const uint8_t* buffer, size_t length);
|
||||
};
|
||||
|
||||
void HandleTftpDatagram(
|
||||
const EthernetFrameHeader ethernetReqHeader,
|
||||
const Ipv4Header ipv4ReqHeader,
|
||||
const UdpDatagramHeader udpReqHeader,
|
||||
const uint8_t* buffer
|
||||
);
|
||||
}; // namespace Net::Tftp
|
||||
|
|
|
|||
74
src/net.cpp
74
src/net.cpp
|
|
@ -7,79 +7,13 @@
|
|||
#include "net-ipv4.h"
|
||||
#include "net-udp.h"
|
||||
#include "net-dhcp.h"
|
||||
#include "net-tftp.h"
|
||||
#include "net.h"
|
||||
#include "types.h"
|
||||
|
||||
#include <uspi.h>
|
||||
#include <uspios.h>
|
||||
|
||||
//
|
||||
// ARP
|
||||
//
|
||||
void SendArpPacket(
|
||||
ArpOperation operation,
|
||||
MacAddress targetMac,
|
||||
MacAddress senderMac,
|
||||
uint32_t targetIp,
|
||||
uint32_t senderIp)
|
||||
{
|
||||
Ipv4ArpPacket arpPacket(operation);
|
||||
arpPacket.targetMac = targetMac;
|
||||
arpPacket.senderMac = senderMac;
|
||||
arpPacket.targetIp = targetIp;
|
||||
arpPacket.senderIp = senderIp;
|
||||
|
||||
EthernetFrameHeader ethernetHeader(senderMac, targetMac, ETHERTYPE_ARP);
|
||||
|
||||
uint8_t buffer[USPI_FRAME_BUFFER_SIZE];
|
||||
size_t size = 0;
|
||||
size += ethernetHeader.Serialize(buffer + size);
|
||||
size += arpPacket.Serialize(buffer + size);
|
||||
USPiSendFrame(buffer, size);
|
||||
}
|
||||
|
||||
void SendArpRequest(
|
||||
MacAddress targetMac, MacAddress senderMac, uint32_t targetIp, uint32_t senderIp)
|
||||
{
|
||||
SendArpPacket(ARP_OPERATION_REQUEST, targetMac, senderMac, targetIp, senderIp);
|
||||
}
|
||||
|
||||
void SendArpReply(
|
||||
MacAddress targetMac, MacAddress senderMac, uint32_t targetIp, uint32_t senderIp)
|
||||
{
|
||||
SendArpPacket(ARP_OPERATION_REPLY, targetMac, senderMac, targetIp, senderIp);
|
||||
}
|
||||
|
||||
void SendArpAnnouncement(MacAddress mac, uint32_t ip)
|
||||
{
|
||||
SendArpReply(MacBroadcast, mac, ip, ip);
|
||||
}
|
||||
|
||||
void HandleArpFrame(const EthernetFrameHeader ethernetHeader, uint8_t* buffer)
|
||||
{
|
||||
const auto macAddress = GetMacAddress();
|
||||
const auto arpPacket = Ipv4ArpPacket::Deserialize(buffer);
|
||||
|
||||
if (
|
||||
arpPacket.hardwareType == 1 &&
|
||||
arpPacket.protocolType == ETHERTYPE_IPV4 &&
|
||||
arpPacket.operation == ARP_OPERATION_REQUEST &&
|
||||
arpPacket.targetIp == Ipv4Address)
|
||||
{
|
||||
SendArpReply(arpPacket.senderMac, macAddress, arpPacket.senderIp, Ipv4Address);
|
||||
}
|
||||
|
||||
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(arpPacket.senderIp, arpPacket.senderMac));
|
||||
}
|
||||
}
|
||||
|
||||
//
|
||||
// IPv4
|
||||
//
|
||||
|
|
@ -90,7 +24,8 @@ void HandleIpv4Packet(
|
|||
const auto offset = Ipv4Header::SerializedLength();
|
||||
|
||||
// Update ARP table
|
||||
ArpTable.insert(std::make_pair(ipv4Header.sourceIp, ethernetHeader.macSource));
|
||||
Net::Arp::ArpTable.insert(
|
||||
std::make_pair(ipv4Header.sourceIp, ethernetHeader.macSource));
|
||||
|
||||
if (ipv4Header.version != 4) return;
|
||||
if (ipv4Header.ihl != 5) return; // Not supported
|
||||
|
|
@ -128,7 +63,7 @@ void HandleUdpDatagram(
|
|||
}
|
||||
else if (udpHeader.destinationPort == UDP_PORT_TFTP)
|
||||
{
|
||||
HandleTftpDatagram(
|
||||
Net::Tftp::HandleTftpDatagram(
|
||||
ethernetHeader,
|
||||
ipv4Header,
|
||||
udpHeader,
|
||||
|
|
@ -326,4 +261,3 @@ uint32_t Ipv4Address = 0xC0A80164;
|
|||
const MacAddress MacBroadcast{0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF};
|
||||
|
||||
bool FileUploaded = false;
|
||||
std::unordered_map<std::uint32_t, MacAddress> ArpTable;
|
||||
|
|
|
|||
25
src/net.h
25
src/net.h
|
|
@ -27,23 +27,6 @@ struct EthernetFrameHeader;
|
|||
struct UdpDatagramHeader;
|
||||
struct Ipv4Header;
|
||||
|
||||
//
|
||||
// ARP
|
||||
//
|
||||
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);
|
||||
void SendArpAnnouncement(MacAddress mac, uint32_t ip);
|
||||
|
||||
//
|
||||
// IPv4
|
||||
//
|
||||
|
|
@ -60,13 +43,6 @@ void HandleUdpDatagram(
|
|||
const size_t size
|
||||
);
|
||||
|
||||
void HandleTftpDatagram(
|
||||
const EthernetFrameHeader ethernetReqHeader,
|
||||
const Ipv4Header ipv4ReqHeader,
|
||||
const UdpDatagramHeader udpReqHeader,
|
||||
const uint8_t* buffer
|
||||
);
|
||||
|
||||
//
|
||||
// ICMP
|
||||
//
|
||||
|
|
@ -84,4 +60,3 @@ extern const MacAddress MacBroadcast;
|
|||
extern uint32_t Ipv4Address;
|
||||
|
||||
extern bool FileUploaded;
|
||||
extern std::unordered_map<std::uint32_t, MacAddress> ArpTable;
|
||||
|
|
|
|||
Loading…
Reference in New Issue