From fc8ba3f56f80ed65f9154b2122e71eddbcd019b3 Mon Sep 17 00:00:00 2001 From: Laurens Valk Date: Mon, 5 Jan 2026 11:46:09 +0100 Subject: [PATCH 1/7] pbio/drv/bluetooth: Don't reset peripheral struct on connect. We should be explicitly resetting only what needs resetting to avoid introducing subtle bugs every time we add something to the peripheral struct that should not be erased. --- lib/pbio/drv/bluetooth/bluetooth.c | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/lib/pbio/drv/bluetooth/bluetooth.c b/lib/pbio/drv/bluetooth/bluetooth.c index 9a1c23f46..6700bbf4a 100644 --- a/lib/pbio/drv/bluetooth/bluetooth.c +++ b/lib/pbio/drv/bluetooth/bluetooth.c @@ -195,15 +195,14 @@ pbio_error_t pbdrv_bluetooth_peripheral_scan_and_connect(pbdrv_bluetooth_periphe return PBIO_ERROR_BUSY; } - // Reset peripheral instance but keep user reference. - void *user = peri->user; - memset(peri, 0, sizeof(pbdrv_bluetooth_peripheral_t)); - peri->user = user; + // Used to compare subsequent advertisements, so we should reset it. + memset(peri->bdaddr, 0, sizeof(peri->bdaddr)); // Initialize operation for handling on the main thread. peri->config = config; peri->func = pbdrv_bluetooth_peripheral_scan_and_connect_func; peri->err = PBIO_ERROR_AGAIN; + peri->cancel = false; pbio_os_timer_set(&peri->timer, config->timeout); pbio_os_request_poll(); From 1153f59da89437fe5e89f4cb86d990d7bfd2ba05 Mon Sep 17 00:00:00 2001 From: Laurens Valk Date: Mon, 5 Jan 2026 11:49:27 +0100 Subject: [PATCH 2/7] pbio/drv/bluetooth_btstack: Associate peripheral state with instance. At the moment we support only one peripheral, so a lot of code relies on global singletons. In the btstack driver, there is a specific handset instance that we want to generalize, so start migrating from there. --- lib/pbio/drv/bluetooth/bluetooth_btstack.c | 115 +++++++++++---------- lib/pbio/include/pbdrv/bluetooth.h | 5 + 2 files changed, 63 insertions(+), 57 deletions(-) diff --git a/lib/pbio/drv/bluetooth/bluetooth_btstack.c b/lib/pbio/drv/bluetooth/bluetooth_btstack.c index dc4444340..1d536e03e 100644 --- a/lib/pbio/drv/bluetooth/bluetooth_btstack.c +++ b/lib/pbio/drv/bluetooth/bluetooth_btstack.c @@ -95,8 +95,8 @@ typedef enum { DISCONNECT_REASON_SEND_SUBSCRIBE_PORT_1_FAILED, } disconnect_reason_t; -// REVISIT: Most of these states should go into pbdrv_bluetooth_peripheral_t -typedef struct { +#if PBDRV_CONFIG_BLUETOOTH_BTSTACK_LE +static struct _pbdrv_bluetooth_peripheral_platform_state_t { gatt_client_notification_t notification; con_state_t con_state; disconnect_reason_t disconnect_reason; @@ -106,7 +106,8 @@ typedef struct { */ gatt_client_characteristic_t current_char; uint8_t btstack_error; -} pup_handset_t; +} _handset; +#endif // PBDRV_CONFIG_BLUETOOTH_BTSTACK_LE // hub name goes in special section so that it can be modified when flashing firmware #if !PBIO_TEST_BUILD @@ -121,7 +122,6 @@ static const pbdrv_bluetooth_btstack_platform_data_t *pdata = &pbdrv_bluetooth_b static hci_con_handle_t le_con_handle = HCI_CON_HANDLE_INVALID; static hci_con_handle_t pybricks_con_handle = HCI_CON_HANDLE_INVALID; static hci_con_handle_t uart_con_handle = HCI_CON_HANDLE_INVALID; -static pup_handset_t handset; #endif // PBDRV_CONFIG_BLUETOOTH_BTSTACK_LE #if PBDRV_CONFIG_BLUETOOTH_BTSTACK_LE @@ -308,7 +308,7 @@ static void packet_handler(uint8_t packet_type, uint16_t channel, uint8_t *packe // We only care about the one characteristic that has at least the requested properties. if ((found_char.properties & peri->char_now->properties) == peri->char_now->properties) { peri->char_now->handle = found_char.value_handle; - gatt_event_characteristic_query_result_get_characteristic(packet, &handset.current_char); + gatt_event_characteristic_query_result_get_characteristic(packet, &peri->platform_state->current_char); } break; } @@ -323,34 +323,34 @@ static void packet_handler(uint8_t packet_type, uint16_t channel, uint8_t *packe break; } case GATT_EVENT_QUERY_COMPLETE: - if (handset.con_state == CON_STATE_WAIT_READ_CHARACTERISTIC) { + if (peri->platform_state->con_state == CON_STATE_WAIT_READ_CHARACTERISTIC) { // Done reading characteristic. - handset.con_state = CON_STATE_READ_CHARACTERISTIC_COMPLETE; - } else if (handset.con_state == CON_STATE_WAIT_DISCOVER_CHARACTERISTICS) { + peri->platform_state->con_state = CON_STATE_READ_CHARACTERISTIC_COMPLETE; + } else if (peri->platform_state->con_state == CON_STATE_WAIT_DISCOVER_CHARACTERISTICS) { // Discovered characteristics, ready enable notifications. if (!peri->char_now->request_notification) { // If no notification is requested, we are done. - handset.con_state = CON_STATE_DISCOVERY_AND_NOTIFICATIONS_COMPLETE; + peri->platform_state->con_state = CON_STATE_DISCOVERY_AND_NOTIFICATIONS_COMPLETE; break; } - handset.btstack_error = gatt_client_write_client_characteristic_configuration( - packet_handler, peri->con_handle, &handset.current_char, + peri->platform_state->btstack_error = gatt_client_write_client_characteristic_configuration( + packet_handler, peri->con_handle, &peri->platform_state->current_char, GATT_CLIENT_CHARACTERISTICS_CONFIGURATION_NOTIFICATION); - if (handset.btstack_error == ERROR_CODE_SUCCESS) { + if (peri->platform_state->btstack_error == ERROR_CODE_SUCCESS) { gatt_client_listen_for_characteristic_value_updates( - &handset.notification, packet_handler, peri->con_handle, &handset.current_char); - handset.con_state = CON_STATE_WAIT_ENABLE_NOTIFICATIONS; + &peri->platform_state->notification, packet_handler, peri->con_handle, &peri->platform_state->current_char); + peri->platform_state->con_state = CON_STATE_WAIT_ENABLE_NOTIFICATIONS; } else { // configuration failed for some reason, so disconnect gap_disconnect(peri->con_handle); - handset.con_state = CON_STATE_WAIT_DISCONNECT; - handset.disconnect_reason = DISCONNECT_REASON_CONFIGURE_CHARACTERISTIC_FAILED; + peri->platform_state->con_state = CON_STATE_WAIT_DISCONNECT; + peri->platform_state->disconnect_reason = DISCONNECT_REASON_CONFIGURE_CHARACTERISTIC_FAILED; } - } else if (handset.con_state == CON_STATE_WAIT_ENABLE_NOTIFICATIONS) { + } else if (peri->platform_state->con_state == CON_STATE_WAIT_ENABLE_NOTIFICATIONS) { // Done enabling notifications. - handset.con_state = CON_STATE_DISCOVERY_AND_NOTIFICATIONS_COMPLETE; + peri->platform_state->con_state = CON_STATE_DISCOVERY_AND_NOTIFICATIONS_COMPLETE; } break; @@ -381,7 +381,7 @@ static void packet_handler(uint8_t packet_type, uint16_t channel, uint8_t *packe pbdrv_bluetooth_advertising_state = PBDRV_BLUETOOTH_ADVERTISING_STATE_NONE; } else { // If we aren't waiting for a peripheral connection, this must be a different connection. - if (handset.con_state != CON_STATE_WAIT_CONNECT) { + if (peri->platform_state->con_state != CON_STATE_WAIT_CONNECT) { break; } @@ -394,9 +394,9 @@ static void packet_handler(uint8_t packet_type, uint16_t channel, uint8_t *packe // delete the bond and start over. gap_delete_bonding(peri->bdaddr_type, peri->bdaddr); sm_request_pairing(peri->con_handle); - handset.con_state = CON_STATE_WAIT_BONDING; + peri->platform_state->con_state = CON_STATE_WAIT_BONDING; } else { - handset.con_state = CON_STATE_CONNECTED; + peri->platform_state->con_state = CON_STATE_CONNECTED; } } @@ -408,9 +408,9 @@ static void packet_handler(uint8_t packet_type, uint16_t channel, uint8_t *packe pybricks_con_handle = HCI_CON_HANDLE_INVALID; uart_con_handle = HCI_CON_HANDLE_INVALID; } else if (hci_event_disconnection_complete_get_connection_handle(packet) == peri->con_handle) { - gatt_client_stop_listening_for_characteristic_value_updates(&handset.notification); + gatt_client_stop_listening_for_characteristic_value_updates(&peri->platform_state->notification); peri->con_handle = HCI_CON_HANDLE_INVALID; - handset.con_state = CON_STATE_NONE; + peri->platform_state->con_state = CON_STATE_NONE; } break; @@ -428,7 +428,7 @@ static void packet_handler(uint8_t packet_type, uint16_t channel, uint8_t *packe pbdrv_bluetooth_observe_callback(event_type, data, data_length, rssi); } - if (handset.con_state == CON_STATE_WAIT_ADV_IND) { + if (peri->platform_state->con_state == CON_STATE_WAIT_ADV_IND) { // Match advertisement data against context-specific filter. pbdrv_bluetooth_ad_match_result_flags_t adv_flags = PBDRV_BLUETOOTH_AD_MATCH_NONE; if (peri->config->match_adv) { @@ -445,9 +445,9 @@ static void packet_handler(uint8_t packet_type, uint16_t channel, uint8_t *packe // Advertising data matched, prepare for scan response. memcpy(peri->bdaddr, address, sizeof(bd_addr_t)); peri->bdaddr_type = gap_event_advertising_report_get_address_type(packet); - handset.con_state = CON_STATE_WAIT_SCAN_RSP; + peri->platform_state->con_state = CON_STATE_WAIT_SCAN_RSP; } - } else if (handset.con_state == CON_STATE_WAIT_SCAN_RSP) { + } else if (peri->platform_state->con_state == CON_STATE_WAIT_SCAN_RSP) { char *detected_name = (char *)&data[2]; const uint8_t max_len = sizeof(peri->name); @@ -460,7 +460,7 @@ static void packet_handler(uint8_t packet_type, uint16_t channel, uint8_t *packe if (rsp_flags & PBDRV_BLUETOOTH_AD_MATCH_NAME_FAILED) { // A name was requested but it doesn't match, so go back to scanning stage. - handset.con_state = CON_STATE_WAIT_ADV_IND; + peri->platform_state->con_state = CON_STATE_WAIT_ADV_IND; break; } @@ -469,12 +469,12 @@ static void packet_handler(uint8_t packet_type, uint16_t channel, uint8_t *packe } gap_stop_scan(); - handset.btstack_error = gap_connect(peri->bdaddr, peri->bdaddr_type); + peri->platform_state->btstack_error = gap_connect(peri->bdaddr, peri->bdaddr_type); - if (handset.btstack_error == ERROR_CODE_SUCCESS) { - handset.con_state = CON_STATE_WAIT_CONNECT; + if (peri->platform_state->btstack_error == ERROR_CODE_SUCCESS) { + peri->platform_state->con_state = CON_STATE_WAIT_CONNECT; } else { - handset.con_state = CON_STATE_NONE; + peri->platform_state->con_state = CON_STATE_NONE; } } } @@ -538,7 +538,7 @@ static void sm_packet_handler(uint8_t packet_type, uint16_t channel, uint8_t *pa // This is the final state for known compatible peripherals // with bonding under normal circumstances. DEBUG_PRINT("Pairing complete, success\n"); - handset.con_state = CON_STATE_CONNECTED; + peripheral_singleton.platform_state->con_state = CON_STATE_CONNECTED; break; case ERROR_CODE_CONNECTION_TIMEOUT: // fall through to disconnect. @@ -717,7 +717,7 @@ pbio_error_t pbdrv_bluetooth_peripheral_scan_and_connect_func(pbio_os_state_t *s PBIO_OS_ASYNC_BEGIN(state); - memset(&handset, 0, sizeof(handset)); + memset(peri->platform_state, 0, sizeof(pbdrv_bluetooth_peripheral_platform_state_t)); peri->con_handle = HCI_CON_HANDLE_INVALID; @@ -725,26 +725,26 @@ pbio_error_t pbdrv_bluetooth_peripheral_scan_and_connect_func(pbio_os_state_t *s // scan interval: 48 * 0.625ms = 30ms gap_set_scan_params(1, 0x30, 0x30, 0); gap_start_scan(); - handset.con_state = CON_STATE_WAIT_ADV_IND; + peri->platform_state->con_state = CON_STATE_WAIT_ADV_IND; PBIO_OS_AWAIT_UNTIL(state, ({ if (peri->cancel || timed_out) { - if (handset.con_state == CON_STATE_WAIT_ADV_IND || handset.con_state == CON_STATE_WAIT_SCAN_RSP) { + if (peri->platform_state->con_state == CON_STATE_WAIT_ADV_IND || peri->platform_state->con_state == CON_STATE_WAIT_SCAN_RSP) { gap_stop_scan(); - } else if (handset.con_state == CON_STATE_WAIT_CONNECT) { + } else if (peri->platform_state->con_state == CON_STATE_WAIT_CONNECT) { gap_connect_cancel(); } else if (peri->con_handle != HCI_CON_HANDLE_INVALID) { gap_disconnect(peri->con_handle); } - handset.con_state = CON_STATE_NONE; + peri->platform_state->con_state = CON_STATE_NONE; return timed_out ? PBIO_ERROR_TIMEDOUT : PBIO_ERROR_CANCELED; } // if there is any failure to connect or error while enumerating // attributes, con_state will be set to CON_STATE_NONE - if (handset.con_state == CON_STATE_NONE) { + if (peri->platform_state->con_state == CON_STATE_NONE) { return PBIO_ERROR_FAILED; } - handset.con_state == CON_STATE_CONNECTED; + peri->platform_state->con_state == CON_STATE_CONNECTED; })); PBIO_OS_ASYNC_END(PBIO_SUCCESS); @@ -756,36 +756,36 @@ pbio_error_t pbdrv_bluetooth_peripheral_discover_characteristic_func(pbio_os_sta PBIO_OS_ASYNC_BEGIN(state); - if (handset.con_state != CON_STATE_CONNECTED) { + if (peri->platform_state->con_state != CON_STATE_CONNECTED) { return PBIO_ERROR_FAILED; } - handset.con_state = CON_STATE_WAIT_DISCOVER_CHARACTERISTICS; + peri->platform_state->con_state = CON_STATE_WAIT_DISCOVER_CHARACTERISTICS; uint16_t handle_max = peri->char_now->handle_max ? peri->char_now->handle_max : 0xffff; - handset.btstack_error = peri->char_now->uuid16 ? + peri->platform_state->btstack_error = peri->char_now->uuid16 ? gatt_client_discover_characteristics_for_handle_range_by_uuid16( packet_handler, peri->con_handle, 0x0001, handle_max, peri->char_now->uuid16) : gatt_client_discover_characteristics_for_handle_range_by_uuid128( packet_handler, peri->con_handle, 0x0001, handle_max, peri->char_now->uuid128); - if (handset.btstack_error != ERROR_CODE_SUCCESS) { + if (peri->platform_state->btstack_error != ERROR_CODE_SUCCESS) { // configuration failed for some reason, so disconnect gap_disconnect(peri->con_handle); - handset.con_state = CON_STATE_WAIT_DISCONNECT; - handset.disconnect_reason = DISCONNECT_REASON_DISCOVER_CHARACTERISTIC_FAILED; + peri->platform_state->con_state = CON_STATE_WAIT_DISCONNECT; + peri->platform_state->disconnect_reason = DISCONNECT_REASON_DISCOVER_CHARACTERISTIC_FAILED; } PBIO_OS_AWAIT_UNTIL(state, ({ // if there is any error while enumerating // attributes, con_state will be set to CON_STATE_NONE - if (handset.con_state == CON_STATE_NONE) { + if (peri->platform_state->con_state == CON_STATE_NONE) { return PBIO_ERROR_FAILED; } - handset.con_state == CON_STATE_DISCOVERY_AND_NOTIFICATIONS_COMPLETE; + peri->platform_state->con_state == CON_STATE_DISCOVERY_AND_NOTIFICATIONS_COMPLETE; })); // State state back to simply connected, so we can discover other characteristics. - handset.con_state = CON_STATE_CONNECTED; + peri->platform_state->con_state = CON_STATE_CONNECTED; PBIO_OS_ASYNC_END(peri->char_now->handle ? PBIO_SUCCESS : PBIO_ERROR_FAILED); } @@ -796,34 +796,34 @@ pbio_error_t pbdrv_bluetooth_peripheral_read_characteristic_func(pbio_os_state_t PBIO_OS_ASYNC_BEGIN(state); - if (handset.con_state != CON_STATE_CONNECTED) { + if (peri->platform_state->con_state != CON_STATE_CONNECTED) { return PBIO_ERROR_FAILED; } gatt_client_characteristic_t characteristic = { .value_handle = peri->char_now->handle, }; - handset.btstack_error = gatt_client_read_value_of_characteristic(packet_handler, peri->con_handle, &characteristic); + peri->platform_state->btstack_error = gatt_client_read_value_of_characteristic(packet_handler, peri->con_handle, &characteristic); - if (handset.btstack_error == ERROR_CODE_SUCCESS) { - handset.con_state = CON_STATE_WAIT_READ_CHARACTERISTIC; + if (peri->platform_state->btstack_error == ERROR_CODE_SUCCESS) { + peri->platform_state->con_state = CON_STATE_WAIT_READ_CHARACTERISTIC; } else { // configuration failed for some reason, so disconnect gap_disconnect(peri->con_handle); - handset.con_state = CON_STATE_WAIT_DISCONNECT; - handset.disconnect_reason = DISCONNECT_REASON_DISCOVER_CHARACTERISTIC_FAILED; + peri->platform_state->con_state = CON_STATE_WAIT_DISCONNECT; + peri->platform_state->disconnect_reason = DISCONNECT_REASON_DISCOVER_CHARACTERISTIC_FAILED; } PBIO_OS_AWAIT_UNTIL(state, ({ // if there is any error while reading, con_state will be set to CON_STATE_NONE - if (handset.con_state == CON_STATE_NONE) { + if (peri->platform_state->con_state == CON_STATE_NONE) { return PBIO_ERROR_FAILED; } - handset.con_state == CON_STATE_READ_CHARACTERISTIC_COMPLETE; + peri->platform_state->con_state == CON_STATE_READ_CHARACTERISTIC_COMPLETE; })); // State state back to simply connected, so we can discover other characteristics. - handset.con_state = CON_STATE_CONNECTED; + peri->platform_state->con_state = CON_STATE_CONNECTED; PBIO_OS_ASYNC_END(PBIO_SUCCESS); } @@ -1130,6 +1130,7 @@ void pbdrv_bluetooth_init_hci(void) { #if PBDRV_CONFIG_BLUETOOTH_BTSTACK_LE // don't need to init the whole struct, so doing this here + peripheral_singleton.platform_state = &_handset; peripheral_singleton.con_handle = HCI_CON_HANDLE_INVALID; #endif diff --git a/lib/pbio/include/pbdrv/bluetooth.h b/lib/pbio/include/pbdrv/bluetooth.h index 5ce72f362..bd2d3c67e 100644 --- a/lib/pbio/include/pbdrv/bluetooth.h +++ b/lib/pbio/include/pbdrv/bluetooth.h @@ -158,6 +158,9 @@ typedef struct { uint32_t timeout; } pbdrv_bluetooth_peripheral_connect_config_t; +/** Platform-specific state needed to operate the peripheral. */ +typedef struct _pbdrv_bluetooth_peripheral_platform_state_t pbdrv_bluetooth_peripheral_platform_state_t; + /** * State of a peripheral that the hub may be connected to, such as a remote. */ @@ -185,6 +188,8 @@ struct _pbdrv_bluetooth_peripheral_t { pbio_os_timer_t watchdog; /** Cancellation requested */ bool cancel; + /** Platform-specific state needed to operate the peripheral. */ + pbdrv_bluetooth_peripheral_platform_state_t *platform_state; }; /** Advertisement types. */ From 1eb18da5ea296685d6f7a71b30e95256919b2b0d Mon Sep 17 00:00:00 2001 From: Laurens Valk Date: Mon, 5 Jan 2026 12:18:07 +0100 Subject: [PATCH 3/7] pbio/drv/bluetooth_btstack: Convert char discovery to task. We can properly return errors to the user this way, and write chronological code. Besides catching more operational errors, we can also let the higher level code decide when and where to disconnect, rather than doing that from some but not all places within these tasks. This will also help when we support multiple peripherals in parallel. --- lib/pbio/drv/bluetooth/bluetooth_btstack.c | 146 ++++++++++++------ .../iodevices/pb_type_iodevices_lwp3device.c | 2 +- 2 files changed, 96 insertions(+), 52 deletions(-) diff --git a/lib/pbio/drv/bluetooth/bluetooth_btstack.c b/lib/pbio/drv/bluetooth/bluetooth_btstack.c index 1d536e03e..68a26ff3d 100644 --- a/lib/pbio/drv/bluetooth/bluetooth_btstack.c +++ b/lib/pbio/drv/bluetooth/bluetooth_btstack.c @@ -156,6 +156,10 @@ static void pybricks_configured(hci_con_handle_t tx_con_handle, uint16_t value) pybricks_con_handle = value ? tx_con_handle : HCI_CON_HANDLE_INVALID; } +static bool hci_event_is_type(uint8_t *packet, uint8_t event_type) { + return packet && hci_event_packet_get_type(packet) == event_type; +} + #endif // PBDRV_CONFIG_BLUETOOTH_BTSTACK_LE static pbio_os_state_t bluetooth_thread_state; @@ -302,17 +306,11 @@ static void packet_handler(uint8_t packet_type, uint16_t channel, uint8_t *packe gatt_event_service_query_result_get_service(packet, &service); break; } - case GATT_EVENT_CHARACTERISTIC_QUERY_RESULT: { - gatt_client_characteristic_t found_char; - gatt_event_characteristic_query_result_get_characteristic(packet, &found_char); - // We only care about the one characteristic that has at least the requested properties. - if ((found_char.properties & peri->char_now->properties) == peri->char_now->properties) { - peri->char_now->handle = found_char.value_handle; - gatt_event_characteristic_query_result_get_characteristic(packet, &peri->platform_state->current_char); - } + case GATT_EVENT_CHARACTERISTIC_QUERY_RESULT: + DEBUG_PRINT("GATT_EVENT_CHARACTERISTIC_QUERY_RESULT\n"); break; - } case GATT_EVENT_CHARACTERISTIC_VALUE_QUERY_RESULT: { + DEBUG_PRINT("GATT_EVENT_CHARACTERISTIC_VALUE_QUERY_RESULT\n"); hci_con_handle_t handle = gatt_event_characteristic_value_query_result_get_handle(packet); uint16_t value_handle = gatt_event_characteristic_value_query_result_get_value_handle(packet); uint16_t value_length = gatt_event_characteristic_value_query_result_get_value_length(packet); @@ -323,34 +321,11 @@ static void packet_handler(uint8_t packet_type, uint16_t channel, uint8_t *packe break; } case GATT_EVENT_QUERY_COMPLETE: + DEBUG_PRINT("GATT_EVENT_QUERY_COMPLETE\n"); + if (peri->platform_state->con_state == CON_STATE_WAIT_READ_CHARACTERISTIC) { // Done reading characteristic. peri->platform_state->con_state = CON_STATE_READ_CHARACTERISTIC_COMPLETE; - } else if (peri->platform_state->con_state == CON_STATE_WAIT_DISCOVER_CHARACTERISTICS) { - - // Discovered characteristics, ready enable notifications. - if (!peri->char_now->request_notification) { - // If no notification is requested, we are done. - peri->platform_state->con_state = CON_STATE_DISCOVERY_AND_NOTIFICATIONS_COMPLETE; - break; - } - - peri->platform_state->btstack_error = gatt_client_write_client_characteristic_configuration( - packet_handler, peri->con_handle, &peri->platform_state->current_char, - GATT_CLIENT_CHARACTERISTICS_CONFIGURATION_NOTIFICATION); - if (peri->platform_state->btstack_error == ERROR_CODE_SUCCESS) { - gatt_client_listen_for_characteristic_value_updates( - &peri->platform_state->notification, packet_handler, peri->con_handle, &peri->platform_state->current_char); - peri->platform_state->con_state = CON_STATE_WAIT_ENABLE_NOTIFICATIONS; - } else { - // configuration failed for some reason, so disconnect - gap_disconnect(peri->con_handle); - peri->platform_state->con_state = CON_STATE_WAIT_DISCONNECT; - peri->platform_state->disconnect_reason = DISCONNECT_REASON_CONFIGURE_CHARACTERISTIC_FAILED; - } - } else if (peri->platform_state->con_state == CON_STATE_WAIT_ENABLE_NOTIFICATIONS) { - // Done enabling notifications. - peri->platform_state->con_state = CON_STATE_DISCOVERY_AND_NOTIFICATIONS_COMPLETE; } break; @@ -753,39 +728,108 @@ pbio_error_t pbdrv_bluetooth_peripheral_scan_and_connect_func(pbio_os_state_t *s pbio_error_t pbdrv_bluetooth_peripheral_discover_characteristic_func(pbio_os_state_t *state, void *context) { pbdrv_bluetooth_peripheral_t *peri = &peripheral_singleton; + gatt_client_characteristic_t *current_char = &peri->platform_state->current_char; + + uint8_t btstack_error; PBIO_OS_ASYNC_BEGIN(state); - if (peri->platform_state->con_state != CON_STATE_CONNECTED) { - return PBIO_ERROR_FAILED; - } + // Nothing found to begin with. + peri->char_now->handle = 0; - peri->platform_state->con_state = CON_STATE_WAIT_DISCOVER_CHARACTERISTICS; uint16_t handle_max = peri->char_now->handle_max ? peri->char_now->handle_max : 0xffff; - peri->platform_state->btstack_error = peri->char_now->uuid16 ? + + // Start characteristic discovery. + btstack_error = peri->char_now->uuid16 ? gatt_client_discover_characteristics_for_handle_range_by_uuid16( packet_handler, peri->con_handle, 0x0001, handle_max, peri->char_now->uuid16) : gatt_client_discover_characteristics_for_handle_range_by_uuid128( packet_handler, peri->con_handle, 0x0001, handle_max, peri->char_now->uuid128); + if (btstack_error != ERROR_CODE_SUCCESS) { + return att_error_to_pbio_error(btstack_error); + } - if (peri->platform_state->btstack_error != ERROR_CODE_SUCCESS) { - // configuration failed for some reason, so disconnect - gap_disconnect(peri->con_handle); - peri->platform_state->con_state = CON_STATE_WAIT_DISCONNECT; - peri->platform_state->disconnect_reason = DISCONNECT_REASON_DISCOVER_CHARACTERISTIC_FAILED; + // Await until discovery is complete, processing each discovered + // characteristic along the way to see if it matches what we need. + PBIO_OS_AWAIT_UNTIL(state, ({ + + // Got disconnected while waiting. + if (peri->con_handle == HCI_CON_HANDLE_INVALID) { + return PBIO_ERROR_NO_DEV; + } + + // Process a discovered characteristic, saving only the first match. + if (!peri->char_now->handle && + hci_event_is_type(event_packet, GATT_EVENT_CHARACTERISTIC_QUERY_RESULT) && + gatt_event_characteristic_query_result_get_handle(event_packet) == peri->con_handle) { + + // Unpack the result. + gatt_event_characteristic_query_result_get_characteristic(event_packet, current_char); + + // We only care about the one characteristic that has at least the requested properties. + if ((current_char->properties & peri->char_now->properties) == peri->char_now->properties) { + DEBUG_PRINT("Found characteristic handle: 0x%04x with properties 0x%04x \n", current_char->value_handle, current_char->properties); + peri->char_now->handle = current_char->value_handle; + peri->char_now->handle_max = current_char->end_handle; + } + } + + // The wait until condition: discovery complete. + hci_event_is_type(event_packet, GATT_EVENT_QUERY_COMPLETE) && + gatt_event_query_complete_get_handle(event_packet) == peri->con_handle; + })); + + // Result of discovery. + btstack_error = gatt_event_query_complete_get_att_status(event_packet); + if (btstack_error != ERROR_CODE_SUCCESS) { + return att_error_to_pbio_error(btstack_error); + } + + if (!peri->char_now->handle) { + // Characteristic not found. + return PBIO_ERROR_FAILED; } + // If no notification is requested, we are done. + if (!peri->char_now->request_notification) { + // Success if we found the characteristic. + return peri->char_now->handle ? PBIO_SUCCESS : PBIO_ERROR_FAILED; + } + + // Discovered characteristics, ready to enable notifications. + btstack_error = gatt_client_write_client_characteristic_configuration( + packet_handler, peri->con_handle, current_char, GATT_CLIENT_CHARACTERISTICS_CONFIGURATION_NOTIFICATION); + + if (btstack_error != ERROR_CODE_SUCCESS) { + return att_error_to_pbio_error(btstack_error); + } + + // We will be waiting for another GATT_EVENT_QUERY_COMPLETE, but the + // current event is exactly this, so yield and wait for the next one. + PBIO_OS_AWAIT_ONCE(state); + PBIO_OS_AWAIT_UNTIL(state, ({ - // if there is any error while enumerating - // attributes, con_state will be set to CON_STATE_NONE - if (peri->platform_state->con_state == CON_STATE_NONE) { - return PBIO_ERROR_FAILED; + if (peri->con_handle == HCI_CON_HANDLE_INVALID) { + return PBIO_ERROR_NO_DEV; } - peri->platform_state->con_state == CON_STATE_DISCOVERY_AND_NOTIFICATIONS_COMPLETE; + hci_event_is_type(event_packet, GATT_EVENT_QUERY_COMPLETE) && + gatt_event_query_complete_get_handle(event_packet) == peri->con_handle; })); - // State state back to simply connected, so we can discover other characteristics. + // Result of enabling notifications. + btstack_error = gatt_event_query_complete_get_att_status(event_packet); + if (btstack_error != ERROR_CODE_SUCCESS) { + return att_error_to_pbio_error(btstack_error); + } + + // Register notification handler. + gatt_client_listen_for_characteristic_value_updates( + &peri->platform_state->notification, packet_handler, peri->con_handle, current_char); + + // REVISIT: Drop this legacy state when all tasks converted. + peri->platform_state->con_state = CON_STATE_DISCOVERY_AND_NOTIFICATIONS_COMPLETE; peri->platform_state->con_state = CON_STATE_CONNECTED; + peri->platform_state->btstack_error = ERROR_CODE_SUCCESS; PBIO_OS_ASYNC_END(peri->char_now->handle ? PBIO_SUCCESS : PBIO_ERROR_FAILED); } @@ -842,7 +886,7 @@ pbio_error_t pbdrv_bluetooth_peripheral_write_characteristic_func(pbio_os_state_ ); if (err != ERROR_CODE_SUCCESS) { - return PBIO_ERROR_FAILED; + return att_error_to_pbio_error(err); } // NB: Value buffer must remain valid until GATT_EVENT_QUERY_COMPLETE, so diff --git a/pybricks/iodevices/pb_type_iodevices_lwp3device.c b/pybricks/iodevices/pb_type_iodevices_lwp3device.c index ee4631ad2..1a6f1ace5 100644 --- a/pybricks/iodevices/pb_type_iodevices_lwp3device.c +++ b/pybricks/iodevices/pb_type_iodevices_lwp3device.c @@ -76,7 +76,7 @@ static const uint8_t pbio_lwp3_hub_service_uuid[] = { */ static pbdrv_bluetooth_peripheral_char_t pb_lwp3device_char = { .handle = 0, // Will be set during discovery. - .properties = 0, + .properties = 0x001c, // Notify, Write Without Response, Write .uuid16 = 0, .uuid128 = { 0x00, 0x00, 0x16, 0x24, 0x12, 0x12, 0xEF, 0xDE, From deb5e0b09b7f3b6bc71c71cff8b818195e1dca81 Mon Sep 17 00:00:00 2001 From: Laurens Valk Date: Mon, 5 Jan 2026 12:42:57 +0100 Subject: [PATCH 4/7] pbio/drv/bluetooth_btstack: Convert char read to task. As in the last commit, improve error handling and allow for multiple peripherals in the future. --- lib/pbio/drv/bluetooth/bluetooth_btstack.c | 57 +++++++++++----------- 1 file changed, 29 insertions(+), 28 deletions(-) diff --git a/lib/pbio/drv/bluetooth/bluetooth_btstack.c b/lib/pbio/drv/bluetooth/bluetooth_btstack.c index 68a26ff3d..0534d2aed 100644 --- a/lib/pbio/drv/bluetooth/bluetooth_btstack.c +++ b/lib/pbio/drv/bluetooth/bluetooth_btstack.c @@ -311,13 +311,6 @@ static void packet_handler(uint8_t packet_type, uint16_t channel, uint8_t *packe break; case GATT_EVENT_CHARACTERISTIC_VALUE_QUERY_RESULT: { DEBUG_PRINT("GATT_EVENT_CHARACTERISTIC_VALUE_QUERY_RESULT\n"); - hci_con_handle_t handle = gatt_event_characteristic_value_query_result_get_handle(packet); - uint16_t value_handle = gatt_event_characteristic_value_query_result_get_value_handle(packet); - uint16_t value_length = gatt_event_characteristic_value_query_result_get_value_length(packet); - if (peri->con_handle == handle && peri->char_now->handle == value_handle) { - peri->char_now->value_len = gatt_event_characteristic_value_query_result_get_value_length(packet); - memcpy(peri->char_now->value, gatt_event_characteristic_value_query_result_get_value(packet), value_length); - } break; } case GATT_EVENT_QUERY_COMPLETE: @@ -826,18 +819,15 @@ pbio_error_t pbdrv_bluetooth_peripheral_discover_characteristic_func(pbio_os_sta gatt_client_listen_for_characteristic_value_updates( &peri->platform_state->notification, packet_handler, peri->con_handle, current_char); - // REVISIT: Drop this legacy state when all tasks converted. - peri->platform_state->con_state = CON_STATE_DISCOVERY_AND_NOTIFICATIONS_COMPLETE; - peri->platform_state->con_state = CON_STATE_CONNECTED; - peri->platform_state->btstack_error = ERROR_CODE_SUCCESS; - - PBIO_OS_ASYNC_END(peri->char_now->handle ? PBIO_SUCCESS : PBIO_ERROR_FAILED); + PBIO_OS_ASYNC_END(PBIO_SUCCESS); } pbio_error_t pbdrv_bluetooth_peripheral_read_characteristic_func(pbio_os_state_t *state, void *context) { pbdrv_bluetooth_peripheral_t *peri = &peripheral_singleton; + uint8_t btstack_error; + PBIO_OS_ASYNC_BEGIN(state); if (peri->platform_state->con_state != CON_STATE_CONNECTED) { @@ -847,27 +837,38 @@ pbio_error_t pbdrv_bluetooth_peripheral_read_characteristic_func(pbio_os_state_t gatt_client_characteristic_t characteristic = { .value_handle = peri->char_now->handle, }; - peri->platform_state->btstack_error = gatt_client_read_value_of_characteristic(packet_handler, peri->con_handle, &characteristic); - - if (peri->platform_state->btstack_error == ERROR_CODE_SUCCESS) { - peri->platform_state->con_state = CON_STATE_WAIT_READ_CHARACTERISTIC; - } else { - // configuration failed for some reason, so disconnect - gap_disconnect(peri->con_handle); - peri->platform_state->con_state = CON_STATE_WAIT_DISCONNECT; - peri->platform_state->disconnect_reason = DISCONNECT_REASON_DISCOVER_CHARACTERISTIC_FAILED; + btstack_error = gatt_client_read_value_of_characteristic(packet_handler, peri->con_handle, &characteristic); + if (btstack_error != ERROR_CODE_SUCCESS) { + return att_error_to_pbio_error(btstack_error); } + // Await until read is complete, processing the result along the way. PBIO_OS_AWAIT_UNTIL(state, ({ - // if there is any error while reading, con_state will be set to CON_STATE_NONE - if (peri->platform_state->con_state == CON_STATE_NONE) { - return PBIO_ERROR_FAILED; + + // Got disconnected while waiting. + if (peri->con_handle == HCI_CON_HANDLE_INVALID) { + return PBIO_ERROR_NO_DEV; } - peri->platform_state->con_state == CON_STATE_READ_CHARACTERISTIC_COMPLETE; + + // Cache the result. + if (hci_event_is_type(event_packet, GATT_EVENT_CHARACTERISTIC_VALUE_QUERY_RESULT) && + gatt_event_characteristic_value_query_result_get_handle(event_packet) == peri->con_handle && + gatt_event_characteristic_value_query_result_get_value_handle(event_packet) == peri->char_now->handle + ) { + peri->char_now->value_len = gatt_event_characteristic_value_query_result_get_value_length(event_packet); + memcpy(peri->char_now->value, gatt_event_characteristic_value_query_result_get_value(event_packet), peri->char_now->value_len); + } + + // The wait until condition: read complete. + hci_event_is_type(event_packet, GATT_EVENT_QUERY_COMPLETE) && + gatt_event_query_complete_get_handle(event_packet) == peri->con_handle; })); - // State state back to simply connected, so we can discover other characteristics. - peri->platform_state->con_state = CON_STATE_CONNECTED; + // Result of read operation. + btstack_error = gatt_event_query_complete_get_att_status(event_packet); + if (btstack_error != ERROR_CODE_SUCCESS) { + return att_error_to_pbio_error(btstack_error); + } PBIO_OS_ASYNC_END(PBIO_SUCCESS); } From 3f714f5b048dca785c4690d9fff6594623419ada Mon Sep 17 00:00:00 2001 From: Laurens Valk Date: Mon, 5 Jan 2026 16:21:30 +0100 Subject: [PATCH 5/7] pbio/drv/bluetooth_btstack: Clean up write and disconnect. Make them consistent with read and discovery tasks. --- lib/pbio/drv/bluetooth/bluetooth_btstack.c | 34 ++++++++++++---------- 1 file changed, 18 insertions(+), 16 deletions(-) diff --git a/lib/pbio/drv/bluetooth/bluetooth_btstack.c b/lib/pbio/drv/bluetooth/bluetooth_btstack.c index 0534d2aed..3a1549d1c 100644 --- a/lib/pbio/drv/bluetooth/bluetooth_btstack.c +++ b/lib/pbio/drv/bluetooth/bluetooth_btstack.c @@ -666,11 +666,6 @@ pbio_error_t pbdrv_bluetooth_send_pybricks_value_notification(pbio_os_state_t *s PBIO_OS_ASYNC_END(PBIO_SUCCESS); } -static void start_observing(void) { - gap_set_scan_params(0, 0x30, 0x30, 0); - gap_start_scan(); -} - pbio_error_t pbdrv_bluetooth_peripheral_scan_and_connect_func(pbio_os_state_t *state, void *context) { pbdrv_bluetooth_peripheral_t *peri = &peripheral_singleton; @@ -877,32 +872,39 @@ pbio_error_t pbdrv_bluetooth_peripheral_write_characteristic_func(pbio_os_state_ pbdrv_bluetooth_peripheral_t *peri = &peripheral_singleton; + uint8_t btstack_error; + PBIO_OS_ASYNC_BEGIN(state); - uint8_t err = gatt_client_write_value_of_characteristic(packet_handler, + btstack_error = gatt_client_write_value_of_characteristic(packet_handler, peri->con_handle, pbdrv_bluetooth_char_write_handle, pbdrv_bluetooth_char_write_size, pbdrv_bluetooth_char_write_data ); - if (err != ERROR_CODE_SUCCESS) { - return att_error_to_pbio_error(err); + if (btstack_error != ERROR_CODE_SUCCESS) { + return att_error_to_pbio_error(btstack_error); } - // NB: Value buffer must remain valid until GATT_EVENT_QUERY_COMPLETE, so - // this wait is not cancelable. PBIO_OS_AWAIT_UNTIL(state, ({ if (peri->con_handle == HCI_CON_HANDLE_INVALID) { // disconnected return PBIO_ERROR_NO_DEV; } - event_packet && - hci_event_packet_get_type(event_packet) == GATT_EVENT_QUERY_COMPLETE && + + // The wait until condition: write complete. + hci_event_is_type(event_packet, GATT_EVENT_QUERY_COMPLETE) && gatt_event_query_complete_get_handle(event_packet) == peri->con_handle; })); - PBIO_OS_ASYNC_END(att_error_to_pbio_error(gatt_event_query_complete_get_att_status(event_packet))); + // Result of write operation. + btstack_error = gatt_event_query_complete_get_att_status(event_packet); + if (btstack_error != ERROR_CODE_SUCCESS) { + return att_error_to_pbio_error(btstack_error); + } + + PBIO_OS_ASYNC_END(PBIO_SUCCESS); } pbio_error_t pbdrv_bluetooth_peripheral_disconnect_func(pbio_os_state_t *state, void *context) { @@ -913,10 +915,9 @@ pbio_error_t pbdrv_bluetooth_peripheral_disconnect_func(pbio_os_state_t *state, if (peri->con_handle != HCI_CON_HANDLE_INVALID) { gap_disconnect(peri->con_handle); + PBIO_OS_AWAIT_UNTIL(state, peri->con_handle == HCI_CON_HANDLE_INVALID); } - PBIO_OS_AWAIT_UNTIL(state, peri->con_handle == HCI_CON_HANDLE_INVALID); - PBIO_OS_ASYNC_END(PBIO_SUCCESS); } @@ -948,7 +949,8 @@ pbio_error_t pbdrv_bluetooth_start_observing_func(pbio_os_state_t *state, void * PBIO_OS_ASYNC_BEGIN(state); if (!pbdrv_bluetooth_is_observing) { - start_observing(); + gap_set_scan_params(0, 0x30, 0x30, 0); + gap_start_scan(); pbdrv_bluetooth_is_observing = true; // REVISIT: use callback to await operation } From 040e39854eaf1b150fe21a8a2ab1958712d4a24f Mon Sep 17 00:00:00 2001 From: Laurens Valk Date: Wed, 7 Jan 2026 20:50:37 +0100 Subject: [PATCH 6/7] pbio/drv/bluetooth_btstack: Make peripheral scan/connect robust. This replaces the state machine logic of the scan and connect task with placing logic synchronously in a protothread. At every stage, we can detect, report and return errors, and handle cancellation correctly. This fixes a longstanding issue where the Xbox Controller would sometimes not pair to a SPIKE Prime Hub, even though a second attempt usually worked. Now we can catch the return codes from the security manager and re-try pairing within the task, or retry the whole task a second time. We also allow re-encryption now rather than disconnecting when this is attempted. Extensive USB logging is added to catch issues and flow of the connection process. --- CHANGELOG.md | 2 + lib/pbio/drv/bluetooth/bluetooth.c | 9 +- lib/pbio/drv/bluetooth/bluetooth_btstack.c | 468 +++++++++++------- lib/pbio/src/main.c | 22 +- .../pb_type_iodevices_xbox_controller.c | 50 +- 5 files changed, 335 insertions(+), 216 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index e25c37f21..7241c0b0d 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -33,7 +33,9 @@ - Fixed Powered Up remote and Xbox Controller working only after the first connection. All of the above Remote and Xbox Controller were introduced in the previous beta release, so did not affect any stable release ([support#2521]). +- Fixed Xbox Controller sometimes not working the first time ([support#1509]). +[support#1509]: https://github.com/pybricks/support/issues/1509 [support#1962]: https://github.com/pybricks/support/issues/1962 [support#2468]: https://github.com/pybricks/support/issues/2468 [support#2497]: https://github.com/pybricks/support/issues/2497 diff --git a/lib/pbio/drv/bluetooth/bluetooth.c b/lib/pbio/drv/bluetooth/bluetooth.c index 6700bbf4a..e8bd67075 100644 --- a/lib/pbio/drv/bluetooth/bluetooth.c +++ b/lib/pbio/drv/bluetooth/bluetooth.c @@ -1,5 +1,5 @@ // SPDX-License-Identifier: MIT -// Copyright (c) 2018-2025 The Pybricks Authors +// Copyright (c) 2018-2026 The Pybricks Authors // Common Bluetooth driver code @@ -195,6 +195,11 @@ pbio_error_t pbdrv_bluetooth_peripheral_scan_and_connect(pbdrv_bluetooth_periphe return PBIO_ERROR_BUSY; } + // Must provide matchers. + if (!config || !config->match_adv || !config->match_adv_rsp) { + return PBIO_ERROR_INVALID_ARG; + } + // Used to compare subsequent advertisements, so we should reset it. memset(peri->bdaddr, 0, sizeof(peri->bdaddr)); @@ -311,6 +316,7 @@ pbio_error_t pbdrv_bluetooth_await_peripheral_command(pbio_os_state_t *state, vo void pbdrv_bluetooth_cancel_operation_request(void) { // Only some peripheral actions support cancellation. + DEBUG_PRINT("Bluetooth operation cancel requested.\n"); peripheral_singleton.cancel = true; } @@ -645,6 +651,7 @@ void pbdrv_bluetooth_deinit(void) { // is, a task got stuck, so exit forcefully. if (advertising_or_scan_err == PBIO_ERROR_AGAIN || peripheral_singleton.err == PBIO_ERROR_AGAIN) { // Hard reset without waiting on completion of any process. + DEBUG_PRINT("Bluetooth deinit: forcing hard reset due to busy tasks.\n"); 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 3a1549d1c..f47097ace 100644 --- a/lib/pbio/drv/bluetooth/bluetooth_btstack.c +++ b/lib/pbio/drv/bluetooth/bluetooth_btstack.c @@ -1,5 +1,5 @@ // SPDX-License-Identifier: MIT -// Copyright (c) 2020-2023 The Pybricks Authors +// Copyright (c) 2020-2026 The Pybricks Authors // Bluetooth driver using BlueKitchen BTStack. @@ -43,6 +43,12 @@ #define HUB_VARIANT 0x0000 #endif +// Timeouts for various steps in the scan and connect process. +#define PERIPHERAL_TIMEOUT_MS_SCAN_RESPONSE (2000) +#define PERIPHERAL_TIMEOUT_MS_CONNECT (5000) +#define PERIPHERAL_TIMEOUT_MS_PAIRING (5000) + + #define DEBUG 0 #if DEBUG @@ -69,44 +75,21 @@ static const hci_dump_t pbdrv_bluetooth_btstack_hci_dump = { }; #endif -typedef enum { - CON_STATE_NONE, - CON_STATE_WAIT_ADV_IND, - CON_STATE_WAIT_SCAN_RSP, - CON_STATE_WAIT_CONNECT, - CON_STATE_WAIT_BONDING, - CON_STATE_CONNECTED, // End of connection state machine - CON_STATE_WAIT_DISCOVER_CHARACTERISTICS, - CON_STATE_WAIT_ENABLE_NOTIFICATIONS, - CON_STATE_DISCOVERY_AND_NOTIFICATIONS_COMPLETE, // End of discovery state machine, goes back to CONNECTED - CON_STATE_WAIT_READ_CHARACTERISTIC, - CON_STATE_READ_CHARACTERISTIC_COMPLETE, // End of read state machine, goes back to CONNECTED - CON_STATE_WAIT_DISCONNECT, -} con_state_t; - -typedef enum { - DISCONNECT_REASON_NONE, - DISCONNECT_REASON_TIMEOUT, - DISCONNECT_REASON_CONNECT_FAILED, - DISCONNECT_REASON_DISCOVER_SERVICE_FAILED, - DISCONNECT_REASON_DISCOVER_CHARACTERISTIC_FAILED, - DISCONNECT_REASON_CONFIGURE_CHARACTERISTIC_FAILED, - DISCONNECT_REASON_SEND_SUBSCRIBE_PORT_0_FAILED, - DISCONNECT_REASON_SEND_SUBSCRIBE_PORT_1_FAILED, -} disconnect_reason_t; - #if PBDRV_CONFIG_BLUETOOTH_BTSTACK_LE +/** + * BTstack state that we need to maintain for each peripheral. + */ static struct _pbdrv_bluetooth_peripheral_platform_state_t { + /** + * Current connection state, needed to unset callback on disconnect. + */ gatt_client_notification_t notification; - con_state_t con_state; - disconnect_reason_t disconnect_reason; /** * Character information used during discovery. Assuming properties and chars * are set up such that only one char is discovered at a time */ gatt_client_characteristic_t current_char; - uint8_t btstack_error; -} _handset; +} peripheral_platform_state; #endif // PBDRV_CONFIG_BLUETOOTH_BTSTACK_LE // hub name goes in special section so that it can be modified when flashing firmware @@ -160,6 +143,50 @@ static bool hci_event_is_type(uint8_t *packet, uint8_t event_type) { return packet && hci_event_packet_get_type(packet) == event_type; } +/** + * Shortcut to check if a LE connection event for a peripheral role occurred. + */ +static bool hci_event_le_peripheral_did_connect(uint8_t *packet) { + return + hci_event_is_type(event_packet, HCI_EVENT_LE_META) && + hci_event_le_meta_get_subevent_code(event_packet) == HCI_SUBEVENT_LE_CONNECTION_COMPLETE && + hci_subevent_le_connection_complete_get_role(event_packet) == HCI_ROLE_MASTER; +} + +/** + * Shortcut to check if a pairing or reencryption complete event for a given + * handle occurred. Indicates end of pairing process. + */ +static bool hci_event_le_peripheral_pairing_did_complete(uint8_t *packet, uint16_t handle) { + if (hci_event_is_type(event_packet, SM_EVENT_PAIRING_COMPLETE)) { + return sm_event_pairing_complete_get_handle(event_packet) == handle; + } + if (hci_event_is_type(event_packet, SM_EVENT_REENCRYPTION_COMPLETE)) { + return sm_event_reencryption_complete_get_handle(event_packet) == handle; + } + return false; +} + +/** + * Wrapper for gap_disconnect that is safe to call if already disconnected. + */ +static void pbdrv_bluetooth_peripheral_disconnect_now(pbdrv_bluetooth_peripheral_t *peri) { + if (peri->con_handle == HCI_CON_HANDLE_INVALID) { + // Already disconnected. We must check this because otherwise + // gap_disconnect() will synchronously call the disconnection complete + // handler and re-enter the event loop recursively. + return; + } + gap_disconnect(peri->con_handle); +} + +/** + * Checks if the given peripheral is connected. + */ +static bool pbdrv_bluetooth_peripheral_is_connected(pbdrv_bluetooth_peripheral_t *peri) { + return peri->con_handle != HCI_CON_HANDLE_INVALID; +} + #endif // PBDRV_CONFIG_BLUETOOTH_BTSTACK_LE static pbio_os_state_t bluetooth_thread_state; @@ -315,13 +342,7 @@ static void packet_handler(uint8_t packet_type, uint16_t channel, uint8_t *packe } case GATT_EVENT_QUERY_COMPLETE: DEBUG_PRINT("GATT_EVENT_QUERY_COMPLETE\n"); - - if (peri->platform_state->con_state == CON_STATE_WAIT_READ_CHARACTERISTIC) { - // Done reading characteristic. - peri->platform_state->con_state = CON_STATE_READ_CHARACTERISTIC_COMPLETE; - } break; - case GATT_EVENT_NOTIFICATION: { if (gatt_event_notification_get_handle(packet) != peri->con_handle) { break; @@ -333,12 +354,10 @@ static void packet_handler(uint8_t packet_type, uint16_t channel, uint8_t *packe } break; } - case HCI_EVENT_LE_META: if (hci_event_le_meta_get_subevent_code(packet) != HCI_SUBEVENT_LE_CONNECTION_COMPLETE) { break; } - // HCI_ROLE_SLAVE means the connecting device is the central and the hub is the peripheral // HCI_ROLE_MASTER means the connecting device is the peripheral and the hub is the central. if (hci_subevent_le_connection_complete_get_role(packet) == HCI_ROLE_SLAVE) { @@ -347,105 +366,36 @@ static void packet_handler(uint8_t packet_type, uint16_t channel, uint8_t *packe // don't start advertising again on disconnect gap_advertisements_enable(false); pbdrv_bluetooth_advertising_state = PBDRV_BLUETOOTH_ADVERTISING_STATE_NONE; - } else { - // If we aren't waiting for a peripheral connection, this must be a different connection. - if (peri->platform_state->con_state != CON_STATE_WAIT_CONNECT) { - break; - } - - peri->con_handle = hci_subevent_le_connection_complete_get_connection_handle(packet); - - // Request pairing if needed for this device, otherwise set - // connection state to complete. - if (peri->config->options & PBDRV_BLUETOOTH_PERIPHERAL_OPTIONS_PAIR) { - // Re-encryption doesn't seem to work reliably, so we just - // delete the bond and start over. - gap_delete_bonding(peri->bdaddr_type, peri->bdaddr); - sm_request_pairing(peri->con_handle); - peri->platform_state->con_state = CON_STATE_WAIT_BONDING; - } else { - peri->platform_state->con_state = CON_STATE_CONNECTED; - } } - break; - case HCI_EVENT_DISCONNECTION_COMPLETE: + DEBUG_PRINT("HCI_EVENT_DISCONNECTION_COMPLETE\n"); if (hci_event_disconnection_complete_get_connection_handle(packet) == le_con_handle) { le_con_handle = HCI_CON_HANDLE_INVALID; pybricks_con_handle = HCI_CON_HANDLE_INVALID; uart_con_handle = HCI_CON_HANDLE_INVALID; } else if (hci_event_disconnection_complete_get_connection_handle(packet) == peri->con_handle) { + DEBUG_PRINT("Peripheral disconnected\n"); gatt_client_stop_listening_for_characteristic_value_updates(&peri->platform_state->notification); peri->con_handle = HCI_CON_HANDLE_INVALID; - peri->platform_state->con_state = CON_STATE_NONE; } - break; case GAP_EVENT_ADVERTISING_REPORT: { uint8_t event_type = gap_event_advertising_report_get_advertising_event_type(packet); uint8_t data_length = gap_event_advertising_report_get_data_length(packet); const uint8_t *data = gap_event_advertising_report_get_data(packet); - bd_addr_t address; - - gap_event_advertising_report_get_address(packet, address); if (pbdrv_bluetooth_observe_callback) { int8_t rssi = gap_event_advertising_report_get_rssi(packet); pbdrv_bluetooth_observe_callback(event_type, data, data_length, rssi); } - if (peri->platform_state->con_state == CON_STATE_WAIT_ADV_IND) { - // Match advertisement data against context-specific filter. - pbdrv_bluetooth_ad_match_result_flags_t adv_flags = PBDRV_BLUETOOTH_AD_MATCH_NONE; - if (peri->config->match_adv) { - adv_flags = peri->config->match_adv(peri->user, event_type, data, NULL, address, peri->bdaddr); - } - - if (adv_flags & PBDRV_BLUETOOTH_AD_MATCH_VALUE) { - if (adv_flags & PBDRV_BLUETOOTH_AD_MATCH_ADDRESS) { - // This was the same device as last time. If the scan response - // didn't match before, it probably won't match now and we - // should try a different device. - break; - } - // Advertising data matched, prepare for scan response. - memcpy(peri->bdaddr, address, sizeof(bd_addr_t)); - peri->bdaddr_type = gap_event_advertising_report_get_address_type(packet); - peri->platform_state->con_state = CON_STATE_WAIT_SCAN_RSP; - } - } else if (peri->platform_state->con_state == CON_STATE_WAIT_SCAN_RSP) { - - char *detected_name = (char *)&data[2]; - const uint8_t max_len = sizeof(peri->name); - - pbdrv_bluetooth_ad_match_result_flags_t rsp_flags = PBDRV_BLUETOOTH_AD_MATCH_NONE; - if (peri->config->match_adv_rsp) { - rsp_flags = peri->config->match_adv_rsp(peri->user, event_type, NULL, detected_name, address, peri->bdaddr); - } - if ((rsp_flags & PBDRV_BLUETOOTH_AD_MATCH_VALUE) && (rsp_flags & PBDRV_BLUETOOTH_AD_MATCH_ADDRESS)) { - - if (rsp_flags & PBDRV_BLUETOOTH_AD_MATCH_NAME_FAILED) { - // A name was requested but it doesn't match, so go back to scanning stage. - peri->platform_state->con_state = CON_STATE_WAIT_ADV_IND; - break; - } - - if (data[1] == BLUETOOTH_DATA_TYPE_COMPLETE_LOCAL_NAME) { - memcpy(peri->name, detected_name, max_len); - } - - gap_stop_scan(); - peri->platform_state->btstack_error = gap_connect(peri->bdaddr, peri->bdaddr_type); - - if (peri->platform_state->btstack_error == ERROR_CODE_SUCCESS) { - peri->platform_state->con_state = CON_STATE_WAIT_CONNECT; - } else { - peri->platform_state->con_state = CON_STATE_NONE; - } - } - } + #if DEBUG + bd_addr_t address; + gap_event_advertising_report_get_address(packet, address); + DEBUG_PRINT("GAP_EVENT_ADVERTISING_REPORT from addr %s type %d len %d\n", bd_addr_to_str(address), event_type, data_length); + #endif break; } @@ -468,9 +418,6 @@ static void sm_packet_handler(uint8_t packet_type, uint16_t channel, uint8_t *pa return; } - bd_addr_t addr; - bd_addr_type_t addr_type; - switch (hci_event_packet_get_type(packet)) { case SM_EVENT_IDENTITY_RESOLVING_STARTED: DEBUG_PRINT("IDENTITY_RESOLVING_STARTED\n"); @@ -501,50 +448,23 @@ static void sm_packet_handler(uint8_t packet_type, uint16_t channel, uint8_t *pa DEBUG_PRINT("Pairing started\n"); break; case SM_EVENT_PAIRING_COMPLETE: - switch (sm_event_pairing_complete_get_status(packet)) { - case ERROR_CODE_SUCCESS: - // This is the final state for known compatible peripherals - // with bonding under normal circumstances. - DEBUG_PRINT("Pairing complete, success\n"); - peripheral_singleton.platform_state->con_state = CON_STATE_CONNECTED; - break; - case ERROR_CODE_CONNECTION_TIMEOUT: - // fall through to disconnect. - case ERROR_CODE_REMOTE_USER_TERMINATED_CONNECTION: - // fall through to disconnect. - case ERROR_CODE_AUTHENTICATION_FAILURE: - // fall through to disconnect. - default: - DEBUG_PRINT( - "Pairing completed with error code %u and reason %u\n", - sm_event_pairing_complete_get_status(packet), - sm_event_pairing_complete_get_reason(packet) - ); - gap_disconnect(sm_event_reencryption_complete_get_handle(packet)); - break; - } + DEBUG_PRINT("Pairing complete\n"); break; - case SM_EVENT_REENCRYPTION_STARTED: + case SM_EVENT_REENCRYPTION_STARTED: { + bd_addr_t addr; sm_event_reencryption_complete_get_address(packet, addr); DEBUG_PRINT("Bonding information exists for addr type %u, identity addr %s -> start re-encryption\n", sm_event_reencryption_started_get_addr_type(packet), bd_addr_to_str(addr)); break; + } case SM_EVENT_REENCRYPTION_COMPLETE: - // BTstack supports re-encryption, but it gets the hub in a hung - // state with certain peripherals. Instead we just delete the bond - // just before connecting. If we still get here, we should delete - // the bond and disconnect, causing the user program stop without - // hanging. We rely on HCI_EVENT_DISCONNECTION_COMPLETE to set - // the connection state appropriately to unblock the task. - DEBUG_PRINT("Re-encryption complete. Handling not implemented.\n"); - sm_event_reencryption_complete_get_address(packet, addr); - addr_type = sm_event_reencryption_started_get_addr_type(packet); - gap_delete_bonding(addr_type, addr); - gap_disconnect(sm_event_reencryption_complete_get_handle(packet)); + DEBUG_PRINT("Re-encryption complete.\n"); break; default: break; } + + propagate_event(packet); } // ATT Client Read Callback for Dynamic Data @@ -669,9 +589,8 @@ pbio_error_t pbdrv_bluetooth_send_pybricks_value_notification(pbio_os_state_t *s pbio_error_t pbdrv_bluetooth_peripheral_scan_and_connect_func(pbio_os_state_t *state, void *context) { pbdrv_bluetooth_peripheral_t *peri = &peripheral_singleton; - - // Scan and connect timeout, if applicable. - bool timed_out = peri->config->timeout && pbio_os_timer_is_expired(&peri->timer); + pbdrv_bluetooth_ad_match_result_flags_t flags; + uint8_t btstack_error; // Operation can be explicitly cancelled or automatically on inactivity. if (!peri->cancel) { @@ -688,29 +607,198 @@ pbio_error_t pbdrv_bluetooth_peripheral_scan_and_connect_func(pbio_os_state_t *s // scan interval: 48 * 0.625ms = 30ms gap_set_scan_params(1, 0x30, 0x30, 0); gap_start_scan(); - peri->platform_state->con_state = CON_STATE_WAIT_ADV_IND; - PBIO_OS_AWAIT_UNTIL(state, ({ - if (peri->cancel || timed_out) { - if (peri->platform_state->con_state == CON_STATE_WAIT_ADV_IND || peri->platform_state->con_state == CON_STATE_WAIT_SCAN_RSP) { - gap_stop_scan(); - } else if (peri->platform_state->con_state == CON_STATE_WAIT_CONNECT) { - gap_connect_cancel(); - } else if (peri->con_handle != HCI_CON_HANDLE_INVALID) { - gap_disconnect(peri->con_handle); - } - peri->platform_state->con_state = CON_STATE_NONE; - return timed_out ? PBIO_ERROR_TIMEDOUT : PBIO_ERROR_CANCELED; +start_scan: + + // Wait for advertisement that matches the filter unless timed out or cancelled. + PBIO_OS_AWAIT_UNTIL(state, (peri->config->timeout && pbio_os_timer_is_expired(&peri->timer)) || + peri->cancel || (hci_event_is_type(event_packet, GAP_EVENT_ADVERTISING_REPORT) && ({ + + uint8_t event_type = gap_event_advertising_report_get_advertising_event_type(event_packet); + const uint8_t *data = gap_event_advertising_report_get_data(event_packet); + bd_addr_t address; + gap_event_advertising_report_get_address(event_packet, address); + + // Match advertisement data against context-specific filter. + flags = peri->config->match_adv(peri->user, event_type, data, NULL, address, peri->bdaddr); + + // Store the address to compare with scan response later. + if (flags & PBDRV_BLUETOOTH_AD_MATCH_VALUE) { + memcpy(peri->bdaddr, address, sizeof(bd_addr_t)); + peri->bdaddr_type = gap_event_advertising_report_get_address_type(event_packet); } - // if there is any failure to connect or error while enumerating - // attributes, con_state will be set to CON_STATE_NONE - if (peri->platform_state->con_state == CON_STATE_NONE) { - return PBIO_ERROR_FAILED; + + // Wait condition: Advertisement matched and it isn't the same as before. + // If it was the same and we're here, it means the scan response didn't match + // so we shouldn't try it again. + (flags & PBDRV_BLUETOOTH_AD_MATCH_VALUE) && !(flags & PBDRV_BLUETOOTH_AD_MATCH_ADDRESS); + }))); + + if ((peri->config->timeout && pbio_os_timer_is_expired(&peri->timer)) || peri->cancel) { + DEBUG_PRINT("Scan %s.\n", peri->cancel ? "canceled": "timed out"); + gap_stop_scan(); + return peri->cancel ? PBIO_ERROR_CANCELED : PBIO_ERROR_TIMEDOUT; + } + + DEBUG_PRINT("Advertisement matched, waiting for scan response\n"); + + // The user timeout applies only to finding the device. We still want to + // have a reasonable timeout for the scan response, connecting and pairing. + pbio_os_timer_set(&peri->timer, PERIPHERAL_TIMEOUT_MS_SCAN_RESPONSE); + + // Wait for advertising response that matches the filter unless timed out or cancelled. + PBIO_OS_AWAIT_UNTIL(state, pbio_os_timer_is_expired(&peri->timer) || peri->cancel || + (hci_event_is_type(event_packet, GAP_EVENT_ADVERTISING_REPORT) && ({ + + uint8_t event_type = gap_event_advertising_report_get_advertising_event_type(event_packet); + const uint8_t *data = gap_event_advertising_report_get_data(event_packet); + char *detected_name = (char *)&data[2]; + bd_addr_t address; + gap_event_advertising_report_get_address(event_packet, address); + + flags = peri->config->match_adv_rsp(peri->user, event_type, NULL, detected_name, address, peri->bdaddr); + + (flags & PBDRV_BLUETOOTH_AD_MATCH_VALUE) && (flags & PBDRV_BLUETOOTH_AD_MATCH_ADDRESS); + }))); + + if (pbio_os_timer_is_expired(&peri->timer) || peri->cancel) { + DEBUG_PRINT("Scan response %s.\n", peri->cancel ? "canceled": "timed out"); + gap_stop_scan(); + return peri->cancel ? PBIO_ERROR_CANCELED : PBIO_ERROR_TIMEDOUT; + } + + if (flags & PBDRV_BLUETOOTH_AD_MATCH_NAME_FAILED) { + DEBUG_PRINT("Name requested but did not match. Scan again.\n"); + goto start_scan; + } + + // When we get here, we have just matched a scan response and we are still + // handling the same event packet, so we can still extract the name. + const uint8_t *data = gap_event_advertising_report_get_data(event_packet); + if (data[1] == BLUETOOTH_DATA_TYPE_COMPLETE_LOCAL_NAME) { + memcpy(peri->name, &data[2], sizeof(peri->name)); + } + + DEBUG_PRINT("Scan response matched, initiate connection to %s.\n", bd_addr_to_str(peri->bdaddr)); + + // We can stop scanning now. + gap_stop_scan(); + + // Initiate connection and await connection complete event. + pbio_os_timer_set(&peri->timer, PERIPHERAL_TIMEOUT_MS_CONNECT); + btstack_error = gap_connect(peri->bdaddr, peri->bdaddr_type); + if (btstack_error != ERROR_CODE_SUCCESS) { + return att_error_to_pbio_error(btstack_error); + } + PBIO_OS_AWAIT_UNTIL(state, pbio_os_timer_is_expired(&peri->timer) || peri->cancel || + hci_event_le_peripheral_did_connect(event_packet)); + + // If we timed out or were cancelled, abort the connection. We have to check + // the event again in case cancellation and connection completed simultaneously. + if (!hci_event_le_peripheral_did_connect(event_packet)) { + DEBUG_PRINT("Connection %s.\n", peri->cancel ? "canceled": "timed out"); + gap_connect_cancel(); + return peri->cancel ? PBIO_ERROR_CANCELED : PBIO_ERROR_TIMEDOUT; + } + + // The wait above was not interrupted, so we are now connected and the event + // packet is still valid. + peri->con_handle = hci_subevent_le_connection_complete_get_connection_handle(event_packet); + DEBUG_PRINT("Connected with handle %d.\n", peri->con_handle); + + // We are done if no pairing is requested. + if (!peri->config->options & PBDRV_BLUETOOTH_PERIPHERAL_OPTIONS_PAIR) { + return PBIO_SUCCESS; + } + +start_pairing: + + if (!pbdrv_bluetooth_peripheral_is_connected(peri)) { + DEBUG_PRINT("Not connected anymore, cannot pair.\n"); + return PBIO_ERROR_NO_DEV; + } + + // Re-encryption doesn't seem to work reliably, so we always delete the + // bond and start over. REVISIT: We should be able to catch this now, so + // allow re-encryption to succeed. + DEBUG_PRINT("Request pairing.\n"); + pbio_os_timer_set(&peri->timer, PERIPHERAL_TIMEOUT_MS_PAIRING); + gap_delete_bonding(peri->bdaddr_type, peri->bdaddr); + sm_request_pairing(peri->con_handle); + + // Wait for pairing to complete unless timed out, cancelled, or disconnected. + PBIO_OS_AWAIT_UNTIL(state, pbio_os_timer_is_expired(&peri->timer) || peri->cancel || + !pbdrv_bluetooth_peripheral_is_connected(peri) || + hci_event_le_peripheral_pairing_did_complete(event_packet, peri->con_handle)); + + // If we timed out or were cancelled, disconnect and leave. + if (!hci_event_le_peripheral_pairing_did_complete(event_packet, peri->con_handle)) { + if (!pbdrv_bluetooth_peripheral_is_connected(peri)) { + DEBUG_PRINT("Not connected anymore, cannot complete pairing.\n"); + return PBIO_ERROR_NO_DEV; } - peri->platform_state->con_state == CON_STATE_CONNECTED; - })); + DEBUG_PRINT("Bonding %s.\n", peri->cancel ? "canceled": "timed out"); + pbdrv_bluetooth_peripheral_disconnect_now(peri); + return peri->cancel ? PBIO_ERROR_CANCELED : PBIO_ERROR_TIMEDOUT; + } - PBIO_OS_ASYNC_END(PBIO_SUCCESS); + // Pairing ended, successfully or not, either as a new pairing or re-encryption. + // Test re-encryption result first. + if (hci_event_is_type(event_packet, SM_EVENT_REENCRYPTION_COMPLETE)) { + btstack_error = sm_event_reencryption_complete_get_status(event_packet); + DEBUG_PRINT("Re-encryption complete, bonded device with error %u.\n", btstack_error); + + if (btstack_error == ERROR_CODE_SUCCESS) { + DEBUG_PRINT("Re-encryption successful, bonded device.\n"); + return PBIO_SUCCESS; + } + + switch (btstack_error) { + case ERROR_CODE_PIN_OR_KEY_MISSING: + DEBUG_PRINT("Bonding information missing.\n"); + break; + case ERROR_CODE_CONNECTION_FAILED_TO_BE_ESTABLISHED: + // fallthrough + case ERROR_CODE_REMOTE_USER_TERMINATED_CONNECTION: + DEBUG_PRINT("Re-encrytion failed. Did remote disconnect?\n"); + break; + default: + DEBUG_PRINT("Other re-encryption failure.\n"); + break; + } + + DEBUG_PRINT("Deleting bond for %s and retrying.\n", bd_addr_to_str(peri->bdaddr)); + gap_delete_bonding(peri->bdaddr_type, peri->bdaddr); + PBIO_OS_AWAIT_ONCE(state); + goto start_pairing; + + } else if (hci_event_is_type(event_packet, SM_EVENT_PAIRING_COMPLETE)) { + // Otherwise we have received pairing complete. Check status and disconnect + // on failure. + btstack_error = sm_event_pairing_complete_get_status(event_packet); + DEBUG_PRINT("New pairing completed with error %u and reason %u.\n", + btstack_error, sm_event_pairing_complete_get_reason(event_packet)); + + if (btstack_error == ERROR_CODE_SUCCESS) { + DEBUG_PRINT("Pairing successful.\n"); + return PBIO_SUCCESS; + } + + switch (btstack_error) { + case ERROR_CODE_REMOTE_USER_TERMINATED_CONNECTION: + DEBUG_PRINT("Did remote disconnect?\n"); + break; + default: + DEBUG_PRINT("Other pairing failure.\n"); + break; + } + + pbdrv_bluetooth_peripheral_disconnect_now(peri); + return att_error_to_pbio_error(btstack_error); + } + + // Unreachable. We should have hit either of the above events. + PBIO_OS_ASYNC_END(PBIO_ERROR_FAILED); } pbio_error_t pbdrv_bluetooth_peripheral_discover_characteristic_func(pbio_os_state_t *state, void *context) { @@ -737,12 +825,13 @@ pbio_error_t pbdrv_bluetooth_peripheral_discover_characteristic_func(pbio_os_sta return att_error_to_pbio_error(btstack_error); } + DEBUG_PRINT("Discovering characteristic\n"); // Await until discovery is complete, processing each discovered // characteristic along the way to see if it matches what we need. PBIO_OS_AWAIT_UNTIL(state, ({ - // Got disconnected while waiting. - if (peri->con_handle == HCI_CON_HANDLE_INVALID) { + if (!pbdrv_bluetooth_peripheral_is_connected(peri)) { + // Got disconnected while waiting. return PBIO_ERROR_NO_DEV; } @@ -796,8 +885,10 @@ pbio_error_t pbdrv_bluetooth_peripheral_discover_characteristic_func(pbio_os_sta // current event is exactly this, so yield and wait for the next one. PBIO_OS_AWAIT_ONCE(state); + DEBUG_PRINT("Waiting for notifications to be enabled.\n"); PBIO_OS_AWAIT_UNTIL(state, ({ - if (peri->con_handle == HCI_CON_HANDLE_INVALID) { + if (!pbdrv_bluetooth_peripheral_is_connected(peri)) { + // Got disconnected while waiting. return PBIO_ERROR_NO_DEV; } hci_event_is_type(event_packet, GATT_EVENT_QUERY_COMPLETE) && @@ -825,8 +916,8 @@ pbio_error_t pbdrv_bluetooth_peripheral_read_characteristic_func(pbio_os_state_t PBIO_OS_ASYNC_BEGIN(state); - if (peri->platform_state->con_state != CON_STATE_CONNECTED) { - return PBIO_ERROR_FAILED; + if (!pbdrv_bluetooth_peripheral_is_connected(peri)) { + return PBIO_ERROR_NO_DEV; } gatt_client_characteristic_t characteristic = { @@ -839,12 +930,10 @@ pbio_error_t pbdrv_bluetooth_peripheral_read_characteristic_func(pbio_os_state_t // Await until read is complete, processing the result along the way. PBIO_OS_AWAIT_UNTIL(state, ({ - - // Got disconnected while waiting. - if (peri->con_handle == HCI_CON_HANDLE_INVALID) { + if (!pbdrv_bluetooth_peripheral_is_connected(peri)) { + // Got disconnected while waiting. return PBIO_ERROR_NO_DEV; } - // Cache the result. if (hci_event_is_type(event_packet, GATT_EVENT_CHARACTERISTIC_VALUE_QUERY_RESULT) && gatt_event_characteristic_value_query_result_get_handle(event_packet) == peri->con_handle && @@ -888,8 +977,8 @@ pbio_error_t pbdrv_bluetooth_peripheral_write_characteristic_func(pbio_os_state_ } PBIO_OS_AWAIT_UNTIL(state, ({ - if (peri->con_handle == HCI_CON_HANDLE_INVALID) { - // disconnected + if (!pbdrv_bluetooth_peripheral_is_connected(peri)) { + // Got disconnected while waiting. return PBIO_ERROR_NO_DEV; } @@ -913,10 +1002,9 @@ pbio_error_t pbdrv_bluetooth_peripheral_disconnect_func(pbio_os_state_t *state, PBIO_OS_ASYNC_BEGIN(state); - if (peri->con_handle != HCI_CON_HANDLE_INVALID) { - gap_disconnect(peri->con_handle); - PBIO_OS_AWAIT_UNTIL(state, peri->con_handle == HCI_CON_HANDLE_INVALID); - } + // If already disconnected, this completes immediately. + pbdrv_bluetooth_peripheral_disconnect_now(peri); + PBIO_OS_AWAIT_UNTIL(state, peri->con_handle == HCI_CON_HANDLE_INVALID); PBIO_OS_ASYNC_END(PBIO_SUCCESS); } @@ -1177,7 +1265,7 @@ void pbdrv_bluetooth_init_hci(void) { #if PBDRV_CONFIG_BLUETOOTH_BTSTACK_LE // don't need to init the whole struct, so doing this here - peripheral_singleton.platform_state = &_handset; + peripheral_singleton.platform_state = &peripheral_platform_state; peripheral_singleton.con_handle = HCI_CON_HANDLE_INVALID; #endif diff --git a/lib/pbio/src/main.c b/lib/pbio/src/main.c index 944b5ad95..306c18cce 100644 --- a/lib/pbio/src/main.c +++ b/lib/pbio/src/main.c @@ -1,10 +1,5 @@ // SPDX-License-Identifier: MIT -// Copyright (c) 2018-2021 The Pybricks Authors - -/** - * @addtogroup Main Library initialization and events - * @{ - */ +// Copyright (c) 2018-2026 The Pybricks Authors #include @@ -19,6 +14,15 @@ #include #include +#define DEBUG 0 + +#if DEBUG +#include +#define DEBUG_PRINT pbio_debug +#else +#define DEBUG_PRINT(...) +#endif + /** * Initialize the Pybricks I/O Library. This function must be called once, * usually at the beginning of a program, before using any other functions in @@ -80,11 +84,15 @@ pbio_error_t pbio_main_stop_application_resources(void) { pbio_os_timer_t timer; pbio_os_timer_set(&timer, 5000); + DEBUG_PRINT("Waiting for Bluetooth user tasks to close...\n"); + // Run event loop until Bluetooth is idle or times out. while ((err = pbdrv_bluetooth_close_user_tasks(&state, &timer)) == PBIO_ERROR_AGAIN) { pbio_os_run_processes_and_wait_for_event(); } + DEBUG_PRINT("Bluetooth user tasks closed with err %d.\n", err); + #if PBIO_CONFIG_LIGHT pbio_light_animation_stop_all(); #endif @@ -98,5 +106,3 @@ pbio_error_t pbio_main_stop_application_resources(void) { return err; } - -/** @} */ diff --git a/pybricks/iodevices/pb_type_iodevices_xbox_controller.c b/pybricks/iodevices/pb_type_iodevices_xbox_controller.c index 787526a3e..529864ce0 100644 --- a/pybricks/iodevices/pb_type_iodevices_xbox_controller.c +++ b/pybricks/iodevices/pb_type_iodevices_xbox_controller.c @@ -12,6 +12,7 @@ #include #include #include +#include #include #include #include @@ -30,7 +31,7 @@ #define DEBUG 0 #if DEBUG -#define DEBUG_PRINT(...) mp_printf(&mp_plat_print, __VA_ARGS__) +#define DEBUG_PRINT pbio_debug #else #define DEBUG_PRINT(...) #endif @@ -81,6 +82,10 @@ typedef struct _pb_type_xbox_obj_t { * The peripheral instance associated with this MicroPython object. */ pbdrv_bluetooth_peripheral_t *peripheral; + /** + * Timer used to delay between connection attempts. + */ + pbio_os_timer_t retry_timer; /** * Buttons object on the Xbox Controller instance. */ @@ -290,27 +295,31 @@ static pbio_error_t xbox_connect_thread(pbio_os_state_t *state, mp_obj_t parent_ pbio_error_t err; - PBIO_OS_ASYNC_BEGIN(state); - pb_type_xbox_obj_t *self = MP_OBJ_TO_PTR(parent_obj); + PBIO_OS_ASYNC_BEGIN(state); + // Get available peripheral instance. pb_assert(pbdrv_bluetooth_peripheral_get_available(&self->peripheral, self)); + pbio_os_timer_set(&self->retry_timer, 0); - // Connect with bonding enabled. On some computers, the pairing step will - // fail if the hub is still connected to Pybricks Code. Since it is unclear - // which computer will have this problem, recommend to disconnect the hub - // if this happens. + // Connect with bonding enabled. On Technic Hub, the driver will take care + // of disconnecting from Pybricks Code if needed. +retry: + DEBUG_PRINT("Attempt to find XBOX controller and connect and pair.\n"); pb_assert(pbdrv_bluetooth_peripheral_scan_and_connect(&scan_config)); PBIO_OS_AWAIT(state, &unused, err = pbdrv_bluetooth_await_peripheral_command(&unused, NULL)); - if (err == PBIO_ERROR_INVALID_OP) { - mp_raise_msg(&mp_type_OSError, MP_ERROR_TEXT( - "Failed to pair. Disconnect the hub from the computer " - "and re-start the program with the green button on the hub\n." - )); + + // On Prime Hub, connecting can fail for a variety of reasons. A second + // attempt usually succeeds. + if (err != PBIO_SUCCESS && !self->retry_timer.duration) { + DEBUG_PRINT("XBOX controller failed to connect with error %d.\n", err); + PBIO_OS_AWAIT_MS(state, &self->retry_timer, 2000); + goto retry; } + pb_assert(err); - DEBUG_PRINT("Connected to XBOX controller.\n"); + DEBUG_PRINT("Connected to XBOX controller. Discovering HID map.\n"); // It seems we need to read the (unused) map only once after pairing // to make the controller active. We'll still read it every time to @@ -322,6 +331,8 @@ static pbio_error_t xbox_connect_thread(pbio_os_state_t *state, mp_obj_t parent_ if (err != PBIO_SUCCESS) { goto disconnect; } + + DEBUG_PRINT("Read HID map.\n"); pb_assert(pbdrv_bluetooth_peripheral_read_characteristic(&pb_type_xbox_char_hid_map)); PBIO_OS_AWAIT(state, &unused, err = pbdrv_bluetooth_await_peripheral_command(&unused, NULL)); if (err != PBIO_SUCCESS) { @@ -329,11 +340,14 @@ static pbio_error_t xbox_connect_thread(pbio_os_state_t *state, mp_obj_t parent_ } // This is the main characteristic that notifies us of button state. + DEBUG_PRINT("Discover HID report.\n"); pb_assert(pbdrv_bluetooth_peripheral_discover_characteristic(&pb_type_xbox_char_hid_report)); PBIO_OS_AWAIT(state, &unused, err = pbdrv_bluetooth_await_peripheral_command(&unused, NULL)); if (err != PBIO_SUCCESS) { goto disconnect; } + + DEBUG_PRINT("Read HID report.\n"); pb_assert(pbdrv_bluetooth_peripheral_read_characteristic(&pb_type_xbox_char_hid_report)); PBIO_OS_AWAIT(state, &unused, err = pbdrv_bluetooth_await_peripheral_command(&unused, NULL)); if (err != PBIO_SUCCESS) { @@ -343,9 +357,11 @@ static pbio_error_t xbox_connect_thread(pbio_os_state_t *state, mp_obj_t parent_ return PBIO_SUCCESS; disconnect: - PBIO_OS_AWAIT(state, &unused, pbdrv_bluetooth_await_peripheral_command(&unused, NULL)); - pbdrv_bluetooth_peripheral_disconnect(); - PBIO_OS_AWAIT(state, &unused, pbdrv_bluetooth_await_peripheral_command(&unused, NULL)); + DEBUG_PRINT("Going to disconnect because of a failure with code %d at line %u.\n", err, *state); + pb_assert(pbdrv_bluetooth_peripheral_disconnect()); + PBIO_OS_AWAIT(state, &unused, err = pbdrv_bluetooth_await_peripheral_command(&unused, NULL)); + DEBUG_PRINT("Disconnect with error code %d.\n", err); + pb_assert(err); // If the controller was most recently connected to another device like the // actual Xbox or a phone, the controller needs to be not just turned on, @@ -356,7 +372,7 @@ static pbio_error_t xbox_connect_thread(pbio_os_state_t *state, mp_obj_t parent_ mp_raise_msg(&mp_type_OSError, MP_ERROR_TEXT( "Connected, but not allowed to read buttons. " - "Is the controller in pairing mode?" + "Try putting the controller in pairing mode." )); PBIO_OS_ASYNC_END(PBIO_ERROR_IO); From 136126d70ff7dabca4adf5dab424ae6403e9b2d3 Mon Sep 17 00:00:00 2001 From: Laurens Valk Date: Thu, 8 Jan 2026 09:39:31 +0100 Subject: [PATCH 7/7] pbio/drv/bluetooth: Always set background function error state. Some functions have an early exit because no changes are needed. In these cases, we'll want to set their error state to success so that asserting their error does not raise if an error was set and raised previously. --- lib/pbio/drv/bluetooth/bluetooth.c | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/lib/pbio/drv/bluetooth/bluetooth.c b/lib/pbio/drv/bluetooth/bluetooth.c index e8bd67075..7ba897973 100644 --- a/lib/pbio/drv/bluetooth/bluetooth.c +++ b/lib/pbio/drv/bluetooth/bluetooth.c @@ -225,6 +225,7 @@ pbio_error_t pbdrv_bluetooth_peripheral_disconnect(void) { // Pass silently for already disconnected. if (!pbdrv_bluetooth_is_connected(PBDRV_BLUETOOTH_CONNECTION_PERIPHERAL)) { + peri->err = PBIO_SUCCESS; return PBIO_SUCCESS; } @@ -345,6 +346,7 @@ pbio_error_t pbdrv_bluetooth_start_advertising(bool start) { // Already in requested state. This makes it safe to call stop advertising // even if it already stopped on becoming connected; if (start == is_advertising) { + advertising_or_scan_err = PBIO_SUCCESS; return PBIO_SUCCESS; } @@ -389,6 +391,7 @@ pbio_error_t pbdrv_bluetooth_start_broadcasting(const uint8_t *data, size_t size if (!is_broadcasting) { // Already stopped. + advertising_or_scan_err = PBIO_SUCCESS; return PBIO_SUCCESS; } advertising_or_scan_err = PBIO_ERROR_AGAIN; @@ -400,6 +403,7 @@ pbio_error_t pbdrv_bluetooth_start_broadcasting(const uint8_t *data, size_t size // Avoid I/O operations if the user tries to broadcast the same data // over and over in a tight loop. if (is_broadcasting && pbdrv_bluetooth_broadcast_data_size == size && !memcmp(pbdrv_bluetooth_broadcast_data, data, size)) { + advertising_or_scan_err = PBIO_SUCCESS; return PBIO_SUCCESS; } pbdrv_bluetooth_broadcast_data_size = size; @@ -431,6 +435,7 @@ pbio_error_t pbdrv_bluetooth_start_observing(pbdrv_bluetooth_start_observing_cal bool should_observe = callback ? true : false; if (should_observe == pbdrv_bluetooth_is_observing) { + advertising_or_scan_err = PBIO_SUCCESS; return PBIO_SUCCESS; }