pbio/drv/usb: Add common USB driver.

laurensvalk · laurensvalk · commit 9aef65ba5159 · 2025-11-06T13:12:11.000+01:00
This de-duplicates common USB code such as handling stdout or sending the hub status. This was quite hard to keep track of between the EV3 and STM32 drivers, causing subtle differences and issues which were not so easy to synchronize.

The simulation driver shows the bare minimum driver-specific functions that each driver must implement, which will we do in the next commits.
diff --git a/lib/pbio/drv/usb/usb.c b/lib/pbio/drv/usb/usb.c
@@ -17,33 +17,208 @@
 #include <pbio/error.h>
 #include <pbio/os.h>
 
+#include <lwrb/lwrb.h>
+
+/**
+ * Host is subscribed to our outgoing event messages.
+ */
+static bool pbdrv_usb_events_subscribed;
+
+bool pbdrv_usb_connection_is_active(void) {
+    return pbdrv_usb_events_subscribed;
+}
+
+/**
+ * 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 scheduled status.
+ */
+static uint8_t status_data[PBIO_PYBRICKS_EVENT_STATUS_REPORT_SIZE];
+static bool status_data_pending;
+
 void pbdrv_usb_schedule_status_update(const uint8_t *status_msg) {
+    // Ignore if message identical to last.
+    if (!memcmp(status_data, status_msg, sizeof(status_data))) {
+        return;
+    }
+
+    // Schedule to send whenever the Bluetooth process gets round to it.
+    memcpy(status_data, status_msg, sizeof(status_data));
+    status_data_pending = true;
+    pbio_os_request_poll();
 }
 
+/**
+ * Buffer for scheduled stdout.
+ */
+static lwrb_t stdout_ring_buf;
+
 pbio_error_t pbdrv_usb_stdout_tx(const uint8_t *data, uint32_t *size) {
+
+    if (!pbdrv_usb_connection_is_active()) {
+        return PBIO_ERROR_INVALID_OP;
+    }
+
+    // Buffer data to send it more efficiently even if the caller is only
+    // writing one byte at a time.
+    if ((*size = lwrb_write(&stdout_ring_buf, data, *size)) == 0) {
+        return PBIO_ERROR_AGAIN;
+    }
+
+    // Poke the process to start tx soon-ish. This way, we can accumulate
+    // data bytes before actually transmitting.
+    pbio_os_request_poll();
+
     return PBIO_SUCCESS;
 }
 
 uint32_t pbdrv_usb_stdout_tx_available(void) {
-    return UINT32_MAX;
+    if (!pbdrv_usb_connection_is_active()) {
+        return UINT32_MAX;
+    }
+    return lwrb_get_free(&stdout_ring_buf);
 }
 
 bool pbdrv_usb_stdout_tx_is_idle(void) {
-    return true;
+    if (!pbdrv_usb_connection_is_active()) {
+        return true;
+    }
+    return lwrb_get_full(&stdout_ring_buf) == 0;
 }
 
