From 1843660e6f85ce3ce95bb4df235cf809abb857b1 Mon Sep 17 00:00:00 2001 From: Ian Gulliver Date: Sat, 11 Apr 2026 09:17:46 +0900 Subject: [PATCH] Add ping rate tests, discover_peer helper, standardize prepend_buffer to 4096 --- firmware/include/icmp.h | 8 +- firmware/lib/arp.cpp | 2 +- firmware/lib/icmp.cpp | 2 +- firmware/lib/igmp.cpp | 2 +- firmware/lib/net.cpp | 2 +- firmware/lib/test_handlers.cpp | 251 +++++++++++++++++++++++++-------- 6 files changed, 199 insertions(+), 68 deletions(-) diff --git a/firmware/include/icmp.h b/firmware/include/icmp.h index 18e1a01..f92ca8e 100644 --- a/firmware/include/icmp.h +++ b/firmware/include/icmp.h @@ -25,15 +25,17 @@ template void prepend_echo_request(Buf& buf, eth::mac_addr src_mac, ipv4::ip4_addr src_ip, eth::mac_addr dst_mac, ipv4::ip4_addr dst_ip, - uint16_t id, uint16_t seq) { + uint16_t id, uint16_t seq, + size_t payload_len = 0) { auto* e = buf.template prepend(); e->type = 8; e->code = 0; e->checksum = 0; e->id = id; e->seq = seq; - e->checksum = ipv4::checksum(e, sizeof(echo)); - ipv4::prepend(buf, dst_mac, src_mac, src_ip, dst_ip, 1, sizeof(echo)); + size_t icmp_len = sizeof(echo) + payload_len; + e->checksum = ipv4::checksum(e, icmp_len); + ipv4::prepend(buf, dst_mac, src_mac, src_ip, dst_ip, 1, icmp_len); } bool parse_echo_reply(std::span frame, ipv4::ip4_addr& src_ip, uint16_t expected_id); diff --git a/firmware/lib/arp.cpp b/firmware/lib/arp.cpp index d44e578..1bafa43 100644 --- a/firmware/lib/arp.cpp +++ b/firmware/lib/arp.cpp @@ -23,7 +23,7 @@ void handle(std::span frame, span_writer& tx, if (arp_hdr->oper != ARP_OP_REQUEST) return; if (arp_hdr->tpa != our_ip) return; - prepend_buffer<128> buf; + prepend_buffer<4096> buf; auto* reply = buf.template prepend
(); reply->htype = ARP_HTYPE_ETH; reply->ptype = ARP_PTYPE_IPV4; diff --git a/firmware/lib/icmp.cpp b/firmware/lib/icmp.cpp index 95f1765..595b642 100644 --- a/firmware/lib/icmp.cpp +++ b/firmware/lib/icmp.cpp @@ -25,7 +25,7 @@ void handle(std::span frame, span_writer& tx, if (!icmp_pkt) return; if (icmp_pkt->type != 8) return; - prepend_buffer<1514> buf; + prepend_buffer<4096> buf; memcpy(buf.append(icmp_len), pb.remaining().data() - sizeof(echo), icmp_len); auto* reply = reinterpret_cast(buf.data()); diff --git a/firmware/lib/igmp.cpp b/firmware/lib/igmp.cpp index 4514ca4..c8aafd6 100644 --- a/firmware/lib/igmp.cpp +++ b/firmware/lib/igmp.cpp @@ -36,7 +36,7 @@ bool is_member_mac(const eth::mac_addr& mac) { static void send_report(const ipv4::ip4_addr& group, eth::mac_addr our_mac, ipv4::ip4_addr our_ip, std::function)> send_raw) { - prepend_buffer<128> buf; + prepend_buffer<4096> buf; prepend_report(buf, our_mac, our_ip, group); send_raw(buf.span()); } diff --git a/firmware/lib/net.cpp b/firmware/lib/net.cpp index 29f06f2..fa76346 100644 --- a/firmware/lib/net.cpp +++ b/firmware/lib/net.cpp @@ -51,7 +51,7 @@ static void handle_udp(std::span frame, span_writer& tx) { msg_handler(pb.remaining().subspan(0, payload_len), [dst_mac, dst_ip, dst_port](std::span resp_data) { - prepend_buffer<1514> buf; + prepend_buffer<4096> buf; buf.append_copy(resp_data); udp::prepend(buf, dst_mac, state.mac, state.ip, dst_ip, PICOMAP_PORT, dst_port, resp_data.size()); diff --git a/firmware/lib/test_handlers.cpp b/firmware/lib/test_handlers.cpp index ba041f5..9d3b4d4 100644 --- a/firmware/lib/test_handlers.cpp +++ b/firmware/lib/test_handlers.cpp @@ -1,4 +1,5 @@ #include "test_handlers.h" +#include #include #include #include "pico/stdlib.h" @@ -10,10 +11,79 @@ #include "parse_buffer.h" #include "prepend_buffer.h" +static constexpr uint16_t PICOMAP_PORT = __builtin_bswap16(28781); + +struct peer_info { + eth::mac_addr mac; + ipv4::ip4_addr ip; +}; + +using peer_callback = std::function; +using fail_callback = std::function; + +static void discover_peer(peer_callback on_found, fail_callback on_timeout) { + 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) { + on_timeout(); + return; + } + buf.append(*encoded); + + udp::prepend(buf, mcast_mac, ns.mac, ns.ip, igmp::PICOMAP_DISCOVERY_GROUP, + PICOMAP_PORT, PICOMAP_PORT, *encoded, 1); + net_send_raw(buf.span()); + + ipv4::ip4_addr our_ip = ns.ip; + + auto timer = std::make_shared(nullptr); + auto cb = std::make_shared)>>(); + *cb = [our_ip, timer, cb, on_found = std::move(on_found)](std::span frame) { + parse_buffer pb(frame); + auto* eth_hdr = pb.consume(); + if (!eth_hdr || eth_hdr->ethertype != eth::ETH_IPV4) { + net_add_frame_callback(*cb); + return; + } + auto* ip = pb.consume(); + if (!ip || ip->protocol != 17) { + net_add_frame_callback(*cb); + return; + } + size_t options_len = ip->header_len() - sizeof(ipv4::header); + if (options_len > 0 && !pb.skip(options_len)) { + net_add_frame_callback(*cb); + return; + } + auto* uhdr = pb.consume(); + if (!uhdr || uhdr->src_port != PICOMAP_PORT) { + net_add_frame_callback(*cb); + return; + } + if (ip->src == our_ip) { + net_add_frame_callback(*cb); + return; + } + dispatch_cancel_timer(*timer); + on_found({eth_hdr->src, ip->src}); + }; + net_add_frame_callback(*cb); + + *timer = dispatch_schedule_ms(5000, [on_timeout = std::move(on_timeout)]() { + on_timeout(); + }); +} + static void test_discovery_igmp(const responder& resp) { auto& ns = net_get_state(); - prepend_buffer<128> buf; + prepend_buffer<4096> buf; igmp::prepend_query(buf, ns.mac, ns.ip, igmp::PICOMAP_DISCOVERY_GROUP); net_send_raw(buf.span()); @@ -40,72 +110,20 @@ static void test_discovery_igmp(const responder& resp) { } static void test_discovery_info(const responder& resp) { - auto& ns = net_get_state(); - - eth::mac_addr mcast_mac = igmp::mac_for_ip(igmp::PICOMAP_DISCOVERY_GROUP); - static constexpr uint16_t PICOMAP_PORT = __builtin_bswap16(28781); - - prepend_buffer<1514> 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) { - resp.respond(ResponseTest{false, {"encode RequestInfo failed"}}); - return; - } - buf.append(*encoded); - size_t payload_len = *encoded; - - udp::prepend(buf, mcast_mac, ns.mac, ns.ip, igmp::PICOMAP_DISCOVERY_GROUP, - PICOMAP_PORT, PICOMAP_PORT, payload_len, 1); - net_send_raw(buf.span()); - - ipv4::ip4_addr our_ip = ns.ip; - - auto timer = std::make_shared(nullptr); - auto cb = std::make_shared)>>(); - *cb = [resp, our_ip, timer, cb](std::span frame) { - parse_buffer pb(frame); - auto* eth_hdr = pb.consume(); - if (!eth_hdr || eth_hdr->ethertype != eth::ETH_IPV4) { - net_add_frame_callback(*cb); - return; - } - auto* ip = pb.consume(); - if (!ip || ip->protocol != 17) { - net_add_frame_callback(*cb); - return; - } - size_t options_len = ip->header_len() - sizeof(ipv4::header); - if (options_len > 0 && !pb.skip(options_len)) { - net_add_frame_callback(*cb); - return; - } - auto* uhdr = pb.consume(); - if (!uhdr || uhdr->src_port != __builtin_bswap16(28781)) { - net_add_frame_callback(*cb); - return; - } - if (ip->src == our_ip) { - net_add_frame_callback(*cb); - return; - } - dispatch_cancel_timer(*timer); - resp.respond(ResponseTest{true, {"got info response from " + ipv4::to_string(ip->src)}}); - }; - net_add_frame_callback(*cb); - - *timer = dispatch_schedule_ms(5000, [resp]() { - resp.respond(ResponseTest{false, {"no info response within 5s"}}); - }); + 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"}}); + }); } static void test_ping(const responder& resp, ipv4::ip4_addr dst_ip) { auto& ns = net_get_state(); uint16_t ping_id = 0x1234; - prepend_buffer<128> buf; + prepend_buffer<4096> buf; icmp::prepend_echo_request(buf, ns.mac, ns.ip, eth::MAC_BROADCAST, dst_ip, ping_id, 1); net_send_raw(buf.span()); @@ -142,6 +160,115 @@ 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; + uint16_t ping_id; + uint16_t count = 0; + uint16_t target; + 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(std::shared_ptr st, + std::shared_ptr)>> cb) { + 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->count + 1, st->payload_len); + net_send_raw(buf.span()); + net_add_frame_callback(*cb); +} + +static void ping_rate_recv(std::shared_ptr st, + std::shared_ptr)>> cb, + std::span frame) { + ipv4::ip4_addr src_ip; + if (!icmp::parse_echo_reply(frame, src_ip, st->ping_id)) { + net_add_frame_callback(*cb); + return; + } + if (src_ip == st->our_ip) { + net_add_frame_callback(*cb); + return; + } + + st->count++; + if (st->count >= 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->count) * 1000000 / elapsed_us); + uint64_t total_bytes = static_cast(st->count) * 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->count, static_cast(elapsed_ms), + static_cast(pps), + static_cast(total_bytes), + static_cast(kbps)); + st->resp.respond(ResponseTest{true, {msg}}); + return; + } + + ping_rate_send(st, cb); +} + +static void start_ping_rate(const responder& resp, uint16_t target, + uint16_t payload_len) { + auto& ns = net_get_state(); + + discover_peer( + [resp, ns, target, payload_len](const peer_info& peer) { + auto st = std::make_shared(); + st->resp = resp; + st->ping_id = 0x5678; + st->target = target; + 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 cb = std::make_shared)>>(); + *cb = [st, cb](std::span frame) { + ping_rate_recv(st, cb, frame); + }; + + st->timer = dispatch_schedule_ms(60000, [st]() { + uint32_t elapsed_us = time_us_32() - st->start_us; + char msg[64]; + snprintf(msg, sizeof(msg), "timeout after %u rt in %lu ms", + st->count, static_cast(elapsed_us / 1000)); + st->resp.respond(ResponseTest{false, {msg}}); + }); + + ping_rate_send(st, cb); + }, + [resp]() { + resp.respond(ResponseTest{false, {"no peer found"}}); + }); +} + +static void test_ping_rate(const responder& resp) { + start_ping_rate(resp, 8192, 0); +} + +static void test_ping_rate_1k(const responder& resp) { + start_ping_rate(resp, 1024, 1024); +} + using sync_test_fn = ResponseTest (*)(const responder&); using async_test_fn = void (*)(const responder&); @@ -155,6 +282,8 @@ static const std::unordered_map tests = { {"discovery_info", {nullptr, test_discovery_info}}, {"ping_subnet", {nullptr, test_ping_subnet}}, {"ping_global", {nullptr, test_ping_global}}, + {"ping_rate", {nullptr, test_ping_rate}}, + {"ping_rate_1k", {nullptr, test_ping_rate_1k}}, }; std::optional handle_list_tests(const responder&, const RequestListTests&) {