pbio/drv/usb: Hook up receive handler and status.

laurensvalk · laurensvalk · commit 204f5cb57062 · 2025-09-23T10:29:45.000+02:00
diff --git a/lib/pbio/drv/usb/usb_ev3.c b/lib/pbio/drv/usb/usb_ev3.c
@@ -879,8 +879,31 @@ static void usb_device_intr(void) {
     HWREG(USB_0_OTGBASE + USB_0_END_OF_INTR) = 0;
 }
 
+/**
+ * Pybricks system command handler.
+ */
+static pbdrv_usb_receive_handler_t pbdrv_usb_receive_handler;
+
+void pbdrv_usb_set_receive_handler(pbdrv_usb_receive_handler_t handler) {
+    pbdrv_usb_receive_handler = handler;
+}
+
+/**
+ * Buffer for scheduled status message.
+ */
+static uint8_t pbdrv_usb_status_data[PBIO_PYBRICKS_EVENT_STATUS_REPORT_SIZE];
+static bool pbdrv_usb_status_data_pending;
+
 void pbdrv_usb_schedule_status_update(const uint8_t *status_msg) {
-    // todo
+    // Ignore if message identical to last.
+    if (!memcmp(pbdrv_usb_status_data, status_msg, sizeof(pbdrv_usb_status_data))) {
+        return;
+    }
+
+    // Schedule to send whenever the USB process gets round to it.
+    memcpy(pbdrv_usb_status_data, status_msg, sizeof(pbdrv_usb_status_data));
+    pbdrv_usb_status_data_pending = true;
+    pbio_os_request_poll();
 }
 
 static pbio_os_process_t pbdrv_usb_ev3_process;
