312 lines
10 KiB
C++
312 lines
10 KiB
C++
#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);
|
|
net_send_raw(buf.span());
|
|
|
|
ipv4::ip4_addr our_ip = ns.ip;
|
|
|
|
auto timer = std::make_shared<timer_handle>(nullptr);
|
|
auto cb = std::make_shared<std::function<void(std::span<const uint8_t>)>>();
|
|
*cb = [our_ip, timer, cb, on_found = std::move(on_found)](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) {
|
|
net_add_frame_callback(*cb);
|
|
return;
|
|
}
|
|
auto* ip = pb.consume<ipv4::header>();
|
|
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<udp::header>();
|
|
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<4096> buf;
|
|
igmp::prepend_query(buf, ns.mac, ns.ip, igmp::PICOMAP_DISCOVERY_GROUP);
|
|
net_send_raw(buf.span());
|
|
|
|
auto timer = std::make_shared<timer_handle>(nullptr);
|
|
auto cb = std::make_shared<std::function<void(std::span<const uint8_t>)>>();
|
|
*cb = [resp, timer, cb](std::span<const uint8_t> frame) {
|
|
ipv4::ip4_addr group;
|
|
if (!igmp::parse_report(frame, group)) {
|
|
net_add_frame_callback(*cb);
|
|
return;
|
|
}
|
|
if (group != igmp::PICOMAP_DISCOVERY_GROUP) {
|
|
net_add_frame_callback(*cb);
|
|
return;
|
|
}
|
|
dispatch_cancel_timer(*timer);
|
|
resp.respond(ResponseTest{true, {"got IGMP report for " + ipv4::to_string(group)}});
|
|
};
|
|
net_add_frame_callback(*cb);
|
|
|
|
*timer = dispatch_schedule_ms(5000, [resp]() {
|
|
resp.respond(ResponseTest{false, {"no IGMP report within 5s"}});
|
|
});
|
|
}
|
|
|
|
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);
|
|
net_send_raw(buf.span());
|
|
|
|
ipv4::ip4_addr our_ip = ns.ip;
|
|
|
|
auto timer = std::make_shared<timer_handle>(nullptr);
|
|
auto cb = std::make_shared<std::function<void(std::span<const uint8_t>)>>();
|
|
*cb = [resp, ping_id, our_ip, timer, cb](std::span<const uint8_t> frame) {
|
|
ipv4::ip4_addr src_ip;
|
|
if (!icmp::parse_echo_reply(frame, src_ip, ping_id)) {
|
|
net_add_frame_callback(*cb);
|
|
return;
|
|
}
|
|
dispatch_cancel_timer(*timer);
|
|
if (src_ip == our_ip) {
|
|
resp.respond(ResponseTest{false, {"got reply from self: " + ipv4::to_string(src_ip)}});
|
|
return;
|
|
}
|
|
resp.respond(ResponseTest{true, {"reply from " + ipv4::to_string(src_ip)}});
|
|
};
|
|
net_add_frame_callback(*cb);
|
|
|
|
*timer = dispatch_schedule_ms(5000, [resp]() {
|
|
resp.respond(ResponseTest{false, {"no reply from non-self host within 5s"}});
|
|
});
|
|
}
|
|
|
|
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;
|
|
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 ping_rate_recv(std::shared_ptr<ping_rate_state> st,
|
|
std::shared_ptr<std::function<void(std::span<const uint8_t>)>> cb,
|
|
std::span<const uint8_t> 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->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;
|
|
}
|
|
|
|
if (st->sent < st->target)
|
|
ping_rate_send_one(st);
|
|
net_add_frame_callback(*cb);
|
|
}
|
|
|
|
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();
|
|
|
|
auto cb = std::make_shared<std::function<void(std::span<const uint8_t>)>>();
|
|
*cb = [st, cb](std::span<const uint8_t> frame) {
|
|
ping_rate_recv(st, cb, frame);
|
|
};
|
|
|
|
for (uint16_t i = 0; i < st->pipeline && st->sent < st->target; i++)
|
|
ping_rate_send_one(st);
|
|
net_add_frame_callback(*cb);
|
|
|
|
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/%u rt in %lu ms",
|
|
st->received, st->sent,
|
|
static_cast<unsigned long>(elapsed_us / 1000));
|
|
st->resp.respond(ResponseTest{false, {msg}});
|
|
});
|
|
},
|
|
[resp]() {
|
|
resp.respond(ResponseTest{false, {"no peer found"}});
|
|
});
|
|
}
|
|
|
|
static void test_ping_rate(const responder& resp) {
|
|
start_ping_rate(resp, 8192, 0, 1);
|
|
}
|
|
|
|
static void test_ping_rate_1k(const responder& resp) {
|
|
start_ping_rate(resp, 1024, 1024, 2);
|
|
}
|
|
|
|
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}},
|
|
{"ping_rate", {nullptr, test_ping_rate}},
|
|
{"ping_rate_1k", {nullptr, test_ping_rate_1k}},
|
|
};
|
|
|
|
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;
|
|
}
|