GitHub/epd42
1
0
Fork 0
mirror of https://github.com/pengwon/epd42.git synced 2026-03-26 10:39:47 +08:00
Code Issues Packages Projects Releases Wiki Activity
Files
3d7db302aaa82b593b4bdfd86ce23514aa43f11e
epd42/components/ble/peer_manager/id_manager.c
Shuanglei Tao f353d23368 Initial commit
2024-11-11 15:35:36 +08:00

731 lines
26 KiB
C
Raw Blame History

/* Copyright (C) 2015 Nordic Semiconductor. All Rights Reserved.
*
* The information contained herein is property of Nordic Semiconductor ASA.
* SEMICONDUCTOR STANDARD SOFTWARE LICENSE AGREEMENT.
*
* Licensees are granted free, non-transferable use of the information. NO
* WARRANTY of ANY KIND is provided. This heading must NOT be removed from
* the file.
*
*/
#include "id_manager.h"
#include <string.h>
#include "nrf_soc.h"
#include "ble_gap.h"
#include "ble_conn_state.h"
#include "peer_manager_types.h"
#include "peer_database.h"
#include "nordic_common.h"
#define IM_MAX_CONN_HANDLES 8
#define IM_NO_INVALID_CONN_HANDLES 0xFF
#define MAX_REGISTRANTS 3
#define WHITELIST_MAX_COUNT MAX(BLE_GAP_WHITELIST_ADDR_MAX_COUNT, \
BLE_GAP_WHITELIST_IRK_MAX_COUNT)
#define IM_ADDR_CLEARTEXT_LENGTH 3
#define IM_ADDR_CIPHERTEXT_LENGTH 3
#define MODULE_INITIALIZED (m_im.n_registrants > 0)
#define VERIFY_MODULE_INITIALIZED() \
do \
{ \
if (!MODULE_INITIALIZED) \
{ \
return NRF_ERROR_INVALID_STATE; \
} \
} while(0)
#define VERIFY_PARAM_NOT_NULL(param) \
do \
{ \
if (param == NULL) \
{ \
return NRF_ERROR_NULL; \
} \
} while(0)
typedef struct
{
pm_peer_id_t peer_id;
uint16_t conn_handle;
ble_gap_addr_t peer_address;
} im_connection_t;
typedef struct
{
im_evt_handler_t evt_handlers[MAX_REGISTRANTS];
uint8_t n_registrants;
im_connection_t connections[8];
pm_peer_id_t whitelist_peer_ids[BLE_GAP_WHITELIST_IRK_MAX_COUNT];
ble_gap_irk_t whitelist_irks[BLE_GAP_WHITELIST_IRK_MAX_COUNT];
ble_gap_addr_t whitelist_addrs[BLE_GAP_WHITELIST_ADDR_MAX_COUNT];
uint8_t n_whitelist_peer_ids;
ble_conn_state_user_flag_id_t conn_state_user_flag_id;
} im_t;
static im_t m_im = {.n_registrants = 0};
static void internal_state_reset()
{
memset(&m_im, 0, sizeof(im_t));
m_im.n_registrants = 0;
m_im.n_whitelist_peer_ids = 0;
m_im.conn_state_user_flag_id = BLE_CONN_STATE_USER_FLAG_INVALID;
for (uint32_t i = 0; i < IM_MAX_CONN_HANDLES; i++)
{
m_im.connections[i].conn_handle = BLE_CONN_HANDLE_INVALID;
}
}
/**@brief Function for sending an event to all registered event handlers.
*
* @param[in] p_event The event to distribute.
*/
static void evt_send(im_evt_t * p_event)
{
for (uint32_t i = 0; i < m_im.n_registrants; i++)
{
m_im.evt_handlers[i](p_event);
}
}
/**@brief Function finding a free position in m_im.connections.
*
* @detail All connection handles in the m_im.connections array are checked against the connection
* state module. The index of the first one that is not a connection handle for a current
* connection is returned. This position in the array can safely be used for a new connection.
*
* @return Either the index of a free position in the array or IM_NO_INVALID_CONN_HANDLES if no free
position exists.
*/
uint8_t get_free_connection()
{
for (uint32_t i = 0; i < IM_MAX_CONN_HANDLES; i++)
{
// Query the connection state module to check if the connection handle does not belong to a
// valid connection.
if (!ble_conn_state_user_flag_get(m_im.connections[i].conn_handle, m_im.conn_state_user_flag_id))
{
return i;
}
}
// If all connection handles belong to a valid connection, return IM_NO_INVALID_CONN_HANDLES.
return IM_NO_INVALID_CONN_HANDLES;
}
/**@brief Function finding a particular connection handle m_im.connections.
*
* @param[in] conn_handle The handle to find.
*
* @return Either the index of the conn_handle in the array or IM_NO_INVALID_CONN_HANDLES if the
* handle was not found.
*/
uint8_t get_connection_by_conn_handle(uint16_t conn_handle)
{
if (ble_conn_state_user_flag_get(conn_handle, m_im.conn_state_user_flag_id))
{
for (uint32_t i = 0; i < IM_MAX_CONN_HANDLES; i++)
{
if (m_im.connections[i].conn_handle == conn_handle)
{
return i;
}
}
}
// If all connection handles belong to a valid connection, return IM_NO_INVALID_CONN_HANDLES.
return IM_NO_INVALID_CONN_HANDLES;
}
/**@brief Function for registering a new connection instance.
*
* @param[in] conn_handle The handle of the new connection.
* @param[in] p_ble_addr The address used to connect.
*
* @return Either the index of the new connection in the array or IM_NO_INVALID_CONN_HANDLES if no
* free position exists.
*/
uint8_t new_connection(uint16_t conn_handle, ble_gap_addr_t * p_ble_addr)
{
uint8_t conn_index = IM_NO_INVALID_CONN_HANDLES;
if ((p_ble_addr != NULL) && (conn_handle != BLE_CONN_HANDLE_INVALID))
{
ble_conn_state_user_flag_set(conn_handle, m_im.conn_state_user_flag_id, true);
conn_index = get_connection_by_conn_handle(conn_handle);
if (conn_index == IM_NO_INVALID_CONN_HANDLES)
{
conn_index = get_free_connection();
}
if (conn_index != IM_NO_INVALID_CONN_HANDLES)
{
m_im.connections[conn_index].conn_handle = conn_handle;
m_im.connections[conn_index].peer_id = PM_PEER_ID_INVALID;
m_im.connections[conn_index].peer_address = *p_ble_addr;
}
}
return conn_index;
}
/**@brief Function checking the validity of an IRK
*
* @detail An all-zero IRK is not valid. This function will check if a given IRK is valid.
*
* @param[in] irk The IRK for which the validity is going to be checked.
*
* @retval true The IRK is valid.
* @retval false The IRK is invalid.
*/
bool is_valid_irk(ble_gap_irk_t const * irk)
{
for (uint32_t i = 0; i < BLE_GAP_SEC_KEY_LEN; i++)
{
if (irk->irk[i] != 0)
{
return true;
}
}
return false;
}
/**@brief Function for comparing two addresses to determine if they are identical
*
* @note The address type need to be identical, as well as every bit in the address itself.
*
* @param[in] p_addr1 The first address to be compared.
* @param[in] p_addr2 The second address to be compared.
*
* @retval true The addresses are identical.
* @retval false The addresses are not identical.
*/
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;
}
// Check that the addr type is identical, return false if it is not
if (p_addr1->addr_type != p_addr2->addr_type)
{
return false;
}
// Check if the addr bytes are is identical
return (memcmp(p_addr1->addr, p_addr2->addr, BLE_GAP_ADDR_LEN) == 0);
}
void im_ble_evt_handler(ble_evt_t * ble_evt)
{
ret_code_t err_code;
switch (ble_evt->header.evt_id)
{
case BLE_GAP_EVT_CONNECTED:
{
pm_peer_id_t bonded_matching_peer_id = PM_PEER_ID_INVALID;
if (ble_evt->evt.gap_evt.params.connected.irk_match == 1)
{
// The peer was matched using a whitelist.
bonded_matching_peer_id
= m_im.whitelist_peer_ids[ble_evt->evt.gap_evt.params.connected.irk_match_idx];
}
else if ( ble_evt->evt.gap_evt.params.connected.peer_addr.addr_type
!= BLE_GAP_ADDR_TYPE_RANDOM_PRIVATE_NON_RESOLVABLE)
{
/* Search the database for bonding data matching the one that triggered the event.
* Public and static addresses can be matched on address alone, while resolvable
* random addresses can be resolved agains known IRKs. Non-resolvable random addresses
* are never matching because they are not longterm form of identification.
*/
pm_peer_id_t compared_peer_id = pdb_next_peer_id_get(PM_PEER_ID_INVALID);
while ( (compared_peer_id != PM_PEER_ID_INVALID)
&& (bonded_matching_peer_id == PM_PEER_ID_INVALID))
{
pm_peer_data_flash_t compared_data;
switch (ble_evt->evt.gap_evt.params.connected.peer_addr.addr_type)
{
case BLE_GAP_ADDR_TYPE_PUBLIC:
/* fall-through */
case BLE_GAP_ADDR_TYPE_RANDOM_STATIC:
err_code = pdb_read_buf_get(compared_peer_id,
PM_PEER_DATA_ID_BONDING,
&compared_data,
NULL);
if ((err_code == NRF_SUCCESS) &&
addr_compare(&ble_evt->evt.gap_evt.params.connected.peer_addr,
&compared_data.data.p_bonding_data->peer_id.id_addr_info)
)
{
bonded_matching_peer_id = compared_peer_id;
}
break;
case BLE_GAP_ADDR_TYPE_RANDOM_PRIVATE_RESOLVABLE:
err_code = pdb_read_buf_get(compared_peer_id,
PM_PEER_DATA_ID_BONDING,
&compared_data,
NULL);
if (err_code == NRF_SUCCESS &&
im_address_resolve(&ble_evt->evt.gap_evt.params.connected.peer_addr,
&compared_data.data.p_bonding_data->peer_id.id_info)
)
{
bonded_matching_peer_id = compared_peer_id;
}
break;
case BLE_GAP_ADDR_TYPE_RANDOM_PRIVATE_NON_RESOLVABLE:
// Should not happen.
break;
default:
break;
}
compared_peer_id = pdb_next_peer_id_get(compared_peer_id);
}
}
new_connection(ble_evt->evt.gap_evt.conn_handle, &ble_evt->evt.gap_evt.params.connected.peer_addr);
if (bonded_matching_peer_id != PM_PEER_ID_INVALID)
{
im_new_peer_id(ble_evt->evt.gap_evt.conn_handle, bonded_matching_peer_id);
// Send a bonded peer event
im_evt_t im_evt;
im_evt.conn_handle = ble_evt->evt.gap_evt.conn_handle;
im_evt.evt_id = IM_EVT_BONDED_PEER_CONNECTED;
evt_send(&im_evt);
}
}
}
}
/**@brief Function to compare two sets of bonding data to check if they belong to the same device.
* @note Invalid irks will never match even though they are identical.
*
* @param[in] p_bonding_data1 First bonding data for comparison
* @param[in] p_bonding_data2 Second bonding data for comparison
*
* @return True if the input matches, false if it does not.
*/
bool is_duplicate_bonding_data(pm_peer_data_bonding_t const * p_bonding_data1,
pm_peer_data_bonding_t const * p_bonding_data2)
{
bool valid_irk = is_valid_irk(&p_bonding_data1->peer_id.id_info);
bool duplicate_irk = valid_irk &&
(memcmp(p_bonding_data1->peer_id.id_info.irk,
p_bonding_data2->peer_id.id_info.irk,
BLE_GAP_SEC_KEY_LEN) == 0
);
bool duplicate_addr = addr_compare(&p_bonding_data1->peer_id.id_addr_info,
&p_bonding_data2->peer_id.id_addr_info
);
return duplicate_irk || duplicate_addr;
}
/**@brief Event handler for events from the peer_database module.
*
* @param[in] p_event The event that has happend with peer id and flags.
*/
static void pdb_evt_handler(pdb_evt_t const * p_event)
{
ret_code_t err_code;
if ((p_event != NULL) && (p_event->evt_id == PDB_EVT_WRITE_BUF_STORED))
{
// If new data about peer id has been stored it is compared to other peers peer ids in
// search of duplicates.
if (p_event->data_id == PM_PEER_DATA_ID_BONDING)
{
pm_peer_data_flash_t written_data;
err_code = pdb_read_buf_get(p_event->peer_id, PM_PEER_DATA_ID_BONDING, &written_data, NULL);
if (err_code == NRF_SUCCESS)
{
pm_peer_id_t compared_peer_id = pdb_next_peer_id_get(PM_PEER_ID_INVALID);
while (compared_peer_id != PM_PEER_ID_INVALID)
{
pm_peer_data_flash_t compared_data;
err_code = pdb_read_buf_get(compared_peer_id,
PM_PEER_DATA_ID_BONDING,
&compared_data,
NULL);
if ( err_code == NRF_SUCCESS &&
p_event->peer_id != compared_peer_id &&
is_duplicate_bonding_data(written_data.data.p_bonding_data,
compared_data.data.p_bonding_data)
)
{
im_evt_t im_evt;
im_evt.conn_handle = im_conn_handle_get(p_event->peer_id);
im_evt.evt_id = IM_EVT_DUPLICATE_ID;
im_evt.params.duplicate_id.peer_id_1 = p_event->peer_id;
im_evt.params.duplicate_id.peer_id_2 = compared_peer_id;
evt_send(&im_evt);
}
compared_peer_id = pdb_next_peer_id_get(compared_peer_id);
}
}
}
}
}
ret_code_t im_register(im_evt_handler_t evt_handler)
{
VERIFY_PARAM_NOT_NULL(evt_handler);
ret_code_t err_code = NRF_SUCCESS;
if (!MODULE_INITIALIZED)
{
internal_state_reset();
m_im.conn_state_user_flag_id = ble_conn_state_user_flag_acquire();
if (m_im.conn_state_user_flag_id == BLE_CONN_STATE_USER_FLAG_INVALID)
{
err_code = NRF_ERROR_NO_MEM;
}
else
{
err_code = pdb_register(pdb_evt_handler);
}
}
if (err_code == NRF_SUCCESS)
{
if ((m_im.n_registrants < MAX_REGISTRANTS))
{
m_im.evt_handlers[m_im.n_registrants++] = evt_handler;
}
else
{
err_code = NRF_ERROR_NO_MEM;
}
}
return err_code;
}
pm_peer_id_t im_peer_id_get_by_conn_handle(uint16_t conn_handle)
{
uint8_t conn_index = get_connection_by_conn_handle(conn_handle);
if (MODULE_INITIALIZED && (conn_index != IM_NO_INVALID_CONN_HANDLES))
{
return m_im.connections[conn_index].peer_id;
}
return PM_PEER_ID_INVALID;
}
ret_code_t im_ble_addr_get(uint16_t conn_handle, ble_gap_addr_t * p_ble_addr)
{
VERIFY_MODULE_INITIALIZED();
VERIFY_PARAM_NOT_NULL(p_ble_addr);
uint8_t conn_index = get_connection_by_conn_handle(conn_handle);
if (conn_index != IM_NO_INVALID_CONN_HANDLES)
{
*p_ble_addr = m_im.connections[conn_index].peer_address;
return NRF_SUCCESS;
}
return NRF_ERROR_NOT_FOUND;
}
/**@brief Function for comparing two master ids
* @note Two invalid master IDs will not match.
*
* @param[in] p_master_id1 First master id for comparison
* @param[in] p_master_id2 Second master id for comparison
*
* @return True if the input matches, false if it does not.
*/
bool master_id_compare(ble_gap_master_id_t const * p_master_id1,
ble_gap_master_id_t const * p_master_id2)
{
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 * p_master_id)
{
ret_code_t err_code;
// For each stored peer, check if the master_id match p_master_id
pm_peer_id_t compared_peer_id = pdb_next_peer_id_get(PM_PEER_ID_INVALID);
while (compared_peer_id != PM_PEER_ID_INVALID)
{
pm_peer_data_flash_t compared_data;
ble_gap_master_id_t const * p_compared_master_id;
err_code = pdb_read_buf_get(compared_peer_id, PM_PEER_DATA_ID_BONDING, &compared_data, NULL);
if (err_code == NRF_SUCCESS)
{
p_compared_master_id = &compared_data.data.p_bonding_data->own_ltk.master_id;
if (compared_data.data.p_bonding_data->own_role == BLE_GAP_ROLE_CENTRAL)
{
p_compared_master_id = &compared_data.data.p_bonding_data->peer_ltk.master_id;
}
if (master_id_compare(p_master_id, p_compared_master_id))
{
// If a matching master_id is found return the peer_id
return compared_peer_id;
}
}
compared_peer_id = pdb_next_peer_id_get(compared_peer_id);
}
// If no matching master_id is found return the PM_PEER_ID_INVALID
return PM_PEER_ID_INVALID;
}
pm_peer_id_t im_peer_id_get_by_irk_match_idx(uint8_t irk_match_idx)
{
// Verify that the requested idx is within the list
if (irk_match_idx < m_im.n_whitelist_peer_ids)
{
// Return the peer_id from the white list
return m_im.whitelist_peer_ids[irk_match_idx];
}
else
{
// Return PM_PEER_ID_INVALID to indicate that there was no peer with the requested idx
return PM_PEER_ID_INVALID;
}
}
uint16_t im_conn_handle_get(pm_peer_id_t peer_id)
{
for (uint32_t i = 0; i < IM_MAX_CONN_HANDLES; i++)
{
if (peer_id == m_im.connections[i].peer_id)
{
return m_im.connections[i].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)
{
uint8_t conn_index = get_connection_by_conn_handle(conn_handle);
if (conn_index != IM_NO_INVALID_CONN_HANDLES)
{
m_im.connections[conn_index].peer_id = peer_id;
}
}
ret_code_t im_wlist_create(pm_peer_id_t * p_peer_ids,
uint8_t n_peer_ids,
ble_gap_whitelist_t * p_whitelist)
{
VERIFY_MODULE_INITIALIZED();
VERIFY_PARAM_NOT_NULL(p_whitelist);
ret_code_t err_code;
p_whitelist->addr_count = 0;
p_whitelist->irk_count = 0;
m_im.n_whitelist_peer_ids = 0;
for (uint32_t peer_index = 0; peer_index < n_peer_ids; peer_index++)
{
bool peer_connected = false;
for (uint32_t conn_index = 0; conn_index < IM_MAX_CONN_HANDLES; conn_index++)
{
if (p_peer_ids[peer_index] == m_im.connections[conn_index].peer_id &&
ble_conn_state_user_flag_get(m_im.connections[conn_index].conn_handle, m_im.conn_state_user_flag_id)
)
{
peer_connected = true;
break;
}
}
if (!peer_connected)
{
pm_peer_data_flash_t peer_data;
err_code = pdb_read_buf_get(p_peer_ids[peer_index], PM_PEER_DATA_ID_BONDING, &peer_data, NULL);
if (err_code == NRF_ERROR_INVALID_PARAM || err_code == NRF_ERROR_NOT_FOUND)
{
return NRF_ERROR_INVALID_PARAM;
}
if (p_whitelist->pp_addrs != NULL &&
peer_data.data.p_bonding_data->peer_id.id_addr_info.addr_type
!= BLE_GAP_ADDR_TYPE_RANDOM_PRIVATE_RESOLVABLE &&
peer_data.data.p_bonding_data->peer_id.id_addr_info.addr_type
!= BLE_GAP_ADDR_TYPE_RANDOM_PRIVATE_NON_RESOLVABLE
)
{
memcpy(m_im.whitelist_addrs[peer_index].addr,
peer_data.data.p_bonding_data->peer_id.id_addr_info.addr,
BLE_GAP_ADDR_LEN
);
m_im.whitelist_addrs[peer_index].addr_type =
peer_data.data.p_bonding_data->peer_id.id_addr_info.addr_type;
p_whitelist->pp_addrs[peer_index] = &m_im.whitelist_addrs[peer_index];
p_whitelist->addr_count++;
}
if (p_whitelist->pp_irks != NULL &&
is_valid_irk(&(peer_data.data.p_bonding_data->peer_id.id_info))
)
{
memcpy(m_im.whitelist_irks[peer_index].irk,
peer_data.data.p_bonding_data->peer_id.id_info.irk,
BLE_GAP_SEC_KEY_LEN
);
p_whitelist->pp_irks[peer_index] = &m_im.whitelist_irks[peer_index];
p_whitelist->irk_count++;
m_im.whitelist_peer_ids[peer_index] = p_peer_ids[peer_index];
m_im.n_whitelist_peer_ids++;
}
}
}
return NRF_SUCCESS;
}
ret_code_t im_wlist_set(ble_gap_whitelist_t * p_whitelist)
{
pm_peer_id_t new_whitelist_peer_ids[BLE_GAP_WHITELIST_IRK_MAX_COUNT];
uint32_t n_new_whitelist_peer_ids = 0;
VERIFY_PARAM_NOT_NULL(p_whitelist);
for (uint32_t i = 0; i < BLE_GAP_WHITELIST_IRK_MAX_COUNT; i++)
{
new_whitelist_peer_ids[i] = PM_PEER_ID_INVALID;
}
pm_peer_id_t compared_peer_id = pdb_next_peer_id_get(PM_PEER_ID_INVALID);
while (compared_peer_id != PM_PEER_ID_INVALID)
{
pm_peer_data_flash_t compared_data;
pdb_read_buf_get(compared_peer_id, PM_PEER_DATA_ID_BONDING, &compared_data, NULL);
for (uint32_t i = 0; i < p_whitelist->irk_count; i++)
{
bool valid_irk = is_valid_irk(&compared_data.data.p_bonding_data->peer_id.id_info);
bool duplicate_irk = valid_irk &&
(memcmp(p_whitelist->pp_irks[i]->irk,
compared_data.data.p_bonding_data->peer_id.id_info.irk,
BLE_GAP_SEC_KEY_LEN) == 0
);
if (duplicate_irk)
{
new_whitelist_peer_ids[i] = compared_peer_id;
n_new_whitelist_peer_ids++;
}
}
compared_peer_id = pdb_next_peer_id_get(compared_peer_id);
}
if (n_new_whitelist_peer_ids != p_whitelist->irk_count)
{
return NRF_ERROR_NOT_FOUND;
}
else
{
for (uint32_t i = 0; i < n_new_whitelist_peer_ids; i++)
{
m_im.whitelist_peer_ids[i] = new_whitelist_peer_ids[i];
}
m_im.n_whitelist_peer_ids = n_new_whitelist_peer_ids;
return NRF_SUCCESS;
}
}
/**@brief Function for calculating the ah() hash function described in Bluetooth core specification
* 4.2 section 3.H.2.2.2.
*
* @detail BLE uses a hash function to calculate the first half of a resolvable address
* from the second half of the address and an irk. This function will use the ECB
* periferal to hash these data acording to the Bluetooth core specification.
*
* @note The ECB expect little endian input and output.
* This function expect big endian and will reverse the data as necessary.
*
* @param[in] p_k The key used in the hash function.
* For address resolution this is should be the irk.
* The array must have a length of 16.
* @param[in] p_r The rand used in the hash function. For generating a new address
* this would be a random number. For resolving a resolvable address
* this would be the last half of the address being resolved.
* The array must have a length of 3.
* @param[out] p_local_hash The result of the hash operation. For address resolution this
* will match the first half of the address being resolved if and only
* if the irk used in the hash function is the same one used to generate
* the address.
* The array must have a length of 16.
*/
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];
}
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)
{
if (p_addr->addr_type != BLE_GAP_ADDR_TYPE_RANDOM_PRIVATE_RESOLVABLE)
{
return false;
}
uint8_t hash[IM_ADDR_CIPHERTEXT_LENGTH];
uint8_t local_hash[IM_ADDR_CIPHERTEXT_LENGTH];
uint8_t prand[IM_ADDR_CLEARTEXT_LENGTH];
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);
}
Reference in New Issue View Git Blame Copy Permalink