wip: HID bonding works!

7 files changed, 170 insertions, 91 deletions

diff --git a/ports/nrf/common-hal/_bleio/Adapter.c b/ports/nrf/common-hal/_bleio/Adapter.c
index 05cd0260b..8c732e73b 100644
--- a/ports/nrf/common-hal/_bleio/Adapter.c
+++ b/ports/nrf/common-hal/_bleio/Adapter.c
@@ -224,6 +224,7 @@ STATIC bool adapter_on_ble_evt(ble_evt_t *ble_evt, void *self_in) {
             }
             ble_drv_remove_event_handler(connection_on_ble_evt, connection);
             connection->conn_handle = BLE_CONN_HANDLE_INVALID;
+            connection->pair_status = PAIR_NOT_PAIRED;
             if (connection->connection_obj != mp_const_none) {
                 bleio_connection_obj_t* obj = connection->connection_obj;
                 obj->connection = NULL;
@@ -657,6 +658,10 @@ mp_obj_t common_hal_bleio_adapter_get_connections(bleio_adapter_obj_t *self) {
     return self->connection_objs;
 }
 
+void common_hal_bleio_adapter_erase_bonding(bleio_adapter_obj_t *self) {
+    bonding_erase_storage();
+}
+
 void bleio_adapter_gc_collect(bleio_adapter_obj_t* adapter) {
     gc_collect_root((void**)adapter, sizeof(bleio_adapter_obj_t) / sizeof(size_t));
     gc_collect_root((void**)connections, sizeof(connections) / sizeof(size_t));
diff --git a/ports/nrf/common-hal/_bleio/Connection.c b/ports/nrf/common-hal/_bleio/Connection.c
index 6c49823c2..664805b66 100644
--- a/ports/nrf/common-hal/_bleio/Connection.c
+++ b/ports/nrf/common-hal/_bleio/Connection.c
@@ -54,7 +54,7 @@
 #define BLE_AD_TYPE_FLAGS_DATA_SIZE 1
 
 static const ble_gap_sec_params_t pairing_sec_params = {
-    .bond = 0,
+    .bond = 1,
     .mitm = 0,
     .lesc = 0,
     .keypress = 0,
@@ -66,6 +66,13 @@ static const ble_gap_sec_params_t pairing_sec_params = {
     .kdist_peer   = { .enc = 1, .id = 1},
 };
 
+#define CONNECTION_DEBUG (1)
+#if CONNECTION_DEBUG
+  #define CONNECTION_DEBUG_PRINTF(...) printf(__VA_ARGS__)
+#else
+  #define CONNECTION_DEBUG_PRINTF(...)
+#endif
+
 static volatile bool m_discovery_in_process;
 static volatile bool m_discovery_successful;
 
@@ -86,7 +93,10 @@ 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;
+
         case BLE_GAP_EVT_PHY_UPDATE_REQUEST: {
             ble_gap_phys_t const phys = {
                 .rx_phys = BLE_GAP_PHY_AUTO,
@@ -173,10 +183,11 @@ 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.
-            // 3. Pair Key exchange ("just works" implemented now; TODO key pairing)
+            // 3. Pair Key exchange ("just works" implemented now; TODO: out-of-band key pairing)
             // 4. Connection is secured: BLE_GAP_EVT_CONN_SEC_UPDATE
             // 5. Long-term Keys exchanged: BLE_GAP_EVT_AUTH_STATUS
 
@@ -205,43 +216,55 @@ 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");
             // Pairing process 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);
             } 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) ) {
-                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("keys found\n");
+                uint32_t err_code = 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);
             }
             break;
         }
 
         case BLE_GAP_EVT_CONN_SEC_UPDATE: { // 0x1a
+            CONNECTION_DEBUG_PRINTF("BLE_GAP_EVT_CONN_SEC_UPDATE\n");
             ble_gap_conn_sec_t* conn_sec = &ble_evt->evt.gap_evt.params.conn_sec_update.conn_sec;
             if (conn_sec->sec_mode.sm <= 1 && conn_sec->sec_mode.lv <= 1) {
                 // Security setup did not succeed:
@@ -250,12 +273,13 @@ bool connection_on_ble_evt(ble_evt_t *ble_evt, void *self_in) {
                 // mode >=1 and/or level >=1 means encryption is set up
                 self->pair_status = PAIR_NOT_PAIRED;
             } else {
-                // Does an sd_ble_gatts_sys_attr_set() with the stored values.
                 if (bonding_load_cccd_info(self->is_central, self->conn_handle, self->ediv)) {
-                    // Not quite paired yet: wait for BLE_GAP_EVT_AUTH_STATUS SUCCESS.
-                    self->ediv = self->bonding_keys.own_enc.master_id.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);
                 }
             }
@@ -263,8 +287,10 @@ 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;
 }
 