-bool pbdrv_usb_connection_is_active(void) {
-    return false;
+static bool respond_soon;
+static pbio_pybricks_error_t respond_result;
+
+/**
+ * Non-blocking poll handler to process pending incoming messages.
+ */
+static void pbdrv_usb_handle_data_in(void) {
+
+    // Ignore incoming data if we haven't sent our previous response yet.
+    if (respond_soon) {
+        return;
+    }
+
+    static uint8_t data_in[PBDRV_USB_PYBRICKS_MAX_PACKET_SIZE];
+    uint32_t size = pbdrv_usb_get_data_in(data_in);
+
+    // Expecting at least EP_MSG and payload.
+    if (size < 2) {
+        return;
+    }
+
+    switch (data_in[0]) {
+        case PBIO_PYBRICKS_OUT_EP_MSG_SUBSCRIBE:
+            pbdrv_usb_events_subscribed = data_in[1];
+            respond_result = PBIO_PYBRICKS_ERROR_OK;
+            respond_soon = true;
+
+            // Schedule sending current status immediately after subscribing.
+            status_data_pending = true;
+            break;
+        case PBIO_PYBRICKS_OUT_EP_MSG_COMMAND:
+            if (pbdrv_usb_receive_handler) {
+                respond_result = pbdrv_usb_receive_handler(data_in + 1, size - 1);
+                respond_soon = true;
+            }
+            break;
+    }
+}
+
+static void pbdrv_usb_reset_state(void) {
+    pbdrv_usb_events_subscribed = false;
+    respond_soon = false;
+    status_data_pending = false;
+    lwrb_reset(&stdout_ring_buf);
+}
+
+static pbio_os_process_t pbdrv_usb_process;
+
+static pbio_error_t pbdrv_usb_process_thread(pbio_os_state_t *state, void *context) {
+
+    static pbio_os_state_t sub;
+
+    static uint8_t out_data[PBDRV_USB_PYBRICKS_MAX_PACKET_SIZE];
+    static uint32_t out_size;
+
+    pbio_error_t err;
+
+    // Runs every time. If there is no connection, there just won't be data.
+    pbdrv_usb_handle_data_in();
+
+    PBIO_OS_ASYNC_BEGIN(state);
+
+    for (;;) {
+
+        // Run charger detection: wait for USB to become physically plugged in.
+        PBIO_OS_AWAIT(state, &sub, err = pbdrv_usb_wait_for_charger(&sub));
+
+        while (pbdrv_usb_process.request != PBIO_OS_PROCESS_REQUEST_TYPE_CANCEL && pbdrv_usb_is_ready()) {
+
+            // Find out what we should send, if anything, priotizing response, then
+            // status, then stdout, then other events.
+            if (respond_soon) {
+                // Pack the response to the most recent message.
+                out_data[0] = PBIO_PYBRICKS_IN_EP_MSG_RESPONSE;
+                pbio_set_uint32_le(&out_data[1], respond_result);
+                out_size = sizeof(uint32_t) + 1;
+                respond_soon = false;
+            } else if (pbdrv_usb_connection_is_active() && status_data_pending) {
+                // Send out status if pending (already includes event code).
+                out_data[0] = PBIO_PYBRICKS_IN_EP_MSG_EVENT;
+                memcpy(&out_data[1], status_data, PBIO_PYBRICKS_EVENT_STATUS_REPORT_SIZE);
+                out_size = PBIO_PYBRICKS_USB_MESSAGE_SIZE(PBIO_PYBRICKS_EVENT_STATUS_REPORT_SIZE);
+                status_data_pending = false;
+            } else if (pbdrv_usb_connection_is_active() && lwrb_get_full(&stdout_ring_buf) != 0) {
+                // Send out stdout if anything is buffered.
+                out_data[0] = PBIO_PYBRICKS_IN_EP_MSG_EVENT;
+                out_data[1] = PBIO_PYBRICKS_EVENT_WRITE_STDOUT;
+                out_size = lwrb_read(&stdout_ring_buf, &out_data[2], PBDRV_USB_PYBRICKS_MAX_PACKET_SIZE - 2) + 2;
+            }
+
+            // If there was anything to send, send it.
+            if (out_size) {
+                PBIO_OS_AWAIT(state, &sub, err = pbdrv_usb_tx(&sub, out_data, out_size));
+                out_size = 0;
+                if (err != PBIO_SUCCESS) {
+                    pbdrv_usb_reset_state();
+                    PBIO_OS_AWAIT(state, &sub, pbdrv_usb_tx_reset(&sub));
+                }
+            } else {
+                // Otherwise yield once before going and check again.
+                PBIO_OS_AWAIT_ONCE(state);
+            }
+        }
+
+        PBIO_OS_AWAIT_WHILE(state, pbdrv_usb_is_ready());
+        pbdrv_usb_reset_state();
+        pbdrv_usb_tx_reset(&sub);
+    }
+
+    // Unreachable. On cancellation, the charger detection step in the above
+    // loop keeps running. It will just skip the data handler.
+    PBIO_OS_ASYNC_END(PBIO_ERROR_FAILED);
 }
 
 void pbdrv_usb_init(void) {
     pbdrv_usb_init_device();
+
+    static uint8_t stdout_buf[PBDRV_USB_PYBRICKS_MAX_PACKET_SIZE * 2];
+    lwrb_init(&stdout_ring_buf, stdout_buf, PBIO_ARRAY_SIZE(stdout_buf));
+
+    pbio_os_process_start(&pbdrv_usb_process, pbdrv_usb_process_thread, NULL);
 }
 
 void pbdrv_usb_deinit(void) {
+    pbdrv_usb_deinit_device();
+    pbio_os_process_make_request(&pbdrv_usb_process, PBIO_OS_PROCESS_REQUEST_TYPE_CANCEL);
 }
 
 #endif // PBDRV_CONFIG_USB_SIMULATION
diff --git a/lib/pbio/drv/usb/usb.h b/lib/pbio/drv/usb/usb.h
@@ -10,6 +10,15 @@
 
 #if PBDRV_CONFIG_USB
 
+#include <pbio/error.h>
+#include <pbio/os.h>
+
+#include <stdint.h>
+
+// revisit, this is also defined elsewhere
+#define PBDRV_USB_PYBRICKS_MAX_PACKET_SIZE (64u)
+#define PBDRV_USB_TRANSMIT_TIMEOUT (50)
+
 /**
  * Initializes the USB driver on boot.
  */
@@ -25,6 +34,64 @@ void pbdrv_usb_init_device(void);
  */
 void pbdrv_usb_deinit(void);
 
+/**
+ * Platform specific device deinitialization.
+ */
+void pbdrv_usb_deinit_device(void);
+
+/**
+ * Gets most recent incoming message and copies it to provided buffer.
+ *
+ * The message is then cleared and the driver prepares to read a new message.
+ *
+ * @param [in] data     Buffer to copy the message to.
+ * @return              Number of bytes copied. Zero means nothing was available.
+ */
+uint32_t pbdrv_usb_get_data_in(uint8_t *data);
+
+/**
+ * Sends and awaits message from hub to host via the Pybricks USB interface OUT endpoint.
+ *
+ * Driver-specific implementation. Must return within ::PBDRV_USB_TRANSMIT_TIMEOUT.
+ *
+ * The USB process ensures that only one call is made at once.
+ *
+ * @param [in] state    Protothread state.
+ * @param [in] data     Data to send.
+ * @param [in] size     Data size.
+ * @return              ::PBIO_SUCCESS on completion.
+ *                      ::PBIO_ERROR_INVALID_OP if there is no connection.
+ *                      ::PBIO_ERROR_AGAIN while awaiting.
+ *                      ::PBIO_ERROR_BUSY if this operation is already ongoing.
+ *                      ::PBIO_ERROR_INVALID_ARG if @p size is too large.
+ *                      ::PBIO_ERROR_TIMEDOUT if the operation was started but could not complete.
+ */
+pbio_error_t pbdrv_usb_tx(pbio_os_state_t *state, const uint8_t *data, uint32_t size);
+
+/**
+ * Resets the driver transmission state.
+ *
+ * @param [in] state    Protothread state.
+ * @return              ::PBIO_SUCCESS on completion.
+ *                      ::PBIO_ERROR_AGAIN while awaiting.
+ */
+pbio_error_t pbdrv_usb_tx_reset(pbio_os_state_t *state);
+
+/**
+ * Waits for USB to be plugged in and detects what charger type is connected.
+ *
+ * @param [in] state    Protothread state.
+ * @return              ::PBIO_SUCCESS on completion.
+ *                      ::PBIO_ERROR_AGAIN while awaiting.
+ *                      ::PBIO_ERROR_NOT_SUPPORTED if platform has no charger.
+ */
+pbio_error_t pbdrv_usb_wait_for_charger(pbio_os_state_t *state);
+
+/**
+ * Tests if USB is ready for communication.
+ */
+bool pbdrv_usb_is_ready(void);
+
 #else // PBDRV_CONFIG_USB
 
 static inline void pbdrv_usb_init(void) {
@@ -36,6 +103,29 @@ static inline void pbdrv_usb_deinit(void) {
 static inline void pbdrv_usb_init_device(void) {
 }
 
+static inline void pbdrv_usb_deinit_device(void) {
+}
+
+static inline pbio_error_t pbdrv_usb_tx(pbio_os_state_t *state, const uint8_t *data, size_t size) {
+    return PBIO_ERROR_NOT_IMPLEMENTED;
+}
+
+static inline uint32_t pbdrv_usb_get_data_in(uint8_t *data) {
+    return 0;
+}
+
+static inline pbio_error_t pbdrv_usb_tx_reset(pbio_os_state_t *state) {
+    return PBIO_ERROR_NOT_IMPLEMENTED;
+}
+
+static inline pbio_error_t pbdrv_usb_wait_for_charger(pbio_os_state_t *state) {
+    return PBIO_ERROR_NOT_IMPLEMENTED;
+}
+
+static inline bool pbdrv_usb_is_ready(void) {
+    return false;
+}
+
 
 #endif // PBDRV_CONFIG_USB
 
diff --git a/lib/pbio/drv/usb/usb_simulation.c b/lib/pbio/drv/usb/usb_simulation.c
