theater/picomap
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Split net stack into eth/arp/ipv4/icmp, deferred handler responses, ping tests

Browse Source
This commit is contained in:
Ian Gulliver
2026-04-11 08:15:41 +09:00
parent 34efaeefd5
commit c35c1de76a
16 changed files with 522 additions and 330 deletions
Download Patch File Download Diff File Expand all files Collapse all files

282
firmware/lib/net.cpp
View File

@@ -1,255 +1,99 @@
#include "net.h"
#include <cstring>
#include <vector>
#include "pico/unique_id.h"
#include "pico/time.h"
#include "eth.h"
#include "arp.h"
#include "ipv4.h"
#include "w6300.h"
#include "debug_log.h"
using mac_addr = std::array<uint8_t, 6>;
using ip4_addr = std::array<uint8_t, 4>;
struct __attribute__((packed)) eth_header {
mac_addr dst;
mac_addr src;
uint16_t ethertype;
};
static_assert(sizeof(eth_header) == 14);
struct __attribute__((packed)) arp_packet {
eth_header eth;
uint16_t htype;
uint16_t ptype;
uint8_t hlen;
uint8_t plen;
uint16_t oper;
mac_addr sha;
ip4_addr spa;
mac_addr tha;
ip4_addr tpa;
};
static_assert(sizeof(arp_packet) == 42);
struct __attribute__((packed)) ipv4_header {
eth_header eth;
uint8_t ver_ihl;
uint8_t dscp_ecn;
uint16_t total_len;
uint16_t identification;
uint16_t flags_frag;
uint8_t ttl;
uint8_t protocol;
uint16_t checksum;
ip4_addr src;
ip4_addr dst;
size_t ip_header_len() const { return (ver_ihl & 0x0F) * 4; }
size_t ip_total_len() const { return __builtin_bswap16(total_len); }
const uint8_t* ip_start() const { return reinterpret_cast<const uint8_t*>(&ver_ihl); }
uint8_t* ip_start() { return reinterpret_cast<uint8_t*>(&ver_ihl); }
};
static_assert(sizeof(ipv4_header) == 34);
struct __attribute__((packed)) udp_header {
ipv4_header ip;
uint16_t src_port;
uint16_t dst_port;
uint16_t length;
uint16_t checksum;
};
static_assert(sizeof(udp_header) == 42);
struct __attribute__((packed)) icmp_echo {
uint8_t type;
uint8_t code;
uint16_t checksum;
uint16_t id;
uint16_t seq;
};
static_assert(sizeof(icmp_echo) == 8);
static constexpr uint16_t ETH_ARP = __builtin_bswap16(0x0806);
static constexpr uint16_t ETH_IPV4 = __builtin_bswap16(0x0800);
static constexpr uint16_t ARP_HTYPE_ETH = __builtin_bswap16(1);
static constexpr uint16_t ARP_PTYPE_IPV4 = __builtin_bswap16(0x0800);
static constexpr uint16_t ARP_OP_REQUEST = __builtin_bswap16(1);
static constexpr uint16_t ARP_OP_REPLY = __builtin_bswap16(2);
static constexpr ipv4::ip4_addr IP_BROADCAST_SUBNET = {169, 254, 255, 255};
static constexpr uint16_t PICOMAP_PORT = __builtin_bswap16(28781);
static constexpr mac_addr MAC_BROADCAST = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF};
static constexpr ip4_addr IP_BROADCAST_ALL = {255, 255, 255, 255};
static constexpr ip4_addr IP_BROADCAST_SUBNET = {169, 254, 255, 255};
static net_state state;
static w6300::socket_id raw_socket{0};
static net_handler msg_handler;
static std::vector<net_frame_callback> frame_callbacks;
static uint16_t ip_checksum(const void* data, size_t len) {
auto p = static_cast<const uint8_t*>(data);
uint32_t sum = 0;
for (size_t i = 0; i < len - 1; i += 2)
sum += (p[i] << 8) | p[i + 1];
if (len & 1)
sum += p[len - 1] << 8;
while (sum >> 16)
sum = (sum & 0xFFFF) + (sum >> 16);
return __builtin_bswap16(~sum);
}
static bool mac_match(const mac_addr& dst) {
return dst == state.mac || dst == MAC_BROADCAST;
}
static bool ip_match(const ip4_addr& dst) {
return dst == state.ip;
}
static bool ip_match_or_broadcast(const ip4_addr& dst) {
return ip_match(dst) || dst == IP_BROADCAST_ALL || dst == IP_BROADCAST_SUBNET;
}
static void send_raw(std::span<const uint8_t> data) {
dlog_if_slow("send_raw", 1000, [&]{
void net_send_raw(std::span<const uint8_t> data) {
dlog_if_slow("net_send_raw", 1000, [&]{
w6300::send(raw_socket, data);
});
}
static void handle_arp(std::span<const uint8_t> frame, span_writer &tx) {
if (frame.size() < sizeof(arp_packet)) return;
auto& pkt = *reinterpret_cast<const arp_packet*>(frame.data());
if (pkt.htype != ARP_HTYPE_ETH) return;
if (pkt.ptype != ARP_PTYPE_IPV4) return;
if (pkt.hlen != 6 || pkt.plen != 4) return;
if (pkt.oper != ARP_OP_REQUEST) return;
if (!ip_match(pkt.tpa)) return;
if (sizeof(arp_packet) > tx.capacity()) return;
auto& reply = *reinterpret_cast<arp_packet*>(tx.data());
reply = {};
reply.eth.dst = pkt.eth.src;
reply.eth.src = state.mac;
reply.eth.ethertype = ETH_ARP;
reply.htype = ARP_HTYPE_ETH;
reply.ptype = ARP_PTYPE_IPV4;
reply.hlen = 6;
reply.plen = 4;
reply.oper = ARP_OP_REPLY;
reply.sha = state.mac;
reply.spa = state.ip;
reply.tha = pkt.sha;
reply.tpa = pkt.spa;
send_raw({tx.data(), sizeof(arp_packet)});
}
static void handle_udp(std::span<const uint8_t> frame, span_writer &tx) {
if (frame.size() < sizeof(udp_header)) return;
auto& pkt = *reinterpret_cast<const udp_header*>(frame.data());
static void handle_udp(std::span<const uint8_t> frame, span_writer& tx) {
if (frame.size() < sizeof(ipv4::udp_header)) return;
auto& pkt = *reinterpret_cast<const ipv4::udp_header*>(frame.data());
if (pkt.dst_port != PICOMAP_PORT) return;
if (!ip_match_or_broadcast(pkt.ip.dst)) return;
if (!msg_handler) return;
size_t udp_len = __builtin_bswap16(pkt.length);
if (udp_len < 8) return;
if (sizeof(eth_header) + pkt.ip.ip_total_len() < sizeof(udp_header) + udp_len - 8) return;
if (sizeof(eth::header) + pkt.ip.ip_total_len() < sizeof(ipv4::udp_header) + udp_len - 8) return;
size_t payload_len = udp_len - 8;
auto resp = tx.subspan(sizeof(udp_header));
auto result = msg_handler(frame.subspan(sizeof(udp_header), payload_len), resp);
if (!result || *result == 0) return;
size_t resp_len = *result;
eth::mac_addr dst_mac = pkt.ip.eth.src;
ipv4::ip4_addr dst_ip = pkt.ip.src;
uint16_t dst_port = pkt.src_port;
size_t ip_total = 20 + 8 + resp_len;
size_t reply_len = sizeof(eth_header) + ip_total;
msg_handler(frame.subspan(sizeof(ipv4::udp_header), payload_len),
[dst_mac, dst_ip, dst_port](std::span<const uint8_t> resp_data) {
size_t ip_total = 20 + 8 + resp_data.size();
size_t reply_len = sizeof(eth::header) + ip_total;
uint8_t reply_buf[1514];
if (reply_len > sizeof(reply_buf)) return;
auto& rip = *reinterpret_cast<ipv4_header*>(tx.data());
rip.eth.dst = pkt.ip.eth.src;
rip.eth.src = state.mac;
rip.eth.ethertype = ETH_IPV4;
rip.ver_ihl = 0x45;
rip.dscp_ecn = 0;
rip.total_len = __builtin_bswap16(ip_total);
rip.identification = 0;
rip.flags_frag = 0;
rip.ttl = 64;
rip.protocol = 17;
rip.checksum = 0;
rip.src = state.ip;
rip.dst = pkt.ip.src;
rip.checksum = ip_checksum(rip.ip_start(), 20);
auto& rip = *reinterpret_cast<ipv4::header*>(reply_buf);
rip.eth.dst = dst_mac;
rip.eth.src = state.mac;
rip.eth.ethertype = eth::ETH_IPV4;
rip.ver_ihl = 0x45;
rip.dscp_ecn = 0;
rip.total_len = __builtin_bswap16(ip_total);
rip.identification = 0;
rip.flags_frag = 0;
rip.ttl = 64;
rip.protocol = 17;
rip.checksum = 0;
rip.src = state.ip;
rip.dst = dst_ip;
rip.checksum = ipv4::checksum(rip.ip_start(), 20);
auto& rudp = *reinterpret_cast<udp_header*>(tx.data());
rudp.src_port = PICOMAP_PORT;
rudp.dst_port = pkt.src_port;
rudp.length = __builtin_bswap16(8 + resp_len);
rudp.checksum = 0;
auto& rudp = *reinterpret_cast<ipv4::udp_header*>(reply_buf);
rudp.src_port = PICOMAP_PORT;
rudp.dst_port = dst_port;
rudp.length = __builtin_bswap16(8 + resp_data.size());
rudp.checksum = 0;
send_raw({tx.data(), reply_len});
memcpy(reply_buf + sizeof(ipv4::udp_header), resp_data.data(), resp_data.size());
net_send_raw({reply_buf, reply_len});
});
}
static void handle_icmp(std::span<const uint8_t> frame, span_writer &tx) {
auto& ip = *reinterpret_cast<const ipv4_header*>(frame.data());
size_t ip_hdr_len = ip.ip_header_len();
size_t ip_total = ip.ip_total_len();
if (sizeof(eth_header) + ip_total > frame.size()) return;
if (ip.protocol != 1) return;
if (!ip_match_or_broadcast(ip.dst)) return;
auto& icmp = *reinterpret_cast<const icmp_echo*>(frame.data() + sizeof(eth_header) + ip_hdr_len);
size_t icmp_len = ip_total - ip_hdr_len;
if (icmp_len < sizeof(icmp_echo)) return;
if (icmp.type != 8) return;
size_t reply_len = sizeof(eth_header) + ip_total;
if (reply_len > tx.capacity()) return;
memcpy(tx.data(), frame.data(), reply_len);
auto& rip = *reinterpret_cast<ipv4_header*>(tx.data());
rip.eth.dst = ip.eth.src;
rip.eth.src = state.mac;
rip.src = state.ip;
rip.dst = ip.src;
rip.ttl = 64;
rip.checksum = 0;
rip.checksum = ip_checksum(rip.ip_start(), ip_hdr_len);
auto& ricmp = *reinterpret_cast<icmp_echo*>(tx.data() + sizeof(eth_header) + ip_hdr_len);
ricmp.type = 0;
ricmp.checksum = 0;
ricmp.checksum = ip_checksum(&ricmp, icmp_len);
send_raw({tx.data(), reply_len});
static bool mac_match(const eth::mac_addr& dst) {
return dst == state.mac || dst == eth::MAC_BROADCAST;
}
static void handle_ipv4(std::span<const uint8_t> frame, span_writer &tx) {
if (frame.size() < sizeof(ipv4_header)) return;
auto& ip = *reinterpret_cast<const ipv4_header*>(frame.data());
if ((ip.ver_ihl >> 4) != 4) return;
static void process_frame(std::span<const uint8_t> frame, span_writer& tx) {
if (frame.size() < sizeof(eth::header)) return;
auto& eth_hdr = *reinterpret_cast<const eth::header*>(frame.data());
switch (ip.protocol) {
case 1:
handle_icmp(frame, tx);
if (!mac_match(eth_hdr.dst)) return;
auto cbs = std::move(frame_callbacks);
frame_callbacks.clear();
for (auto& cb : cbs)
cb(frame);
switch (eth_hdr.ethertype) {
case eth::ETH_ARP:
arp::handle(frame, tx, state.mac, state.ip, net_send_raw);
break;
case 17:
handle_udp(frame, tx);
break;
}
}
static void process_frame(std::span<const uint8_t> frame, span_writer &tx) {
if (frame.size() < sizeof(eth_header)) return;
auto& eth = *reinterpret_cast<const eth_header*>(frame.data());
if (!mac_match(eth.dst)) return;
switch (eth.ethertype) {
case ETH_ARP:
handle_arp(frame, tx);
break;
case ETH_IPV4:
handle_ipv4(frame, tx);
case eth::ETH_IPV4:
ipv4::handle(frame, tx, state.mac, state.ip, IP_BROADCAST_SUBNET, net_send_raw, handle_udp);
break;
}
}
@@ -288,6 +132,10 @@ void net_set_handler(net_handler handler) {
msg_handler = std::move(handler);
}
void net_add_frame_callback(net_frame_callback cb) {
frame_callbacks.push_back(std::move(cb));
}
void net_poll(std::span<uint8_t> tx) {
if (!w6300::irq_pending) return;
w6300::irq_pending = false;