Add firmware

2026-02-06 19:45:57 +01:00
parent eca8d2532f
commit 05e7398731
17 changed files with 1298 additions and 0 deletions
--- a/firmware/base/tusb/tusb_stdio_driver.c
+++ b/firmware/base/tusb/tusb_stdio_driver.c
@@ -0,0 +1,130 @@
+#include "tusb_stdio_driver.h"
+#include "tusb.h"
+
+#include "class/cdc/cdc_device.h"
+#include "device/usbd.h"
+#include "pico/sem.h"
+#include "pico/stdio/driver.h"
+#include "pico/time.h"
+
+
+#include "hardware/irq.h"
+#include "hardware/timer.h"
+
+#define STDIO_DUAL_USB_STDOUT_TIMEOUT_US 10000
+
+semaphore_t stdio_dual_usb_mutex;
+static uint8_t low_priority_irq_num;
+
+bool stdio_dual_usb_connected() {
+  const int itf = get_core_num();
+  return tud_cdc_n_connected(itf);
+}
+
+static void low_priority_irq_worker(void) {
+  if (sem_try_acquire(&stdio_dual_usb_mutex)) {
+    tud_task();
+    sem_release(&stdio_dual_usb_mutex);
+  }
+}
+
+static void stdio_dual_usb_out_chars(const char *buf, int length) {
+  static uint64_t last_avail_time[2] = {};
+  const int itf = get_core_num();
+  // if (!sem_acquire_timeout_ms(&stdio_dual_usb_mutex, 500)) {
+  //   return;
+  // }
+  sem_acquire_blocking(&stdio_dual_usb_mutex);
+  if (stdio_dual_usb_connected()) {
+    for (int i = 0; i < length;) {
+      int n = length - i;
+      int avail = (int)tud_cdc_n_write_available(itf);
+      if (n > avail)
+        n = avail;
+      if (n) {
+        int n2 = (int)tud_cdc_n_write(itf, buf + i, (uint32_t)n);
+        tud_task();
+        tud_cdc_n_write_flush(itf);
+        i += n2;
+        last_avail_time[itf] = time_us_64();
+      } else {
+        tud_task();
+        tud_cdc_n_write_flush(itf);
+        if (!stdio_dual_usb_connected() ||
+            (!tud_cdc_write_available() &&
+             time_us_64() >
+                 last_avail_time[itf] + STDIO_DUAL_USB_STDOUT_TIMEOUT_US)) {
+          break;
+        }
+      }
+    }
+  } else {
+    last_avail_time[itf] = 0;
+    tud_task();
+  }
+  sem_release(&stdio_dual_usb_mutex);
+}
+
+static void stdio_dual_usb_out_flush(void) {
+  const int itf = get_core_num();
+  // if (!sem_acquire_timeout_ms(&stdio_dual_usb_mutex, 500)) {
+  //   return;
+  // }
+  sem_acquire_blocking(&stdio_dual_usb_mutex);
+  int last_time = time_us_64();
+  do {
+    tud_task();
+  } while (tud_cdc_n_write_flush(itf) && time_us_64() > last_time + STDIO_DUAL_USB_STDOUT_TIMEOUT_US);
+  sem_release(&stdio_dual_usb_mutex);
+}
+
+int stdio_dual_usb_in_chars(char *buf, int length) {
+  const int itf = get_core_num();
+  int rc = PICO_ERROR_NO_DATA;
+  // if (!sem_acquire_timeout_ms(&stdio_dual_usb_mutex, 500)) {
+  //   return rc;
+  // }
+  sem_acquire_blocking(&stdio_dual_usb_mutex);
+  if (stdio_dual_usb_connected() && tud_cdc_n_available(itf)) {
+    int count = (int)tud_cdc_n_read(itf, buf, (uint32_t)length);
+    rc = count ? count : PICO_ERROR_NO_DATA;
+  } else {
+    tud_task();
+  }
+  sem_release(&stdio_dual_usb_mutex);
+  return rc;
+}
+
+void irq_handler() { irq_set_pending(low_priority_irq_num); }
+
+stdio_driver_t stdio_dual_usb = {
+    .out_chars = stdio_dual_usb_out_chars,
+    .out_flush = stdio_dual_usb_out_flush,
+    .in_chars = stdio_dual_usb_in_chars,
+    .crlf_enabled = PICO_STDIO_DEFAULT_CRLF,
+};
+
+bool stdio_dual_usb_init(void) {
+  if (get_core_num() != alarm_pool_core_num(alarm_pool_get_default())) {
+    assert(false);
+    return false;
+  }
+
+  assert(tud_inited());
+
+  sem_init(&stdio_dual_usb_mutex, 1,1);
+  // low_priority_irq_num = user_irq_claim_unused(true);
+  // irq_set_exclusive_handler(low_priority_irq_num, low_priority_irq_worker);
+  // irq_set_enabled(low_priority_irq_num, true);
+
+  // if (irq_has_handler(USBCTRL_IRQ)) {
+  //   irq_add_shared_handler(USBCTRL_IRQ, irq_handler,
+  //                          PICO_SHARED_IRQ_HANDLER_LOWEST_ORDER_PRIORITY);
+  // } else {
+  //   return false;
+  // }
+
+  stdio_set_driver_enabled(&stdio_dual_usb, true);
+
+  return true;
+}