diff --git a/ports/nrf/common-hal/_bleio/bonding.c b/ports/nrf/common-hal/_bleio/bonding.c
index 1175f72e4..0476d722b 100644
--- a/ports/nrf/common-hal/_bleio/bonding.c
+++ b/ports/nrf/common-hal/_bleio/bonding.c
@@ -43,14 +43,12 @@
 #define BONDING_PAGES_START_ADDR CIRCUITPY_BLE_CONFIG_START_ADDR
 #define BONDING_PAGES_END_ADDR   (CIRCUITPY_BLE_CONFIG_START_ADDR + CIRCUITPY_BLE_CONFIG_SIZE)
 
-// First and last four bytes are magic bytes for id and version. Start data after that.
+// First and last four bytes are magic bytes for id and version. Data is in between.
 // 'BD01'
-const uint32_t BONDING_START_FLAG = ('1' | '0' << 8 | 'D' << 16 | 'B' << 24);
-// 'ED01'
-const uint32_t BONDING_END_FLAG = ('1' | '0' << 8 | 'D' << 16 | 'E' << 24);
+const uint32_t BONDING_FLAG = ('1' | '0' << 8 | 'D' << 16 | 'B' << 24);
 
-#define BONDING_DATA_START_ADDR (BONDING_PAGES_START_ADDR + sizeof(BONDING_START_FLAG))
-#define BONDING_DATA_END_ADDR (BONDING_PAGES_END_ADDR - sizeof(BONDING_END_FLAG))
+#define BONDING_DATA_START_ADDR (BONDING_PAGES_START_ADDR + sizeof(BONDING_FLAG))
+#define BONDING_DATA_END_ADDR (BONDING_PAGES_END_ADDR - sizeof(BONDING_FLAG))
 
 #define BONDING_START_FLAG_ADDR BONDING_PAGES_START_ADDR
 #define BONDING_END_FLAG_ADDR BONDING_DATA_END_ADDR
@@ -59,14 +57,28 @@ const uint32_t BONDING_END_FLAG = ('1' | '0' << 8 | 'D' << 16 | 'E' << 24);
 #define SYS_ATTR_FLAGS      (BLE_GATTS_SYS_ATTR_FLAG_SYS_SRVCS | BLE_GATTS_SYS_ATTR_FLAG_USR_SRVCS)
 
 STATIC bonding_block_t *bonding_unused_block = NULL;
-nrf_mutex_t queued_bonding_block_list_mutex;
+nrf_mutex_t queued_bonding_block_entries_mutex;
 
-STATIC inline size_t compute_block_size(uint16_t data_length) {
+#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",
+               (uint32_t) block, block->is_central, block->type, block->ediv, block->data_length);
+}
+
+void bonding_print_keys(bonding_keys_t *keys) {
+    for (size_t i = 0; i < sizeof(bonding_keys_t); i ++) {
+        printf("%x", ((uint8_t*) keys)[i]);
+    }
+    printf("\n");
+}
+#endif
+
+STATIC size_t compute_block_size(uint16_t data_length) {
     // Round data size up to the nearest 32-bit address.
-    return sizeof(bonding_block_t) + ((data_length + 3) & 0x3);
+    return sizeof(bonding_block_t) + ((data_length + 3) & ~0x3);
 }
 
