put bonding to-do flags into Connection objects instead of using a heap-allocated queue

This commit is contained in:
Dan Halbert 2020-01-13 17:52:32 -05:00
parent 9e7f8743c2
commit 4ad004f24e
5 changed files with 83 additions and 182 deletions

View File

@ -46,6 +46,7 @@
#include "shared-bindings/_bleio/Characteristic.h"
#include "shared-bindings/_bleio/Service.h"
#include "shared-bindings/_bleio/UUID.h"
#include "supervisor/shared/tick.h"
#include "common-hal/_bleio/bonding.h"
@ -93,7 +94,6 @@ bool connection_on_ble_evt(ble_evt_t *ble_evt, void *self_in) {
switch (ble_evt->header.evt_id) {
case BLE_GAP_EVT_DISCONNECTED:
CONNECTION_DEBUG_PRINTF("BLE_GAP_EVT_DISCONNECTED\n");
// Adapter.c does the work for this event.
break;
@ -130,7 +130,12 @@ bool connection_on_ble_evt(ble_evt_t *ble_evt, void *self_in) {
self->pair_status == PAIR_PAIRED &&
ble_evt->evt.gatts_evt.params.write.uuid.type == BLE_UUID_TYPE_BLE &&
ble_evt->evt.gatts_evt.params.write.uuid.uuid == 0x2902) {
bonding_save_cccd_info(self->is_central, self->conn_handle, self->ediv);
//
// Save sys_attr data (CCCD state) in bonding area at
// next opportunity, but also remember time of this
// request, so we can consolidate closely-spaced requests.
self->do_bond_cccds = true;
self->do_bond_cccds_request_time = supervisor_ticks_ms64();
}
break;
@ -194,7 +199,6 @@ bool connection_on_ble_evt(ble_evt_t *ble_evt, void *self_in) {
break;
}
case BLE_GAP_EVT_SEC_PARAMS_REQUEST: {
CONNECTION_DEBUG_PRINTF("BLE_GAP_EVT_SEC_PARAMS_REQUEST\n");
// First time pairing.
// 1. Either we or peer initiate the process
// 2. Peer asks for security parameters using BLE_GAP_EVT_SEC_PARAMS_REQUEST.
@ -227,47 +231,40 @@ bool connection_on_ble_evt(ble_evt_t *ble_evt, void *self_in) {
}
case BLE_GAP_EVT_LESC_DHKEY_REQUEST:
CONNECTION_DEBUG_PRINTF("BLE_GAP_EVT_LESC_DHKEY_REQUEST\n");
// TODO for LESC pairing:
// sd_ble_gap_lesc_dhkey_reply(...);
break;
case BLE_GAP_EVT_AUTH_STATUS: { // 0x19
CONNECTION_DEBUG_PRINTF("BLE_GAP_EVT_AUTH_STATUS\n");
// Key exchange completed.
ble_gap_evt_auth_status_t* status = &ble_evt->evt.gap_evt.params.auth_status;
self->sec_status = status->auth_status;
if (status->auth_status == BLE_GAP_SEC_STATUS_SUCCESS) {
self->ediv = self->bonding_keys.own_enc.master_id.ediv;
self->pair_status = PAIR_PAIRED;
CONNECTION_DEBUG_PRINTF("PAIR_PAIRED\n");
bonding_save_keys(self->is_central, self->conn_handle, &self->bonding_keys);
// Save keys in bonding area at next opportunity.
self->do_bond_keys = true;
} else {
// Inform busy-waiter pairing has failed.
self->pair_status = PAIR_NOT_PAIRED;
CONNECTION_DEBUG_PRINTF("PAIR_NOT_PAIRED\n");
}
break;
}
case BLE_GAP_EVT_SEC_INFO_REQUEST: { // 0x14
CONNECTION_DEBUG_PRINTF("BLE_GAP_EVT_SEC_INFO_REQUEST\n");
// Peer asks for the stored keys.
// - load key and return if bonded previously.
// - Else return NULL --> Initiate key exchange
ble_gap_evt_sec_info_request_t* sec_info_request = &ble_evt->evt.gap_evt.params.sec_info_request;
(void) sec_info_request;
if ( bonding_load_keys(self->is_central, sec_info_request->master_id.ediv, &self->bonding_keys) ) {
CONNECTION_DEBUG_PRINTF("keys found\n");
uint32_t err_code = sd_ble_gap_sec_info_reply(
sd_ble_gap_sec_info_reply(
self->conn_handle,
&self->bonding_keys.own_enc.enc_info,
&self->bonding_keys.peer_id.id_info,
NULL);
CONNECTION_DEBUG_PRINTF("sd_ble_gap_sec_info_reply err_code: %0lx\n", err_code);
self->ediv = self->bonding_keys.own_enc.master_id.ediv;
} else {
CONNECTION_DEBUG_PRINTF("keys not found\n");
// We don't have stored keys. Ask for keys.
sd_ble_gap_sec_info_reply(self->conn_handle, NULL, NULL, NULL);
}
@ -277,8 +274,6 @@ bool connection_on_ble_evt(ble_evt_t *ble_evt, void *self_in) {
case BLE_GAP_EVT_CONN_SEC_UPDATE: { // 0x1a
// We get this both on first-time pairing and on subsequent pairings using stored keys.
ble_gap_conn_sec_t* conn_sec = &ble_evt->evt.gap_evt.params.conn_sec_update.conn_sec;
CONNECTION_DEBUG_PRINTF("BLE_GAP_EVT_CONN_SEC_UPDATE, sm: %d, lv: %d\n",
conn_sec->sec_mode.sm, conn_sec->sec_mode.lv);
if (conn_sec->sec_mode.sm <= 1 && conn_sec->sec_mode.lv <= 1) {
// Security setup did not succeed:
// mode 0, level 0 means no access
@ -289,10 +284,8 @@ bool connection_on_ble_evt(ble_evt_t *ble_evt, void *self_in) {
if (bonding_load_cccd_info(self->is_central, self->conn_handle, self->ediv)) {
// Did an sd_ble_gatts_sys_attr_set() with the stored sys_attr values.
// Not quite paired yet: wait for BLE_GAP_EVT_AUTH_STATUS with BLE_GAP_SEC_STATUS_SUCCESS.
CONNECTION_DEBUG_PRINTF("did bonding_load_cccd_info()\n");
} else {
// No matching bonding found, so use fresh system attributes.
CONNECTION_DEBUG_PRINTF("bonding_load_cccd_info() failed\n");
sd_ble_gatts_sys_attr_set(self->conn_handle, NULL, 0, 0);
}
self->pair_status = PAIR_PAIRED;
@ -301,10 +294,8 @@ bool connection_on_ble_evt(ble_evt_t *ble_evt, void *self_in) {
}
default:
CONNECTION_DEBUG_PRINTF("Unhandled event: %04x\n", ble_evt->header.evt_id);
return false;
}
CONNECTION_DEBUG_PRINTF("Handled event: %04x\n", ble_evt->header.evt_id);
return true;
}
@ -590,8 +581,7 @@ STATIC bool discovery_on_ble_evt(ble_evt_t *ble_evt, mp_obj_t payload) {
break;
default:
// For debugging.
// mp_printf(&mp_plat_print, "Unhandled discovery event: 0x%04x\n", ble_evt->header.evt_id);
// CONNECTION_DEBUG_PRINTF(&mp_plat_print, "Unhandled discovery event: 0x%04x\n", ble_evt->header.evt_id);
return false;
break;
}

View File

@ -57,8 +57,8 @@ typedef struct {
// The advertising data and scan response buffers are held by us, not by the SD, so we must
// maintain them and not change it. If we need to change the contents during advertising,
// there are tricks to get the SD to notice (see DevZone - TBS).
// EDIV: Encrypted Diversifier: Identifies LTK during legacy pairing.
bonding_keys_t bonding_keys;
// EDIV: Encrypted Diversifier: Identifies LTK during legacy pairing.
uint16_t ediv;
volatile pair_status_t pair_status;
uint8_t sec_status; // Internal security status.
@ -66,6 +66,13 @@ typedef struct {
ble_drv_evt_handler_entry_t handler_entry;
ble_gap_conn_params_t conn_params;
volatile bool conn_params_updating;
// Request that CCCD info for this connection be saved.
volatile bool do_bond_cccds;
// Request that security key info for this connection be saved.
volatile bool do_bond_keys;
// Time of setting do_bond_ccds: we delay a bit to consolidate multiple CCCD changes
// into one write. Time is currently in ticks_ms.
uint64_t do_bond_cccds_request_time;
} bleio_connection_internal_t;
typedef struct {

View File

@ -36,7 +36,6 @@
#include "supervisor/shared/tick.h"
#include "nrf_soc.h"
#include "sd_mutex.h"
#include "bonding.h"
@ -57,9 +56,6 @@ const uint32_t BONDING_FLAG = ('1' | '0' << 8 | 'D' << 16 | 'B' << 24);
// Save both system and user service info.
#define SYS_ATTR_FLAGS (BLE_GATTS_SYS_ATTR_FLAG_SYS_SRVCS | BLE_GATTS_SYS_ATTR_FLAG_USR_SRVCS)
STATIC nrf_mutex_t queued_bonding_block_entries_mutex;
STATIC uint64_t block_queued_at_ticks_ms = 0;
#if BONDING_DEBUG
void bonding_print_block(bonding_block_t *block) {
printf("at 0x%08lx: is_central: %1d, type: 0x%x, ediv: 0x%04x, data_length: %d\n",
@ -80,7 +76,6 @@ STATIC size_t compute_block_size(uint16_t data_length) {
}
void bonding_erase_storage(void) {
BONDING_DEBUG_PRINTF("bonding_erase_storage()\n");
// Erase all pages in the bonding area.
for(uint32_t page_address = BONDING_PAGES_START_ADDR;
page_address < BONDING_PAGES_END_ADDR;
@ -125,7 +120,7 @@ STATIC bonding_block_t *next_block(bonding_block_t *block) {
// Find the block with given is_central, type and ediv value.
// If type == BLOCK_UNUSED, ediv is ignored and the the sole unused block at the end is returned.
// If not found, return NULL.
STATIC bonding_block_t *find_candidate_block(bool is_central, bonding_block_type_t type, uint16_t ediv) {
STATIC bonding_block_t *find_existing_block(bool is_central, bonding_block_type_t type, uint16_t ediv) {
bonding_block_t *block = NULL;
while (1) {
block = next_block(block);
@ -149,7 +144,7 @@ STATIC bonding_block_t *find_candidate_block(bool is_central, bonding_block_type
// Get an empty block large enough to store data_length data.
STATIC bonding_block_t* find_unused_block(uint16_t data_length) {
bonding_block_t *unused_block = find_candidate_block(true, BLOCK_UNUSED, EDIV_INVALID);
bonding_block_t *unused_block = find_existing_block(true, BLOCK_UNUSED, EDIV_INVALID);
// If no more room, erase all existing blocks and start over.
if (!unused_block ||
(uint8_t *) unused_block + compute_block_size(data_length) >= (uint8_t *) BONDING_DATA_END_ADDR) {
@ -162,56 +157,10 @@ STATIC bonding_block_t* find_unused_block(uint16_t data_length) {
// Set the header word to all 0's, to mark the block as invalid.
// We don't change data_length, so we can still skip over this block.
STATIC void invalidate_block(bonding_block_t *block) {
BONDING_DEBUG_PRINTF("invalidate_block()\n");
uint32_t zero = 0;
sd_flash_write_sync((uint32_t *) block, &zero, 1);
}
STATIC void queue_write_block(bool is_central, bonding_block_type_t type, uint16_t ediv, uint16_t conn_handle, uint8_t *data, uint16_t data_length) {
if (compute_block_size(data_length) > BONDING_DATA_END_ADDR - BONDING_DATA_START_ADDR) {
// Ridiculous size.
return;
}
// No heap available, so never mind. This might be called between VM instantiations.
if (!gc_alloc_possible()) {
return;
}
queued_bonding_block_entry_t* queued_entry =
m_malloc_maybe(sizeof(queued_bonding_block_entry_t) + data_length, false);
if (!queued_entry) {
// Failed to allocate. Not much we can do, since this might be during an evt handler.
return;
}
queued_entry->block.is_central = is_central;
queued_entry->block.type = type;
queued_entry->block.ediv = ediv;
queued_entry->block.conn_handle = conn_handle;
queued_entry->block.data_length = data_length;
if (data && data_length != 0) {
memcpy(&queued_entry->block.data, data, data_length);
}
// Note: blocks are added in LIFO order, for simplicity and speed.
// The assumption is that there won't be stale blocks on the
// list. The sys_attr blocks don't contain sys_attr data, just a
// request to store the latest value. The key blocks are assumed
// not to be superseded quickly. If this assumption becomes
// invalid, the queue should be changed to FIFO.
// Add this new element to the front of the list.
sd_mutex_acquire_wait(&queued_bonding_block_entries_mutex);
queued_entry->next = MP_STATE_VM(queued_bonding_block_entries);
MP_STATE_VM(queued_bonding_block_entries) = queued_entry;
sd_mutex_release(&queued_bonding_block_entries_mutex);
// Remember when we last queued a block, so we avoid excesive
// sys_attr writes.
block_queued_at_ticks_ms = supervisor_ticks_ms64();
}
// Write bonding block header.
STATIC void write_block_header(bonding_block_t *dest_block, bonding_block_t *source_block_header) {
sd_flash_write_sync((uint32_t *) dest_block, (uint32_t *) source_block_header, sizeof(bonding_block_t) / 4);
@ -237,134 +186,109 @@ STATIC void write_block_data(bonding_block_t *dest_block, uint8_t *data, uint16_
}
}
STATIC void write_sys_attr_block(bonding_block_t *block) {
STATIC void write_sys_attr_block(bleio_connection_internal_t *connection) {
uint16_t length = 0;
// First find out how big a buffer we need, then fetch the data.
if(sd_ble_gatts_sys_attr_get(block->conn_handle, NULL, &length, SYS_ATTR_FLAGS) != NRF_SUCCESS) {
if(sd_ble_gatts_sys_attr_get(connection->conn_handle, NULL, &length, SYS_ATTR_FLAGS) != NRF_SUCCESS) {
return;
}
uint8_t sys_attr[length];
if(sd_ble_gatts_sys_attr_get(block->conn_handle, sys_attr, &length, SYS_ATTR_FLAGS) != NRF_SUCCESS) {
if(sd_ble_gatts_sys_attr_get(connection->conn_handle, sys_attr, &length, SYS_ATTR_FLAGS) != NRF_SUCCESS) {
return;
}
// Now we know the data size.
block->data_length = length;
// Is there an existing sys_attr block that matches the current sys_attr data?
bonding_block_t *candidate_block = find_candidate_block(block->is_central, block->type, block->ediv);
if (candidate_block) {
if (length == candidate_block->data_length &&
memcmp(sys_attr, candidate_block->data, block->data_length) == 0) {
BONDING_DEBUG_PRINTF("Identical sys_attr block already stored.\n");
bonding_block_t *existing_block =
find_existing_block(connection->is_central, BLOCK_SYS_ATTR, connection->ediv);
if (existing_block) {
if (length == existing_block->data_length &&
memcmp(sys_attr, existing_block->data, length) == 0) {
// Identical block found. No need to store again.
return;
}
// Data doesn't match. Invalidate block and store a new one.
invalidate_block(candidate_block);
invalidate_block(existing_block);
}
bonding_block_t block_header = {
.is_central = connection->is_central,
.type = BLOCK_SYS_ATTR,
.ediv = connection->ediv,
.conn_handle = connection->conn_handle,
.data_length = length,
};
bonding_block_t *new_block = find_unused_block(length);
write_block_header(new_block, block);
write_block_header(new_block, &block_header);
write_block_data(new_block, sys_attr, length);
return;
}
STATIC void write_keys_block(bonding_block_t *block) {
if (block->data_length != sizeof(bonding_keys_t)) {
// Bad length.
return;
}
STATIC void write_keys_block(bleio_connection_internal_t *connection) {
uint16_t const ediv = connection->is_central
? connection->bonding_keys.peer_enc.master_id.ediv
: connection->bonding_keys.own_enc.master_id.ediv;
// Is there an existing keys block that matches?
bonding_block_t *candidate_block = find_candidate_block(block->is_central, block->type, block->ediv);
if (candidate_block) {
if (block->data_length == candidate_block->data_length &&
memcmp(block->data, candidate_block->data, block->data_length) == 0) {
BONDING_DEBUG_PRINTF("Identical keys block already stored.\n");
bonding_block_t *existing_block = find_existing_block(connection->is_central, BLOCK_KEYS, ediv);
if (existing_block) {
if (existing_block->data_length == sizeof(bonding_keys_t) &&
memcmp(existing_block->data, &connection->bonding_keys, sizeof(bonding_keys_t)) == 0) {
// Identical block found. No need to store again.
return;
}
// Data doesn't match. Invalidate block and store a new one.
invalidate_block(candidate_block);
invalidate_block(existing_block);
}
bonding_keys_t *bonding_keys = (bonding_keys_t *) block->data;
block->ediv = block->is_central
? bonding_keys->peer_enc.master_id.ediv
: bonding_keys->own_enc.master_id.ediv;
bonding_block_t block_header = {
.is_central = connection->is_central,
.type = BLOCK_KEYS,
.ediv = ediv,
.conn_handle = connection->conn_handle,
.data_length = sizeof(bonding_keys_t),
};
bonding_block_t *new_block = find_unused_block(sizeof(bonding_keys_t));
write_block_header(new_block, block);
write_block_data(new_block, (uint8_t *) bonding_keys, sizeof(bonding_keys_t));
write_block_header(new_block, &block_header);
write_block_data(new_block, (uint8_t *) &connection->bonding_keys, sizeof(bonding_keys_t));
}
void bonding_clear_keys(bonding_keys_t *bonding_keys) {
memset((uint8_t*) bonding_keys, 0, sizeof(bonding_keys_t));
}
// Call only when SD is enabled.
void bonding_reset(void) {
MP_STATE_VM(queued_bonding_block_entries) = NULL;
sd_mutex_new(&queued_bonding_block_entries_mutex);
if (BONDING_FLAG != *((uint32_t *) BONDING_START_FLAG_ADDR) ||
BONDING_FLAG != *((uint32_t *) BONDING_END_FLAG_ADDR)) {
bonding_erase_storage();
}
}
// Write bonding blocks to flash. These have been queued during event handlers.
// We do one at a time, on each background call.
// Write bonding blocks to flash. Requests have been queued during evt handlers.
void bonding_background(void) {
uint8_t sd_en = 0;
(void) sd_softdevice_is_enabled(&sd_en);
if (!sd_en) {
return;
}
// A paired connection will request that its keys and CCCD values be stored.
// The CCCD store whenever a CCCD value is written.
for (size_t i = 0; i < BLEIO_TOTAL_CONNECTION_COUNT; i++) {
bleio_connection_internal_t *connection = &connections[i];
if (block_queued_at_ticks_ms == 0) {
// No writes have been queued yet.
return;
}
uint64_t current_ticks_ms = supervisor_ticks_ms64();
// Wait at least one second before saving CCCD, to consolidate
// writes that involve multiple CCCDs. For instance, for HID,
// three CCCD's are set in short succession by the HID client.
if (connection->do_bond_cccds &&
current_ticks_ms - connection->do_bond_cccds_request_time >= 1000) {
write_sys_attr_block(connection);
connection->do_bond_cccds = false;
}
// Wait at least one second before writing a block, to consolidate writes
// that will be duplicates.
uint64_t current_ticks_ms = supervisor_ticks_ms64();
if (current_ticks_ms - block_queued_at_ticks_ms < 1000) {
return;
}
// Get block at front of list.
bonding_block_t *block = NULL;
sd_mutex_acquire_wait(&queued_bonding_block_entries_mutex);
if (MP_STATE_VM(queued_bonding_block_entries)) {
block = &(MP_STATE_VM(queued_bonding_block_entries)->block);
// Remove entry from list.
MP_STATE_VM(queued_bonding_block_entries) = MP_STATE_VM(queued_bonding_block_entries)->next;
}
sd_mutex_release(&queued_bonding_block_entries_mutex);
if (!block) {
// List is empty.
return;
}
switch (block->type) {
case BLOCK_SYS_ATTR:
write_sys_attr_block(block);
break;
case BLOCK_KEYS:
write_keys_block(block);
break;
default:
break;
if (connection->do_bond_keys) {
write_keys_block(connection);
connection->do_bond_keys = false;
}
}
}
bool bonding_load_cccd_info(bool is_central, uint16_t conn_handle, uint16_t ediv) {
bonding_block_t *block = find_candidate_block(is_central, BLOCK_SYS_ATTR, ediv);
bonding_block_t *block = find_existing_block(is_central, BLOCK_SYS_ATTR, ediv);
if (block == NULL) {
return false;
}
@ -374,7 +298,7 @@ bool bonding_load_cccd_info(bool is_central, uint16_t conn_handle, uint16_t ediv
}
bool bonding_load_keys(bool is_central, uint16_t ediv, bonding_keys_t *bonding_keys) {
bonding_block_t *block = find_candidate_block(is_central, BLOCK_KEYS, ediv);
bonding_block_t *block = find_existing_block(is_central, BLOCK_KEYS, ediv);
if (block == NULL) {
return false;
}
@ -386,15 +310,3 @@ bool bonding_load_keys(bool is_central, uint16_t ediv, bonding_keys_t *bonding_k
memcpy(bonding_keys, block->data, block->data_length);
return true;
}
void bonding_save_cccd_info(bool is_central, uint16_t conn_handle, uint16_t ediv) {
BONDING_DEBUG_PRINTF("bonding_save_cccd_info()\n");
queue_write_block(is_central, BLOCK_SYS_ATTR, ediv, conn_handle, NULL, 0);
}
void bonding_save_keys(bool is_central, uint16_t conn_handle, bonding_keys_t *bonding_keys) {
uint16_t const ediv = is_central
? bonding_keys->peer_enc.master_id.ediv
: bonding_keys->own_enc.master_id.ediv;
queue_write_block(is_central, BLOCK_KEYS, ediv, conn_handle, (uint8_t *) bonding_keys, sizeof(bonding_keys_t));
}

View File

@ -48,11 +48,12 @@
#define BONDING_DEBUG_PRINT_KEYS(keys)
#endif
// Bonding data is stored in variable-length blocks consecutively in erased flash.
// The blocks are 32-bit aligned, though the data may be any number of bytes.
// You can hop through the blocks using the size field to find the next block.
// When you hit a word that is all one's, you have reached the end of the blocks.
// You can write a new block there.
// Bonding data is stored in variable-length blocks consecutively in
// erased flash (all 1's). The blocks are 32-bit aligned, though the
// data may be any number of bytes. We hop through the blocks using
// the size field to find the next block. When we hit a word that is
// all 1's, we have reached the end of the blocks. We can write a new
// block there.
typedef enum {
BLOCK_INVALID = 0, // Ignore this block
@ -69,24 +70,16 @@ typedef struct {
uint16_t conn_handle; // Connection handle: used when a BLOCK_SYS_ATTR is queued to write.
// Not used as a key, etc.
uint16_t data_length; // Length of data in bytes, including ediv, not including padding.
// 32-bit boundary here.
// End of block header. 32-bit boundary here.
uint8_t data[]; // Rest of data in the block. Needs to be 32-bit aligned.
// Block is padded to 32-bit alignment.
} bonding_block_t;
// Bonding blocks that need to be written are stored in a linked list.
typedef struct _queued_bonding_block_entry_t {
struct _queued_bonding_block_entry_t *next;
bonding_block_t block; // variable length, based on data_length.
} queued_bonding_block_entry_t;
void bonding_background(void);
void bonding_erase_storage(void);
void bonding_reset(void);
void bonding_clear_keys(bonding_keys_t *bonding_keys);
bool bonding_load_cccd_info(bool is_central, uint16_t conn_handle, uint16_t ediv);
bool bonding_load_keys(bool is_central, uint16_t ediv, bonding_keys_t *bonding_keys);
void bonding_save_cccd_info(bool is_central, uint16_t conn_handle, uint16_t ediv);
void bonding_save_keys(bool is_central, uint16_t conn_handle, bonding_keys_t *bonding_keys);
#endif // MICROPY_INCLUDED_NRF_COMMON_HAL_BLEIO_BONDING_H

View File

@ -156,7 +156,6 @@
#define MICROPY_PORT_ROOT_POINTERS \
CIRCUITPY_COMMON_ROOT_POINTERS \
ble_drv_evt_handler_entry_t* ble_drv_evt_handler_entries; \
queued_bonding_block_entry_t* queued_bonding_block_entries; \
#endif // NRF5_MPCONFIGPORT_H__