123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599600601602603604605606607608609610611612613614615616617618619620621622623624625626627628629630631632633634635636637638639640641642643644645646647648649650651652653654655656657658659660661662663664665666667668669670671672673674675676677678679680681682683684685686687688689690691692693694695696697698699700701702703704705706707708709710711712713714715716717718719720721722723724725726727728729730731732733734735736737738739740741742743744745746747748749750751752753754755756757758 |
- #include "sdk_common.h"
- #if NRF_MODULE_ENABLED(PEER_MANAGER)
- #include "id_manager.h"
- #include <string.h>
- #include "ble.h"
- #include "ble_gap.h"
- #include "ble_err.h"
- #include "peer_manager_types.h"
- #include "peer_database.h"
- #include "peer_data_storage.h"
- #include "nrf_soc.h"
- #include "ble_conn_state.h"
- #define NRF_LOG_MODULE_NAME peer_manager_im
- #if PM_LOG_ENABLED
- #define NRF_LOG_LEVEL PM_LOG_LEVEL
- #define NRF_LOG_INFO_COLOR PM_LOG_INFO_COLOR
- #define NRF_LOG_DEBUG_COLOR PM_LOG_DEBUG_COLOR
- #else
- #define NRF_LOG_LEVEL 0
- #endif
- #include "nrf_log.h"
- #include "nrf_log_ctrl.h"
- NRF_LOG_MODULE_REGISTER();
- #define IM_MAX_CONN_HANDLES (20)
- #define IM_NO_INVALID_CONN_HANDLES (0xFF)
- #define IM_ADDR_CLEARTEXT_LENGTH (3)
- #define IM_ADDR_CIPHERTEXT_LENGTH (3)
- #define IM_EVENT_HANDLERS_CNT (sizeof(m_evt_handlers) / sizeof(m_evt_handlers[0]))
- extern void pm_im_evt_handler(pm_evt_t * p_event);
- extern void gcm_im_evt_handler(pm_evt_t * p_event);
- static pm_evt_handler_internal_t const m_evt_handlers[] =
- {
- pm_im_evt_handler,
- gcm_im_evt_handler
- };
- typedef struct
- {
- pm_peer_id_t peer_id;
- ble_gap_addr_t peer_address;
- } im_connection_t;
- static im_connection_t m_connections[IM_MAX_CONN_HANDLES];
- static uint8_t m_wlisted_peer_cnt;
- static pm_peer_id_t m_wlisted_peers[BLE_GAP_WHITELIST_ADDR_MAX_COUNT];
- static void evt_send(pm_evt_t * p_event)
- {
- for (uint32_t i = 0; i < IM_EVENT_HANDLERS_CNT; i++)
- {
- m_evt_handlers[i](p_event);
- }
- }
- bool is_valid_irk(ble_gap_irk_t const * p_irk)
- {
- NRF_PM_DEBUG_CHECK(p_irk != NULL);
- for (uint32_t i = 0; i < BLE_GAP_SEC_KEY_LEN; i++)
- {
- if (p_irk->irk[i] != 0)
- {
- return true;
- }
- }
- return false;
- }
- bool addr_compare(ble_gap_addr_t const * p_addr1, ble_gap_addr_t const * p_addr2)
- {
-
- if ((p_addr1 == NULL) || (p_addr2 == NULL))
- {
- return false;
- }
-
- if (p_addr1->addr_type != p_addr2->addr_type)
- {
- return false;
- }
-
- return (memcmp(p_addr1->addr, p_addr2->addr, BLE_GAP_ADDR_LEN) == 0);
- }
- void im_ble_evt_handler(ble_evt_t const * ble_evt)
- {
- ble_gap_evt_t gap_evt;
- pm_peer_id_t bonded_matching_peer_id;
- if (ble_evt->header.evt_id != BLE_GAP_EVT_CONNECTED)
- {
-
- return;
- }
- gap_evt = ble_evt->evt.gap_evt;
- bonded_matching_peer_id = PM_PEER_ID_INVALID;
- if ( gap_evt.params.connected.peer_addr.addr_type
- != BLE_GAP_ADDR_TYPE_RANDOM_PRIVATE_NON_RESOLVABLE)
- {
-
- pm_peer_id_t peer_id;
- pm_peer_data_flash_t peer_data;
- pds_peer_data_iterate_prepare();
- switch (gap_evt.params.connected.peer_addr.addr_type)
- {
- case BLE_GAP_ADDR_TYPE_PUBLIC:
- case BLE_GAP_ADDR_TYPE_RANDOM_STATIC:
- {
- while (pds_peer_data_iterate(PM_PEER_DATA_ID_BONDING, &peer_id, &peer_data))
- {
- if (addr_compare(&gap_evt.params.connected.peer_addr,
- &peer_data.p_bonding_data->peer_ble_id.id_addr_info))
- {
- bonded_matching_peer_id = peer_id;
- break;
- }
- }
- }
- break;
- case BLE_GAP_ADDR_TYPE_RANDOM_PRIVATE_RESOLVABLE:
- {
- while (pds_peer_data_iterate(PM_PEER_DATA_ID_BONDING, &peer_id, &peer_data))
- {
- if (im_address_resolve(&gap_evt.params.connected.peer_addr,
- &peer_data.p_bonding_data->peer_ble_id.id_info))
- {
- bonded_matching_peer_id = peer_id;
- break;
- }
- }
- }
- break;
- default:
- NRF_PM_DEBUG_CHECK(false);
- break;
- }
- }
- m_connections[gap_evt.conn_handle].peer_id = bonded_matching_peer_id;
- m_connections[gap_evt.conn_handle].peer_address = gap_evt.params.connected.peer_addr;
- if (bonded_matching_peer_id != PM_PEER_ID_INVALID)
- {
-
- pm_evt_t im_evt;
- im_evt.conn_handle = gap_evt.conn_handle;
- im_evt.peer_id = bonded_matching_peer_id;
- im_evt.evt_id = PM_EVT_BONDED_PEER_CONNECTED;
- evt_send(&im_evt);
- }
- }
- bool im_is_duplicate_bonding_data(pm_peer_data_bonding_t const * p_bonding_data1,
- pm_peer_data_bonding_t const * p_bonding_data2)
- {
- NRF_PM_DEBUG_CHECK(p_bonding_data1 != NULL);
- NRF_PM_DEBUG_CHECK(p_bonding_data2 != NULL);
- ble_gap_addr_t const * p_addr1 = &p_bonding_data1->peer_ble_id.id_addr_info;
- ble_gap_addr_t const * p_addr2 = &p_bonding_data2->peer_ble_id.id_addr_info;
- bool duplicate_irk = ((memcmp(p_bonding_data1->peer_ble_id.id_info.irk,
- p_bonding_data2->peer_ble_id.id_info.irk,
- BLE_GAP_SEC_KEY_LEN) == 0)
- && is_valid_irk(&p_bonding_data1->peer_ble_id.id_info)
- && is_valid_irk(&p_bonding_data2->peer_ble_id.id_info));
- bool duplicate_addr = addr_compare(p_addr1, p_addr2);
- bool id_addrs = ((p_addr1->addr_type != BLE_GAP_ADDR_TYPE_RANDOM_PRIVATE_RESOLVABLE)
- && (p_addr1->addr_type != BLE_GAP_ADDR_TYPE_RANDOM_PRIVATE_NON_RESOLVABLE)
- && (p_addr2->addr_type != BLE_GAP_ADDR_TYPE_RANDOM_PRIVATE_RESOLVABLE)
- && (p_addr2->addr_type != BLE_GAP_ADDR_TYPE_RANDOM_PRIVATE_NON_RESOLVABLE));
- return (duplicate_addr && id_addrs) || (duplicate_irk && !id_addrs);
- }
- pm_peer_id_t im_find_duplicate_bonding_data(pm_peer_data_bonding_t const * p_bonding_data,
- pm_peer_id_t peer_id_skip)
- {
- pm_peer_id_t peer_id;
- pm_peer_data_flash_t peer_data_duplicate;
- NRF_PM_DEBUG_CHECK(p_bonding_data != NULL);
- pds_peer_data_iterate_prepare();
- while (pds_peer_data_iterate(PM_PEER_DATA_ID_BONDING, &peer_id, &peer_data_duplicate))
- {
- if ( (peer_id != peer_id_skip)
- && im_is_duplicate_bonding_data(p_bonding_data,
- peer_data_duplicate.p_bonding_data))
- {
- return peer_id;
- }
- }
- return PM_PEER_ID_INVALID;
- }
- pm_peer_id_t im_peer_id_get_by_conn_handle(uint16_t conn_handle)
- {
- if ((conn_handle >= IM_MAX_CONN_HANDLES) || !ble_conn_state_valid(conn_handle))
- {
- return PM_PEER_ID_INVALID;
- }
- return m_connections[conn_handle].peer_id;
- }
- ret_code_t im_ble_addr_get(uint16_t conn_handle, ble_gap_addr_t * p_ble_addr)
- {
- NRF_PM_DEBUG_CHECK(p_ble_addr != NULL);
- if ((conn_handle >= IM_MAX_CONN_HANDLES) || !ble_conn_state_valid(conn_handle))
- {
- return BLE_ERROR_INVALID_CONN_HANDLE;
- }
- *p_ble_addr = m_connections[conn_handle].peer_address;
- return NRF_SUCCESS;
- }
- bool im_master_ids_compare(ble_gap_master_id_t const * p_master_id1,
- ble_gap_master_id_t const * p_master_id2)
- {
- NRF_PM_DEBUG_CHECK(p_master_id1 != NULL);
- NRF_PM_DEBUG_CHECK(p_master_id2 != NULL);
- if (!im_master_id_is_valid(p_master_id1))
- {
- return false;
- }
- if (p_master_id1->ediv != p_master_id2->ediv)
- {
- return false;
- }
- return (memcmp(p_master_id1->rand, p_master_id2->rand, BLE_GAP_SEC_RAND_LEN) == 0);
- }
- pm_peer_id_t im_peer_id_get_by_master_id(ble_gap_master_id_t const * p_master_id)
- {
- pm_peer_id_t peer_id;
- pm_peer_data_flash_t peer_data;
- NRF_PM_DEBUG_CHECK(p_master_id != NULL);
- pds_peer_data_iterate_prepare();
-
- while (pds_peer_data_iterate(PM_PEER_DATA_ID_BONDING, &peer_id, &peer_data))
- {
- if (im_master_ids_compare(p_master_id, &peer_data.p_bonding_data->own_ltk.master_id) ||
- im_master_ids_compare(p_master_id, &peer_data.p_bonding_data->peer_ltk.master_id))
- {
-
- return peer_id;
- }
- }
-
- return PM_PEER_ID_INVALID;
- }
- uint16_t im_conn_handle_get(pm_peer_id_t peer_id)
- {
- if (peer_id == PM_PEER_ID_INVALID)
- {
- return BLE_CONN_HANDLE_INVALID;
- }
- for (uint16_t conn_handle = 0; conn_handle < IM_MAX_CONN_HANDLES; conn_handle++)
- {
- if ((m_connections[conn_handle].peer_id == peer_id) && ble_conn_state_valid(conn_handle))
- {
- return conn_handle;
- }
- }
- return BLE_CONN_HANDLE_INVALID;
- }
- bool im_master_id_is_valid(ble_gap_master_id_t const * p_master_id)
- {
- if (p_master_id->ediv != 0)
- {
- return true;
- }
- for (uint32_t i = 0; i < BLE_GAP_SEC_RAND_LEN; i++)
- {
- if (p_master_id->rand[i] != 0)
- {
- return true;
- }
- }
- return false;
- }
- void im_new_peer_id(uint16_t conn_handle, pm_peer_id_t peer_id)
- {
- if (conn_handle < IM_MAX_CONN_HANDLES)
- {
- m_connections[conn_handle].peer_id = peer_id;
- }
- }
- ret_code_t im_peer_free(pm_peer_id_t peer_id)
- {
- uint16_t conn_handle;
- ret_code_t ret;
- conn_handle = im_conn_handle_get(peer_id);
- ret = pdb_peer_free(peer_id);
- if (ret == NRF_SUCCESS && (conn_handle < IM_MAX_CONN_HANDLES))
- {
- m_connections[conn_handle].peer_id = PM_PEER_ID_INVALID;
- }
- return ret;
- }
- static ret_code_t peers_id_keys_get(pm_peer_id_t const * p_peers,
- uint32_t peer_cnt,
- ble_gap_addr_t * p_gap_addrs,
- uint32_t * p_addr_cnt,
- ble_gap_irk_t * p_gap_irks,
- uint32_t * p_irk_cnt)
- {
- ret_code_t ret;
- pm_peer_data_bonding_t bond_data;
- pm_peer_data_t peer_data;
- uint32_t const buf_size = sizeof(bond_data);
- bool copy_addrs = false;
- bool copy_irks = false;
- NRF_PM_DEBUG_CHECK(p_peers != NULL);
-
- NRF_PM_DEBUG_CHECK((p_gap_addrs != NULL) || (p_gap_irks != NULL));
- if ((p_gap_addrs != NULL) && (p_addr_cnt != NULL))
- {
- NRF_PM_DEBUG_CHECK((*p_addr_cnt) >= peer_cnt);
- copy_addrs = true;
- *p_addr_cnt = 0;
- }
- if ((p_gap_irks != NULL) && (p_irk_cnt != NULL))
- {
- NRF_PM_DEBUG_CHECK((*p_irk_cnt) >= peer_cnt);
- copy_irks = true;
- *p_irk_cnt = 0;
- }
- memset(&peer_data, 0x00, sizeof(peer_data));
- peer_data.p_bonding_data = &bond_data;
-
- for (uint32_t i = 0; i < peer_cnt; i++)
- {
- memset(&bond_data, 0x00, sizeof(bond_data));
-
- ret = pds_peer_data_read(p_peers[i], PM_PEER_DATA_ID_BONDING,
- &peer_data, &buf_size);
- if ((ret == NRF_ERROR_NOT_FOUND) || (ret == NRF_ERROR_INVALID_PARAM))
- {
-
- return NRF_ERROR_NOT_FOUND;
- }
- uint8_t const addr_type = bond_data.peer_ble_id.id_addr_info.addr_type;
- if ((addr_type != BLE_GAP_ADDR_TYPE_PUBLIC) &&
- (addr_type != BLE_GAP_ADDR_TYPE_RANDOM_STATIC))
- {
-
- return BLE_ERROR_GAP_INVALID_BLE_ADDR;
- }
-
- if (copy_addrs)
- {
- memcpy(&p_gap_addrs[i], &bond_data.peer_ble_id.id_addr_info, sizeof(ble_gap_addr_t));
- (*p_addr_cnt)++;
- }
-
- if (copy_irks)
- {
- memcpy(&p_gap_irks[i], bond_data.peer_ble_id.id_info.irk, BLE_GAP_SEC_KEY_LEN);
- (*p_irk_cnt)++;
- }
- }
- return NRF_SUCCESS;
- }
- ret_code_t im_device_identities_list_set(pm_peer_id_t const * p_peers,
- uint32_t peer_cnt)
- {
- ret_code_t ret;
- pm_peer_data_t peer_data;
- pm_peer_data_bonding_t bond_data;
- ble_gap_id_key_t keys[BLE_GAP_DEVICE_IDENTITIES_MAX_COUNT];
- ble_gap_id_key_t const * key_ptrs[BLE_GAP_DEVICE_IDENTITIES_MAX_COUNT];
- if (peer_cnt > BLE_GAP_DEVICE_IDENTITIES_MAX_COUNT)
- {
- return NRF_ERROR_INVALID_PARAM;
- }
- if ((p_peers == NULL) || (peer_cnt == 0))
- {
-
- return sd_ble_gap_device_identities_set(NULL, NULL, 0);
- }
- peer_data.p_bonding_data = &bond_data;
- uint32_t const buf_size = sizeof(bond_data);
- memset(keys, 0x00, sizeof(keys));
- for (uint32_t i = 0; i < BLE_GAP_DEVICE_IDENTITIES_MAX_COUNT; i++)
- {
- key_ptrs[i] = &keys[i];
- }
- for (uint32_t i = 0; i < peer_cnt; i++)
- {
- memset(&bond_data, 0x00, sizeof(bond_data));
-
- ret = pds_peer_data_read(p_peers[i], PM_PEER_DATA_ID_BONDING,
- &peer_data, &buf_size);
- if ((ret == NRF_ERROR_NOT_FOUND) || (ret == NRF_ERROR_INVALID_PARAM))
- {
- NRF_LOG_WARNING("peer id %d: Peer data could not be found in flash. Remove the peer ID "
- "from the peer list and try again.",
- p_peers[i]);
- return NRF_ERROR_NOT_FOUND;
- }
- uint8_t const addr_type = bond_data.peer_ble_id.id_addr_info.addr_type;
- if ((addr_type != BLE_GAP_ADDR_TYPE_PUBLIC) &&
- (addr_type != BLE_GAP_ADDR_TYPE_RANDOM_STATIC))
- {
- NRF_LOG_WARNING("peer id %d: The address shared by the peer during bonding cannot be "
- "whitelisted. Remove the peer ID from the peer list and try again.",
- p_peers[i]);
- return BLE_ERROR_GAP_INVALID_BLE_ADDR;
- }
-
- memcpy(&keys[i], &bond_data.peer_ble_id, sizeof(ble_gap_id_key_t));
- }
- return sd_ble_gap_device_identities_set(key_ptrs, NULL, peer_cnt);
- }
- ret_code_t im_id_addr_set(ble_gap_addr_t const * p_addr)
- {
- return sd_ble_gap_addr_set(p_addr);
- }
- ret_code_t im_id_addr_get(ble_gap_addr_t * p_addr)
- {
- NRF_PM_DEBUG_CHECK(p_addr != NULL);
- return sd_ble_gap_addr_get(p_addr);
- }
- ret_code_t im_privacy_set(pm_privacy_params_t const * p_privacy_params)
- {
- return sd_ble_gap_privacy_set(p_privacy_params);
- }
- ret_code_t im_privacy_get(pm_privacy_params_t * p_privacy_params)
- {
- return sd_ble_gap_privacy_get(p_privacy_params);
- }
- ret_code_t im_whitelist_get(ble_gap_addr_t * p_addrs,
- uint32_t * p_addr_cnt,
- ble_gap_irk_t * p_irks,
- uint32_t * p_irk_cnt)
- {
-
- NRF_PM_DEBUG_CHECK((p_addrs != NULL) || (p_irks != NULL));
- NRF_PM_DEBUG_CHECK((p_addr_cnt != NULL) || (p_irk_cnt != NULL));
- if (((p_addr_cnt != NULL) && (m_wlisted_peer_cnt > *p_addr_cnt)) ||
- ((p_irk_cnt != NULL) && (m_wlisted_peer_cnt > *p_irk_cnt)))
- {
-
- return NRF_ERROR_NO_MEM;
- }
-
-
-
- return peers_id_keys_get(m_wlisted_peers, m_wlisted_peer_cnt,
- p_addrs, p_addr_cnt,
- p_irks, p_irk_cnt);
- }
- ret_code_t im_whitelist_set(pm_peer_id_t const * p_peers,
- uint32_t peer_cnt)
- {
-
- memset(m_wlisted_peers, 0x00, sizeof(m_wlisted_peers));
- if ((p_peers == NULL) || (peer_cnt == 0))
- {
-
- m_wlisted_peer_cnt = 0;
-
-
- return sd_ble_gap_whitelist_set(NULL, 0);
- }
-
- m_wlisted_peer_cnt = peer_cnt;
- memcpy(m_wlisted_peers, p_peers, sizeof(pm_peer_id_t) * peer_cnt);
- ret_code_t ret;
- uint32_t wlist_addr_cnt = 0;
- ble_gap_addr_t const * addr_ptrs[BLE_GAP_WHITELIST_ADDR_MAX_COUNT];
- ble_gap_addr_t addrs[BLE_GAP_WHITELIST_ADDR_MAX_COUNT];
- memset(addrs, 0x00, sizeof(addrs));
-
- ret = peers_id_keys_get(p_peers, peer_cnt, addrs, &wlist_addr_cnt, NULL, NULL);
- if (ret != NRF_SUCCESS)
- {
-
-
- return ret;
- }
- for (uint32_t i = 0; i < BLE_GAP_WHITELIST_ADDR_MAX_COUNT; i++)
- {
- addr_ptrs[i] = &addrs[i];
- }
-
-
- return sd_ble_gap_whitelist_set(addr_ptrs, peer_cnt);
- }
- void ah(uint8_t const * p_k, uint8_t const * p_r, uint8_t * p_local_hash)
- {
- nrf_ecb_hal_data_t ecb_hal_data;
- for (uint32_t i = 0; i < SOC_ECB_KEY_LENGTH; i++)
- {
- ecb_hal_data.key[i] = p_k[SOC_ECB_KEY_LENGTH - 1 - i];
- }
- memset(ecb_hal_data.cleartext, 0, SOC_ECB_KEY_LENGTH - IM_ADDR_CLEARTEXT_LENGTH);
- for (uint32_t i = 0; i < IM_ADDR_CLEARTEXT_LENGTH; i++)
- {
- ecb_hal_data.cleartext[SOC_ECB_KEY_LENGTH - 1 - i] = p_r[i];
- }
-
- (void) sd_ecb_block_encrypt(&ecb_hal_data);
- for (uint32_t i = 0; i < IM_ADDR_CIPHERTEXT_LENGTH; i++)
- {
- p_local_hash[i] = ecb_hal_data.ciphertext[SOC_ECB_KEY_LENGTH - 1 - i];
- }
- }
- bool im_address_resolve(ble_gap_addr_t const * p_addr, ble_gap_irk_t const * p_irk)
- {
- uint8_t hash[IM_ADDR_CIPHERTEXT_LENGTH];
- uint8_t local_hash[IM_ADDR_CIPHERTEXT_LENGTH];
- uint8_t prand[IM_ADDR_CLEARTEXT_LENGTH];
- if (p_addr->addr_type != BLE_GAP_ADDR_TYPE_RANDOM_PRIVATE_RESOLVABLE)
- {
- return false;
- }
- memcpy(hash, p_addr->addr, IM_ADDR_CIPHERTEXT_LENGTH);
- memcpy(prand, &p_addr->addr[IM_ADDR_CIPHERTEXT_LENGTH], IM_ADDR_CLEARTEXT_LENGTH);
- ah(p_irk->irk, prand, local_hash);
- return (memcmp(hash, local_hash, IM_ADDR_CIPHERTEXT_LENGTH) == 0);
- }
- #endif
|