initial import from picomap firmware

src/arp.cpp

@@ -0,0 +1,47 @@
+#include "arp.h"
+#include "net.h"
+#include "parse_buffer.h"
+#include "prepend_buffer.h"
+
+namespace arp {
+
+static constexpr uint16_t ARP_HTYPE_ETH = __builtin_bswap16(1);
+static constexpr uint16_t ARP_PTYPE_IPV4 = __builtin_bswap16(0x0800);
+static constexpr uint16_t ARP_OP_REQUEST = __builtin_bswap16(1);
+static constexpr uint16_t ARP_OP_REPLY = __builtin_bswap16(2);
+
+void handle(std::span<const uint8_t> frame, span_writer& tx) {
+    const auto& ns = net_get_state();
+    parse_buffer pb(frame);
+    pb.consume<eth::header>();
+    auto* arp_hdr = pb.consume<header>();
+    if (!arp_hdr) return;
+
+    if (arp_hdr->htype != ARP_HTYPE_ETH) return;
+    if (arp_hdr->ptype != ARP_PTYPE_IPV4) return;
+    if (arp_hdr->hlen != 6 || arp_hdr->plen != 4) return;
+    if (arp_hdr->oper != ARP_OP_REQUEST) return;
+    if (arp_hdr->tpa != ns.ip) return;
+
+    prepend_buffer<4096> buf;
+    auto* reply = buf.template prepend<header>();
+    reply->htype = ARP_HTYPE_ETH;
+    reply->ptype = ARP_PTYPE_IPV4;
+    reply->hlen = 6;
+    reply->plen = 4;
+    reply->oper = ARP_OP_REPLY;
+    reply->sha = ns.mac;
+    reply->spa = ns.ip;
+    reply->tha = arp_hdr->sha;
+    reply->tpa = arp_hdr->spa;
+    eth::prepend(buf, arp_hdr->sha, ns.mac, eth::ETH_ARP);
+
+    net_send_raw(buf.span());
+}
+
+__attribute__((constructor))
+static void register_ethertype() {
+    net_register_ethertype(eth::ETH_ARP, handle);
+}
+
+} // namespace arp

src/dispatch.cpp

@@ -0,0 +1,67 @@
+#include "dispatch.h"
+#include <array>
+#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_fn, 128> handler_map{};
+static uint16_t listen_port_be = 0;
+
+uint16_t dispatch_listen_port_be() { return listen_port_be; }
+
+static void igmp_reannounce() {
+    igmp::send_all_reports();
+    dispatch_schedule_ms(60000, igmp_reannounce);
+}
+
+static void on_udp_message(std::span<const uint8_t> payload, const udp::address& from) {
+    auto msg = try_decode(payload.data(), payload.size());
+    if (!msg) return;
+    if (msg->type_id < 0 || !handler_map[msg->type_id]) {
+        dlogf("dispatch: unknown type_id %d", msg->type_id);
+        return;
+    }
+    responder resp{msg->message_id, from};
+    handler_map[msg->type_id](resp, msg->payload);
+}
+
+void dispatch_init(uint16_t port_be) {
+    listen_port_be = port_be;
+    udp::register_port(port_be, on_udp_message);
+    net_init();
+    dispatch_schedule_ms(60000, igmp_reannounce);
+    dlog("dispatch_init complete");
+}
+
+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<unsigned long>(ms));
+    return h;
+}
+
+bool dispatch_cancel_timer(timer_handle h) {
+    return timers.cancel(h);
+}
+
+[[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) {
+        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}); });
+
+        __wfi();
+        restore_interrupts(save);
+    }
+}

src/flash.cpp

@@ -0,0 +1,163 @@
+#include "flash.h"
+#include <cstring>
+#include "pico/sha256.h"
+#include "boot/picobin.h"
+
+namespace flash {
+namespace {
+
+constexpr uint32_t PICOBIN_MARKER_END = 0xab123579;
+
+struct __attribute__((packed)) last_item {
+    uint8_t type;
+    uint16_t block_item_words;
+    uint8_t pad;
+    int32_t next_block_offset;
+    uint32_t marker_end;
+};
+
+struct __attribute__((packed)) hash_def_header {
+    uint8_t type;
+    uint8_t size_words;
+    uint8_t reserved;
+    uint8_t hash_type;
+    uint16_t block_words_to_hash;
+    uint16_t pad;
+};
+
+struct __attribute__((packed)) load_map_header {
+    uint8_t type;
+    uint8_t size_words;
+    uint8_t reserved;
+    uint8_t flags_and_count;
+
+    uint8_t count() const { return flags_and_count & 0x7f; }
+    bool absolute() const { return flags_and_count & 0x80; }
+};
+
+struct load_map_entry {
+    uint32_t storage_addr;
+    uint32_t runtime_addr;
+    uint32_t size;
+};
+
+struct parsed_item {
+    uint8_t type;
+    uint8_t size_words;
+    const uint32_t* words;
+};
+
+struct parsed_block {
+    const uint32_t* base;
+    parsed_item items[16];
+    uint8_t item_count;
+    int32_t next_block_offset;
+};
+
+bool parse_block(const uint32_t* start, const uint32_t* limit, parsed_block& out) {
+    if (start >= limit || *start != PICOBIN_BLOCK_MARKER_START) return false;
+    out.base = start;
+    out.item_count = 0;
+    out.next_block_offset = 0;
+
+    auto* w = start + 1;
+    while (w + 3 < limit && out.item_count < 16) {
+        uint8_t type = *w & 0xff;
+        if (type == PICOBIN_BLOCK_ITEM_2BS_LAST) {
+            auto* last = reinterpret_cast<const last_item*>(w);
+            out.next_block_offset = last->next_block_offset;
+            return last->marker_end == PICOBIN_MARKER_END;
+        }
+        uint8_t size_words = (*w >> 8) & 0xff;
+        if (w + size_words > limit) return false;
+        out.items[out.item_count++] = {type, size_words, w};
+        w += size_words;
+    }
+    return false;
+}
+
+const parsed_item* find_item(const parsed_block& blk, uint8_t type) {
+    for (uint8_t i = 0; i < blk.item_count; i++)
+        if (blk.items[i].type == type) return &blk.items[i];
+    return nullptr;
+}
+
+bool verify_hash(const parsed_block& last) {
+    auto* lm_item = find_item(last, PICOBIN_BLOCK_ITEM_LOAD_MAP);
+    auto* hd_item = find_item(last, PICOBIN_BLOCK_ITEM_1BS_HASH_DEF);
+    auto* hv_item = find_item(last, PICOBIN_BLOCK_ITEM_HASH_VALUE);
+    if (!lm_item || !hd_item || !hv_item) return false;
+    if (hd_item->size_words < 2 || hv_item->size_words < 9) return false;
+
+    auto* hd = reinterpret_cast<const hash_def_header*>(hd_item->words);
+    if (hd->hash_type != PICOBIN_HASH_SHA256) return false;
+
+    auto* lm = reinterpret_cast<const load_map_header*>(lm_item->words);
+    auto* entries = reinterpret_cast<const load_map_entry*>(lm_item->words + 1);
+    uint32_t lm_xip_addr = reinterpret_cast<uint32_t>(lm_item->words);
+
+    pico_sha256_state_t sha;
+    if (pico_sha256_try_start(&sha, SHA256_BIG_ENDIAN, false) != PICO_OK) return false;
+
+    for (uint8_t i = 0; i < lm->count(); i++) {
+        uint32_t storage_addr = entries[i].storage_addr;
+        uint32_t size = entries[i].size;
+        if (lm->absolute()) size -= entries[i].runtime_addr;
+        if (storage_addr == 0) {
+            pico_sha256_update_blocking(&sha, reinterpret_cast<const uint8_t*>(&size), 4);
+        } else {
+            if (!lm->absolute()) storage_addr += lm_xip_addr;
+            pico_sha256_update_blocking(&sha, reinterpret_cast<const uint8_t*>(storage_addr), size);
+        }
+    }
+
+    pico_sha256_update_blocking(&sha,
+        reinterpret_cast<const uint8_t*>(last.base),
+        static_cast<size_t>(hd->block_words_to_hash) * 4);
+
+    sha256_result_t result;
+    pico_sha256_finish(&sha, &result);
+
+    auto* expected = reinterpret_cast<const uint8_t*>(hv_item->words + 1);
+    return memcmp(result.bytes, expected, 32) == 0;
+}
+
+}
+
+slot scan(uint32_t flash_offset) {
+    slot info{};
+    constexpr uint32_t scan_limit = 4096;
+    constexpr uint32_t slot_size = 512 * 1024;
+    auto* slot_base = reinterpret_cast<const uint32_t*>(FLASH_BASE + flash_offset);
+    auto* slot_end = reinterpret_cast<const uint32_t*>(FLASH_BASE + flash_offset + slot_size);
+
+    auto* s = slot_base;
+    auto* s_end = slot_base + scan_limit / 4;
+    while (s < s_end && *s != PICOBIN_BLOCK_MARKER_START) s++;
+    if (s >= s_end) return info;
+
+    parsed_block start;
+    if (!parse_block(s, slot_end, start)) return info;
+
+    for (uint8_t i = 0; i < start.item_count; i++) {
+        if (start.items[i].type == PICOBIN_BLOCK_ITEM_1BS_VERSION && start.items[i].size_words >= 2)
+            info.version = start.items[i].words[1];
+    }
+
+    auto* cur = &start;
+    parsed_block next;
+    while (cur->next_block_offset != 0) {
+        auto* np = reinterpret_cast<const uint32_t*>(
+            reinterpret_cast<const uint8_t*>(cur->base) + cur->next_block_offset);
+        if (np <= slot_base || np >= slot_end) return info;
+        if (np == start.base) break;
+        if (!parse_block(np, slot_end, next)) return info;
+        cur = &next;
+    }
+
+    info.valid = true;
+    info.hash_ok = verify_hash(*cur);
+    return info;
+}
+
+}

src/handlers.cpp

@@ -0,0 +1,104 @@
+#include "handlers.h"
+#include "pico/unique_id.h"
+#include "pico/bootrom.h"
+#include "hardware/flash.h"
+#include "hardware/watchdog.h"
+#include "flash.h"
+#include "dispatch.h"
+#include "net.h"
+#include "debug_log.h"
+
+static boot_reason detected_boot_reason;
+
+static void poke_watchdog() {
+    watchdog_update();
+    dispatch_schedule_ms(500, poke_watchdog);
+}
+
+void handlers_init() {
+    auto val = static_cast<boot_reason>(watchdog_hw->scratch[0]);
+    if (val == boot_reason::request_reboot || val == boot_reason::watchdog)
+        detected_boot_reason = val;
+    else
+        detected_boot_reason = boot_reason::cold_boot;
+    watchdog_hw->scratch[0] = static_cast<uint32_t>(boot_reason::watchdog);
+    watchdog_enable(1000, true);
+}
+
+void handlers_start() {
+    poke_watchdog();
+}
+
+std::optional<ResponseInfo> handle_info(const responder&, const RequestInfo&) {
+    ResponseInfo resp;
+    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();
+    resp.mac = ns.mac;
+    resp.ip = ns.ip;
+    resp.firmware_name = firmware_name;
+    resp.boot = detected_boot_reason;
+    resp.build_epoch = BUILD_EPOCH;
+    return resp;
+}
+
+std::optional<ResponseLog> handle_log(const responder&, const RequestLog&) {
+    ResponseLog resp;
+    for (auto& e : g_debug_log)
+        resp.entries.push_back(LogEntry{e.timestamp_us, e.message});
+    return resp;
+}
+
+std::optional<ResponseFlashErase> handle_flash_erase(const responder&, const RequestFlashErase& req) {
+    if (req.addr < flash::FLASH_BASE || req.addr + req.len > flash::FLASH_BASE + flash::FLASH_SIZE) {
+        dlogf("flash erase: out of range %08lx+%lu",
+              static_cast<unsigned long>(req.addr), static_cast<unsigned long>(req.len));
+        return std::nullopt;
+    }
+    uint32_t offset = req.addr - flash::FLASH_BASE;
+    if (offset % FLASH_SECTOR_SIZE != 0 || req.len % FLASH_SECTOR_SIZE != 0 || req.len == 0) {
+        dlogf("flash erase: bad alignment %08lx+%lu",
+              static_cast<unsigned long>(req.addr), static_cast<unsigned long>(req.len));
+        return std::nullopt;
+    }
+    flash_range_erase(offset, req.len);
+    return ResponseFlashErase{};
+}
+
+std::optional<ResponseFlashWrite> handle_flash_write(const responder&, const RequestFlashWrite& req) {
+    if (req.addr < flash::FLASH_BASE || req.addr + req.data.size() > flash::FLASH_BASE + flash::FLASH_SIZE) {
+        dlogf("flash write: out of range %08lx+%zu",
+              static_cast<unsigned long>(req.addr), req.data.size());
+        return std::nullopt;
+    }
+    uint32_t offset = req.addr - flash::FLASH_BASE;
+    if (offset % FLASH_PAGE_SIZE != 0 || req.data.size() % FLASH_PAGE_SIZE != 0 || req.data.empty()) {
+        dlogf("flash write: bad alignment %08lx+%zu",
+              static_cast<unsigned long>(req.addr), req.data.size());
+        return std::nullopt;
+    }
+    flash_range_program(offset, req.data.data(), req.data.size());
+    return ResponseFlashWrite{};
+}
+
+std::optional<ResponseFlashStatus> handle_flash_status(const responder&, const RequestFlashStatus&) {
+    ResponseFlashStatus resp;
+    boot_info_t bi;
+    if (rom_get_boot_info(&bi))
+        resp.boot_partition = bi.partition;
+    else
+        resp.boot_partition = -1;
+    resp.slot_a = flash::scan(0x00000);
+    resp.slot_b = flash::scan(0x80000);
+    return resp;
+}
+
+std::optional<ResponseReboot> handle_reboot(const responder&, const RequestReboot&) {
+    dispatch_schedule_ms(100, []{
+        watchdog_hw->scratch[0] = static_cast<uint32_t>(boot_reason::request_reboot);
+        watchdog_reboot(0, 0, 0);
+    });
+    return ResponseReboot{};
+}
+

src/icmp.cpp

@@ -0,0 +1,68 @@
+#include "icmp.h"
+#include <cstring>
+#include "ipv4.h"
+#include "net.h"
+#include "parse_buffer.h"
+#include "prepend_buffer.h"
+
+namespace icmp {
+
+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;
+    if (!ipv4::addressed_to_us(ip->dst)) return;
+
+    size_t options_len = ip->header_len() - sizeof(ipv4::header);
+    if (options_len > 0 && !pb.skip(options_len)) return;
+
+    size_t icmp_len = ip->total() - ip->header_len();
+    if (pb.remaining_size() < icmp_len) return;
+
+    auto* icmp_pkt = pb.consume<echo>();
+    if (!icmp_pkt) return;
+    if (icmp_pkt->type != 8) return;
+
+    const auto& ns = net_get_state();
+    prepend_buffer<4096> buf;
+    memcpy(buf.append(icmp_len), pb.remaining().data() - sizeof(echo), icmp_len);
+
+    auto* reply = reinterpret_cast<echo*>(buf.data());
+    reply->type = 0;
+    reply->checksum = 0;
+    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());
+}
+
+__attribute__((constructor))
+static void register_protocol() {
+    ipv4::register_protocol(1, handle);
+}
+
+bool parse_echo_reply(std::span<const uint8_t> frame, ipv4::ip4_addr& src_ip, uint16_t expected_id) {
+    parse_buffer pb(frame);
+    auto* eth_hdr = pb.consume<eth::header>();
+    if (!eth_hdr) return false;
+    if (eth_hdr->ethertype != eth::ETH_IPV4) return false;
+
+    auto* ip = pb.consume<ipv4::header>();
+    if (!ip) return false;
+    if ((ip->ver_ihl >> 4) != 4) return false;
+    if (ip->protocol != 1) return false;
+
+    size_t options_len = ip->header_len() - sizeof(ipv4::header);
+    if (options_len > 0 && !pb.skip(options_len)) return false;
+
+    auto* icmp_pkt = pb.consume<echo>();
+    if (!icmp_pkt) return false;
+    if (icmp_pkt->type != 0) return false;
+    if (icmp_pkt->id != expected_id) return false;
+
+    src_ip = ip->src;
+    return true;
+}
+
+} // namespace icmp

src/igmp.cpp

