diff --git a/nimble/host/include/host/ble_ead.h b/nimble/host/include/host/ble_ead.h index 38cf15037..8d6b60821 100644 --- a/nimble/host/include/host/ble_ead.h +++ b/nimble/host/include/host/ble_ead.h @@ -121,8 +121,6 @@ int ble_ead_decrypt(const uint8_t session_key[BLE_EAD_KEY_SIZE], const uint8_t iv[BLE_EAD_IV_SIZE], const uint8_t *encrypted_payload, size_t encrypted_payload_size, uint8_t *payload); -int ble_ead_serialize_data(const struct enc_adv_data *input, uint8_t *output); - #endif /* ENC_ADV_DATA */ #ifdef __cplusplus diff --git a/nimble/host/include/host/ble_hs_adv.h b/nimble/host/include/host/ble_hs_adv.h index 8d5e26a39..f05ec1e12 100644 --- a/nimble/host/include/host/ble_hs_adv.h +++ b/nimble/host/include/host/ble_hs_adv.h @@ -22,6 +22,7 @@ #include #include "host/ble_uuid.h" +#include "syscfg/syscfg.h" #ifdef __cplusplus extern "C" { @@ -32,6 +33,8 @@ extern "C" { /** Max field payload size (account for 2-byte header). */ #define BLE_HS_ADV_MAX_FIELD_SZ (BLE_HS_ADV_MAX_SZ - 2) +#define BLE_HS_ADV_LE_SUPP_FEAT_LEN 6 + struct ble_hs_adv_field { uint8_t length; uint8_t type; @@ -113,7 +116,7 @@ struct ble_hs_adv_fields { unsigned device_addr_type; unsigned device_addr_is_present:1; - /*** 0xF1 - 32-bit service solicitation UUIDs */ + /*** 0x1f - 32-bit service solicitation UUIDs */ const ble_uuid32_t *sol_uuids32; uint8_t sol_num_uuids32; @@ -133,6 +136,21 @@ struct ble_hs_adv_fields { const uint8_t *uri; uint8_t uri_len; + /*** 0x27 - LE Supported Features. */ + /*** Core Spec v5.4 Vol. 6 Part B 4.6 */ + uint8_t le_supp_feat[BLE_HS_ADV_LE_SUPP_FEAT_LEN]; + unsigned le_supp_feat_is_present; + + /*** 0x2f - Advertising interval - long. */ + uint32_t adv_itvl_long; + unsigned adv_itvl_long_is_present:1; + +#if MYNEWT_VAL(ENC_ADV_DATA) + /*** 0x31 - Encrypted Advertising Data. */ + const uint8_t *enc_adv_data; + uint8_t enc_adv_data_len; +#endif + /*** 0xff - Manufacturer specific data. */ const uint8_t *mfg_data; uint8_t mfg_data_len; @@ -164,9 +182,12 @@ struct ble_hs_adv_fields { #define BLE_HS_ADV_TYPE_SVC_DATA_UUID32 0x20 #define BLE_HS_ADV_TYPE_SVC_DATA_UUID128 0x21 #define BLE_HS_ADV_TYPE_URI 0x24 +#define BLE_HS_ADV_TYPE_LE_SUPP_FEAT 0x27 #define BLE_HS_ADV_TYPE_MESH_PROV 0x29 #define BLE_HS_ADV_TYPE_MESH_MESSAGE 0x2a #define BLE_HS_ADV_TYPE_MESH_BEACON 0x2b +#define BLE_HS_ADV_TYPE_ADV_ITVL_LONG 0x2f +#define BLE_HS_ADV_TYPE_ENC_ADV_DATA 0x31 #define BLE_HS_ADV_TYPE_MFG_DATA 0xff #define BLE_HS_ADV_FLAGS_LEN 1 @@ -198,6 +219,8 @@ struct ble_hs_adv_fields { #define BLE_HS_ADV_ADDR_TYPE_LEN 1 +#define BLE_HS_ADV_ADV_ITVL_LONG_LEN 4 + int ble_hs_adv_set_fields_mbuf(const struct ble_hs_adv_fields *adv_fields, struct os_mbuf *om); diff --git a/nimble/host/services/gap/src/ble_svc_gap.c b/nimble/host/services/gap/src/ble_svc_gap.c index 5950e53d3..5f5679145 100644 --- a/nimble/host/services/gap/src/ble_svc_gap.c +++ b/nimble/host/services/gap/src/ble_svc_gap.c @@ -42,6 +42,7 @@ static char ble_svc_gap_name[BLE_SVC_GAP_NAME_MAX_LEN + 1] = static uint16_t ble_svc_gap_appearance = MYNEWT_VAL(BLE_SVC_GAP_APPEARANCE); #if MYNEWT_VAL(ENC_ADV_DATA) +static uint16_t ble_svc_gap_enc_adv_data_handle; static struct key_material km = { .session_key = {0}, .iv = {0}, @@ -97,7 +98,8 @@ static const struct ble_gatt_svc_def ble_svc_gap_defs[] = { #if MYNEWT_VAL(ENC_ADV_DATA) .uuid = BLE_UUID16_DECLARE(BLE_SVC_GAP_CHR_UUID16_KEY_MATERIAL), .access_cb = ble_svc_gap_access, - .flags = BLE_GATT_CHR_F_READ, + .val_handle = &ble_svc_gap_enc_adv_data_handle, + .flags = BLE_GATT_CHR_F_READ | BLE_GATT_CHR_F_READ_ENC | BLE_GATT_CHR_F_READ_AUTHEN | BLE_GATT_CHR_F_READ_AUTHOR | BLE_GATT_CHR_F_INDICATE, }, { #endif #if MYNEWT_VAL(BLE_SVC_GAP_GATT_SECURITY_LEVEL) @@ -345,6 +347,7 @@ ble_svc_gap_device_key_material_set(uint8_t *session_key, uint8_t *iv) { memcpy(&km.session_key, session_key, BLE_EAD_KEY_SIZE); memcpy(&km.iv, iv, BLE_EAD_IV_SIZE); + ble_gatts_chr_updated(ble_svc_gap_enc_adv_data_handle); return 0; } #endif diff --git a/nimble/host/src/ble_aes_ccm.c b/nimble/host/src/ble_aes_ccm.c index 285db47e9..7af098302 100644 --- a/nimble/host/src/ble_aes_ccm.c +++ b/nimble/host/src/ble_aes_ccm.c @@ -265,14 +265,20 @@ int ble_aes_ccm_decrypt(const uint8_t key[16], uint8_t nonce[13], const uint8_t uint8_t *out_msg, size_t mic_size) { uint8_t mic[16]; + uint8_t key_reversed[16]; if (aad_len >= 0xff00 || mic_size > sizeof(mic)) { return BLE_HS_EINVAL; } - ble_aes_ccm_crypt(key, nonce, enc_msg, out_msg, msg_len); + /** Setting the correct endian-ness of the key */ + for (int i = 0; i < 16; i++) { + key_reversed[i] = key[15 - i]; + } - ble_aes_ccm_auth(key, nonce, out_msg, msg_len, aad, aad_len, mic, mic_size); + ble_aes_ccm_crypt(key_reversed, nonce, enc_msg, out_msg, msg_len); + + ble_aes_ccm_auth(key_reversed, nonce, out_msg, msg_len, aad, aad_len, mic, mic_size); /*if (memcmp(mic, enc_msg + msg_len, mic_size)) { printf("\n%s return here", __func__); @@ -286,16 +292,25 @@ int ble_aes_ccm_encrypt(const uint8_t key[16], uint8_t nonce[13], const uint8_t size_t msg_len, const uint8_t *aad, size_t aad_len, uint8_t *out_msg, size_t mic_size) { + /** MIC starts after encrypted message and is part of encrypted advertisement data */ uint8_t *mic = out_msg + msg_len; + uint8_t key_reversed[16]; /* Unsupported AAD size */ if (aad_len >= 0xff00 || mic_size > 16) { return BLE_HS_EINVAL; } - ble_aes_ccm_auth(key, nonce, out_msg, msg_len, aad, aad_len, mic, mic_size); + /* Correcting the endian-ness of the key */ + for (int i = 0; i < 16; i++) { + key_reversed[i] = key[15 - i]; + } - ble_aes_ccm_crypt(key, nonce, msg, out_msg, msg_len); + /** Calculating MIC */ + ble_aes_ccm_auth(key_reversed, nonce, msg, msg_len, aad, aad_len, mic, mic_size); + + /** Encrypting advertisment */ + ble_aes_ccm_crypt(key_reversed, nonce, msg, out_msg, msg_len); return 0; } diff --git a/nimble/host/src/ble_ead.c b/nimble/host/src/ble_ead.c index 8f437fa8f..be497b525 100644 --- a/nimble/host/src/ble_ead.c +++ b/nimble/host/src/ble_ead.c @@ -68,15 +68,18 @@ static int ead_encrypt(const uint8_t session_key[BLE_EAD_KEY_SIZE], const uint8_ int err; uint8_t nonce[BLE_EAD_NONCE_SIZE]; + /** Nonce is concatenation of Randomizer and IV */ err = ble_ead_generate_nonce(iv, randomizer, nonce); if (err != 0) { return -1; } + /** Copying Randomizer to the start of encrypted advertisment data */ memcpy(encrypted_payload, nonce, BLE_EAD_RANDOMIZER_SIZE); err = ble_aes_ccm_encrypt(session_key, nonce, payload, payload_size, ble_ead_aad, BLE_EAD_AAD_SIZE, &encrypted_payload[BLE_EAD_RANDOMIZER_SIZE], BLE_EAD_MIC_SIZE); + if (err != 0) { BLE_HS_LOG(DEBUG, "Failed to encrypt the payload (ble_ccm_encrypt err %d)", err); return -1; @@ -178,30 +181,6 @@ int ble_ead_decrypt(const uint8_t session_key[BLE_EAD_KEY_SIZE], const uint8_t i return ead_decrypt(session_key, iv, encrypted_payload, encrypted_payload_size, payload); } -int ble_ead_serialize_data(const struct enc_adv_data *input, uint8_t *output) -{ - if ( input == NULL) { - BLE_HS_LOG(DEBUG, "input is NULL"); - return 0; - } - - if ( output == NULL) { - BLE_HS_LOG(DEBUG, "output is NULL"); - return 0; - } - - uint8_t adv_data_len = input->len; - uint8_t data_len = adv_data_len + 1; - - output[0] = data_len; - output[1] = input->type; - - memcpy(&output[2], input->data, adv_data_len); - - return data_len + 1; - -} - #endif /* ENC_ADV_DATA */ #ifdef __cplusplus diff --git a/nimble/host/src/ble_hs_adv.c b/nimble/host/src/ble_hs_adv.c index aef917f79..0403c479d 100644 --- a/nimble/host/src/ble_hs_adv.c +++ b/nimble/host/src/ble_hs_adv.c @@ -22,6 +22,9 @@ #include "nimble/ble.h" #include "host/ble_hs_adv.h" #include "ble_hs_priv.h" +#if MYNEWT_VAL(ENC_ADV_DATA) +#include "host/ble_ead.h" +#endif struct find_field_data { uint8_t type; @@ -526,6 +529,33 @@ adv_set_fields(const struct ble_hs_adv_fields *adv_fields, } } + /*** 0x27 - LE Supported Features. */ + if (adv_fields->le_supp_feat_is_present) { + rc = ble_hs_adv_set_flat_mbuf(BLE_HS_ADV_TYPE_LE_SUPP_FEAT, + BLE_HS_ADV_LE_SUPP_FEAT_LEN, + adv_fields->le_supp_feat, dst, &dst_len_local, + max_len, om); + } + + /*** 0x2f - Advertising interval - long. */ + if (adv_fields->adv_itvl_long_is_present && adv_fields->adv_itvl_long > 0xFFFF) { + rc = ble_hs_adv_set_flat_mbuf(BLE_HS_ADV_TYPE_ADV_ITVL_LONG, + BLE_HS_ADV_ADV_ITVL_LONG_LEN, + &adv_fields->adv_itvl_long, dst, &dst_len_local, + max_len, om); + } + +#if MYNEWT_VAL(ENC_ADV_DATA) + /*** 0x31 - Encrypted Advertising Data. */ + if ((adv_fields->enc_adv_data != NULL) && + (adv_fields->enc_adv_data_len > BLE_EAD_RANDOMIZER_SIZE + BLE_EAD_MIC_SIZE)) { + rc = ble_hs_adv_set_flat_mbuf(BLE_HS_ADV_TYPE_ENC_ADV_DATA, + adv_fields->enc_adv_data_len, + adv_fields->enc_adv_data, dst, &dst_len_local, + max_len, om); + } +#endif + /*** 0xff - Manufacturer specific data. */ if ((adv_fields->mfg_data != NULL) && (adv_fields->mfg_data_len >= 2)) { rc = ble_hs_adv_set_flat_mbuf(BLE_HS_ADV_TYPE_MFG_DATA, @@ -862,6 +892,13 @@ ble_hs_adv_parse_one_field(struct ble_hs_adv_fields *adv_fields, } break; +#if MYNEWT_VAL(ENC_ADV_DATA) + case BLE_HS_ADV_TYPE_ENC_ADV_DATA: + adv_fields->enc_adv_data = data; + adv_fields->enc_adv_data_len = data_len; + break; +#endif + case BLE_HS_ADV_TYPE_MFG_DATA: adv_fields->mfg_data = data; adv_fields->mfg_data_len = data_len; diff --git a/nimble/host/store/config/src/ble_store_nvs.c b/nimble/host/store/config/src/ble_store_nvs.c index 8ae391e4c..00339c29c 100644 --- a/nimble/host/store/config/src/ble_store_nvs.c +++ b/nimble/host/store/config/src/ble_store_nvs.c @@ -66,7 +66,7 @@ get_nvs_key_string(int obj_type, int index, char *key_string) } else if (obj_type == BLE_STORE_OBJ_TYPE_OUR_SEC) { sprintf(key_string, "%s_%d", NIMBLE_NVS_OUR_SEC_KEY, index); #if MYNEWT_VAL(ENC_ADV_DATA) - } else if (obj_type == NIMBLE_NVS_EAD_SEC_KEY) { + } else if (obj_type == BLE_STORE_OBJ_TYPE_ENC_ADV_DATA) { sprintf(key_string, "%s_%d", NIMBLE_NVS_EAD_SEC_KEY, index); #endif } else if (obj_type == BLE_STORE_OBJ_TYPE_LOCAL_IRK) { @@ -111,7 +111,7 @@ get_nvs_max_obj_value(int obj_type) if (obj_type == BLE_STORE_OBJ_TYPE_CCCD) { return MYNEWT_VAL(BLE_STORE_MAX_CCCDS); #if MYNEWT_VAL(ENC_ADV_DATA) - } else if (obj_type == BLE_STORE_OBJ_TYPE_EAD) { + } else if (obj_type == BLE_STORE_OBJ_TYPE_ENC_ADV_DATA) { return MYNEWT_VAL(BLE_STORE_MAX_EADS); #endif } else { @@ -177,7 +177,7 @@ get_nvs_db_value(int obj_type, char *key_string, union ble_store_value *val) err = nvs_get_blob(nimble_handle, key_string, &val->cccd, &required_size); #if MYNEWT_VAL(ENC_ADV_DATA) - } else if (obj_type == BLE_STORE_OBJ_TYPE_EAD) { + } else if (obj_type == BLE_STORE_OBJ_TYPE_ENC_ADV_DATA) { err = nvs_get_blob(nimble_handle, key_string, &val->ead, &required_size); #endif @@ -256,7 +256,7 @@ get_nvs_db_attribute(int obj_type, bool empty, void *value, int num_value) err = get_nvs_matching_index(&cur.sec, value, num_value, sizeof(struct ble_store_value_cccd)); #if MYNEWT_VAL(ENC_ADV_DATA) - } else if (obj_type == BLE_STORE_OBJ_TYPE_EAD) { + } else if (obj_type == BLE_STORE_OBJ_TYPE_ENC_ADV_DATA) { err = get_nvs_matching_index(&cur.sec, value, num_value, sizeof(struct ble_store_value_ead)); #endif @@ -393,7 +393,7 @@ ble_store_nvs_write(int obj_type, const union ble_store_value *val) return ble_nvs_write_key_value(key_string, &val->cccd, sizeof(struct ble_store_value_cccd)); #if MYNEWT_VAL(ENC_ADV_DATA) - } else if (obj_type == BLE_STORE_OBJ_TYPE_EAD) { + } else if (obj_type == BLE_STORE_OBJ_TYPE_ENC_ADV_DATA) { return ble_nvs_write_key_value(key_string, &val->ead, sizeof(struct ble_store_value_ead)); #endif @@ -490,7 +490,7 @@ populate_db_from_nvs(int obj_type, void *dst, int *db_num) db_item += sizeof(struct ble_store_value_cccd); (*db_num)++; #if MYNEWT_VAL(ENC_ADV_DATA) - } if (obj_type == BLE_STORE_OBJ_TYPE_EAD) { + } if (obj_type == BLE_STORE_OBJ_TYPE_ENC_ADV_DATA) { ESP_LOGD(TAG, "EAD in RAM is filled up from NVS index = %d", i); memcpy(db_item, &cur.ead, sizeof(struct ble_store_value_ead)); db_item += sizeof(struct ble_store_value_ead); @@ -582,7 +582,7 @@ ble_nvs_restore_sec_keys(void) #endif #if MYNEWT_VAL(ENC_ADV_DATA) - err = populate_db_from_nvs(BLE_STORE_OBJ_TYPE_EAD, ble_store_config_eads, + err = populate_db_from_nvs(BLE_STORE_OBJ_TYPE_ENC_ADV_DATA, ble_store_config_eads, &ble_store_config_num_eads); if (err != ESP_OK) { ESP_LOGE(TAG, "NVS operation failed for 'EAD'"); @@ -693,7 +693,7 @@ int ble_store_config_persist_eads(void) return BLE_HS_ESTORE_FAIL; } ESP_LOGD(TAG, "Deleting EAD, nvs idx = %d", nvs_idx); - return ble_nvs_delete_value(BLE_STORE_OBJ_TYPE_EAD, nvs_idx); + return ble_nvs_delete_value(BLE_STORE_OBJ_TYPE_ENC_ADV_DATA, nvs_idx); } return 0; }