picomap/firmware/test.cpp

#include <unordered_map>
#include "pico/stdlib.h"
#include "pico/time.h"
#include "hardware/gpio.h"
#include "dispatch.h"
#include "handlers.h"
#include "w6300.h"

static constexpr uint8_t LED_PIN = 25;

static void led_toggle() {
    gpio_xor_mask(1 << LED_PIN);
    dispatch_schedule_ms(1000, led_toggle);
}

std::string_view firmware_name = "picomap_test";

static constexpr uint16_t PICOMAP_DISCOVERY_PORT = 28777;

static constexpr std::array<uint8_t, 4> picomap_discovery_ip = {239, 0, 112, 109};

static constexpr std::array<uint8_t, 6> picomap_discovery_mac = {
    0x01, 0x00, 0x5e, 0x00, 0x70, 0x6d,
};

static w6300::socket_id test_socket{1};

static ResponseTest test_discovery() {
    ResponseTest resp;
    resp.pass = true;

    w6300::ip_address dest = {};
    std::copy(picomap_discovery_ip.begin(), picomap_discovery_ip.end(), dest.ip.begin());
    dest.len = 4;

    w6300::set_socket_dest_mac(test_socket, picomap_discovery_mac);

    auto req = encode_request(0, RequestInfo{});
    auto send_result = w6300::sendto(test_socket, std::span<const uint8_t>{req}, dest,
                                     w6300::port_num{PICOMAP_DISCOVERY_PORT});
    if (!send_result) {
        resp.pass = false;
        resp.messages.push_back("sendto: error " + std::to_string(static_cast<int>(send_result.error())));
        return resp;
    }

    uint8_t rx_buf[512];
    w6300::ip_address src_addr = {};
    w6300::port_num src_port{0};

    auto deadline = make_timeout_time_ms(5000);
    std::expected<uint16_t, w6300::sock_error> recv_result = std::unexpected(w6300::sock_error::busy);
    while (get_absolute_time() < deadline) {
        recv_result = w6300::recvfrom(test_socket, std::span{rx_buf}, src_addr, src_port);
        if (recv_result || recv_result.error() != w6300::sock_error::busy) break;
    }

    if (!recv_result) {
        resp.pass = false;
        if (recv_result.error() == w6300::sock_error::busy) {
            resp.messages.push_back("recvfrom: timed out after 5s");
        } else {
            resp.messages.push_back("recvfrom: error " + std::to_string(static_cast<int>(recv_result.error())));
        }
        return resp;
    }

    resp.messages.push_back("received " + std::to_string(*recv_result) + " bytes from port " +
                            std::to_string(static_cast<uint16_t>(src_port)));

    auto info = decode_response<ResponseInfo>(rx_buf, *recv_result);
    if (!info) {
        resp.pass = false;
        resp.messages.push_back("decode: msgpack error " + std::to_string(static_cast<int>(info.error())));
        return resp;
    }

    if (info->firmware_name.empty()) {
        resp.pass = false;
        resp.messages.push_back("firmware_name is empty");
    } else {
        resp.messages.push_back("firmware_name: " + info->firmware_name);
    }

    bool mac_zero = true;
    for (auto b : info->mac) { if (b != 0) { mac_zero = false; break; } }
    if (mac_zero) {
        resp.pass = false;
        resp.messages.push_back("mac is all zeros");
    }

    bool ip_zero = true;
    for (auto b : info->ip) { if (b != 0) { ip_zero = false; break; } }
    if (ip_zero) {
        resp.pass = false;
        resp.messages.push_back("ip is all zeros");
    }

    return resp;
}

using test_fn = ResponseTest (*)();

static const std::unordered_map<std::string_view, test_fn> tests = {
    {"discovery", test_discovery},
};

static std::vector<std::vector<uint8_t>> handle_test(uint32_t message_id, const RequestTest& req) {
    auto it = tests.find(req.name);
    if (it == tests.end()) {
        return {encode_response(message_id, ResponseTest{false, {"unknown test: " + req.name}})};
    }

    return {encode_response(message_id, it->second())};
}

static constexpr handler_entry handlers[] = {
    {RequestPICOBOOT::ext_id, handle_picoboot},
    {RequestInfo::ext_id, handle_info},
    {RequestTest::ext_id, typed_handler<RequestTest, handle_test>},
};

int main() {
    dispatch_init();

    gpio_init(LED_PIN);
    gpio_set_dir(LED_PIN, GPIO_OUT);
    dispatch_schedule_ms(1000, led_toggle);

    dispatch_run(handlers);
}