Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Fork of nrf51-sdk by
Revision 4:ee5f0a37316e, committed 2016-04-06
- Comitter:
- LancasterUniversity
- Date:
- Wed Apr 06 23:55:25 2016 +0100
- Parent:
- 3:ae5e20530aa5
- Child:
- 5:f2faa0f0b8f0
- Commit message:
- Synchronized with git rev 62ad4f4e
Author: James Devine
nrf51-sdk : rough first draft of working BLE STACK
added correct IRK matching which softdevice doesn't do correctly.
Fixed whitelisting from adding IRK and Address.
Changed in this revision
| source/nordic_sdk/components/ble/device_manager/device_manager_peripheral.c | Show annotated file Show diff for this revision Revisions of this file |
--- a/source/nordic_sdk/components/ble/device_manager/device_manager_peripheral.c Wed Apr 06 23:55:24 2016 +0100
+++ b/source/nordic_sdk/components/ble/device_manager/device_manager_peripheral.c Wed Apr 06 23:55:25 2016 +0100
@@ -209,7 +209,7 @@
* @defgroup api_param_check API Parameters check macros.
*
* @details Macros for verifying parameters passed to the module in the APIs. These macros
- * could be mapped to nothing in the final version of the code in order to save execution
+ * could be mapped to nothing in the final version of the code in order to save execution
* time and program size.
* @{
*/
@@ -374,7 +374,7 @@
/**@brief Peer identification information.
*/
typedef struct
-{
+{
ble_gap_id_key_t peer_id; /**< IRK and/or address of peer. */
uint16_t ediv; /**< Peer's encrypted diversifier. */
uint8_t id_bitmap; /**< Contains information if above field is valid. */
@@ -614,11 +614,11 @@
static __INLINE void connection_instance_init(uint32_t index)
{
DM_TRC("[DM]: Initializing Connection Instance 0x%08X.\r\n", index);
-
+
m_connection_table[index].state = STATE_IDLE;
m_connection_table[index].conn_handle = BLE_CONN_HANDLE_INVALID;
m_connection_table[index].bonded_dev_id = DM_INVALID_ID;
-
+
memset(&m_connection_table[index].peer_addr, 0, sizeof (ble_gap_addr_t));
}
@@ -630,7 +630,7 @@
static __INLINE void peer_instance_init(uint32_t index)
{
DM_TRC("[DM]: Initializing Peer Instance 0x%08X.\r\n", index);
-
+
memset(m_peer_table[index].peer_id.id_addr_info.addr, 0, BLE_GAP_ADDR_LEN);
memset(m_peer_table[index].peer_id.id_info.irk, 0, BLE_GAP_SEC_KEY_LEN);
@@ -721,9 +721,9 @@
for (index = 0; index < DEVICE_MANAGER_MAX_BONDS; index++)
{
- DM_TRC("[DM]:[DI 0x%02X]: Device type 0x%02X.\r\n",
+ printf("[DM]:[DI 0x%02X]: Device type 0x%02X.\r\n",
index, m_peer_table[index].peer_id.id_addr_info.addr_type);
- DM_TRC("[DM]: Device Addr 0x%02X 0x%02X 0x%02X 0x%02X 0x%02X 0x%02X.\r\n",
+ printf("[DM]: Device Addr 0x%02X 0x%02X 0x%02X 0x%02X 0x%02X 0x%02X.\r\n",
m_peer_table[index].peer_id.id_addr_info.addr[0],
m_peer_table[index].peer_id.id_addr_info.addr[1],
m_peer_table[index].peer_id.id_addr_info.addr[2],
@@ -746,8 +746,8 @@
(*p_device_index) = index;
err_code = NRF_SUCCESS;
- DM_LOG("[DM]: Allocated device instance 0x%02X\r\n", index);
-
+ printf("[DM]: Allocated device instance 0x%02X\r\n", index);
+
break;
}
}
@@ -801,10 +801,10 @@
uint32_t index;
err_code = NRF_ERROR_NOT_FOUND;
-
+
if (NULL != p_addr)
{
- DM_TRC("[DM]: Searching for device 0x%02X 0x%02X 0x%02X 0x%02X 0x%02X 0x%02X.\r\n",
+ printf("[DM]: Searching for device 0x%02X 0x%02X 0x%02X 0x%02X 0x%02X 0x%02X.\r\n",
p_addr->addr[0],
p_addr->addr[1],
p_addr->addr[2],
@@ -815,9 +815,9 @@
for (index = 0; index < DEVICE_MANAGER_MAX_BONDS; index++)
{
- DM_TRC("[DM]:[DI 0x%02X]: Device type 0x%02X.\r\n",
+ printf("[DM]:[DI 0x%02X]: Device type 0x%02X.\r\n",
index, m_peer_table[index].peer_id.id_addr_info.addr_type);
- DM_TRC("[DM]: Device Addr 0x%02X 0x%02X 0x%02X 0x%02X 0x%02X 0x%02X.\r\n",
+ printf("[DM]: Device Addr 0x%02X 0x%02X 0x%02X 0x%02X 0x%02X 0x%02X.\r\n",
m_peer_table[index].peer_id.id_addr_info.addr[0],
m_peer_table[index].peer_id.id_addr_info.addr[1],
m_peer_table[index].peer_id.id_addr_info.addr[2],
@@ -878,18 +878,18 @@
{
uint32_t err_code;
- DM_TRC("[DM]: Request to allocation connection instance\r\n");
+ printf("[DM]: Request to allocation connection instance\r\n");
err_code = connection_instance_find(BLE_CONN_HANDLE_INVALID, STATE_IDLE, p_instance);
if (err_code == NRF_SUCCESS)
{
- DM_LOG("[DM]:[%02X]: Connection Instance Allocated.\r\n", (*p_instance));
+ printf("[DM]:[%02X]: Connection Instance Allocated.\r\n", (*p_instance));
m_connection_table[*p_instance].state = STATE_CONNECTED;
}
else
{
- DM_LOG("[DM]: No free connection instances available\r\n");
+ printf("[DM]: No free connection instances available\r\n");
err_code = NRF_ERROR_NO_MEM;
}
@@ -904,11 +904,11 @@
*/
static __INLINE void connection_instance_free(uint32_t const * p_instance)
{
- DM_TRC("[DM]:[CI 0x%02X]: Freeing connection instance\r\n", (*p_instance));
+ printf("[DM]:[CI 0x%02X]: Freeing connection instance\r\n", (*p_instance));
if (m_connection_table[*p_instance].state != STATE_IDLE)
{
- DM_LOG("[DM]:[%02X]: Freed connection instance.\r\n", (*p_instance));
+ printf("[DM]:[%02X]: Freed connection instance.\r\n", (*p_instance));
connection_instance_init(*p_instance);
}
}
@@ -959,21 +959,21 @@
(m_connection_table[p_handle->connection_id].state & STATE_BOND_INFO_UPDATE)) ||
(state == UPDATE_PEER_ADDR))
{
- DM_LOG("[DM]:[DI %02X]:[CI %02X]: -> Updating bonding information.\r\n",
+ printf("[DM]:[DI %02X]:[CI %02X]: -> Updating bonding information.\r\n",
p_handle->device_id, p_handle->connection_id);
store_fn = pstorage_update;
}
else if (state == FIRST_BOND_STORE)
{
- DM_LOG("[DM]:[DI %02X]:[CI %02X]: -> Storing bonding information.\r\n",
+ printf("[DM]:[DI %02X]:[CI %02X]: -> Storing bonding information.\r\n",
p_handle->device_id, p_handle->connection_id);
store_fn = pstorage_store;
}
else
{
- DM_LOG("[DM]:[DI %02X]:[CI %02X]: -> No update in bonding information.\r\n",
+ printf("[DM]:[DI %02X]:[CI %02X]: -> No update in bonding information.\r\n",
p_handle->device_id, p_handle->connection_id);
//No operation needed.
@@ -1715,20 +1715,22 @@
// In case a peer device could not be loaded successfully, rest of the
// initialization procedure are skipped and an error is sent to the
// application.
- DM_ERR(
+ printf(
"[DM]: Failed to load peer device %08X from storage, reason %08X.\r\n",
index,
err_code);
+
+
m_module_initialized = false;
break;
}
else
{
- DM_TRC("[DM]:[DI 0x%02X]: Device type 0x%02X.\r\n",
+ printf("[DM]:[DI 0x%02X]: Device type 0x%02X.\r\n",
index,
m_peer_table[index].peer_id.id_addr_info.addr_type);
- DM_TRC("[DM]: Device Addr 0x%02X 0x%02X 0x%02X 0x%02X 0x%02X 0x%02X.\r\n",
+ printf("[DM]: Device Addr 0x%02X 0x%02X 0x%02X 0x%02X 0x%02X 0x%02X.\r\n",
m_peer_table[index].peer_id.id_addr_info.addr[0],
m_peer_table[index].peer_id.id_addr_info.addr[1],
m_peer_table[index].peer_id.id_addr_info.addr[2],
@@ -1741,7 +1743,7 @@
{
//In case a peer device could not be loaded successfully, rest of the
//initialization procedure are skipped and an error is sent to the application.
- DM_LOG("[DM]: Failed to get block handle for instance %08X, reason %08X.\r\n",
+ printf("[DM]: Failed to get block handle for instance %08X, reason %08X.\r\n",
index,
err_code);
@@ -1761,6 +1763,16 @@
DM_ERR("[DM]: Failed to register with storage module, reason 0x%08X.\r\n", err_code);
}
+ uint32_t irkCount = 0;
+
+ for (uint32_t index = DEVICE_MANAGER_MAX_BONDS; index > 0; index--)
+ if((m_peer_table[index].id_bitmap & IRK_ENTRY) == 0 && (m_peer_table[index].id_bitmap & ADDR_ENTRY) == 1)
+ {
+ printf("[0x%02X]Index %d is an IRK, placing %d\r\n", m_peer_table[index].id_bitmap,index,irkCount);
+ m_irk_index_table[irkCount] = index;
+ irkCount++;
+ }
+
DM_MUTEX_UNLOCK();
DM_TRC("[DM]: << dm_init.\r\n");
@@ -1912,8 +1924,9 @@
if (connected == false)
{
+ printf("irkentry %d addrentry %d", (m_peer_table[index].id_bitmap & IRK_ENTRY), (m_peer_table[index].id_bitmap & ADDR_ENTRY));
if ((irk_count < p_whitelist->irk_count) &&
- ((m_peer_table[index].id_bitmap & IRK_ENTRY) == 0))
+ ((m_peer_table[index].id_bitmap & IRK_ENTRY) == 0) && (m_peer_table[index].id_bitmap & ADDR_ENTRY) != 0)
{
p_whitelist->pp_irks[irk_count] = &m_peer_table[index].peer_id.id_info;
m_irk_index_table[irk_count] = index;
@@ -1932,7 +1945,7 @@
p_whitelist->addr_count = addr_count;
p_whitelist->irk_count = irk_count;
- DM_LOG("[DM]: Created whitelist, number of IRK = 0x%02X, number of addr = 0x%02X\r\n",
+ printf("[DM]: Created whitelist, number of IRK = 0x%02X, number of addr = 0x%02X\r\n",
irk_count,
addr_count);
@@ -2490,7 +2503,7 @@
&block_handle);
if (err_code == NRF_SUCCESS)
{
- DM_LOG(
+ printf(
"[DM]:[%02X]:[Block ID 0x%08X]:Loading bond information at %p, size 0x%08X, offset 0x%08X.\r\n",
p_handle->connection_id,
block_handle.block_id,
@@ -2505,12 +2518,12 @@
if (err_code != NRF_SUCCESS)
{
- DM_ERR("[DM]:[%02X]: Failed to load Bond information, reason %08X\r\n",
+ printf("[DM]:[%02X]: Failed to load Bond information, reason %08X\r\n",
p_handle->connection_id,
err_code);
}
- DM_LOG(
+ printf(
"[DM]:[%02X]:Loading service context at %p, size 0x%08X, offset 0x%08X.\r\n",
p_handle->connection_id,
&m_gatts_table[p_handle->connection_id],
@@ -2523,7 +2536,7 @@
if (err_code != NRF_SUCCESS)
{
- DM_ERR(
+ printf(
"[DM]:[%02X]: Failed to load service information, reason %08X\r\n",
p_handle->connection_id,
err_code);
@@ -2531,11 +2544,21 @@
}
else
{
- DM_ERR("[DM]:[%02X]: Failed to get block identifier for "
+ printf("[DM]:[%02X]: Failed to get block identifier for "
"device %08X, reason %08X.\r\n", p_handle->connection_id, p_handle->device_id, err_code);
}
}
+int address_to_device_id(ble_gap_addr_t const * p_addr)
+{
+ for(int peer_iterator = 0; peer_iterator < DEVICE_MANAGER_MAX_BONDS; peer_iterator++)
+ {
+ if(im_address_resolve(p_addr, &m_peer_table[peer_iterator].peer_id.id_info))
+ return peer_iterator;
+ }
+
+ return DM_INVALID_ID;
+}
void dm_ble_evt_handler(ble_evt_t * p_ble_evt)
{
@@ -2595,23 +2618,23 @@
m_connection_table[index].peer_addr =
p_ble_evt->evt.gap_evt.params.connected.peer_addr;
- if (p_ble_evt->evt.gap_evt.params.connected.irk_match == 1)
+ if ((device_index = address_to_device_id(&p_ble_evt->evt.gap_evt.params.connected.peer_addr)) != DM_INVALID_ID)
{
- if (m_irk_index_table[p_ble_evt->evt.gap_evt.params.connected.irk_match_idx] != DM_INVALID_ID)
- {
- device_index = im_peer_id_get_by_irk_match_idx(p_ble_evt->evt.gap_evt.params.connected.irk_match_idx);
- err_code = NRF_SUCCESS;
- }
+ err_code = NRF_SUCCESS;
+
+ printf("CONNECT: matched, addr_to_did %d\r\n", device_index);
}
else
{
//Use the device address to check if the device exists in the bonded device list.
err_code = device_instance_find(&p_ble_evt->evt.gap_evt.params.connected.peer_addr,
&device_index, EDIV_INIT_VAL);
+ printf("CONNECT: not matched, status: %d found manually: %d\r\n", err_code, device_index);
}
if (err_code == NRF_SUCCESS)
{
+ printf("CONNECT: SUCCESS!\r\n");
m_connection_table[index].bonded_dev_id = device_index;
m_connection_table[index].state |= STATE_BONDED;
handle.device_id = device_index;
@@ -2619,12 +2642,15 @@
bond_data_load(&handle);
}
}
+
+ printf("CONNECT: did %d bdid %d\r\n", handle.device_id, m_connection_table[index].bonded_dev_id);
+
break;
case BLE_GAP_EVT_DISCONNECTED:
//Disconnection could be peer or self initiated hence disconnecting and connecting
//both states are permitted, however, connection handle must be known.
- DM_LOG("[DM]: Disconnect Reason 0x%04X\r\n",
+ printf("[DM]: Disconnect Reason 0x%04X\r\n",
p_ble_evt->evt.gap_evt.params.disconnected.reason);
m_connection_table[index].state &= (~STATE_CONNECTED);
@@ -2654,11 +2680,13 @@
break;
case BLE_GAP_EVT_SEC_INFO_REQUEST:
- DM_LOG("[DM]: >> BLE_GAP_EVT_SEC_INFO_REQUEST\r\n");
+ printf("[DM]: >> BLE_GAP_EVT_SEC_INFO_REQUEST\r\n");
//If the device is already bonded, respond with existing info, else NULL.
if (m_connection_table[index].bonded_dev_id == DM_INVALID_ID)
{
+ printf("INVALID ID \r\n");
+
//Find device based on div.
err_code = device_instance_find(NULL,&device_index, p_ble_evt->evt.gap_evt.params.sec_info_request.master_id.ediv);
if (err_code == NRF_SUCCESS)
@@ -2673,6 +2701,7 @@
if (m_connection_table[index].bonded_dev_id != DM_INVALID_ID)
{
+ printf("VALID ID index %d bdid %d\r\n",index, m_connection_table[index].bonded_dev_id);
p_enc_info = &m_bond_table[index].peer_enc_key.enc_info;
DM_DUMP((uint8_t *)p_enc_info, sizeof(ble_gap_enc_info_t));
}
@@ -2684,7 +2713,7 @@
if (err_code != NRF_SUCCESS)
{
- DM_ERR("[DM]:[CI %02X]:[DI %02X]: Security information response failed, reason "
+ printf("[DM]:[CI %02X]:[DI %02X]: Security information response failed, reason "
"0x%08X\r\n", index, m_connection_table[index].bonded_dev_id, err_code);
}
break;
@@ -2692,13 +2721,26 @@
case BLE_GAP_EVT_SEC_PARAMS_REQUEST:
DM_LOG("[DM]: >> BLE_GAP_EVT_SEC_PARAMS_REQUEST\r\n");
+ printf("SEC_PARAM irkm: %d idx: %d \r\n",p_ble_evt->evt.gap_evt.params.connected.irk_match, p_ble_evt->evt.gap_evt.params.connected.irk_match_idx);
+
event.event_id = DM_EVT_SECURITY_SETUP;
m_connection_table[index].state |= STATE_PAIRING;
notify_app = true;
+ if ((device_index = address_to_device_id(&p_ble_evt->evt.gap_evt.params.connected.peer_addr)) != DM_INVALID_ID)
+ {
+ handle.device_id = device_index;
+ m_connection_table[index].bonded_dev_id = device_index;
+ err_code = NRF_SUCCESS;
+
+ printf("REBONDING bdid %d, did %d, conn %d\r\n",m_connection_table[index].bonded_dev_id, handle.device_id, index);
+ }
+
if (m_connection_table[index].bonded_dev_id == DM_INVALID_ID)
{
+ printf("allocating new \r\n");
+
//Assign a peer index as a new bond or update existing bonds.
err_code = device_instance_allocate((uint8_t *)&device_index,
&m_connection_table[index].peer_addr);
@@ -2706,20 +2748,22 @@
//Allocation successful.
if (err_code == NRF_SUCCESS)
{
- DM_LOG("[DM]:[CI 0x%02X]:[DI 0x%02X]: Bonded!\r\n",index, device_index);
+
+ printf("[DM]:[CI 0x%02X]:[DI 0x%02X]: Bonded!\r\n",index, device_index);
handle.device_id = device_index;
m_connection_table[index].bonded_dev_id = device_index;
}
else
{
- DM_LOG("[DM]: Security parameter request failed, reason 0x%08X.\r\n", err_code);
+ printf("[DM]: Security parameter request failed, reason 0x%08X.\r\n", err_code);
event_result = err_code;
notify_app = true;
}
}
else
{
+ printf("refresh\r\n");
//Bond/key refresh.
event.event_id = DM_EVT_SECURITY_SETUP_REFRESH;
memset(m_gatts_table[index].attributes, 0, DM_GATT_SERVER_ATTR_MAX_SIZE);
@@ -2728,6 +2772,8 @@
m_connection_table[index].state |= STATE_BOND_INFO_UPDATE;
}
+ printf("FINAL [DM]:[CI 0x%02X]:[DI 0x%02X]: Bonded!\r\n",index, device_index);
+
ble_gap_sec_keyset_t keys_exchanged;
DM_LOG("[DM]: 0x%02X, 0x%02X, 0x%02X, 0x%02X\r\n",
@@ -2737,7 +2783,7 @@
p_ble_evt->evt.gap_evt.params.sec_params_request.peer_params.bond);
keys_exchanged.keys_central.p_enc_key = NULL;
- keys_exchanged.keys_central.p_id_key = &m_peer_table[m_connection_table[index].bonded_dev_id].peer_id;
+ keys_exchanged.keys_central.p_id_key = &m_peer_table[m_connection_table[index].bonded_dev_id].peer_id;
keys_exchanged.keys_central.p_sign_key = NULL;
keys_exchanged.keys_periph.p_enc_key = &m_bond_table[index].peer_enc_key;
keys_exchanged.keys_periph.p_id_key = NULL;
@@ -2745,7 +2791,7 @@
err_code = sd_ble_gap_sec_params_reply(p_ble_evt->evt.gap_evt.conn_handle,
BLE_GAP_SEC_STATUS_SUCCESS,
- &m_application_table[0].sec_param,
+ &m_application_table[0].sec_param,
&keys_exchanged);
if (err_code != NRF_SUCCESS)
@@ -2768,6 +2814,7 @@
if (p_ble_evt->evt.gap_evt.params.auth_status.auth_status != BLE_GAP_SEC_STATUS_SUCCESS)
{
+ printf("bond failed\r\n");
// Free the allocation as bonding failed.
ret_code_t result = device_instance_free(m_connection_table[index].bonded_dev_id);
(void) result;
@@ -2775,6 +2822,7 @@
}
else
{
+ printf("bonding\r\n");
DM_DUMP((uint8_t *)&p_ble_evt->evt.gap_evt.params.auth_status,
sizeof(ble_gap_evt_auth_status_t));
DM_DUMP((uint8_t *)&m_bond_table[index], sizeof(bond_context_t));
@@ -2788,11 +2836,13 @@
//IRK and/or public address is shared, update it.
if (p_ble_evt->evt.gap_evt.params.auth_status.kdist_central.id == 1)
{
+ printf("shared\r\n");
m_peer_table[handle.device_id].id_bitmap &= (~IRK_ENTRY);
}
if (m_connection_table[index].bonded_dev_id != DM_INVALID_ID)
{
+ printf("bonded index %d bdid %d\r\n", index,m_connection_table[index].bonded_dev_id);
DM_LOG("[DM]:[CI 0x%02X]:[DI 0x%02X]: Bonded!\r\n",
index,
handle.device_id);
@@ -2800,6 +2850,8 @@
if (m_connection_table[index].peer_addr.addr_type !=
BLE_GAP_ADDR_TYPE_RANDOM_PRIVATE_RESOLVABLE)
{
+ printf("not RPR\r\n");
+
m_peer_table[handle.device_id].peer_id.id_addr_info =
m_connection_table[index].peer_addr;
m_peer_table[handle.device_id].id_bitmap &= (~ADDR_ENTRY);
@@ -2809,6 +2861,7 @@
}
else
{
+ printf("PR\r\n");
// Here we must fetch the keys from the keyset distributed.
m_peer_table[handle.device_id].ediv = m_bond_table[index].peer_enc_key.master_id.ediv;
m_peer_table[handle.device_id].id_bitmap &= (~IRK_ENTRY);
@@ -2837,7 +2890,7 @@
{
//Lost bond case, generate a security refresh event!
memset(m_gatts_table[index].attributes, 0, DM_GATT_SERVER_ATTR_MAX_SIZE);
-
+
event.event_id = DM_EVT_SECURITY_SETUP_REFRESH;
m_connection_table[index].state |= STATE_PAIRING_PENDING;
m_connection_table[index].state |= STATE_BOND_INFO_UPDATE;
@@ -2854,7 +2907,7 @@
if (err_code != NRF_SUCCESS)
{
DM_ERR("[DM]:[CI 0x%02X]:[DI 0x%02X]: Failed to apply service context\r\n",
- handle.connection_id,
+ handle.connection_id,
handle.device_id);
event_result = DM_SERVICE_CONTEXT_NOT_APPLIED;
@@ -2862,7 +2915,7 @@
}
event_result = NRF_SUCCESS;
notify_app = true;
-
+
break;
case BLE_GATTS_EVT_SYS_ATTR_MISSING:
@@ -2874,14 +2927,14 @@
case BLE_GAP_EVT_SEC_REQUEST:
DM_LOG("[DM]: >> BLE_GAP_EVT_SEC_REQUEST\r\n");
-
+
//Verify if the device is already bonded, and if it is bonded, initiate encryption.
- //If the device is not bonded, an instance needs to be allocated in order to initiate
- //bonding. The application have to initiate the procedure, the module will not do this
+ //If the device is not bonded, an instance needs to be allocated in order to initiate
+ //bonding. The application have to initiate the procedure, the module will not do this
//automatically.
event.event_id = DM_EVT_SECURITY_SETUP;
notify_app = true;
-
+
break;
default:
