picomap/firmware/lib/test_handlers.cpp

#include "test_handlers.h"
#include <cstring>
#include <memory>
#include <unordered_map>
#include "pico/stdlib.h"
#include "pico/time.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 PICOMAP_PORT = __builtin_bswap16(28781);

struct peer_info {
    eth::mac_addr mac;
    ipv4::ip4_addr ip;
};

using peer_callback = std::function<void(const peer_info&)>;
using fail_callback = std::function<void()>;

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);

    ipv4::ip4_addr our_ip = ns.ip;

    auto timer = std::make_shared<timer_handle>(nullptr);
    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);
        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) 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();
    });

    net_send_raw(buf.span());
}

static void test_discovery_igmp(const responder& resp) {
    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<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;
        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"}});
    });

    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"}});
        });
}

static void test_ping(const responder& resp, ipv4::ip4_addr dst_ip) {
    auto& ns = net_get_state();
    uint16_t ping_id = 0x1234;

    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<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());
}

static void test_ping_subnet(const responder& resp) {
    test_ping(resp, {169, 254, 255, 255});
}

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<ping_rate_state> st) {
    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);
    net_send_raw(buf.span());
    st->sent++;
}

static void start_ping_rate(const responder& resp, uint16_t target,
                             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;
                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<uint32_t>(
                        static_cast<uint64_t>(st->received) * 1000000 / elapsed_us);
                    uint64_t total_bytes = static_cast<uint64_t>(st->received) * 2 * st->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",
                             st->received, static_cast<unsigned long>(elapsed_ms),
                             static_cast<unsigned long>(pps),
                             static_cast<unsigned long>(total_bytes),
                             static_cast<unsigned long>(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<unsigned long>(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"}});
        });
}

static void test_packet_rate(const responder& resp) {
    start_ping_rate(resp, 8192, 0, 8);
}

static void test_byte_rate(const responder& resp) {
    start_ping_rate(resp, 2048, 1400, 8);
}

using sync_test_fn = ResponseTest (*)(const responder&);
using async_test_fn = void (*)(const responder&);

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) {
    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 std::nullopt;
}