@@ -0,0 +1,109 @@
+#include "igmp.h"
+#include <vector>
+#include "ipv4.h"
+#include "net.h"
+#include "parse_buffer.h"
+#include "prepend_buffer.h"
+
+namespace igmp {
+
+struct group_entry {
+    ipv4::ip4_addr ip;
+    eth::mac_addr mac;
+};
+
+static std::vector<group_entry> groups;
+
+eth::mac_addr mac_for_ip(const ipv4::ip4_addr& group) {
+    return {0x01, 0x00, 0x5E,
+            static_cast<uint8_t>(group[1] & 0x7F), group[2], group[3]};
+}
+
+bool is_member(const ipv4::ip4_addr& ip) {
+    if (ip == ALL_HOSTS) return true;
+    for (auto& g : groups)
+        if (g.ip == ip) return true;
+    return false;
+}
+
+bool is_member_mac(const eth::mac_addr& mac) {
+    static constexpr eth::mac_addr ALL_HOSTS_MAC = {0x01, 0x00, 0x5E, 0x00, 0x00, 0x01};
+    if (mac == ALL_HOSTS_MAC) return true;
+    for (auto& g : groups)
+        if (g.mac == mac) return true;
+    return false;
+}
+
+static void send_report(const ipv4::ip4_addr& group) {
+    const auto& ns = net_get_state();
+    prepend_buffer<4096> buf;
+    prepend_report(buf, ns.mac, ns.ip, group);
+    net_send_raw(buf.span());
+}
+
+void join(const ipv4::ip4_addr& group) {
+    for (auto& g : groups)
+        if (g.ip == group) return;
+    groups.push_back({group, mac_for_ip(group)});
+    send_report(group);
+}
+
+void send_all_reports() {
+    for (auto& g : groups)
+        send_report(g.ip);
+}
+
+void handle(std::span<const uint8_t> frame, span_writer& tx) {
+    parse_buffer pb(frame);
+    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* msg = pb.consume<message>();
+    if (!msg) return;
+    if (msg->type != 0x11) return;
+
+    if (msg->group == ipv4::ip4_addr{0, 0, 0, 0}) {
+        for (auto& g : groups)
+            send_report(g.ip);
+    } else {
+        for (auto& g : groups) {
+            if (g.ip == msg->group) {
+                send_report(g.ip);
+                break;
+            }
+        }
+    }
+}
+
+__attribute__((constructor))
+static void register_protocol() {
+    ipv4::register_protocol(2, handle);
+}
+
+bool parse_report(std::span<const uint8_t> frame, ipv4::ip4_addr& group) {
+    parse_buffer pb(frame);
+    auto* eth_hdr = pb.consume<eth::header>();
+    if (!eth_hdr) return false;
+    if (eth_hdr->ethertype != eth::ETH_IPV4) return false;
+
+    auto* ip = pb.consume<ipv4::header>();
+    if (!ip) return false;
+    if ((ip->ver_ihl >> 4) != 4) return false;
+    if (ip->protocol != 2) return false;
+
+    size_t options_len = ip->header_len() - sizeof(ipv4::header);
+    if (options_len > 0 && !pb.skip(options_len)) return false;
+
+    auto* msg = pb.consume<message>();
+    if (!msg) return false;
+    if (msg->type != 0x16) return false;
+
+    group = msg->group;
+    return true;
+}
+
+} // namespace igmp

src/ipv4.cpp

@@ -0,0 +1,63 @@
+#include "ipv4.h"
+#include <array>
+#include "igmp.h"
+#include "net.h"
+#include "parse_buffer.h"
+
+namespace ipv4 {
+
+static constexpr ip4_addr IP_BROADCAST_ALL = {255, 255, 255, 255};
+
+uint16_t checksum(const void* data, size_t len) {
+    auto p = static_cast<const uint8_t*>(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);
+}
+
+bool addressed_to_us(ip4_addr dst) {
+    const auto& ns = net_get_state();
+    return dst == ns.ip || dst == IP_BROADCAST_ALL || dst == SUBNET_BROADCAST || igmp::is_member(dst);
+}
+
+struct protocol_entry {
+    uint8_t protocol;
+    protocol_handler fn;
+};
+static std::array<protocol_entry, 8> protocol_handlers;
+static size_t protocol_handler_count = 0;
+
+void register_protocol(uint8_t protocol, protocol_handler fn) {
+    if (protocol_handler_count < protocol_handlers.size())
+        protocol_handlers[protocol_handler_count++] = {protocol, fn};
+}
+
+void handle(std::span<const uint8_t> frame, span_writer& tx) {
+    parse_buffer pb(frame);
+    pb.consume<eth::header>();
+    auto* ip = pb.consume<header>();
+    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;
+
+    for (size_t i = 0; i < protocol_handler_count; i++) {
+        if (protocol_handlers[i].protocol == ip->protocol) {
+            protocol_handlers[i].fn(frame, tx);
+            break;
+        }
+    }
+}
+
+__attribute__((constructor))
+static void register_ethertype() {
+    net_register_ethertype(eth::ETH_IPV4, handle);
+}
+
+} // namespace ipv4

