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

Eliminate std::function: fn-pointer callbacks, per-test test_state structs, udp.cpp with link-time udp::client::handler, udp::address

Browse Source
This commit is contained in:
Ian Gulliver
2026-04-19 00:32:13 -07:00
parent 40f7fb5941
commit 32044a5cbd
11 changed files with 378 additions and 285 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
firmware/CMakeLists.txt
View File

@@ -18,6 +18,7 @@ set(LIB_SOURCES
lib/igmp.cpp lib/igmp.cpp
lib/ipv4.cpp lib/ipv4.cpp
lib/net.cpp lib/net.cpp
lib/udp.cpp
w6300/w6300.cpp w6300/w6300.cpp
) )

4
firmware/include/debug_log.h
View File

@@ -1,7 +1,6 @@
#pragma once #pragma once
#include <cstdarg> #include <cstdarg>
#include <cstdio> #include <cstdio>
#include <functional>
#include <string> #include <string>
#include <string_view> #include <string_view>
#include <vector> #include <vector>
@@ -29,7 +28,8 @@ inline void dlogf(const char* fmt, ...) {
dlog(buf); dlog(buf);
} }
inline void dlog_if_slow(std::string_view label, uint32_t threshold_us, std::function<void()> fn) { template <typename F>
inline void dlog_if_slow(std::string_view label, uint32_t threshold_us, F&& fn) {
uint32_t t0 = time_us_32(); uint32_t t0 = time_us_32();
fn(); fn();
uint32_t elapsed = time_us_32() - t0; uint32_t elapsed = time_us_32() - t0;

23
firmware/include/dispatch.h
View File

@@ -1,26 +1,33 @@
#pragma once #pragma once
#include <cstdint> #include <cstdint>
#include <cstdio> #include <cstdio>
#include <functional>
#include <optional> #include <optional>
#include <span> #include <span>
#include "wire.h" #include "wire.h"
#include "timer_queue.h" #include "timer_queue.h"
#include "net.h" #include "net.h"
#include "prepend_buffer.h"
#include "udp.h"
struct responder { struct responder {
uint32_t message_id; uint32_t message_id;
send_fn send; udp::address reply_to;
template <typename T> template <typename T>
void respond(const T& msg) const { void respond(const T& msg) const {
send([&](span_writer& out) { const auto& ns = net_get_state();
return encode_response_into(out, message_id, msg); 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<const uint8_t> payload); using handler_fn = void (*)(const responder& resp, std::span<const uint8_t> payload);
struct handler_entry { struct handler_entry {
int8_t type_id; int8_t type_id;
@@ -28,7 +35,7 @@ struct handler_entry {
}; };
template <typename Req, auto Fn> template <typename Req, auto Fn>
void typed_handler(responder resp, std::span<const uint8_t> payload) { void typed_handler(const responder& resp, std::span<const uint8_t> payload) {
msgpack::parser p(payload.data(), static_cast<int>(payload.size())); msgpack::parser p(payload.data(), static_cast<int>(payload.size()));
Req req; Req req;
auto tup = req.as_tuple(); auto tup = req.as_tuple();
@@ -46,6 +53,6 @@ void typed_handler(responder resp, std::span<const uint8_t> payload) {
} }
void dispatch_init(); void dispatch_init();
timer_handle dispatch_schedule_ms(uint32_t ms, std::function<void()> fn); timer_handle dispatch_schedule_ms(uint32_t ms, void (*fn)());
bool dispatch_cancel_timer(timer_handle h); bool dispatch_cancel_timer(timer_handle h);
[[noreturn]] void dispatch_run(std::span<const handler_entry> handlers); [[noreturn]] void dispatch_run(std::span<const handler_entry> handlers);

16
firmware/include/net.h
View File

@@ -1,11 +1,9 @@
#pragma once #pragma once
#include <expected> #include <cstdint>
#include <functional>
#include <span> #include <span>
#include "eth.h" #include "eth.h"
#include "ipv4.h" #include "ipv4.h"
#include "span_writer.h" #include "span_writer.h"
#include "msgpack.h"
#include "callback_list.h" #include "callback_list.h"
struct net_state { struct net_state {
@@ -13,22 +11,16 @@ struct net_state {
ipv4::ip4_addr ip; ipv4::ip4_addr ip;
}; };
using encode_fn = std::function<msgpack::result<size_t>(span_writer&)>; using net_frame_callback = bool (*)(std::span<const uint8_t> frame);
using send_fn = std::function<void(encode_fn)>;
using net_handler = std::function<void(std::span<const uint8_t> payload,
send_fn send)>;
using net_frame_callback = std::function<bool(std::span<const uint8_t> frame)>;
using frame_cb_list = callback_list<net_frame_callback, 16>; using frame_cb_list = callback_list<net_frame_callback, 16>;
using frame_cb_handle = frame_cb_list::node*; using frame_cb_handle = frame_cb_list::node*;
inline constexpr uint16_t PICOMAP_PORT_BE = __builtin_bswap16(28781);
bool net_init(); bool net_init();
const net_state& net_get_state(); const net_state& net_get_state();
void net_set_handler(net_handler handler);
frame_cb_handle net_add_frame_callback(net_frame_callback cb); frame_cb_handle net_add_frame_callback(net_frame_callback cb);
void net_remove_frame_callback(frame_cb_handle h); void net_remove_frame_callback(frame_cb_handle h);
void net_poll(std::span<uint8_t> tx); void net_poll(std::span<uint8_t> tx);
void net_send_raw(std::span<const uint8_t> data); void net_send_raw(std::span<const uint8_t> data);
void net_handle_udp(std::span<const uint8_t> frame, span_writer& tx);

13
firmware/include/timer_queue.h
View File

@@ -1,11 +1,10 @@
#pragma once #pragma once
#include <functional>
#include "pico/time.h" #include "pico/time.h"
#include "callback_list.h" #include "callback_list.h"
struct timer_entry { struct timer_entry {
absolute_time_t when; absolute_time_t when;
std::function<void()> fn; void (*fn)() = nullptr;
}; };
using timer_handle = callback_list<timer_entry, 16>::node*; using timer_handle = callback_list<timer_entry, 16>::node*;
@@ -15,8 +14,8 @@ struct timer_queue {
alarm_id_t alarm = -1; alarm_id_t alarm = -1;
volatile bool irq_pending = false; volatile bool irq_pending = false;
timer_handle schedule(absolute_time_t when, std::function<void()> fn) { timer_handle schedule(absolute_time_t when, void (*fn)()) {
auto* n = list.insert_sorted({when, std::move(fn)}, auto* n = list.insert_sorted({when, fn},
[](const timer_entry& a, const timer_entry& b) { [](const timer_entry& a, const timer_entry& b) {
return absolute_time_diff_us(b.when, a.when) < 0; return absolute_time_diff_us(b.when, a.when) < 0;
}); });
@@ -24,8 +23,8 @@ struct timer_queue {
return n; return n;
} }
timer_handle schedule_ms(uint32_t ms, std::function<void()> fn) { timer_handle schedule_ms(uint32_t ms, void (*fn)()) {
return schedule(make_timeout_time_ms(ms), std::move(fn)); return schedule(make_timeout_time_ms(ms), fn);
} }
bool cancel(timer_handle h) { bool cancel(timer_handle h) {
@@ -40,7 +39,7 @@ struct timer_queue {
irq_pending = false; irq_pending = false;
while (auto* n = list.front()) { while (auto* n = list.front()) {
if (absolute_time_diff_us(get_absolute_time(), n->value.when) > 0) break; 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); list.remove(n);
fn(); fn();
} }

19
firmware/include/udp.h
View File

@@ -1,7 +1,10 @@
#pragma once #pragma once
#include <cstdint> #include <cstdint>
#include <span>
#include "eth.h" #include "eth.h"
#include "ipv4.h" #include "ipv4.h"
#include "net.h"
#include "span_writer.h"
namespace udp { namespace udp {
@@ -13,6 +16,14 @@ struct __attribute__((packed)) header {
}; };
static_assert(sizeof(header) == 8); 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 <typename Buf> template <typename Buf>
void prepend(Buf& buf, const eth::mac_addr& dst_mac, const eth::mac_addr& src_mac, 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, 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); ipv4::prepend(buf, dst_mac, src_mac, src_ip, dst_ip, 17, sizeof(header) + payload_len, ttl);
} }
void handle(std::span<const uint8_t> 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<const uint8_t> payload, const address& from);
} // namespace client
} // namespace udp } // namespace udp

42
firmware/lib/dispatch.cpp
View File

@@ -1,13 +1,16 @@
#include "dispatch.h" #include "dispatch.h"
#include <array>
#include "pico/stdlib.h" #include "pico/stdlib.h"
#include "wire.h" #include "wire.h"
#include "timer_queue.h" #include "timer_queue.h"
#include "net.h" #include "net.h"
#include "igmp.h" #include "igmp.h"
#include "udp.h"
#include "debug_log.h" #include "debug_log.h"
#include "hardware/sync.h" #include "hardware/sync.h"
static timer_queue timers; static timer_queue timers;
static std::array<handler_fn, 128> handler_map{};
static void igmp_reannounce() { static void igmp_reannounce() {
auto& ns = net_get_state(); auto& ns = net_get_state();
@@ -21,8 +24,8 @@ void dispatch_init() {
dlog("dispatch_init complete"); dlog("dispatch_init complete");
} }
timer_handle dispatch_schedule_ms(uint32_t ms, std::function<void()> fn) { timer_handle dispatch_schedule_ms(uint32_t ms, void (*fn)()) {
auto h = timers.schedule_ms(ms, std::move(fn)); auto h = timers.schedule_ms(ms, fn);
if (!h) dlogf("timer alloc failed: %lu ms", static_cast<unsigned long>(ms)); if (!h) dlogf("timer alloc failed: %lu ms", static_cast<unsigned long>(ms));
return h; return h;
} }
@@ -31,29 +34,22 @@ bool dispatch_cancel_timer(timer_handle h) {
return timers.cancel(h); return timers.cancel(h);
} }
[[noreturn]] void dispatch_run(std::span<const handler_entry> handlers) { void udp::client::handler(std::span<const uint8_t> payload, const udp::address& from) {
std::array<handler_fn, 128> handler_map{};
for (auto& entry : handlers) {
handler_map[entry.type_id] = entry.handle;
}
static std::array<uint8_t, 4096> 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<const uint8_t> payload,
send_fn send) {
auto msg = try_decode(payload.data(), payload.size()); auto msg = try_decode(payload.data(), payload.size());
if (!msg) return; if (!msg) return;
dispatch_msg(*msg, std::move(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, 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;
static std::array<uint8_t, 4096> tx_buf;
while (true) { while (true) {
uint32_t save = save_and_disable_interrupts(); uint32_t save = save_and_disable_interrupts();

4
firmware/lib/ipv4.cpp
View File

@@ -1,7 +1,7 @@
#include "ipv4.h" #include "ipv4.h"
#include "icmp.h" #include "icmp.h"
#include "igmp.h" #include "igmp.h"
#include "net.h" #include "udp.h"
#include "parse_buffer.h" #include "parse_buffer.h"
namespace ipv4 { namespace ipv4 {
@@ -47,7 +47,7 @@ void handle(std::span<const uint8_t> frame, span_writer& tx,
case 17: case 17:
if (!ip_match(ip->dst, our_ip, subnet_broadcast)) if (!ip_match(ip->dst, our_ip, subnet_broadcast))
return; return;
net_handle_udp(frame, tx); udp::handle(frame, tx);
break; break;
} }
} }

44
firmware/lib/net.cpp
View File

@@ -12,11 +12,9 @@
#include "debug_log.h" #include "debug_log.h"
static constexpr ipv4::ip4_addr IP_BROADCAST_SUBNET = {169, 254, 255, 255}; 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 net_state state;
static w6300::socket_id raw_socket{0}; static w6300::socket_id raw_socket{0};
static net_handler msg_handler;
static frame_cb_list frame_callbacks; static frame_cb_list frame_callbacks;
void net_send_raw(std::span<const uint8_t> data) { void net_send_raw(std::span<const uint8_t> data) {
@@ -28,42 +26,6 @@ void net_send_raw(std::span<const uint8_t> data) {
}); });
} }
void net_handle_udp(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;
size_t options_len = ip->header_len() - sizeof(ipv4::header);
if (options_len > 0 && !pb.skip(options_len)) return;
auto* uhdr = pb.consume<udp::header>();
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) { static bool mac_match(const eth::mac_addr& dst) {
return dst == state.mac || dst == eth::MAC_BROADCAST || igmp::is_member_mac(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; 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) { 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"); if (!h) dlog("frame callback alloc failed");
return h; return h;
} }

420
firmware/lib/test_handlers.cpp
View File

@@ -1,6 +1,5 @@
#include "test_handlers.h" #include "test_handlers.h"
#include <cstring> #include <cstring>
#include <memory>
#include <unordered_map> #include <unordered_map>
#include "pico/stdlib.h" #include "pico/stdlib.h"
#include "pico/time.h" #include "pico/time.h"
@@ -11,39 +10,98 @@
#include "parse_buffer.h" #include "parse_buffer.h"
#include "prepend_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 { struct peer_info {
eth::mac_addr mac; eth::mac_addr mac;
ipv4::ip4_addr ip; ipv4::ip4_addr ip;
}; };
using peer_callback = std::function<void(const peer_info&)>; // Shared sub-struct types. Used as fields inside per-test-command structs
using fail_callback = std::function<void()>; // when two tests happen to carry the same shape of data.
static void discover_peer(peer_callback on_found, fail_callback on_timeout) { // State for the discover_peer primitive.
auto& ns = net_get_state(); struct discovery_data {
eth::mac_addr mcast_mac = igmp::mac_for_ip(igmp::PICOMAP_DISCOVERY_GROUP); void (*on_found)(const peer_info&) = nullptr;
void (*on_timeout)() = nullptr;
};
prepend_buffer<4096> buf; // Rolling state for a rate-testing ping run.
uint8_t* payload = buf.payload_ptr(); struct ping_rate_data {
span_writer out(payload, 1024); peer_info peer;
RequestInfo req_msg; uint16_t target;
auto encoded = encode_response_into(out, 0xFFFF, req_msg); uint16_t pipeline;
if (!encoded) { uint16_t payload_len;
on_timeout(); uint16_t sent;
return; 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;
} }
buf.append(*encoded);
udp::prepend(buf, mcast_mac, ns.mac, ns.ip, igmp::PICOMAP_DISCOVERY_GROUP, // ----- discover_peer (shared building block) -----
PICOMAP_PORT, PICOMAP_PORT, *encoded, 1);
ipv4::ip4_addr our_ip = ns.ip; // 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.
auto timer = std::make_shared<timer_handle>(nullptr); static bool discover_reply_cb(std::span<const uint8_t> frame) {
auto cb = std::make_shared<frame_cb_handle>(nullptr);
*cb = net_add_frame_callback([our_ip, timer, on_found = std::move(on_found)](std::span<const uint8_t> frame) -> bool {
parse_buffer pb(frame); parse_buffer pb(frame);
auto* eth_hdr = pb.consume<eth::header>(); auto* eth_hdr = pb.consume<eth::header>();
if (!eth_hdr || eth_hdr->ethertype != eth::ETH_IPV4) return false; if (!eth_hdr || eth_hdr->ethertype != eth::ETH_IPV4) return false;
@@ -52,202 +110,229 @@ static void discover_peer(peer_callback on_found, fail_callback on_timeout) {
size_t options_len = ip->header_len() - sizeof(ipv4::header); size_t options_len = ip->header_len() - sizeof(ipv4::header);
if (options_len > 0 && !pb.skip(options_len)) return false; if (options_len > 0 && !pb.skip(options_len)) return false;
auto* uhdr = pb.consume<udp::header>(); auto* uhdr = pb.consume<udp::header>();
if (!uhdr || uhdr->src_port != PICOMAP_PORT) return false; if (!uhdr || uhdr->src_port != PICOMAP_PORT_BE) return false;
if (ip->src == our_ip) return false; if (ip->src == net_get_state().ip) return false;
dispatch_cancel_timer(*timer); dispatch_cancel_timer(ts.timer);
on_found({eth_hdr->src, ip->src}); 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; return true;
}); }
*timer = dispatch_schedule_ms(5000, [cb, on_timeout = std::move(on_timeout)]() { static void discover_timeout_cb() {
net_remove_frame_callback(*cb); net_remove_frame_callback(ts.frame_cb);
on_timeout(); 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;
uint8_t* payload = buf.payload_ptr();
span_writer out(payload, 1024);
RequestInfo req_msg;
auto encoded = encode_response_into(out, 0xFFFF, req_msg);
if (!encoded) {
ts.active_discovery = nullptr;
timeout();
return;
}
buf.append(*encoded);
udp::prepend(buf, mcast_mac, ns.mac, ns.ip, igmp::PICOMAP_DISCOVERY_GROUP,
PICOMAP_PORT_BE, PICOMAP_PORT_BE, *encoded, 1);
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 void test_discovery_igmp(const responder& resp) { // ----- discovery_igmp -----
auto& ns = net_get_state();
prepend_buffer<4096> buf; static bool igmp_report_cb(std::span<const uint8_t> frame) {
igmp::prepend_query(buf, ns.mac, ns.ip, igmp::PICOMAP_DISCOVERY_GROUP);
auto timer = std::make_shared<timer_handle>(nullptr);
auto cb = std::make_shared<frame_cb_handle>(nullptr);
*cb = net_add_frame_callback([resp, timer](std::span<const uint8_t> frame) -> bool {
ipv4::ip4_addr group; ipv4::ip4_addr group;
if (!igmp::parse_report(frame, group)) return false; if (!igmp::parse_report(frame, group)) return false;
if (group != igmp::PICOMAP_DISCOVERY_GROUP) return false; if (group != igmp::PICOMAP_DISCOVERY_GROUP) return false;
dispatch_cancel_timer(*timer); ts.frame_cb = nullptr; // self-consumed via `return true`
resp.respond(ResponseTest{true, {"got IGMP report for " + ipv4::to_string(group)}}); test_end({true, {"got IGMP report for " + ipv4::to_string(group)}});
return true; return true;
}); }
*timer = dispatch_schedule_ms(5000, [cb, resp]() { static void igmp_timeout_cb() {
net_remove_frame_callback(*cb); ts.timer = nullptr; // already fired
resp.respond(ResponseTest{false, {"no IGMP report within 5s"}}); 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);
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 test_discovery_info(const responder& resp) { // ----- discovery_info -----
discover_peer(
[resp](const peer_info& peer) { static void info_found(const peer_info& peer) {
resp.respond(ResponseTest{true, {"got info response from " + ipv4::to_string(peer.ip)}}); test_end({true, {"got info response from " + ipv4::to_string(peer.ip)}});
},
[resp]() {
resp.respond(ResponseTest{false, {"no info response within 5s"}});
});
} }
static void test_ping(const responder& resp, ipv4::ip4_addr dst_ip) { static void info_timeout() {
auto& ns = net_get_state(); test_end({false, {"no info response within 5s"}});
uint16_t ping_id = 0x1234; }
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<const uint8_t> 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; prepend_buffer<4096> buf;
icmp::prepend_echo_request(buf, ns.mac, ns.ip, icmp::prepend_echo_request(buf, ns.mac, ns.ip,
eth::MAC_BROADCAST, dst_ip, ping_id, 1); eth::MAC_BROADCAST, dst_ip, PING_ECHO_ID, 1);
ts.frame_cb = net_add_frame_callback(ping_reply_cb);
ipv4::ip4_addr our_ip = ns.ip; ts.timer = dispatch_schedule_ms(5000, ping_timeout_cb);
auto timer = std::make_shared<timer_handle>(nullptr);
auto cb = std::make_shared<frame_cb_handle>(nullptr);
*cb = net_add_frame_callback([resp, ping_id, our_ip, timer](std::span<const uint8_t> 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"}});
});
net_send_raw(buf.span()); net_send_raw(buf.span());
} }
static void test_ping_subnet(const responder& resp) { static void test_ping_subnet() { start_ping({169, 254, 255, 255}); }
test_ping(resp, {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) { static void ping_rate_send_one() {
test_ping(resp, {255, 255, 255, 255}); const auto& ns = net_get_state();
} auto& r = *ts.active_rate;
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<ping_rate_state> st) {
prepend_buffer<4096> buf; prepend_buffer<4096> buf;
if (st->payload_len > 0) if (r.payload_len > 0)
memset(buf.append(st->payload_len), 0xAA, st->payload_len); memset(buf.append(r.payload_len), 0xAA, r.payload_len);
icmp::prepend_echo_request(buf, st->our_mac, st->our_ip, icmp::prepend_echo_request(buf, ns.mac, ns.ip,
st->peer.mac, st->peer.ip, st->ping_id, r.peer.mac, r.peer.ip, PING_RATE_ECHO_ID,
st->sent + 1, st->payload_len); r.sent + 1, r.payload_len);
net_send_raw(buf.span()); net_send_raw(buf.span());
st->sent++; r.sent++;
} }
static void start_ping_rate(const responder& resp, uint16_t target, static bool ping_rate_reply_cb(std::span<const uint8_t> frame) {
uint16_t payload_len, uint16_t pipeline) {
auto& ns = net_get_state();
discover_peer(
[resp, ns, target, payload_len, pipeline](const peer_info& peer) {
auto st = std::make_shared<ping_rate_state>();
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();
st->cb_handle = net_add_frame_callback([st](std::span<const uint8_t> frame) -> bool {
ipv4::ip4_addr src_ip; ipv4::ip4_addr src_ip;
if (!icmp::parse_echo_reply(frame, src_ip, st->ping_id)) return false; if (!icmp::parse_echo_reply(frame, src_ip, PING_RATE_ECHO_ID)) return false;
if (src_ip == st->our_ip) return false; if (src_ip == net_get_state().ip) return false;
st->received++; auto& r = *ts.active_rate;
if (st->received >= st->target) { r.received++;
dispatch_cancel_timer(st->timer); if (r.received >= r.target) {
uint32_t elapsed_us = time_us_32() - st->start_us; uint32_t elapsed_us = time_us_32() - r.start_us;
uint32_t elapsed_ms = elapsed_us / 1000; uint32_t elapsed_ms = elapsed_us / 1000;
uint32_t pps = static_cast<uint32_t>( uint32_t pps = static_cast<uint32_t>(
static_cast<uint64_t>(st->received) * 1000000 / elapsed_us); static_cast<uint64_t>(r.received) * 1000000 / elapsed_us);
uint64_t total_bytes = static_cast<uint64_t>(st->received) * 2 * st->frame_size(); uint64_t total_bytes = static_cast<uint64_t>(r.received) * 2 * ping_rate_frame_size();
uint32_t kbps = static_cast<uint32_t>(total_bytes * 1000 / elapsed_us); uint32_t kbps = static_cast<uint32_t>(total_bytes * 1000 / elapsed_us);
char msg[128]; char msg[128];
snprintf(msg, sizeof(msg), snprintf(msg, sizeof(msg),
"%u rt in %lu ms, %lu pps, %lu bytes, %lu KB/s", "%u rt in %lu ms, %lu pps, %lu bytes, %lu KB/s",
st->received, static_cast<unsigned long>(elapsed_ms), r.received, static_cast<unsigned long>(elapsed_ms),
static_cast<unsigned long>(pps), static_cast<unsigned long>(pps),
static_cast<unsigned long>(total_bytes), static_cast<unsigned long>(total_bytes),
static_cast<unsigned long>(kbps)); static_cast<unsigned long>(kbps));
st->resp.respond(ResponseTest{true, {msg}}); ts.frame_cb = nullptr; // self-consumed via `return true`
test_end({true, {msg}});
return true; return true;
} }
if (st->sent < st->target) if (r.sent < r.target)
ping_rate_send_one(st); ping_rate_send_one();
return false; return false;
}); }
st->timer = dispatch_schedule_ms(10000, [st]() { static void ping_rate_timeout_cb() {
net_remove_frame_callback(st->cb_handle); ts.timer = nullptr; // already fired
uint32_t elapsed_us = time_us_32() - st->start_us; auto& r = *ts.active_rate;
uint32_t elapsed_us = time_us_32() - r.start_us;
char msg[64]; char msg[64];
snprintf(msg, sizeof(msg), "timeout after %u/%u rt in %lu ms", snprintf(msg, sizeof(msg), "timeout after %u/%u rt in %lu ms",
st->received, st->sent, r.received, r.sent,
static_cast<unsigned long>(elapsed_us / 1000)); static_cast<unsigned long>(elapsed_us / 1000));
st->resp.respond(ResponseTest{false, {msg}}); test_end({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"}});
});
} }
static void test_packet_rate(const responder& resp) { static void ping_rate_found(const peer_info& peer) {
start_ping_rate(resp, 8192, 0, 8); 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();
} }
static void test_byte_rate(const responder& resp) { static void ping_rate_no_peer() {
start_ping_rate(resp, 2048, 1400, 8); test_end({false, {"no peer found"}});
} }
using sync_test_fn = ResponseTest (*)(const responder&); static void start_ping_rate(discovery_data& d, ping_rate_data& r,
using async_test_fn = void (*)(const responder&); 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 { struct test_entry {
sync_test_fn sync; sync_test_fn sync;
@@ -271,11 +356,16 @@ std::optional<ResponseListTests> handle_list_tests(const responder&, const Reque
} }
std::optional<ResponseTest> handle_test(const responder& resp, const RequestTest& req) { std::optional<ResponseTest> handle_test(const responder& resp, const RequestTest& req) {
if (ts.in_flight)
return ResponseTest{false, {"test already running"}};
auto it = tests.find(req.name); auto it = tests.find(req.name);
if (it == tests.end()) if (it == tests.end())
return ResponseTest{false, {"unknown test: " + req.name}}; return ResponseTest{false, {"unknown test: " + req.name}};
if (it->second.sync) if (it->second.sync)
return it->second.sync(resp); return it->second.sync();
it->second.async(resp);
ts.in_flight = true;
ts.resp = resp;
it->second.async();
return std::nullopt; return std::nullopt;
} }

31
firmware/lib/udp.cpp Normal file
View File

@@ -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<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;
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);
}
} // namespace udp