-STATIC void erase_bonding_storage(void) {
+void bonding_erase_storage(void) {
     // Erase all pages in the bonding area.
     for(uint32_t page_address = BONDING_PAGES_START_ADDR;
         page_address < BONDING_PAGES_END_ADDR;
@@ -75,10 +87,9 @@ STATIC void erase_bonding_storage(void) {
         sd_flash_page_erase_sync(page_address / FLASH_PAGE_SIZE);
     }
     // Write marker words at the beginning and the end of the bonding area.
-    uint32_t flag = BONDING_START_FLAG;
-    sd_flash_write_sync((uint32_t *) BONDING_DATA_START_ADDR, &flag, 1);
-    flag = BONDING_END_FLAG;
-    sd_flash_write_sync((uint32_t *) BONDING_DATA_END_ADDR, &flag, 1);
+    uint32_t flag = BONDING_FLAG;
+    sd_flash_write_sync((uint32_t *) BONDING_START_FLAG_ADDR, &flag, 1);
+    sd_flash_write_sync((uint32_t *) BONDING_END_FLAG_ADDR, &flag, 1);
     // First unused block is at the beginning.
     bonding_unused_block = (bonding_block_t *) BONDING_DATA_START_ADDR;
 }
@@ -90,7 +101,6 @@ STATIC bonding_block_t *next_block(bonding_block_t *block) {
     while (1) {
         // Advance to next block.
         if (block == NULL) {
-            // Return first block (which might be unused if block list is empty).
             return (bonding_block_t *) BONDING_DATA_START_ADDR;
         } else if (block->type == BLOCK_UNUSED) {
             // Already at last block (the unused block).
@@ -115,20 +125,26 @@ STATIC bonding_block_t *next_block(bonding_block_t *block) {
 // Find the block with given 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_block(bool is_central, bonding_block_type_t type, uint16_t ediv) {
+STATIC bonding_block_t *find_block_with_keys(bool is_central, bonding_block_type_t type, uint16_t ediv) {
     bonding_block_t *block = NULL;
+    BONDING_DEBUG_PRINTF("find_block_with_keys(): looking through blocks:\n");
     while (1) {
         block = next_block(block);
         if (block == NULL) {
             return NULL;
         }
-        if (block->type == BLOCK_UNUSED) {
-            return block;
+        BONDING_DEBUG_PRINT_BLOCK(block);
+        if (block->type == BLOCK_INVALID) {
+            // Skip discarded blocks.
+            continue;
         }
-        if (is_central == block->is_central &&
-            type == block->type &&
-            ediv == block->ediv) {
+        // If types match, and block is unused, just return it.
+        // Otherwise check that is_central and ediv match.
+        if (type == block->type) {
+            if (type == BLOCK_UNUSED ||
+                (is_central == block->is_central && ediv == block->ediv)) {
             return block;
+            }
         }
     }
 }
@@ -137,6 +153,7 @@ STATIC bonding_block_t *find_block(bool is_central, bonding_block_type_t type, u
 // 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");
+    BONDING_DEBUG_PRINT_BLOCK(block);
     uint32_t zero = 0;
     sd_flash_write_sync((uint32_t *) block, &zero, 1);
 }
@@ -147,29 +164,28 @@ STATIC void queue_write_block(bool is_central, bonding_block_type_t type, uint16
         return;
     }
 
-    queued_bonding_block_list_elt_t* queued_elt =
-        m_malloc_maybe(sizeof(queued_bonding_block_list_elt_t) + data_length, false);
+    queued_bonding_block_entry_t* queued_entry =
+        m_malloc_maybe(sizeof(queued_bonding_block_entry_t) + data_length, false);
 
-    if (!queued_elt) {
+    if (!queued_entry) {
         // Failed to allocate. Not much we can do, since this might be during an evt handler.
         return;
     }
 
-    // Add this new element to the front of the list.
-    sd_mutex_acquire_wait(&queued_bonding_block_list_mutex);
-    queued_elt->next_queued_block = MP_STATE_VM(queued_bonding_block_list);
-    MP_STATE_VM(queued_bonding_block_list) = queued_elt;
-    sd_mutex_release(&queued_bonding_block_list_mutex);
-
-    //
-    queued_elt->bonding_block.is_central = is_central;
-    queued_elt->bonding_block.type = type;
-    queued_elt->bonding_block.ediv = ediv;
-    queued_elt->bonding_block.conn_handle = conn_handle;
-    queued_elt->bonding_block.data_length = data_length;
+    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_elt->bonding_block.data, data, data_length);
+        memcpy(&queued_entry->block.data, data, data_length);
     }
+
+    // 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);
 }
 
 // Write bonding block header.
@@ -178,7 +194,7 @@ STATIC void write_block_header(bonding_block_t *block) {
     if (bonding_unused_block == NULL ||
         (uint8_t *) bonding_unused_block + compute_block_size(block->data_length) >=
         (uint8_t *)BONDING_DATA_END_ADDR) {
-        erase_bonding_storage();
+        bonding_erase_storage();
     }
 
     sd_flash_write_sync((uint32_t *) bonding_unused_block, (uint32_t *) block, sizeof(bonding_block_t) / 4);
@@ -198,6 +214,7 @@ STATIC void write_block_data(uint8_t *data, uint16_t data_length) {
             break;
         }
         data_length -= 4;
+        data += 4;
         // Increment by word size.
         flash_word_p++;
     }
@@ -231,10 +248,10 @@ STATIC bool write_keys_block(bonding_block_t *block) {
     }
 
     bonding_keys_t *bonding_keys = (bonding_keys_t *) block->data;
+    BONDING_DEBUG_PRINT_KEYS(bonding_keys);
     block->ediv = block->is_central
         ? bonding_keys->peer_enc.master_id.ediv
         : bonding_keys->own_enc.master_id.ediv;
-
     write_block_header(block);
     write_block_data((uint8_t *) bonding_keys, sizeof(bonding_keys_t));
     return true;
@@ -247,13 +264,13 @@ void bonding_clear_keys(bonding_keys_t *bonding_keys) {
 
 // Call only when SD is enabled.
 void bonding_reset(void) {
-    MP_STATE_VM(queued_bonding_block_list) = NULL;
-    sd_mutex_new(&queued_bonding_block_list_mutex);
-    if (BONDING_START_FLAG != *((uint32_t *) BONDING_START_FLAG_ADDR) ||
-        BONDING_END_FLAG != *((uint32_t *) BONDING_END_FLAG_ADDR)) {
-        erase_bonding_storage();
+    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();
     } else {
-        bonding_unused_block = find_block(true, BLOCK_UNUSED, EDIV_INVALID);
+        bonding_unused_block = find_block_with_keys(true, BLOCK_UNUSED, EDIV_INVALID);
     }
 }
 
@@ -266,29 +283,34 @@ void bonding_background(void) {
         return;
     }
     // Get block at front of list.
-    sd_mutex_acquire_wait(&queued_bonding_block_list_mutex);
-    bonding_block_t *block = (bonding_block_t *) MP_STATE_VM(queued_bonding_block_list);
-    if (block) {
-        // Remove block from list.
-        MP_STATE_VM(queued_bonding_block_list) = MP_STATE_VM(queued_bonding_block_list)->next_queued_block;
+    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_list_mutex);
+    sd_mutex_release(&queued_bonding_block_entries_mutex);
     if (!block) {
         return;
     }
 
     // Is there an existing block whose keys match?
-    bonding_block_t *matching_block = find_block(block->is_central, block->type, block->ediv);
-    if (matching_block) {
-        if (block->data_length == matching_block->data_length &&
-            memcmp(block->data, matching_block->data, block->data_length) == 0) {
+    BONDING_DEBUG_PRINTF("bonding_background(): processing queued block:\n");
+    BONDING_DEBUG_PRINT_BLOCK(block);
+    bonding_block_t *block_with_keys = find_block_with_keys(block->is_central, block->type, block->ediv);
+    if (block_with_keys) {
+        BONDING_DEBUG_PRINTF("bonding_background(): block with same keys found:\n");
+        BONDING_DEBUG_PRINT_BLOCK(block_with_keys);
+        if (block->data_length == block_with_keys->data_length &&
+            memcmp(block->data, block_with_keys->data, block->data_length) == 0) {
             // Identical block found. No need to store again.
-            BONDING_DEBUG_PRINTF("bonding_background(): identical block found\n");
+            BONDING_DEBUG_PRINTF("bonding_background(): block is identical to block_with_keys\n");
             return;
         }
         // Block keys match but data doesn't. Invalidate block and store a new one.
-        BONDING_DEBUG_PRINTF("bonding_background(): invalidating block\n");
-        invalidate_block(matching_block);
+        BONDING_DEBUG_PRINTF("bonding_background(): invalidating block_with_keys\n");
+        invalidate_block(block_with_keys);
     }
 
     switch (block->type) {
@@ -301,26 +323,27 @@ void bonding_background(void) {
             break;
 
         default:
+            BONDING_DEBUG_PRINTF("unknown block type: %x\n", block->type);
             break;
     }
 }
 
 bool bonding_load_cccd_info(bool is_central, uint16_t conn_handle, uint16_t ediv) {
-    bonding_block_t *block = find_block(is_central, BLOCK_SYS_ATTR, ediv);
+    bonding_block_t *block = find_block_with_keys(is_central, BLOCK_SYS_ATTR, ediv);
     if (block == NULL) {
-        BONDING_DEBUG_PRINTF("bonding_load_cccd_info(): block not found\n");
+        BONDING_DEBUG_PRINTF("bonding_load_cccd_info(): block not found, ediv: %04x\n", ediv);
         return false;
     }
 
-    BONDING_DEBUG_PRINTF("bonding_load_cccd_info(): block found\n");
+    BONDING_DEBUG_PRINTF("bonding_load_cccd_info(): block found, ediv: %04x\n", ediv);
     return NRF_SUCCESS ==
         sd_ble_gatts_sys_attr_set(conn_handle, block->data, block->data_length, SYS_ATTR_FLAGS);
 }
 
 bool bonding_load_keys(bool is_central, uint16_t ediv, bonding_keys_t *bonding_keys) {
-    bonding_block_t *block = find_block(is_central, BLOCK_SYS_ATTR, ediv);
+    bonding_block_t *block = find_block_with_keys(is_central, BLOCK_KEYS, ediv);
     if (block == NULL) {
-        BONDING_DEBUG_PRINTF("bonding_load_keys(): block not found\n");
+        BONDING_DEBUG_PRINTF("bonding_load_keys(): block not found, ediv: %04x\n", ediv);
         return false;
     }
     if (sizeof(bonding_keys_t) != block->data_length) {
@@ -328,18 +351,23 @@ bool bonding_load_keys(bool is_central, uint16_t ediv, bonding_keys_t *bonding_k
         return false;
     }
 
-    BONDING_DEBUG_PRINTF("bonding_load_keys(): block found\n");
+    BONDING_DEBUG_PRINTF("bonding_load_keys(): block found, ediv: %04x\n", ediv);
     memcpy(bonding_keys, block->data, block->data_length);
+    BONDING_DEBUG_PRINT_KEYS(bonding_keys);
     return true;
 }
 
 void bonding_save_cccd_info(bool is_central, uint16_t conn_handle, uint16_t ediv) {
-    queue_write_block(is_central, ediv, conn_handle, BLOCK_SYS_ATTR, NULL, 0);
+    BONDING_DEBUG_PRINTF("bonding_save_cccd_info: is_central: %d, conn_handle: %04x, ediv: %04x\n",
+                         is_central, conn_handle, ediv);
+    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;
+    BONDING_DEBUG_PRINTF("bonding_save_keys: is_central: %d, conn_handle: %04x, ediv: %04x\n",
+                         is_central, conn_handle, ediv);
     queue_write_block(is_central, BLOCK_KEYS, ediv, conn_handle, (uint8_t *) bonding_keys, sizeof(bonding_keys_t));
 }
diff --git a/ports/nrf/common-hal/_bleio/bonding.h b/ports/nrf/common-hal/_bleio/bonding.h
index 1b0083865..35347af2a 100644
--- a/ports/nrf/common-hal/_bleio/bonding.h
+++ b/ports/nrf/common-hal/_bleio/bonding.h
@@ -40,8 +40,12 @@
 #define BONDING_DEBUG (1)
 #if BONDING_DEBUG
   #define BONDING_DEBUG_PRINTF(...) printf(__VA_ARGS__)
+  #define BONDING_DEBUG_PRINT_BLOCK(block) bonding_print_block(block)
+  #define BONDING_DEBUG_PRINT_KEYS(keys) bonding_print_keys(keys)
 #else
   #define BONDING_DEBUG_PRINTF(...)
+  #define BONDING_DEBUG_PRINT_BLOCK(block)
+  #define BONDING_DEBUG_PRINT_KEYS(keys)
 #endif
 
 // Bonding data is stored in variable-length blocks consecutively in erased flash.
@@ -71,12 +75,13 @@ typedef struct {
 } bonding_block_t;
 
 // Bonding blocks that need to be written are stored in a linked list.
-typedef struct _queued_bonding_block_list_elt_t {
-    struct _queued_bonding_block_list_elt_t *next_queued_block;
-    bonding_block_t bonding_block;  // variable length, based on data_length.
-} queued_bonding_block_list_elt_t;
+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);
diff --git a/ports/nrf/mpconfigport.h b/ports/nrf/mpconfigport.h
index 14f7632fd..a480ec1e0 100644
--- a/ports/nrf/mpconfigport.h
+++ b/ports/nrf/mpconfigport.h
@@ -156,7 +156,7 @@
 #define MICROPY_PORT_ROOT_POINTERS \
     CIRCUITPY_COMMON_ROOT_POINTERS \
     ble_drv_evt_handler_entry_t* ble_drv_evt_handler_entries; \
-    queued_bonding_block_list_elt_t* queued_bonding_block_list; \
+    queued_bonding_block_entry_t* queued_bonding_block_entries; \
 
 
 #endif  // NRF5_MPCONFIGPORT_H__
diff --git a/shared-bindings/_bleio/Adapter.c b/shared-bindings/_bleio/Adapter.c
index 5d3211b0c..500055cf3 100644
--- a/shared-bindings/_bleio/Adapter.c
+++ b/shared-bindings/_bleio/Adapter.c
@@ -360,22 +360,35 @@ STATIC mp_obj_t bleio_adapter_connect(mp_uint_t n_args, const mp_obj_t *pos_args
 }
 STATIC MP_DEFINE_CONST_FUN_OBJ_KW(bleio_adapter_connect_obj, 2, bleio_adapter_connect);
 
+//|   .. method:: erase_bonding()
+//|
+//|     Erase all bonding information stored in flash memory.
+STATIC mp_obj_t bleio_adapter_erase_bonding(mp_obj_t self_in) {
+    bleio_adapter_obj_t *self = MP_OBJ_TO_PTR(self_in);
+
+    common_hal_bleio_adapter_erase_bonding(self);
+
+    return mp_const_none;
+}
+STATIC MP_DEFINE_CONST_FUN_OBJ_1(bleio_adapter_erase_bonding_obj, bleio_adapter_erase_bonding);
 
 STATIC const mp_rom_map_elem_t bleio_adapter_locals_dict_table[] = {
     { MP_ROM_QSTR(MP_QSTR_enabled), MP_ROM_PTR(&bleio_adapter_enabled_obj) },
     { MP_ROM_QSTR(MP_QSTR_address), MP_ROM_PTR(&bleio_adapter_address_obj) },
-    { MP_ROM_QSTR(MP_QSTR_name), MP_ROM_PTR(&bleio_adapter_name_obj) },
+    { MP_ROM_QSTR(MP_QSTR_name),    MP_ROM_PTR(&bleio_adapter_name_obj) },
 
-    { MP_ROM_QSTR(MP_QSTR_start_advertising),        MP_ROM_PTR(&bleio_adapter_start_advertising_obj) },
-    { MP_ROM_QSTR(MP_QSTR_stop_advertising),         MP_ROM_PTR(&bleio_adapter_stop_advertising_obj) },
+    { MP_ROM_QSTR(MP_QSTR_start_advertising), MP_ROM_PTR(&bleio_adapter_start_advertising_obj) },
+    { MP_ROM_QSTR(MP_QSTR_stop_advertising),  MP_ROM_PTR(&bleio_adapter_stop_advertising_obj) },
 
     { MP_ROM_QSTR(MP_QSTR_start_scan), MP_ROM_PTR(&bleio_adapter_start_scan_obj) },
-    { MP_ROM_QSTR(MP_QSTR_stop_scan), MP_ROM_PTR(&bleio_adapter_stop_scan_obj) },
+    { MP_ROM_QSTR(MP_QSTR_stop_scan),  MP_ROM_PTR(&bleio_adapter_stop_scan_obj) },
 
-    { MP_ROM_QSTR(MP_QSTR_connect), MP_ROM_PTR(&bleio_adapter_connect_obj) },
+    { MP_ROM_QSTR(MP_QSTR_connect),    MP_ROM_PTR(&bleio_adapter_connect_obj) },
 
-    { MP_ROM_QSTR(MP_QSTR_connected), MP_ROM_PTR(&bleio_adapter_connected_obj) },
+    { MP_ROM_QSTR(MP_QSTR_connected),   MP_ROM_PTR(&bleio_adapter_connected_obj) },
     { MP_ROM_QSTR(MP_QSTR_connections), MP_ROM_PTR(&bleio_adapter_connections_obj) },
+
+    { MP_ROM_QSTR(MP_QSTR_erase_bonding), MP_ROM_PTR(&bleio_adapter_erase_bonding_obj) },
 };
 
 STATIC MP_DEFINE_CONST_DICT(bleio_adapter_locals_dict, bleio_adapter_locals_dict_table);
diff --git a/shared-bindings/_bleio/Adapter.h b/shared-bindings/_bleio/Adapter.h
index 4340d82c1..9b20a461a 100644
--- a/shared-bindings/_bleio/Adapter.h
+++ b/shared-bindings/_bleio/Adapter.h
@@ -48,13 +48,15 @@ extern void common_hal_bleio_adapter_set_name(bleio_adapter_obj_t *self, const c
 extern uint32_t _common_hal_bleio_adapter_start_advertising(bleio_adapter_obj_t *self, bool connectable, float interval, uint8_t *advertising_data, uint16_t advertising_data_len, uint8_t *scan_response_data, uint16_t scan_response_data_len);
 
 extern void common_hal_bleio_adapter_start_advertising(bleio_adapter_obj_t *self, bool connectable, mp_float_t interval, mp_buffer_info_t *advertising_data_bufinfo, mp_buffer_info_t *scan_response_data_bufinfo);
-void common_hal_bleio_adapter_stop_advertising(bleio_adapter_obj_t *self);
+extern void common_hal_bleio_adapter_stop_advertising(bleio_adapter_obj_t *self);
 
-mp_obj_t common_hal_bleio_adapter_start_scan(bleio_adapter_obj_t *self, uint8_t* prefixes, size_t prefix_length, bool extended, mp_int_t buffer_size, mp_float_t timeout, mp_float_t interval, mp_float_t window, mp_int_t minimum_rssi, bool active);
-void common_hal_bleio_adapter_stop_scan(bleio_adapter_obj_t *self);
+extern mp_obj_t common_hal_bleio_adapter_start_scan(bleio_adapter_obj_t *self, uint8_t* prefixes, size_t prefix_length, bool extended, mp_int_t buffer_size, mp_float_t timeout, mp_float_t interval, mp_float_t window, mp_int_t minimum_rssi, bool active);
+extern void common_hal_bleio_adapter_stop_scan(bleio_adapter_obj_t *self);
 
-bool common_hal_bleio_adapter_get_connected(bleio_adapter_obj_t *self);
-mp_obj_t common_hal_bleio_adapter_get_connections(bleio_adapter_obj_t *self);
-mp_obj_t common_hal_bleio_adapter_connect(bleio_adapter_obj_t *self, bleio_address_obj_t *address, mp_float_t timeout);
+extern bool common_hal_bleio_adapter_get_connected(bleio_adapter_obj_t *self);
+extern mp_obj_t common_hal_bleio_adapter_get_connections(bleio_adapter_obj_t *self);
+extern mp_obj_t common_hal_bleio_adapter_connect(bleio_adapter_obj_t *self, bleio_address_obj_t *address, mp_float_t timeout);
+
+extern void common_hal_bleio_adapter_erase_bonding(bleio_adapter_obj_t *self);
 
 #endif // MICROPY_INCLUDED_SHARED_BINDINGS_BLEIO_ADAPTER_H