blackbeard420/esp-idf
1
0
Fork 0
mirror of https://github.com/espressif/esp-idf synced 2025-03-09 17:19:09 -04:00
Code Issues Projects Releases Wiki Activity

fix(nimble): Fixed encrypted advertisement data example

Browse Source
This commit is contained in:
Sumeet Singh 2024-07-02 17:59:10 +05:30
parent c9337d120b
commit c240d55f6c
6 changed files with 178 additions and 125 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
components/bt/host/nimble/Kconfig.in
View File

@ -740,7 +740,6 @@ config BT_NIMBLE_VS_SUPPORT
config BT_NIMBLE_ENC_ADV_DATA
bool "Encrypted Advertising Data"
depends on BT_NIMBLE_50_FEATURE_SUPPORT
select BT_NIMBLE_EXT_ADV
help
This option is used to enable encrypted advertising data.

125
examples/bluetooth/nimble/ble_enc_adv_data/enc_adv_data_cent/main/main.c
View File

@ -261,8 +261,11 @@ enc_adv_data_cent_decrypt(uint8_t length_data, const uint8_t *data, const uint8_
dec_data_len = temp[0];
MODLOG_DFLT(DEBUG, "Data after decryption:");
print_bytes(temp, dec_data_len);
MODLOG_DFLT(INFO, "Data after decryption:");
for (int i = 0; i < dec_data_len + 1; i++) {
MODLOG_DFLT(INFO, "0x%02X ", temp[i]);
}
MODLOG_DFLT(INFO, "\n");
return 1;
default:
@ -279,13 +282,19 @@ enc_adv_data_cent_decrypt(uint8_t length_data, const uint8_t *data, const uint8_
* advertises connectability and support for the Key Characteristic service.
*/
static int
ext_enc_adv_data_cent_should_connect(const struct ble_gap_ext_disc_desc *disc)
enc_adv_data_cent_should_connect(const struct ble_gap_disc_desc *disc)
{
int offset = 0;
int ad_struct_len = 0;
struct ble_hs_adv_fields fields;
int rc;
int i;
if (disc->legacy_event_type != BLE_HCI_ADV_RPT_EVTYPE_ADV_IND &&
disc->legacy_event_type != BLE_HCI_ADV_RPT_EVTYPE_DIR_IND) {
if (disc->event_type != BLE_HCI_ADV_RPT_EVTYPE_ADV_IND &&
disc->event_type != BLE_HCI_ADV_RPT_EVTYPE_DIR_IND) {
return 0;
}
rc = ble_hs_adv_parse_fields(&fields, disc->data, disc->length_data);
if (rc != 0) {
return 0;
}
@ -302,41 +311,31 @@ ext_enc_adv_data_cent_should_connect(const struct ble_gap_ext_disc_desc *disc)
/* The device has to advertise support for the Key Characteristic
* service (0x2B88)
*
* Check if custom UUID 0x2C01 is advertised
*/
do {
ad_struct_len = disc->data[offset];
for (i = 0; i < fields.num_uuids16; i++) {
if (ble_uuid_u16(&fields.uuids16[i].u) == 0x2C01) {
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));
if (!ad_struct_len) {
break;
}
memcpy(&kmp[counter].peer_addr, &disc->addr.val, PEER_ADDR_VAL_SIZE);
counter++;
/* Search if custom service UUID (0x2C01) is advertised */
if (disc->data[offset] == 0x03 && disc->data[offset + 1] == 0x03) {
if ( disc->data[offset + 2] == 0x2C && disc->data[offset + 3] == 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;
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;
} while ( offset < disc->length_data );
}
return 0;
}
@ -354,7 +353,7 @@ 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 (!ext_enc_adv_data_cent_should_connect((struct ble_gap_ext_disc_desc *)disc)) {
if (!enc_adv_data_cent_should_connect((struct ble_gap_disc_desc *)disc)) {
return;
}
@ -377,7 +376,7 @@ enc_adv_data_cent_connect_if_interesting(void *disc)
/* Try to connect the the advertiser. Allow 30 seconds (30000 ms) for
* timeout.
*/
addr = &((struct ble_gap_ext_disc_desc *)disc)->addr;
addr = &((struct ble_gap_disc_desc *)disc)->addr;
rc = ble_gap_connect(own_addr_type, addr, 30000, NULL,
enc_adv_data_cent_gap_event, NULL);
@ -459,13 +458,16 @@ enc_adv_data_cent_gap_event(struct ble_gap_event *event, void *arg)
return 0;
}
/* Perform service discovery */
rc = peer_disc_all(event->connect.conn_handle,
enc_adv_data_cent_on_disc_complete, NULL);
/** Authorization is required for this characterisitc */
rc = ble_gap_security_initiate(event->connect.conn_handle);
if (rc != 0) {
MODLOG_DFLT(ERROR, "Failed to discover services; rc=%d\n", rc);
return 0;
MODLOG_DFLT(INFO, "Security could not be initiated, rc = %d\n", rc);
return ble_gap_terminate(event->connect.conn_handle,
BLE_ERR_REM_USER_CONN_TERM);
} else {
MODLOG_DFLT(INFO, "Connection secured\n");
}
} else {
/* Connection attempt failed; resume scanning. */
MODLOG_DFLT(ERROR, "Error: Connection failed; status=%d\n",
@ -493,6 +495,21 @@ enc_adv_data_cent_gap_event(struct ble_gap_event *event, void *arg)
event->disc_complete.reason);
return 0;
case BLE_GAP_EVENT_ENC_CHANGE:
MODLOG_DFLT(INFO, "encryption change event; status=%d ",
event->enc_change.status);
rc = ble_gap_conn_find(event->enc_change.conn_handle, &desc);
assert(rc == 0);
print_conn_desc(&desc);
/* Perform service discovery */
rc = peer_disc_all(event->enc_change.conn_handle,
enc_adv_data_cent_on_disc_complete, NULL);
if (rc != 0) {
MODLOG_DFLT(ERROR, "Failed to discover services; rc=%d\n", rc);
}
return 0;
case BLE_GAP_EVENT_NOTIFY_RX:
/* Peer sent us a notification or indication. */
MODLOG_DFLT(INFO, "received %s; conn_handle=%d attr_handle=%d "
@ -515,11 +532,25 @@ enc_adv_data_cent_gap_event(struct ble_gap_event *event, void *arg)
event->mtu.value);
return 0;
#if MYNEWT_VAL(BLE_EXT_ADV)
case BLE_GAP_EVENT_EXT_DISC:
/* An advertisement report was received during GAP discovery. */
ext_print_adv_report(&event->disc);
return 0;
#endif
case BLE_GAP_EVENT_PASSKEY_ACTION:
ESP_LOGI(tag, "PASSKEY_ACTION_EVENT started %d", event->passkey.params.action);
struct ble_sm_io pkey = {0};
if (event->passkey.params.action == BLE_SM_IOACT_INPUT) {
pkey.action = event->passkey.params.action;
pkey.passkey = 123456;
ESP_LOGI(tag, "Entering passkey %" PRIu32, pkey.passkey);
rc = ble_sm_inject_io(event->passkey.conn_handle, &pkey);
ESP_LOGI(tag, "ble_sm_inject_io result: %d", rc);
}
enc_adv_data_cent_connect_if_interesting(&event->disc);
return 0;
default:
@ -577,12 +608,16 @@ app_main(void)
ble_hs_cfg.sync_cb = enc_adv_data_cent_on_sync;
ble_hs_cfg.store_status_cb = ble_store_util_status_rr;
/** This feature requires authentication */
ble_hs_cfg.sm_mitm = 1;
ble_hs_cfg.sm_io_cap = BLE_SM_IO_CAP_KEYBOARD_ONLY;
/* Initialize data structures to track connected peers. */
rc = peer_init(MYNEWT_VAL(BLE_MAX_CONNECTIONS), 64, 64, 64);
assert(rc == 0);
/* Set the default device name. */
rc = ble_svc_gap_device_name_set("nimble-enc_adv_data_cent");
rc = ble_svc_gap_device_name_set("enc_adv_data_cent");
assert(rc == 0);
/* XXX Need to have template for store */

2
examples/bluetooth/nimble/ble_enc_adv_data/enc_adv_data_cent/sdkconfig.defaults
View File

@ -10,5 +10,5 @@ CONFIG_BTDM_CTRL_MODE_BR_EDR_ONLY=n
CONFIG_BTDM_CTRL_MODE_BTDM=n
CONFIG_BT_BLUEDROID_ENABLED=n
CONFIG_BT_NIMBLE_ENABLED=y
CONFIG_BT_NIMBLE_EXT_ADV=y
CONFIG_BT_NIMBLE_EXT_ADV=n
CONFIG_BT_NIMBLE_50_FEATURE_SUPPORT=y

2
examples/bluetooth/nimble/ble_enc_adv_data/enc_adv_data_prph/main/Kconfig.projbuild
View File

@ -2,7 +2,7 @@ menu "Example Configuration"
choice EXAMPLE_USE_IO_TYPE
prompt "I/O Capability"
default BLE_SM_IO_CAP_NO_IO
default BLE_SM_IO_CAP_DISP_ONLY
help
I/O capability of device.

171
examples/bluetooth/nimble/ble_enc_adv_data/enc_adv_data_prph/main/main.c
View File

@ -16,27 +16,21 @@
#include "enc_adv_data_prph.h"
#if CONFIG_EXAMPLE_ENC_ADV_DATA
static uint8_t km_adv_pattern_1[] = {
0x02, 0x01, 0x06,
0x03, 0x03, 0x2C, 0x01,
0x04, 0X09, 'k', 'e', 'y',
};
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'};
static uint8_t ext_adv_pattern_1[] = {
0x02, 0x01, 0x06,
0x03, 0x03, 0x2C, 0x00,
0x05, 0X09, 'p', 'r', 'p', 'h',
static uint8_t unencrypted_adv_pattern[] = {
0x05, 0X09, 'p', 'r', 'p', 'h'
};
struct key_material km = {
.session_key = {
0xC0, 0xC1, 0xC2, 0xC3, 0xC4, 0xC5, 0xC6, 0xC7, 0xC8, 0xC9, 0xCA, 0xCB,
0xCC, 0xCD, 0xCE, 0xCF
0x19, 0x6a, 0x0a, 0xd1, 0x2a, 0x61, 0x20, 0x1e,
0x13, 0x6e, 0x2e, 0xd1, 0x12, 0xda, 0xa9, 0x57
},
.iv = {0xFB, 0x56, 0xE1, 0xDA, 0xDC, 0x7E, 0xAD, 0xF5},
.iv = {0x9E, 0x7a, 0x00, 0xef, 0xb1, 0x7a, 0xe7, 0x46},
};
#if CONFIG_EXAMPLE_RANDOM_ADDR
@ -74,31 +68,24 @@ enc_adv_data_prph_print_conn_desc(struct ble_gap_conn_desc *desc)
desc->sec_state.bonded);
}
static const struct enc_adv_data ead[] = {
ENC_ADV_DATA(BLE_GAP_ENC_ADV_DATA, ext_adv_pattern_1, sizeof(ext_adv_pattern_1)),
};
static void enc_adv_data_prph_encrypt_set(uint8_t instance, struct os_mbuf *data)
static void
enc_adv_data_prph_encrypt_set(uint8_t * out_encrypted_adv_data,
const unsigned encrypted_adv_data_len)
{
int rc;
uint8_t enc_data_flag = BLE_GAP_ENC_ADV_DATA; //0x31
const unsigned unencrypted_adv_data_len = sizeof(unencrypted_adv_pattern);
uint8_t ext_adv_pattern_sz = ead[0].len;
uint8_t unencrypted_adv_data[unencrypted_adv_data_len];
uint8_t encrypted_adv_data[encrypted_adv_data_len];
size_t adv_data_sz = BLE_GAP_DATA_SERIALIZED_SIZE(ext_adv_pattern_sz);
uint8_t adv_data[adv_data_sz];
memcpy(unencrypted_adv_data, unencrypted_adv_pattern, sizeof(unencrypted_adv_pattern));
size_t enc_adv_data_sz = BLE_EAD_ENCRYPTED_PAYLOAD_SIZE(adv_data_sz);
uint8_t enc_adv_data[enc_adv_data_sz];
MODLOG_DFLT(INFO, "Data before encryption:");
print_bytes(unencrypted_adv_data, unencrypted_adv_data_len);
MODLOG_DFLT(INFO, "\n");
ble_ead_serialize_data(&ead[0], adv_data);
MODLOG_DFLT(DEBUG, "Data before encryption:");
print_bytes(adv_data, adv_data_sz);
MODLOG_DFLT(DEBUG, "\n");
rc = ble_ead_encrypt(km.session_key, km.iv, adv_data, adv_data_sz, enc_adv_data);
rc = ble_ead_encrypt(km.session_key, km.iv, unencrypted_adv_data, unencrypted_adv_data_len, encrypted_adv_data);
if (rc == 0) {
MODLOG_DFLT(INFO, "Encryption of adv data done successfully");
} else {
@ -106,20 +93,12 @@ static void enc_adv_data_prph_encrypt_set(uint8_t instance, struct os_mbuf *data
return;
}
MODLOG_DFLT(DEBUG, "Data after encryption:");
print_bytes(enc_adv_data, enc_adv_data_sz);
MODLOG_DFLT(DEBUG, "\n");
MODLOG_DFLT(INFO, "Data after encryption:");
print_bytes(encrypted_adv_data, encrypted_adv_data_len);
MODLOG_DFLT(INFO, "\n");
//Copying encrypted data
rc = os_mbuf_append(data, &enc_adv_data_sz, sizeof(uint8_t));
rc = os_mbuf_append(data, &enc_data_flag, sizeof(uint8_t));
rc = os_mbuf_append(data, enc_adv_data, enc_adv_data_sz);
assert(rc == 0);
MODLOG_DFLT(INFO, "Advertising data:");
print_mbuf(data);
/** Contains Randomiser ## Encrypted Advertising Data ## MIC */
memcpy(out_encrypted_adv_data, encrypted_adv_data, encrypted_adv_data_len);
}
/**
@ -128,57 +107,59 @@ static void enc_adv_data_prph_encrypt_set(uint8_t instance, struct os_mbuf *data
* o Undirected connectable mode.
*/
static void
ext_enc_adv_data_prph_advertise(void)
enc_adv_data_prph_advertise(void)
{
struct ble_gap_ext_adv_params params;
uint8_t instance = 0;
struct ble_gap_adv_params params;
struct ble_hs_adv_fields fields;
uint8_t own_addr_type;
int rc;
struct os_mbuf *data;
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)) {
if (ble_gap_adv_active()) {
return;
}
/* use defaults for non-set params */
memset (&params, 0, sizeof(params));
memset (&fields, 0, sizeof(fields));
own_addr_type = BLE_OWN_ADDR_PUBLIC;
/* 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.conn_mode = BLE_GAP_CONN_MODE_UND;
params.disc_mode = BLE_GAP_DISC_MODE_GEN;
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, &params, NULL,
enc_adv_data_prph_gap_event, NULL);
assert (rc == 0);
fields.flags = BLE_HS_ADV_F_DISC_GEN | BLE_HS_ADV_F_BREDR_UNSUP;
/* in this case only scan response is allowed */
/* get mbuf for scan rsp data */
data = os_msys_get_pkthdr(sizeof(km_adv_pattern_1), 0);
assert(data);
fields.name = device_name;
fields.name_len = 3;
fields.name_is_complete = 1;
rc = os_mbuf_append(data, km_adv_pattern_1, sizeof(km_adv_pattern_1));
assert(rc == 0);
fields.uuids16 = (ble_uuid16_t[]) {
BLE_UUID16_INIT(0x2C01) /** For the central to recognise this device */
};
fields.num_uuids16 = 1;
fields.uuids16_is_complete = 1;
//Encrypted advertising data
enc_adv_data_prph_encrypt_set(instance, data);
/** Getting the encrypted advertising data */
enc_adv_data_prph_encrypt_set(encrypted_adv_data, encrypted_adv_data_len);
rc = ble_gap_ext_adv_set_data(instance, data);
fields.enc_adv_data = encrypted_adv_data;
fields.enc_adv_data_len = encrypted_adv_data_len;
rc = ble_gap_adv_set_fields(&fields);
assert (rc == 0);
/* start advertising */
rc = ble_gap_ext_adv_start(instance, 0, 0);
rc = ble_gap_adv_start(own_addr_type, NULL, BLE_HS_FOREVER,
&params, enc_adv_data_prph_gap_event, NULL);
assert (rc == 0);
}
@ -218,7 +199,7 @@ enc_adv_data_prph_gap_event(struct ble_gap_event *event, void *arg)
if (event->connect.status != 0) {
/* Connection failed; resume advertising. */
ext_enc_adv_data_prph_advertise();
enc_adv_data_prph_advertise();
}
return 0;
@ -229,7 +210,7 @@ enc_adv_data_prph_gap_event(struct ble_gap_event *event, void *arg)
MODLOG_DFLT(INFO, "\n");
/* Connection terminated; resume advertising. */
ext_enc_adv_data_prph_advertise();
enc_adv_data_prph_advertise();
return 0;
case BLE_GAP_EVENT_CONN_UPDATE:
@ -247,6 +228,30 @@ enc_adv_data_prph_gap_event(struct ble_gap_event *event, void *arg)
event->adv_complete.reason);
return 0;
case BLE_GAP_EVENT_ENC_CHANGE:
MODLOG_DFLT(INFO, "encryption change event; status=%d ",
event->enc_change.status);
rc = ble_gap_conn_find(event->enc_change.conn_handle, &desc);
assert(rc == 0);
enc_adv_data_prph_print_conn_desc(&desc);
MODLOG_DFLT(INFO, "\n");
return 0;
case BLE_GAP_EVENT_PASSKEY_ACTION:
ESP_LOGI(tag, "PASSKEY_ACTION_EVENT started");
struct ble_sm_io pkey = {0};
/** For now only BLE_SM_IOACT_DISP is handled */
if (event->passkey.params.action == BLE_SM_IOACT_DISP) {
pkey.action = event->passkey.params.action;
pkey.passkey = 123456;
ESP_LOGI(tag, "Enter passkey %" PRIu32 " on the peer side", pkey.passkey);
rc = ble_sm_inject_io(event->passkey.conn_handle, &pkey);
ESP_LOGI(tag, "ble_sm_inject_io result: %d", rc);
}
return 0;
case BLE_GAP_EVENT_NOTIFY_TX:
MODLOG_DFLT(INFO, "notify_tx event; conn_handle=%d attr_handle=%d "
"status=%d is_indication=%d",
@ -274,6 +279,15 @@ enc_adv_data_prph_gap_event(struct ble_gap_event *event, void *arg)
event->mtu.channel_id,
event->mtu.value);
return 0;
case BLE_GAP_EVENT_AUTHORIZE:
MODLOG_DFLT(INFO, "authorization event; conn_handle=%d attr_handle=%d is_read=%d",
event->authorize.conn_handle,
event->authorize.attr_handle,
event->authorize.is_read);
/** Accept all authorization requests for now */
event->authorize.out_response = BLE_GAP_AUTHORIZE_ACCEPT;
return 0;
}
return 0;
@ -337,7 +351,7 @@ enc_adv_data_prph_on_sync(void)
MODLOG_DFLT(INFO, "\n");
/* Begin advertising. */
ext_enc_adv_data_prph_advertise();
enc_adv_data_prph_advertise();
}
void enc_adv_data_prph_host_task(void *param)
@ -373,17 +387,21 @@ app_main(void)
ble_hs_cfg.gatts_register_cb = gatt_svr_register_cb;
ble_hs_cfg.store_status_cb = ble_store_util_status_rr;
ble_hs_cfg.sm_io_cap = CONFIG_EXAMPLE_IO_TYPE;
#ifdef CONFIG_EXAMPLE_BONDING
#if CONFIG_EXAMPLE_BONDING
ble_hs_cfg.sm_bonding = 1;
/* Enable the appropriate bit masks to make sure the keys
* that are needed are exchanged
*/
ble_hs_cfg.sm_our_key_dist |= BLE_SM_PAIR_KEY_DIST_ENC;
ble_hs_cfg.sm_their_key_dist |= BLE_SM_PAIR_KEY_DIST_ENC;
#else
ble_hs_cfg.sm_bonding = 0;
#endif
/** This feature requires authentication */
ble_hs_cfg.sm_mitm = 1;
ble_hs_cfg.sm_io_cap = CONFIG_EXAMPLE_IO_TYPE;
#ifdef CONFIG_EXAMPLE_USE_SC
ble_hs_cfg.sm_sc = 1;
#else
@ -403,6 +421,7 @@ app_main(void)
assert(rc == 0);
/* Set the session key and initialization vector */
rc = ble_svc_gap_device_key_material_set(km.session_key, km.iv);
assert(rc == 0);

2
examples/bluetooth/nimble/ble_enc_adv_data/enc_adv_data_prph/sdkconfig.defaults
View File

@ -11,4 +11,4 @@ CONFIG_BTDM_CTRL_MODE_BTDM=n
CONFIG_BT_BLUEDROID_ENABLED=n
CONFIG_BT_NIMBLE_ENABLED=y
CONFIG_BT_NIMBLE_HCI_EVT_BUF_SIZE=70
CONFIG_BT_NIMBLE_EXT_ADV=y
CONFIG_BT_NIMBLE_EXT_ADV=n