Add dispatch loop, copy_to_ram, retry picotool load without sleep

firmware/lib/dispatch.cpp

@@ -0,0 +1,50 @@
+#include "dispatch.h"
+#include <unordered_map>
+#include "pico/stdlib.h"
+#include "tusb.h"
+#include "wire.h"
+#include "timer_queue.h"
+#include "net.h"
+
+[[noreturn]] void dispatch(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;
+    }
+
+    tusb_init();
+    net_init();
+
+    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();
+    }
+}