wip; compiles

9 files changed, 274 insertions, 129 deletions

diff --git a/ports/nrf/Makefile b/ports/nrf/Makefile
index e6070fc32..92985e4e9 100755
--- a/ports/nrf/Makefile
+++ b/ports/nrf/Makefile
@@ -73,8 +73,6 @@ INC += -I$(BUILD)
 INC += -I$(BUILD)/genhdr
 INC += -I./../../lib/cmsis/inc
 INC += -I./boards/$(BOARD)
-INC += -I./modules/ubluepy
-INC += -I./modules/ble
 INC += -I./nrfx
 INC += -I./nrfx/hal
 INC += -I./nrfx/mdk
@@ -156,6 +154,7 @@ SRC_C += \
 	boards/$(BOARD)/pins.c \
 	device/$(MCU_VARIANT)/startup_$(MCU_SUB_VARIANT).c \
 	bluetooth/ble_drv.c \
+	common-hal/_bleio/bonding.c \
 	lib/libc/string0.c \
 	lib/mp-readline/readline.c \
 	lib/oofatfs/ff.c \
diff --git a/ports/nrf/background.c b/ports/nrf/background.c
index 629967b3d..966c56e0b 100644
--- a/ports/nrf/background.c
+++ b/ports/nrf/background.c
@@ -43,6 +43,7 @@
 
 #if CIRCUITPY_BLEIO
 #include "supervisor/shared/bluetooth.h"
+#include "common-hal/_bleio/bonding.h"
 #endif
 
 static bool running_background_tasks = false;
@@ -68,6 +69,7 @@ void run_background_tasks(void) {
 
 #if CIRCUITPY_BLEIO
     supervisor_bluetooth_background();
+    bonding_background();
 #endif
 
     #if CIRCUITPY_DISPLAYIO
diff --git a/ports/nrf/common-hal/_bleio/Adapter.c b/ports/nrf/common-hal/_bleio/Adapter.c
index cee40a5a0..12d509073 100644
--- a/ports/nrf/common-hal/_bleio/Adapter.c
+++ b/ports/nrf/common-hal/_bleio/Adapter.c
@@ -32,6 +32,7 @@
 
 #include "ble.h"
 #include "ble_drv.h"
+#include "bonding.h"
 #include "nrfx_power.h"
 #include "nrf_nvic.h"
 #include "nrf_sdm.h"
@@ -669,4 +670,5 @@ void bleio_adapter_reset(bleio_adapter_obj_t* adapter) {
         bleio_connection_internal_t *connection = &connections[i];
         connection->connection_obj = mp_const_none;
     }
+    bonding_reset();
 }
diff --git a/ports/nrf/common-hal/_bleio/Characteristic.c b/ports/nrf/common-hal/_bleio/Characteristic.c
index f80c52ae4..81639898f 100644
--- a/ports/nrf/common-hal/_bleio/Characteristic.c
+++ b/ports/nrf/common-hal/_bleio/Characteristic.c
@@ -33,6 +33,7 @@
 #include "shared-bindings/_bleio/Service.h"
 
 #include "common-hal/_bleio/Adapter.h"
+#include "common-hal/_bleio/bonding.h"
 
 STATIC uint16_t characteristic_get_cccd(uint16_t cccd_handle, uint16_t conn_handle) {
     uint16_t cccd;
@@ -92,9 +93,11 @@ STATIC bool characteristic_on_ble_evt(ble_evt_t *ble_evt, void *param) {
                 bleio_connection_obj_t *connection = self->service->connection;
                 uint16_t conn_handle = bleio_connection_get_conn_handle(connection);
                 if (conn_handle != BLE_CONN_HANDLE_INVALID &&
-                    connection->pairing_status == PAIR_PAIRED &&
-                    ble_evt->gatts_evt.params.write.handle == self->cccd_handle) {
-                    bonding_save_cccd_later(connection->is_central, conn_handle, connection->ediv);
+                    common_hal_bleio_connection_get_paired(connection) &&
+                    ble_evt->evt.gatts_evt.params.write.handle == self->cccd_handle) {
+                    bonding_save_cccd_info(
+                        connection->connection->is_central, conn_handle, connection->connection->ediv);
+                }
             }
             break;
         }
