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 9a1c23f46..7ba897973 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,15 +195,19 @@ 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; + // 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)); // 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(); @@ -221,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; } @@ -312,6 +317,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; } @@ -340,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; } @@ -384,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; @@ -395,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; @@ -426,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; } @@ -646,6 +656,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 dc4444340..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,22 @@ 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; - -// REVISIT: Most of these states should go into pbdrv_bluetooth_peripheral_t -typedef struct { +#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; -} pup_handset_t; +} peripheral_platform_state; +#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 +105,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 @@ -156,6 +139,54 @@ 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; +} + +/** + * 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; @@ -302,58 +333,16 @@ 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, &handset.current_char); - } + case GATT_EVENT_CHARACTERISTIC_QUERY_RESULT: + DEBUG_PRINT("GATT_EVENT_CHARACTERISTIC_QUERY_RESULT\n"); break; - } case GATT_EVENT_CHARACTERISTIC_VALUE_QUERY_RESULT: { - 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); - } + DEBUG_PRINT("GATT_EVENT_CHARACTERISTIC_VALUE_QUERY_RESULT\n"); break; } case GATT_EVENT_QUERY_COMPLETE: - if (handset.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) { - - // 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; - break; - } - - handset.btstack_error = gatt_client_write_client_characteristic_configuration( - packet_handler, peri->con_handle, &handset.current_char, - GATT_CLIENT_CHARACTERISTICS_CONFIGURATION_NOTIFICATION); - if (handset.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; - } 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; - } - } else if (handset.con_state == CON_STATE_WAIT_ENABLE_NOTIFICATIONS) { - // Done enabling notifications. - handset.con_state = CON_STATE_DISCOVERY_AND_NOTIFICATIONS_COMPLETE; - } + DEBUG_PRINT("GATT_EVENT_QUERY_COMPLETE\n"); break; - case GATT_EVENT_NOTIFICATION: { if (gatt_event_notification_get_handle(packet) != peri->con_handle) { break; @@ -365,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) { @@ -379,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 (handset.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); - handset.con_state = CON_STATE_WAIT_BONDING; - } else { - handset.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) { - gatt_client_stop_listening_for_characteristic_value_updates(&handset.notification); + DEBUG_PRINT("Peripheral disconnected\n"); + 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; } - 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 (handset.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); - handset.con_state = CON_STATE_WAIT_SCAN_RSP; - } - } else if (handset.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. - handset.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(); - handset.btstack_error = gap_connect(peri->bdaddr, peri->bdaddr_type); - - if (handset.btstack_error == ERROR_CODE_SUCCESS) { - handset.con_state = CON_STATE_WAIT_CONNECT; - } else { - handset.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; } @@ -500,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"); @@ -533,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"); - handset.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 @@ -698,17 +586,11 @@ 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; - - // 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) { @@ -717,7 +599,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,105 +607,352 @@ 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; - 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) { - gap_stop_scan(); - } else if (handset.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; - 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 (handset.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; } - handset.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) { 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 (handset.con_state != CON_STATE_CONNECTED) { - return PBIO_ERROR_FAILED; - } + // Nothing found to begin with. + peri->char_now->handle = 0; - handset.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 ? + + // 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); + } + + 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, ({ + + if (!pbdrv_bluetooth_peripheral_is_connected(peri)) { + // Got disconnected while waiting. + 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; + } + } - if (handset.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; + // 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); + + DEBUG_PRINT("Waiting for notifications to be enabled.\n"); 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) { - return PBIO_ERROR_FAILED; + if (!pbdrv_bluetooth_peripheral_is_connected(peri)) { + // Got disconnected while waiting. + return PBIO_ERROR_NO_DEV; } - handset.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. - handset.con_state = CON_STATE_CONNECTED; + // 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); - 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 (handset.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 = { .value_handle = peri->char_now->handle, }; - handset.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; - } 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; + 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 (handset.con_state == CON_STATE_NONE) { - return PBIO_ERROR_FAILED; + 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 && + 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); } - handset.con_state == CON_STATE_READ_CHARACTERISTIC_COMPLETE; + + // 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. - handset.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); } @@ -832,32 +961,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 PBIO_ERROR_FAILED; + 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 + if (!pbdrv_bluetooth_peripheral_is_connected(peri)) { + // Got disconnected while waiting. 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) { @@ -866,10 +1002,8 @@ 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); - } - + // 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); @@ -903,7 +1037,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 } @@ -1130,6 +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 = &peripheral_platform_state; 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. */ 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_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, 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);