From 2513f8f50d5036fb8c399d54c61ecc4f03b8accb Mon Sep 17 00:00:00 2001 From: R Date: Wed, 30 Jul 2025 19:08:37 +0100 Subject: [PATCH 01/13] pbio/drv/usb/usb_ev3.c: Implement Pybricks control transfers These transfers allow reading "characteristics" which would otherwise be exchanged over BLE GATT. They contain information about the firmware version and hub capabilities. Also move the bRequest values into pbio/protocol.h so they can be shared --- lib/pbio/drv/usb/usb_ev3.c | 96 +++++++++++++++++++++++++++----- lib/pbio/drv/usb/usb_nxt.c | 6 +- lib/pbio/drv/usb/usb_stm32.c | 7 +-- lib/pbio/include/pbio/protocol.h | 8 +++ 4 files changed, 96 insertions(+), 21 deletions(-) diff --git a/lib/pbio/drv/usb/usb_ev3.c b/lib/pbio/drv/usb/usb_ev3.c index 7ee7f4da1..2a556d7c0 100644 --- a/lib/pbio/drv/usb/usb_ev3.c +++ b/lib/pbio/drv/usb/usb_ev3.c @@ -10,12 +10,17 @@ #include #include +#include +#include #include #include #include #include #include +#include +#include +#include #include @@ -179,13 +184,15 @@ static const pbdrv_usb_ev3_conf_1_union_t configuration_1_desc_fs = { } }; -// We generate a serial number string descriptors at runtime -// so this dynamic buffer is needed -#define STRING_DESC_MAX_SZ 64 +// This dynamic buffer is needed in order to have an aligned, +// global-lifetime buffer for sending dynamic data in response +// to control transfers. This is used for the serial number string +// and for Pybricks protocol requests. static union { - uint8_t b[STRING_DESC_MAX_SZ]; - uint32_t u[STRING_DESC_MAX_SZ / 4]; -} usb_string_desc_buffer; + uint8_t b[EP0_BUF_SZ]; + uint32_t u[EP0_BUF_SZ / sizeof(uint32_t)]; +} pbdrv_usb_ev3_ep0_buffer; +_Static_assert(PBIO_PYBRICKS_HUB_CAPABILITIES_VALUE_SIZE <= EP0_BUF_SZ); // Defined in pbio/platform/ev3/platform.c extern uint8_t pbdrv_ev3_bluetooth_mac_address[6]; @@ -456,17 +463,17 @@ static bool usb_get_descriptor(uint16_t wValue) { return true; case STRING_DESC_SERIAL: - usb_string_desc_buffer.b[0] = 2 * 2 * 6 + 2; - usb_string_desc_buffer.b[1] = DESC_TYPE_STRING; + pbdrv_usb_ev3_ep0_buffer.b[0] = 2 * 2 * 6 + 2; + pbdrv_usb_ev3_ep0_buffer.b[1] = DESC_TYPE_STRING; for (i = 0; i < 6; i++) { - usb_string_desc_buffer.b[2 + 4 * i + 0] = "0123456789ABCDEF"[pbdrv_ev3_bluetooth_mac_address[i] >> 4]; - usb_string_desc_buffer.b[2 + 4 * i + 1] = 0; - usb_string_desc_buffer.b[2 + 4 * i + 2] = "0123456789ABCDEF"[pbdrv_ev3_bluetooth_mac_address[i] & 0xf]; - usb_string_desc_buffer.b[2 + 4 * i + 3] = 0; + pbdrv_usb_ev3_ep0_buffer.b[2 + 4 * i + 0] = "0123456789ABCDEF"[pbdrv_ev3_bluetooth_mac_address[i] >> 4]; + pbdrv_usb_ev3_ep0_buffer.b[2 + 4 * i + 1] = 0; + pbdrv_usb_ev3_ep0_buffer.b[2 + 4 * i + 2] = "0123456789ABCDEF"[pbdrv_ev3_bluetooth_mac_address[i] & 0xf]; + pbdrv_usb_ev3_ep0_buffer.b[2 + 4 * i + 3] = 0; } - pbdrv_usb_setup_data_to_send = usb_string_desc_buffer.u; - pbdrv_usb_setup_data_to_send_sz = usb_string_desc_buffer.b[0]; + pbdrv_usb_setup_data_to_send = pbdrv_usb_ev3_ep0_buffer.u; + pbdrv_usb_setup_data_to_send_sz = pbdrv_usb_ev3_ep0_buffer.b[0]; return true; } break; @@ -678,6 +685,67 @@ static void usb_device_intr(void) { } break; } + break; + + case BM_REQ_TYPE_CLASS: + if ((setup_pkt.s.bmRequestType & BM_REQ_RECIP_MASK) != BM_REQ_RECIP_IF) { + // Pybricks class requests must be directed at the interface + break; + } + + switch (setup_pkt.s.bRequest) { + const char *s; + + case PBIO_PYBRICKS_USB_INTERFACE_READ_CHARACTERISTIC_GATT: + // Standard GATT characteristic + switch (setup_pkt.s.wValue) { + case 0x2A00: + // GATT Device Name characteristic + s = pbdrv_bluetooth_get_hub_name(); + pbdrv_usb_setup_data_to_send_sz = strlen(s); + memcpy(pbdrv_usb_ev3_ep0_buffer.b, s, pbdrv_usb_setup_data_to_send_sz); + pbdrv_usb_setup_data_to_send = pbdrv_usb_ev3_ep0_buffer.u; + handled = true; + break; + + case 0x2A26: + // GATT Firmware Revision characteristic + s = PBIO_VERSION_STR; + pbdrv_usb_setup_data_to_send_sz = strlen(s); + memcpy(pbdrv_usb_ev3_ep0_buffer.b, s, pbdrv_usb_setup_data_to_send_sz); + pbdrv_usb_setup_data_to_send = pbdrv_usb_ev3_ep0_buffer.u; + handled = true; + break; + + case 0x2A28: + // GATT Software Revision characteristic + s = PBIO_PROTOCOL_VERSION_STR; + pbdrv_usb_setup_data_to_send_sz = strlen(s); + memcpy(pbdrv_usb_ev3_ep0_buffer.b, s, pbdrv_usb_setup_data_to_send_sz); + pbdrv_usb_setup_data_to_send = pbdrv_usb_ev3_ep0_buffer.u; + handled = true; + break; + } + break; + + case PBIO_PYBRICKS_USB_INTERFACE_READ_CHARACTERISTIC_PYBRICKS: + // Pybricks characteristic + switch (setup_pkt.s.wValue) { + case 0x0003: + pbio_pybricks_hub_capabilities( + pbdrv_usb_ev3_ep0_buffer.b, + (pbdrv_usb_is_usb_hs ? PYBRICKS_EP_PKT_SZ_HS : PYBRICKS_EP_PKT_SZ_FS) - 1, + PBSYS_CONFIG_APP_FEATURE_FLAGS, + pbsys_storage_get_maximum_program_size(), + PBSYS_CONFIG_HMI_NUM_SLOTS); + pbdrv_usb_setup_data_to_send = pbdrv_usb_ev3_ep0_buffer.u; + pbdrv_usb_setup_data_to_send_sz = PBIO_PYBRICKS_HUB_CAPABILITIES_VALUE_SIZE; + handled = true; + break; + } + break; + } + break; } // Note regarding the setting of the USB_CSRL0_RXRDYC bit: diff --git a/lib/pbio/drv/usb/usb_nxt.c b/lib/pbio/drv/usb/usb_nxt.c index bc81e5b0e..48ad6877b 100644 --- a/lib/pbio/drv/usb/usb_nxt.c +++ b/lib/pbio/drv/usb/usb_nxt.c @@ -549,7 +549,8 @@ static void pbdrv_usb_nxt_handle_class_request(pbdrv_usb_nxt_setup_packet_t *pac case USB_BMREQUEST_RCPT_INT: // Ignoring wIndex for now as we only have one interface. switch (packet->request) { - case 0x01: // Standard GATT characteristic + case PBIO_PYBRICKS_USB_INTERFACE_READ_CHARACTERISTIC_GATT: + // Standard GATT characteristic switch (packet->value) { case 0x2A00: { // device name const char *name = pbdrv_bluetooth_get_hub_name(); @@ -574,7 +575,8 @@ static void pbdrv_usb_nxt_handle_class_request(pbdrv_usb_nxt_setup_packet_t *pac break; } break; - case 0x02: // Pybricks characteristic + case PBIO_PYBRICKS_USB_INTERFACE_READ_CHARACTERISTIC_PYBRICKS: + // Pybricks characteristic switch (packet->value) { case 0x0003: { // hub capabilities uint8_t caps[PBIO_PYBRICKS_HUB_CAPABILITIES_VALUE_SIZE]; diff --git a/lib/pbio/drv/usb/usb_stm32.c b/lib/pbio/drv/usb/usb_stm32.c index 5a4b0c950..2c4398cd8 100644 --- a/lib/pbio/drv/usb/usb_stm32.c +++ b/lib/pbio/drv/usb/usb_stm32.c @@ -292,14 +292,11 @@ static USBD_StatusTypeDef Pybricks_Itf_TransmitCplt(uint8_t *Buf, uint32_t Len, return ret; } -#define USBD_PYBRICKS_INTERFACE_READ_CHARACTERISTIC_GATT 0x01 -#define USBD_PYBRICKS_INTERFACE_READ_CHARACTERISTIC_PYBRICKS 0x02 - static USBD_StatusTypeDef Pybricks_Itf_ReadCharacteristic(USBD_HandleTypeDef *pdev, USBD_SetupReqTypedef *req) { USBD_StatusTypeDef ret = USBD_OK; switch (req->bRequest) { - case USBD_PYBRICKS_INTERFACE_READ_CHARACTERISTIC_GATT: + case PBIO_PYBRICKS_USB_INTERFACE_READ_CHARACTERISTIC_GATT: switch (req->wValue) { case 0x2A00: { // GATT Device Name characteristic @@ -328,7 +325,7 @@ static USBD_StatusTypeDef Pybricks_Itf_ReadCharacteristic(USBD_HandleTypeDef *pd break; } break; - case USBD_PYBRICKS_INTERFACE_READ_CHARACTERISTIC_PYBRICKS: + case PBIO_PYBRICKS_USB_INTERFACE_READ_CHARACTERISTIC_PYBRICKS: switch (req->wValue) { case 0x0003: { // Pybricks hub capabilities characteristic diff --git a/lib/pbio/include/pbio/protocol.h b/lib/pbio/include/pbio/protocol.h index 95f661947..f1781e80b 100644 --- a/lib/pbio/include/pbio/protocol.h +++ b/lib/pbio/include/pbio/protocol.h @@ -427,6 +427,14 @@ extern const uint8_t pbio_nus_tx_char_uuid[]; /** USB bDeviceProtocol for Pybricks hubs */ #define PBIO_PYBRICKS_USB_DEVICE_PROTOCOL 0xF5 +/** USB bRequest for Pybricks class-specific requests */ +enum { + /** Retrieve GATT characteristics */ + PBIO_PYBRICKS_USB_INTERFACE_READ_CHARACTERISTIC_GATT = 0x01, + /** Retrieve Pybricks characteristics */ + PBIO_PYBRICKS_USB_INTERFACE_READ_CHARACTERISTIC_PYBRICKS = 0x02, +}; + #endif // _PBIO_PROTOCOL_H_ /** @} */ From 35f783562fd67820883b0e838ea4817283e728c5 Mon Sep 17 00:00:00 2001 From: R Date: Wed, 30 Jul 2025 20:43:48 +0100 Subject: [PATCH 02/13] pbio/drv/usb/usb_ev3.c: Handle multiple TX completions in ISR --- lib/pbio/drv/usb/usb_ev3.c | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/lib/pbio/drv/usb/usb_ev3.c b/lib/pbio/drv/usb/usb_ev3.c index 2a556d7c0..fb710d12f 100644 --- a/lib/pbio/drv/usb/usb_ev3.c +++ b/lib/pbio/drv/usb/usb_ev3.c @@ -838,7 +838,7 @@ static void usb_device_intr(void) { pbio_os_request_poll(); } - if (dma_q_pend_0 & (1 << TX_COMPQ1)) { + while (dma_q_pend_0 & (1 << TX_COMPQ1)) { // DMA for EP 1 IN is done // Pop the descriptor from the queue @@ -854,6 +854,10 @@ static void usb_device_intr(void) { usb_tx_stdout_is_not_ready = false; pbio_os_request_poll(); } + + // Multiple TX completions can happen at once + // (since we have up to three descriptors in flight) + dma_q_pend_0 = HWREG(USB_0_OTGBASE + CPDMA_PEND_0_REGISTER); } HWREG(USB_0_OTGBASE + USB_0_INTR_SRC_CLEAR) = intr_src; From a937beb2ba205e958ed01759d325faf61739f226 Mon Sep 17 00:00:00 2001 From: R Date: Wed, 30 Jul 2025 20:48:57 +0100 Subject: [PATCH 03/13] pbio/drv/usb/usb_ev3.c: Detect disconnection In order to be able to detect disconnection, we cannot force device mode in the PHY. Forcing device mode overrides the analog comparators used to detect VBUS. Not forcing device mode should be fine because device mode is the default (enabling host mode requires setting DEVCTL.SESSION explicitly, and the ID pin is explicitly disconnected on the EV3 PCB.) --- lib/pbio/drv/usb/usb_ev3.c | 10 +++++++++- 1 file changed, 9 insertions(+), 1 deletion(-) diff --git a/lib/pbio/drv/usb/usb_ev3.c b/lib/pbio/drv/usb/usb_ev3.c index fb710d12f..42929cbb3 100644 --- a/lib/pbio/drv/usb/usb_ev3.c +++ b/lib/pbio/drv/usb/usb_ev3.c @@ -491,6 +491,14 @@ static void usb_device_intr(void) { IntSystemStatusClear(SYS_INT_USB0); uint32_t intr_src = HWREG(USB_0_OTGBASE + USB_0_INTR_SRC); + if (intr_src & USBOTG_INTR_DISCON) { + // USB cable disconnected + + // Mark config and address as 0 for main loop to detect + pbdrv_usb_addr = 0; + pbdrv_usb_config = 0; + } + if (intr_src & USBOTG_INTR_RESET) { // USB reset @@ -940,7 +948,6 @@ void pbdrv_usb_init(void) { ~CFGCHIP2_OTGMODE & ~CFGCHIP2_PHYPWRDN & // Make sure PHY is on ~CFGCHIP2_OTGPWRDN) | // Make sure OTG subsystem is on - CFGCHIP2_FORCE_DEVICE | // We only ever want device operation CFGCHIP2_DATPOL | // Data lines are *not* inverted CFGCHIP2_SESENDEN | // Enable various analog comparators CFGCHIP2_VBDTCTEN; @@ -973,6 +980,7 @@ void pbdrv_usb_init(void) { // Enable the interrupts we actually care about HWREG(USB_0_OTGBASE + USB_0_INTR_MASK_SET) = + USBOTG_INTR_DISCON | USBOTG_INTR_RESET | USBOTG_INTR_EP1_OUT | USBOTG_INTR_EP1_IN | From 492892e400b51c7512845011b150014e7bc0325e Mon Sep 17 00:00:00 2001 From: R Date: Wed, 30 Jul 2025 22:18:50 +0100 Subject: [PATCH 04/13] pbio/drv/usb/usb_ev3.c: Implement command and status handling This handles Pybricks commands, status flag reporting, and timeouts. stdout is not yet handled. --- lib/pbio/drv/usb/stm32_usbd/usbd_pybricks.h | 22 ----- lib/pbio/drv/usb/usb_ev3.c | 93 +++++++++++++++++---- lib/pbio/drv/usb/usb_stm32.c | 18 ++-- lib/pbio/include/pbio/protocol.h | 30 +++++++ 4 files changed, 116 insertions(+), 47 deletions(-) diff --git a/lib/pbio/drv/usb/stm32_usbd/usbd_pybricks.h b/lib/pbio/drv/usb/stm32_usbd/usbd_pybricks.h index 042e89c84..c040d9813 100644 --- a/lib/pbio/drv/usb/stm32_usbd/usbd_pybricks.h +++ b/lib/pbio/drv/usb/stm32_usbd/usbd_pybricks.h @@ -45,28 +45,6 @@ extern "C" { * @} */ -// NOTE: These enums values are sent over the wire, so cannot be changed. Also, -// 0 is skipped to avoid a zeroed buffer from being misinterpreted as a message. - -/** Hub to host messages via the Pybricks interface IN endpoint. */ -enum { - /** - * Analog of BLE status response. Emitted in response to every OUT message - * received. - */ - USBD_PYBRICKS_IN_EP_MSG_RESPONSE = 1, - /**Analog to BLE notification. Only emitted if subscribed. */ - USBD_PYBRICKS_IN_EP_MSG_EVENT = 2, -}; - -/** Host to hub messages via the Pybricks USB interface OUT endpoint. */ -enum { - /** Analog of BLE Client Characteristic Configuration Descriptor (CCCD). */ - USBD_PYBRICKS_OUT_EP_MSG_SUBSCRIBE = 1, - /** Analog of BLE Client Characteristic Write with response. */ - USBD_PYBRICKS_OUT_EP_MSG_COMMAND = 2, -}; - /** @defgroup USBD_Pybricks_Exported_TypesDefinitions * @{ diff --git a/lib/pbio/drv/usb/usb_ev3.c b/lib/pbio/drv/usb/usb_ev3.c index 42929cbb3..952f616f9 100644 --- a/lib/pbio/drv/usb/usb_ev3.c +++ b/lib/pbio/drv/usb/usb_ev3.c @@ -19,7 +19,9 @@ #include #include #include +#include #include +#include #include #include @@ -214,6 +216,9 @@ static uint32_t pbdrv_usb_setup_misc_tx_byte; // Whether the device is using USB high-speed mode or not static bool pbdrv_usb_is_usb_hs; +// Whether there is a host app listening to events +static bool pbdrv_usb_is_events_subscribed; + // Buffers, used for different logical flows on the data endpoint static uint8_t ep1_rx_buf[PYBRICKS_EP_PKT_SZ_HS]; static uint8_t ep1_tx_response_buf[PYBRICKS_EP_PKT_SZ_HS]; @@ -875,11 +880,18 @@ static void usb_device_intr(void) { static pbio_os_process_t pbdrv_usb_ev3_process; static pbio_error_t pbdrv_usb_ev3_process_thread(pbio_os_state_t *state, void *context) { + static pbio_os_timer_t delay_timer; + static pbio_os_timer_t tx_timeout_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; + + (void)ep1_tx_stdout_buf; + PBIO_OS_ASYNC_BEGIN(state); for (;;) { - PBIO_OS_AWAIT_UNTIL(state, usb_rx_is_ready); - if (usb_rx_is_ready) { // This barrier prevents *subsequent* memory reads from being // speculatively moved *earlier*, outside the if statement @@ -887,29 +899,78 @@ static pbio_error_t pbdrv_usb_ev3_process_thread(pbio_os_state_t *state, void *c pbdrv_compiler_memory_barrier(); uint32_t usb_rx_sz = cppi_descriptors[CPPI_DESC_RX].word0.pktLength; - - // TODO: Remove this echo test - unsigned int i; - for (i = 0; i < usb_rx_sz; i++) { - ep1_tx_response_buf[i] = ep1_rx_buf[i] + 1; - } - for (; i < 512; i++) { - ep1_tx_response_buf[i] = 0xaa; + pbio_pybricks_error_t result; + bool usb_send_response = false; + // Skip empty commands, or commands sent when previous response is still busy + if (usb_rx_sz && !usb_tx_response_is_not_ready) { + switch (ep1_rx_buf[0]) { + case PBIO_PYBRICKS_OUT_EP_MSG_SUBSCRIBE: + pbdrv_usb_is_events_subscribed = ep1_rx_buf[1]; + ep1_tx_response_buf[0] = PBIO_PYBRICKS_IN_EP_MSG_RESPONSE; + pbio_set_uint32_le(&ep1_tx_response_buf[1], PBIO_PYBRICKS_ERROR_OK); + usb_send_response = true; + break; + case PBIO_PYBRICKS_OUT_EP_MSG_COMMAND: + result = pbsys_command(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; + break; + } } - (void)ep1_tx_status_buf; - (void)ep1_tx_stdout_buf; - - unsigned int tx_sz = pbdrv_usb_is_usb_hs ? PYBRICKS_EP_PKT_SZ_HS : PYBRICKS_EP_PKT_SZ_FS; - if (!usb_tx_response_is_not_ready) { + if (usb_send_response) { usb_tx_response_is_not_ready = true; - usb_setup_tx_dma_desc(CPPI_DESC_TX_RESPONSE, ep1_tx_response_buf, tx_sz); + usb_setup_tx_dma_desc(CPPI_DESC_TX_RESPONSE, ep1_tx_response_buf, PBIO_PYBRICKS_USB_MESSAGE_SIZE(sizeof(uint32_t))); } // Re-queue RX buffer after processing is complete usb_rx_is_ready = false; usb_setup_rx_dma_desc(); } + + // 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) { + 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; + usb_setup_tx_dma_desc(CPPI_DESC_TX_STATUS, ep1_tx_status_buf, usb_status_sz); + + prev_status_flags = new_status_flags; + } + + // Handle timeouts + is_transmitting = usb_tx_response_is_not_ready || usb_tx_status_is_not_ready || usb_tx_stdout_is_not_ready; + if (was_transmitting && is_transmitting) { + if (pbio_os_timer_is_expired(&tx_timeout_timer)) { + // Flush _all_ TX packets + while (HWREGB(USB0_BASE + USB_O_TXCSRL1) & USB_TXCSRL1_TXRDY) { + HWREGB(USB0_BASE + USB_O_TXCSRL1) = USB_TXCSRL1_FLUSH; + // We need to wait a bit until the DMA refills the FIFO. + // There doesn't seem to be a good way to figure out if + // there are packets in flight *within* the DMA engine itself + // (i.e. no longer in the queue but in the transfer engine). + PBIO_OS_AWAIT_MS(state, &delay_timer, 1); + } + usb_tx_response_is_not_ready = false; + usb_tx_status_is_not_ready = false; + usb_tx_stdout_is_not_ready = false; + pbdrv_usb_is_events_subscribed = false; + // Resend status flags when host subscribes + prev_status_flags = ~0; + } + } else if (!was_transmitting && is_transmitting) { + pbio_os_timer_set(&tx_timeout_timer, 5); + } + was_transmitting = is_transmitting; + + // Make this process loop run from the beginning. + // Because we check so many different conditions, + // we don't use the normal await macros. + PBIO_OS_ASYNC_RESET(state); + return PBIO_ERROR_AGAIN; } PBIO_OS_ASYNC_END(PBIO_SUCCESS); diff --git a/lib/pbio/drv/usb/usb_stm32.c b/lib/pbio/drv/usb/usb_stm32.c index 2c4398cd8..d97707abf 100644 --- a/lib/pbio/drv/usb/usb_stm32.c +++ b/lib/pbio/drv/usb/usb_stm32.c @@ -35,8 +35,8 @@ PROCESS(pbdrv_usb_process, "USB"); // These buffers need to be 32-bit aligned because the USB driver moves data // to/from FIFOs in 32-bit chunks. static uint8_t usb_in_buf[USBD_PYBRICKS_MAX_PACKET_SIZE] __aligned(4); -static uint8_t usb_response_buf[1 + sizeof(uint32_t)] __aligned(4); -static uint8_t usb_status_buf[1 + PBSYS_STATUS_REPORT_SIZE] __aligned(4); +static uint8_t usb_response_buf[PBIO_PYBRICKS_USB_MESSAGE_SIZE(sizeof(uint32_t))] __aligned(4); +static uint8_t usb_status_buf[PBIO_PYBRICKS_USB_MESSAGE_SIZE(PBSYS_STATUS_REPORT_SIZE)] __aligned(4); static uint8_t usb_stdout_buf[USBD_PYBRICKS_MAX_PACKET_SIZE] __aligned(4); static volatile uint32_t usb_in_sz; static volatile uint32_t usb_response_sz; @@ -174,7 +174,7 @@ pbio_error_t pbdrv_usb_stdout_tx(const uint8_t *data, uint32_t *size) { return PBIO_ERROR_AGAIN; } - *ptr++ = USBD_PYBRICKS_IN_EP_MSG_EVENT; + *ptr++ = PBIO_PYBRICKS_IN_EP_MSG_EVENT; ptr_len--; *ptr++ = PBIO_PYBRICKS_EVENT_WRITE_STDOUT; @@ -452,16 +452,16 @@ PROCESS_THREAD(pbdrv_usb_process, ev, data) { if (usb_in_sz) { switch (usb_in_buf[0]) { - case USBD_PYBRICKS_OUT_EP_MSG_SUBSCRIBE: + case PBIO_PYBRICKS_OUT_EP_MSG_SUBSCRIBE: pbdrv_usb_stm32_is_events_subscribed = usb_in_buf[1]; - usb_response_buf[0] = USBD_PYBRICKS_IN_EP_MSG_RESPONSE; + usb_response_buf[0] = PBIO_PYBRICKS_IN_EP_MSG_RESPONSE; pbio_set_uint32_le(&usb_response_buf[1], PBIO_PYBRICKS_ERROR_OK); usb_response_sz = sizeof(usb_response_buf); break; - case USBD_PYBRICKS_OUT_EP_MSG_COMMAND: + case PBIO_PYBRICKS_OUT_EP_MSG_COMMAND: if (usb_response_sz == 0) { result = pbsys_command(usb_in_buf + 1, usb_in_sz - 1); - usb_response_buf[0] = USBD_PYBRICKS_IN_EP_MSG_RESPONSE; + 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); } @@ -492,10 +492,10 @@ PROCESS_THREAD(pbdrv_usb_process, ev, data) { 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)) { - usb_status_buf[0] = USBD_PYBRICKS_IN_EP_MSG_EVENT; + 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 = 1 + pbsys_status_get_status_report(&usb_status_buf[1]); + usb_status_sz = PBIO_PYBRICKS_USB_MESSAGE_SIZE(pbsys_status_get_status_report(&usb_status_buf[1])); // 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 diff --git a/lib/pbio/include/pbio/protocol.h b/lib/pbio/include/pbio/protocol.h index f1781e80b..992301c34 100644 --- a/lib/pbio/include/pbio/protocol.h +++ b/lib/pbio/include/pbio/protocol.h @@ -435,6 +435,36 @@ enum { PBIO_PYBRICKS_USB_INTERFACE_READ_CHARACTERISTIC_PYBRICKS = 0x02, }; +// NOTE: These enums values are sent over the wire, so cannot be changed. Also, +// 0 is skipped to avoid a zeroed buffer from being misinterpreted as a message. + +/** Hub to host messages via the Pybricks interface IN endpoint. */ +enum { + /** + * Analog of BLE status response. Emitted in response to every OUT message + * received. + */ + PBIO_PYBRICKS_IN_EP_MSG_RESPONSE = 1, + /**Analog to BLE notification. Only emitted if subscribed. */ + PBIO_PYBRICKS_IN_EP_MSG_EVENT = 2, +}; + +/** Host to hub messages via the Pybricks USB interface OUT endpoint. */ +enum { + /** Analog of BLE Client Characteristic Configuration Descriptor (CCCD). */ + PBIO_PYBRICKS_OUT_EP_MSG_SUBSCRIBE = 1, + /** Analog of BLE Client Characteristic Write with response. */ + PBIO_PYBRICKS_OUT_EP_MSG_COMMAND = 2, +}; + +/** + * Size of USB messages for Pybricks USB interface. + * + * USB has one extra byte header for a message type discriminator + * compared to BLE messages. + */ +#define PBIO_PYBRICKS_USB_MESSAGE_SIZE(n) (1 + n) + #endif // _PBIO_PROTOCOL_H_ /** @} */ From c85cf2ad960f668a10e59089b93a7a4d6448fa11 Mon Sep 17 00:00:00 2001 From: R Date: Wed, 30 Jul 2025 22:30:18 +0100 Subject: [PATCH 05/13] pbio/drv/usb/usb_ev3.c: Correctly report host connection status --- lib/pbio/drv/usb/usb_ev3.c | 15 +++++++++++---- 1 file changed, 11 insertions(+), 4 deletions(-) diff --git a/lib/pbio/drv/usb/usb_ev3.c b/lib/pbio/drv/usb/usb_ev3.c index 952f616f9..7b93866cd 100644 --- a/lib/pbio/drv/usb/usb_ev3.c +++ b/lib/pbio/drv/usb/usb_ev3.c @@ -502,6 +502,7 @@ static void usb_device_intr(void) { // Mark config and address as 0 for main loop to detect pbdrv_usb_addr = 0; pbdrv_usb_config = 0; + pbio_os_request_poll(); } if (intr_src & USBOTG_INTR_RESET) { @@ -892,6 +893,12 @@ static pbio_error_t pbdrv_usb_ev3_process_thread(pbio_os_state_t *state, void *c 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; + } + if (usb_rx_is_ready) { // This barrier prevents *subsequent* memory reads from being // speculatively moved *earlier*, outside the if statement @@ -1067,6 +1074,10 @@ pbdrv_usb_bcd_t pbdrv_usb_get_bcd(void) { return PBDRV_USB_BCD_NONE; } +bool pbdrv_usb_connection_is_active(void) { + return pbdrv_usb_is_events_subscribed; +} + // TODO: Reimplement the following functions as appropriate // (mphalport.c and the "host" layer are currently being refactored) uint32_t pbdrv_usb_write(const uint8_t *data, uint32_t size) { @@ -1085,8 +1096,4 @@ int32_t pbdrv_usb_get_char(void) { void pbdrv_usb_tx_flush(void) { } -bool pbdrv_usb_connection_is_active(void) { - return false; -} - #endif // PBDRV_CONFIG_USB_EV3 From e6ead10581a25cf4784644f6205dcb6987be820a Mon Sep 17 00:00:00 2001 From: R Date: Wed, 30 Jul 2025 22:58:24 +0100 Subject: [PATCH 06/13] pbio/drv/usb/usb_ev3.c: Implement stdout handling This replaces the old stopgap stdout logic with stdio over USB --- bricks/ev3/mphalport.c | 54 ++++++++++++++-------------- lib/pbio/drv/usb/usb_ev3.c | 72 +++++++++++++++++++++++++++++++------- 2 files changed, 87 insertions(+), 39 deletions(-) diff --git a/bricks/ev3/mphalport.c b/bricks/ev3/mphalport.c index b5de81c2a..7f869fc36 100644 --- a/bricks/ev3/mphalport.c +++ b/bricks/ev3/mphalport.c @@ -8,10 +8,8 @@ #include #include -#include - #include -#include +#include #include "py/runtime.h" #include "py/mphal.h" @@ -32,49 +30,53 @@ void mp_hal_delay_ms(mp_uint_t Delay) { } while (pbdrv_clock_get_ms() - start < Delay); } -extern uint32_t pbdrv_usb_rx_data_available(void); - -extern int32_t pbdrv_usb_get_char(void); - uintptr_t mp_hal_stdio_poll(uintptr_t poll_flags) { - uintptr_t ret = 0; - if ((poll_flags & MP_STREAM_POLL_RD) && pbdrv_usb_rx_data_available() > 0) { + if ((poll_flags & MP_STREAM_POLL_RD) && pbsys_host_stdin_get_available()) { ret |= MP_STREAM_POLL_RD; } return ret; } +// Receive single character int mp_hal_stdin_rx_chr(void) { - int c; - while (((c = pbdrv_uart_debug_get_char()) == -1) && (c = pbdrv_usb_get_char()) == -1) { - MICROPY_EVENT_POLL_HOOK; + uint32_t size; + uint8_t c; + + // wait for rx interrupt + while (size = 1, pbsys_host_stdin_read(&c, &size) != PBIO_SUCCESS) { + MICROPY_EVENT_POLL_HOOK } + return c; } -extern uint32_t pbdrv_usb_write(const uint8_t *data, uint32_t size); - +// Send string of given length mp_uint_t mp_hal_stdout_tx_strn(const char *str, size_t len) { - pbdrv_uart_debug_printf("%.*s", len, str); - - uint32_t done = 0; - while (done < len) { - done += pbdrv_usb_write((uint8_t *)str + done, len - done); - MICROPY_VM_HOOK_LOOP; - } + size_t remaining = len; + + while (remaining) { + uint32_t size = remaining; + pbio_error_t err = pbsys_host_stdout_write((const uint8_t *)str, &size); + if (err == PBIO_SUCCESS) { + str += size; + remaining -= size; + } else if (err != PBIO_ERROR_AGAIN) { + // Ignoring error for now. This means stdout is lost if Bluetooth/USB + // is disconnected. + return len - remaining; + } - while (!pbdrv_uart_debug_is_done()) { - MICROPY_VM_HOOK_LOOP; + MICROPY_EVENT_POLL_HOOK } return len; } -extern void pbdrv_usb_tx_flush(void); - void mp_hal_stdout_tx_flush(void) { - pbdrv_usb_tx_flush(); + while (!pbsys_host_tx_is_idle()) { + MICROPY_EVENT_POLL_HOOK + } } diff --git a/lib/pbio/drv/usb/usb_ev3.c b/lib/pbio/drv/usb/usb_ev3.c index 7b93866cd..ab25affbc 100644 --- a/lib/pbio/drv/usb/usb_ev3.c +++ b/lib/pbio/drv/usb/usb_ev3.c @@ -888,8 +888,6 @@ static pbio_error_t pbdrv_usb_ev3_process_thread(pbio_os_state_t *state, void *c static uint32_t prev_status_flags = ~0; static uint32_t new_status_flags = 0; - (void)ep1_tx_stdout_buf; - PBIO_OS_ASYNC_BEGIN(state); for (;;) { @@ -1078,22 +1076,70 @@ bool pbdrv_usb_connection_is_active(void) { return pbdrv_usb_is_events_subscribed; } -// TODO: Reimplement the following functions as appropriate -// (mphalport.c and the "host" layer are currently being refactored) -uint32_t pbdrv_usb_write(const uint8_t *data, uint32_t size) { - // Return the size requested so that caller doesn't block. - return size; -} +/** + * Queues data to be transmitted via USB. + * @param data [in] The data to be sent. + * @param size [in, out] The size of @p data in bytes. After return, @p size + * contains the number of bytes actually written. + * @return ::PBIO_SUCCESS if some @p data was queued, ::PBIO_ERROR_AGAIN + * if no @p data could not be queued at this time (e.g. buffer + * is full), ::PBIO_ERROR_INVALID_OP if there is not an + * active USB connection + */ +pbio_error_t pbdrv_usb_stdout_tx(const uint8_t *data, uint32_t *size) { + if (!pbdrv_usb_is_events_subscribed) { + // If the app hasn't subscribed to events, we can't send stdout. + return PBIO_ERROR_INVALID_OP; + } + + uint8_t *ptr = ep1_tx_stdout_buf; + uint32_t ptr_len = pbdrv_usb_is_usb_hs ? PYBRICKS_EP_PKT_SZ_HS : PYBRICKS_EP_PKT_SZ_FS; + + if (usb_tx_stdout_is_not_ready) { + *size = 0; + return PBIO_ERROR_AGAIN; + } + + *ptr++ = PBIO_PYBRICKS_IN_EP_MSG_EVENT; + ptr_len--; -uint32_t pbdrv_usb_rx_data_available(void) { - return 0; + *ptr++ = PBIO_PYBRICKS_EVENT_WRITE_STDOUT; + ptr_len--; + + if (*size > ptr_len) { + *size = ptr_len; + } + memcpy(ptr, data, *size); + + usb_tx_stdout_is_not_ready = true; + usb_setup_tx_dma_desc(CPPI_DESC_TX_STDOUT, ep1_tx_stdout_buf, 2 + *size); + + return PBIO_SUCCESS; } -int32_t pbdrv_usb_get_char(void) { - return -1; +// REVISIT: These two functions do not keep track of how much data +// is held in the DMA engine or the packet FIFO (i.e. the DMA engine +// has returned the descriptor to us (and thus we can queue more), +// but the hardware is still buffering data which we cannot see without +// performing much more accurate accounting) +uint32_t pbdrv_usb_stdout_tx_available(void) { + if (!pbdrv_usb_is_events_subscribed) { + return UINT32_MAX; + } + + if (usb_tx_stdout_is_not_ready) { + return 0; + } + + // Subtract 2 bytes for header + if (pbdrv_usb_is_usb_hs) { + return PYBRICKS_EP_PKT_SZ_HS - 2; + } + return PYBRICKS_EP_PKT_SZ_FS - 2; } -void pbdrv_usb_tx_flush(void) { +bool pbdrv_usb_stdout_tx_is_idle(void) { + return !usb_tx_stdout_is_not_ready; } #endif // PBDRV_CONFIG_USB_EV3 From 0198f7b54aa99d2a5d51f2e2d21b8feae19f8bf8 Mon Sep 17 00:00:00 2001 From: R Date: Wed, 30 Jul 2025 23:07:46 +0100 Subject: [PATCH 07/13] pbio/drv/usb/usb_ev3.c: Implement status keepalive timer Without this timer, if the hub state doesn't change and the user program doesn't output anything, the hub won't be able to detect that the host software has closed (because it never sends anything which could time out). Keeping this state reasonably up-to-date is important for the hub auto-power-off functionality. --- lib/pbio/drv/usb/usb_ev3.c | 14 +++++++++++++- 1 file changed, 13 insertions(+), 1 deletion(-) diff --git a/lib/pbio/drv/usb/usb_ev3.c b/lib/pbio/drv/usb/usb_ev3.c index ab25affbc..122551498 100644 --- a/lib/pbio/drv/usb/usb_ev3.c +++ b/lib/pbio/drv/usb/usb_ev3.c @@ -883,6 +883,7 @@ static pbio_os_process_t pbdrv_usb_ev3_process; static pbio_error_t pbdrv_usb_ev3_process_thread(pbio_os_state_t *state, void *context) { static pbio_os_timer_t delay_timer; static pbio_os_timer_t tx_timeout_timer; + static pbio_os_timer_t keepalive_timer; static bool was_transmitting = false; static bool is_transmitting = false; static uint32_t prev_status_flags = ~0; @@ -910,6 +911,7 @@ static pbio_error_t pbdrv_usb_ev3_process_thread(pbio_os_state_t *state, void *c if (usb_rx_sz && !usb_tx_response_is_not_ready) { switch (ep1_rx_buf[0]) { case PBIO_PYBRICKS_OUT_EP_MSG_SUBSCRIBE: + pbio_os_timer_set(&keepalive_timer, 1000); pbdrv_usb_is_events_subscribed = ep1_rx_buf[1]; ep1_tx_response_buf[0] = PBIO_PYBRICKS_IN_EP_MSG_RESPONSE; pbio_set_uint32_le(&ep1_tx_response_buf[1], PBIO_PYBRICKS_ERROR_OK); @@ -936,7 +938,8 @@ 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) { + 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))) { 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])); @@ -944,6 +947,15 @@ static pbio_error_t pbdrv_usb_ev3_process_thread(pbio_os_state_t *state, void *c 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 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); + } } // Handle timeouts From 2acb66a7dc37d1c62c1f872baa64cbc50529a505 Mon Sep 17 00:00:00 2001 From: R Date: Fri, 1 Aug 2025 15:38:09 +0100 Subject: [PATCH 08/13] pbio/drv/ioport: Allow IO port VCC control to be optional The EV3 technically has an IO port power control, but it doesn't only control the main peripheral ports but also the ADC and USB. It also doesn't require quirk handling to be able to power-cycle the ports. Make this feature compile-time optional rather than having dummy values. This was previously silently dereferencing a null pointer. --- lib/pbio/drv/ioport/ioport.c | 2 ++ lib/pbio/include/pbdrv/ioport.h | 7 +++++++ lib/pbio/platform/city_hub/pbdrvconfig.h | 1 + lib/pbio/platform/essential_hub/pbdrvconfig.h | 1 + lib/pbio/platform/ev3/platform.c | 6 ------ lib/pbio/platform/move_hub/pbdrvconfig.h | 1 + lib/pbio/platform/nxt/pbdrvconfig.h | 1 + lib/pbio/platform/prime_hub/pbdrvconfig.h | 1 + lib/pbio/platform/technic_hub/pbdrvconfig.h | 1 + lib/pbio/platform/test/pbdrvconfig.h | 1 + lib/pbio/platform/virtual_hub/pbdrvconfig.h | 1 + 11 files changed, 17 insertions(+), 6 deletions(-) diff --git a/lib/pbio/drv/ioport/ioport.c b/lib/pbio/drv/ioport/ioport.c index 192a46fe5..dbd7a7ff9 100644 --- a/lib/pbio/drv/ioport/ioport.c +++ b/lib/pbio/drv/ioport/ioport.c @@ -65,6 +65,7 @@ pbio_error_t pbdrv_ioport_p5p6_set_mode(const pbdrv_ioport_pins_t *pins, pbdrv_u return PBIO_ERROR_NOT_SUPPORTED; } +#if PBDRV_CONFIG_HAS_PORT_VCC_CONTROL void pbdrv_ioport_enable_vcc(bool enable) { if (enable) { pbdrv_gpio_out_high(&pbdrv_ioport_platform_data_vcc_pin); @@ -72,5 +73,6 @@ void pbdrv_ioport_enable_vcc(bool enable) { pbdrv_gpio_out_low(&pbdrv_ioport_platform_data_vcc_pin); } } +#endif // PBDRV_CONFIG_HAS_PORT_VCC_CONTROL #endif // PBDRV_CONFIG_IOPORT diff --git a/lib/pbio/include/pbdrv/ioport.h b/lib/pbio/include/pbdrv/ioport.h index cbfe5390d..c4dd749a7 100644 --- a/lib/pbio/include/pbdrv/ioport.h +++ b/lib/pbio/include/pbdrv/ioport.h @@ -118,12 +118,17 @@ typedef struct { uint32_t supported_modes; } pbdrv_ioport_platform_data_t; +#if PBDRV_CONFIG_HAS_PORT_VCC_CONTROL /** * Enables or disables VCC on pin 4 of all ioports. * * @param [in] enable Choose true to enable VCC, false to disable. */ void pbdrv_ioport_enable_vcc(bool enable); +#else +static inline void pbdrv_ioport_enable_vcc(bool enable) { +} +#endif // PBDRV_CONFIG_HAS_PORT_VCC_CONTROL /** * Sets the mode of the P5P6 pins on this port. @@ -143,7 +148,9 @@ pbio_error_t pbdrv_ioport_p5p6_set_mode(const pbdrv_ioport_pins_t *pins, pbdrv_u extern const pbdrv_ioport_platform_data_t pbdrv_ioport_platform_data[PBDRV_CONFIG_IOPORT_NUM_DEV]; +#if PBDRV_CONFIG_HAS_PORT_VCC_CONTROL extern const pbdrv_gpio_t pbdrv_ioport_platform_data_vcc_pin; +#endif // PBDRV_CONFIG_HAS_PORT_VCC_CONTROL #endif // PBDRV_IOPORT_H diff --git a/lib/pbio/platform/city_hub/pbdrvconfig.h b/lib/pbio/platform/city_hub/pbdrvconfig.h index 7d6a99a5d..82daaefda 100644 --- a/lib/pbio/platform/city_hub/pbdrvconfig.h +++ b/lib/pbio/platform/city_hub/pbdrvconfig.h @@ -82,6 +82,7 @@ #define PBDRV_CONFIG_HAS_PORT_A (1) #define PBDRV_CONFIG_HAS_PORT_B (1) +#define PBDRV_CONFIG_HAS_PORT_VCC_CONTROL (1) #define PBDRV_CONFIG_SYS_CLOCK_RATE 48000000 #define PBDRV_CONFIG_INIT_ENABLE_INTERRUPTS_ARM (1) diff --git a/lib/pbio/platform/essential_hub/pbdrvconfig.h b/lib/pbio/platform/essential_hub/pbdrvconfig.h index 3930106ed..fef78a134 100644 --- a/lib/pbio/platform/essential_hub/pbdrvconfig.h +++ b/lib/pbio/platform/essential_hub/pbdrvconfig.h @@ -113,6 +113,7 @@ #define PBDRV_CONFIG_HAS_PORT_A (1) #define PBDRV_CONFIG_HAS_PORT_B (1) +#define PBDRV_CONFIG_HAS_PORT_VCC_CONTROL (1) #define PBDRV_CONFIG_SYS_CLOCK_RATE 96000000 #define PBDRV_CONFIG_INIT_ENABLE_INTERRUPTS_ARM (1) diff --git a/lib/pbio/platform/ev3/platform.c b/lib/pbio/platform/ev3/platform.c index 9fe6cb51f..951146592 100644 --- a/lib/pbio/platform/ev3/platform.c +++ b/lib/pbio/platform/ev3/platform.c @@ -253,12 +253,6 @@ const pbdrv_uart_ev3_platform_data_t }, }; -// TODO. -const pbdrv_gpio_t pbdrv_ioport_platform_data_vcc_pin = { - .bank = NULL, - .pin = 0, -}; - const pbdrv_ioport_platform_data_t pbdrv_ioport_platform_data[PBDRV_CONFIG_IOPORT_NUM_DEV] = { { .port_id = PBIO_PORT_ID_A, diff --git a/lib/pbio/platform/move_hub/pbdrvconfig.h b/lib/pbio/platform/move_hub/pbdrvconfig.h index 9be8dece7..4f9d0155d 100644 --- a/lib/pbio/platform/move_hub/pbdrvconfig.h +++ b/lib/pbio/platform/move_hub/pbdrvconfig.h @@ -80,6 +80,7 @@ #define PBDRV_CONFIG_HAS_PORT_B (1) #define PBDRV_CONFIG_HAS_PORT_C (1) #define PBDRV_CONFIG_HAS_PORT_D (1) +#define PBDRV_CONFIG_HAS_PORT_VCC_CONTROL (1) #define PBDRV_CONFIG_SYS_CLOCK_RATE 48000000 #define PBDRV_CONFIG_INIT_ENABLE_INTERRUPTS_ARM (1) diff --git a/lib/pbio/platform/nxt/pbdrvconfig.h b/lib/pbio/platform/nxt/pbdrvconfig.h index 04d0e78ed..0bb64dad2 100644 --- a/lib/pbio/platform/nxt/pbdrvconfig.h +++ b/lib/pbio/platform/nxt/pbdrvconfig.h @@ -35,6 +35,7 @@ #define PBDRV_CONFIG_HAS_PORT_2 (1) #define PBDRV_CONFIG_HAS_PORT_3 (1) #define PBDRV_CONFIG_HAS_PORT_4 (1) +#define PBDRV_CONFIG_HAS_PORT_VCC_CONTROL (1) #define PBDRV_CONFIG_RESET (1) #define PBDRV_CONFIG_RESET_NXT (1) diff --git a/lib/pbio/platform/prime_hub/pbdrvconfig.h b/lib/pbio/platform/prime_hub/pbdrvconfig.h index cece15f76..5f65324c8 100644 --- a/lib/pbio/platform/prime_hub/pbdrvconfig.h +++ b/lib/pbio/platform/prime_hub/pbdrvconfig.h @@ -137,6 +137,7 @@ #define PBDRV_CONFIG_HAS_PORT_D (1) #define PBDRV_CONFIG_HAS_PORT_E (1) #define PBDRV_CONFIG_HAS_PORT_F (1) +#define PBDRV_CONFIG_HAS_PORT_VCC_CONTROL (1) #define PBDRV_CONFIG_SYS_CLOCK_RATE 96000000 #define PBDRV_CONFIG_INIT_ENABLE_INTERRUPTS_ARM (1) diff --git a/lib/pbio/platform/technic_hub/pbdrvconfig.h b/lib/pbio/platform/technic_hub/pbdrvconfig.h index e0e90a48f..0a63e9480 100644 --- a/lib/pbio/platform/technic_hub/pbdrvconfig.h +++ b/lib/pbio/platform/technic_hub/pbdrvconfig.h @@ -94,6 +94,7 @@ #define PBDRV_CONFIG_HAS_PORT_B (1) #define PBDRV_CONFIG_HAS_PORT_C (1) #define PBDRV_CONFIG_HAS_PORT_D (1) +#define PBDRV_CONFIG_HAS_PORT_VCC_CONTROL (1) #define PBDRV_CONFIG_SYS_CLOCK_RATE 80000000 #define PBDRV_CONFIG_INIT_ENABLE_INTERRUPTS_ARM (1) diff --git a/lib/pbio/platform/test/pbdrvconfig.h b/lib/pbio/platform/test/pbdrvconfig.h index 798aa09d0..0d11040e2 100644 --- a/lib/pbio/platform/test/pbdrvconfig.h +++ b/lib/pbio/platform/test/pbdrvconfig.h @@ -44,3 +44,4 @@ #define PBDRV_CONFIG_HAS_PORT_D (1) #define PBDRV_CONFIG_HAS_PORT_E (1) #define PBDRV_CONFIG_HAS_PORT_F (1) +#define PBDRV_CONFIG_HAS_PORT_VCC_CONTROL (1) diff --git a/lib/pbio/platform/virtual_hub/pbdrvconfig.h b/lib/pbio/platform/virtual_hub/pbdrvconfig.h index 029e601af..6cbc45b38 100644 --- a/lib/pbio/platform/virtual_hub/pbdrvconfig.h +++ b/lib/pbio/platform/virtual_hub/pbdrvconfig.h @@ -29,3 +29,4 @@ #define PBDRV_CONFIG_HAS_PORT_D (1) #define PBDRV_CONFIG_HAS_PORT_E (1) #define PBDRV_CONFIG_HAS_PORT_F (1) +#define PBDRV_CONFIG_HAS_PORT_VCC_CONTROL (1) From 99b3360271c07bc37c970a347b680ad39a3e04c1 Mon Sep 17 00:00:00 2001 From: R Date: Fri, 1 Aug 2025 16:34:27 +0100 Subject: [PATCH 09/13] pbio/src/port.c: Only turn off motors if the port has a motor Otherwise this dereferences a null pointer. --- lib/pbio/src/port.c | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/lib/pbio/src/port.c b/lib/pbio/src/port.c index f6655bd6c..c34b7bddf 100644 --- a/lib/pbio/src/port.c +++ b/lib/pbio/src/port.c @@ -447,7 +447,9 @@ void pbio_port_stop_user_actions(bool reset) { } // Stops and resets motors. Also stops higher level controls like servo. - pbio_dcmotor_reset(port->dcmotor, reset); + if (port->dcmotor) { + pbio_dcmotor_reset(port->dcmotor, reset); + } } } From 13a21d5695e3490a927769a31b15760192df0959 Mon Sep 17 00:00:00 2001 From: R Date: Fri, 1 Aug 2025 15:39:18 +0100 Subject: [PATCH 10/13] pbio/platform/ev3/start.S: Disable MMU The MMU was being previously left on by U-boot. Turn it off before starting the system. --- lib/pbio/platform/ev3/start.S | 1 + 1 file changed, 1 insertion(+) diff --git a/lib/pbio/platform/ev3/start.S b/lib/pbio/platform/ev3/start.S index 1673fbf06..c9e721787 100755 --- a/lib/pbio/platform/ev3/start.S +++ b/lib/pbio/platform/ev3/start.S @@ -58,6 +58,7 @@ Entry: mrc p15, #0, r0, c1, c0,#0 @ Read System Control Register bic r0, r0, #0x1000 @ Clear bit 12 to disable ICache bic r0, r0, #0x0004 @ Clear bit 2 to disable DCache + bic r0, r0, #0x0001 @ Clear bit 0 to disable MMU mcr p15, #0, r0, c1, c0, #0 @ Write back to System Control Register mov r0, #0 @ Set r0 to 0 mcr p15, #0, r0, c7, c7, #0 @ Invalidate all caches From 9caac6c1e5ab024e5092302ed6328edc780a6a98 Mon Sep 17 00:00:00 2001 From: R Date: Fri, 1 Aug 2025 15:40:43 +0100 Subject: [PATCH 11/13] pbio/platform/ev3/platform.c: Initial MMU setup This creates an identity mapping for only the valid bits of the address space. It also adds logic to the exception handler to print out fault addresses. This currently marks the entire address space as uncacheable. Drivers will need to be updated to take caches into account before we can mark RAM as cacheable. --- lib/pbio/platform/ev3/platform.c | 77 +++++++++++++++++++++ lib/tiam1808/system_config/armv5/gcc/cp15.c | 73 ++++++++++++++++--- lib/tiam1808/tiam1808/armv5/cp15.h | 9 +++ 3 files changed, 148 insertions(+), 11 deletions(-) diff --git a/lib/pbio/platform/ev3/platform.c b/lib/pbio/platform/ev3/platform.c index 951146592..f7dc238aa 100644 --- a/lib/pbio/platform/ev3/platform.c +++ b/lib/pbio/platform/ev3/platform.c @@ -41,6 +41,7 @@ #include #include #include +#include #include #include #include @@ -54,6 +55,7 @@ #include +#include #include #include @@ -483,6 +485,13 @@ void ev3_panic_handler(int except_type, ev3_panic_ctx *except_data) { panic_puts("\r\nSPSR: 0x"); panic_putu32(except_data->spsr); + panic_puts("\r\nDFSR: 0x"); + panic_putu32(CP15GetDFSR()); + panic_puts("\r\nIFSR: 0x"); + panic_putu32(CP15GetIFSR()); + panic_puts("\r\nFAR: 0x"); + panic_putu32(CP15GetFAR()); + panic_puts("\r\nSystem will now reboot...\r\n"); // Poke the watchdog timer with a bad value to immediately trigger it @@ -627,6 +636,73 @@ static void Edma3CCErrHandlerIsr(void) { } } +#define MMU_SECTION_SHIFT 20 +#define MMU_SECTION_SZ (1 << MMU_SECTION_SHIFT) +#define MMU_L1_ENTS (1 << (32 - 20)) +#define MMU_L1_ALIGN (16 * 1024) +#define MMU_L1_SECTION(addr, ap, domain, c, b) ( \ + ((addr) & ~(MMU_SECTION_SZ - 1)) | \ + (((ap) & 3) << 10) | \ + (((domain) & 0xf) << 5) | \ + (1 << 4) | \ + ((!!(c)) << 3) | \ + ((!!(b)) << 2) | \ + (1 << 1) \ + ) +static uint32_t l1_page_table[MMU_L1_ENTS] __attribute__((aligned(MMU_L1_ALIGN))); +#define SYSTEM_RAM_SZ_MB 64 + +static void mmu_init(void) { + // Invalidate TLB + CP15InvTLB(); + + // Program domain D0 = no access, D1 = manager (no permission checks) + // We generally don't bother with permission checks, anything valid is RWX + CP15DomainAccessSet(0b1100); + + // For simplicity, everything is mapped as sections (1 MiB chunks) + // This potentially fails to catch certain out-of-bounds accesses, + // but as a tradeoff we do not need any L2 sections. + + // MMIO register ranges + l1_page_table[0x01C00000 >> MMU_SECTION_SHIFT] = MMU_L1_SECTION(0x01C00000, 0, 1, 0, 0); + l1_page_table[0x01D00000 >> MMU_SECTION_SHIFT] = MMU_L1_SECTION(0x01D00000, 0, 1, 0, 0); + l1_page_table[0x01E00000 >> MMU_SECTION_SHIFT] = MMU_L1_SECTION(0x01E00000, 0, 1, 0, 0); + l1_page_table[0x01F00000 >> MMU_SECTION_SHIFT] = MMU_L1_SECTION(0x01F00000, 0, 1, 0, 0); + + // On-chip RAM, which is used to share control structures with the PRUs + l1_page_table[0x80000000 >> MMU_SECTION_SHIFT] = MMU_L1_SECTION(0x80000000, 0, 1, 0, 0); + + // Off-chip main DDR RAM + for (unsigned int i = 0; i < SYSTEM_RAM_SZ_MB; i++) { + uint32_t addr = 0xC0000000 + i * MMU_SECTION_SZ; + // TODO: Enable caching once DMA code is upgraded to handle cache + l1_page_table[addr >> MMU_SECTION_SHIFT] = MMU_L1_SECTION(addr, 0, 1, 0, 0); + } + // Off-chip main DDR RAM, uncacheable mirror @ 0xD0000000 + for (unsigned int i = 0; i < SYSTEM_RAM_SZ_MB; i++) { + uint32_t addr_phys = 0xC0000000 + i * MMU_SECTION_SZ; + uint32_t addr_virt = 0xD0000000 + i * MMU_SECTION_SZ; + l1_page_table[addr_virt >> MMU_SECTION_SHIFT] = MMU_L1_SECTION(addr_phys, 0, 1, 0, 0); + } + + // ARM local RAM, interrupt controller + l1_page_table[0xFFF00000 >> MMU_SECTION_SHIFT] = MMU_L1_SECTION(0xFFF00000, 0, 1, 0, 0); + + // Make sure this all makes its way into memory + pbdrv_compiler_memory_barrier(); + + // Set TTBR + CP15TtbSet((uint32_t)l1_page_table); + + uint32_t c15_control = CP15ControlGet(); + // Clear R and S protection bits (even though we don't use them) + c15_control &= ~((1 << 9) | (1 << 8)); + // Enable I-cache, D-cache, alignment faults, and MMU + c15_control |= (1 << 12) | (1 << 2) | (1 << 1) | (1 << 0); + CP15ControlSet(c15_control); +} + enum { BOOT_EEPROM_I2C_ADDRESS = 0x50, }; @@ -637,6 +713,7 @@ uint8_t pbdrv_ev3_bluetooth_mac_address[6]; // initialization (low level in pbdrv, high level in pbio), and system level // functions for running user code (currently a hardcoded MicroPython script). void SystemInit(void) { + mmu_init(); SysCfgRegistersUnlock(); diff --git a/lib/tiam1808/system_config/armv5/gcc/cp15.c b/lib/tiam1808/system_config/armv5/gcc/cp15.c index b700a4229..39d5f28dc 100644 --- a/lib/tiam1808/system_config/armv5/gcc/cp15.c +++ b/lib/tiam1808/system_config/armv5/gcc/cp15.c @@ -37,6 +37,8 @@ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ +#include + /****************************************************************************** ** FUNCTION DEFINITIONS ******************************************************************************/ @@ -218,21 +220,10 @@ void CP15DCacheCleanBuff(unsigned int bufPtr, unsigned int size) **/ void CP15TtbSet(unsigned int ttb) { - /* Invalidates all TLBs.Domain access is selected as - * client by configuring domain access register, - * in that case access controlled by permission value - * set by page table entry - */ - __asm(" mov r1, #0\n\t" - " mcr p15, #0, r1, c8, c7, #0\n\t" - " ldr r1, =0x55555555\n\t" - " mcr p15, #0, r1, c3, c0, #0\n\t"); - /* sets translation table base resgister with page table * starting address. */ __asm(" mcr p15, #0, %[value], c2, c0, 0":: [value] "r" (ttb)); - } /** @@ -268,4 +259,64 @@ void CP15MMUEnable(void) " mcr p15, #0, r0, c1, c0, #0\n\t"); } +/** + * \brief This function gets the data fault status register + */ +uint32_t CP15GetDFSR(void) +{ + uint32_t ret; + __asm("mrc p15, 0, %0, c5, c0, 0" : "=r" (ret)); + return ret; +} + +/** + * \brief This function gets the instruction fault status register + */ +uint32_t CP15GetIFSR(void) +{ + uint32_t ret; + __asm("mrc p15, 0, %0, c5, c0, 1" : "=r" (ret)); + return ret; +} + +/** + * \brief This function gets the fault address register + */ +uint32_t CP15GetFAR(void) +{ + uint32_t ret; + __asm("mrc p15, 0, %0, c6, c0, 0" : "=r" (ret)); + return ret; +} + +/** + * \brief This function invalidates the TLB + */ +void CP15InvTLB(void) +{ + __asm(" mov r0, #0\n\t" + " mcr p15, #0, r0, c8, c7, #0" + ::: "r0"); +} + +/** + * \brief This function programs the domain access control register + */ +void CP15DomainAccessSet(uint32_t domains) +{ + __asm("mcr p15, 0, %0, c3, c0, 0" :: "r"(domains)); +} + +uint32_t CP15ControlGet(void) +{ + uint32_t ret; + __asm("mrc p15, 0, %0, c1, c0, 0" : "=r" (ret)); + return ret; +} + +void CP15ControlSet(uint32_t control) +{ + __asm("mcr p15, 0, %0, c1, c0, 0" :: "r" (control)); +} + /********************************* End Of File *******************************/ diff --git a/lib/tiam1808/tiam1808/armv5/cp15.h b/lib/tiam1808/tiam1808/armv5/cp15.h index 84ba4110e..847e0cf69 100644 --- a/lib/tiam1808/tiam1808/armv5/cp15.h +++ b/lib/tiam1808/tiam1808/armv5/cp15.h @@ -41,6 +41,8 @@ #ifndef __CP15_H #define __CP15_H +#include + #ifdef __cplusplus extern "C" { #endif @@ -60,6 +62,13 @@ extern void CP15MMUDisable(void); extern void CP15MMUEnable(void); extern void CP15ICacheFlushBuff(unsigned int ptr, unsigned int size); extern void CP15DCacheCleanBuff(unsigned int bufPtr, unsigned int size); +extern uint32_t CP15GetDFSR(void); +extern uint32_t CP15GetIFSR(void); +extern uint32_t CP15GetFAR(void); +extern void CP15InvTLB(void); +extern void CP15DomainAccessSet(uint32_t domains); +extern uint32_t CP15ControlGet(void); +extern void CP15ControlSet(uint32_t control); #ifdef __cplusplus } From 55ce50d8200b6a467a7d096472ff104b55ac7169 Mon Sep 17 00:00:00 2001 From: R Date: Fri, 1 Aug 2025 16:26:54 +0100 Subject: [PATCH 12/13] pbio/platform/ev3/platform.c: Use lockdown TLB This makes sure that we can never take a TLB miss penalty when accessing MMIO or local RAM. These "lockdown" TLB entries are not used by general-purpose operating systems like Linux, and they are entirely separate from the "normal" TLB entries. --- lib/pbio/platform/ev3/platform.c | 30 ++++++++++++++++++++++++++++++ 1 file changed, 30 insertions(+) diff --git a/lib/pbio/platform/ev3/platform.c b/lib/pbio/platform/ev3/platform.c index f7dc238aa..8ed303a8b 100644 --- a/lib/pbio/platform/ev3/platform.c +++ b/lib/pbio/platform/ev3/platform.c @@ -652,6 +652,21 @@ static void Edma3CCErrHandlerIsr(void) { static uint32_t l1_page_table[MMU_L1_ENTS] __attribute__((aligned(MMU_L1_ALIGN))); #define SYSTEM_RAM_SZ_MB 64 +static void mmu_tlb_lock(uint32_t addr) { + uint32_t tmp; + __asm__ volatile ( + "mrc p15, 0, %0, c10, c0, 0\n" // read lockdown register + "orr %0, #1\n" // set P bit + "mcr p15, 0, %0, c10, c0, 0\n" // write lockdown register + "ldr %0, [%1]\n" // force a TLB load + "mrc p15, 0, %0, c10, c0, 0\n" // read lockdown register + "bic %0, #1\n" // clear P bit + "mcr p15, 0, %0, c10, c0, 0\n" // write lockdown register + : "=&r" (tmp) + : "r" (addr) + ); +} + static void mmu_init(void) { // Invalidate TLB CP15InvTLB(); @@ -701,6 +716,21 @@ static void mmu_init(void) { // Enable I-cache, D-cache, alignment faults, and MMU c15_control |= (1 << 12) | (1 << 2) | (1 << 1) | (1 << 0); CP15ControlSet(c15_control); + + // Set victim field in TLB lockdown register to 0 + __asm__ volatile ( + "movs r0, #0\n" + "mcr p15, 0, r0, c10, c0, 0" + ::: "r0" + ); + // Lock all the TLB entries other than main DDR RAM + // This helps improve real-time performance as we will never TLB miss on them + mmu_tlb_lock(0x01C00000); + mmu_tlb_lock(0x01D00000); + mmu_tlb_lock(0x01E00000); + mmu_tlb_lock(0x01F00000); + mmu_tlb_lock(0x80000000); + mmu_tlb_lock(0xFFF00000); } enum { From 1a71f8e61c34115dc350c5e825b67e6580ea6238 Mon Sep 17 00:00:00 2001 From: R Date: Fri, 1 Aug 2025 19:55:21 +0100 Subject: [PATCH 13/13] pbio/platform/ev3/platform.c: Offset address for abort exceptions The actual faulting instruction for these exceptions is offset (due to CPU pipelining in this core, later as part of compatibility). Print the actual faulting instruction address so that the developer doesn't have to manually subtract 8. --- lib/pbio/platform/ev3/exceptionhandler.S | 14 ++++++-------- lib/pbio/platform/ev3/exceptionhandler.h | 9 +++++++++ lib/pbio/platform/ev3/platform.c | 15 ++++++++++++++- 3 files changed, 29 insertions(+), 9 deletions(-) create mode 100644 lib/pbio/platform/ev3/exceptionhandler.h diff --git a/lib/pbio/platform/ev3/exceptionhandler.S b/lib/pbio/platform/ev3/exceptionhandler.S index 14ee6eb30..147c13370 100644 --- a/lib/pbio/platform/ev3/exceptionhandler.S +++ b/lib/pbio/platform/ev3/exceptionhandler.S @@ -1,5 +1,7 @@ @ SPDX-License-Identifier: MPL-1.0 @ Copyright (c) 2016 Tobias Schießl +#include "exceptionhandler.h" + .set MODE_USR, 0x10 .set MODE_FIQ, 0x11 .set MODE_IRQ, 0x12 @@ -12,10 +14,6 @@ .equ I_F_BIT, 0xC0 .equ MASK_SWI_NUM, 0xFF000000 .equ MASK_SR_MODE, 0x1F - .equ EXCEPT_TYPE_UNKNOWN, 0 - .equ EXCEPT_TYPE_UNDEF, 1 - .equ EXCEPT_TYPE_PREFETCH_ABORT, 2 - .equ EXCEPT_TYPE_DATA_ABORT, 3 @ This will be placed at 0xffff0000 by the linker script .section .vector,"ax",%progbits @@ -35,25 +33,25 @@ ResetToEntry: UndefHandlder: @ Save r0-r12, lr stmfd sp!, {r0-r12, lr} - mov r7, #EXCEPT_TYPE_UNDEF + mov r7, #EV3_PANIC_UNDEFINED_INSTR b CommonPanicHandler PrefetchAbortHandler: @ Save r0-r12, lr stmfd sp!, {r0-r12, lr} - mov r7, #EXCEPT_TYPE_PREFETCH_ABORT + mov r7, #EV3_PANIC_PREFETCH_ABORT b CommonPanicHandler DataAbortHandler: @ Save r0-r12, lr stmfd sp!, {r0-r12, lr} - mov r7, #EXCEPT_TYPE_DATA_ABORT + mov r7, #EV3_PANIC_DATA_ABORT b CommonPanicHandler UnknownExceptionHandler: @ Save r0-r12, lr stmfd sp!, {r0-r12, lr} - mov r7, #EXCEPT_TYPE_UNKNOWN + mov r7, #EV3_PANIC_UNKNOWN @ b CommonPanicHandler @ fall through diff --git a/lib/pbio/platform/ev3/exceptionhandler.h b/lib/pbio/platform/ev3/exceptionhandler.h new file mode 100644 index 000000000..bf1b58e5e --- /dev/null +++ b/lib/pbio/platform/ev3/exceptionhandler.h @@ -0,0 +1,9 @@ +#ifndef PBIO_EV3_EXCEPTIONHANDLER_H_ +#define PBIO_EV3_EXCEPTIONHANDLER_H_ + +#define EV3_PANIC_UNKNOWN 0 +#define EV3_PANIC_UNDEFINED_INSTR 1 +#define EV3_PANIC_PREFETCH_ABORT 2 +#define EV3_PANIC_DATA_ABORT 3 + +#endif // PBIO_EV3_EXCEPTIONHANDLER_H_ diff --git a/lib/pbio/platform/ev3/platform.c b/lib/pbio/platform/ev3/platform.c index 8ed303a8b..c89c888b7 100644 --- a/lib/pbio/platform/ev3/platform.c +++ b/lib/pbio/platform/ev3/platform.c @@ -59,6 +59,8 @@ #include #include +#include "exceptionhandler.h" + #include "../../drv/block_device/block_device_ev3.h" #include "../../drv/button/button_gpio.h" #include "../../drv/display/display_ev3.h" @@ -417,6 +419,7 @@ static const char *const panic_types[] = { "Prefetch Abort", "Data Abort", }; + typedef struct { uint32_t r13; uint32_t r14; @@ -481,7 +484,17 @@ void ev3_panic_handler(int except_type, ev3_panic_ctx *except_data) { panic_puts("\r\nR14: 0x"); panic_putu32(except_data->r14); panic_puts("\r\nR15: 0x"); - panic_putu32(except_data->exc_lr); + switch (except_type) { + case EV3_PANIC_PREFETCH_ABORT: + panic_putu32(except_data->exc_lr - 4); + break; + case EV3_PANIC_DATA_ABORT: + panic_putu32(except_data->exc_lr - 8); + break; + default: + panic_putu32(except_data->exc_lr); + break; + } panic_puts("\r\nSPSR: 0x"); panic_putu32(except_data->spsr);