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

firmware/firmware.cpp

@@ -1,5 +1,6 @@
 #include "dispatch.h"
 #include "handlers.h"
+#include "igmp.h"
 
 std::string_view firmware_name = "picomap";
 
@@ -14,7 +15,8 @@ static constexpr handler_entry handlers[] = {
 
 int main() {
     handlers_init();
-    dispatch_init();
+    dispatch_init(PICOMAP_PORT_BE);
+    igmp::join(PICOMAP_DISCOVERY_GROUP);
     handlers_start();
     dispatch_run(handlers);
 }

firmware/include/dispatch.h

@@ -9,6 +9,8 @@
 #include "prepend_buffer.h"
 #include "udp.h"
 
+uint16_t dispatch_listen_port_be();
+
 struct responder {
     uint32_t message_id;
     udp::address reply_to;
@@ -22,7 +24,7 @@ struct responder {
         if (!r) return;
         buf.append(*r);
         udp::prepend(buf, reply_to.mac, ns.mac, ns.ip, reply_to.ip,
-                     PICOMAP_PORT_BE, reply_to.port, *r);
+                     dispatch_listen_port_be(), reply_to.port, *r);
         net_send_raw(buf.span());
     }
 };
@@ -52,7 +54,7 @@ void typed_handler(const responder& resp, std::span<const uint8_t> payload) {
         resp.respond(*result);
 }
 
-void dispatch_init();
+void dispatch_init(uint16_t listen_port_be);
 timer_handle dispatch_schedule_ms(uint32_t ms, void (*fn)());
 bool dispatch_cancel_timer(timer_handle h);
 [[noreturn]] void dispatch_run(std::span<const handler_entry> handlers);

firmware/include/handlers.h

@@ -1,9 +1,14 @@
 #pragma once
+#include <cstdint>
 #include <optional>
 #include <string_view>
 #include "dispatch.h"
+#include "ipv4.h"
 #include "wire.h"
 
+inline constexpr uint16_t PICOMAP_PORT_BE = __builtin_bswap16(28781);
+inline constexpr ipv4::ip4_addr PICOMAP_DISCOVERY_GROUP = {239, 112, 77, 1};
+
 extern std::string_view firmware_name;
 
 void handlers_init();

firmware/include/icmp.h

@@ -16,8 +16,7 @@ struct __attribute__((packed)) echo {
 };
 static_assert(sizeof(echo) == 8);
 
