From b99e5ed88d9b796c267103adbf0a244efb142b40 Mon Sep 17 00:00:00 2001 From: Sumeet Singh Date: Wed, 25 Mar 2026 22:53:27 +0530 Subject: [PATCH] fix(nimble): Fix enc adv NVS struct and add ext adv support --- .../enc_adv_data_cent/main/main.c | 131 ++++++++++++++---- .../enc_adv_data_prph/main/main.c | 95 ++++++++++++- 2 files changed, 201 insertions(+), 25 deletions(-) diff --git a/examples/bluetooth/nimble/ble_enc_adv_data/enc_adv_data_cent/main/main.c b/examples/bluetooth/nimble/ble_enc_adv_data/enc_adv_data_cent/main/main.c index a31f144e12..e315ae0292 100644 --- a/examples/bluetooth/nimble/ble_enc_adv_data/enc_adv_data_cent/main/main.c +++ b/examples/bluetooth/nimble/ble_enc_adv_data/enc_adv_data_cent/main/main.c @@ -101,31 +101,22 @@ enc_adv_data_cent_on_read(uint16_t conn_handle, value_ead.km_present = 1; - value_ead.km = (struct key_material *) malloc (sizeof(struct key_material)); - - if (value_ead.km == NULL) { - MODLOG_DFLT(ERROR, "Failed to allocate memory for key material"); - goto err; - } - - memset(value_ead.km, 0, sizeof(struct key_material)); + memset(&value_ead.km, 0, sizeof(struct key_material)); /* Validate mbuf has enough data before copying */ if (attr->om == NULL || OS_MBUF_PKTLEN(attr->om) < (BLE_EAD_KEY_SIZE + BLE_EAD_IV_SIZE)) { MODLOG_DFLT(ERROR, "Invalid mbuf or insufficient data size"); - free(value_ead.km); - value_ead.km = NULL; goto err; } - os_mbuf_copydata(attr->om, 0, BLE_EAD_KEY_SIZE, &value_ead.km->session_key); - os_mbuf_copydata(attr->om, BLE_EAD_KEY_SIZE, BLE_EAD_IV_SIZE, &value_ead.km->iv); + os_mbuf_copydata(attr->om, 0, BLE_EAD_KEY_SIZE, &value_ead.km.session_key); + os_mbuf_copydata(attr->om, BLE_EAD_KEY_SIZE, BLE_EAD_IV_SIZE, &value_ead.km.iv); MODLOG_DFLT(DEBUG, "Session key:"); - print_bytes(value_ead.km->session_key, BLE_EAD_KEY_SIZE); + print_bytes(value_ead.km.session_key, BLE_EAD_KEY_SIZE); MODLOG_DFLT(DEBUG, "IV:"); - print_bytes(value_ead.km->iv, BLE_EAD_IV_SIZE); + print_bytes(value_ead.km.iv, BLE_EAD_IV_SIZE); memcpy(&value_ead.peer_addr.val, &p->peer_addr, PEER_ADDR_VAL_SIZE); @@ -134,17 +125,8 @@ enc_adv_data_cent_on_read(uint16_t conn_handle, MODLOG_DFLT(INFO, "Writing of session key, iv, and peer addr to NVS success"); } - if (value_ead.km != NULL) { - free(value_ead.km); - value_ead.km = NULL; - } - err: /* Terminate the connection. */ - if (value_ead.km != NULL) { - free(value_ead.km); - value_ead.km = NULL; - } return ble_gap_terminate(conn_handle, BLE_ERR_REM_USER_CONN_TERM); } @@ -296,8 +278,9 @@ enc_adv_data_cent_decrypt(uint8_t length_data, const uint8_t *data, const uint8_ MODLOG_DFLT(INFO, "Read session key and iv from NVS successfully"); } - rc = ble_ead_decrypt(value_ead.km->session_key, value_ead.km->iv, enc_data, + rc = ble_ead_decrypt(value_ead.km.session_key, value_ead.km.iv, enc_data, enc_payload_len, temp); + if (rc == 0) { MODLOG_DFLT(INFO, "Decryption of adv data done successfully"); } else { @@ -329,6 +312,93 @@ enc_adv_data_cent_decrypt(uint8_t length_data, const uint8_t *data, const uint8_ * advertisement. The function returns a positive result if the device * advertises connectability and support for the Key Characteristic service. */ +#if MYNEWT_VAL(BLE_EXT_ADV) +static int +enc_adv_data_cent_ext_should_connect(const struct ble_gap_ext_disc_desc *disc) +{ + int offset = 0; + int ad_struct_len = 0; +#if CONFIG_EXAMPLE_USE_CI_ADDRESS + uint32_t *addr_offset; +#endif // CONFIG_EXAMPLE_USE_CI_ADDRESS + uint8_t test_addr[6]; + uint32_t peer_addr[6]; + + memset(peer_addr, 0x0, sizeof peer_addr); + + if (disc->legacy_event_type != BLE_HCI_ADV_RPT_EVTYPE_ADV_IND && + disc->legacy_event_type != BLE_HCI_ADV_RPT_EVTYPE_DIR_IND) { + return 0; + } + if (strlen(CONFIG_EXAMPLE_PEER_ADDR) && (strncmp(CONFIG_EXAMPLE_PEER_ADDR, "ADDR_ANY", strlen ("ADDR_ANY")) != 0)) { +#if !CONFIG_EXAMPLE_USE_CI_ADDRESS + ESP_LOGI(tag, "Peer address from menuconfig: %s", CONFIG_EXAMPLE_PEER_ADDR); + /* Convert string to address */ + sscanf(CONFIG_EXAMPLE_PEER_ADDR, "%lx:%lx:%lx:%lx:%lx:%lx", + &peer_addr[5], &peer_addr[4], &peer_addr[3], + &peer_addr[2], &peer_addr[1], &peer_addr[0]); +#endif + + /* Conversion */ + for(int i=0; i<6; i++) { + test_addr[i] = (uint8_t )peer_addr[i]; + } + +#if CONFIG_EXAMPLE_USE_CI_ADDRESS + addr_offset = (uint32_t *)&test_addr[1]; + *addr_offset = atoi(CONFIG_EXAMPLE_PEER_ADDR); + test_addr[5] = 0xC3; + test_addr[0] = TEST_CI_ADDRESS_CHIP_OFFSET; +#endif + if (memcmp(test_addr, disc->addr.val, sizeof(disc->addr.val)) != 0) { + return 0; + } + } + + /* The device has to advertise support for the Key Characteristic + * service (0x2B88) + * + * Check if custom UUID 0x2C01 is advertised + */ + while(offset < disc->length_data) { + ad_struct_len = disc->data[offset]; + + if (ad_struct_len == 0 || offset + ad_struct_len + 1 > disc->length_data) { + break; + } + + /* Search if Custom UUID 0x2C01 is advertised */ + if (ad_struct_len >= 3 && (disc->data[offset + 1] == 0x02 || disc->data[offset + 1] == 0x03)) { + for (int i = 2; i + 1 <= ad_struct_len; i+= 2) { + if (disc->data[offset + i] == 0x2C && disc->data[offset + i + 1] == 0x01) { + if (enc_adv_data_find_peer(disc->addr.val) != -1) { + MODLOG_DFLT(INFO, "Peer was already added with addr : %s", + addr_str(&disc->addr.val)); + } else { + MODLOG_DFLT(INFO, "Adding peer addr : %s", addr_str(&disc->addr.val)); + + memcpy(&kmp[counter].peer_addr, &disc->addr.val, PEER_ADDR_VAL_SIZE); + counter++; + + if (counter > CONFIG_BT_NIMBLE_MAX_CONNECTIONS) { + counter = 0; + } + } + if (enc_adv_data_check_km_exist(disc->addr.val)) { + return enc_adv_data_cent_decrypt(disc->length_data, disc->data, disc->addr.val); + } else { + return 1; + } + } + } + } + + offset += ad_struct_len + 1; + } + + return 0; +} +#else static int enc_adv_data_cent_should_connect(const struct ble_gap_disc_desc *disc) { @@ -397,6 +467,7 @@ enc_adv_data_cent_should_connect(const struct ble_gap_disc_desc *disc) return 0; } +#endif /** * Connects to the sender of the specified advertisement of it looks @@ -411,9 +482,15 @@ enc_adv_data_cent_connect_if_interesting(void *disc) ble_addr_t *addr; /* Don't do anything if we don't care about this advertiser. */ +#if MYNEWT_VAL(BLE_EXT_ADV) + if(!enc_adv_data_cent_ext_should_connect((struct ble_gap_ext_disc_desc *) disc)) { + return; + } +#else if (!enc_adv_data_cent_should_connect((struct ble_gap_disc_desc *)disc)) { return; } +#endif #if !(MYNEWT_VAL(BLE_HOST_ALLOW_CONNECT_WITH_SCAN)) /* Scanning must be stopped before a connection can be initiated. */ @@ -434,7 +511,11 @@ enc_adv_data_cent_connect_if_interesting(void *disc) /* Try to connect the the advertiser. Allow 30 seconds (30000 ms) for * timeout. */ +#if MYNEWT_VAL(BLE_EXT_ADV) + addr = &((struct ble_gap_ext_disc_desc *)disc)->addr; +#else addr = &((struct ble_gap_disc_desc *)disc)->addr; +#endif rc = ble_gap_connect(own_addr_type, addr, 30000, NULL, enc_adv_data_cent_gap_event, NULL); @@ -598,6 +679,8 @@ enc_adv_data_cent_gap_event(struct ble_gap_event *event, void *arg) case BLE_GAP_EVENT_EXT_DISC: /* An advertisement report was received during GAP discovery. */ ext_print_adv_report(&event->ext_disc); + + enc_adv_data_cent_connect_if_interesting(&event->ext_disc); return 0; #endif diff --git a/examples/bluetooth/nimble/ble_enc_adv_data/enc_adv_data_prph/main/main.c b/examples/bluetooth/nimble/ble_enc_adv_data/enc_adv_data_prph/main/main.c index 1d162ef869..e28495ee06 100644 --- a/examples/bluetooth/nimble/ble_enc_adv_data/enc_adv_data_prph/main/main.c +++ b/examples/bluetooth/nimble/ble_enc_adv_data/enc_adv_data_prph/main/main.c @@ -1,5 +1,5 @@ /* - * SPDX-FileCopyrightText: 2021-2025 Espressif Systems (Shanghai) CO LTD + * SPDX-FileCopyrightText: 2021-2026 Espressif Systems (Shanghai) CO LTD * * SPDX-License-Identifier: Unlicense OR CC0-1.0 */ @@ -21,6 +21,14 @@ static const char *tag = "ENC_ADV_DATA_PRPH"; static int enc_adv_data_prph_gap_event(struct ble_gap_event *event, void *arg); const uint8_t device_name[3] = {'k', 'e', 'y'}; +#if MYNEWT_VAL(BLE_EXT_ADV) +static uint8_t ext_adv_pattern[] = { + 0x02, BLE_HS_ADV_TYPE_FLAGS, 0x06, + 0x03, BLE_HS_ADV_TYPE_COMP_UUIDS16, 0xab, 0xcd, + 0x03, BLE_HS_ADV_TYPE_COMP_UUIDS16, 0x2C, 0x01, +}; +#endif + static uint8_t unencrypted_adv_pattern[] = { 0x05, 0X09, 'p', 'r', 'p', 'h' }; @@ -102,6 +110,78 @@ enc_adv_data_prph_encrypt_set(uint8_t *out_encrypted_adv_data, return 0; } +#if MYNEWT_VAL(BLE_EXT_ADV) +/** + * Enables advertising with the following parameters: + * o General discoverable mode. + * o Undirected connectable mode. + */ +static void +enc_adv_data_prph_ext_advertise(void) +{ + struct ble_gap_ext_adv_params params; + struct os_mbuf *data; + uint8_t * temp; + uint8_t instance = 0; + int rc; + + const unsigned encrypted_adv_data_len = BLE_EAD_ENCRYPTED_PAYLOAD_SIZE(sizeof(unencrypted_adv_pattern)); + uint8_t encrypted_adv_data[encrypted_adv_data_len]; + memset(encrypted_adv_data, 0, encrypted_adv_data_len); + + /* First check if any instance is already active */ + if(ble_gap_ext_adv_active(instance)) { + return; + } + + /* use defaults for non-set params */ + memset (¶ms, 0, sizeof(params)); + + /* enable connectable advertising */ + params.connectable = 1; + + /* advertise using random addr */ + params.own_addr_type = BLE_OWN_ADDR_PUBLIC; + + params.primary_phy = BLE_HCI_LE_PHY_1M; + params.secondary_phy = BLE_HCI_LE_PHY_2M; + params.tx_power = 127; + params.sid = 1; + + params.itvl_min = BLE_GAP_ADV_FAST_INTERVAL1_MIN; + params.itvl_max = BLE_GAP_ADV_FAST_INTERVAL1_MIN; + + /* configure instance 0 */ + rc = ble_gap_ext_adv_configure(instance, ¶ms, NULL, + enc_adv_data_prph_gap_event, NULL); + assert (rc == 0); + + rc = enc_adv_data_prph_encrypt_set(encrypted_adv_data, encrypted_adv_data_len); + if (rc != 0) { + return; + } + + /* get mbuf with adv data */ + temp = malloc(sizeof(ext_adv_pattern) + 2 + encrypted_adv_data_len); + memcpy(temp, ext_adv_pattern, sizeof(ext_adv_pattern)); + temp[sizeof(ext_adv_pattern)] = 1 + encrypted_adv_data_len; + temp[sizeof(ext_adv_pattern) + 1] = BLE_GAP_ENC_ADV_DATA; + memcpy(temp + sizeof(ext_adv_pattern) + 2, encrypted_adv_data, encrypted_adv_data_len); + + data = ble_hs_mbuf_from_flat(temp, sizeof(ext_adv_pattern) + 2 + encrypted_adv_data_len); + assert(data); + + free(temp); + temp = NULL; + + rc = ble_gap_ext_adv_set_data(instance, data); + assert (rc == 0); + + /* start advertising */ + rc = ble_gap_ext_adv_start(instance, 0, 0); + assert (rc == 0); +} +#else /** * Enables advertising with the following parameters: * o General discoverable mode. @@ -165,6 +245,7 @@ enc_adv_data_prph_advertise(void) ¶ms, enc_adv_data_prph_gap_event, NULL); assert (rc == 0); } +#endif /** * The nimble host executes this callback when a GAP event occurs. The @@ -202,7 +283,11 @@ enc_adv_data_prph_gap_event(struct ble_gap_event *event, void *arg) if (event->connect.status != 0) { /* Connection failed; resume advertising. */ +#if MYNEWT_VAL(BLE_EXT_ADV) + enc_adv_data_prph_ext_advertise(); +#else enc_adv_data_prph_advertise(); +#endif } return 0; @@ -213,7 +298,11 @@ enc_adv_data_prph_gap_event(struct ble_gap_event *event, void *arg) MODLOG_DFLT(INFO, "\n"); /* Connection terminated; resume advertising. */ +#if MYNEWT_VAL(BLE_EXT_ADV) + enc_adv_data_prph_ext_advertise(); +#else enc_adv_data_prph_advertise(); +#endif return 0; case BLE_GAP_EVENT_CONN_UPDATE: @@ -356,7 +445,11 @@ enc_adv_data_prph_on_sync(void) MODLOG_DFLT(INFO, "\n"); /* Begin advertising. */ +#if MYNEWT_VAL(BLE_EXT_ADV) + enc_adv_data_prph_ext_advertise(); +#else enc_adv_data_prph_advertise(); +#endif } void enc_adv_data_prph_host_task(void *param)