From f40c73b4e72dd1cf4241c39610371faedf3f28f5 Mon Sep 17 00:00:00 2001 From: Matthias Ringwald Date: Tue, 18 Jan 2022 15:55:28 +0100 Subject: [PATCH] hci: manage periodic advertiser list on controller --- src/hci.c | 168 +++++++++++++++++++++++++++++++++++++++++++++++++++++- src/hci.h | 28 +++++++++ 2 files changed, 194 insertions(+), 2 deletions(-) diff --git a/src/hci.c b/src/hci.c index 461ba85bc..00414fa1f 100644 --- a/src/hci.c +++ b/src/hci.c @@ -215,6 +215,7 @@ static bool hci_run_general_gap_le(void); #endif #ifdef ENABLE_LE_PERIPHERAL #ifdef ENABLE_LE_EXTENDED_ADVERTISING +static void hci_periodic_advertiser_list_free(void); static le_advertising_set_t * hci_advertising_set_for_handle(uint8_t advertising_handle); #endif /* ENABLE_LE_EXTENDED_ADVERTISING */ #endif /* ENABLE_LE_PERIPHERAL */ @@ -4832,6 +4833,8 @@ uint8_t hci_le_extended_advertising_operation_for_chunk(uint16_t pos, uint16_t l static bool hci_run_general_gap_le(void){ + btstack_linked_list_iterator_t lit; + // Phase 1: collect what to stop bool scanning_stop = false; @@ -4858,7 +4861,6 @@ static bool hci_run_general_gap_le(void){ // check if whitelist needs modification bool whitelist_modification_pending = false; - btstack_linked_list_iterator_t lit; btstack_linked_list_iterator_init(&lit, &hci_stack->le_whitelist); while (btstack_linked_list_iterator_has_next(&lit)){ whitelist_entry_t * entry = (whitelist_entry_t*) btstack_linked_list_iterator_next(&lit); @@ -4867,16 +4869,31 @@ static bool hci_run_general_gap_le(void){ break; } } + // check if resolving list needs modification bool resolving_list_modification_pending = false; #ifdef ENABLE_LE_PRIVACY_ADDRESS_RESOLUTION - bool resolving_list_supported = hci_command_supported(SUPPORTED_HCI_COMMAND_LE_SET_ADDRESS_RESOLUTION_ENABLE); if (resolving_list_supported && hci_stack->le_resolving_list_state != LE_RESOLVING_LIST_DONE){ resolving_list_modification_pending = true; } #endif + // check if periodic advertiser list needs modification +#ifdef ENABLE_LE_CENTRAL +#ifdef ENABLE_LE_EXTENDED_ADVERTISING + bool periodic_list_modification_pending = false; + btstack_linked_list_iterator_init(&lit, &hci_stack->le_periodic_advertiser_list); + while (btstack_linked_list_iterator_has_next(&lit)){ + periodic_advertiser_list_entry_t * entry = (periodic_advertiser_list_entry_t*) btstack_linked_list_iterator_next(&lit); + if (entry->state & (LE_PERIODIC_ADVERTISER_LIST_ENTRY_ADD_TO_CONTROLLER | LE_PERIODIC_ADVERTISER_LIST_ENTRY_REMOVE_FROM_CONTROLLER)){ + periodic_list_modification_pending = true; + break; + } + } +#endif +#endif + #ifdef ENABLE_LE_CENTRAL // scanning control if (hci_stack->le_scanning_active) { @@ -5397,6 +5414,34 @@ static bool hci_run_general_gap_le(void){ hci_stack->le_resolving_list_state = LE_RESOLVING_LIST_DONE; #endif +#ifdef ENABLE_LE_CENTRAL +#ifdef ENABLE_LE_EXTENDED_ADVERTISING + // LE Whitelist Management + if (periodic_list_modification_pending){ + // add/remove entries + btstack_linked_list_iterator_init(&lit, &hci_stack->le_periodic_advertiser_list); + while (btstack_linked_list_iterator_has_next(&lit)){ + periodic_advertiser_list_entry_t * entry = (periodic_advertiser_list_entry_t*) btstack_linked_list_iterator_next(&lit); + if (entry->state & LE_PERIODIC_ADVERTISER_LIST_ENTRY_REMOVE_FROM_CONTROLLER){ + entry->state &= ~LE_PERIODIC_ADVERTISER_LIST_ENTRY_REMOVE_FROM_CONTROLLER; + hci_send_cmd(&hci_le_remove_device_from_periodic_advertiser_list, entry->address_type, entry->address); + return true; + } + if (entry->state & LE_PERIODIC_ADVERTISER_LIST_ENTRY_ADD_TO_CONTROLLER){ + entry->state &= ~LE_PERIODIC_ADVERTISER_LIST_ENTRY_ADD_TO_CONTROLLER; + entry->state |= LE_PERIODIC_ADVERTISER_LIST_ENTRY_ON_CONTROLLER; + hci_send_cmd(&hci_le_add_device_to_periodic_advertiser_list, entry->address_type, entry->address); + return true; + } + if ((entry->state & LE_PERIODIC_ADVERTISER_LIST_ENTRY_ON_CONTROLLER) == 0){ + btstack_linked_list_remove(&hci_stack->le_periodic_advertiser_list, (btstack_linked_item_t *) entry); + btstack_memory_periodic_advertiser_list_entry_free(entry); + } + } + } +#endif +#endif + // post-pone all actions until stack is fully working if (hci_stack->state != HCI_STATE_WORKING) return false; @@ -7904,6 +7949,125 @@ uint8_t gap_load_resolving_list_from_le_device_db(void){ } #endif +#ifdef ENABLE_BLE +#ifdef ENABLE_LE_CENTRAL +#ifdef ENABLE_LE_EXTENDED_ADVERTISING + +static uint8_t hci_periodic_advertiser_list_add(bd_addr_type_t address_type, const bd_addr_t address, uint8_t advertising_sid){ + // check if already in list + btstack_linked_list_iterator_t it; + btstack_linked_list_iterator_init(&it, &hci_stack->le_periodic_advertiser_list); + while (btstack_linked_list_iterator_has_next(&it)) { + periodic_advertiser_list_entry_t *entry = (periodic_advertiser_list_entry_t *) btstack_linked_list_iterator_next(&it); + if (entry->sid != advertising_sid) { + continue; + } + if (entry->address_type != address_type) { + continue; + } + if (memcmp(entry->address, address, 6) != 0) { + continue; + } + // disallow if already scheduled to add + if ((entry->state & LE_PERIODIC_ADVERTISER_LIST_ENTRY_ADD_TO_CONTROLLER) != 0){ + return ERROR_CODE_COMMAND_DISALLOWED; + } + // still on controller, but scheduled to remove -> re-add + entry->state |= LE_PERIODIC_ADVERTISER_LIST_ENTRY_ADD_TO_CONTROLLER; + return ERROR_CODE_SUCCESS; + } + // alloc and add to list + periodic_advertiser_list_entry_t * entry = btstack_memory_periodic_advertiser_list_entry_get(); + if (!entry) return BTSTACK_MEMORY_ALLOC_FAILED; + entry->sid = advertising_sid; + entry->address_type = address_type; + (void)memcpy(entry->address, address, 6); + entry->state = LE_PERIODIC_ADVERTISER_LIST_ENTRY_ADD_TO_CONTROLLER; + btstack_linked_list_add(&hci_stack->le_periodic_advertiser_list, (btstack_linked_item_t*) entry); + return ERROR_CODE_SUCCESS; +} + +static uint8_t hci_periodic_advertiser_list_remove(bd_addr_type_t address_type, const bd_addr_t address, uint8_t advertising_sid){ + btstack_linked_list_iterator_t it; + btstack_linked_list_iterator_init(&it, &hci_stack->le_periodic_advertiser_list); + while (btstack_linked_list_iterator_has_next(&it)){ + periodic_advertiser_list_entry_t * entry = (periodic_advertiser_list_entry_t*) btstack_linked_list_iterator_next(&it); + if (entry->sid != advertising_sid) { + continue; + } + if (entry->address_type != address_type) { + continue; + } + if (memcmp(entry->address, address, 6) != 0) { + continue; + } + if (entry->state & LE_PERIODIC_ADVERTISER_LIST_ENTRY_ON_CONTROLLER){ + // remove from controller if already present + entry->state |= LE_PERIODIC_ADVERTISER_LIST_ENTRY_REMOVE_FROM_CONTROLLER; + } else { + // directly remove entry from whitelist + btstack_linked_list_iterator_remove(&it); + btstack_memory_periodic_advertiser_list_entry_free(entry); + } + return ERROR_CODE_SUCCESS; + } + return ERROR_CODE_UNKNOWN_CONNECTION_IDENTIFIER; +} + +static void hci_periodic_advertiser_list_clear(void){ + btstack_linked_list_iterator_t it; + btstack_linked_list_iterator_init(&it, &hci_stack->le_periodic_advertiser_list); + while (btstack_linked_list_iterator_has_next(&it)){ + periodic_advertiser_list_entry_t * entry = (periodic_advertiser_list_entry_t*) btstack_linked_list_iterator_next(&it); + if (entry->state & LE_PERIODIC_ADVERTISER_LIST_ENTRY_ON_CONTROLLER){ + // remove from controller if already present + entry->state |= LE_PERIODIC_ADVERTISER_LIST_ENTRY_REMOVE_FROM_CONTROLLER; + continue; + } + // directly remove entry from whitelist + btstack_linked_list_iterator_remove(&it); + btstack_memory_periodic_advertiser_list_entry_free(entry); + } +} + +// free all entries unconditionally +static void hci_periodic_advertiser_list_free(void){ + btstack_linked_list_iterator_t lit; + btstack_linked_list_iterator_init(&lit, &hci_stack->le_periodic_advertiser_list); + while (btstack_linked_list_iterator_has_next(&lit)){ + periodic_advertiser_list_entry_t * entry = (periodic_advertiser_list_entry_t*) btstack_linked_list_iterator_next(&lit); + btstack_linked_list_remove(&hci_stack->le_periodic_advertiser_list, (btstack_linked_item_t *) entry); + btstack_memory_periodic_advertiser_list_entry_free(entry); + } +} + +uint8_t gap_periodic_advertiser_list_clear(void){ + hci_periodic_advertiser_list_clear(); + hci_run(); + return ERROR_CODE_SUCCESS; +} + +uint8_t gap_periodic_advertiser_list_add(bd_addr_type_t address_type, const bd_addr_t address, uint8_t advertising_sid){ + uint8_t status = hci_periodic_advertiser_list_add(address_type, address, advertising_sid); + if (status){ + return status; + } + hci_run(); + return ERROR_CODE_SUCCESS; +} + +uint8_t gap_periodic_advertiser_list_remove(bd_addr_type_t address_type, const bd_addr_t address, uint8_t advertising_sid){ + uint8_t status = hci_periodic_advertiser_list_remove(address_type, address, advertising_sid); + if (status){ + return status; + } + hci_run(); + return ERROR_CODE_SUCCESS; +} +#endif +#endif +#endif + #ifdef FUZZING_BUILD_MODE_UNSAFE_FOR_PRODUCTION void hci_setup_test_connections_fuzz(void){ hci_connection_t * conn; diff --git a/src/hci.h b/src/hci.h index 505332626..74fd85ac8 100644 --- a/src/hci.h +++ b/src/hci.h @@ -1064,6 +1064,10 @@ typedef struct { uint16_t le_connection_scan_window; uint8_t le_connection_own_addr_type; bd_addr_t le_connection_own_address; + +#ifdef ENABLE_LE_EXTENDED_ADVERTISING + btstack_linked_list_t le_periodic_advertiser_list; +#endif #endif le_connection_parameter_range_t le_connection_parameter_range; @@ -1484,6 +1488,30 @@ void hci_remove_le_device_db_entry_from_resolving_list(uint16_t le_device_db_ind */ uint16_t hci_number_free_acl_slots_for_connection_type(bd_addr_type_t address_type); +/** + * @brief Clear Periodic Advertiser List + * @return status + */ +uint8_t gap_periodic_advertiser_list_clear(void); + +/** + * @brief Add entry to Periodic Advertiser List + * @param address_type + * @param address + * @param advertising_sid + * @return + */ +uint8_t gap_periodic_advertiser_list_add(bd_addr_type_t address_type, const bd_addr_t address, uint8_t advertising_sid); + +/** + * Remove entry from Periodic Advertising List + * @param address_type + * @param address + * @param advertising_sid + * @return + */ +uint8_t gap_periodic_advertiser_list_remove(bd_addr_type_t address_type, const bd_addr_t address, uint8_t advertising_sid); + /** * @brief Get Manufactured * @return manufacturer id