Sukkin Pang
Initial release
Fork of
nrf51-sdk
by 
Lancaster University
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: