diff --git a/firmware/firmware.cpp b/firmware/firmware.cpp index f8441c1..807147c 100644 --- a/firmware/firmware.cpp +++ b/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); } diff --git a/firmware/include/dispatch.h b/firmware/include/dispatch.h index fe0686c..1137c11 100644 --- a/firmware/include/dispatch.h +++ b/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 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 handlers); diff --git a/firmware/include/handlers.h b/firmware/include/handlers.h index 7e0082d..6c740f9 100644 --- a/firmware/include/handlers.h +++ b/firmware/include/handlers.h @@ -1,9 +1,14 @@ #pragma once +#include #include #include #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(); diff --git a/firmware/include/icmp.h b/firmware/include/icmp.h index 4946587..b4f67b9 100644 --- a/firmware/include/icmp.h +++ b/firmware/include/icmp.h @@ -16,8 +16,7 @@ struct __attribute__((packed)) echo { }; static_assert(sizeof(echo) == 8); -void handle(std::span frame, span_writer& tx, - eth::mac_addr our_mac, ipv4::ip4_addr our_ip); +void handle(std::span frame, span_writer& tx); template void prepend_echo_request(Buf& buf, diff --git a/firmware/include/igmp.h b/firmware/include/igmp.h index c066e1a..fc6bf10 100644 --- a/firmware/include/igmp.h +++ b/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, - eth::mac_addr our_mac, ipv4::ip4_addr our_ip); +void join(const ipv4::ip4_addr& group); -void send_all_reports(eth::mac_addr our_mac, ipv4::ip4_addr our_ip); +void send_all_reports(); -void handle(std::span frame, span_writer& tx, - eth::mac_addr our_mac, ipv4::ip4_addr our_ip); +void handle(std::span frame, span_writer& tx); template void prepend_report(Buf& buf, const eth::mac_addr& src_mac, ipv4::ip4_addr src_ip, diff --git a/firmware/include/ipv4.h b/firmware/include/ipv4.h index eea75a8..2f4ad92 100644 --- a/firmware/include/ipv4.h +++ b/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 frame, span_writer& tx); +bool addressed_to_us(ip4_addr dst); + +using protocol_handler = void (*)(std::span frame, span_writer& tx); +void register_protocol(uint8_t protocol, protocol_handler fn); + } // namespace ipv4 diff --git a/firmware/include/net.h b/firmware/include/net.h index 912fc2c..813204f 100644 --- a/firmware/include/net.h +++ b/firmware/include/net.h @@ -18,8 +18,6 @@ using frame_cb_handle = frame_cb_list::node*; using ethertype_handler = void (*)(std::span 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); diff --git a/firmware/include/udp.h b/firmware/include/udp.h index c496835..71c6e9e 100644 --- a/firmware/include/udp.h +++ b/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 frame, span_writer& tx); -namespace client { -// Defined by the higher layer (dispatch) to receive decoded UDP payloads -// addressed to PICOMAP_PORT_BE. Resolved at link time. -void handler(std::span payload, const address& from); -} // namespace client +using port_handler = void (*)(std::span payload, const address& from); +void register_port(uint16_t port_be, port_handler fn); } // namespace udp diff --git a/firmware/lib/dispatch.cpp b/firmware/lib/dispatch.cpp index 75fffe1..a4b9cc0 100644 --- a/firmware/lib/dispatch.cpp +++ b/firmware/lib/dispatch.cpp @@ -11,14 +11,29 @@ static timer_queue timers; static std::array 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 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 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 handlers) { for (auto& entry : handlers) handler_map[entry.type_id] = entry.handle; diff --git a/firmware/lib/icmp.cpp b/firmware/lib/icmp.cpp index 0fe864b..9b717f5 100644 --- a/firmware/lib/icmp.cpp +++ b/firmware/lib/icmp.cpp @@ -7,13 +7,12 @@ namespace icmp { -void handle(std::span frame, span_writer& tx, - eth::mac_addr our_mac, ipv4::ip4_addr our_ip) { +void handle(std::span frame, span_writer& tx) { parse_buffer pb(frame); auto* eth_hdr = pb.consume(); auto* ip = pb.consume(); 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 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 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 frame, ipv4::ip4_addr& src_ip, uint16_t expected_id) { parse_buffer pb(frame); auto* eth_hdr = pb.consume(); diff --git a/firmware/lib/igmp.cpp b/firmware/lib/igmp.cpp index b8878cd..43aaa2e 100644 --- a/firmware/lib/igmp.cpp +++ b/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, - 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 frame, span_writer& tx, - eth::mac_addr our_mac, ipv4::ip4_addr our_ip) { +void handle(std::span frame, span_writer& tx) { parse_buffer pb(frame); pb.consume(); auto* ip = pb.consume(); @@ -70,17 +68,22 @@ void handle(std::span 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 frame, ipv4::ip4_addr& group) { parse_buffer pb(frame); auto* eth_hdr = pb.consume(); diff --git a/firmware/lib/ipv4.cpp b/firmware/lib/ipv4.cpp index e42f1b9..99fd7b4 100644 --- a/firmware/lib/ipv4.cpp +++ b/firmware/lib/ipv4.cpp @@ -1,8 +1,7 @@ #include "ipv4.h" -#include "icmp.h" +#include #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_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 frame, span_writer& tx) { - const auto& ns = net_get_state(); parse_buffer pb(frame); pb.consume(); auto* ip = pb.consume
(); @@ -36,20 +47,11 @@ void handle(std::span 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; + } } } diff --git a/firmware/lib/net.cpp b/firmware/lib/net.cpp index 352871f..1a4378c 100644 --- a/firmware/lib/net.cpp +++ b/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; } diff --git a/firmware/lib/test_handlers.cpp b/firmware/lib/test_handlers.cpp index e756ae7..7fdadee 100644 --- a/firmware/lib/test_handlers.cpp +++ b/firmware/lib/test_handlers.cpp @@ -3,6 +3,7 @@ #include #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 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); diff --git a/firmware/lib/udp.cpp b/firmware/lib/udp.cpp index 28f809c..52af99d 100644 --- a/firmware/lib/udp.cpp +++ b/firmware/lib/udp.cpp @@ -1,4 +1,5 @@ #include "udp.h" +#include #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_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 frame, span_writer& tx) { parse_buffer pb(frame); auto* eth_hdr = pb.consume(); auto* ip = pb.consume(); 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
(); 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 diff --git a/firmware/test.cpp b/firmware/test.cpp index 92025a7..5d65c75 100644 --- a/firmware/test.cpp +++ b/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);