src/net.cpp

@@ -0,0 +1,112 @@
+#include "net.h"
+#include <array>
+#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;
+
+struct ethertype_entry {
+    uint16_t ethertype_be;
+    ethertype_handler fn;
+};
+static std::array<ethertype_entry, 8> eth_handlers;
+static 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};
+}
+
+void net_send_raw(std::span<const uint8_t> 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<int>(result.error()));
+    });
+}
+
+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<const uint8_t> frame, span_writer& tx) {
+    if (frame.size() < sizeof(eth::header)) return;
+    auto& eth_hdr = *reinterpret_cast<const eth::header*>(frame.data());
+
+    if (!mac_match(eth_hdr.dst)) return;
+
+    frame_callbacks.for_each([&](frame_cb_list::node* n) {
+        if (n->value(frame))
+            frame_callbacks.remove(n);
+    });
+
+    for (size_t i = 0; i < eth_handler_count; i++) {
+        if (eth_handlers[i].ethertype_be == eth_hdr.ethertype) {
+            eth_handlers[i].fn(frame, tx);
+            break;
+        }
+    }
+}
+
+bool net_init() {
+    w6300::init_spi();
+    w6300::reset();
+    w6300::init();
+    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];
+
+    state.ip[0] = 169;
+    state.ip[1] = 254;
+    state.ip[2] = state.mac[4];
+    state.ip[3] = state.mac[5];
+
+    w6300::open_socket(raw_socket, w6300::protocol::macraw, w6300::sock_flag::none);
+    w6300::set_interrupt_mask(w6300::ik_sock_0);
+
+    return true;
+}
+
+const net_state& net_get_state() {
+    return state;
+}
+
+frame_cb_handle net_add_frame_callback(net_frame_callback cb) {
+    auto h = frame_callbacks.insert(cb);
+    if (!h) dlog("frame callback alloc failed");
+    return h;
+}
+
+void net_remove_frame_callback(frame_cb_handle h) {
+    frame_callbacks.remove(h);
+}
+
+void net_poll(std::span<uint8_t> 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;
+        span_writer tx_writer(tx);
+        process_frame({rx_buf, *result}, tx_writer);
+    }
+    w6300::rearm_gpio_irq();
+}

src/test_handlers.cpp

