From 32044a5cbd13225d591eee8657f62783359d22fe Mon Sep 17 00:00:00 2001 From: Ian Gulliver Date: Sun, 19 Apr 2026 00:32:13 -0700 Subject: [PATCH] Eliminate std::function: fn-pointer callbacks, per-test test_state structs, udp.cpp with link-time udp::client::handler, udp::address --- firmware/CMakeLists.txt | 1 + firmware/include/debug_log.h | 4 +- firmware/include/dispatch.h | 23 +- firmware/include/net.h | 16 +- firmware/include/timer_queue.h | 13 +- firmware/include/udp.h | 19 ++ firmware/lib/dispatch.cpp | 40 ++- firmware/lib/ipv4.cpp | 4 +- firmware/lib/net.cpp | 44 +--- firmware/lib/test_handlers.cpp | 468 ++++++++++++++++++++------------- firmware/lib/udp.cpp | 31 +++ 11 files changed, 378 insertions(+), 285 deletions(-) create mode 100644 firmware/lib/udp.cpp diff --git a/firmware/CMakeLists.txt b/firmware/CMakeLists.txt index 6ed2e0f..cfb4c4c 100644 --- a/firmware/CMakeLists.txt +++ b/firmware/CMakeLists.txt @@ -18,6 +18,7 @@ set(LIB_SOURCES lib/igmp.cpp lib/ipv4.cpp lib/net.cpp + lib/udp.cpp w6300/w6300.cpp ) diff --git a/firmware/include/debug_log.h b/firmware/include/debug_log.h index f0da1d6..ce241a9 100644 --- a/firmware/include/debug_log.h +++ b/firmware/include/debug_log.h @@ -1,7 +1,6 @@ #pragma once #include #include -#include #include #include #include @@ -29,7 +28,8 @@ inline void dlogf(const char* fmt, ...) { dlog(buf); } -inline void dlog_if_slow(std::string_view label, uint32_t threshold_us, std::function fn) { +template +inline void dlog_if_slow(std::string_view label, uint32_t threshold_us, F&& fn) { uint32_t t0 = time_us_32(); fn(); uint32_t elapsed = time_us_32() - t0; diff --git a/firmware/include/dispatch.h b/firmware/include/dispatch.h index dd5fc67..fe0686c 100644 --- a/firmware/include/dispatch.h +++ b/firmware/include/dispatch.h @@ -1,26 +1,33 @@ #pragma once #include #include -#include #include #include #include "wire.h" #include "timer_queue.h" #include "net.h" +#include "prepend_buffer.h" +#include "udp.h" struct responder { uint32_t message_id; - send_fn send; + udp::address reply_to; template void respond(const T& msg) const { - send([&](span_writer& out) { - return encode_response_into(out, message_id, msg); - }); + 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; + buf.append(*r); + udp::prepend(buf, reply_to.mac, ns.mac, ns.ip, reply_to.ip, + PICOMAP_PORT_BE, reply_to.port, *r); + net_send_raw(buf.span()); } }; -using handler_fn = void (*)(responder resp, std::span payload); +using handler_fn = void (*)(const responder& resp, std::span payload); struct handler_entry { int8_t type_id; @@ -28,7 +35,7 @@ struct handler_entry { }; template -void typed_handler(responder resp, std::span payload) { +void typed_handler(const responder& resp, std::span payload) { msgpack::parser p(payload.data(), static_cast(payload.size())); Req req; auto tup = req.as_tuple(); @@ -46,6 +53,6 @@ void typed_handler(responder resp, std::span payload) { } void dispatch_init(); -timer_handle dispatch_schedule_ms(uint32_t ms, std::function fn); +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/net.h b/firmware/include/net.h index 0b485f6..a369368 100644 --- a/firmware/include/net.h +++ b/firmware/include/net.h @@ -1,11 +1,9 @@ #pragma once -#include -#include +#include #include #include "eth.h" #include "ipv4.h" #include "span_writer.h" -#include "msgpack.h" #include "callback_list.h" struct net_state { @@ -13,22 +11,16 @@ struct net_state { ipv4::ip4_addr ip; }; -using encode_fn = std::function(span_writer&)>; -using send_fn = std::function; - -using net_handler = std::function payload, - send_fn send)>; - -using net_frame_callback = std::function frame)>; +using net_frame_callback = bool (*)(std::span frame); using frame_cb_list = callback_list; using frame_cb_handle = frame_cb_list::node*; +inline constexpr uint16_t PICOMAP_PORT_BE = __builtin_bswap16(28781); + bool net_init(); const net_state& net_get_state(); -void net_set_handler(net_handler handler); 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_handle_udp(std::span frame, span_writer& tx); diff --git a/firmware/include/timer_queue.h b/firmware/include/timer_queue.h index 7c22699..c8c304d 100644 --- a/firmware/include/timer_queue.h +++ b/firmware/include/timer_queue.h @@ -1,11 +1,10 @@ #pragma once -#include #include "pico/time.h" #include "callback_list.h" struct timer_entry { absolute_time_t when; - std::function fn; + void (*fn)() = nullptr; }; using timer_handle = callback_list::node*; @@ -15,8 +14,8 @@ struct timer_queue { alarm_id_t alarm = -1; volatile bool irq_pending = false; - timer_handle schedule(absolute_time_t when, std::function fn) { - auto* n = list.insert_sorted({when, std::move(fn)}, + timer_handle schedule(absolute_time_t when, void (*fn)()) { + auto* n = list.insert_sorted({when, fn}, [](const timer_entry& a, const timer_entry& b) { return absolute_time_diff_us(b.when, a.when) < 0; }); @@ -24,8 +23,8 @@ struct timer_queue { return n; } - timer_handle schedule_ms(uint32_t ms, std::function fn) { - return schedule(make_timeout_time_ms(ms), std::move(fn)); + timer_handle schedule_ms(uint32_t ms, void (*fn)()) { + return schedule(make_timeout_time_ms(ms), fn); } bool cancel(timer_handle h) { @@ -40,7 +39,7 @@ struct timer_queue { irq_pending = false; while (auto* n = list.front()) { if (absolute_time_diff_us(get_absolute_time(), n->value.when) > 0) break; - auto fn = std::move(n->value.fn); + auto fn = n->value.fn; list.remove(n); fn(); } diff --git a/firmware/include/udp.h b/firmware/include/udp.h index d5b87bd..c496835 100644 --- a/firmware/include/udp.h +++ b/firmware/include/udp.h @@ -1,7 +1,10 @@ #pragma once #include +#include #include "eth.h" #include "ipv4.h" +#include "net.h" +#include "span_writer.h" namespace udp { @@ -13,6 +16,14 @@ struct __attribute__((packed)) header { }; static_assert(sizeof(header) == 8); +struct address { + // mac is carried here until we grow an ARP cache; once we do, a + // destination mac comes from resolving ip and this field goes away. + eth::mac_addr mac; + ipv4::ip4_addr ip; + uint16_t port; +}; + template void prepend(Buf& buf, const eth::mac_addr& dst_mac, const eth::mac_addr& src_mac, ipv4::ip4_addr src_ip, ipv4::ip4_addr dst_ip, @@ -26,4 +37,12 @@ void prepend(Buf& buf, const eth::mac_addr& dst_mac, const eth::mac_addr& src_ma ipv4::prepend(buf, dst_mac, src_mac, src_ip, dst_ip, 17, sizeof(header) + payload_len, ttl); } +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 + } // namespace udp diff --git a/firmware/lib/dispatch.cpp b/firmware/lib/dispatch.cpp index e141fe5..75fffe1 100644 --- a/firmware/lib/dispatch.cpp +++ b/firmware/lib/dispatch.cpp @@ -1,13 +1,16 @@ #include "dispatch.h" +#include #include "pico/stdlib.h" #include "wire.h" #include "timer_queue.h" #include "net.h" #include "igmp.h" +#include "udp.h" #include "debug_log.h" #include "hardware/sync.h" static timer_queue timers; +static std::array handler_map{}; static void igmp_reannounce() { auto& ns = net_get_state(); @@ -21,8 +24,8 @@ void dispatch_init() { dlog("dispatch_init complete"); } -timer_handle dispatch_schedule_ms(uint32_t ms, std::function fn) { - auto h = timers.schedule_ms(ms, std::move(fn)); +timer_handle dispatch_schedule_ms(uint32_t ms, void (*fn)()) { + auto h = timers.schedule_ms(ms, fn); if (!h) dlogf("timer alloc failed: %lu ms", static_cast(ms)); return h; } @@ -31,30 +34,23 @@ bool dispatch_cancel_timer(timer_handle h) { return timers.cancel(h); } -[[noreturn]] void dispatch_run(std::span handlers) { - std::array handler_map{}; - for (auto& entry : handlers) { - handler_map[entry.type_id] = entry.handle; +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; static std::array tx_buf; - auto dispatch_msg = [&](const DecodedMessage& msg, send_fn send) { - 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, std::move(send)}; - handler_map[msg.type_id](resp, msg.payload); - }; - - net_set_handler([&](std::span payload, - send_fn send) { - auto msg = try_decode(payload.data(), payload.size()); - if (!msg) return; - dispatch_msg(*msg, std::move(send)); - }); - while (true) { uint32_t save = save_and_disable_interrupts(); diff --git a/firmware/lib/ipv4.cpp b/firmware/lib/ipv4.cpp index 93efd2a..e0fc52f 100644 --- a/firmware/lib/ipv4.cpp +++ b/firmware/lib/ipv4.cpp @@ -1,7 +1,7 @@ #include "ipv4.h" #include "icmp.h" #include "igmp.h" -#include "net.h" +#include "udp.h" #include "parse_buffer.h" namespace ipv4 { @@ -47,7 +47,7 @@ void handle(std::span frame, span_writer& tx, case 17: if (!ip_match(ip->dst, our_ip, subnet_broadcast)) return; - net_handle_udp(frame, tx); + udp::handle(frame, tx); break; } } diff --git a/firmware/lib/net.cpp b/firmware/lib/net.cpp index f4884e6..9d65609 100644 --- a/firmware/lib/net.cpp +++ b/firmware/lib/net.cpp @@ -12,11 +12,9 @@ #include "debug_log.h" static constexpr ipv4::ip4_addr IP_BROADCAST_SUBNET = {169, 254, 255, 255}; -static constexpr uint16_t PICOMAP_PORT = __builtin_bswap16(28781); static net_state state; static w6300::socket_id raw_socket{0}; -static net_handler msg_handler; static frame_cb_list frame_callbacks; void net_send_raw(std::span data) { @@ -28,42 +26,6 @@ void net_send_raw(std::span data) { }); } -void net_handle_udp(std::span frame, span_writer& tx) { - parse_buffer pb(frame); - auto* eth_hdr = pb.consume(); - auto* ip = pb.consume(); - if (!ip) 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) return; - if (!msg_handler) return; - - size_t udp_len = __builtin_bswap16(uhdr->length); - if (udp_len < sizeof(udp::header)) return; - size_t payload_len = udp_len - sizeof(udp::header); - if (pb.remaining_size() < payload_len) return; - - eth::mac_addr dst_mac = eth_hdr->src; - ipv4::ip4_addr dst_ip = ip->src; - uint16_t dst_port = uhdr->src_port; - - msg_handler(pb.remaining().subspan(0, payload_len), - [dst_mac, dst_ip, dst_port](encode_fn encode) { - prepend_buffer<4096> buf; - span_writer out(buf.payload_ptr(), 2048); - auto r = encode(out); - if (!r) return; - buf.append(*r); - udp::prepend(buf, dst_mac, state.mac, state.ip, dst_ip, - PICOMAP_PORT, dst_port, *r); - net_send_raw(buf.span()); - }); -} - static bool mac_match(const eth::mac_addr& dst) { return dst == state.mac || dst == eth::MAC_BROADCAST || igmp::is_member_mac(dst); } @@ -121,12 +83,8 @@ const net_state& net_get_state() { return state; } -void net_set_handler(net_handler handler) { - msg_handler = std::move(handler); -} - frame_cb_handle net_add_frame_callback(net_frame_callback cb) { - auto h = frame_callbacks.insert(std::move(cb)); + auto h = frame_callbacks.insert(cb); if (!h) dlog("frame callback alloc failed"); return h; } diff --git a/firmware/lib/test_handlers.cpp b/firmware/lib/test_handlers.cpp index 0e38ffb..f732668 100644 --- a/firmware/lib/test_handlers.cpp +++ b/firmware/lib/test_handlers.cpp @@ -1,6 +1,5 @@ #include "test_handlers.h" #include -#include #include #include "pico/stdlib.h" #include "pico/time.h" @@ -11,18 +10,134 @@ #include "parse_buffer.h" #include "prepend_buffer.h" -static constexpr uint16_t PICOMAP_PORT = __builtin_bswap16(28781); +// Echo IDs are just tags used to match our ping replies; constants suffice +// since tests are one-at-a-time. +static constexpr uint16_t PING_ECHO_ID = 0x1234; +static constexpr uint16_t PING_RATE_ECHO_ID = 0x5678; struct peer_info { eth::mac_addr mac; ipv4::ip4_addr ip; }; -using peer_callback = std::function; -using fail_callback = std::function; +// Shared sub-struct types. Used as fields inside per-test-command structs +// when two tests happen to carry the same shape of data. -static void discover_peer(peer_callback on_found, fail_callback on_timeout) { - auto& ns = net_get_state(); +// State for the discover_peer primitive. +struct discovery_data { + void (*on_found)(const peer_info&) = nullptr; + void (*on_timeout)() = nullptr; +}; + +// Rolling state for a rate-testing ping run. +struct ping_rate_data { + peer_info peer; + uint16_t target; + uint16_t pipeline; + uint16_t payload_len; + uint16_t sent; + uint16_t received; + uint32_t start_us; +}; + +// One struct per test command. Empty structs (where a test command has no +// extra state beyond the common fields) are kept for symmetry/documentation. +struct discovery_igmp_test {}; +struct discovery_info_test { + discovery_data discovery; +}; +struct ping_subnet_test {}; +struct ping_global_test {}; +struct packet_rate_test { + discovery_data discovery; + ping_rate_data rate; +}; +struct byte_rate_test { + discovery_data discovery; + ping_rate_data rate; +}; + +// All test state lives in this single instance. The protocol is one-test-at- +// a-time: handle_test rejects a new request while in_flight is set. Only the +// fields for the running test are populated; the rest are wasted bytes traded +// for clarity. +struct test_state { + // Common to every async test. + bool in_flight = false; + responder resp; + timer_handle timer = nullptr; + frame_cb_handle frame_cb = nullptr; + + // Views into the running test's shared sub-state so the shared primitive + // callbacks (discover_peer, ping_rate) know where to find it. + discovery_data* active_discovery = nullptr; + ping_rate_data* active_rate = nullptr; + + // Per-test-command storage. + discovery_igmp_test discovery_igmp; + discovery_info_test discovery_info; + ping_subnet_test ping_subnet; + ping_global_test ping_global; + packet_rate_test packet_rate; + byte_rate_test byte_rate; +}; + +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; } + ts.active_discovery = nullptr; + ts.active_rate = nullptr; + ts.resp.respond(result); + ts.in_flight = false; +} + +// ----- discover_peer (shared building block) ----- + +// Note on frame/timer handle lifetimes: when a callback fires, the dispatcher +// (net or timer_queue) has already taken ownership of the node. Callbacks that +// self-consume (frame returning true, or any timer fire) must null the +// matching `ts.` handle before calling test_end, so test_end doesn't try to +// cancel a stale handle. + +static bool discover_reply_cb(std::span frame) { + parse_buffer pb(frame); + auto* eth_hdr = pb.consume(); + if (!eth_hdr || eth_hdr->ethertype != eth::ETH_IPV4) return false; + auto* ip = pb.consume(); + if (!ip || ip->protocol != 17) return false; + size_t options_len = ip->header_len() - sizeof(ipv4::header); + 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; + dispatch_cancel_timer(ts.timer); + ts.timer = nullptr; + ts.frame_cb = nullptr; // self-consumed via `return true` below + auto cont = ts.active_discovery ? ts.active_discovery->on_found : nullptr; + ts.active_discovery = nullptr; + peer_info peer{eth_hdr->src, ip->src}; + if (cont) cont(peer); + return true; +} + +static void discover_timeout_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; + ts.active_discovery = nullptr; + if (cont) cont(); +} + +static void discover_peer(discovery_data& d, + void (*found)(const peer_info&), void (*timeout)()) { + d.on_found = found; + d.on_timeout = timeout; + ts.active_discovery = &d; + + const auto& ns = net_get_state(); eth::mac_addr mcast_mac = igmp::mac_for_ip(igmp::PICOMAP_DISCOVERY_GROUP); prepend_buffer<4096> buf; @@ -31,223 +146,193 @@ static void discover_peer(peer_callback on_found, fail_callback on_timeout) { RequestInfo req_msg; auto encoded = encode_response_into(out, 0xFFFF, req_msg); if (!encoded) { - on_timeout(); + ts.active_discovery = nullptr; + timeout(); return; } buf.append(*encoded); udp::prepend(buf, mcast_mac, ns.mac, ns.ip, igmp::PICOMAP_DISCOVERY_GROUP, - PICOMAP_PORT, PICOMAP_PORT, *encoded, 1); + PICOMAP_PORT_BE, PICOMAP_PORT_BE, *encoded, 1); - ipv4::ip4_addr our_ip = ns.ip; - - auto timer = std::make_shared(nullptr); - auto cb = std::make_shared(nullptr); - *cb = net_add_frame_callback([our_ip, timer, on_found = std::move(on_found)](std::span frame) -> bool { - parse_buffer pb(frame); - auto* eth_hdr = pb.consume(); - if (!eth_hdr || eth_hdr->ethertype != eth::ETH_IPV4) return false; - auto* ip = pb.consume(); - if (!ip || ip->protocol != 17) return false; - size_t options_len = ip->header_len() - sizeof(ipv4::header); - if (options_len > 0 && !pb.skip(options_len)) return false; - auto* uhdr = pb.consume(); - if (!uhdr || uhdr->src_port != PICOMAP_PORT) return false; - if (ip->src == our_ip) return false; - dispatch_cancel_timer(*timer); - on_found({eth_hdr->src, ip->src}); - return true; - }); - - *timer = dispatch_schedule_ms(5000, [cb, on_timeout = std::move(on_timeout)]() { - net_remove_frame_callback(*cb); - on_timeout(); - }); + ts.frame_cb = net_add_frame_callback(discover_reply_cb); + ts.timer = dispatch_schedule_ms(5000, discover_timeout_cb); net_send_raw(buf.span()); } -static void test_discovery_igmp(const responder& resp) { - auto& ns = net_get_state(); +// ----- discovery_igmp ----- +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; + ts.frame_cb = nullptr; // self-consumed via `return true` + test_end({true, {"got IGMP report for " + ipv4::to_string(group)}}); + return true; +} + +static void igmp_timeout_cb() { + ts.timer = nullptr; // already fired + test_end({false, {"no IGMP report within 5s"}}); +} + +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); - auto timer = std::make_shared(nullptr); - auto cb = std::make_shared(nullptr); - *cb = net_add_frame_callback([resp, timer](std::span frame) -> bool { - ipv4::ip4_addr group; - if (!igmp::parse_report(frame, group)) return false; - if (group != igmp::PICOMAP_DISCOVERY_GROUP) return false; - dispatch_cancel_timer(*timer); - resp.respond(ResponseTest{true, {"got IGMP report for " + ipv4::to_string(group)}}); - return true; - }); - - *timer = dispatch_schedule_ms(5000, [cb, resp]() { - net_remove_frame_callback(*cb); - resp.respond(ResponseTest{false, {"no IGMP report within 5s"}}); - }); + ts.frame_cb = net_add_frame_callback(igmp_report_cb); + ts.timer = dispatch_schedule_ms(5000, igmp_timeout_cb); net_send_raw(buf.span()); } -static void test_discovery_info(const responder& resp) { - discover_peer( - [resp](const peer_info& peer) { - resp.respond(ResponseTest{true, {"got info response from " + ipv4::to_string(peer.ip)}}); - }, - [resp]() { - resp.respond(ResponseTest{false, {"no info response within 5s"}}); - }); +// ----- discovery_info ----- + +static void info_found(const peer_info& peer) { + test_end({true, {"got info response from " + ipv4::to_string(peer.ip)}}); } -static void test_ping(const responder& resp, ipv4::ip4_addr dst_ip) { - auto& ns = net_get_state(); - uint16_t ping_id = 0x1234; +static void info_timeout() { + test_end({false, {"no info response within 5s"}}); +} +static void test_discovery_info() { + discover_peer(ts.discovery_info.discovery, info_found, info_timeout); +} + +// ----- ping_subnet / ping_global ----- + +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; // self-consumed via `return true` + 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)}}); + return true; +} + +static void ping_timeout_cb() { + ts.timer = nullptr; // already fired + test_end({false, {"no reply from non-self host within 5s"}}); +} + +static void start_ping(ipv4::ip4_addr dst_ip) { + 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_id, 1); - - ipv4::ip4_addr our_ip = ns.ip; - - auto timer = std::make_shared(nullptr); - auto cb = std::make_shared(nullptr); - *cb = net_add_frame_callback([resp, ping_id, our_ip, timer](std::span frame) -> bool { - ipv4::ip4_addr src_ip; - if (!icmp::parse_echo_reply(frame, src_ip, ping_id)) return false; - dispatch_cancel_timer(*timer); - if (src_ip == our_ip) { - resp.respond(ResponseTest{false, {"got reply from self: " + ipv4::to_string(src_ip)}}); - return true; - } - resp.respond(ResponseTest{true, {"reply from " + ipv4::to_string(src_ip)}}); - return true; - }); - - *timer = dispatch_schedule_ms(5000, [cb, resp]() { - net_remove_frame_callback(*cb); - resp.respond(ResponseTest{false, {"no reply from non-self host within 5s"}}); - }); - + eth::MAC_BROADCAST, dst_ip, PING_ECHO_ID, 1); + ts.frame_cb = net_add_frame_callback(ping_reply_cb); + ts.timer = dispatch_schedule_ms(5000, ping_timeout_cb); net_send_raw(buf.span()); } -static void test_ping_subnet(const responder& resp) { - test_ping(resp, {169, 254, 255, 255}); +static void test_ping_subnet() { start_ping({169, 254, 255, 255}); } +static void test_ping_global() { start_ping({255, 255, 255, 255}); } + +// ----- packet_rate / byte_rate ----- + +static size_t ping_rate_frame_size() { + return sizeof(eth::header) + sizeof(ipv4::header) + sizeof(icmp::echo) + + ts.active_rate->payload_len; } -static void test_ping_global(const responder& resp) { - test_ping(resp, {255, 255, 255, 255}); -} - -struct ping_rate_state { - responder resp; - timer_handle timer = nullptr; - frame_cb_handle cb_handle = nullptr; - uint16_t ping_id; - uint16_t sent = 0; - uint16_t received = 0; - uint16_t target; - uint16_t pipeline; - uint16_t payload_len; - uint32_t start_us; - peer_info peer; - ipv4::ip4_addr our_ip; - eth::mac_addr our_mac; - - size_t frame_size() const { - return sizeof(eth::header) + sizeof(ipv4::header) + sizeof(icmp::echo) + payload_len; - } -}; - -static void ping_rate_send_one(std::shared_ptr st) { +static void ping_rate_send_one() { + const auto& ns = net_get_state(); + auto& r = *ts.active_rate; prepend_buffer<4096> buf; - if (st->payload_len > 0) - memset(buf.append(st->payload_len), 0xAA, st->payload_len); - icmp::prepend_echo_request(buf, st->our_mac, st->our_ip, - st->peer.mac, st->peer.ip, st->ping_id, - st->sent + 1, st->payload_len); + if (r.payload_len > 0) + memset(buf.append(r.payload_len), 0xAA, r.payload_len); + 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()); - st->sent++; + r.sent++; } -static void start_ping_rate(const responder& resp, uint16_t target, - uint16_t payload_len, uint16_t pipeline) { - auto& ns = net_get_state(); +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; - discover_peer( - [resp, ns, target, payload_len, pipeline](const peer_info& peer) { - auto st = std::make_shared(); - st->resp = resp; - st->ping_id = 0x5678; - st->target = target; - st->pipeline = pipeline; - st->payload_len = payload_len; - st->our_mac = ns.mac; - st->our_ip = ns.ip; - st->peer = peer; - st->start_us = time_us_32(); + auto& r = *ts.active_rate; + r.received++; + if (r.received >= r.target) { + uint32_t elapsed_us = time_us_32() - r.start_us; + uint32_t elapsed_ms = elapsed_us / 1000; + uint32_t pps = static_cast( + static_cast(r.received) * 1000000 / elapsed_us); + uint64_t total_bytes = static_cast(r.received) * 2 * ping_rate_frame_size(); + uint32_t kbps = static_cast(total_bytes * 1000 / elapsed_us); + char msg[128]; + snprintf(msg, sizeof(msg), + "%u rt in %lu ms, %lu pps, %lu bytes, %lu KB/s", + r.received, static_cast(elapsed_ms), + static_cast(pps), + static_cast(total_bytes), + static_cast(kbps)); + ts.frame_cb = nullptr; // self-consumed via `return true` + test_end({true, {msg}}); + return true; + } - st->cb_handle = net_add_frame_callback([st](std::span frame) -> bool { - ipv4::ip4_addr src_ip; - if (!icmp::parse_echo_reply(frame, src_ip, st->ping_id)) return false; - if (src_ip == st->our_ip) return false; - - st->received++; - if (st->received >= st->target) { - dispatch_cancel_timer(st->timer); - uint32_t elapsed_us = time_us_32() - st->start_us; - uint32_t elapsed_ms = elapsed_us / 1000; - uint32_t pps = static_cast( - static_cast(st->received) * 1000000 / elapsed_us); - uint64_t total_bytes = static_cast(st->received) * 2 * st->frame_size(); - uint32_t kbps = static_cast(total_bytes * 1000 / elapsed_us); - char msg[128]; - snprintf(msg, sizeof(msg), - "%u rt in %lu ms, %lu pps, %lu bytes, %lu KB/s", - st->received, static_cast(elapsed_ms), - static_cast(pps), - static_cast(total_bytes), - static_cast(kbps)); - st->resp.respond(ResponseTest{true, {msg}}); - return true; - } - - if (st->sent < st->target) - ping_rate_send_one(st); - return false; - }); - - st->timer = dispatch_schedule_ms(10000, [st]() { - net_remove_frame_callback(st->cb_handle); - uint32_t elapsed_us = time_us_32() - st->start_us; - char msg[64]; - snprintf(msg, sizeof(msg), "timeout after %u/%u rt in %lu ms", - st->received, st->sent, - static_cast(elapsed_us / 1000)); - st->resp.respond(ResponseTest{false, {msg}}); - }); - - for (uint16_t i = 0; i < st->pipeline && st->sent < st->target; i++) - ping_rate_send_one(st); - }, - [resp]() { - resp.respond(ResponseTest{false, {"no peer found"}}); - }); + if (r.sent < r.target) + ping_rate_send_one(); + return false; } -static void test_packet_rate(const responder& resp) { - start_ping_rate(resp, 8192, 0, 8); +static void ping_rate_timeout_cb() { + ts.timer = nullptr; // already fired + auto& r = *ts.active_rate; + uint32_t elapsed_us = time_us_32() - r.start_us; + char msg[64]; + snprintf(msg, sizeof(msg), "timeout after %u/%u rt in %lu ms", + r.received, r.sent, + static_cast(elapsed_us / 1000)); + test_end({false, {msg}}); } -static void test_byte_rate(const responder& resp) { - start_ping_rate(resp, 2048, 1400, 8); +static void ping_rate_found(const peer_info& peer) { + auto& r = *ts.active_rate; + r.peer = peer; + r.sent = 0; + r.received = 0; + r.start_us = time_us_32(); + + 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++) + ping_rate_send_one(); } -using sync_test_fn = ResponseTest (*)(const responder&); -using async_test_fn = void (*)(const responder&); +static void ping_rate_no_peer() { + test_end({false, {"no peer found"}}); +} + +static void start_ping_rate(discovery_data& d, ping_rate_data& r, + uint16_t target, uint16_t payload_len, uint16_t pipeline) { + r.target = target; + r.payload_len = payload_len; + r.pipeline = pipeline; + ts.active_rate = &r; + discover_peer(d, ping_rate_found, ping_rate_no_peer); +} + +static void test_packet_rate() { + start_ping_rate(ts.packet_rate.discovery, ts.packet_rate.rate, 8192, 0, 8); +} +static void test_byte_rate() { + start_ping_rate(ts.byte_rate.discovery, ts.byte_rate.rate, 2048, 1400, 8); +} + +// ----- registry ----- + +using sync_test_fn = ResponseTest (*)(); +using async_test_fn = void (*)(); struct test_entry { sync_test_fn sync; @@ -257,10 +342,10 @@ struct test_entry { static const std::unordered_map tests = { {"discovery_igmp", {nullptr, test_discovery_igmp}}, {"discovery_info", {nullptr, test_discovery_info}}, - {"ping_subnet", {nullptr, test_ping_subnet}}, - {"ping_global", {nullptr, test_ping_global}}, - {"packet_rate", {nullptr, test_packet_rate}}, - {"byte_rate", {nullptr, test_byte_rate}}, + {"ping_subnet", {nullptr, test_ping_subnet}}, + {"ping_global", {nullptr, test_ping_global}}, + {"packet_rate", {nullptr, test_packet_rate}}, + {"byte_rate", {nullptr, test_byte_rate}}, }; std::optional handle_list_tests(const responder&, const RequestListTests&) { @@ -271,11 +356,16 @@ std::optional handle_list_tests(const responder&, const Reque } std::optional handle_test(const responder& resp, const RequestTest& req) { + if (ts.in_flight) + return ResponseTest{false, {"test already running"}}; auto it = tests.find(req.name); if (it == tests.end()) return ResponseTest{false, {"unknown test: " + req.name}}; if (it->second.sync) - return it->second.sync(resp); - it->second.async(resp); + return it->second.sync(); + + ts.in_flight = true; + ts.resp = resp; + it->second.async(); return std::nullopt; } diff --git a/firmware/lib/udp.cpp b/firmware/lib/udp.cpp new file mode 100644 index 0000000..28f809c --- /dev/null +++ b/firmware/lib/udp.cpp @@ -0,0 +1,31 @@ +#include "udp.h" +#include "eth.h" +#include "ipv4.h" +#include "net.h" +#include "parse_buffer.h" + +namespace udp { + +void handle(std::span frame, span_writer& tx) { + parse_buffer pb(frame); + auto* eth_hdr = pb.consume(); + auto* ip = pb.consume(); + if (!ip) 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); +} + +} // namespace udp