From b82c038091086065b17b5ca6d979a8712f93494b Mon Sep 17 00:00:00 2001 From: Ian Gulliver Date: Fri, 1 May 2026 10:15:54 -0700 Subject: [PATCH] net::/ipv4:: namespace, mac/addr filter framework with default filters, dlog on registry overflow --- include/dispatch.h | 8 ++- include/ipv4.h | 3 + include/net.h | 26 +++++--- src/arp.cpp | 6 +- src/dispatch.cpp | 4 +- src/handlers.cpp | 2 +- src/icmp.cpp | 4 +- src/igmp.cpp | 6 +- src/ipv4.cpp | 89 +++++++++++++++++++-------- src/net.cpp | 137 ++++++++++++++++++++++++++++-------------- src/test_handlers.cpp | 36 +++++------ src/udp.cpp | 8 ++- 12 files changed, 216 insertions(+), 113 deletions(-) diff --git a/include/dispatch.h b/include/dispatch.h index 1137c11..7b3a9a7 100644 --- a/include/dispatch.h +++ b/include/dispatch.h @@ -17,15 +17,17 @@ struct responder { template void respond(const T& msg) const { - const auto& ns = net_get_state(); + const auto& ns = net::get_state(); prepend_buffer<4096> buf; span_writer out(buf.payload_ptr(), 2048); auto r = encode_response_into(out, message_id, msg); - if (!r) return; + if (!r) { + return; + } buf.append(*r); udp::prepend(buf, reply_to.mac, ns.mac, ns.ip, reply_to.ip, dispatch_listen_port_be(), reply_to.port, *r); - net_send_raw(buf.span()); + net::send_raw(buf.span()); } }; diff --git a/include/ipv4.h b/include/ipv4.h index 2f4ad92..8cae60d 100644 --- a/include/ipv4.h +++ b/include/ipv4.h @@ -64,4 +64,7 @@ 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); +using addr_filter = bool (*)(const ip4_addr& dst); +void register_addr_filter(addr_filter fn); + } // namespace ipv4 diff --git a/include/net.h b/include/net.h index 813204f..44110a1 100644 --- a/include/net.h +++ b/include/net.h @@ -6,22 +6,28 @@ #include "span_writer.h" #include "callback_list.h" -struct net_state { +namespace net { + +struct state { eth::mac_addr mac; ipv4::ip4_addr ip; }; -using net_frame_callback = bool (*)(std::span frame); +using frame_callback = bool (*)(std::span frame); -using frame_cb_list = callback_list; +using frame_cb_list = callback_list; using frame_cb_handle = frame_cb_list::node*; using ethertype_handler = void (*)(std::span frame, span_writer& tx); +using mac_filter = bool (*)(const eth::mac_addr& dst); -bool net_init(); -const net_state& net_get_state(); -frame_cb_handle net_add_frame_callback(net_frame_callback cb); -void net_remove_frame_callback(frame_cb_handle h); -void net_poll(std::span tx); -void net_send_raw(std::span data); -void net_register_ethertype(uint16_t ethertype_be, ethertype_handler fn); +bool init(); +const state& get_state(); +frame_cb_handle add_frame_callback(frame_callback cb); +void remove_frame_callback(frame_cb_handle h); +void poll(std::span tx); +void send_raw(std::span data); +void register_ethertype(uint16_t ethertype_be, ethertype_handler fn); +void register_mac_filter(mac_filter fn); + +} // namespace net diff --git a/src/arp.cpp b/src/arp.cpp index 0fce56b..30d20b8 100644 --- a/src/arp.cpp +++ b/src/arp.cpp @@ -11,7 +11,7 @@ static constexpr uint16_t ARP_OP_REQUEST = __builtin_bswap16(1); static constexpr uint16_t ARP_OP_REPLY = __builtin_bswap16(2); void handle(std::span frame, span_writer& tx) { - const auto& ns = net_get_state(); + const auto& ns = net::get_state(); parse_buffer pb(frame); pb.consume(); auto* arp_hdr = pb.consume
(); @@ -36,12 +36,12 @@ void handle(std::span frame, span_writer& tx) { reply->tpa = arp_hdr->spa; eth::prepend(buf, arp_hdr->sha, ns.mac, eth::ETH_ARP); - net_send_raw(buf.span()); + net::send_raw(buf.span()); } __attribute__((constructor)) static void register_ethertype() { - net_register_ethertype(eth::ETH_ARP, handle); + net::register_ethertype(eth::ETH_ARP, handle); } } // namespace arp diff --git a/src/dispatch.cpp b/src/dispatch.cpp index a4b9cc0..2470a11 100644 --- a/src/dispatch.cpp +++ b/src/dispatch.cpp @@ -34,7 +34,7 @@ static void on_udp_message(std::span payload, const udp::address& void dispatch_init(uint16_t port_be) { listen_port_be = port_be; udp::register_port(port_be, on_udp_message); - net_init(); + net::init(); dispatch_schedule_ms(60000, igmp_reannounce); dlog("dispatch_init complete"); } @@ -59,7 +59,7 @@ bool dispatch_cancel_timer(timer_handle h) { uint32_t save = save_and_disable_interrupts(); dlog_if_slow("timers", 1000, [&]{ timers.run(); }); - dlog_if_slow("net_poll", 1000, [&]{ net_poll(std::span{tx_buf}); }); + dlog_if_slow("net::poll", 1000, [&]{ net::poll(std::span{tx_buf}); }); __wfi(); restore_interrupts(save); diff --git a/src/handlers.cpp b/src/handlers.cpp index 31bd0cb..c010095 100644 --- a/src/handlers.cpp +++ b/src/handlers.cpp @@ -34,7 +34,7 @@ std::optional handle_info(const responder&, const RequestInfo&) { pico_unique_board_id_t uid; pico_get_unique_board_id(&uid); std::copy(uid.id, uid.id + 8, resp.board_id.begin()); - auto& ns = net_get_state(); + auto& ns = net::get_state(); resp.mac = ns.mac; resp.ip = ns.ip; resp.firmware_name = firmware_name; diff --git a/src/icmp.cpp b/src/icmp.cpp index 9b717f5..686d424 100644 --- a/src/icmp.cpp +++ b/src/icmp.cpp @@ -24,7 +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(); + const auto& ns = net::get_state(); prepend_buffer<4096> buf; memcpy(buf.append(icmp_len), pb.remaining().data() - sizeof(echo), icmp_len); @@ -34,7 +34,7 @@ void handle(std::span frame, span_writer& tx) { reply->checksum = ipv4::checksum(reply, icmp_len); ipv4::prepend(buf, eth_hdr->src, ns.mac, ns.ip, ip->src, 1, icmp_len); - net_send_raw(buf.span()); + net::send_raw(buf.span()); } __attribute__((constructor)) diff --git a/src/igmp.cpp b/src/igmp.cpp index 43aaa2e..0849e41 100644 --- a/src/igmp.cpp +++ b/src/igmp.cpp @@ -35,10 +35,10 @@ bool is_member_mac(const eth::mac_addr& mac) { } static void send_report(const ipv4::ip4_addr& group) { - const auto& ns = net_get_state(); + const auto& ns = net::get_state(); prepend_buffer<4096> buf; prepend_report(buf, ns.mac, ns.ip, group); - net_send_raw(buf.span()); + net::send_raw(buf.span()); } void join(const ipv4::ip4_addr& group) { @@ -82,6 +82,8 @@ void handle(std::span frame, span_writer& tx) { __attribute__((constructor)) static void register_protocol() { ipv4::register_protocol(2, handle); + net::register_mac_filter(is_member_mac); + ipv4::register_addr_filter(is_member); } bool parse_report(std::span frame, ipv4::ip4_addr& group) { diff --git a/src/ipv4.cpp b/src/ipv4.cpp index 99fd7b4..2e24223 100644 --- a/src/ipv4.cpp +++ b/src/ipv4.cpp @@ -1,51 +1,87 @@ #include "ipv4.h" #include -#include "igmp.h" #include "net.h" #include "parse_buffer.h" +#include "debug_log.h" namespace ipv4 { -static constexpr ip4_addr IP_BROADCAST_ALL = {255, 255, 255, 255}; +namespace { -uint16_t checksum(const void* data, size_t len) { - auto p = static_cast(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); -} +constexpr ip4_addr IP_BROADCAST_ALL = {255, 255, 255, 255}; -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); -} +std::array addr_filters; +size_t addr_filter_count = 0; struct protocol_entry { uint8_t protocol; protocol_handler fn; }; -static std::array protocol_handlers; -static size_t protocol_handler_count = 0; +std::array protocol_handlers; +size_t protocol_handler_count = 0; + +bool default_addr_filter(const ip4_addr& dst) { + const auto& ns = net::get_state(); + return dst == ns.ip || dst == IP_BROADCAST_ALL || dst == SUBNET_BROADCAST; +} + +} // namespace + +uint16_t checksum(const void* data, size_t len) { + auto p = static_cast(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); +} + +void register_addr_filter(addr_filter fn) { + if (addr_filter_count >= addr_filters.size()) { + dlog("ipv4::register_addr_filter overflow: filter dropped"); + return; + } + addr_filters[addr_filter_count++] = fn; +} + +bool addressed_to_us(ip4_addr dst) { + for (size_t i = 0; i < addr_filter_count; i++) { + if (addr_filters[i](dst)) { + return true; + } + } + return false; +} void register_protocol(uint8_t protocol, protocol_handler fn) { - if (protocol_handler_count < protocol_handlers.size()) - protocol_handlers[protocol_handler_count++] = {protocol, fn}; + if (protocol_handler_count >= protocol_handlers.size()) { + dlogf("ipv4::register_protocol overflow: protocol=%u dropped", protocol); + return; + } + protocol_handlers[protocol_handler_count++] = {protocol, fn}; } void handle(std::span frame, span_writer& tx) { parse_buffer pb(frame); pb.consume(); auto* ip = pb.consume
(); - if (!ip) return; - if ((ip->ver_ihl >> 4) != 4) return; + if (!ip) { + return; + } + if ((ip->ver_ihl >> 4) != 4) { + return; + } size_t options_len = ip->header_len() - sizeof(header); - if (options_len > 0 && !pb.skip(options_len)) return; + if (options_len > 0 && !pb.skip(options_len)) { + return; + } for (size_t i = 0; i < protocol_handler_count; i++) { if (protocol_handlers[i].protocol == ip->protocol) { @@ -56,8 +92,9 @@ void handle(std::span frame, span_writer& tx) { } __attribute__((constructor)) -static void register_ethertype() { - net_register_ethertype(eth::ETH_IPV4, handle); +static void register_self() { + net::register_ethertype(eth::ETH_IPV4, handle); + register_addr_filter(default_addr_filter); } } // namespace ipv4 diff --git a/src/net.cpp b/src/net.cpp index 1a4378c..d47a8cd 100644 --- a/src/net.cpp +++ b/src/net.cpp @@ -3,50 +3,56 @@ #include "pico/unique_id.h" #include "pico/time.h" #include "eth.h" -#include "igmp.h" #include "parse_buffer.h" #include "prepend_buffer.h" #include "w6300.h" #include "debug_log.h" -static net_state state; -static w6300::socket_id raw_socket{0}; -static frame_cb_list frame_callbacks; +namespace net { + +namespace { + +state g_state; +w6300::socket_id raw_socket{0}; +frame_cb_list frame_callbacks; struct ethertype_entry { uint16_t ethertype_be; ethertype_handler fn; }; -static std::array eth_handlers; -static size_t eth_handler_count = 0; +std::array eth_handlers; +size_t eth_handler_count = 0; -void net_register_ethertype(uint16_t ethertype_be, ethertype_handler fn) { - if (eth_handler_count < eth_handlers.size()) - eth_handlers[eth_handler_count++] = {ethertype_be, fn}; +std::array mac_filters; +size_t mac_filter_count = 0; + +bool default_mac_filter(const eth::mac_addr& dst) { + return dst == g_state.mac || dst == eth::MAC_BROADCAST; } -void net_send_raw(std::span data) { - dlog_if_slow("net_send_raw", 1000, [&]{ - auto result = w6300::send(raw_socket, data); - if (!result) - dlogf("w6300 send failed: %zu bytes, err %d", - data.size(), static_cast(result.error())); - }); +bool mac_match(const eth::mac_addr& dst) { + for (size_t i = 0; i < mac_filter_count; i++) { + if (mac_filters[i](dst)) { + return true; + } + } + return false; } -static bool mac_match(const eth::mac_addr& dst) { - return dst == state.mac || dst == eth::MAC_BROADCAST || igmp::is_member_mac(dst); -} - -static void process_frame(std::span frame, span_writer& tx) { - if (frame.size() < sizeof(eth::header)) return; +void process_frame(std::span frame, span_writer& tx) { + if (frame.size() < sizeof(eth::header)) { + return; + } auto& eth_hdr = *reinterpret_cast(frame.data()); - if (!mac_match(eth_hdr.dst)) return; + if (!mac_match(eth_hdr.dst)) { + return; + } frame_callbacks.for_each([&](frame_cb_list::node* n) { - if (n->value(frame)) + if (n->value(frame)) { frame_callbacks.remove(n); + } }); for (size_t i = 0; i < eth_handler_count; i++) { @@ -57,25 +63,60 @@ static void process_frame(std::span frame, span_writer& tx) { } } -bool net_init() { +__attribute__((constructor)) +void register_default_mac_filter() { + register_mac_filter(default_mac_filter); +} + +} // namespace + +void register_ethertype(uint16_t ethertype_be, ethertype_handler fn) { + if (eth_handler_count >= eth_handlers.size()) { + dlogf("net::register_ethertype overflow: ethertype=0x%04x dropped", __builtin_bswap16(ethertype_be)); + return; + } + eth_handlers[eth_handler_count++] = {ethertype_be, fn}; +} + +void register_mac_filter(mac_filter fn) { + if (mac_filter_count >= mac_filters.size()) { + dlog("net::register_mac_filter overflow: filter dropped"); + return; + } + mac_filters[mac_filter_count++] = fn; +} + +void send_raw(std::span data) { + dlog_if_slow("net::send_raw", 1000, [&]{ + auto result = w6300::send(raw_socket, data); + if (!result) { + dlogf("w6300 send failed: %zu bytes, err %d", + data.size(), static_cast(result.error())); + } + }); +} + +bool init() { w6300::init_spi(); w6300::reset(); w6300::init(); - if (!w6300::check()) return false; + if (!w6300::check()) { + return false; + } pico_unique_board_id_t uid; pico_get_unique_board_id(&uid); - state.mac[0] = (uid.id[0] & 0xFC) | 0x02; - state.mac[1] = uid.id[1]; - state.mac[2] = uid.id[2]; - state.mac[3] = uid.id[3]; - state.mac[4] = uid.id[4]; - state.mac[5] = uid.id[5]; + g_state.mac[0] = (uid.id[0] & 0xFC) | 0x02; + g_state.mac[1] = uid.id[1]; + g_state.mac[2] = uid.id[2]; + g_state.mac[3] = uid.id[3]; + g_state.mac[4] = uid.id[4]; + g_state.mac[5] = uid.id[5]; - state.ip[0] = 169; - state.ip[1] = 254; - state.ip[2] = state.mac[4]; - state.ip[3] = state.mac[5]; + g_state.ip[0] = 169; + g_state.ip[1] = 254; + g_state.ip[2] = g_state.mac[4]; + g_state.ip[3] = g_state.mac[5]; w6300::open_socket(raw_socket, w6300::protocol::macraw, w6300::sock_flag::none); w6300::set_interrupt_mask(w6300::ik_sock_0); @@ -83,30 +124,38 @@ bool net_init() { return true; } -const net_state& net_get_state() { - return state; +const state& get_state() { + return g_state; } -frame_cb_handle net_add_frame_callback(net_frame_callback cb) { +frame_cb_handle add_frame_callback(frame_callback cb) { auto h = frame_callbacks.insert(cb); - if (!h) dlog("frame callback alloc failed"); + if (!h) { + dlog("net::add_frame_callback overflow: callback dropped"); + } return h; } -void net_remove_frame_callback(frame_cb_handle h) { +void remove_frame_callback(frame_cb_handle h) { frame_callbacks.remove(h); } -void net_poll(std::span tx) { - if (!w6300::irq_pending) return; +void poll(std::span tx) { + if (!w6300::irq_pending) { + return; + } w6300::irq_pending = false; w6300::clear_interrupt(w6300::ik_int_all); static uint8_t rx_buf[1518]; for (int i = 0; i < 16 && w6300::get_socket_recv_buf(raw_socket) > 0; i++) { auto result = w6300::recv(raw_socket, std::span{rx_buf}); - if (!result) break; + if (!result) { + break; + } span_writer tx_writer(tx); process_frame({rx_buf, *result}, tx_writer); } w6300::rearm_gpio_irq(); } + +} // namespace net diff --git a/src/test_handlers.cpp b/src/test_handlers.cpp index 7fdadee..9c23cb2 100644 --- a/src/test_handlers.cpp +++ b/src/test_handlers.cpp @@ -55,7 +55,7 @@ struct test_state { bool in_flight = false; responder resp; timer_handle timer = nullptr; - frame_cb_handle frame_cb = nullptr; + net::frame_cb_handle frame_cb = nullptr; discovery_data* active_discovery = nullptr; ping_rate_data* active_rate = nullptr; @@ -72,7 +72,7 @@ static test_state ts; static void test_end(const ResponseTest& result) { if (ts.timer) { dispatch_cancel_timer(ts.timer); ts.timer = nullptr; } - if (ts.frame_cb) { net_remove_frame_callback(ts.frame_cb); ts.frame_cb = nullptr; } + if (ts.frame_cb) { net::remove_frame_callback(ts.frame_cb); ts.frame_cb = nullptr; } ts.active_discovery = nullptr; ts.active_rate = nullptr; ts.resp.respond(result); @@ -93,7 +93,7 @@ static bool discover_reply_cb(std::span frame) { if (options_len > 0 && !pb.skip(options_len)) return false; auto* uhdr = pb.consume(); if (!uhdr || uhdr->src_port != PICOMAP_PORT_BE) return false; - if (ip->src == net_get_state().ip) return false; + if (ip->src == net::get_state().ip) return false; dispatch_cancel_timer(ts.timer); ts.timer = nullptr; ts.frame_cb = nullptr; @@ -105,7 +105,7 @@ static bool discover_reply_cb(std::span frame) { } static void discover_timeout_cb() { - net_remove_frame_callback(ts.frame_cb); + net::remove_frame_callback(ts.frame_cb); ts.frame_cb = nullptr; ts.timer = nullptr; auto cont = ts.active_discovery ? ts.active_discovery->on_timeout : nullptr; @@ -119,7 +119,7 @@ static void discover_peer(discovery_data& d, d.on_timeout = timeout; ts.active_discovery = &d; - const auto& ns = net_get_state(); + const auto& ns = net::get_state(); eth::mac_addr mcast_mac = igmp::mac_for_ip(PICOMAP_DISCOVERY_GROUP); prepend_buffer<4096> buf; @@ -137,10 +137,10 @@ static void discover_peer(discovery_data& d, 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); + ts.frame_cb = net::add_frame_callback(discover_reply_cb); ts.timer = dispatch_schedule_ms(5000, discover_timeout_cb); - net_send_raw(buf.span()); + net::send_raw(buf.span()); } static bool igmp_report_cb(std::span frame) { @@ -158,14 +158,14 @@ static void igmp_timeout_cb() { } static void test_discovery_igmp() { - const auto& ns = net_get_state(); + const auto& ns = net::get_state(); prepend_buffer<4096> buf; igmp::prepend_query(buf, ns.mac, ns.ip, PICOMAP_DISCOVERY_GROUP); - ts.frame_cb = net_add_frame_callback(igmp_report_cb); + ts.frame_cb = net::add_frame_callback(igmp_report_cb); ts.timer = dispatch_schedule_ms(5000, igmp_timeout_cb); - net_send_raw(buf.span()); + net::send_raw(buf.span()); } static void info_found(const peer_info& peer) { @@ -184,7 +184,7 @@ static bool ping_reply_cb(std::span frame) { ipv4::ip4_addr src_ip; if (!icmp::parse_echo_reply(frame, src_ip, PING_ECHO_ID)) return false; ts.frame_cb = nullptr; - if (src_ip == net_get_state().ip) + if (src_ip == net::get_state().ip) test_end({false, {"got reply from self: " + ipv4::to_string(src_ip)}}); else test_end({true, {"reply from " + ipv4::to_string(src_ip)}}); @@ -197,13 +197,13 @@ static void ping_timeout_cb() { } static void start_ping(ipv4::ip4_addr dst_ip) { - const auto& ns = net_get_state(); + const auto& ns = net::get_state(); prepend_buffer<4096> buf; icmp::prepend_echo_request(buf, ns.mac, ns.ip, eth::MAC_BROADCAST, dst_ip, PING_ECHO_ID, 1); - ts.frame_cb = net_add_frame_callback(ping_reply_cb); + ts.frame_cb = net::add_frame_callback(ping_reply_cb); ts.timer = dispatch_schedule_ms(5000, ping_timeout_cb); - net_send_raw(buf.span()); + net::send_raw(buf.span()); } static void test_ping_subnet() { start_ping({169, 254, 255, 255}); } @@ -215,7 +215,7 @@ static size_t ping_rate_frame_size() { } static void ping_rate_send_one() { - const auto& ns = net_get_state(); + const auto& ns = net::get_state(); auto& r = *ts.active_rate; prepend_buffer<4096> buf; if (r.payload_len > 0) @@ -223,14 +223,14 @@ static void ping_rate_send_one() { icmp::prepend_echo_request(buf, ns.mac, ns.ip, r.peer.mac, r.peer.ip, PING_RATE_ECHO_ID, r.sent + 1, r.payload_len); - net_send_raw(buf.span()); + net::send_raw(buf.span()); r.sent++; } static bool ping_rate_reply_cb(std::span frame) { ipv4::ip4_addr src_ip; if (!icmp::parse_echo_reply(frame, src_ip, PING_RATE_ECHO_ID)) return false; - if (src_ip == net_get_state().ip) return false; + if (src_ip == net::get_state().ip) return false; auto& r = *ts.active_rate; r.received++; @@ -276,7 +276,7 @@ static void ping_rate_found(const peer_info& peer) { r.received = 0; r.start_us = time_us_32(); - ts.frame_cb = net_add_frame_callback(ping_rate_reply_cb); + ts.frame_cb = net::add_frame_callback(ping_rate_reply_cb); ts.timer = dispatch_schedule_ms(10000, ping_rate_timeout_cb); for (uint16_t i = 0; i < r.pipeline && r.sent < r.target; i++) diff --git a/src/udp.cpp b/src/udp.cpp index 52af99d..d5c7542 100644 --- a/src/udp.cpp +++ b/src/udp.cpp @@ -4,6 +4,7 @@ #include "ipv4.h" #include "net.h" #include "parse_buffer.h" +#include "debug_log.h" namespace udp { @@ -15,8 +16,11 @@ 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}; + if (port_handler_count >= port_handlers.size()) { + dlogf("udp::register_port overflow: port=%u dropped", __builtin_bswap16(port_be)); + return; + } + port_handlers[port_handler_count++] = {port_be, fn}; } void handle(std::span frame, span_writer& tx) {