@@ -0,0 +1,343 @@
+#include "test_handlers.h"
+#include <cstring>
+#include <unordered_map>
+#include "pico/stdlib.h"
+#include "pico/time.h"
+#include "handlers.h"
+#include "net.h"
+#include "icmp.h"
+#include "igmp.h"
+#include "udp.h"
+#include "parse_buffer.h"
+#include "prepend_buffer.h"
+
+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;
+};
+
+struct discovery_data {
+    void (*on_found)(const peer_info&) = nullptr;
+    void (*on_timeout)() = nullptr;
+};
+
+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;
+};
+
+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;
+};
+
+// One test runs at a time; in_flight gates that. active_* let shared
+// primitive callbacks find the running test's sub-state.
+struct test_state {
+    bool in_flight = false;
+    responder resp;
+    timer_handle timer = nullptr;
+    frame_cb_handle frame_cb = nullptr;
+
+    discovery_data* active_discovery = nullptr;
+    ping_rate_data* active_rate = nullptr;
+
+    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;
+}
+
+// When a callback fires, its dispatcher (net or timer_queue) has already
+// removed the node; the matching ts.timer / ts.frame_cb is stale. Callbacks
+// that self-consume must null that handle before calling test_end.
+
+static bool discover_reply_cb(std::span<const uint8_t> frame) {
+    parse_buffer pb(frame);
+    auto* eth_hdr = pb.consume<eth::header>();
+    if (!eth_hdr || eth_hdr->ethertype != eth::ETH_IPV4) return false;
+    auto* ip = pb.consume<ipv4::header>();
+    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<udp::header>();
+    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;
+    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(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, 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());
+}
+
+static bool igmp_report_cb(std::span<const uint8_t> frame) {
+    ipv4::ip4_addr group;
+    if (!igmp::parse_report(frame, group)) return false;
+    if (group != PICOMAP_DISCOVERY_GROUP) return false;
+    ts.frame_cb = nullptr;
+    test_end({true, {"got IGMP report for " + ipv4::to_string(group)}});
+    return true;
+}
+
+static void igmp_timeout_cb() {
+    ts.timer = nullptr;
+    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, 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());
+}
+
+static void info_found(const peer_info& peer) {
+    test_end({true, {"got info response from " + ipv4::to_string(peer.ip)}});
+}
+
+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);
+}
+
+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;
+    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;
+    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_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() { start_ping({169, 254, 255, 255}); }
+static void test_ping_global() { start_ping({255, 255, 255, 255}); }
+
+static size_t ping_rate_frame_size() {
+    return sizeof(eth::header) + sizeof(ipv4::header) + sizeof(icmp::echo)
+         + ts.active_rate->payload_len;
+}
+
+static void ping_rate_send_one() {
+    const auto& ns = net_get_state();
+    auto& r = *ts.active_rate;
+    prepend_buffer<4096> buf;
+    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());
+    r.sent++;
+}
+
+static bool ping_rate_reply_cb(std::span<const uint8_t> 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;
+
+    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<uint32_t>(
+            static_cast<uint64_t>(r.received) * 1000000 / elapsed_us);
+        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);
+        char msg[128];
+        snprintf(msg, sizeof(msg),
+                 "%u rt in %lu ms, %lu pps, %lu bytes, %lu KB/s",
+                 r.received, static_cast<unsigned long>(elapsed_ms),
+                 static_cast<unsigned long>(pps),
+                 static_cast<unsigned long>(total_bytes),
+                 static_cast<unsigned long>(kbps));
+        ts.frame_cb = nullptr;
+        test_end({true, {msg}});
+        return true;
+    }
+
+    if (r.sent < r.target)
+        ping_rate_send_one();
+    return false;
+}
+
+static void ping_rate_timeout_cb() {
+    ts.timer = nullptr;
+    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<unsigned long>(elapsed_us / 1000));
+    test_end({false, {msg}});
+}
+
+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();
+}
+
+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);
+}
+
+using sync_test_fn = ResponseTest (*)();
+using async_test_fn = void (*)();
+
+struct test_entry {
+    sync_test_fn sync;
+    async_test_fn async;
+};
+
+static const std::unordered_map<std::string_view, test_entry> 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}},
+};
+
+std::optional<ResponseListTests> handle_list_tests(const responder&, const RequestListTests&) {
+    ResponseListTests resp;
+    for (const auto& [name, _] : tests)
+        resp.names.emplace_back(name);
+    return resp;
+}
+
+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);
+    if (it == tests.end())
+        return ResponseTest{false, {"unknown test: " + req.name}};
+    if (it->second.sync)
+        return it->second.sync();
+
+    ts.in_flight = true;
+    ts.resp = resp;
+    it->second.async();
+    return std::nullopt;
+}

src/udp.cpp

@@ -0,0 +1,54 @@
+#include "udp.h"
+#include <array>
+#include "eth.h"
+#include "ipv4.h"
+#include "net.h"
+#include "parse_buffer.h"
+
+namespace udp {
+
+struct port_entry {
+    uint16_t port_be;
+    port_handler fn;
+};
+static std::array<port_entry, 8> port_handlers;
+static size_t port_handler_count = 0;
+
+void register_port(uint16_t port_be, port_handler fn) {
+    if (port_handler_count < port_handlers.size())
+        port_handlers[port_handler_count++] = {port_be, fn};
+}
+
+void handle(std::span<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;
+    if (!ipv4::addressed_to_us(ip->dst)) return;
+
+    size_t options_len = ip->header_len() - sizeof(ipv4::header);
+    if (options_len > 0 && !pb.skip(options_len)) return;
+
+    auto* uhdr = pb.consume<header>();
+    if (!uhdr) 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;
+
+    for (size_t i = 0; i < port_handler_count; i++) {
+        if (port_handlers[i].port_be == uhdr->dst_port) {
+            address from{eth_hdr->src, ip->src, uhdr->src_port};
+            port_handlers[i].fn(pb.remaining().subspan(0, payload_len), from);
+            break;
+        }
+    }
+}
+
+__attribute__((constructor))
+static void register_protocol() {
+    ipv4::register_protocol(17, handle);
+}
+
+} // namespace udp