-void handle(std::span<const uint8_t> frame, span_writer& tx,
+void handle(std::span<const uint8_t> frame, span_writer& tx);
-            eth::mac_addr our_mac, ipv4::ip4_addr our_ip);
 
 template <typename Buf>
 void prepend_echo_request(Buf& buf,

firmware/include/igmp.h

@@ -7,7 +7,6 @@
 
 namespace igmp {
 
-static constexpr ipv4::ip4_addr PICOMAP_DISCOVERY_GROUP = {239, 112, 77, 1};
 static constexpr ipv4::ip4_addr ALL_HOSTS = {224, 0, 0, 1};
 
 struct __attribute__((packed)) message {
@@ -22,13 +21,11 @@ eth::mac_addr mac_for_ip(const ipv4::ip4_addr& group);
 bool is_member(const ipv4::ip4_addr& ip);
 bool is_member_mac(const eth::mac_addr& mac);
 
-void join(const ipv4::ip4_addr& group,
+void join(const ipv4::ip4_addr& group);
-          eth::mac_addr our_mac, ipv4::ip4_addr our_ip);
 
-void send_all_reports(eth::mac_addr our_mac, ipv4::ip4_addr our_ip);
+void send_all_reports();
 
-void handle(std::span<const uint8_t> frame, span_writer& tx,
+void handle(std::span<const uint8_t> frame, span_writer& tx);
-            eth::mac_addr our_mac, ipv4::ip4_addr our_ip);
 
 template <typename Buf>
 void prepend_report(Buf& buf, const eth::mac_addr& src_mac, ipv4::ip4_addr src_ip,

firmware/include/ipv4.h

@@ -59,4 +59,9 @@ void prepend(Buf& buf, const eth::mac_addr& dst_mac, const eth::mac_addr& src_ma
 
 void handle(std::span<const uint8_t> frame, span_writer& tx);
 
+bool addressed_to_us(ip4_addr dst);
+
+using protocol_handler = void (*)(std::span<const uint8_t> frame, span_writer& tx);
+void register_protocol(uint8_t protocol, protocol_handler fn);
+
 } // namespace ipv4

firmware/include/net.h

@@ -18,8 +18,6 @@ using frame_cb_handle = frame_cb_list::node*;
 
 using ethertype_handler = void (*)(std::span<const uint8_t> frame, span_writer& tx);
 
-inline constexpr uint16_t PICOMAP_PORT_BE = __builtin_bswap16(28781);
-
 bool net_init();
 const net_state& net_get_state();
 frame_cb_handle net_add_frame_callback(net_frame_callback cb);

firmware/include/udp.h

@@ -39,10 +39,7 @@ void prepend(Buf& buf, const eth::mac_addr& dst_mac, const eth::mac_addr& src_ma
 
 void handle(std::span<const uint8_t> frame, span_writer& tx);
 
-namespace client {
+using port_handler = void (*)(std::span<const uint8_t> payload, const address& from);
-// Defined by the higher layer (dispatch) to receive decoded UDP payloads
+void register_port(uint16_t port_be, port_handler fn);
-// addressed to PICOMAP_PORT_BE. Resolved at link time.
-void handler(std::span<const uint8_t> payload, const address& from);
-} // namespace client
 
 } // namespace udp

firmware/lib/dispatch.cpp

@@ -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();
-    igmp::send_all_reports(ns.mac, ns.ip);
     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;

firmware/lib/icmp.cpp

@@ -7,13 +7,12 @@
 
 namespace icmp {
 
-void handle(std::span<const uint8_t> frame, span_writer& tx,
+void handle(std::span<const uint8_t> frame, span_writer& tx) {
-            eth::mac_addr our_mac, ipv4::ip4_addr our_ip) {
     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>();

firmware/lib/igmp.cpp

@@ -34,28 +34,26 @@ bool is_member_mac(const eth::mac_addr& mac) {
     return false;
 }
 
-static void send_report(const ipv4::ip4_addr& group,
+static void send_report(const ipv4::ip4_addr& group) {
-                         eth::mac_addr our_mac, ipv4::ip4_addr our_ip) {
+    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,
+void join(const ipv4::ip4_addr& group) {
-          eth::mac_addr our_mac, ipv4::ip4_addr our_ip) {
     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,
+void handle(std::span<const uint8_t> frame, span_writer& tx) {
-            eth::mac_addr our_mac, ipv4::ip4_addr our_ip) {
     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>();

firmware/lib/ipv4.cpp

@@ -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) {
+bool addressed_to_us(ip4_addr dst) {
-    return dst == our_ip || dst == IP_BROADCAST_ALL || dst == SUBNET_BROADCAST || igmp::is_member(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) {
+    for (size_t i = 0; i < protocol_handler_count; i++) {
-    case 1:
+        if (protocol_handlers[i].protocol == ip->protocol) {
-        if (!ip_match(ip->dst, ns.ip))
+            protocol_handlers[i].fn(frame, tx);
-            return;
+            break;
-        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;
     }
 }
 

firmware/lib/net.cpp

@@ -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;
 }
 

firmware/lib/test_handlers.cpp

@@ -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);

firmware/lib/udp.cpp

@@ -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};
+    for (size_t i = 0; i < port_handler_count; i++) {
-    client::handler(pb.remaining().subspan(0, payload_len), from);
+        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

firmware/test.cpp

@@ -2,6 +2,7 @@
 #include "hardware/gpio.h"
 #include "dispatch.h"
 #include "handlers.h"
+#include "igmp.h"
 #include "test_handlers.h"
 
 static constexpr uint8_t LED_PIN = 25;
@@ -26,7 +27,8 @@ static constexpr handler_entry handlers[] = {
 
 int main() {
     handlers_init();
-    dispatch_init();
+    dispatch_init(PICOMAP_PORT_BE);
+    igmp::join(PICOMAP_DISCOVERY_GROUP);
     handlers_start();
 
     gpio_init(LED_PIN);