R1 code for micro:bit based train controller code, requires second micro:bit running rx code to operate - see https://meanderingpi.wordpress.com/ for more information
Fork of nrf51-sdk by
id_manager.c
00001 /* 00002 * Copyright (c) Nordic Semiconductor ASA 00003 * All rights reserved. 00004 * 00005 * Redistribution and use in source and binary forms, with or without modification, 00006 * are permitted provided that the following conditions are met: 00007 * 00008 * 1. Redistributions of source code must retain the above copyright notice, this 00009 * list of conditions and the following disclaimer. 00010 * 00011 * 2. Redistributions in binary form must reproduce the above copyright notice, this 00012 * list of conditions and the following disclaimer in the documentation and/or 00013 * other materials provided with the distribution. 00014 * 00015 * 3. Neither the name of Nordic Semiconductor ASA nor the names of other 00016 * contributors to this software may be used to endorse or promote products 00017 * derived from this software without specific prior written permission. 00018 * 00019 * 00020 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 00021 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 00022 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 00023 * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR 00024 * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 00025 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00026 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON 00027 * ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 00028 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 00029 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 00030 * 00031 */ 00032 00033 #include "id_manager.h" 00034 00035 #include <string.h> 00036 #include "nrf_soc.h" 00037 #include "ble_gap.h" 00038 #include "ble_conn_state.h " 00039 #include "peer_manager_types.h " 00040 #include "peer_database.h" 00041 #include "nordic_common.h" 00042 00043 #define IM_MAX_CONN_HANDLES 8 00044 #define IM_NO_INVALID_CONN_HANDLES 0xFF 00045 #define MAX_REGISTRANTS 3 00046 #define WHITELIST_MAX_COUNT MAX(BLE_GAP_WHITELIST_ADDR_MAX_COUNT, \ 00047 BLE_GAP_WHITELIST_IRK_MAX_COUNT) 00048 #define IM_ADDR_CLEARTEXT_LENGTH 3 00049 #define IM_ADDR_CIPHERTEXT_LENGTH 3 00050 00051 #define MODULE_INITIALIZED (m_im.n_registrants > 0) 00052 00053 #define VERIFY_MODULE_INITIALIZED() \ 00054 do \ 00055 { \ 00056 if (!MODULE_INITIALIZED) \ 00057 { \ 00058 return NRF_ERROR_INVALID_STATE; \ 00059 } \ 00060 } while(0) 00061 00062 #define VERIFY_PARAM_NOT_NULL(param) \ 00063 do \ 00064 { \ 00065 if (param == NULL) \ 00066 { \ 00067 return NRF_ERROR_NULL; \ 00068 } \ 00069 } while(0) 00070 00071 00072 typedef struct 00073 { 00074 pm_peer_id_t peer_id; 00075 uint16_t conn_handle; 00076 ble_gap_addr_t peer_address; 00077 } im_connection_t; 00078 00079 typedef struct 00080 { 00081 im_evt_handler_t evt_handlers[MAX_REGISTRANTS]; 00082 uint8_t n_registrants; 00083 im_connection_t connections[8]; 00084 pm_peer_id_t whitelist_peer_ids[BLE_GAP_WHITELIST_IRK_MAX_COUNT]; 00085 ble_gap_irk_t whitelist_irks[BLE_GAP_WHITELIST_IRK_MAX_COUNT]; 00086 ble_gap_addr_t whitelist_addrs[BLE_GAP_WHITELIST_ADDR_MAX_COUNT]; 00087 uint8_t n_whitelist_peer_ids; 00088 ble_conn_state_user_flag_id_t conn_state_user_flag_id; 00089 } im_t; 00090 00091 static im_t m_im = {.n_registrants = 0}; 00092 00093 static void internal_state_reset() 00094 { 00095 memset(&m_im, 0, sizeof(im_t)); 00096 m_im.n_registrants = 0; 00097 m_im.n_whitelist_peer_ids = 0; 00098 m_im.conn_state_user_flag_id = BLE_CONN_STATE_USER_FLAG_INVALID; 00099 for (uint32_t i = 0; i < IM_MAX_CONN_HANDLES; i++) 00100 { 00101 m_im.connections[i].conn_handle = BLE_CONN_HANDLE_INVALID; 00102 } 00103 } 00104 00105 00106 /**@brief Function for sending an event to all registered event handlers. 00107 * 00108 * @param[in] p_event The event to distribute. 00109 */ 00110 static void evt_send(im_evt_t * p_event) 00111 { 00112 for (uint32_t i = 0; i < m_im.n_registrants; i++) 00113 { 00114 m_im.evt_handlers[i](p_event); 00115 } 00116 } 00117 00118 /**@brief Function finding a free position in m_im.connections. 00119 * 00120 * @detail All connection handles in the m_im.connections array are checked against the connection 00121 * state module. The index of the first one that is not a connection handle for a current 00122 * connection is returned. This position in the array can safely be used for a new connection. 00123 * 00124 * @return Either the index of a free position in the array or IM_NO_INVALID_CONN_HANDLES if no free 00125 position exists. 00126 */ 00127 uint8_t get_free_connection() 00128 { 00129 for (uint32_t i = 0; i < IM_MAX_CONN_HANDLES; i++) 00130 { 00131 // Query the connection state module to check if the connection handle does not belong to a 00132 // valid connection. 00133 if (!ble_conn_state_user_flag_get(m_im.connections[i].conn_handle, m_im.conn_state_user_flag_id)) 00134 { 00135 return i; 00136 } 00137 } 00138 // If all connection handles belong to a valid connection, return IM_NO_INVALID_CONN_HANDLES. 00139 return IM_NO_INVALID_CONN_HANDLES; 00140 } 00141 00142 00143 /**@brief Function finding a particular connection handle m_im.connections. 00144 * 00145 * @param[in] conn_handle The handle to find. 00146 * 00147 * @return Either the index of the conn_handle in the array or IM_NO_INVALID_CONN_HANDLES if the 00148 * handle was not found. 00149 */ 00150 uint8_t get_connection_by_conn_handle(uint16_t conn_handle) 00151 { 00152 if (ble_conn_state_user_flag_get(conn_handle, m_im.conn_state_user_flag_id)) 00153 { 00154 for (uint32_t i = 0; i < IM_MAX_CONN_HANDLES; i++) 00155 { 00156 if (m_im.connections[i].conn_handle == conn_handle) 00157 { 00158 return i; 00159 } 00160 } 00161 } 00162 // If all connection handles belong to a valid connection, return IM_NO_INVALID_CONN_HANDLES. 00163 return IM_NO_INVALID_CONN_HANDLES; 00164 } 00165 00166 00167 /**@brief Function for registering a new connection instance. 00168 * 00169 * @param[in] conn_handle The handle of the new connection. 00170 * @param[in] p_ble_addr The address used to connect. 00171 * 00172 * @return Either the index of the new connection in the array or IM_NO_INVALID_CONN_HANDLES if no 00173 * free position exists. 00174 */ 00175 uint8_t new_connection(uint16_t conn_handle, ble_gap_addr_t * p_ble_addr) 00176 { 00177 uint8_t conn_index = IM_NO_INVALID_CONN_HANDLES; 00178 00179 if ((p_ble_addr != NULL) && (conn_handle != BLE_CONN_HANDLE_INVALID)) 00180 { 00181 ble_conn_state_user_flag_set(conn_handle, m_im.conn_state_user_flag_id, true); 00182 00183 conn_index = get_connection_by_conn_handle(conn_handle); 00184 if (conn_index == IM_NO_INVALID_CONN_HANDLES) 00185 { 00186 conn_index = get_free_connection(); 00187 } 00188 00189 if (conn_index != IM_NO_INVALID_CONN_HANDLES) 00190 { 00191 m_im.connections[conn_index].conn_handle = conn_handle; 00192 m_im.connections[conn_index].peer_id = PM_PEER_ID_INVALID; 00193 m_im.connections[conn_index].peer_address = *p_ble_addr; 00194 } 00195 } 00196 return conn_index; 00197 } 00198 00199 00200 /**@brief Function checking the validity of an IRK 00201 * 00202 * @detail An all-zero IRK is not valid. This function will check if a given IRK is valid. 00203 * 00204 * @param[in] irk The IRK for which the validity is going to be checked. 00205 * 00206 * @retval true The IRK is valid. 00207 * @retval false The IRK is invalid. 00208 */ 00209 bool is_valid_irk(ble_gap_irk_t const * irk) 00210 { 00211 for (uint32_t i = 0; i < BLE_GAP_SEC_KEY_LEN; i++) 00212 { 00213 if (irk->irk[i] != 0) 00214 { 00215 return true; 00216 } 00217 } 00218 return false; 00219 } 00220 00221 00222 /**@brief Function for comparing two addresses to determine if they are identical 00223 * 00224 * @note The address type need to be identical, as well as every bit in the address itself. 00225 * 00226 * @param[in] p_addr1 The first address to be compared. 00227 * @param[in] p_addr2 The second address to be compared. 00228 * 00229 * @retval true The addresses are identical. 00230 * @retval false The addresses are not identical. 00231 */ 00232 bool addr_compare(ble_gap_addr_t const * p_addr1, ble_gap_addr_t const * p_addr2) 00233 { 00234 if ((p_addr1 == NULL) || (p_addr2 == NULL)) 00235 { 00236 return false; 00237 } 00238 00239 // Check that the addr type is identical, return false if it is not 00240 if (p_addr1->addr_type != p_addr2->addr_type) 00241 { 00242 return false; 00243 } 00244 // Check if the addr bytes are is identical 00245 return (memcmp(p_addr1->addr, p_addr2->addr, BLE_GAP_ADDR_LEN) == 0); 00246 } 00247 00248 00249 void im_ble_evt_handler(ble_evt_t * ble_evt) 00250 { 00251 ret_code_t err_code; 00252 switch (ble_evt->header.evt_id) 00253 { 00254 case BLE_GAP_EVT_CONNECTED: 00255 { 00256 pm_peer_id_t bonded_matching_peer_id = PM_PEER_ID_INVALID; 00257 00258 if (ble_evt->evt.gap_evt.params.connected.irk_match == 1) 00259 { 00260 // The peer was matched using a whitelist. 00261 bonded_matching_peer_id 00262 = m_im.whitelist_peer_ids[ble_evt->evt.gap_evt.params.connected.irk_match_idx]; 00263 } 00264 else if ( ble_evt->evt.gap_evt.params.connected.peer_addr.addr_type 00265 != BLE_GAP_ADDR_TYPE_RANDOM_PRIVATE_NON_RESOLVABLE) 00266 { 00267 /* Search the database for bonding data matching the one that triggered the event. 00268 * Public and static addresses can be matched on address alone, while resolvable 00269 * random addresses can be resolved agains known IRKs. Non-resolvable random addresses 00270 * are never matching because they are not longterm form of identification. 00271 */ 00272 pm_peer_id_t compared_peer_id = pdb_next_peer_id_get(PM_PEER_ID_INVALID); 00273 while ( (compared_peer_id != PM_PEER_ID_INVALID) 00274 && (bonded_matching_peer_id == PM_PEER_ID_INVALID)) 00275 { 00276 pm_peer_data_flash_t compared_data; 00277 switch (ble_evt->evt.gap_evt.params.connected.peer_addr.addr_type) 00278 { 00279 case BLE_GAP_ADDR_TYPE_PUBLIC: 00280 /* fall-through */ 00281 case BLE_GAP_ADDR_TYPE_RANDOM_STATIC: 00282 err_code = pdb_read_buf_get(compared_peer_id, 00283 PM_PEER_DATA_ID_BONDING, 00284 &compared_data, 00285 NULL); 00286 if ((err_code == NRF_SUCCESS) && 00287 addr_compare(&ble_evt->evt.gap_evt.params.connected.peer_addr, 00288 &compared_data.data.p_bonding_data->peer_id.id_addr_info) 00289 ) 00290 { 00291 bonded_matching_peer_id = compared_peer_id; 00292 } 00293 break; 00294 00295 case BLE_GAP_ADDR_TYPE_RANDOM_PRIVATE_RESOLVABLE: 00296 err_code = pdb_read_buf_get(compared_peer_id, 00297 PM_PEER_DATA_ID_BONDING, 00298 &compared_data, 00299 NULL); 00300 if (err_code == NRF_SUCCESS && 00301 im_address_resolve(&ble_evt->evt.gap_evt.params.connected.peer_addr, 00302 &compared_data.data.p_bonding_data->peer_id.id_info) 00303 ) 00304 { 00305 bonded_matching_peer_id = compared_peer_id; 00306 } 00307 break; 00308 00309 case BLE_GAP_ADDR_TYPE_RANDOM_PRIVATE_NON_RESOLVABLE: 00310 // Should not happen. 00311 break; 00312 00313 default: 00314 break; 00315 } 00316 compared_peer_id = pdb_next_peer_id_get(compared_peer_id); 00317 } 00318 } 00319 new_connection(ble_evt->evt.gap_evt.conn_handle, &ble_evt->evt.gap_evt.params.connected.peer_addr); 00320 00321 if (bonded_matching_peer_id != PM_PEER_ID_INVALID) 00322 { 00323 im_new_peer_id(ble_evt->evt.gap_evt.conn_handle, bonded_matching_peer_id); 00324 00325 // Send a bonded peer event 00326 im_evt_t im_evt; 00327 im_evt.conn_handle = ble_evt->evt.gap_evt.conn_handle; 00328 im_evt.evt_id = IM_EVT_BONDED_PEER_CONNECTED; 00329 evt_send(&im_evt); 00330 } 00331 } 00332 } 00333 } 00334 00335 00336 /**@brief Function to compare two sets of bonding data to check if they belong to the same device. 00337 * @note Invalid irks will never match even though they are identical. 00338 * 00339 * @param[in] p_bonding_data1 First bonding data for comparison 00340 * @param[in] p_bonding_data2 Second bonding data for comparison 00341 * 00342 * @return True if the input matches, false if it does not. 00343 */ 00344 bool is_duplicate_bonding_data(pm_peer_data_bonding_t const * p_bonding_data1, 00345 pm_peer_data_bonding_t const * p_bonding_data2) 00346 { 00347 bool valid_irk = is_valid_irk(&p_bonding_data1->peer_id.id_info); 00348 bool duplicate_irk = valid_irk && 00349 (memcmp(p_bonding_data1->peer_id.id_info.irk, 00350 p_bonding_data2->peer_id.id_info.irk, 00351 BLE_GAP_SEC_KEY_LEN) == 0 00352 ); 00353 bool duplicate_addr = addr_compare(&p_bonding_data1->peer_id.id_addr_info, 00354 &p_bonding_data2->peer_id.id_addr_info 00355 ); 00356 return duplicate_irk || duplicate_addr; 00357 } 00358 00359 00360 /**@brief Event handler for events from the peer_database module. 00361 * 00362 * @param[in] p_event The event that has happend with peer id and flags. 00363 */ 00364 static void pdb_evt_handler(pdb_evt_t const * p_event) 00365 { 00366 ret_code_t err_code; 00367 if ((p_event != NULL) && (p_event->evt_id == PDB_EVT_WRITE_BUF_STORED)) 00368 { 00369 // If new data about peer id has been stored it is compared to other peers peer ids in 00370 // search of duplicates. 00371 if (p_event->data_id == PM_PEER_DATA_ID_BONDING) 00372 { 00373 pm_peer_data_flash_t written_data; 00374 err_code = pdb_read_buf_get(p_event->peer_id, PM_PEER_DATA_ID_BONDING, &written_data, NULL); 00375 if (err_code == NRF_SUCCESS) 00376 { 00377 pm_peer_id_t compared_peer_id = pdb_next_peer_id_get(PM_PEER_ID_INVALID); 00378 while (compared_peer_id != PM_PEER_ID_INVALID) 00379 { 00380 pm_peer_data_flash_t compared_data; 00381 err_code = pdb_read_buf_get(compared_peer_id, 00382 PM_PEER_DATA_ID_BONDING, 00383 &compared_data, 00384 NULL); 00385 if ( err_code == NRF_SUCCESS && 00386 p_event->peer_id != compared_peer_id && 00387 is_duplicate_bonding_data(written_data.data.p_bonding_data, 00388 compared_data.data.p_bonding_data) 00389 ) 00390 { 00391 im_evt_t im_evt; 00392 im_evt.conn_handle = im_conn_handle_get(p_event->peer_id); 00393 im_evt.evt_id = IM_EVT_DUPLICATE_ID; 00394 im_evt.params.duplicate_id.peer_id_1 = p_event->peer_id; 00395 im_evt.params.duplicate_id.peer_id_2 = compared_peer_id; 00396 evt_send(&im_evt); 00397 } 00398 compared_peer_id = pdb_next_peer_id_get(compared_peer_id); 00399 } 00400 } 00401 } 00402 } 00403 } 00404 00405 00406 ret_code_t im_register(im_evt_handler_t evt_handler) 00407 { 00408 VERIFY_PARAM_NOT_NULL(evt_handler); 00409 ret_code_t err_code = NRF_SUCCESS; 00410 00411 if (!MODULE_INITIALIZED) 00412 { 00413 internal_state_reset(); 00414 m_im.conn_state_user_flag_id = ble_conn_state_user_flag_acquire(); 00415 if (m_im.conn_state_user_flag_id == BLE_CONN_STATE_USER_FLAG_INVALID) 00416 { 00417 err_code = NRF_ERROR_NO_MEM; 00418 } 00419 else 00420 { 00421 err_code = pdb_register(pdb_evt_handler); 00422 } 00423 } 00424 if (err_code == NRF_SUCCESS) 00425 { 00426 if ((m_im.n_registrants < MAX_REGISTRANTS)) 00427 { 00428 m_im.evt_handlers[m_im.n_registrants++] = evt_handler; 00429 } 00430 else 00431 { 00432 err_code = NRF_ERROR_NO_MEM; 00433 } 00434 } 00435 return err_code; 00436 } 00437 00438 00439 pm_peer_id_t im_peer_id_get_by_conn_handle(uint16_t conn_handle) 00440 { 00441 uint8_t conn_index = get_connection_by_conn_handle(conn_handle); 00442 00443 if (MODULE_INITIALIZED && (conn_index != IM_NO_INVALID_CONN_HANDLES)) 00444 { 00445 return m_im.connections[conn_index].peer_id; 00446 } 00447 00448 return PM_PEER_ID_INVALID; 00449 } 00450 00451 00452 ret_code_t im_ble_addr_get(uint16_t conn_handle, ble_gap_addr_t * p_ble_addr) 00453 { 00454 VERIFY_MODULE_INITIALIZED(); 00455 VERIFY_PARAM_NOT_NULL(p_ble_addr); 00456 00457 uint8_t conn_index = get_connection_by_conn_handle(conn_handle); 00458 if (conn_index != IM_NO_INVALID_CONN_HANDLES) 00459 { 00460 *p_ble_addr = m_im.connections[conn_index].peer_address; 00461 return NRF_SUCCESS; 00462 } 00463 00464 return NRF_ERROR_NOT_FOUND; 00465 } 00466 00467 00468 /**@brief Function for comparing two master ids 00469 * @note Two invalid master IDs will not match. 00470 * 00471 * @param[in] p_master_id1 First master id for comparison 00472 * @param[in] p_master_id2 Second master id for comparison 00473 * 00474 * @return True if the input matches, false if it does not. 00475 */ 00476 bool master_id_compare(ble_gap_master_id_t const * p_master_id1, 00477 ble_gap_master_id_t const * p_master_id2) 00478 { 00479 if(!im_master_id_is_valid(p_master_id1)) 00480 { 00481 return false; 00482 } 00483 if (p_master_id1->ediv != p_master_id2->ediv) 00484 { 00485 return false; 00486 } 00487 return (memcmp(p_master_id1->rand, p_master_id2->rand, BLE_GAP_SEC_RAND_LEN) == 0); 00488 } 00489 00490 00491 pm_peer_id_t im_peer_id_get_by_master_id(ble_gap_master_id_t * p_master_id) 00492 { 00493 ret_code_t err_code; 00494 // For each stored peer, check if the master_id match p_master_id 00495 pm_peer_id_t compared_peer_id = pdb_next_peer_id_get(PM_PEER_ID_INVALID); 00496 while (compared_peer_id != PM_PEER_ID_INVALID) 00497 { 00498 pm_peer_data_flash_t compared_data; 00499 ble_gap_master_id_t const * p_compared_master_id; 00500 00501 err_code = pdb_read_buf_get(compared_peer_id, PM_PEER_DATA_ID_BONDING, &compared_data, NULL); 00502 if (err_code == NRF_SUCCESS) 00503 { 00504 p_compared_master_id = &compared_data.data.p_bonding_data->own_ltk.master_id; 00505 if (compared_data.data.p_bonding_data->own_role == BLE_GAP_ROLE_CENTRAL) 00506 { 00507 p_compared_master_id = &compared_data.data.p_bonding_data->peer_ltk.master_id; 00508 } 00509 if (master_id_compare(p_master_id, p_compared_master_id)) 00510 { 00511 // If a matching master_id is found return the peer_id 00512 return compared_peer_id; 00513 } 00514 } 00515 compared_peer_id = pdb_next_peer_id_get(compared_peer_id); 00516 } 00517 // If no matching master_id is found return the PM_PEER_ID_INVALID 00518 return PM_PEER_ID_INVALID; 00519 } 00520 00521 00522 pm_peer_id_t im_peer_id_get_by_irk_match_idx(uint8_t irk_match_idx) 00523 { 00524 // Verify that the requested idx is within the list 00525 if (irk_match_idx < m_im.n_whitelist_peer_ids) 00526 { 00527 // Return the peer_id from the white list 00528 return m_im.whitelist_peer_ids[irk_match_idx]; 00529 } 00530 else 00531 { 00532 // Return PM_PEER_ID_INVALID to indicate that there was no peer with the requested idx 00533 return PM_PEER_ID_INVALID; 00534 } 00535 } 00536 00537 00538 uint16_t im_conn_handle_get(pm_peer_id_t peer_id) 00539 { 00540 for (uint32_t i = 0; i < IM_MAX_CONN_HANDLES; i++) 00541 { 00542 if (peer_id == m_im.connections[i].peer_id) 00543 { 00544 return m_im.connections[i].conn_handle; 00545 } 00546 } 00547 return BLE_CONN_HANDLE_INVALID; 00548 } 00549 00550 00551 bool im_master_id_is_valid(ble_gap_master_id_t const * p_master_id) 00552 { 00553 00554 if (p_master_id->ediv != 0) 00555 { 00556 return true; 00557 } 00558 for (uint32_t i = 0; i < BLE_GAP_SEC_RAND_LEN; i++) 00559 { 00560 if (p_master_id->rand[i] != 0) 00561 { 00562 return true; 00563 } 00564 } 00565 return false; 00566 } 00567 00568 00569 void im_new_peer_id(uint16_t conn_handle, pm_peer_id_t peer_id) 00570 { 00571 uint8_t conn_index = get_connection_by_conn_handle(conn_handle); 00572 if (conn_index != IM_NO_INVALID_CONN_HANDLES) 00573 { 00574 m_im.connections[conn_index].peer_id = peer_id; 00575 } 00576 } 00577 00578 00579 ret_code_t im_wlist_create(pm_peer_id_t * p_peer_ids, 00580 uint8_t n_peer_ids, 00581 ble_gap_whitelist_t * p_whitelist) 00582 { 00583 VERIFY_MODULE_INITIALIZED(); 00584 VERIFY_PARAM_NOT_NULL(p_whitelist); 00585 ret_code_t err_code; 00586 p_whitelist->addr_count = 0; 00587 p_whitelist->irk_count = 0; 00588 m_im.n_whitelist_peer_ids = 0; 00589 for (uint32_t peer_index = 0; peer_index < n_peer_ids; peer_index++) 00590 { 00591 bool peer_connected = false; 00592 for (uint32_t conn_index = 0; conn_index < IM_MAX_CONN_HANDLES; conn_index++) 00593 { 00594 if (p_peer_ids[peer_index] == m_im.connections[conn_index].peer_id && 00595 ble_conn_state_user_flag_get(m_im.connections[conn_index].conn_handle, m_im.conn_state_user_flag_id) 00596 ) 00597 { 00598 peer_connected = true; 00599 break; 00600 } 00601 } 00602 if (!peer_connected) 00603 { 00604 pm_peer_data_flash_t peer_data; 00605 err_code = pdb_read_buf_get(p_peer_ids[peer_index], PM_PEER_DATA_ID_BONDING, &peer_data, NULL); 00606 if (err_code == NRF_ERROR_INVALID_PARAM || err_code == NRF_ERROR_NOT_FOUND) 00607 { 00608 return NRF_ERROR_INVALID_PARAM; 00609 } 00610 if (p_whitelist->pp_addrs != NULL && 00611 peer_data.data.p_bonding_data->peer_id.id_addr_info.addr_type 00612 != BLE_GAP_ADDR_TYPE_RANDOM_PRIVATE_RESOLVABLE && 00613 peer_data.data.p_bonding_data->peer_id.id_addr_info.addr_type 00614 != BLE_GAP_ADDR_TYPE_RANDOM_PRIVATE_NON_RESOLVABLE 00615 ) 00616 { 00617 memcpy(m_im.whitelist_addrs[peer_index].addr, 00618 peer_data.data.p_bonding_data->peer_id.id_addr_info.addr, 00619 BLE_GAP_ADDR_LEN 00620 ); 00621 m_im.whitelist_addrs[peer_index].addr_type = 00622 peer_data.data.p_bonding_data->peer_id.id_addr_info.addr_type; 00623 p_whitelist->pp_addrs[peer_index] = &m_im.whitelist_addrs[peer_index]; 00624 p_whitelist->addr_count++; 00625 } 00626 if (p_whitelist->pp_irks != NULL && 00627 is_valid_irk(&(peer_data.data.p_bonding_data->peer_id.id_info)) 00628 ) 00629 { 00630 memcpy(m_im.whitelist_irks[peer_index].irk, 00631 peer_data.data.p_bonding_data->peer_id.id_info.irk, 00632 BLE_GAP_SEC_KEY_LEN 00633 ); 00634 p_whitelist->pp_irks[peer_index] = &m_im.whitelist_irks[peer_index]; 00635 p_whitelist->irk_count++; 00636 m_im.whitelist_peer_ids[peer_index] = p_peer_ids[peer_index]; 00637 m_im.n_whitelist_peer_ids++; 00638 } 00639 } 00640 } 00641 return NRF_SUCCESS; 00642 } 00643 00644 00645 ret_code_t im_wlist_set(ble_gap_whitelist_t * p_whitelist) 00646 { 00647 pm_peer_id_t new_whitelist_peer_ids[BLE_GAP_WHITELIST_IRK_MAX_COUNT]; 00648 uint32_t n_new_whitelist_peer_ids = 0; 00649 VERIFY_PARAM_NOT_NULL(p_whitelist); 00650 for (uint32_t i = 0; i < BLE_GAP_WHITELIST_IRK_MAX_COUNT; i++) 00651 { 00652 new_whitelist_peer_ids[i] = PM_PEER_ID_INVALID; 00653 } 00654 pm_peer_id_t compared_peer_id = pdb_next_peer_id_get(PM_PEER_ID_INVALID); 00655 while (compared_peer_id != PM_PEER_ID_INVALID) 00656 { 00657 pm_peer_data_flash_t compared_data; 00658 pdb_read_buf_get(compared_peer_id, PM_PEER_DATA_ID_BONDING, &compared_data, NULL); 00659 for (uint32_t i = 0; i < p_whitelist->irk_count; i++) 00660 { 00661 bool valid_irk = is_valid_irk(&compared_data.data.p_bonding_data->peer_id.id_info); 00662 bool duplicate_irk = valid_irk && 00663 (memcmp(p_whitelist->pp_irks[i]->irk, 00664 compared_data.data.p_bonding_data->peer_id.id_info.irk, 00665 BLE_GAP_SEC_KEY_LEN) == 0 00666 ); 00667 if (duplicate_irk) 00668 { 00669 new_whitelist_peer_ids[i] = compared_peer_id; 00670 n_new_whitelist_peer_ids++; 00671 } 00672 } 00673 compared_peer_id = pdb_next_peer_id_get(compared_peer_id); 00674 } 00675 if (n_new_whitelist_peer_ids != p_whitelist->irk_count) 00676 { 00677 return NRF_ERROR_NOT_FOUND; 00678 } 00679 else 00680 { 00681 for (uint32_t i = 0; i < n_new_whitelist_peer_ids; i++) 00682 { 00683 m_im.whitelist_peer_ids[i] = new_whitelist_peer_ids[i]; 00684 } 00685 m_im.n_whitelist_peer_ids = n_new_whitelist_peer_ids; 00686 return NRF_SUCCESS; 00687 } 00688 } 00689 00690 00691 /**@brief Function for calculating the ah() hash function described in Bluetooth core specification 00692 * 4.2 section 3.H.2.2.2. 00693 * 00694 * @detail BLE uses a hash function to calculate the first half of a resolvable address 00695 * from the second half of the address and an irk. This function will use the ECB 00696 * periferal to hash these data acording to the Bluetooth core specification. 00697 * 00698 * @note The ECB expect little endian input and output. 00699 * This function expect big endian and will reverse the data as necessary. 00700 * 00701 * @param[in] p_k The key used in the hash function. 00702 * For address resolution this is should be the irk. 00703 * The array must have a length of 16. 00704 * @param[in] p_r The rand used in the hash function. For generating a new address 00705 * this would be a random number. For resolving a resolvable address 00706 * this would be the last half of the address being resolved. 00707 * The array must have a length of 3. 00708 * @param[out] p_local_hash The result of the hash operation. For address resolution this 00709 * will match the first half of the address being resolved if and only 00710 * if the irk used in the hash function is the same one used to generate 00711 * the address. 00712 * The array must have a length of 16. 00713 */ 00714 void ah(uint8_t const * p_k, uint8_t const * p_r, uint8_t * p_local_hash) 00715 { 00716 nrf_ecb_hal_data_t ecb_hal_data; 00717 for (uint32_t i = 0; i < SOC_ECB_KEY_LENGTH; i++) 00718 { 00719 ecb_hal_data.key[i] = p_k[SOC_ECB_KEY_LENGTH - 1 - i]; 00720 } 00721 memset(ecb_hal_data.cleartext, 0, SOC_ECB_KEY_LENGTH - IM_ADDR_CLEARTEXT_LENGTH); 00722 00723 for (uint32_t i = 0; i < IM_ADDR_CLEARTEXT_LENGTH; i++) 00724 { 00725 ecb_hal_data.cleartext[SOC_ECB_KEY_LENGTH - 1 - i] = p_r[i]; 00726 } 00727 00728 sd_ecb_block_encrypt(&ecb_hal_data); 00729 00730 for (uint32_t i = 0; i < IM_ADDR_CIPHERTEXT_LENGTH; i++) 00731 { 00732 p_local_hash[i] = ecb_hal_data.ciphertext[SOC_ECB_KEY_LENGTH - 1 - i]; 00733 } 00734 } 00735 00736 00737 bool im_address_resolve(ble_gap_addr_t const * p_addr, ble_gap_irk_t const * p_irk) 00738 { 00739 if (p_addr->addr_type != BLE_GAP_ADDR_TYPE_RANDOM_PRIVATE_RESOLVABLE) 00740 { 00741 return false; 00742 } 00743 uint8_t hash[IM_ADDR_CIPHERTEXT_LENGTH]; 00744 uint8_t local_hash[IM_ADDR_CIPHERTEXT_LENGTH]; 00745 uint8_t prand[IM_ADDR_CLEARTEXT_LENGTH]; 00746 memcpy(hash, p_addr->addr, IM_ADDR_CIPHERTEXT_LENGTH); 00747 memcpy(prand, &p_addr->addr[IM_ADDR_CIPHERTEXT_LENGTH], IM_ADDR_CLEARTEXT_LENGTH); 00748 ah(p_irk->irk, prand, local_hash); 00749 00750 return (memcmp(hash, local_hash, IM_ADDR_CIPHERTEXT_LENGTH) == 0); 00751 }
Generated on Tue Jul 12 2022 19:00:12 by
![doxygen](doxygen.png)