114 lines
3.6 KiB
C++
114 lines
3.6 KiB
C++
#include "dispatch.h"
|
|
#include <unordered_map>
|
|
#include "pico/stdlib.h"
|
|
#include "tusb.h"
|
|
#include "wire.h"
|
|
#include "usb_cdc.h"
|
|
#include "timer_queue.h"
|
|
#include "net.h"
|
|
#include "igmp.h"
|
|
#include "debug_log.h"
|
|
#include "hardware/sync.h"
|
|
|
|
static timer_queue timers;
|
|
|
|
static void igmp_reannounce() {
|
|
auto& ns = net_get_state();
|
|
igmp::send_all_reports(ns.mac, ns.ip, net_send_raw);
|
|
dispatch_schedule_ms(60000, igmp_reannounce);
|
|
}
|
|
|
|
void dispatch_init() {
|
|
tusb_init();
|
|
net_init();
|
|
dispatch_schedule_ms(60000, igmp_reannounce);
|
|
dlog("dispatch_init complete");
|
|
}
|
|
|
|
timer_handle dispatch_schedule_ms(uint32_t ms, std::function<void()> fn) {
|
|
auto h = timers.schedule_ms(ms, std::move(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) {
|
|
std::unordered_map<int8_t, handler_fn> handler_map;
|
|
for (auto& entry : handlers) {
|
|
handler_map[entry.type_id] = entry.handle;
|
|
}
|
|
|
|
static usb_cdc usb;
|
|
static static_vector<uint8_t, 4096> usb_rx_buf;
|
|
static std::array<uint8_t, 4096> tx_buf;
|
|
|
|
auto dispatch_msg = [&](const DecodedMessage& msg, send_fn send) {
|
|
auto it = handler_map.find(msg.type_id);
|
|
if (it == handler_map.end()) {
|
|
dlogf("dispatch: unknown type_id %d", msg.type_id);
|
|
return;
|
|
}
|
|
responder resp{msg.message_id, std::move(send)};
|
|
it->second(resp, msg.payload);
|
|
};
|
|
|
|
net_set_handler([&](std::span<const uint8_t> payload,
|
|
send_fn send) {
|
|
auto msg = try_decode(payload.data(), payload.size());
|
|
if (!msg) return;
|
|
dispatch_msg(*msg, std::move(send));
|
|
});
|
|
|
|
while (true) {
|
|
uint32_t save = save_and_disable_interrupts();
|
|
|
|
dlog_if_slow("tud_task", 1000, [&]{ tud_task(); });
|
|
dlog_if_slow("drain", 1000, [&]{ usb.drain(); });
|
|
dlog_if_slow("timers", 1000, [&]{ timers.run(); });
|
|
dlog_if_slow("net_poll", 1000, [&]{ net_poll(std::span{tx_buf}); });
|
|
|
|
while (tud_cdc_available()) {
|
|
uint8_t byte;
|
|
if (tud_cdc_read(&byte, 1) != 1) break;
|
|
|
|
usb_rx_buf.push_back(byte);
|
|
|
|
auto msg = try_decode(usb_rx_buf);
|
|
if (!msg) {
|
|
if (usb_rx_buf.full()) usb_rx_buf.clear();
|
|
continue;
|
|
}
|
|
|
|
dispatch_msg(*msg, [&](encode_fn encode) {
|
|
uint8_t buf[4096];
|
|
span_writer out(buf, sizeof(buf));
|
|
auto r = encode(out);
|
|
if (!r) return;
|
|
std::span<const uint8_t> data{buf, *r};
|
|
if (data.size() <= usb.tx.free()) {
|
|
if (!usb.send(data))
|
|
dlogf("usb send failed: %zu bytes, %u free", data.size(), usb.tx.free());
|
|
} else {
|
|
dlogf("usb response too large: %zu bytes, %u free", data.size(), usb.tx.free());
|
|
uint8_t err_buf[256];
|
|
span_writer err_out(err_buf, sizeof(err_buf));
|
|
auto err = encode_response_into(err_out, msg->message_id,
|
|
[&]{
|
|
char m[48];
|
|
snprintf(m, sizeof(m), "response too large: %zu", data.size());
|
|
return DeviceError{2, m};
|
|
}());
|
|
if (err) usb.send(std::span<const uint8_t>{err_buf, *err});
|
|
}
|
|
});
|
|
usb_rx_buf.clear();
|
|
}
|
|
|
|
__wfi();
|
|
restore_interrupts(save);
|
|
}
|
|
}
|