diff --git a/ports/nrf/common-hal/_bleio/Connection.c b/ports/nrf/common-hal/_bleio/Connection.c
index 725a9a11a..6c49823c2 100644
--- a/ports/nrf/common-hal/_bleio/Connection.c
+++ b/ports/nrf/common-hal/_bleio/Connection.c
@@ -47,6 +47,8 @@
 #include "shared-bindings/_bleio/Service.h"
 #include "shared-bindings/_bleio/UUID.h"
 
+#include "common-hal/_bleio/bonding.h"
+
 #define BLE_ADV_LENGTH_FIELD_SIZE   1
 #define BLE_ADV_AD_TYPE_FIELD_SIZE  1
 #define BLE_AD_TYPE_FLAGS_DATA_SIZE 1
@@ -212,7 +214,7 @@ bool connection_on_ble_evt(ble_evt_t *ble_evt, void *self_in) {
             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 = bonding_keys->own_enc.master_id.ediv;
+                self->ediv = self->bonding_keys.own_enc.master_id.ediv;
                 self->pair_status = PAIR_PAIRED;
                 bonding_save_keys(self->is_central, self->conn_handle, &self->bonding_keys);
             } else {
@@ -227,13 +229,12 @@ bool connection_on_ble_evt(ble_evt_t *ble_evt, void *self_in) {
             // - 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;
-            bond_keys bond_keys_t;
             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
+                sd_ble_gap_sec_info_reply(self->conn_handle,
                                           &self->bonding_keys.own_enc.enc_info,
                                           &self->bonding_keys.peer_id.id_info,
                                           NULL);
-                self->ediv = bond_keys.own_enc.master_id.ediv;
+                self->ediv = self->bonding_keys.own_enc.master_id.ediv;
             } else {
                 sd_ble_gap_sec_info_reply(self->conn_handle, NULL, NULL, NULL);
             }
@@ -249,12 +250,10 @@ 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 {
-                uint8_t *sys_attr;
-                uint16_t sys_attr_len;
-                if (bonding_load_cccd_info(self->is_central, self->conn_handle, self->ediv, sys_attr, sys_attr_len)) {
-                    sd_ble_gatts_sys_attr_set(self->conn_handle, sys_attr, sys_attr_len, SVC_CONTEXT_FLAG);
-                // Not quite paired yet: wait for BLE_GAP_EVT_AUTH_STATUS SUCCESS.
-                self->ediv = self->bonding_keys.own_enc.master_id.ediv;
+                // 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;
                 } else {
                     // No matching bonding found, so use fresh system attributes.
                     sd_ble_gatts_sys_attr_set(self->conn_handle, NULL, 0, 0);
@@ -263,10 +262,6 @@ bool connection_on_ble_evt(ble_evt_t *ble_evt, void *self_in) {
             break;
         }
 
-        case BLE_GATTS_EVT_WRITE: {
-            if (self->pair_status == PAIR_PAIRED) &&
-
-
         default:
             return false;
     }
@@ -278,7 +273,7 @@ void bleio_connection_clear(bleio_connection_internal_t *self) {
 
     self->conn_handle = BLE_CONN_HANDLE_INVALID;
     self->pair_status = PAIR_NOT_PAIRED;
-    bonding_clear_keys(self);
+    bonding_clear_keys(&self->bonding_keys);
 }
 
 bool common_hal_bleio_connection_get_paired(bleio_connection_obj_t *self) {
diff --git a/ports/nrf/common-hal/_bleio/__init__.h b/ports/nrf/common-hal/_bleio/__init__.h
index ecacf7c85..e216795fc 100644
--- a/ports/nrf/common-hal/_bleio/__init__.h
+++ b/ports/nrf/common-hal/_bleio/__init__.h
@@ -30,9 +30,9 @@
 void bleio_reset(void);
 
 typedef struct {
-  ble_gap_enc_key_t own_enc;
-  ble_gap_enc_key_t peer_enc;
-  ble_gap_id_key_t peer_id;
+    ble_gap_enc_key_t own_enc;
+    ble_gap_enc_key_t peer_enc;
+    ble_gap_id_key_t peer_id;
 } bonding_keys_t;
 
 // We assume variable length data.
diff --git a/ports/nrf/common-hal/_bleio/bonding.c b/ports/nrf/common-hal/_bleio/bonding.c
index c717d0816..1675ade43 100644
--- a/ports/nrf/common-hal/_bleio/bonding.c
+++ b/ports/nrf/common-hal/_bleio/bonding.c
@@ -34,6 +34,9 @@
 #include "shared-bindings/_bleio/Adapter.h"
 #include "shared-bindings/nvm/ByteArray.h"
 
+#include "nrf_nvmc.h"
+#include "sd_mutex.h"
+
 #include "bonding.h"
 
 // Internal flash area reserved for bonding storage.
@@ -42,97 +45,64 @@
 
 // First and last four bytes are magic bytes for id and version. Start data after that.
 // 'BD01'
-const uint32_t BONDING_START_FLAG ('1' | '0' << 8 | 'D' << 16 | 'B' << 24);
+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_END_FLAG = ('1' | '0' << 8 | 'D' << 16 | 'E' << 24);
 
-#define BONDING_DATA_START_ADDR (BONDING_DATA_START_ADDR + sizeof(BONDING_START_BYTES))
-#define BONDING_DATA_END_ADDR (BONDING_PAGES_END_ADDR - sizeof(BONDING_END_BYTES))
+#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_START_FLAG_ADDR BONDING_DATA_START_ADDR
+#define BONDING_START_FLAG_ADDR BONDING_PAGES_START_ADDR
 #define BONDING_END_FLAG_ADDR BONDING_DATA_END_ADDR
 
-// 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.
-
-typdef enum {
-    BLOCK_INVALID = 0,      // Ignore this block
-    BLOCK_KEYS = 1,         // Block contains bonding keys.
-    BLOCK_SYS_ATTR = 2,     // Block contains sys_attr values (CCCD settings, etc.).
-    BLOCK_UNUSED = 0xff,    // Initial erased value.
-} bonding_block_type_t;
-
-typedef struct {
-    uint16_t central:1;            // 1 if data is for a central role.
-    uint16_t reserved: 7;          // Not currently used
-    bonding_block_type_t type: 8;  // What kind of data is stored in.
-    uint16_t ediv;                 // ediv value; used as a lookup key.
-    uint32_t data_length;          // Length of data in bytes, including ediv, not including padding.
-                                   // data_length only needs to be 16 bits, but easier to write a word at a time.
-    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;
-
-STATIC inline size_t bonding_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);
+// 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 bonding_block_t *bonding_unused_block = NULL;
+nrf_mutex_t queued_bonding_block_list_mutex;
 
-void bonding_clear_keys(bonding_keys_t *bonding_keys) {
-    memset(bonding_keys, 0, sizeof(bonding_keys));
+STATIC inline 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);
 }
 
-STATIC void bonding_erase_storage(void) {
+STATIC void erase_bonding_storage(void) {
     // Erase all pages in the bonding area.
+    BONDING_DEBUG_PRINTF("erase_bonding_storage()\n");
     for(uint32_t page_address = BONDING_PAGES_START_ADDR;
         page_address < BONDING_PAGES_END_ADDR;
-        page_address += FLASH_PAGE) {
+        page_address += FLASH_PAGE_SIZE) {
         nrf_nvmc_page_erase(page_address);
     }
     // Write marker words at the beginning and the end of the bonding area.
     nrf_nvmc_write_word(BONDING_DATA_START_ADDR, BONDING_START_FLAG_ADDR);
     nrf_nvmc_write_word(BONDING_DATA_END_ADDR, BONDING_END_FLAG_ADDR);
     // First unused block is at the beginning.
-    bonding_unused_block = BONDING_DATA_START_ADDR;
-}
-
-STATIC bonding_block_t *bonding_unused_block = NULL;
-
-void bonding_init(void) {
-    if (BONDING_START_BYTES != *((uint32_t *) BONDING_START_FLAG_ADDR) ||
-        BONDING_END_BYTES != *((uint32_t *) BONDING_END_FLAG_ADDR)) {
-        bonding_erase_storage();
-    } else {
-        bonding_unused_block = bonding_find_block(BLOCK_UNUSED, EDIV_INVALID);
-    }
+    bonding_unused_block = (bonding_block_t *) BONDING_DATA_START_ADDR;
 }
 
 // Given NULL to start or block address, return the address of the next valid block.
 // The last block returned is the unused block at the end.
 // Return NULL if we have run off the end of the bonding space.
-STATIC bonding_block_t *bonding_next_block(bonding_block_t *block) {
+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_DATA_START_ADDR;
+            return (bonding_block_t *) BONDING_DATA_START_ADDR;
         } else if (block->type == BLOCK_UNUSED) {
             // Already at last block (the unused block).
             return NULL;
         }
 
         // Advance to next block.
-        block += (bonding_block_t *) ((uint8_t *) block + bonding_block_word_size(block->data_length));
+        block = (bonding_block_t *) ((uint8_t *) block + compute_block_size(block->data_length));
 
-        }
         if (block >= (bonding_block_t *) BONDING_DATA_END_ADDR) {
             // Went past end of bonding space.
             return NULL;
         }
-        if (block.valid) {
+        if (block->type != BLOCK_INVALID) {
             // Found an empty or a valid block.
             return block;
         }
@@ -143,17 +113,19 @@ STATIC bonding_block_t *bonding_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 *bonding_find_block(bonding_block_type_t type, uint16_t ediv) {
+STATIC bonding_block_t *find_block(bool is_central, bonding_block_type_t type, uint16_t ediv) {
     bonding_block_t *block = NULL;
     while (1) {
-        block = bonding_next_block(block);
+        block = next_block(block);
         if (block == NULL) {
             return NULL;
         }
-        if (block.type == BLOCK_UNUSED) {
+        if (block->type == BLOCK_UNUSED) {
             return block;
         }
-        if (type == block.type && ediv = block.ediv) {
+        if (is_central == block->is_central &&
+            type == block->type &&
+            ediv == block->ediv) {
             return block;
         }
     }
@@ -161,80 +133,207 @@ STATIC bonding_block_t *bonding_find_block(bonding_block_type_t type, uint16_t e
 
 // 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 bonding_invalidate_block(bonding_block_t *block) {
-    nrf_nvmc_write_word((uint32_t) bonding_unused_block, 0x00000000);
+STATIC void invalidate_block(bonding_block_t *block) {
+    BONDING_DEBUG_PRINTF("invalidate_block()\n");
+    nrf_nvmc_write_word((uint32_t) block, 0x00000000);
 }
 
-// Try to write a new block. If no room, erase everything and start again.
-// TODO: could do garbage collection instead.
-STATIC bool bonding_write_block(bonding_block_type_t type, uint16_t ediv, uint8_t *data, uint16_t data_length) {
-    size_t block_size = bonding_block_word_size(data_length);
-    if (block_size > BONDING_DATA_END_ADDR - BONDING_DATA_START_ADDR) {
+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 false;
+        return;
+    }
+
+    queued_bonding_block_list_elt_t* queued_elt =
+        m_malloc_maybe(sizeof(queued_bonding_block_list_elt_t) + data_length, false);
+
+    if (!queued_elt) {
+        // Failed to allocate. Not much we can do, since this might be during an evt handler.
+        return;
     }
 
-    // No more room. Erase all existing bonding info and start over.
-    if (bonding_unused_block == NULL || bonding_unused_block + block_size >= BONDING_DATA_END_ADDR) {
-        bonding_erase_storage();
+    // 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;
+    if (data && data_length != 0) {
+        memcpy(&queued_elt->bonding_block.data, data, data_length);
     }
+}
 
-    bonding_block_t block_without_data;
-    block_without_data.valid = 1;
-    block_without_data.type = type;
-    block_without_data.ediv = ediv;
-    block_without_data.data_length = data_length;
+// Write bonding block header.
+STATIC void write_block_header(bonding_block_t *block) {
+    // If no more room, erase all existing blocks and start over.
+    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();
+    }
 
-    // Write header data.
-    nrf_nvmc_write_words((uint32_t) bonding_unused_block, (uint32_t *) &block_without_data,
-                         sizeof(block_without_data) / 4);
+    nrf_nvmc_write_words((uint32_t) bonding_unused_block, (uint32_t *) block, sizeof(bonding_block_t) / 4);
+}
 
-    // Write variable-length data.
+// Write variable-length data at end of bonding block.
+STATIC void write_block_data(uint8_t *data, uint16_t data_length) {
     // Minimize the number of writes. Datasheet says no more than two writes per word before erasing again.
-    uint32_t *word_p = (uint32_t) bonding_unused_block + sizeof(block_without_data);
+
+    // Start writing after the current header.
+    uint32_t *flash_word_p = (uint32_t *) ((uint8_t *) bonding_unused_block + sizeof(bonding_block_t));
     while (1) {
         uint32_t word = 0xffffffff;
         memcpy(&word, data, data_length >= 4 ? 4 : data_length);
-        nrf_nvmc_write_word(word_p, word);
+        nrf_nvmc_write_word((uint32_t) flash_word_p, word);
         if (data_length <= 4) {
             break;
         }
         data_length -= 4;
-        word_p++;
+        // Increment by word size.
+        flash_word_p++;
+    }
+    bonding_unused_block = (bonding_block_t *) flash_word_p;
+}
+
+STATIC bool write_sys_attr_block(bonding_block_t *block) {
+    BONDING_DEBUG_PRINTF("write_sys_attr_block()\n");
+    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) {
+        return false;
     }
+
+    uint8_t sys_attr[length];
+    if(sd_ble_gatts_sys_attr_get(block->conn_handle, sys_attr, &length, SYS_ATTR_FLAGS) != NRF_SUCCESS) {
+        return false;
+    }
+
+    // Now we know the data size.
+    block->data_length = length;
+    write_block_header(block);
+    write_block_data(sys_attr, length);
     return true;
 }
 
+STATIC bool write_keys_block(bonding_block_t *block) {
+    BONDING_DEBUG_PRINTF("write_keys_block()\n");
+    if (block->data_length != sizeof(bonding_keys_t)) {
+        return false;
+    }
 
+    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;
 
-bool bonding_load_cccd_info(bool is_central, uint16_t conn_handle, uint16_t ediv,
-                            uint8_t **sys_attr, uint16_t *sys_attr_len) {
-    bonding_block_t *block = bonding_find_matching_block(BLOCK_SYS_ATTR, ediv);
-    if (block) {
-        *sys_attr = block.data;
-        *sys_attr_len = block.data_length;
-        return true;
+    write_block_header(block);
+    write_block_data((uint8_t *) bonding_keys, sizeof(bonding_keys_t));
+    return true;
+}
+
+
+void bonding_clear_keys(bonding_keys_t *bonding_keys) {
+    memset((uint8_t*) bonding_keys, 0, sizeof(bonding_keys_t));
+}
+
+void bonding_reset(void) {
+    BONDING_DEBUG_PRINTF("bonding_reset()\n");
+    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();
     } else {
+        bonding_unused_block = find_block(true, BLOCK_UNUSED, EDIV_INVALID);
+    }
+}
+
+// Write bonding blocks to flash. These have been queued during event handlers.
+// We do one at a time, on each background call.
+void bonding_background(void) {
+
+    // Get block at front of list.
+    sd_mutex_acquire_wait(&queued_bonding_block_list_mutex);
+    bonding_block_t *block = &(MP_STATE_VM(queued_bonding_block_list)->bonding_block);
+    if (block) {
+        // Remove written block from front of list.
+        MP_STATE_VM(queued_bonding_block_list) = MP_STATE_VM(queued_bonding_block_list)->next_queued_block;
+    }
+    sd_mutex_release(&queued_bonding_block_list_mutex);
+
+    if (!block) {
+        // No blocks in queue.
+        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) {
+            // Identical block found. No need to store again.
+            BONDING_DEBUG_PRINTF("bonding_background(): identical block found\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);
+    }
+
+    switch (block->type) {
+        case BLOCK_SYS_ATTR:
+            write_sys_attr_block(block);
+            break;
+
+        case BLOCK_KEYS:
+            write_keys_block(block);
+            break;
+
+        default:
+            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);
+    if (block == NULL) {
+        BONDING_DEBUG_PRINTF("bonding_load_cccd_info(): block not found\n");
         return false;
     }
+
+    BONDING_DEBUG_PRINTF("bonding_load_cccd_info(): block found\n");
+    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 = bonding_find_matching_block(BLOCK_SYS_ATTR, ediv);
-    if (block) {
-        memcpy(bonding_keys, block.data, block.data_length);
-        return true;
-    } else {
+    bonding_block_t *block = find_block(is_central, BLOCK_SYS_ATTR, ediv);
+    if (block == NULL) {
+        BONDING_DEBUG_PRINTF("bonding_load_keys(): block not found\n");
+        return false;
+    }
+    if (sizeof(bonding_keys_t) != block->data_length) {
+        // bonding_keys_t is a fixed length, so lengths should match.
         return false;
     }
+
+    BONDING_DEBUG_PRINTF("bonding_load_keys(): block found\n");
+    memcpy(bonding_keys, block->data, block->data_length);
+    return true;
 }
 
-bool bonding_save_cccd_info_later(bool is_central, uint16_t conn_handle, uint16_t ediv, uint8_t *sys_attr, uint16_t sys_attr_len) {
-    // save in id role/ediv
-    // sys_attr
+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);
+}
 
-bool bonding_save_keys(bool is_central, uint16_t conn_handle, bonding_keys_t *bonding_keys) {
-    // save in id role/ediv:
-    // bonding keys
-    // peer name, or if no name, then human-readable address
+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));
 }
diff --git a/ports/nrf/common-hal/_bleio/bonding.h b/ports/nrf/common-hal/_bleio/bonding.h
index 906d9d194..1b0083865 100644
--- a/ports/nrf/common-hal/_bleio/bonding.h
+++ b/ports/nrf/common-hal/_bleio/bonding.h
@@ -24,20 +24,64 @@
  * THE SOFTWARE.
  */
 
+#ifndef MICROPY_INCLUDED_NRF_COMMON_HAL_BLEIO_BONDING_H
+#define MICROPY_INCLUDED_NRF_COMMON_HAL_BLEIO_BONDING_H
+
 #include <stdint.h>
 #include <stdio.h>
 #include <string.h>
 
 #include "ble.h"
 #include "ble_drv.h"
-#include "shared-bindings/_bleio/__init__.h"
-#include "shared-bindings/_bleio/Adapter.h"
-#include "shared-bindings/nvm/ByteArray.h"
+#include "common-hal/_bleio/__init__.h"
 
 #define EDIV_INVALID (0xffff)
 
+#define BONDING_DEBUG (1)
+#if BONDING_DEBUG
+  #define BONDING_DEBUG_PRINTF(...) printf(__VA_ARGS__)
+#else
+  #define BONDING_DEBUG_PRINTF(...)
+#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.
+
+typedef enum {
+    BLOCK_INVALID = 0,      // Ignore this block
+    BLOCK_KEYS = 1,         // Block contains bonding keys.
+    BLOCK_SYS_ATTR = 2,     // Block contains sys_attr values (CCCD settings, etc.).
+    BLOCK_UNUSED = 0xff,    // Initial erased value.
+} bonding_block_type_t;
+
+typedef struct {
+    bool is_central: 1;            // 1 if data is for a central role.
+    uint16_t reserved: 7;          // Not currently used
+    bonding_block_type_t type: 8;  // What kind of data is stored in.
+    uint16_t ediv;                 // ediv value; used as a lookup key.
+    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.
+    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_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;
+
+void bonding_background(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);
-bool bonding_save_cccd_info_later(bool is_central, uint16_t conn_handle, uint16_t ediv, uint8_t *sys_attr, uint16_t sys_attr_len);
-bool bonding_save_keys(bool is_central, uint16_t conn_handle, 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
diff --git a/ports/nrf/mpconfigport.h b/ports/nrf/mpconfigport.h
index cb25870dc..14f7632fd 100644
--- a/ports/nrf/mpconfigport.h
+++ b/ports/nrf/mpconfigport.h
@@ -29,6 +29,7 @@
 #define NRF5_MPCONFIGPORT_H__
 
 #include "ble_drv.h"
+#include "common-hal/_bleio/bonding.h"
 
 #include "nrf_mbr.h"  // for MBR_SIZE
 #include "nrf_sdm.h"  // for SD_FLASH_SIZE
@@ -152,10 +153,10 @@
 #endif
 
 
-
-
 #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; \
+
 
 #endif  // NRF5_MPCONFIGPORT_H__