From 424dcbf50f360d316dabccf523ecb7578e1bcaf4 Mon Sep 17 00:00:00 2001 From: Laurens Valk Date: Tue, 23 Dec 2025 14:36:17 +0100 Subject: [PATCH 01/14] pbio/drv/bluetooth_btstack: Add init hook. This is needed for the POSIX port, so we don't have to make special handlers to protect init from running twice. --- lib/pbio/drv/bluetooth/bluetooth_btstack.c | 6 ++++++ lib/pbio/drv/bluetooth/bluetooth_btstack.h | 11 +++++++++++ lib/pbio/drv/bluetooth/bluetooth_btstack_ev3.c | 4 ++++ lib/pbio/drv/bluetooth/bluetooth_btstack_stm32_hal.c | 6 ++++++ lib/pbio/test/drv/test_bluetooth_btstack.c | 3 +++ 5 files changed, 30 insertions(+) diff --git a/lib/pbio/drv/bluetooth/bluetooth_btstack.c b/lib/pbio/drv/bluetooth/bluetooth_btstack.c index 88f1102b5..60f7727c1 100644 --- a/lib/pbio/drv/bluetooth/bluetooth_btstack.c +++ b/lib/pbio/drv/bluetooth/bluetooth_btstack.c @@ -1055,6 +1055,12 @@ const uint8_t cc256x_init_script[] = {}; void pbdrv_bluetooth_init_hci(void) { + // Proceed to start Bluetooth process only if platform init passes. + pbio_error_t err = pbdrv_bluetooth_btstack_platform_init(); + if (err != PBIO_SUCCESS) { + return; + } + static btstack_packet_callback_registration_t hci_event_callback_registration; #if PBDRV_CONFIG_BLUETOOTH_BTSTACK_LE diff --git a/lib/pbio/drv/bluetooth/bluetooth_btstack.h b/lib/pbio/drv/bluetooth/bluetooth_btstack.h index 26e0304bd..65e47b04c 100644 --- a/lib/pbio/drv/bluetooth/bluetooth_btstack.h +++ b/lib/pbio/drv/bluetooth/bluetooth_btstack.h @@ -11,6 +11,17 @@ #include #include +#include + +/** + * Optional platform initialization before BTstack takes over. + * + * Unlike some BTstack init code, this is guaranteed to be called only once. + * + * @return Any error code to skip Bluetooth or ::PBIO_SUCCESS to proceed. + */ +pbio_error_t pbdrv_bluetooth_btstack_platform_init(void); + /** Chipset info */ typedef struct { /** Version */ diff --git a/lib/pbio/drv/bluetooth/bluetooth_btstack_ev3.c b/lib/pbio/drv/bluetooth/bluetooth_btstack_ev3.c index 5ec20340e..e5d40cead 100644 --- a/lib/pbio/drv/bluetooth/bluetooth_btstack_ev3.c +++ b/lib/pbio/drv/bluetooth/bluetooth_btstack_ev3.c @@ -66,6 +66,10 @@ static const hci_dump_t bluetooth_btstack_classic_hci_dump = { #define DEBUG_PRINT(...) #endif +pbio_error_t pbdrv_bluetooth_btstack_platform_init(void) { + return PBIO_SUCCESS; +} + void pbdrv_bluetooth_btstack_set_chipset(uint16_t lmp_pal_subversion) { const pbdrv_bluetooth_btstack_chipset_info_t *info = lmp_pal_subversion == cc2560_info.lmp_version ? &cc2560_info : &cc2560a_info; diff --git a/lib/pbio/drv/bluetooth/bluetooth_btstack_stm32_hal.c b/lib/pbio/drv/bluetooth/bluetooth_btstack_stm32_hal.c index f065cf02f..72b2e343f 100644 --- a/lib/pbio/drv/bluetooth/bluetooth_btstack_stm32_hal.c +++ b/lib/pbio/drv/bluetooth/bluetooth_btstack_stm32_hal.c @@ -24,6 +24,12 @@ #include +#include + +pbio_error_t pbdrv_bluetooth_btstack_platform_init(void) { + return PBIO_SUCCESS; +} + void pbdrv_bluetooth_btstack_set_chipset(uint16_t lmp_pal_subversion) { const pbdrv_bluetooth_btstack_platform_data_t *pdata = diff --git a/lib/pbio/test/drv/test_bluetooth_btstack.c b/lib/pbio/test/drv/test_bluetooth_btstack.c index 9d4559851..3a80bce04 100644 --- a/lib/pbio/test/drv/test_bluetooth_btstack.c +++ b/lib/pbio/test/drv/test_bluetooth_btstack.c @@ -773,6 +773,9 @@ static void handle_data_source(btstack_data_source_t *ds, btstack_data_source_c void pbdrv_bluetooth_btstack_set_chipset(uint16_t lmp_pal_subversion) { } +pbio_error_t pbdrv_bluetooth_btstack_platform_init(void) { + return PBIO_SUCCESS; +} static pbio_error_t test_btstack_run_loop_contiki_poll(pbio_os_state_t *state, void *context) { static btstack_data_source_t data_source; From 89c66a9948fcb61ab69da5fb7a6a6443161df3fb Mon Sep 17 00:00:00 2001 From: Laurens Valk Date: Tue, 23 Dec 2025 14:44:29 +0100 Subject: [PATCH 02/14] pbio/drv/bluetooth_btstack: Use standard mechanism for IRQ poll. Most BTstack ports do it this way, including embedded variants. This will let us use the POSIX libusb variant without modifying it. --- lib/pbio/drv/bluetooth/bluetooth_btstack.c | 23 +++++++++++-------- lib/pbio/drv/bluetooth/bluetooth_btstack.h | 2 -- .../drv/bluetooth/bluetooth_btstack_ev3.c | 8 +++---- .../bluetooth/bluetooth_btstack_stm32_hal.c | 4 ++-- lib/pbio/test/drv/test_bluetooth_btstack.c | 6 ++--- 5 files changed, 23 insertions(+), 20 deletions(-) diff --git a/lib/pbio/drv/bluetooth/bluetooth_btstack.c b/lib/pbio/drv/bluetooth/bluetooth_btstack.c index 60f7727c1..f3506272b 100644 --- a/lib/pbio/drv/bluetooth/bluetooth_btstack.c +++ b/lib/pbio/drv/bluetooth/bluetooth_btstack.c @@ -1000,6 +1000,17 @@ static void bluetooth_btstack_run_loop_execute(void) { // not used } +/** + * The event loop runs on every system poll, but we only need to go through + * the btstack data sources when explicitly requested. + */ +static volatile bool pbdrv_bluetooth_btstack_poll_requested; + +static void pbdrv_bluetooth_btstack_run_loop_trigger(void) { + pbdrv_bluetooth_btstack_poll_requested = true; + pbio_os_request_poll(); +} + static const btstack_run_loop_t bluetooth_btstack_run_loop = { .init = btstack_run_loop_base_init, .add_data_source = btstack_run_loop_base_add_data_source, @@ -1012,15 +1023,9 @@ static const btstack_run_loop_t bluetooth_btstack_run_loop = { .execute = bluetooth_btstack_run_loop_execute, .dump_timer = btstack_run_loop_base_dump_timer, .get_time_ms = pbdrv_clock_get_ms, + .poll_data_sources_from_irq = pbdrv_bluetooth_btstack_run_loop_trigger, }; -static bool do_poll_handler; - -void pbdrv_bluetooth_btstack_run_loop_trigger(void) { - do_poll_handler = true; - pbio_os_request_poll(); -} - static pbio_os_process_t pbdrv_bluetooth_hci_process; /** @@ -1029,8 +1034,8 @@ static pbio_os_process_t pbdrv_bluetooth_hci_process; */ static pbio_error_t pbdrv_bluetooth_hci_process_thread(pbio_os_state_t *state, void *context) { - if (do_poll_handler) { - do_poll_handler = false; + if (pbdrv_bluetooth_btstack_poll_requested) { + pbdrv_bluetooth_btstack_poll_requested = false; btstack_run_loop_base_poll_data_sources(); } diff --git a/lib/pbio/drv/bluetooth/bluetooth_btstack.h b/lib/pbio/drv/bluetooth/bluetooth_btstack.h index 65e47b04c..176457584 100644 --- a/lib/pbio/drv/bluetooth/bluetooth_btstack.h +++ b/lib/pbio/drv/bluetooth/bluetooth_btstack.h @@ -32,8 +32,6 @@ typedef struct { const uint32_t init_script_size; } pbdrv_bluetooth_btstack_chipset_info_t; -void pbdrv_bluetooth_btstack_run_loop_trigger(void); - /** * Hook called when BTstack reads the local version information. * diff --git a/lib/pbio/drv/bluetooth/bluetooth_btstack_ev3.c b/lib/pbio/drv/bluetooth/bluetooth_btstack_ev3.c index e5d40cead..4f80cb63f 100644 --- a/lib/pbio/drv/bluetooth/bluetooth_btstack_ev3.c +++ b/lib/pbio/drv/bluetooth/bluetooth_btstack_ev3.c @@ -222,7 +222,7 @@ static volatile bool rx_interrupts_enabled; void pbdrv_bluetooth_btstack_ev3_handle_tx_complete(void) { EDMA3DisableTransfer(EDMA_BASE, DMA_CHA_TX, EDMA3_TRIG_MODE_EVENT); dma_write_complete = true; - pbdrv_bluetooth_btstack_run_loop_trigger(); + btstack_run_loop_poll_data_sources_from_irq(); } static inline void uart_rx_interrupt_set_enabled(bool enabled) { @@ -254,11 +254,11 @@ static void uart_rx_interrupt_handler(void) { // RX buffer full, disable further RX interrupts until btstack consumes // some of the data. uart_rx_interrupt_set_enabled(false); - pbdrv_bluetooth_btstack_run_loop_trigger(); + btstack_run_loop_poll_data_sources_from_irq(); return; } if (read_buf && (size_t)read_buf_len <= lwrb_get_full(&uart_rx_pending_ring_buffer)) { - pbdrv_bluetooth_btstack_run_loop_trigger(); + btstack_run_loop_poll_data_sources_from_irq(); } } @@ -431,7 +431,7 @@ static void pbdrv_bluetooth_btstack_ev3_receive_block(uint8_t *buffer, read_buf = buffer; read_buf_len = len; - pbdrv_bluetooth_btstack_run_loop_trigger(); + btstack_run_loop_poll_data_sources_from_irq(); } static void pbdrv_bluetooth_btstack_ev3_send_block(const uint8_t *data, diff --git a/lib/pbio/drv/bluetooth/bluetooth_btstack_stm32_hal.c b/lib/pbio/drv/bluetooth/bluetooth_btstack_stm32_hal.c index 72b2e343f..86fdd5ab7 100644 --- a/lib/pbio/drv/bluetooth/bluetooth_btstack_stm32_hal.c +++ b/lib/pbio/drv/bluetooth/bluetooth_btstack_stm32_hal.c @@ -98,12 +98,12 @@ static void (*block_received)(void); void HAL_UART_TxCpltCallback(UART_HandleTypeDef *huart) { send_complete = true; - pbdrv_bluetooth_btstack_run_loop_trigger(); + btstack_run_loop_poll_data_sources_from_irq(); } void HAL_UART_RxCpltCallback(UART_HandleTypeDef *huart) { receive_complete = true; - pbdrv_bluetooth_btstack_run_loop_trigger(); + btstack_run_loop_poll_data_sources_from_irq(); } static int btstack_stm32_hal_init(const btstack_uart_config_t *config) { diff --git a/lib/pbio/test/drv/test_bluetooth_btstack.c b/lib/pbio/test/drv/test_bluetooth_btstack.c index 3a80bce04..f967a5677 100644 --- a/lib/pbio/test/drv/test_bluetooth_btstack.c +++ b/lib/pbio/test/drv/test_bluetooth_btstack.c @@ -789,20 +789,20 @@ static pbio_error_t test_btstack_run_loop_contiki_poll(pbio_os_state_t *state, v btstack_run_loop_enable_data_source_callbacks(&data_source, DATA_SOURCE_CALLBACK_POLL); btstack_run_loop_add_data_source(&data_source); - pbdrv_bluetooth_btstack_run_loop_trigger(); + btstack_run_loop_poll_data_sources_from_irq(); PBIO_OS_AWAIT_ONCE(state); tt_want_uint_op(callback_count, ==, 1); callback_count = 0; btstack_run_loop_disable_data_source_callbacks(&data_source, DATA_SOURCE_CALLBACK_POLL); - pbdrv_bluetooth_btstack_run_loop_trigger(); + btstack_run_loop_poll_data_sources_from_irq(); PBIO_OS_AWAIT_ONCE(state); tt_want_uint_op(callback_count, ==, 0); callback_count = 0; btstack_run_loop_enable_data_source_callbacks(&data_source, DATA_SOURCE_CALLBACK_POLL); btstack_run_loop_remove_data_source(&data_source); - pbdrv_bluetooth_btstack_run_loop_trigger(); + btstack_run_loop_poll_data_sources_from_irq(); PBIO_OS_AWAIT_ONCE(state); tt_want_uint_op(callback_count, ==, 0); From 0c8509052d012b1e9c16fbe711654019edc713ef Mon Sep 17 00:00:00 2001 From: Laurens Valk Date: Tue, 23 Dec 2025 14:57:37 +0100 Subject: [PATCH 03/14] pbio/drv/bluetooth_btstack: Add poll hook. This is needed for the POSIX port, which needs to poll files. --- lib/pbio/drv/bluetooth/bluetooth_btstack.c | 2 ++ lib/pbio/drv/bluetooth/bluetooth_btstack.h | 5 +++++ lib/pbio/drv/bluetooth/bluetooth_btstack_ev3.c | 3 +++ lib/pbio/drv/bluetooth/bluetooth_btstack_stm32_hal.c | 3 +++ lib/pbio/test/drv/test_bluetooth_btstack.c | 3 +++ 5 files changed, 16 insertions(+) diff --git a/lib/pbio/drv/bluetooth/bluetooth_btstack.c b/lib/pbio/drv/bluetooth/bluetooth_btstack.c index f3506272b..66638400b 100644 --- a/lib/pbio/drv/bluetooth/bluetooth_btstack.c +++ b/lib/pbio/drv/bluetooth/bluetooth_btstack.c @@ -1039,6 +1039,8 @@ static pbio_error_t pbdrv_bluetooth_hci_process_thread(pbio_os_state_t *state, v btstack_run_loop_base_poll_data_sources(); } + pbdrv_bluetooth_btstack_platform_poll(); + static pbio_os_timer_t btstack_timer = { .duration = 1, }; diff --git a/lib/pbio/drv/bluetooth/bluetooth_btstack.h b/lib/pbio/drv/bluetooth/bluetooth_btstack.h index 176457584..e78b9109e 100644 --- a/lib/pbio/drv/bluetooth/bluetooth_btstack.h +++ b/lib/pbio/drv/bluetooth/bluetooth_btstack.h @@ -22,6 +22,11 @@ */ pbio_error_t pbdrv_bluetooth_btstack_platform_init(void); +/** + * Optional platform poll handler, called on every process iteration. + */ +void pbdrv_bluetooth_btstack_platform_poll(void); + /** Chipset info */ typedef struct { /** Version */ diff --git a/lib/pbio/drv/bluetooth/bluetooth_btstack_ev3.c b/lib/pbio/drv/bluetooth/bluetooth_btstack_ev3.c index 4f80cb63f..37eb9b42f 100644 --- a/lib/pbio/drv/bluetooth/bluetooth_btstack_ev3.c +++ b/lib/pbio/drv/bluetooth/bluetooth_btstack_ev3.c @@ -70,6 +70,9 @@ pbio_error_t pbdrv_bluetooth_btstack_platform_init(void) { return PBIO_SUCCESS; } +void pbdrv_bluetooth_btstack_platform_poll(void) { +} + void pbdrv_bluetooth_btstack_set_chipset(uint16_t lmp_pal_subversion) { const pbdrv_bluetooth_btstack_chipset_info_t *info = lmp_pal_subversion == cc2560_info.lmp_version ? &cc2560_info : &cc2560a_info; diff --git a/lib/pbio/drv/bluetooth/bluetooth_btstack_stm32_hal.c b/lib/pbio/drv/bluetooth/bluetooth_btstack_stm32_hal.c index 86fdd5ab7..311dc5172 100644 --- a/lib/pbio/drv/bluetooth/bluetooth_btstack_stm32_hal.c +++ b/lib/pbio/drv/bluetooth/bluetooth_btstack_stm32_hal.c @@ -30,6 +30,9 @@ pbio_error_t pbdrv_bluetooth_btstack_platform_init(void) { return PBIO_SUCCESS; } +void pbdrv_bluetooth_btstack_platform_poll(void) { +} + void pbdrv_bluetooth_btstack_set_chipset(uint16_t lmp_pal_subversion) { const pbdrv_bluetooth_btstack_platform_data_t *pdata = diff --git a/lib/pbio/test/drv/test_bluetooth_btstack.c b/lib/pbio/test/drv/test_bluetooth_btstack.c index f967a5677..701078ecf 100644 --- a/lib/pbio/test/drv/test_bluetooth_btstack.c +++ b/lib/pbio/test/drv/test_bluetooth_btstack.c @@ -777,6 +777,9 @@ pbio_error_t pbdrv_bluetooth_btstack_platform_init(void) { return PBIO_SUCCESS; } +void pbdrv_bluetooth_btstack_platform_poll(void) { +} + static pbio_error_t test_btstack_run_loop_contiki_poll(pbio_os_state_t *state, void *context) { static btstack_data_source_t data_source; static uint32_t callback_count; From 252e0fd7997ae54bb078447faa33cd12a49dd3a6 Mon Sep 17 00:00:00 2001 From: Laurens Valk Date: Tue, 23 Dec 2025 15:44:40 +0100 Subject: [PATCH 04/14] pbio/drv/bluetooth_btstack: Read full HCI version info payload. Co-authored-by: James Aguilar --- lib/pbio/drv/bluetooth/bluetooth_btstack.c | 42 +++++++++++++------ lib/pbio/drv/bluetooth/bluetooth_btstack.h | 20 ++++++++- .../drv/bluetooth/bluetooth_btstack_ev3.c | 4 +- .../bluetooth/bluetooth_btstack_stm32_hal.c | 2 +- lib/pbio/test/drv/test_bluetooth_btstack.c | 2 +- 5 files changed, 52 insertions(+), 18 deletions(-) diff --git a/lib/pbio/drv/bluetooth/bluetooth_btstack.c b/lib/pbio/drv/bluetooth/bluetooth_btstack.c index 66638400b..e533f20c5 100644 --- a/lib/pbio/drv/bluetooth/bluetooth_btstack.c +++ b/lib/pbio/drv/bluetooth/bluetooth_btstack.c @@ -216,6 +216,32 @@ static void nordic_spp_packet_handler(uint8_t packet_type, uint16_t channel, uin } #endif // PBDRV_CONFIG_BLUETOOTH_BTSTACK_LE +/** + * Parses the HCI Read Local Version Information command response. + * + * @param [out] info The device info to populate. + * @param [in] payload The payload of the HCI event. + */ +static void parse_hci_local_version_information(pbdrv_bluetooth_btstack_local_version_info_t *info, const uint8_t *payload) { + + info->hci_version = payload[1]; + info->hci_revision = pbio_get_uint16_le(&payload[2]); + info->lmp_pal_version = payload[4]; + info->manufacturer = pbio_get_uint16_le(&payload[5]); + info->lmp_pal_subversion = pbio_get_uint16_le(&payload[7]); + + #if DEBUG + // Show version in ev3dev format. + uint16_t chip = (info->lmp_pal_subversion & 0x7C00) >> 10; + uint16_t min_ver = (info->lmp_pal_subversion & 0x007F); + uint16_t maj_ver = (info->lmp_pal_subversion & 0x0380) >> 7; + if (info->lmp_pal_subversion & 0x8000) { + maj_ver |= 0x0008; + } + DEBUG_PRINT("LMP %04x: TIInit_%d.%d.%d.bts\n", info->lmp_pal_subversion, chip, maj_ver, min_ver); + #endif +} + // currently, this function just handles the Powered Up handset control. static void packet_handler(uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size) { @@ -228,19 +254,9 @@ static void packet_handler(uint8_t packet_type, uint16_t channel, uint8_t *packe const uint8_t *rp = hci_event_command_complete_get_return_parameters(packet); switch (hci_event_command_complete_get_command_opcode(packet)) { case HCI_OPCODE_HCI_READ_LOCAL_VERSION_INFORMATION: { - uint16_t lmp_pal_subversion = pbio_get_uint16_le(&rp[7]); - pbdrv_bluetooth_btstack_set_chipset(lmp_pal_subversion); - - #if DEBUG - // Show version in ev3dev format. - uint16_t chip = (lmp_pal_subversion & 0x7C00) >> 10; - uint16_t min_ver = (lmp_pal_subversion & 0x007F); - uint16_t maj_ver = (lmp_pal_subversion & 0x0380) >> 7; - if (lmp_pal_subversion & 0x8000) { - maj_ver |= 0x0008; - } - DEBUG_PRINT("LMP %04x: TIInit_%d.%d.%d.bts\n", lmp_pal_subversion, chip, maj_ver, min_ver); - #endif + pbdrv_bluetooth_btstack_local_version_info_t info; + parse_hci_local_version_information(&info, rp); + pbdrv_bluetooth_btstack_set_chipset(&info); break; } default: diff --git a/lib/pbio/drv/bluetooth/bluetooth_btstack.h b/lib/pbio/drv/bluetooth/bluetooth_btstack.h index e78b9109e..0bd0b90f9 100644 --- a/lib/pbio/drv/bluetooth/bluetooth_btstack.h +++ b/lib/pbio/drv/bluetooth/bluetooth_btstack.h @@ -37,13 +37,31 @@ typedef struct { const uint32_t init_script_size; } pbdrv_bluetooth_btstack_chipset_info_t; +/** + * Local HCI version info returned by the chipset. + */ +typedef struct { + /** HCI version. */ + uint8_t hci_version; + /** HCI revision. */ + uint16_t hci_revision; + /** LMP/PAL version. */ + uint8_t lmp_pal_version; + /** Manufacturer. */ + uint16_t manufacturer; + /** LMP/PAL subversion. */ + uint16_t lmp_pal_subversion; +} pbdrv_bluetooth_btstack_local_version_info_t; + /** * Hook called when BTstack reads the local version information. * * This is called _after_ hci_set_chipset but _before_ the init script is sent * over the wire, so this can be used to dynamically select the init script. + * + * @param device_info The device information read from the Bluetooth chip. */ -void pbdrv_bluetooth_btstack_set_chipset(uint16_t lmp_pal_subversion); +void pbdrv_bluetooth_btstack_set_chipset(pbdrv_bluetooth_btstack_local_version_info_t *device_info); typedef struct { const hci_transport_t *(*transport_instance)(void); diff --git a/lib/pbio/drv/bluetooth/bluetooth_btstack_ev3.c b/lib/pbio/drv/bluetooth/bluetooth_btstack_ev3.c index 37eb9b42f..fe27f4443 100644 --- a/lib/pbio/drv/bluetooth/bluetooth_btstack_ev3.c +++ b/lib/pbio/drv/bluetooth/bluetooth_btstack_ev3.c @@ -73,8 +73,8 @@ pbio_error_t pbdrv_bluetooth_btstack_platform_init(void) { void pbdrv_bluetooth_btstack_platform_poll(void) { } -void pbdrv_bluetooth_btstack_set_chipset(uint16_t lmp_pal_subversion) { - const pbdrv_bluetooth_btstack_chipset_info_t *info = lmp_pal_subversion == cc2560_info.lmp_version ? +void pbdrv_bluetooth_btstack_set_chipset(pbdrv_bluetooth_btstack_local_version_info_t *device_info) { + const pbdrv_bluetooth_btstack_chipset_info_t *info = device_info->lmp_pal_subversion == cc2560_info.lmp_version ? &cc2560_info : &cc2560a_info; btstack_chipset_cc256x_set_init_script((uint8_t *)info->init_script, info->init_script_size); diff --git a/lib/pbio/drv/bluetooth/bluetooth_btstack_stm32_hal.c b/lib/pbio/drv/bluetooth/bluetooth_btstack_stm32_hal.c index 311dc5172..7888c801b 100644 --- a/lib/pbio/drv/bluetooth/bluetooth_btstack_stm32_hal.c +++ b/lib/pbio/drv/bluetooth/bluetooth_btstack_stm32_hal.c @@ -33,7 +33,7 @@ pbio_error_t pbdrv_bluetooth_btstack_platform_init(void) { void pbdrv_bluetooth_btstack_platform_poll(void) { } -void pbdrv_bluetooth_btstack_set_chipset(uint16_t lmp_pal_subversion) { +void pbdrv_bluetooth_btstack_set_chipset(pbdrv_bluetooth_btstack_local_version_info_t *device_info) { const pbdrv_bluetooth_btstack_platform_data_t *pdata = &pbdrv_bluetooth_btstack_platform_data; diff --git a/lib/pbio/test/drv/test_bluetooth_btstack.c b/lib/pbio/test/drv/test_bluetooth_btstack.c index 701078ecf..78b254072 100644 --- a/lib/pbio/test/drv/test_bluetooth_btstack.c +++ b/lib/pbio/test/drv/test_bluetooth_btstack.c @@ -770,7 +770,7 @@ static void handle_data_source(btstack_data_source_t *ds, btstack_data_source_c } } -void pbdrv_bluetooth_btstack_set_chipset(uint16_t lmp_pal_subversion) { +void pbdrv_bluetooth_btstack_set_chipset(pbdrv_bluetooth_btstack_local_version_info_t *device_info) { } pbio_error_t pbdrv_bluetooth_btstack_platform_init(void) { From 3fa8d78b43b21ef08777d71edc353499e76c60af Mon Sep 17 00:00:00 2001 From: Laurens Valk Date: Mon, 29 Dec 2025 10:03:11 +0100 Subject: [PATCH 05/14] pbio/platform/virtual_hub: Always enable USB mock. This will provide stdio even when Bluetooth is not enabled. The current Bluetooth mock driver does this too so we are currently duplicating it, but this will be replaced by an actual Bluetooth driver shortly. --- lib/pbio/platform/virtual_hub/pbdrvconfig.h | 2 -- lib/pbio/platform/virtual_hub/pbsysconfig.h | 2 +- 2 files changed, 1 insertion(+), 3 deletions(-) diff --git a/lib/pbio/platform/virtual_hub/pbdrvconfig.h b/lib/pbio/platform/virtual_hub/pbdrvconfig.h index 9e5a08b80..1130ebcf6 100644 --- a/lib/pbio/platform/virtual_hub/pbdrvconfig.h +++ b/lib/pbio/platform/virtual_hub/pbdrvconfig.h @@ -50,11 +50,9 @@ #define PBDRV_CONFIG_HAS_PORT_VCC_CONTROL (1) // USB mock driver used on CI. -#ifdef PBDRV_CONFIG_RUN_ON_CI #define PBDRV_CONFIG_USB (1) #define PBDRV_CONFIG_USB_SIMULATION (1) #define PBDRV_CONFIG_USB_MAX_PACKET_SIZE (64) #define PBDRV_CONFIG_USB_NUM_BUFFERED_PACKETS (2) #define PBDRV_CONFIG_USB_MFG_STR u"Pybricks" #define PBDRV_CONFIG_USB_PROD_STR u"Virtual Hub" -#endif // PBDRV_CONFIG_RUN_ON_CI diff --git a/lib/pbio/platform/virtual_hub/pbsysconfig.h b/lib/pbio/platform/virtual_hub/pbsysconfig.h index c19c10d26..7a24a7240 100644 --- a/lib/pbio/platform/virtual_hub/pbsysconfig.h +++ b/lib/pbio/platform/virtual_hub/pbsysconfig.h @@ -23,7 +23,7 @@ #define PBSYS_CONFIG_STATUS_LIGHT (0) #define PBSYS_CONFIG_STATUS_LIGHT_BATTERY (0) #define PBSYS_CONFIG_STATUS_LIGHT_BLUETOOTH (0) -#define PBSYS_CONFIG_STATUS_LIGHT_STATE_ANIMATIONS (1) +#define PBSYS_CONFIG_STATUS_LIGHT_STATE_ANIMATIONS (0) #define PBSYS_CONFIG_STATUS_LIGHT_STATE_ANIMATIONS_HUE (240) #define PBSYS_CONFIG_TELEMETRY (1) #define PBSYS_CONFIG_USER_PROGRAM (0) From 17a5b83d537e6649c00cfead265c11af38e3c902 Mon Sep 17 00:00:00 2001 From: Laurens Valk Date: Mon, 29 Dec 2025 10:04:44 +0100 Subject: [PATCH 06/14] pybricks.hubs.VirtualHub: Enable BLE features. This lets us test BLE features on the REPL as we enable the new Virtual Hub driver, so we can see it working. --- bricks/virtualhub/mpconfigport.h | 14 +++++++++++--- pybricks/hubs/pb_type_virtualhub.c | 29 +++++++++++++++++++++++++++++ 2 files changed, 40 insertions(+), 3 deletions(-) diff --git a/bricks/virtualhub/mpconfigport.h b/bricks/virtualhub/mpconfigport.h index becb70f4e..8324e11db 100644 --- a/bricks/virtualhub/mpconfigport.h +++ b/bricks/virtualhub/mpconfigport.h @@ -11,7 +11,7 @@ // Pybricks modules #define PYBRICKS_PY_COMMON (1) -#define PYBRICKS_PY_COMMON_BLE (0) +#define PYBRICKS_PY_COMMON_BLE (1) #define PYBRICKS_PY_COMMON_CHARGER (1) #define PYBRICKS_PY_COMMON_COLOR_LIGHT (1) #define PYBRICKS_PY_COMMON_CONTROL (1) @@ -29,14 +29,22 @@ #define PYBRICKS_PY_EV3DEVICES (0) #define PYBRICKS_PY_EXPERIMENTAL (1) #define PYBRICKS_PY_HUBS (1) -#define PYBRICKS_PY_IODEVICES (0) +#define PYBRICKS_PY_IODEVICES (1) +#define PYBRICKS_PY_IODEVICES_ANALOG_SENSOR (0) +#define PYBRICKS_PY_IODEVICES_DC_MOTOR (0) +#define PYBRICKS_PY_IODEVICES_I2C_DEVICE (0) +#define PYBRICKS_PY_IODEVICES_LUMP_DEVICE (0) +#define PYBRICKS_PY_IODEVICES_LWP3_DEVICE (1) +#define PYBRICKS_PY_IODEVICES_PUP_DEVICE (0) +#define PYBRICKS_PY_IODEVICES_UART_DEVICE (0) +#define PYBRICKS_PY_IODEVICES_XBOX_CONTROLLER (1) #define PYBRICKS_PY_NXTDEVICES (0) #define PYBRICKS_PY_PARAMETERS (1) #define PYBRICKS_PY_PARAMETERS_BUTTON (1) #define PYBRICKS_PY_PARAMETERS_ICON (0) #define PYBRICKS_PY_PARAMETERS_IMAGE (1) #define PYBRICKS_PY_PUPDEVICES (1) -#define PYBRICKS_PY_PUPDEVICES_REMOTE (0) +#define PYBRICKS_PY_PUPDEVICES_REMOTE (1) #define PYBRICKS_PY_DEVICES (1) #define PYBRICKS_PY_ROBOTICS (1) #define PYBRICKS_PY_ROBOTICS_DRIVEBASE_SPIKE (0) diff --git a/pybricks/hubs/pb_type_virtualhub.c b/pybricks/hubs/pb_type_virtualhub.c index 92e752915..d24d79694 100644 --- a/pybricks/hubs/pb_type_virtualhub.c +++ b/pybricks/hubs/pb_type_virtualhub.c @@ -14,6 +14,10 @@ #include #include +#include + +#include +#include #include #include @@ -26,11 +30,33 @@ typedef struct _hubs_VirtualHub_obj_t { mp_obj_t light; mp_obj_t screen; mp_obj_t system; + #if PYBRICKS_PY_COMMON_BLE + mp_obj_t ble; + #endif } hubs_VirtualHub_obj_t; static mp_obj_t hubs_VirtualHub_make_new(const mp_obj_type_t *type, size_t n_args, size_t n_kw, const mp_obj_t *args) { + PB_PARSE_ARGS_CLASS(n_args, n_kw, args, + PB_ARG_DEFAULT_OBJ(top_side, pb_type_Axis_Z_obj), + PB_ARG_DEFAULT_OBJ(front_side, pb_type_Axis_X_obj) + #if PYBRICKS_PY_COMMON_BLE + , PB_ARG_DEFAULT_NONE(broadcast_channel) + , PB_ARG_DEFAULT_OBJ(observe_channels, mp_const_empty_tuple_obj) + #endif + ); + + (void)top_side_in; + (void)front_side_in; + + + hubs_VirtualHub_obj_t *self = mp_obj_malloc(hubs_VirtualHub_obj_t, type); self->battery = MP_OBJ_FROM_PTR(&pb_module_battery); + + #if PYBRICKS_PY_COMMON_BLE + self->ble = pb_type_BLE_new(broadcast_channel_in, observe_channels_in); + #endif + self->buttons = pb_type_Keypad_obj_new(MP_OBJ_FROM_PTR(self), pb_type_button_pressed_hub_single_button); // FIXME: Implement lights. // self->light = common_ColorLight_internal_obj_new(pbsys_status_light_main); @@ -41,6 +67,9 @@ static mp_obj_t hubs_VirtualHub_make_new(const mp_obj_type_t *type, size_t n_arg static const pb_attr_dict_entry_t hubs_VirtualHub_attr_dict[] = { PB_DEFINE_CONST_ATTR_RO(MP_QSTR_battery, hubs_VirtualHub_obj_t, battery), + #if PYBRICKS_PY_COMMON_BLE + PB_DEFINE_CONST_ATTR_RO(MP_QSTR_ble, hubs_VirtualHub_obj_t, ble), + #endif PB_DEFINE_CONST_ATTR_RO(MP_QSTR_buttons, hubs_VirtualHub_obj_t, buttons), // PB_DEFINE_CONST_ATTR_RO(MP_QSTR_light, hubs_VirtualHub_obj_t, light), PB_DEFINE_CONST_ATTR_RO(MP_QSTR_screen, hubs_VirtualHub_obj_t, screen), From 709b9ee4f2f34b5a4c1f8eb0e006fb163174d939 Mon Sep 17 00:00:00 2001 From: Laurens Valk Date: Mon, 29 Dec 2025 10:05:50 +0100 Subject: [PATCH 07/14] pybricks.common.BLE: Drop single-init assert. In practice, we are allowing users to reinstantiate the hub. The question is whether we really should, but for now the simulation should be consistent with reality. --- pybricks/common/pb_type_ble.c | 2 -- 1 file changed, 2 deletions(-) diff --git a/pybricks/common/pb_type_ble.c b/pybricks/common/pb_type_ble.c index c7163c65a..1d8b8c2d4 100644 --- a/pybricks/common/pb_type_ble.c +++ b/pybricks/common/pb_type_ble.c @@ -539,8 +539,6 @@ static MP_DEFINE_CONST_OBJ_TYPE(pb_type_BLE, * @throws ValueError If either parameter contains an out of range channel number. */ mp_obj_t pb_type_BLE_new(mp_obj_t broadcast_channel_in, mp_obj_t observe_channels_in) { - // making the assumption that this is only called once before each pb_type_ble_start_cleanup() - assert(observed_data == NULL); // Validate channel arguments. if (broadcast_channel_in != mp_const_none && (mp_obj_get_int(broadcast_channel_in) < 0 || mp_obj_get_int(broadcast_channel_in) > UINT8_MAX)) { From 77695efed90db562872929bc68475a11e4a020e6 Mon Sep 17 00:00:00 2001 From: Laurens Valk Date: Mon, 29 Dec 2025 11:33:41 +0100 Subject: [PATCH 08/14] pbio/drv/bluetooth_btstack: Add hook for packet handler. The POSIX variant needs to handle specific packets that are not relevant to the other platforms. --- lib/pbio/drv/bluetooth/bluetooth_btstack.c | 3 +++ lib/pbio/drv/bluetooth/bluetooth_btstack.h | 10 ++++++++++ lib/pbio/drv/bluetooth/bluetooth_btstack_ev3.c | 3 +++ lib/pbio/drv/bluetooth/bluetooth_btstack_stm32_hal.c | 3 +++ lib/pbio/test/drv/test_bluetooth_btstack.c | 3 +++ 5 files changed, 22 insertions(+) diff --git a/lib/pbio/drv/bluetooth/bluetooth_btstack.c b/lib/pbio/drv/bluetooth/bluetooth_btstack.c index e533f20c5..28ded83ae 100644 --- a/lib/pbio/drv/bluetooth/bluetooth_btstack.c +++ b/lib/pbio/drv/bluetooth/bluetooth_btstack.c @@ -245,6 +245,9 @@ static void parse_hci_local_version_information(pbdrv_bluetooth_btstack_local_ve // currently, this function just handles the Powered Up handset control. static void packet_handler(uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size) { + // Platform-specific platform handler has priority. + pbdrv_bluetooth_btstack_platform_packet_handler(packet_type, channel, packet, size); + #if PBDRV_CONFIG_BLUETOOTH_BTSTACK_LE pbdrv_bluetooth_peripheral_t *peri = &peripheral_singleton; #endif // PBDRV_CONFIG_BLUETOOTH_BTSTACK_LE diff --git a/lib/pbio/drv/bluetooth/bluetooth_btstack.h b/lib/pbio/drv/bluetooth/bluetooth_btstack.h index 0bd0b90f9..436fe0018 100644 --- a/lib/pbio/drv/bluetooth/bluetooth_btstack.h +++ b/lib/pbio/drv/bluetooth/bluetooth_btstack.h @@ -27,6 +27,16 @@ pbio_error_t pbdrv_bluetooth_btstack_platform_init(void); */ void pbdrv_bluetooth_btstack_platform_poll(void); +/** + * Optional platform packet handler, called on HCI packets before common handler. + * + * @param [in] packet_type The HCI packet type. + * @param [in] channel The HCI channel. + * @param [in] packet Pointer to the raw packet data. + * @param [in] size Size of the packet data. + */ +void pbdrv_bluetooth_btstack_platform_packet_handler(uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size); + /** Chipset info */ typedef struct { /** Version */ diff --git a/lib/pbio/drv/bluetooth/bluetooth_btstack_ev3.c b/lib/pbio/drv/bluetooth/bluetooth_btstack_ev3.c index fe27f4443..bd249a6b0 100644 --- a/lib/pbio/drv/bluetooth/bluetooth_btstack_ev3.c +++ b/lib/pbio/drv/bluetooth/bluetooth_btstack_ev3.c @@ -73,6 +73,9 @@ pbio_error_t pbdrv_bluetooth_btstack_platform_init(void) { void pbdrv_bluetooth_btstack_platform_poll(void) { } +void pbdrv_bluetooth_btstack_platform_packet_handler(uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size) { +} + void pbdrv_bluetooth_btstack_set_chipset(pbdrv_bluetooth_btstack_local_version_info_t *device_info) { const pbdrv_bluetooth_btstack_chipset_info_t *info = device_info->lmp_pal_subversion == cc2560_info.lmp_version ? &cc2560_info : &cc2560a_info; diff --git a/lib/pbio/drv/bluetooth/bluetooth_btstack_stm32_hal.c b/lib/pbio/drv/bluetooth/bluetooth_btstack_stm32_hal.c index 7888c801b..fe5a5da41 100644 --- a/lib/pbio/drv/bluetooth/bluetooth_btstack_stm32_hal.c +++ b/lib/pbio/drv/bluetooth/bluetooth_btstack_stm32_hal.c @@ -33,6 +33,9 @@ pbio_error_t pbdrv_bluetooth_btstack_platform_init(void) { void pbdrv_bluetooth_btstack_platform_poll(void) { } +void pbdrv_bluetooth_btstack_platform_packet_handler(uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size) { +} + void pbdrv_bluetooth_btstack_set_chipset(pbdrv_bluetooth_btstack_local_version_info_t *device_info) { const pbdrv_bluetooth_btstack_platform_data_t *pdata = diff --git a/lib/pbio/test/drv/test_bluetooth_btstack.c b/lib/pbio/test/drv/test_bluetooth_btstack.c index 78b254072..bca369c40 100644 --- a/lib/pbio/test/drv/test_bluetooth_btstack.c +++ b/lib/pbio/test/drv/test_bluetooth_btstack.c @@ -780,6 +780,9 @@ pbio_error_t pbdrv_bluetooth_btstack_platform_init(void) { void pbdrv_bluetooth_btstack_platform_poll(void) { } +void pbdrv_bluetooth_btstack_platform_packet_handler(uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size) { +} + static pbio_error_t test_btstack_run_loop_contiki_poll(pbio_os_state_t *state, void *context) { static btstack_data_source_t data_source; static uint32_t callback_count; From fee7546e306a03772f935cb33c2ad0b33985ee11 Mon Sep 17 00:00:00 2001 From: Laurens Valk Date: Mon, 29 Dec 2025 12:20:55 +0100 Subject: [PATCH 09/14] pbio/debug: Add new module. This will redirect debug output to usb and uart, when available. --- bricks/_common/sources.mk | 1 + lib/pbio/drv/bluetooth/bluetooth.c | 4 ++-- lib/pbio/drv/uart/uart_debug_first_port.c | 26 +++-------------------- lib/pbio/drv/uart/uart_debug_first_port.h | 6 ++---- lib/pbio/drv/usb/usb.c | 17 ++++----------- lib/pbio/include/pbdrv/usb.h | 21 +++++------------- lib/pbio/include/pbio/debug.h | 16 ++++++++++++++ lib/pbio/src/debug.c | 21 ++++++++++++++++++ 8 files changed, 54 insertions(+), 58 deletions(-) create mode 100644 lib/pbio/include/pbio/debug.h create mode 100644 lib/pbio/src/debug.c diff --git a/bricks/_common/sources.mk b/bricks/_common/sources.mk index c13c99f74..c127d63e0 100644 --- a/bricks/_common/sources.mk +++ b/bricks/_common/sources.mk @@ -204,6 +204,7 @@ PBIO_SRC_C = $(addprefix lib/pbio/,\ src/color/util.c \ src/control_settings.c \ src/control.c \ + src/debug.c \ src/dcmotor.c \ src/differentiator.c \ src/drivebase.c \ diff --git a/lib/pbio/drv/bluetooth/bluetooth.c b/lib/pbio/drv/bluetooth/bluetooth.c index 130f87db2..e147b1e54 100644 --- a/lib/pbio/drv/bluetooth/bluetooth.c +++ b/lib/pbio/drv/bluetooth/bluetooth.c @@ -23,8 +23,8 @@ #define DEBUG 0 #if DEBUG -#include -#define DEBUG_PRINT pbdrv_uart_debug_printf +#include +#define DEBUG_PRINT pbio_debug #else #define DEBUG_PRINT(...) #endif diff --git a/lib/pbio/drv/uart/uart_debug_first_port.c b/lib/pbio/drv/uart/uart_debug_first_port.c index b2af38571..ba7d141cd 100644 --- a/lib/pbio/drv/uart/uart_debug_first_port.c +++ b/lib/pbio/drv/uart/uart_debug_first_port.c @@ -37,21 +37,18 @@ int pbdrv_uart_debug_next_char(void) { /** * Formats and stores a string in the UART debug ring buffer. * - * This function works similarly to vprintf, but instead of printing to the - * standard output. The formatted string will be written to the UART when the + * The formatted string will be written to the UART when the * buffer is processed. * * @param format The format string, similar to printf. * @param va_list The variable arguments, as a va_list. */ -void pbdrv_uart_debug_vprintf(const char *format, va_list args) { +void pbdrv_uart_debug_print(const char *data, size_t len) { if (!lwrb_is_ready(&ring_buffer)) { lwrb_init(&ring_buffer, ring_buf_storage, sizeof(ring_buf_storage)); } - char buf[256]; - size_t len = vsnprintf(buf, sizeof(buf), format, args); - lwrb_write(&ring_buffer, (const uint8_t *)buf, len); + lwrb_write(&ring_buffer, (const uint8_t *)data, len); if (!debug_uart) { // Not initialized yet, so just buffer for now. @@ -62,23 +59,6 @@ void pbdrv_uart_debug_vprintf(const char *format, va_list args) { pbio_os_request_poll(); } -/** - * Formats and stores a string in the UART debug ring buffer. - * - * This function works similarly to printf, but instead of printing to the - * standard output. The formatted string will be written to the UART when the - * buffer is processed. - * - * @param format The format string, similar to printf. - * @param ... The variable arguments, similar to printf. - */ -void pbdrv_uart_debug_printf(const char *format, ...) { - va_list args; - va_start(args, format); - pbdrv_uart_debug_vprintf(format, args); - va_end(args); -} - /** * Gets a character from the UART debug port. * diff --git a/lib/pbio/drv/uart/uart_debug_first_port.h b/lib/pbio/drv/uart/uart_debug_first_port.h index e251b7f75..f6e1e05fe 100644 --- a/lib/pbio/drv/uart/uart_debug_first_port.h +++ b/lib/pbio/drv/uart/uart_debug_first_port.h @@ -12,8 +12,7 @@ #if PBDRV_CONFIG_UART_DEBUG_FIRST_PORT -void pbdrv_uart_debug_printf(const char *format, ...); -void pbdrv_uart_debug_vprintf(const char *format, va_list argptr); +void pbdrv_uart_debug_print(const char *data, size_t len); bool pbdrv_uart_debug_is_done(void); @@ -26,8 +25,7 @@ int pbdrv_uart_debug_next_char(void); #else // PBDRV_CONFIG_UART_DEBUG_FIRST_PORT -#define pbdrv_uart_debug_printf(...) -#define pbdrv_uart_debug_vprintf(format, argptr) +#define pbdrv_uart_debug_print(data, len) #define pbdrv_uart_debug_is_done() (true) diff --git a/lib/pbio/drv/usb/usb.c b/lib/pbio/drv/usb/usb.c index 30e3f55d4..e6ad44b78 100644 --- a/lib/pbio/drv/usb/usb.c +++ b/lib/pbio/drv/usb/usb.c @@ -108,32 +108,23 @@ bool pbdrv_usb_stdout_tx_is_idle(void) { return lwrb_get_full(&pbdrv_usb_stdout_ring_buf) == 0 && !pbdrv_usb_noti_size[PBIO_PYBRICKS_EVENT_WRITE_STDOUT]; } -void pbdrv_usb_debug_vprintf(const char *format, va_list args) { +void pbdrv_usb_debug_print(const char *data, size_t len) { + if (!lwrb_is_ready(&pbdrv_usb_stdout_ring_buf)) { return; } - char buf[256]; - size_t len = vsnprintf(buf, sizeof(buf), format, args); - // Buffer result with \r injected before \n. for (size_t i = 0; i < len; i++) { - if (buf[i] == '\n') { + if (data[i] == '\n') { lwrb_write(&pbdrv_usb_stdout_ring_buf, (const uint8_t *)"\r", 1); } - lwrb_write(&pbdrv_usb_stdout_ring_buf, (const uint8_t *)&buf[i], 1); + lwrb_write(&pbdrv_usb_stdout_ring_buf, (const uint8_t *)&data[i], 1); } pbio_os_request_poll(); } -void pbdrv_usb_debug_printf(const char *format, ...) { - va_list args; - va_start(args, format); - pbdrv_usb_debug_vprintf(format, args); - va_end(args); -} - pbio_error_t pbdrv_usb_send_event_notification(pbio_os_state_t *state, pbio_pybricks_event_t event_type, const uint8_t *data, size_t size) { PBIO_OS_ASYNC_BEGIN(state); diff --git a/lib/pbio/include/pbdrv/usb.h b/lib/pbio/include/pbdrv/usb.h index e31084fd2..c68ef857f 100644 --- a/lib/pbio/include/pbdrv/usb.h +++ b/lib/pbio/include/pbdrv/usb.h @@ -115,20 +115,12 @@ bool pbdrv_usb_connection_is_active(void); pbio_error_t pbdrv_usb_send_event_notification(pbio_os_state_t *state, pbio_pybricks_event_t event, const uint8_t *data, size_t size); /** - * Formats and stores a string in the USB stdout ring buffer using a va_list. + * Stores a string in the USB stdout ring buffer. * - * @param format The format string, similar to printf. - * @param args The variable arguments, as a va_list. + * @param data The string data. + * @param len The length of the string data. */ -void pbdrv_usb_debug_vprintf(const char *format, va_list args); - -/** - * Formats and stores a string in the USB stdout ring buffer. - * - * @param format The format string, similar to printf. - * @param ... The variable arguments, similar to printf. - */ -void pbdrv_usb_debug_printf(const char *format, ...); +void pbdrv_usb_debug_print(const char *data, size_t len); #else // PBDRV_CONFIG_USB @@ -162,10 +154,7 @@ static inline pbio_error_t pbdrv_usb_send_event_notification(pbio_os_state_t *st return PBIO_ERROR_NOT_SUPPORTED; } -static inline void pbdrv_usb_debug_vprintf(const char *format, va_list args) { -} - -static inline void pbdrv_usb_debug_printf(const char *format, ...) { +static inline void pbdrv_usb_debug_print(const char *data, size_t len) { } #endif // PBDRV_CONFIG_USB diff --git a/lib/pbio/include/pbio/debug.h b/lib/pbio/include/pbio/debug.h new file mode 100644 index 000000000..466cb93e8 --- /dev/null +++ b/lib/pbio/include/pbio/debug.h @@ -0,0 +1,16 @@ +// SPDX-License-Identifier: MIT +// Copyright (c) 2025 The Pybricks Authors + +#ifndef _PBIO_DEBUG_H_ +#define _PBIO_DEBUG_H_ + +#include +#include +#include +#include + +void pbio_debug_va(const char *format, va_list args); + +void pbio_debug(const char *format, ...); + +#endif // _PBIO_DEBUG_H_ diff --git a/lib/pbio/src/debug.c b/lib/pbio/src/debug.c new file mode 100644 index 000000000..506e5ccfa --- /dev/null +++ b/lib/pbio/src/debug.c @@ -0,0 +1,21 @@ +// SPDX-License-Identifier: MIT +// Copyright (c) 2021-2025 The Pybricks Authors + +#include +#include + +#include + +void pbio_debug_va(const char *format, va_list args) { + char buf[256]; + size_t len = vsnprintf(buf, sizeof(buf), format, args); + pbdrv_usb_debug_print(buf, len); + pbdrv_uart_debug_print(buf, len); +} + +void pbio_debug(const char *format, ...) { + va_list args; + va_start(args, format); + pbio_debug_va(format, args); + va_end(args); +} From 0772d8713463d04a3fae67a59539a5e7c2e7b70c Mon Sep 17 00:00:00 2001 From: Laurens Valk Date: Mon, 29 Dec 2025 12:27:19 +0100 Subject: [PATCH 10/14] pbio/debug: Replace usage with common module. No need to hook directly into the uart driver anymore. --- lib/pbio/drv/bluetooth/bluetooth_btstack.c | 4 +-- .../drv/bluetooth/bluetooth_btstack_ev3.c | 10 ++++---- .../drv/bluetooth/bluetooth_stm32_bluenrg.c | 4 +-- .../drv/bluetooth/bluetooth_stm32_cc2640.c | 6 ++--- lib/pbio/drv/counter/counter_ev3.c | 4 +-- lib/pbio/drv/i2c/i2c_ev3.c | 4 +-- lib/pbio/drv/motor_driver/motor_driver_ev3.c | 4 +-- lib/pbio/drv/uart/uart_debug_first_port.h | 3 --- lib/pbio/drv/usb/usb_stm32.c | 9 ------- lib/pbio/src/port.c | 4 +-- lib/pbio/src/port_dcm_pup.c | 4 +-- lib/pbio/src/port_lump.c | 4 +-- lib/pbio/sys/battery_temp.c | 25 +++++++++++++------ lib/pbio/sys/hmi_lcd.c | 4 +-- lib/pbio/sys/hmi_pup.c | 4 +-- 15 files changed, 44 insertions(+), 49 deletions(-) diff --git a/lib/pbio/drv/bluetooth/bluetooth_btstack.c b/lib/pbio/drv/bluetooth/bluetooth_btstack.c index 28ded83ae..d979c4351 100644 --- a/lib/pbio/drv/bluetooth/bluetooth_btstack.c +++ b/lib/pbio/drv/bluetooth/bluetooth_btstack.c @@ -46,8 +46,8 @@ #define DEBUG 0 #if DEBUG -#include -#define DEBUG_PRINT pbdrv_uart_debug_printf +#include +#define DEBUG_PRINT pbio_debug #else #define DEBUG_PRINT(...) #endif diff --git a/lib/pbio/drv/bluetooth/bluetooth_btstack_ev3.c b/lib/pbio/drv/bluetooth/bluetooth_btstack_ev3.c index bd249a6b0..e27d11aea 100644 --- a/lib/pbio/drv/bluetooth/bluetooth_btstack_ev3.c +++ b/lib/pbio/drv/bluetooth/bluetooth_btstack_ev3.c @@ -46,16 +46,16 @@ #define DEBUG 0 #if DEBUG -#include -#define DEBUG_PRINT pbdrv_uart_debug_printf +#include +#define DEBUG_PRINT pbio_debug static void pbdrv_hci_dump_reset(void) { } static void pbdrv_hci_dump_log_packet(uint8_t packet_type, uint8_t in, uint8_t *packet, uint16_t len) { - pbdrv_uart_debug_printf("HCI %s packet type: %02x, len: %u\n", in ? "in" : "out", packet_type, len); + pbio_debug("HCI %s packet type: %02x, len: %u\n", in ? "in" : "out", packet_type, len); } static void pbdrv_hci_dump_log_message(int log_level, const char *format, va_list argptr) { - pbdrv_uart_debug_vprintf(format, argptr); - pbdrv_uart_debug_printf("\n"); + pbio_debug_va(format, argptr); + pbio_debug("\n"); } static const hci_dump_t bluetooth_btstack_classic_hci_dump = { .reset = pbdrv_hci_dump_reset, diff --git a/lib/pbio/drv/bluetooth/bluetooth_stm32_bluenrg.c b/lib/pbio/drv/bluetooth/bluetooth_stm32_bluenrg.c index 2702b2ef0..a42141820 100644 --- a/lib/pbio/drv/bluetooth/bluetooth_stm32_bluenrg.c +++ b/lib/pbio/drv/bluetooth/bluetooth_stm32_bluenrg.c @@ -38,8 +38,8 @@ #define DEBUG 0 #if DEBUG -#include -#define DEBUG_PRINT pbdrv_uart_debug_printf +#include +#define DEBUG_PRINT pbio_debug #else #define DEBUG_PRINT(...) #endif diff --git a/lib/pbio/drv/bluetooth/bluetooth_stm32_cc2640.c b/lib/pbio/drv/bluetooth/bluetooth_stm32_cc2640.c index b255444d0..e797068fd 100644 --- a/lib/pbio/drv/bluetooth/bluetooth_stm32_cc2640.c +++ b/lib/pbio/drv/bluetooth/bluetooth_stm32_cc2640.c @@ -46,11 +46,9 @@ #define DEBUG 0 #if DEBUG -#include -// You can selectively enable these as relevant for the debug session. -// Remember to set PBDRV_CONFIG_UART_DEBUG_FIRST_PORT in pbdrvconfig. +#include #define DBG(...) -#define DEBUG_PRINT pbdrv_uart_debug_printf +#define DEBUG_PRINT pbio_debug #else #define DBG(...) #define DEBUG_PRINT(...) diff --git a/lib/pbio/drv/counter/counter_ev3.c b/lib/pbio/drv/counter/counter_ev3.c index 3b4db62b1..f76369b13 100644 --- a/lib/pbio/drv/counter/counter_ev3.c +++ b/lib/pbio/drv/counter/counter_ev3.c @@ -33,8 +33,8 @@ #if DEBUG #include #include -#include -#define DEBUG_PRINT pbdrv_uart_debug_printf +#include +#define DEBUG_PRINT pbio_debug #else #define DEBUG_PRINT(...) #endif diff --git a/lib/pbio/drv/i2c/i2c_ev3.c b/lib/pbio/drv/i2c/i2c_ev3.c index dfbb6bec8..d3fdfb744 100644 --- a/lib/pbio/drv/i2c/i2c_ev3.c +++ b/lib/pbio/drv/i2c/i2c_ev3.c @@ -31,8 +31,8 @@ #if DEBUG #include #include -#include -#define debug_pr pbdrv_uart_debug_printf +#include +#define debug_pr pbio_debug #define DBG_ERR(expr) expr #else #define debug_pr(...) diff --git a/lib/pbio/drv/motor_driver/motor_driver_ev3.c b/lib/pbio/drv/motor_driver/motor_driver_ev3.c index 98fce23a2..3db1ac3c3 100644 --- a/lib/pbio/drv/motor_driver/motor_driver_ev3.c +++ b/lib/pbio/drv/motor_driver/motor_driver_ev3.c @@ -33,8 +33,8 @@ #if DEBUG #include #include -#include -#define debug_pr pbdrv_uart_debug_printf +#include +#define debug_pr pbio_debug #define DBG_ERR(expr) expr #else #define debug_pr(...) diff --git a/lib/pbio/drv/uart/uart_debug_first_port.h b/lib/pbio/drv/uart/uart_debug_first_port.h index f6e1e05fe..b1cb08014 100644 --- a/lib/pbio/drv/uart/uart_debug_first_port.h +++ b/lib/pbio/drv/uart/uart_debug_first_port.h @@ -37,7 +37,4 @@ static inline int pbdrv_uart_debug_next_char(void) { #endif // PBDRV_CONFIG_UART_DEBUG_FIRST_PORT -// Convenient shorthand for pbdrv_uart_debug_printf. -#define pbdrv_dbg(...) pbdrv_uart_debug_printf(__VA_ARGS__) - #endif // _INTERNAL_PBDRV_UART_DEBUG_FIRST_PORT_H_ diff --git a/lib/pbio/drv/usb/usb_stm32.c b/lib/pbio/drv/usb/usb_stm32.c index 4f10f103d..b474d3438 100644 --- a/lib/pbio/drv/usb/usb_stm32.c +++ b/lib/pbio/drv/usb/usb_stm32.c @@ -31,15 +31,6 @@ #include "./usb.h" #include "./usb_stm32.h" -#define DEBUG 0 - -#if DEBUG -#include -// Remember to set PBDRV_CONFIG_UART_DEBUG_FIRST_PORT in pbdrvconfig. -#define DEBUG_PRINT pbdrv_uart_debug_printf -#else -#define DEBUG_PRINT(...) -#endif #if USBD_PYBRICKS_MAX_PACKET_SIZE != PBDRV_CONFIG_USB_MAX_PACKET_SIZE #error Inconsistent USB packet size diff --git a/lib/pbio/src/port.c b/lib/pbio/src/port.c index 0a4b64b60..9cd02f5e8 100644 --- a/lib/pbio/src/port.c +++ b/lib/pbio/src/port.c @@ -25,8 +25,8 @@ #if DEBUG #include #include -#include -#define debug_pr pbdrv_uart_debug_printf +#include +#define debug_pr pbio_debug #define DBG_ERR(expr) expr #else #define debug_pr(...) diff --git a/lib/pbio/src/port_dcm_pup.c b/lib/pbio/src/port_dcm_pup.c index 7d3aa2bde..767c8aa6c 100644 --- a/lib/pbio/src/port_dcm_pup.c +++ b/lib/pbio/src/port_dcm_pup.c @@ -16,8 +16,8 @@ #if DEBUG #include #include -#include -#define debug_pr pbdrv_uart_debug_printf +#include +#define debug_pr pbio_debug #define DBG_ERR(expr) expr #else #define debug_pr(...) diff --git a/lib/pbio/src/port_lump.c b/lib/pbio/src/port_lump.c index 65f4db3a1..370737d6d 100644 --- a/lib/pbio/src/port_lump.c +++ b/lib/pbio/src/port_lump.c @@ -21,8 +21,8 @@ #if DEBUG #include #include -#include -#define debug_pr pbdrv_uart_debug_printf +#include +#define debug_pr pbio_debug #define DBG_ERR(expr) expr #else #define debug_pr(...) diff --git a/lib/pbio/sys/battery_temp.c b/lib/pbio/sys/battery_temp.c index cdbb7e0f4..3466b4079 100644 --- a/lib/pbio/sys/battery_temp.c +++ b/lib/pbio/sys/battery_temp.c @@ -8,6 +8,15 @@ // LEGO MINDSTORMS EV3 battery temperature estimation from lms2012 #if PBSYS_CONFIG_BATTERY_TEMP_ESTIMATION +#define DEBUG 0 + +#if DEBUG +#include +#define DEBUG_PRINT pbio_debug +#else +#define DEBUG_PRINT(...) +#endif + /** * Function for estimating new battery temperature based on measurements * of battery voltage and battery current. @@ -132,9 +141,9 @@ float pbsys_battery_temp_update(float V_bat, float I_bat) { // Save R_bat_model for use in the next function call bat_temp.R_bat_model_old = R_bat_model; - // pbdrv_uart_debug_printf("%c %f %f %f %f %f %f\r\n", bat_temp.has_passed_7v5_flag ? 'Y' : 'N', - // (double)R_1A, (double)R_2A, (double)slope_A, (double)intercept_b, - // (double)(R_bat_model - bat_temp.R_bat_model_old), (double)bat_temp.R_bat); + DEBUG_PRINT("%c %f %f %f %f %f %f\r\n", bat_temp.has_passed_7v5_flag ? 'Y' : 'N', + (double)R_1A, (double)R_2A, (double)slope_A, (double)intercept_b, + (double)(R_bat_model - bat_temp.R_bat_model_old), (double)bat_temp.R_bat); /**** Calculate the 4 types of temperature change for the batteries ****/ @@ -182,11 +191,11 @@ float pbsys_battery_temp_update(float V_bat, float I_bat) { /*****************************************************************************/ - // pbdrv_uart_debug_printf("%f %f %f %f %f <> %f %f %f %f %f\r\n", - // (double)dT_bat_own, (double)dT_bat_loss_to_elec, - // (double)dT_bat_gain_from_elec, (double)dT_bat_loss_to_room, (double)bat_temp.T_bat, - // (double)dT_elec_own, (double)dT_elec_loss_to_bat, (double)dT_elec_gain_from_bat, - // (double)dT_elec_loss_to_room, (double)bat_temp.T_elec); + DEBUG_PRINT("%f %f %f %f %f <> %f %f %f %f %f\r\n", + (double)dT_bat_own, (double)dT_bat_loss_to_elec, + (double)dT_bat_gain_from_elec, (double)dT_bat_loss_to_room, (double)bat_temp.T_bat, + (double)dT_elec_own, (double)dT_elec_loss_to_bat, (double)dT_elec_gain_from_bat, + (double)dT_elec_loss_to_room, (double)bat_temp.T_elec); // Refresh battery temperature bat_temp.T_bat += dT_bat_own + dT_bat_loss_to_elec + dT_bat_gain_from_elec + dT_bat_loss_to_room; diff --git a/lib/pbio/sys/hmi_lcd.c b/lib/pbio/sys/hmi_lcd.c index 49f134ac8..c2436d594 100644 --- a/lib/pbio/sys/hmi_lcd.c +++ b/lib/pbio/sys/hmi_lcd.c @@ -32,8 +32,8 @@ #define DEBUG 0 #if DEBUG -#include -#define DEBUG_PRINT pbdrv_uart_debug_printf +#include +#define DEBUG_PRINT pbio_debug #else #define DEBUG_PRINT(...) #endif diff --git a/lib/pbio/sys/hmi_pup.c b/lib/pbio/sys/hmi_pup.c index ef12eff20..defb9606a 100644 --- a/lib/pbio/sys/hmi_pup.c +++ b/lib/pbio/sys/hmi_pup.c @@ -31,8 +31,8 @@ #define DEBUG 0 #if DEBUG -#include -#define DEBUG_PRINT pbdrv_uart_debug_printf +#include +#define DEBUG_PRINT pbio_debug #else #define DEBUG_PRINT(...) #endif From 58765614ca697430dafba71684279e058a7c001e Mon Sep 17 00:00:00 2001 From: Laurens Valk Date: Mon, 29 Dec 2025 12:40:21 +0100 Subject: [PATCH 11/14] pbio/drv/bluetooth_btstack: Use common HCI logging. We don't need duplicates for EV3 and the POSIX variants. --- lib/pbio/drv/bluetooth/bluetooth_btstack.c | 24 +++++++++++++++++++ .../drv/bluetooth/bluetooth_btstack_ev3.c | 21 ---------------- 2 files changed, 24 insertions(+), 21 deletions(-) diff --git a/lib/pbio/drv/bluetooth/bluetooth_btstack.c b/lib/pbio/drv/bluetooth/bluetooth_btstack.c index d979c4351..84d3b39ee 100644 --- a/lib/pbio/drv/bluetooth/bluetooth_btstack.c +++ b/lib/pbio/drv/bluetooth/bluetooth_btstack.c @@ -52,6 +52,23 @@ #define DEBUG_PRINT(...) #endif +#if DEBUG == 2 +static void btstack_hci_dump_reset(void) { +} +static void btstack_hci_dump_log_packet(uint8_t packet_type, uint8_t in, uint8_t *packet, uint16_t len) { + pbio_debug("HCI %s packet type: %02x, len: %u\n", in ? "in" : "out", packet_type, len); +} +static void btstack_hci_dump_log_message(int log_level, const char *format, va_list argptr) { + pbio_debug_va(format, argptr); + pbio_debug("\n"); +} +static const hci_dump_t pbdrv_bluetooth_btstack_hci_dump = { + .reset = btstack_hci_dump_reset, + .log_packet = btstack_hci_dump_log_packet, + .log_message = btstack_hci_dump_log_message, +}; +#endif + typedef enum { CON_STATE_NONE, CON_STATE_WAIT_ADV_IND, @@ -1087,6 +1104,13 @@ void pbdrv_bluetooth_init_hci(void) { return; } + #if DEBUG == 2 + hci_dump_init(&pbdrv_bluetooth_btstack_hci_dump); + hci_dump_enable_log_level(HCI_DUMP_LOG_LEVEL_INFO, true); + hci_dump_enable_log_level(HCI_DUMP_LOG_LEVEL_ERROR, true); + hci_dump_enable_log_level(HCI_DUMP_LOG_LEVEL_DEBUG, true); + #endif + static btstack_packet_callback_registration_t hci_event_callback_registration; #if PBDRV_CONFIG_BLUETOOTH_BTSTACK_LE diff --git a/lib/pbio/drv/bluetooth/bluetooth_btstack_ev3.c b/lib/pbio/drv/bluetooth/bluetooth_btstack_ev3.c index e27d11aea..e6442470f 100644 --- a/lib/pbio/drv/bluetooth/bluetooth_btstack_ev3.c +++ b/lib/pbio/drv/bluetooth/bluetooth_btstack_ev3.c @@ -48,20 +48,6 @@ #if DEBUG #include #define DEBUG_PRINT pbio_debug -static void pbdrv_hci_dump_reset(void) { -} -static void pbdrv_hci_dump_log_packet(uint8_t packet_type, uint8_t in, uint8_t *packet, uint16_t len) { - pbio_debug("HCI %s packet type: %02x, len: %u\n", in ? "in" : "out", packet_type, len); -} -static void pbdrv_hci_dump_log_message(int log_level, const char *format, va_list argptr) { - pbio_debug_va(format, argptr); - pbio_debug("\n"); -} -static const hci_dump_t bluetooth_btstack_classic_hci_dump = { - .reset = pbdrv_hci_dump_reset, - .log_packet = pbdrv_hci_dump_log_packet, - .log_message = pbdrv_hci_dump_log_message, -}; #else #define DEBUG_PRINT(...) #endif @@ -146,13 +132,6 @@ static void ev3_control_init(const void *config) { // Start the module in a defined (and disabled) state. ev3_control_off(); - #if DEBUG - hci_dump_init(&bluetooth_btstack_classic_hci_dump); - hci_dump_enable_log_level(HCI_DUMP_LOG_LEVEL_INFO, true); - hci_dump_enable_log_level(HCI_DUMP_LOG_LEVEL_ERROR, true); - hci_dump_enable_log_level(HCI_DUMP_LOG_LEVEL_DEBUG, true); - #endif - DEBUG_PRINT("Bluetooth: Finished init control\n"); } From 2e656ff3da82216f227aad9c870d9bc9e91bd656 Mon Sep 17 00:00:00 2001 From: Laurens Valk Date: Mon, 29 Dec 2025 12:49:03 +0100 Subject: [PATCH 12/14] pbio/drv/bluetooth_btstack: Don't call hci_set_chipset during init. We call this again when we discover the local version. We shouldn't be calling this twice. --- lib/pbio/drv/bluetooth/bluetooth_btstack.c | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/lib/pbio/drv/bluetooth/bluetooth_btstack.c b/lib/pbio/drv/bluetooth/bluetooth_btstack.c index 84d3b39ee..5d31e11f6 100644 --- a/lib/pbio/drv/bluetooth/bluetooth_btstack.c +++ b/lib/pbio/drv/bluetooth/bluetooth_btstack.c @@ -1121,8 +1121,9 @@ void pbdrv_bluetooth_init_hci(void) { btstack_memory_init(); btstack_run_loop_init(&bluetooth_btstack_run_loop); + // Chipset is not set here. This is done dynamically when we get the + // local version information. hci_init(pdata->transport_instance(), pdata->transport_config()); - hci_set_chipset(pdata->chipset_instance()); hci_set_control(pdata->control_instance()); // REVISIT: do we need to call btstack_chipset_cc256x_set_power() or btstack_chipset_cc256x_set_power_vector()? From 3e7a286f571a5a4fea1cd12b554ee1609708c2f7 Mon Sep 17 00:00:00 2001 From: Laurens Valk Date: Mon, 29 Dec 2025 14:39:00 +0100 Subject: [PATCH 13/14] pbio/drv/bluetooth: Fix peripheral disconnect if config unset. The pbdrv_bluetooth_is_connected check should protect against this, but if it fails we get a segfault if config was never set, so if no peripheral was ever connected. --- lib/pbio/drv/bluetooth/bluetooth.c | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/lib/pbio/drv/bluetooth/bluetooth.c b/lib/pbio/drv/bluetooth/bluetooth.c index e147b1e54..e37032dfc 100644 --- a/lib/pbio/drv/bluetooth/bluetooth.c +++ b/lib/pbio/drv/bluetooth/bluetooth.c @@ -188,8 +188,12 @@ pbio_error_t pbdrv_bluetooth_peripheral_disconnect(void) { return PBIO_SUCCESS; } + // Clear notification handler to avoid receiving further data. + if (peri->config) { + peri->config->notification_handler = NULL; + } + // Initialize operation for handling on the main thread. - peri->config->notification_handler = NULL; peri->func = pbdrv_bluetooth_peripheral_disconnect_func; peri->err = PBIO_ERROR_AGAIN; pbio_os_request_poll(); From a17c1d2c369baac351e0ce00376177a80232dbd3 Mon Sep 17 00:00:00 2001 From: Laurens Valk Date: Mon, 29 Dec 2025 15:18:42 +0100 Subject: [PATCH 14/14] pbio/drv/bluetooth: Check for HCI enabled. On some platforms it may not be enabled, e.g. when the USB Bluetooth dongle is not plugged in. --- lib/pbio/drv/bluetooth/bluetooth.c | 23 +++++++- lib/pbio/drv/bluetooth/bluetooth_btstack.c | 59 ++++++++++++------- lib/pbio/drv/bluetooth/bluetooth_simulation.c | 5 ++ .../drv/bluetooth/bluetooth_stm32_bluenrg.c | 5 ++ .../drv/bluetooth/bluetooth_stm32_cc2640.c | 5 ++ lib/pbio/include/pbdrv/bluetooth.h | 2 + 6 files changed, 76 insertions(+), 23 deletions(-) diff --git a/lib/pbio/drv/bluetooth/bluetooth.c b/lib/pbio/drv/bluetooth/bluetooth.c index e37032dfc..593b61ce9 100644 --- a/lib/pbio/drv/bluetooth/bluetooth.c +++ b/lib/pbio/drv/bluetooth/bluetooth.c @@ -158,6 +158,10 @@ pbio_error_t pbdrv_bluetooth_peripheral_scan_and_connect(pbdrv_bluetooth_periphe pbdrv_bluetooth_peripheral_t *peri = &peripheral_singleton; + if (!pbdrv_bluetooth_is_connected(PBDRV_BLUETOOTH_CONNECTION_HCI)) { + return PBIO_ERROR_INVALID_OP; + } + // Can't connect if already connected or already busy. if (pbdrv_bluetooth_is_connected(PBDRV_BLUETOOTH_CONNECTION_PERIPHERAL) || peri->func) { return PBIO_ERROR_BUSY; @@ -300,6 +304,10 @@ pbdrv_bluetooth_advertising_state_t pbdrv_bluetooth_advertising_state; pbio_error_t pbdrv_bluetooth_start_advertising(bool start) { + if (!pbdrv_bluetooth_is_connected(PBDRV_BLUETOOTH_CONNECTION_HCI)) { + return PBIO_ERROR_INVALID_OP; + } + bool is_advertising = pbdrv_bluetooth_advertising_state == PBDRV_BLUETOOTH_ADVERTISING_STATE_ADVERTISING_PYBRICKS; // Already in requested state. This makes it safe to call stop advertising @@ -330,6 +338,10 @@ uint8_t pbdrv_bluetooth_broadcast_data_size; pbio_error_t pbdrv_bluetooth_start_broadcasting(const uint8_t *data, size_t size) { + if (!pbdrv_bluetooth_is_connected(PBDRV_BLUETOOTH_CONNECTION_HCI)) { + return PBIO_ERROR_INVALID_OP; + } + if (advertising_or_scan_func) { return PBIO_ERROR_BUSY; } @@ -374,6 +386,10 @@ pbdrv_bluetooth_start_observing_callback_t pbdrv_bluetooth_observe_callback; pbio_error_t pbdrv_bluetooth_start_observing(pbdrv_bluetooth_start_observing_callback_t callback) { + if (!pbdrv_bluetooth_is_connected(PBDRV_BLUETOOTH_CONNECTION_HCI)) { + return PBIO_ERROR_INVALID_OP; + } + if (advertising_or_scan_func) { return PBIO_ERROR_BUSY; } @@ -593,6 +609,11 @@ pbio_error_t pbdrv_bluetooth_close_user_tasks(pbio_os_state_t *state, pbio_os_ti void pbdrv_bluetooth_deinit(void) { + // If Bluetooth is not even initialized, nothing to do. + if (!pbdrv_bluetooth_is_connected(PBDRV_BLUETOOTH_CONNECTION_HCI)) { + return; + } + // Under normal operation ::pbdrv_bluetooth_close_user_tasks completes // normally and there should be no user activity at this point. If there // is, a task got stuck, so exit forcefully. @@ -600,7 +621,7 @@ void pbdrv_bluetooth_deinit(void) { if (pbdrv_bluetooth_await_advertise_or_scan_command(&unused, NULL) != PBIO_SUCCESS || pbdrv_bluetooth_await_peripheral_command(&unused, NULL) != PBIO_SUCCESS) { - // Hard reset without waitng on completion of any process. + // Hard reset without waiting on completion of any process. pbdrv_bluetooth_controller_reset_hard(); return; } diff --git a/lib/pbio/drv/bluetooth/bluetooth_btstack.c b/lib/pbio/drv/bluetooth/bluetooth_btstack.c index 5d31e11f6..ff101e890 100644 --- a/lib/pbio/drv/bluetooth/bluetooth_btstack.c +++ b/lib/pbio/drv/bluetooth/bluetooth_btstack.c @@ -124,28 +124,6 @@ static hci_con_handle_t uart_con_handle = HCI_CON_HANDLE_INVALID; static pup_handset_t handset; #endif // PBDRV_CONFIG_BLUETOOTH_BTSTACK_LE -bool pbdrv_bluetooth_is_connected(pbdrv_bluetooth_connection_t connection) { - #if PBDRV_CONFIG_BLUETOOTH_BTSTACK_LE - if (connection == PBDRV_BLUETOOTH_CONNECTION_LE && le_con_handle != HCI_CON_HANDLE_INVALID) { - return true; - } - - if (connection == PBDRV_BLUETOOTH_CONNECTION_PYBRICKS && pybricks_con_handle != HCI_CON_HANDLE_INVALID) { - return true; - } - - if (connection == PBDRV_BLUETOOTH_CONNECTION_UART && uart_con_handle != HCI_CON_HANDLE_INVALID) { - return true; - } - - if (connection == PBDRV_BLUETOOTH_CONNECTION_PERIPHERAL && peripheral_singleton.con_handle != HCI_CON_HANDLE_INVALID) { - return true; - } - #endif // PBDRV_CONFIG_BLUETOOTH_BTSTACK_LE - - return false; -} - #if PBDRV_CONFIG_BLUETOOTH_BTSTACK_LE /** @@ -202,6 +180,39 @@ static void propagate_event(uint8_t *packet) { event_packet = NULL; } +bool pbdrv_bluetooth_is_connected(pbdrv_bluetooth_connection_t connection) { + + // Nothing connected if HCI is not running. + if (bluetooth_thread_err != PBIO_ERROR_AGAIN) { + return false; + } + + #if PBDRV_CONFIG_BLUETOOTH_BTSTACK_LE + + if (connection == PBDRV_BLUETOOTH_CONNECTION_HCI) { + return true; + } + + if (connection == PBDRV_BLUETOOTH_CONNECTION_LE && le_con_handle != HCI_CON_HANDLE_INVALID) { + return true; + } + + if (connection == PBDRV_BLUETOOTH_CONNECTION_PYBRICKS && pybricks_con_handle != HCI_CON_HANDLE_INVALID) { + return true; + } + + if (connection == PBDRV_BLUETOOTH_CONNECTION_UART && uart_con_handle != HCI_CON_HANDLE_INVALID) { + return true; + } + + if (connection == PBDRV_BLUETOOTH_CONNECTION_PERIPHERAL && peripheral_singleton.con_handle != HCI_CON_HANDLE_INVALID) { + return true; + } + #endif // PBDRV_CONFIG_BLUETOOTH_BTSTACK_LE + + return false; +} + #if PBDRV_CONFIG_BLUETOOTH_BTSTACK_LE static void nordic_spp_packet_handler(uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size) { switch (packet_type) { @@ -993,6 +1004,10 @@ static pbio_error_t bluetooth_btstack_handle_power_control(pbio_os_state_t *stat pbio_error_t pbdrv_bluetooth_controller_reset(pbio_os_state_t *state, pbio_os_timer_t *timer) { + if (pbdrv_bluetooth_is_connected(PBDRV_BLUETOOTH_CONNECTION_HCI)) { + return PBIO_ERROR_INVALID_OP; + } + #if PBDRV_CONFIG_BLUETOOTH_BTSTACK_LE static pbio_os_state_t sub; diff --git a/lib/pbio/drv/bluetooth/bluetooth_simulation.c b/lib/pbio/drv/bluetooth/bluetooth_simulation.c index c67198b48..1fea286e2 100644 --- a/lib/pbio/drv/bluetooth/bluetooth_simulation.c +++ b/lib/pbio/drv/bluetooth/bluetooth_simulation.c @@ -31,6 +31,11 @@ pbio_error_t pbdrv_bluetooth_stop_advertising_func(pbio_os_state_t *state, void } bool pbdrv_bluetooth_is_connected(pbdrv_bluetooth_connection_t connection) { + + if (connection == PBDRV_BLUETOOTH_CONNECTION_HCI) { + return true; + } + if (connection == PBDRV_BLUETOOTH_CONNECTION_LE) { return true; } diff --git a/lib/pbio/drv/bluetooth/bluetooth_stm32_bluenrg.c b/lib/pbio/drv/bluetooth/bluetooth_stm32_bluenrg.c index a42141820..3f120bc4a 100644 --- a/lib/pbio/drv/bluetooth/bluetooth_stm32_bluenrg.c +++ b/lib/pbio/drv/bluetooth/bluetooth_stm32_bluenrg.c @@ -290,6 +290,11 @@ pbio_error_t pbdrv_bluetooth_stop_advertising_func(pbio_os_state_t *state, void bool pbdrv_bluetooth_is_connected(pbdrv_bluetooth_connection_t connection) { + + if (connection == PBDRV_BLUETOOTH_CONNECTION_HCI) { + return true; + } + if (connection == PBDRV_BLUETOOTH_CONNECTION_LE && conn_handle) { return true; } diff --git a/lib/pbio/drv/bluetooth/bluetooth_stm32_cc2640.c b/lib/pbio/drv/bluetooth/bluetooth_stm32_cc2640.c index e797068fd..994292f3c 100644 --- a/lib/pbio/drv/bluetooth/bluetooth_stm32_cc2640.c +++ b/lib/pbio/drv/bluetooth/bluetooth_stm32_cc2640.c @@ -321,6 +321,11 @@ pbio_error_t pbdrv_bluetooth_stop_advertising_func(pbio_os_state_t *state, void } bool pbdrv_bluetooth_is_connected(pbdrv_bluetooth_connection_t connection) { + + if (connection == PBDRV_BLUETOOTH_CONNECTION_HCI) { + return true; + } + if (connection == PBDRV_BLUETOOTH_CONNECTION_LE && conn_handle != NO_CONNECTION && !busy_disconnecting) { return true; } diff --git a/lib/pbio/include/pbdrv/bluetooth.h b/lib/pbio/include/pbdrv/bluetooth.h index 67f50abee..9fd98cb7b 100644 --- a/lib/pbio/include/pbdrv/bluetooth.h +++ b/lib/pbio/include/pbdrv/bluetooth.h @@ -22,6 +22,8 @@ * BLE characteristic connection identifiers. */ typedef enum { + /* Whether Bluetooth is present and enabled on this platform. */ + PBDRV_BLUETOOTH_CONNECTION_HCI, /* A low energy device connection. */ PBDRV_BLUETOOTH_CONNECTION_LE, /** The Pybricks service. */