picomap/firmware/lib/dispatch.cpp

#include "dispatch.h"
#include <unordered_map>
#include "pico/stdlib.h"
#include "tusb.h"
#include "wire.h"
#include "timer_queue.h"
#include "net.h"

void dispatch_init() {
    tusb_init();
    net_init();
}

[[noreturn]] void dispatch_run(std::span<const handler_entry> handlers) {
    std::unordered_map<int8_t, void (*)(usb_cdc&, uint32_t)> handler_map;
    for (auto& entry : handlers) {
        handler_map[entry.type_id] = entry.handle;
    }

    static usb_cdc usb;
    static timer_queue timers;
    static static_vector<uint8_t, 256> rx_buf;

    while (true) {
        tud_task();

        usb.drain();
        timers.run();

        while (tud_cdc_available()) {
            uint8_t byte;
            if (tud_cdc_read(&byte, 1) != 1) break;

            rx_buf.push_back(byte);

            auto msg = try_decode(rx_buf);
            if (!msg) {
                if (rx_buf.full()) rx_buf.clear();
                continue;
            }

            rx_buf.clear();

            auto it = handler_map.find(msg->type_id);
            if (it != handler_map.end()) {
                it->second(usb, msg->message_id);
            }
        }

        __wfi();
    }
}