@@ -891,16 +914,14 @@ static pbio_error_t pbdrv_usb_ev3_process_thread(pbio_os_state_t *state, void *c
     static pbio_os_timer_t keepalive_timer;
     static bool was_transmitting = false;
     static bool is_transmitting = false;
-    static uint32_t prev_status_flags = ~0;
-    static uint32_t new_status_flags = 0;
 
     PBIO_OS_ASYNC_BEGIN(state);
 
     for (;;) {
         if (pbdrv_usb_config == 0) {
             pbdrv_usb_is_events_subscribed = false;
             // Resend status flags when host subscribes
-            prev_status_flags = ~0;
+            pbdrv_usb_status_data_pending = true;
         }
 
         if (usb_rx_is_ready) {
@@ -925,7 +946,10 @@ static pbio_error_t pbdrv_usb_ev3_process_thread(pbio_os_state_t *state, void *c
                         usb_send_response = true;
                         break;
                     case PBIO_PYBRICKS_OUT_EP_MSG_COMMAND:
-                        result = pbsys_command(ep1_rx_buf + 1, usb_rx_sz - 1);
+                        if (!pbdrv_usb_receive_handler) {
+                            break;
+                        }
+                        result = pbdrv_usb_receive_handler(ep1_rx_buf + 1, usb_rx_sz - 1);
                         ep1_tx_response_buf[0] = PBIO_PYBRICKS_IN_EP_MSG_RESPONSE;
                         pbio_set_uint32_le(&ep1_tx_response_buf[1], result);
                         usb_send_response = true;
@@ -945,26 +969,24 @@ static pbio_error_t pbdrv_usb_ev3_process_thread(pbio_os_state_t *state, void *c
         }
 
         // Send status flags if they've changed (and we can)
-        new_status_flags = pbsys_status_get_flags();
         if (pbdrv_usb_is_events_subscribed && !usb_tx_status_is_not_ready &&
-            (new_status_flags != prev_status_flags || pbio_os_timer_is_expired(&keepalive_timer))) {
+            (pbdrv_usb_status_data_pending || pbio_os_timer_is_expired(&keepalive_timer))) {
             ep1_tx_status_buf[0] = PBIO_PYBRICKS_IN_EP_MSG_EVENT;
             uint32_t usb_status_sz = PBIO_PYBRICKS_USB_MESSAGE_SIZE(pbsys_status_get_status_report(&ep1_tx_status_buf[1]));
 
             usb_tx_status_is_not_ready = true;
             pbdrv_cache_prepare_before_dma(ep1_tx_status_buf, sizeof(ep1_tx_status_buf));
             usb_setup_tx_dma_desc(CPPI_DESC_TX_STATUS, ep1_tx_status_buf, usb_status_sz);
 
-            prev_status_flags = new_status_flags;
-
-            if (new_status_flags != prev_status_flags) {
+            if (pbdrv_usb_status_data_pending) {
                 // If we are sending a status because the flags have changed,
                 // we can bump out the keepalive timer.
                 pbio_os_timer_set(&keepalive_timer, 1000);
             } else {
                 // Otherwise, we want to send keepalives at a particular rate.
                 pbio_os_timer_extend(&keepalive_timer);
             }
+            pbdrv_usb_status_data_pending = false;
         }
 
         // Handle timeouts
@@ -985,7 +1007,7 @@ static pbio_error_t pbdrv_usb_ev3_process_thread(pbio_os_state_t *state, void *c
                 usb_tx_stdout_is_not_ready = false;
                 pbdrv_usb_is_events_subscribed = false;
                 // Resend status flags when host subscribes
-                prev_status_flags = ~0;
+                pbdrv_usb_status_data_pending = true;
             }
         } else if (!was_transmitting && is_transmitting) {
             pbio_os_timer_set(&tx_timeout_timer, 50);
diff --git a/lib/pbio/drv/usb/usb_nxt.c b/lib/pbio/drv/usb/usb_nxt.c
@@ -14,6 +14,8 @@
 #include <string.h>
 
 #include <pbdrv/bluetooth.h>
+#include <pbdrv/usb.h>
+
 #include <pbio/protocol.h>
 #include <pbio/version.h>
 #include <pbsys/config.h>
@@ -887,6 +889,16 @@ void pbdrv_usb_schedule_status_update(const uint8_t *status_msg) {
     // todo
 }
 
+uint32_t pbdrv_usb_stdout_tx_available(void) {
+    return UINT32_MAX;
+}
+
+static pbdrv_usb_receive_handler_t pbdrv_usb_receive_handler;
+
+void pbdrv_usb_set_receive_handler(pbdrv_usb_receive_handler_t handler) {
+    pbdrv_usb_receive_handler = handler;
+}
+
 void nx_usb_read(uint8_t *data, uint32_t length) {
     pbdrv_usb_nxt_state.rx_data = data;
     pbdrv_usb_nxt_state.rx_size = length;
diff --git a/lib/pbio/drv/usb/usb_stm32.c b/lib/pbio/drv/usb/usb_stm32.c
@@ -389,8 +389,31 @@ pbdrv_usb_bcd_t pbdrv_usb_get_bcd(void) {
     return pbdrv_usb_bcd;
 }
 
+/**
+ * Pybricks system command handler.
+ */
+static pbdrv_usb_receive_handler_t pbdrv_usb_receive_handler;
+
+void pbdrv_usb_set_receive_handler(pbdrv_usb_receive_handler_t handler) {
+    pbdrv_usb_receive_handler = handler;
+}
+
+/**
+ * Buffer for scheduled status message.
+ */
+static uint8_t pbdrv_usb_status_data[PBIO_PYBRICKS_EVENT_STATUS_REPORT_SIZE];
+static bool pbdrv_usb_status_data_pending;
+
 void pbdrv_usb_schedule_status_update(const uint8_t *status_msg) {
-    // todo
+    // Ignore if message identical to last.
+    if (!memcmp(pbdrv_usb_status_data, status_msg, sizeof(pbdrv_usb_status_data))) {
+        return;
+    }
+
+    // Schedule to send whenever the Bluetooth process gets round to it.
+    memcpy(pbdrv_usb_status_data, status_msg, sizeof(pbdrv_usb_status_data));
+    pbdrv_usb_status_data_pending = true;
+    process_poll(&pbdrv_usb_process);
 }
 
 // Event loop
@@ -404,8 +427,6 @@ PROCESS_THREAD(pbdrv_usb_process, ev, data) {
     static pbio_pybricks_error_t result;
     static struct etimer status_timer;
     static struct etimer transmit_timer;
-    static uint32_t prev_status_flags = ~0;
-    static uint32_t new_status_flags;
 
     PROCESS_POLLHANDLER({
         if (!bcd_busy && pbio_oneshot(!vbus_active, &no_vbus_oneshot)) {
@@ -463,8 +484,8 @@ PROCESS_THREAD(pbdrv_usb_process, ev, data) {
                     usb_response_sz = sizeof(usb_response_buf);
                     break;
                 case PBIO_PYBRICKS_OUT_EP_MSG_COMMAND:
-                    if (usb_response_sz == 0) {
-                        result = pbsys_command(usb_in_buf + 1, usb_in_sz - 1);
+                    if (usb_response_sz == 0 && pbdrv_usb_receive_handler) {
+                        result = pbdrv_usb_receive_handler(usb_in_buf + 1, usb_in_sz - 1);
                         usb_response_buf[0] = PBIO_PYBRICKS_IN_EP_MSG_RESPONSE;
                         pbio_set_uint32_le(&usb_response_buf[1], result);
                         usb_response_sz = sizeof(usb_response_buf);
@@ -488,25 +509,26 @@ PROCESS_THREAD(pbdrv_usb_process, ev, data) {
             continue;
         }
 
-        new_status_flags = pbsys_status_get_flags();
-
         // Transmit. Give priority to response, then status updates, then stdout.
         if (usb_response_sz) {
             transmitting = true;
             USBD_Pybricks_TransmitPacket(&husbd, usb_response_buf, usb_response_sz);
         } else if (pbdrv_usb_stm32_is_events_subscribed) {
-            if ((new_status_flags != prev_status_flags) || etimer_expired(&status_timer)) {
+            if (pbdrv_usb_status_data_pending || etimer_expired(&status_timer)) {
+                pbdrv_usb_status_data_pending = false;
+
                 usb_status_buf[0] = PBIO_PYBRICKS_IN_EP_MSG_EVENT;
                 _Static_assert(sizeof(usb_status_buf) + 1 >= PBIO_PYBRICKS_EVENT_STATUS_REPORT_SIZE,
                     "size of status report does not match size of event");
-                usb_status_sz = PBIO_PYBRICKS_USB_MESSAGE_SIZE(pbsys_status_get_status_report(&usb_status_buf[1]));
+
+                memcpy(&usb_status_buf[1], pbdrv_usb_status_data, sizeof(pbdrv_usb_status_data));
+                usb_status_sz = PBIO_PYBRICKS_USB_MESSAGE_SIZE(sizeof(pbdrv_usb_status_data));
 
                 // REVISIT: we really shouldn't need a status timer on USB since
                 // it's not a lossy transport. We just need to make sure we send
                 // status updates when they change and send the current status
                 // immediately after subscribing to events.
                 etimer_restart(&status_timer);
-                prev_status_flags = new_status_flags;
 
                 transmitting = true;
                 USBD_Pybricks_TransmitPacket(&husbd, usb_status_buf, usb_status_sz);
diff --git a/lib/pbio/include/pbdrv/usb.h b/lib/pbio/include/pbdrv/usb.h
@@ -10,9 +10,11 @@
 #define _PBDRV_USB_H_
 
 #include <stdbool.h>
+#include <stdint.h>
 
 #include <pbdrv/config.h>
 #include <pbio/error.h>
+#include <pbio/protocol.h>
 
 /**
  * Indicates battery charging capabilites that were detected on a USB port.
@@ -32,6 +34,14 @@ typedef enum {
     PBDRV_USB_BCD_DEDICATED_CHARGING = 4,
 } pbdrv_usb_bcd_t;
 
+/**
+ * Callback that is called when receiving a Pybricks command.
+ *
+ * @param [in]  data        The data that was received.
+ * @param [in]  size        The size of @p data in bytes.
+ */
+typedef pbio_pybricks_error_t (*pbdrv_usb_receive_handler_t)(const uint8_t *data, uint32_t size);
+
 #if PBDRV_CONFIG_USB
 
 /**
@@ -40,6 +50,13 @@ typedef enum {
  */
 pbdrv_usb_bcd_t pbdrv_usb_get_bcd(void);
 
+/**
+ * Registers a callback that will be called when Pybricks data is received.
+ *
+ * @param [in]  handler     The function that will be called.
+ */
+void pbdrv_usb_set_receive_handler(pbdrv_usb_receive_handler_t handler);
+
 /**
  * Schedules Pybricks status to be sent soon.
  *
@@ -82,6 +99,9 @@ static inline pbdrv_usb_bcd_t pbdrv_usb_get_bcd(void) {
     return PBDRV_USB_BCD_NONE;
 }
 
+static inline void pbdrv_usb_set_receive_handler(pbdrv_usb_receive_handler_t handler) {
+}
+
 static inline void pbdrv_usb_schedule_status_update(const uint8_t *status_msg) {
 }
 
diff --git a/lib/pbio/sys/host.c b/lib/pbio/sys/host.c
@@ -9,17 +9,20 @@
 
 #include <pbdrv/bluetooth.h>
 #include <pbdrv/usb.h>
-#include <pbsys/host.h>
 
 #include "bluetooth.h"
+#include <pbsys/command.h>
+#include <pbsys/host.h>
 
 static pbsys_host_stdin_event_callback_t pbsys_host_stdin_event_callback;
 static lwrb_t pbsys_host_stdin_ring_buf;
 
 void pbsys_host_init(void) {
     static uint8_t stdin_buf[PBSYS_CONFIG_HOST_STDIN_BUF_SIZE];
     lwrb_init(&pbsys_host_stdin_ring_buf, stdin_buf, PBIO_ARRAY_SIZE(stdin_buf));
+
     pbsys_bluetooth_init();
+    pbdrv_usb_set_receive_handler(pbsys_command);
 }
 
 /**
