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

Self-registering IP-protocol and UDP-port dispatch; move picomap-specific constants out of generic net/dispatch layers

Browse Source
This commit is contained in:
Ian Gulliver
2026-04-19 08:39:13 -07:00
parent 7e493b7d70
commit 4db5c36931
16 changed files with 120 additions and 77 deletions
Download Patch File Download Diff File Expand all files Collapse all files

32
firmware/lib/dispatch.cpp
View File

@@ -11,14 +11,29 @@
static timer_queue timers;
static std::array<handler_fn, 128> handler_map{};
static uint16_t listen_port_be = 0;
uint16_t dispatch_listen_port_be() { return listen_port_be; }
static void igmp_reannounce() {
auto& ns = net_get_state();
igmp::send_all_reports(ns.mac, ns.ip);
igmp::send_all_reports();
dispatch_schedule_ms(60000, igmp_reannounce);
}
void dispatch_init() {
static void on_udp_message(std::span<const uint8_t> payload, const udp::address& from) {
auto msg = try_decode(payload.data(), payload.size());
if (!msg) return;
if (msg->type_id < 0 || !handler_map[msg->type_id]) {
dlogf("dispatch: unknown type_id %d", msg->type_id);
return;
}
responder resp{msg->message_id, from};
handler_map[msg->type_id](resp, msg->payload);
}
void dispatch_init(uint16_t port_be) {
listen_port_be = port_be;
udp::register_port(port_be, on_udp_message);
net_init();
dispatch_schedule_ms(60000, igmp_reannounce);
dlog("dispatch_init complete");
@@ -34,17 +49,6 @@ bool dispatch_cancel_timer(timer_handle h) {
return timers.cancel(h);
}
void udp::client::handler(std::span<const uint8_t> payload, const udp::address& from) {
auto msg = try_decode(payload.data(), payload.size());
if (!msg) return;
if (msg->type_id < 0 || !handler_map[msg->type_id]) {
dlogf("dispatch: unknown type_id %d", msg->type_id);
return;
}
responder resp{msg->message_id, from};
handler_map[msg->type_id](resp, msg->payload);
}
[[noreturn]] void dispatch_run(std::span<const handler_entry> handlers) {
for (auto& entry : handlers)
handler_map[entry.type_id] = entry.handle;

13
firmware/lib/icmp.cpp
View File

@@ -7,13 +7,12 @@
namespace icmp {
void handle(std::span<const uint8_t> frame, span_writer& tx,
eth::mac_addr our_mac, ipv4::ip4_addr our_ip) {
void handle(std::span<const uint8_t> frame, span_writer& tx) {
parse_buffer pb(frame);
auto* eth_hdr = pb.consume<eth::header>();
auto* ip = pb.consume<ipv4::header>();
if (!ip) return;
if (ip->protocol != 1) return;
if (!ipv4::addressed_to_us(ip->dst)) return;
size_t options_len = ip->header_len() - sizeof(ipv4::header);
if (options_len > 0 && !pb.skip(options_len)) return;
@@ -25,6 +24,7 @@ void handle(std::span<const uint8_t> frame, span_writer& tx,
if (!icmp_pkt) return;
if (icmp_pkt->type != 8) return;
const auto& ns = net_get_state();
prepend_buffer<4096> buf;
memcpy(buf.append(icmp_len), pb.remaining().data() - sizeof(echo), icmp_len);
@@ -33,10 +33,15 @@ void handle(std::span<const uint8_t> frame, span_writer& tx,
reply->checksum = 0;
reply->checksum = ipv4::checksum(reply, icmp_len);
ipv4::prepend(buf, eth_hdr->src, our_mac, our_ip, ip->src, 1, icmp_len);
ipv4::prepend(buf, eth_hdr->src, ns.mac, ns.ip, ip->src, 1, icmp_len);
net_send_raw(buf.span());
}
__attribute__((constructor))
static void register_protocol() {
ipv4::register_protocol(1, handle);
}
bool parse_echo_reply(std::span<const uint8_t> frame, ipv4::ip4_addr& src_ip, uint16_t expected_id) {
parse_buffer pb(frame);
auto* eth_hdr = pb.consume<eth::header>();

27
firmware/lib/igmp.cpp
View File

@@ -34,28 +34,26 @@ bool is_member_mac(const eth::mac_addr& mac) {
return false;
}
static void send_report(const ipv4::ip4_addr& group,
eth::mac_addr our_mac, ipv4::ip4_addr our_ip) {
static void send_report(const ipv4::ip4_addr& group) {
const auto& ns = net_get_state();
prepend_buffer<4096> buf;
prepend_report(buf, our_mac, our_ip, group);
prepend_report(buf, ns.mac, ns.ip, group);
net_send_raw(buf.span());
}
void join(const ipv4::ip4_addr& group,
eth::mac_addr our_mac, ipv4::ip4_addr our_ip) {
void join(const ipv4::ip4_addr& group) {
for (auto& g : groups)
if (g.ip == group) return;
groups.push_back({group, mac_for_ip(group)});
send_report(group, our_mac, our_ip);
send_report(group);
}
void send_all_reports(eth::mac_addr our_mac, ipv4::ip4_addr our_ip) {
void send_all_reports() {
for (auto& g : groups)
send_report(g.ip, our_mac, our_ip);
send_report(g.ip);
}
void handle(std::span<const uint8_t> frame, span_writer& tx,
eth::mac_addr our_mac, ipv4::ip4_addr our_ip) {
void handle(std::span<const uint8_t> frame, span_writer& tx) {
parse_buffer pb(frame);
pb.consume<eth::header>();
auto* ip = pb.consume<ipv4::header>();
@@ -70,17 +68,22 @@ void handle(std::span<const uint8_t> frame, span_writer& tx,
if (msg->group == ipv4::ip4_addr{0, 0, 0, 0}) {
for (auto& g : groups)
send_report(g.ip, our_mac, our_ip);
send_report(g.ip);
} else {
for (auto& g : groups) {
if (g.ip == msg->group) {
send_report(g.ip, our_mac, our_ip);
send_report(g.ip);
break;
}
}
}
}
__attribute__((constructor))
static void register_protocol() {
ipv4::register_protocol(2, handle);
}
bool parse_report(std::span<const uint8_t> frame, ipv4::ip4_addr& group) {
parse_buffer pb(frame);
auto* eth_hdr = pb.consume<eth::header>();

40
firmware/lib/ipv4.cpp
View File

@@ -1,8 +1,7 @@
#include "ipv4.h"
#include "icmp.h"
#include <array>
#include "igmp.h"
#include "net.h"
#include "udp.h"
#include "parse_buffer.h"
namespace ipv4 {
@@ -21,12 +20,24 @@ uint16_t checksum(const void* data, size_t len) {
return __builtin_bswap16(~sum);
}
static bool ip_match(const ip4_addr& dst, const ip4_addr& our_ip) {
return dst == our_ip || dst == IP_BROADCAST_ALL || dst == SUBNET_BROADCAST || igmp::is_member(dst);
bool addressed_to_us(ip4_addr dst) {
const auto& ns = net_get_state();
return dst == ns.ip || dst == IP_BROADCAST_ALL || dst == SUBNET_BROADCAST || igmp::is_member(dst);
}
struct protocol_entry {
uint8_t protocol;
protocol_handler fn;
};
static std::array<protocol_entry, 8> protocol_handlers;
static size_t protocol_handler_count = 0;
void register_protocol(uint8_t protocol, protocol_handler fn) {
if (protocol_handler_count < protocol_handlers.size())
protocol_handlers[protocol_handler_count++] = {protocol, fn};
}
void handle(std::span<const uint8_t> frame, span_writer& tx) {
const auto& ns = net_get_state();
parse_buffer pb(frame);
pb.consume<eth::header>();
auto* ip = pb.consume<header>();
@@ -36,20 +47,11 @@ void handle(std::span<const uint8_t> frame, span_writer& tx) {
size_t options_len = ip->header_len() - sizeof(header);
if (options_len > 0 && !pb.skip(options_len)) return;
switch (ip->protocol) {
case 1:
if (!ip_match(ip->dst, ns.ip))
return;
icmp::handle(frame, tx, ns.mac, ns.ip);
break;
case 2:
igmp::handle(frame, tx, ns.mac, ns.ip);
break;
case 17:
if (!ip_match(ip->dst, ns.ip))
return;
udp::handle(frame, tx);
break;
for (size_t i = 0; i < protocol_handler_count; i++) {
if (protocol_handlers[i].protocol == ip->protocol) {
protocol_handlers[i].fn(frame, tx);
break;
}
}
}

2
firmware/lib/net.cpp
View File

@@ -80,8 +80,6 @@ bool net_init() {
w6300::open_socket(raw_socket, w6300::protocol::macraw, w6300::sock_flag::none);
w6300::set_interrupt_mask(w6300::ik_sock_0);
igmp::join(igmp::PICOMAP_DISCOVERY_GROUP, state.mac, state.ip);
return true;
}

9
firmware/lib/test_handlers.cpp
View File

@@ -3,6 +3,7 @@
#include <unordered_map>
#include "pico/stdlib.h"
#include "pico/time.h"
#include "handlers.h"
#include "net.h"
#include "icmp.h"
#include "igmp.h"
@@ -119,7 +120,7 @@ static void discover_peer(discovery_data& d,
ts.active_discovery = &d;
const auto& ns = net_get_state();
eth::mac_addr mcast_mac = igmp::mac_for_ip(igmp::PICOMAP_DISCOVERY_GROUP);
eth::mac_addr mcast_mac = igmp::mac_for_ip(PICOMAP_DISCOVERY_GROUP);
prepend_buffer<4096> buf;
uint8_t* payload = buf.payload_ptr();
@@ -133,7 +134,7 @@ static void discover_peer(discovery_data& d,
}
buf.append(*encoded);
udp::prepend(buf, mcast_mac, ns.mac, ns.ip, igmp::PICOMAP_DISCOVERY_GROUP,
udp::prepend(buf, mcast_mac, ns.mac, ns.ip, PICOMAP_DISCOVERY_GROUP,
PICOMAP_PORT_BE, PICOMAP_PORT_BE, *encoded, 1);
ts.frame_cb = net_add_frame_callback(discover_reply_cb);
@@ -145,7 +146,7 @@ static void discover_peer(discovery_data& d,
static bool igmp_report_cb(std::span<const uint8_t> frame) {
ipv4::ip4_addr group;
if (!igmp::parse_report(frame, group)) return false;
if (group != igmp::PICOMAP_DISCOVERY_GROUP) return false;
if (group != PICOMAP_DISCOVERY_GROUP) return false;
ts.frame_cb = nullptr;
test_end({true, {"got IGMP report for " + ipv4::to_string(group)}});
return true;
@@ -159,7 +160,7 @@ static void igmp_timeout_cb() {
static void test_discovery_igmp() {
const auto& ns = net_get_state();
prepend_buffer<4096> buf;
igmp::prepend_query(buf, ns.mac, ns.ip, igmp::PICOMAP_DISCOVERY_GROUP);
igmp::prepend_query(buf, ns.mac, ns.ip, PICOMAP_DISCOVERY_GROUP);
ts.frame_cb = net_add_frame_callback(igmp_report_cb);
ts.timer = dispatch_schedule_ms(5000, igmp_timeout_cb);

29
firmware/lib/udp.cpp
View File

@@ -1,4 +1,5 @@
#include "udp.h"
#include <array>
#include "eth.h"
#include "ipv4.h"
#include "net.h"
@@ -6,26 +7,48 @@
namespace udp {
struct port_entry {
uint16_t port_be;
port_handler fn;
};
static std::array<port_entry, 8> port_handlers;
static size_t port_handler_count = 0;
void register_port(uint16_t port_be, port_handler fn) {
if (port_handler_count < port_handlers.size())
port_handlers[port_handler_count++] = {port_be, fn};
}
void handle(std::span<const uint8_t> frame, span_writer& tx) {
parse_buffer pb(frame);
auto* eth_hdr = pb.consume<eth::header>();
auto* ip = pb.consume<ipv4::header>();
if (!ip) return;
if (!ipv4::addressed_to_us(ip->dst)) return;
size_t options_len = ip->header_len() - sizeof(ipv4::header);
if (options_len > 0 && !pb.skip(options_len)) return;
auto* uhdr = pb.consume<header>();
if (!uhdr) return;
if (uhdr->dst_port != PICOMAP_PORT_BE) return;
size_t udp_len = __builtin_bswap16(uhdr->length);
if (udp_len < sizeof(header)) return;
size_t payload_len = udp_len - sizeof(header);
if (pb.remaining_size() < payload_len) return;
address from{eth_hdr->src, ip->src, uhdr->src_port};
client::handler(pb.remaining().subspan(0, payload_len), from);
for (size_t i = 0; i < port_handler_count; i++) {
if (port_handlers[i].port_be == uhdr->dst_port) {
address from{eth_hdr->src, ip->src, uhdr->src_port};
port_handlers[i].fn(pb.remaining().subspan(0, payload_len), from);
break;
}
}
}
__attribute__((constructor))
static void register_protocol() {
ipv4::register_protocol(17, handle);
}
} // namespace udp