Enable a BLE workflow

nRF CircuitPython boards will now provide the file transfer
service defined here: https://github.com/adafruit/Adafruit_CircuitPython_BLE_File_Transfer

USB capable boards will only advertise if previously bonded to a
device or if the reset button is pressed during the fast blue
flashes on start up. When pressed, the board will restart again but
the blue period will not flash.

Boards without USB will always advertise.

When previously bonded, the advertisement is private so that no
other peers can connect. If advertising publicly, the tx power is
lowered to reduce the likelihood of bonding from a distance.

This PR also fixes issues with loading identities of bonded peers
so that our address can now be resolved and we can resolve others'
addresses when scanning.

.github/workflows/build.yml

@@ -139,7 +139,7 @@ jobs:
       run: |
         echo "Uploading dev release to PyPi"
         python setup.py sdist
-        twine upload dist/*
+        [ -z "$TWINE_USERNAME" ] || twine upload dist/*
 
   mpy-cross-mac:
     runs-on: macos-10.15

devices/ble_hci/common-hal/_bleio/Adapter.c

@@ -645,7 +645,11 @@ STATIC void check_data_fit(size_t data_len, bool connectable) {
 //     return true;
 // }
 
-uint32_t _common_hal_bleio_adapter_start_advertising(bleio_adapter_obj_t *self, bool connectable, bool anonymous, uint32_t timeout, float interval, uint8_t *advertising_data, uint16_t advertising_data_len, uint8_t *scan_response_data, uint16_t scan_response_data_len, mp_int_t tx_power) {
+uint32_t _common_hal_bleio_adapter_start_advertising(bleio_adapter_obj_t *self,
+    bool connectable, bool anonymous, uint32_t timeout, float interval,
+    const uint8_t *advertising_data, uint16_t advertising_data_len,
+    const uint8_t *scan_response_data, uint16_t scan_response_data_len,
+    mp_int_t tx_power, const bleio_address_obj_t *directed_to) {
     check_enabled(self);
 
     if (self->now_advertising) {
@@ -769,7 +773,11 @@ uint32_t _common_hal_bleio_adapter_start_advertising(bleio_adapter_obj_t *self,
     return 0;
 }
 
-void common_hal_bleio_adapter_start_advertising(bleio_adapter_obj_t *self, bool connectable, bool anonymous, uint32_t timeout, mp_float_t interval, mp_buffer_info_t *advertising_data_bufinfo, mp_buffer_info_t *scan_response_data_bufinfo, mp_int_t tx_power) {
+void common_hal_bleio_adapter_start_advertising(bleio_adapter_obj_t *self,
+    bool connectable, bool anonymous, uint32_t timeout, mp_float_t interval,
+    mp_buffer_info_t *advertising_data_bufinfo,
+    mp_buffer_info_t *scan_response_data_bufinfo,
+    mp_int_t tx_power, const bleio_address_obj_t *directed_to) {
     check_enabled(self);
 
     // interval value has already been validated.
@@ -803,7 +811,7 @@ void common_hal_bleio_adapter_start_advertising(bleio_adapter_obj_t *self, bool
         advertising_data_bufinfo->len,
         scan_response_data_bufinfo->buf,
         scan_response_data_bufinfo->len,
-        tx_power);
+        tx_power, directed_to);
 
     if (result) {
         mp_raise_bleio_BluetoothError(translate("Already advertising"));

devices/ble_hci/common-hal/_bleio/PacketBuffer.c

@@ -148,7 +148,8 @@ mp_int_t common_hal_bleio_packet_buffer_readinto(bleio_packet_buffer_obj_t *self
     return ret;
 }
 
-mp_int_t common_hal_bleio_packet_buffer_write(bleio_packet_buffer_obj_t *self, uint8_t *data, size_t len, uint8_t *header, size_t header_len) {
+mp_int_t common_hal_bleio_packet_buffer_write(bleio_packet_buffer_obj_t *self,
+    const uint8_t *data, size_t len, uint8_t *header, size_t header_len) {
     if (self->outgoing[0] == NULL) {
         mp_raise_bleio_BluetoothError(translate("Writes not supported on Characteristic"));
     }

locale/circuitpython.pot

@@ -595,10 +595,6 @@ msgstr ""
 msgid "Buffer must be at least length 1"
 msgstr ""
 
-#: ports/nrf/common-hal/_bleio/PacketBuffer.c
-msgid "Buffer too large and unable to allocate"
-msgstr ""
-
 #: shared-bindings/_bleio/PacketBuffer.c
 #, c-format
 msgid "Buffer too short by %d bytes"
@@ -873,6 +869,10 @@ msgstr ""
 msgid "Data chunk must follow fmt chunk"
 msgstr ""
 
+#: ports/nrf/common-hal/_bleio/Adapter.c
+msgid "Data not supported with directed advertising"
+msgstr ""
+
 #: ports/nrf/common-hal/_bleio/Adapter.c
 msgid "Data too large for advertisement packet"
 msgstr ""
@@ -1667,7 +1667,6 @@ msgstr ""
 msgid "Not a valid IP string"
 msgstr ""
 
-#: ports/nrf/common-hal/_bleio/PacketBuffer.c
 #: ports/nrf/common-hal/_bleio/__init__.c
 #: shared-bindings/_bleio/CharacteristicBuffer.c
 msgid "Not connected"
@@ -1710,6 +1709,10 @@ msgid ""
 "Only Windows format, uncompressed BMP supported: given header size is %d"
 msgstr ""
 
+#: shared-bindings/_bleio/Adapter.c
+msgid "Only connectable advertisements can be directed"
+msgstr ""
+
 #: ports/stm/common-hal/alarm/pin/PinAlarm.c
 msgid "Only edge detection is available on this hardware"
 msgstr ""

main.c

@@ -696,6 +696,11 @@ int __attribute__((used)) main(void) {
 
     stack_init();
 
+    #if CIRCUITPY_BLEIO
+    // Early init so that a reset press can cause BLE public advertising.
+    supervisor_bluetooth_init();
+    #endif
+
     // Create a new filesystem only if we're not in a safe mode.
     // A power brownout here could make it appear as if there's
     // no SPI flash filesystem, and we might erase the existing one.

ports/nrf/bluetooth/ble_drv.c

@@ -39,7 +39,6 @@
 #include "py/mpstate.h"
 
 #include "supervisor/shared/bluetooth.h"
-#include "supervisor/bluetooth.h"
 
 nrf_nvic_state_t nrf_nvic_state = { 0 };
 
@@ -56,6 +55,14 @@ void ble_drv_reset() {
 }
 
 void ble_drv_add_event_handler_entry(ble_drv_evt_handler_entry_t *entry, ble_drv_evt_handler_t func, void *param) {
+    ble_drv_evt_handler_entry_t *it = MP_STATE_VM(ble_drv_evt_handler_entries);
+    while (it != NULL) {
+        // If event handler and its corresponding param are already on the list, don't add again.
+        if ((it->func == func) && (it->param == param)) {
+            return;
+        }
+        it = it->next;
+    }
     entry->next = MP_STATE_VM(ble_drv_evt_handler_entries);
     entry->param = param;
     entry->func = func;
@@ -85,6 +92,8 @@ void ble_drv_remove_event_handler(ble_drv_evt_handler_t func, void *param) {
         if ((it->func == func) && (it->param == param)) {
             // Splice out the matching handler.
             *prev = it->next;
+            // Clear next of the removed node so it's clearly not in a list.
+            it->next = NULL;
             return;
         }
         prev = &(it->next);
@@ -138,21 +147,26 @@ void SD_EVT_IRQHandler(void) {
 
         ble_evt_t *event = (ble_evt_t *)m_ble_evt_buf;
         #if CIRCUITPY_VERBOSE_BLE
-        mp_printf(&mp_plat_print, "BLE event: 0x%04x\n", event->header.evt_id);
+        size_t eid = event->header.evt_id;
-        #endif
+        if (eid != 0x1d) {
-
+            if (BLE_GAP_EVT_BASE <= eid && eid <= BLE_GAP_EVT_LAST) {
-        if (supervisor_bluetooth_hook(event)) {
+                mp_printf(&mp_plat_print, "BLE GAP event: %d\n", eid - BLE_GAP_EVT_BASE);
-            continue;
+            } else {
+                mp_printf(&mp_plat_print, "BLE event: 0x%04x\n", event->header.evt_id);
+            }
         }
+        #endif
 
         ble_drv_evt_handler_entry_t *it = MP_STATE_VM(ble_drv_evt_handler_entries);
         bool done = false;
         while (it != NULL) {
             #if CIRCUITPY_VERBOSE_BLE
-            // mp_printf(&mp_plat_print, "  calling handler: 0x%08lx, param: 0x%08lx\n", it->func-1, it->param);
+            // mp_printf(&mp_plat_print, "  calling handler: 0x%08lx, param: 0x%08lx\n", it->func - 1, it->param);
             #endif
+            // Capture next before calling the function in case it removes itself from the list.
+            ble_drv_evt_handler_entry_t *next = it->next;
             done = it->func(event, it->param) || done;
-            it = it->next;
+            it = next;
         }
         #if CIRCUITPY_VERBOSE_BLE
         if (event->header.evt_id == BLE_GATTS_EVT_WRITE) {

ports/nrf/common-hal/_bleio/Adapter.c

@@ -40,6 +40,7 @@
 #include "py/gc.h"
 #include "py/objstr.h"
 #include "py/runtime.h"
+#include "supervisor/shared/bluetooth.h"
 #include "supervisor/shared/safe_mode.h"
 #include "supervisor/shared/tick.h"
 #include "supervisor/usb.h"
@@ -174,6 +175,16 @@ STATIC uint32_t ble_stack_enable(void) {
         return err_code;
     }
 
+
+    // Make sure service changed characteristic is on. This lets us prompt a peer to re-discover
+    // portions of our attribute table.
+    memset(&ble_conf, 0, sizeof(ble_conf));
+    ble_conf.gatts_cfg.service_changed.service_changed = 1;
+    err_code = sd_ble_cfg_set(BLE_GATTS_CFG_SERVICE_CHANGED, &ble_conf, sd_ram_end);
+    if (err_code != NRF_SUCCESS) {
+        return err_code;
+    }
+
     // Increase the GATT Server attribute size to accomodate both the CircuitPython built-in service
     // and anything the user does.
     memset(&ble_conf, 0, sizeof(ble_conf));
@@ -231,6 +242,9 @@ STATIC bool adapter_on_ble_evt(ble_evt_t *ble_evt, void *self_in) {
     // For debugging.
     // mp_printf(&mp_plat_print, "Adapter event: 0x%04x\n", ble_evt->header.evt_id);
 
+    // Always queue a background run after a BLE event.
+    background_callback_add_core(&self->background_callback);
+
     switch (ble_evt->header.evt_id) {
         case BLE_GAP_EVT_CONNECTED: {
             // Find an empty connection. One must always be available because the SD has the same
@@ -299,10 +313,6 @@ STATIC bool adapter_on_ble_evt(ble_evt_t *ble_evt, void *self_in) {
             break;
         }
 
-        case BLE_GAP_EVT_ADV_SET_TERMINATED:
-            self->current_advertising_data = NULL;
-            break;
-
         default:
             // For debugging.
             // mp_printf(&mp_plat_print, "Unhandled adapter event: 0x%04x\n", ble_evt->header.evt_id);
@@ -333,6 +343,11 @@ STATIC void bleio_adapter_reset_name(bleio_adapter_obj_t *self) {
     common_hal_bleio_adapter_set_name(self, (char *)default_ble_name);
 }
 
+static void bluetooth_adapter_background(void *data) {
+    supervisor_bluetooth_background();
+    bleio_background();
+}
+
 void common_hal_bleio_adapter_set_enabled(bleio_adapter_obj_t *self, bool enabled) {
     const bool is_enabled = common_hal_bleio_adapter_get_enabled(self);
 
@@ -341,7 +356,7 @@ void common_hal_bleio_adapter_set_enabled(bleio_adapter_obj_t *self, bool enable
         return;
     }
 
-    uint32_t err_code;
+    uint32_t err_code = NRF_SUCCESS;
     if (enabled) {
         // The SD takes over the POWER module and will fail if the module is already in use.
         // Occurs when USB is initialized previously
@@ -365,11 +380,19 @@ void common_hal_bleio_adapter_set_enabled(bleio_adapter_obj_t *self, bool enable
             bleio_connection_internal_t *connection = &bleio_connections[i];
             // Reset connection.
             bleio_connection_clear(connection);
+            ble_drv_remove_event_handler(connection_on_ble_evt, connection);
             connection->conn_handle = BLE_CONN_HANDLE_INVALID;
         }
+        self->background_callback.fun = bluetooth_adapter_background;
+        self->background_callback.data = self;
         bleio_adapter_reset_name(self);
-        ble_drv_add_event_handler_entry(&self->handler_entry, adapter_on_ble_evt, self);
+        ble_drv_add_event_handler_entry(&self->connection_handler_entry, adapter_on_ble_evt, self);
+        bluetooth_adapter_background(self);
     } else {
+        ble_drv_remove_event_handler(adapter_on_ble_evt, self);
+        if (self->current_advertising_data != NULL) {
+            common_hal_bleio_adapter_stop_advertising(self);
+        }
         ble_drv_reset();
         self->scan_results = NULL;
         self->current_advertising_data = NULL;
@@ -428,6 +451,22 @@ void common_hal_bleio_adapter_set_name(bleio_adapter_obj_t *self, const char *na
     sd_ble_gap_device_name_set(&sec, (const uint8_t *)name, strlen(name));
 }
 
+STATIC uint32_t _update_identities(bool is_central) {
+    const ble_gap_id_key_t *keys[BLE_GAP_DEVICE_IDENTITIES_MAX_COUNT];
+    // TODO: Make sure we don't store more than BLE_GAP_DEVICE_IDENTITIES_MAX_COUNT identities of
+    // each type. Right now, we'll silently ignore those keys.
+    size_t len = bonding_load_identities(is_central, keys, BLE_GAP_DEVICE_IDENTITIES_MAX_COUNT);
+    uint32_t status = NRF_SUCCESS;
+    if (len > 0) {
+        status = sd_ble_gap_device_identities_set(
+            keys,
+            NULL, // Don't set local IRK because we use our device IRK for private addresses.
+            len
+            );
+    }
+    return status;
+};
+
 STATIC bool scan_on_ble_evt(ble_evt_t *ble_evt, void *scan_results_in) {
     bleio_scanresults_obj_t *scan_results = (bleio_scanresults_obj_t *)scan_results_in;
 
@@ -476,6 +515,11 @@ mp_obj_t common_hal_bleio_adapter_start_scan(bleio_adapter_obj_t *self, uint8_t
     sd_data->len = max_packet_size;
     sd_data->p_data = raw_data + sizeof(ble_data_t);
 
+
+    // Update the identities of peripheral peers so they can use a private
+    // resolvable address in their advertisements.
+    check_nrf_error(_update_identities(true));
+
     ble_drv_add_event_handler(scan_on_ble_evt, self->scan_results);
 
     uint32_t nrf_timeout = SEC_TO_UNITS(timeout, UNIT_10_MS) + 0.5f;
@@ -500,9 +544,8 @@ mp_obj_t common_hal_bleio_adapter_start_scan(bleio_adapter_obj_t *self, uint8_t
         .scan_phys = BLE_GAP_PHY_1MBPS,
         .active = active
     };
-    uint32_t err_code;
+
-    vm_used_ble = true;
+    uint32_t err_code = sd_ble_gap_scan_start(&scan_params, sd_data);
-    err_code = sd_ble_gap_scan_start(&scan_params, sd_data);
 
     if (err_code != NRF_SUCCESS) {
         ble_drv_remove_event_handler(scan_on_ble_evt, self->scan_results);
@@ -514,6 +557,9 @@ mp_obj_t common_hal_bleio_adapter_start_scan(bleio_adapter_obj_t *self, uint8_t
 }
 
 void common_hal_bleio_adapter_stop_scan(bleio_adapter_obj_t *self) {
+    if (self->scan_results == NULL) {
+        return;
+    }
     sd_ble_gap_scan_stop();
     shared_module_bleio_scanresults_set_done(self->scan_results, true);
     ble_drv_remove_event_handler(scan_on_ble_evt, self->scan_results);
@@ -548,14 +594,16 @@ STATIC bool connect_on_ble_evt(ble_evt_t *ble_evt, void *info_in) {
     return true;
 }
 
-mp_obj_t common_hal_bleio_adapter_connect(bleio_adapter_obj_t *self, bleio_address_obj_t *address, mp_float_t timeout) {
+STATIC void _convert_address(const bleio_address_obj_t *address, ble_gap_addr_t *sd_address) {
-
+    sd_address->addr_type = address->type;
-    ble_gap_addr_t addr;
-
-    addr.addr_type = address->type;
     mp_buffer_info_t address_buf_info;
     mp_get_buffer_raise(address->bytes, &address_buf_info, MP_BUFFER_READ);
-    memcpy(addr.addr, (uint8_t *)address_buf_info.buf, NUM_BLEIO_ADDRESS_BYTES);
+    memcpy(sd_address->addr, (uint8_t *)address_buf_info.buf, NUM_BLEIO_ADDRESS_BYTES);
+}
+
+mp_obj_t common_hal_bleio_adapter_connect(bleio_adapter_obj_t *self, bleio_address_obj_t *address, mp_float_t timeout) {
+    ble_gap_addr_t addr;
+    _convert_address(address, &addr);
 
     ble_gap_scan_params_t scan_params = {
         .interval = MSEC_TO_UNITS(100, UNIT_0_625_MS),
@@ -576,7 +624,6 @@ mp_obj_t common_hal_bleio_adapter_connect(bleio_adapter_obj_t *self, bleio_addre
     ble_drv_add_event_handler(connect_on_ble_evt, &event_info);
     event_info.done = false;
 
-    vm_used_ble = true;
     uint32_t err_code = sd_ble_gap_connect(&addr, &scan_params, &conn_params, BLE_CONN_CFG_TAG_CUSTOM);
 
     if (err_code != NRF_SUCCESS) {
@@ -594,6 +641,17 @@ mp_obj_t common_hal_bleio_adapter_connect(bleio_adapter_obj_t *self, bleio_addre
     if (conn_handle == BLE_CONN_HANDLE_INVALID) {
         mp_raise_bleio_BluetoothError(translate("Failed to connect: timeout"));
     }
+    // If we have keys, then try and encrypt the connection.
+    const ble_gap_enc_key_t *encryption_key = bonding_load_peer_encryption_key(true, &addr);
+    pair_status_t pair_status = PAIR_NOT_PAIRED;
+    if (encryption_key != NULL) {
+        err_code = sd_ble_gap_encrypt(conn_handle, &encryption_key->master_id, &encryption_key->enc_info);
+        pair_status = PAIR_WAITING;
+
+        if (err_code != NRF_SUCCESS) {
+            pair_status = PAIR_NOT_PAIRED;
+        }
+    }
 
     // Negotiate for better PHY, larger MTU and data lengths since we are the central. These are
     // nice-to-haves so ignore any errors.
@@ -610,6 +668,7 @@ mp_obj_t common_hal_bleio_adapter_connect(bleio_adapter_obj_t *self, bleio_addre
         bleio_connection_internal_t *connection = &bleio_connections[i];
         if (connection->conn_handle == conn_handle) {
             connection->is_central = true;
+            connection->pair_status = pair_status;
             return bleio_connection_new_from_internal(connection);
         }
     }
@@ -634,9 +693,14 @@ STATIC bool advertising_on_ble_evt(ble_evt_t *ble_evt, void *self_in) {
     bleio_adapter_obj_t *self = (bleio_adapter_obj_t *)self_in;
 
     switch (ble_evt->header.evt_id) {
+        case BLE_GAP_EVT_CONNECTED: // Connecting also stops an advertisement.
+            // Set the tx_power for the connection higher than the advertisement.
+            sd_ble_gap_tx_power_set(BLE_GAP_TX_POWER_ROLE_CONN, ble_evt->evt.gap_evt.conn_handle, 0);
+            common_hal_bleio_adapter_stop_advertising(self);
+            return false;
+            break;
         case BLE_GAP_EVT_ADV_SET_TERMINATED:
             common_hal_bleio_adapter_stop_advertising(self);
-            ble_drv_remove_event_handler(advertising_on_ble_evt, self_in);
             break;
 
         default:
@@ -648,10 +712,11 @@ STATIC bool advertising_on_ble_evt(ble_evt_t *ble_evt, void *self_in) {
     return true;
 }
 
-uint32_t _common_hal_bleio_adapter_start_advertising(bleio_adapter_obj_t *self, bool connectable,
+uint32_t _common_hal_bleio_adapter_start_advertising(bleio_adapter_obj_t *self,
-    bool anonymous, uint32_t timeout, float interval, uint8_t *advertising_data,
+    bool connectable, bool anonymous, uint32_t timeout, float interval,
-    uint16_t advertising_data_len, uint8_t *scan_response_data, uint16_t scan_response_data_len,
+    const uint8_t *advertising_data, uint16_t advertising_data_len,
-    mp_int_t tx_power) {
+    const uint8_t *scan_response_data, uint16_t scan_response_data_len,
+    mp_int_t tx_power, const bleio_address_obj_t *directed_to) {
     if (self->current_advertising_data != NULL && self->current_advertising_data == self->advertising_data) {
         return NRF_ERROR_BUSY;
     }
@@ -667,6 +732,8 @@ uint32_t _common_hal_bleio_adapter_start_advertising(bleio_adapter_obj_t *self,
         scan_response_data_len > BLE_GAP_ADV_SET_DATA_SIZE_MAX;
 
     uint8_t adv_type;
+    ble_gap_addr_t *peer = NULL;
+    ble_gap_addr_t peer_address;
     if (extended) {
         if (connectable) {
             adv_type = BLE_GAP_ADV_TYPE_EXTENDED_CONNECTABLE_NONSCANNABLE_UNDIRECTED;
@@ -676,7 +743,17 @@ uint32_t _common_hal_bleio_adapter_start_advertising(bleio_adapter_obj_t *self,
             adv_type = BLE_GAP_ADV_TYPE_EXTENDED_NONCONNECTABLE_NONSCANNABLE_UNDIRECTED;
         }
     } else if (connectable) {
-        adv_type = BLE_GAP_ADV_TYPE_CONNECTABLE_SCANNABLE_UNDIRECTED;
+        if (directed_to == NULL) {
+            adv_type = BLE_GAP_ADV_TYPE_CONNECTABLE_SCANNABLE_UNDIRECTED;
+        } else if (interval <= 3.5 && timeout <= 1.3) {
+            adv_type = BLE_GAP_ADV_TYPE_CONNECTABLE_NONSCANNABLE_DIRECTED_HIGH_DUTY_CYCLE;
+            _convert_address(directed_to, &peer_address);
+            peer = &peer_address;
+        } else {
+            adv_type = BLE_GAP_ADV_TYPE_CONNECTABLE_NONSCANNABLE_DIRECTED;
+            _convert_address(directed_to, &peer_address);
+            peer = &peer_address;
+        }
     } else if (scan_response_data_len > 0) {
         adv_type = BLE_GAP_ADV_TYPE_NONCONNECTABLE_SCANNABLE_UNDIRECTED;
     } else {
@@ -714,26 +791,34 @@ uint32_t _common_hal_bleio_adapter_start_advertising(bleio_adapter_obj_t *self,
         .duration = SEC_TO_UNITS(timeout, UNIT_10_MS),
         .filter_policy = BLE_GAP_ADV_FP_ANY,
         .primary_phy = BLE_GAP_PHY_1MBPS,
+        .p_peer_addr = peer,
     };
 
     const ble_gap_adv_data_t ble_gap_adv_data = {
-        .adv_data.p_data = advertising_data,
+        .adv_data.p_data = (uint8_t *)advertising_data,
         .adv_data.len = advertising_data_len,
-        .scan_rsp_data.p_data = scan_response_data_len > 0 ? scan_response_data : NULL,
+        .scan_rsp_data.p_data = scan_response_data_len > 0 ? (uint8_t *)scan_response_data : NULL,
         .scan_rsp_data.len = scan_response_data_len,
     };
 
+    // Update the identities of central peers so they can use a private address
+    // in the scan and connection initiation.
+    err_code = _update_identities(false);
+    if (err_code != NRF_SUCCESS) {
+        return err_code;
+    }
+
     err_code = sd_ble_gap_adv_set_configure(&adv_handle, &ble_gap_adv_data, &adv_params);
     if (err_code != NRF_SUCCESS) {
         return err_code;
     }
 
-    ble_drv_add_event_handler(advertising_on_ble_evt, self);
+    ble_drv_add_event_handler_entry(&self->advertising_handler_entry, advertising_on_ble_evt, self);
+
     err_code = sd_ble_gap_tx_power_set(BLE_GAP_TX_POWER_ROLE_ADV, adv_handle, tx_power);
     if (err_code != NRF_SUCCESS) {
         return err_code;
     }
-    vm_used_ble = true;
     err_code = sd_ble_gap_adv_start(adv_handle, BLE_CONN_CFG_TAG_CUSTOM);
     if (err_code != NRF_SUCCESS) {
         return err_code;
@@ -743,9 +828,16 @@ uint32_t _common_hal_bleio_adapter_start_advertising(bleio_adapter_obj_t *self,
 }
 
 
-void common_hal_bleio_adapter_start_advertising(bleio_adapter_obj_t *self, bool connectable, bool anonymous, uint32_t timeout, mp_float_t interval, mp_buffer_info_t *advertising_data_bufinfo, mp_buffer_info_t *scan_response_data_bufinfo, mp_int_t tx_power) {
+void common_hal_bleio_adapter_start_advertising(bleio_adapter_obj_t *self, bool connectable,
-    if (self->current_advertising_data != NULL && self->current_advertising_data == self->advertising_data) {
+    bool anonymous, uint32_t timeout, mp_float_t interval,
+    mp_buffer_info_t *advertising_data_bufinfo, mp_buffer_info_t *scan_response_data_bufinfo,
+    mp_int_t tx_power, const bleio_address_obj_t *directed_to) {
+    if (self->user_advertising) {
         mp_raise_bleio_BluetoothError(translate("Already advertising."));
+    } else if (self->current_advertising_data != NULL) {
+        // If the user isn't advertising, then the background is. So, stop the
+        // background advertising so the user can.
+        common_hal_bleio_adapter_stop_advertising(self);
     }
     // interval value has already been validated.
 
@@ -756,6 +848,11 @@ void common_hal_bleio_adapter_start_advertising(bleio_adapter_obj_t *self, bool
         mp_raise_bleio_BluetoothError(translate("Extended advertisements with scan response not supported."));
     }
 
+
+    if (advertising_data_bufinfo->len > 0 && directed_to != NULL) {
+        mp_raise_bleio_BluetoothError(translate("Data not supported with directed advertising"));
+    }
+
     // Anonymous mode requires a timeout so that we don't continue to broadcast
     // the same data while cycling the MAC address -- otherwise, what's the
     // point of randomizing the MAC address?
@@ -792,7 +889,9 @@ void common_hal_bleio_adapter_start_advertising(bleio_adapter_obj_t *self, bool
         advertising_data_bufinfo->len,
         self->scan_response_data,
         scan_response_data_bufinfo->len,
-        tx_power));
+        tx_power,
+        directed_to));
+    self->user_advertising = true;
 }
 
 void common_hal_bleio_adapter_stop_advertising(bleio_adapter_obj_t *self) {
@@ -800,9 +899,10 @@ void common_hal_bleio_adapter_stop_advertising(bleio_adapter_obj_t *self) {
         return;
     }
 
-    // TODO: Don't actually stop. Switch to advertising CircuitPython if we don't already have a connection.
     const uint32_t err_code = sd_ble_gap_adv_stop(adv_handle);
+    ble_drv_remove_event_handler(advertising_on_ble_evt, self);
     self->current_advertising_data = NULL;
+    self->user_advertising = false;
 
     if ((err_code != NRF_SUCCESS) && (err_code != NRF_ERROR_INVALID_STATE)) {
         check_nrf_error(err_code);
@@ -847,6 +947,10 @@ void common_hal_bleio_adapter_erase_bonding(bleio_adapter_obj_t *self) {
     bonding_erase_storage();
 }
 
+bool common_hal_bleio_adapter_is_bonded_to_central(bleio_adapter_obj_t *self) {
+    return bonding_peripheral_bond_count() > 0;
+}
+
 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 **)bleio_connections, sizeof(bleio_connections) / sizeof(size_t));

ports/nrf/common-hal/_bleio/Adapter.h

@@ -35,6 +35,8 @@
 #include "shared-bindings/_bleio/Connection.h"
 #include "shared-bindings/_bleio/ScanResults.h"
 
+#include "supervisor/background_callback.h"
+
 #ifndef BLEIO_TOTAL_CONNECTION_COUNT
 #define BLEIO_TOTAL_CONNECTION_COUNT 5
 #endif
@@ -43,13 +45,18 @@ extern bleio_connection_internal_t bleio_connections[BLEIO_TOTAL_CONNECTION_COUN
 
 typedef struct {
     mp_obj_base_t base;
+    // Pointer to buffers we maintain so that the data is long lived.
     uint8_t *advertising_data;
     uint8_t *scan_response_data;
-    uint8_t *current_advertising_data;
+    // Pointer to current data.
+    const uint8_t *current_advertising_data;
     bleio_scanresults_obj_t *scan_results;
     mp_obj_t name;
     mp_obj_tuple_t *connection_objs;
-    ble_drv_evt_handler_entry_t handler_entry;
+    ble_drv_evt_handler_entry_t connection_handler_entry;
+    ble_drv_evt_handler_entry_t advertising_handler_entry;
+    background_callback_t background_callback;
+    bool user_advertising;
 } bleio_adapter_obj_t;
 
 void bleio_adapter_gc_collect(bleio_adapter_obj_t *adapter);

ports/nrf/common-hal/_bleio/Characteristic.c

@@ -83,15 +83,36 @@ STATIC void characteristic_gatts_notify_indicate(uint16_t handle, uint16_t conn_
     }
 }
 
-void common_hal_bleio_characteristic_construct(bleio_characteristic_obj_t *self, bleio_service_obj_t *service, uint16_t handle, bleio_uuid_obj_t *uuid, bleio_characteristic_properties_t props, bleio_attribute_security_mode_t read_perm, bleio_attribute_security_mode_t write_perm, mp_int_t max_length, bool fixed_length, mp_buffer_info_t *initial_value_bufinfo) {
+void common_hal_bleio_characteristic_construct(bleio_characteristic_obj_t *self, bleio_service_obj_t *service,
+    uint16_t handle, bleio_uuid_obj_t *uuid, bleio_characteristic_properties_t props,
+    bleio_attribute_security_mode_t read_perm, bleio_attribute_security_mode_t write_perm,
+    mp_int_t max_length, bool fixed_length, mp_buffer_info_t *initial_value_bufinfo) {
     self->service = service;
     self->uuid = uuid;
     self->handle = BLE_GATT_HANDLE_INVALID;
     self->props = props;
     self->read_perm = read_perm;
     self->write_perm = write_perm;
-    self->initial_value = mp_obj_new_bytes(initial_value_bufinfo->buf, initial_value_bufinfo->len);
+    self->initial_value_len = 0;
-    self->descriptor_list = mp_obj_new_list(0, NULL);
+    self->initial_value = NULL;
+    if (initial_value_bufinfo != NULL) {
+        // Copy the initial value if it's on the heap. Otherwise it's internal and we may not be able
+        // to allocate.
+        self->initial_value_len = initial_value_bufinfo->len;
+        if (gc_alloc_possible()) {
+            if (gc_nbytes(initial_value_bufinfo->buf) > 0) {
+                uint8_t *initial_value = m_malloc(self->initial_value_len, false);
+                memcpy(initial_value, initial_value_bufinfo->buf, self->initial_value_len);
+                self->initial_value = initial_value;
+            } else {
+                self->initial_value = initial_value_bufinfo->buf;
+            }
+            self->descriptor_list = mp_obj_new_list(0, NULL);
+        } else {
+            self->initial_value = initial_value_bufinfo->buf;
+            self->descriptor_list = NULL;
+        }
+    }
 
     const mp_int_t max_length_max = fixed_length ? BLE_GATTS_FIX_ATTR_LEN_MAX : BLE_GATTS_VAR_ATTR_LEN_MAX;
     if (max_length < 0 || max_length > max_length_max) {
@@ -109,6 +130,9 @@ void common_hal_bleio_characteristic_construct(bleio_characteristic_obj_t *self,
 }
 
 mp_obj_tuple_t *common_hal_bleio_characteristic_get_descriptors(bleio_characteristic_obj_t *self) {
+    if (self->descriptor_list == NULL) {
+        return mp_const_empty_tuple;
+    }
     return mp_obj_new_tuple(self->descriptor_list->len, self->descriptor_list->items);
 }
 
@@ -193,6 +217,11 @@ bleio_characteristic_properties_t common_hal_bleio_characteristic_get_properties
 }
 
 void common_hal_bleio_characteristic_add_descriptor(bleio_characteristic_obj_t *self, bleio_descriptor_obj_t *descriptor) {
+    if (self->descriptor_list == NULL) {
+        // This should only happen from internal use so we just fail silently instead of raising an
+        // exception.
+        return;
+    }
     ble_uuid_t desc_uuid;
     bleio_uuid_convert_to_nrf_ble_uuid(descriptor->uuid, &desc_uuid);
 

ports/nrf/common-hal/_bleio/Characteristic.h

@@ -39,9 +39,9 @@ typedef struct _bleio_characteristic_obj {
     // Will be MP_OBJ_NULL before being assigned to a Service.
     bleio_service_obj_t *service;
     bleio_uuid_obj_t *uuid;
-    mp_obj_t initial_value;
+    const uint8_t *initial_value;
+    uint16_t initial_value_len;
     uint16_t max_length;
-    bool fixed_length;
     uint16_t handle;
     bleio_characteristic_properties_t props;
     bleio_attribute_security_mode_t read_perm;
@@ -50,6 +50,7 @@ typedef struct _bleio_characteristic_obj {
     uint16_t user_desc_handle;
     uint16_t cccd_handle;
     uint16_t sccd_handle;
+    bool fixed_length;
 } bleio_characteristic_obj_t;
 
 #endif // MICROPY_INCLUDED_NRF_COMMON_HAL_BLEIO_CHARACTERISTIC_H

ports/nrf/common-hal/_bleio/Connection.c

@@ -309,6 +309,9 @@ bool connection_on_ble_evt(ble_evt_t *ble_evt, void *self_in) {
             } else {
                 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.
+                    // Indicate ATTR table change because we may have reloaded since the peer last
+                    // connected.
+                    sd_ble_gatts_service_changed(self->conn_handle, 0xC, 0xFFFF);
                 } else {
                     // No matching bonding found, so use fresh system attributes.
                     sd_ble_gatts_sys_attr_set(self->conn_handle, NULL, 0, 0);
@@ -351,6 +354,14 @@ void common_hal_bleio_connection_disconnect(bleio_connection_internal_t *self) {
 }
 
 void common_hal_bleio_connection_pair(bleio_connection_internal_t *self, bool bond) {
+    // We may already be trying to pair if we just reconnected to a peer we're
+    // bonded with.
+    while (self->pair_status == PAIR_WAITING && !mp_hal_is_interrupted()) {
+        RUN_BACKGROUND_TASKS;
+    }
+    if (self->pair_status == PAIR_PAIRED) {
+        return;
+    }
     self->pair_status = PAIR_WAITING;
 
     check_nrf_error(sd_ble_gap_authenticate(self->conn_handle, &pairing_sec_params));

ports/nrf/common-hal/_bleio/PacketBuffer.c

@@ -76,7 +76,7 @@ STATIC uint32_t queue_next_write(bleio_packet_buffer_obj_t *self) {
             ble_gattc_write_params_t write_params = {
                 .write_op = self->write_type,
                 .handle = self->characteristic->handle,
-                .p_value = self->outgoing[self->pending_index],
+                .p_value = (const uint8_t *)self->outgoing[self->pending_index],
                 .len = self->pending_size,
             };
 
@@ -89,7 +89,7 @@ STATIC uint32_t queue_next_write(bleio_packet_buffer_obj_t *self) {
                 .type = self->write_type,
                 .offset = 0,
                 .p_len = &hvx_len,
-                .p_data = self->outgoing[self->pending_index],
+                .p_data = (const uint8_t *)self->outgoing[self->pending_index],
             };
             err_code = sd_ble_gatts_hvx(conn_handle, &hvx_params);
         }
@@ -173,6 +173,8 @@ STATIC bool packet_buffer_on_ble_server_evt(ble_evt_t *ble_evt, void *param) {
             }
             break;
         }
+        case BLE_GAP_EVT_CONNECTED:
+            break;
         case BLE_GAP_EVT_DISCONNECTED:
             if (self->conn_handle == ble_evt->evt.gap_evt.conn_handle) {
                 self->conn_handle = BLE_CONN_HANDLE_INVALID;
@@ -188,12 +190,16 @@ STATIC bool packet_buffer_on_ble_server_evt(ble_evt_t *ble_evt, void *param) {
     return true;
 }
 
-void common_hal_bleio_packet_buffer_construct(
+
+void _common_hal_bleio_packet_buffer_construct(
     bleio_packet_buffer_obj_t *self, bleio_characteristic_obj_t *characteristic,
-    size_t buffer_size, size_t max_packet_size) {
+    uint32_t *incoming_buffer, size_t incoming_buffer_size,
+    uint32_t *outgoing_buffer1, uint32_t *outgoing_buffer2, size_t max_packet_size,
+    void *static_handler_entry) {
 
     self->characteristic = characteristic;
     self->client = self->characteristic->service->is_remote;
+    self->max_packet_size = max_packet_size;
     bleio_characteristic_properties_t incoming = self->characteristic->props & (CHAR_PROP_WRITE_NO_RESPONSE | CHAR_PROP_WRITE);
     bleio_characteristic_properties_t outgoing = self->characteristic->props & (CHAR_PROP_NOTIFY | CHAR_PROP_INDICATE);
 
@@ -207,28 +213,25 @@ void common_hal_bleio_packet_buffer_construct(
         self->conn_handle = BLE_CONN_HANDLE_INVALID;
     }
 
-    // Cap the packet size to our implementation limits.
-    self->max_packet_size = MIN(max_packet_size, BLE_GATTS_VAR_ATTR_LEN_MAX - 3);
-
     if (incoming) {
-        if (!ringbuf_alloc(&self->ringbuf, buffer_size * (sizeof(uint16_t) + self->max_packet_size), false)) {
+        self->ringbuf.buf = (uint8_t *)incoming_buffer;
-            mp_raise_ValueError(translate("Buffer too large and unable to allocate"));
+        self->ringbuf.size = incoming_buffer_size;
-        }
+        self->ringbuf.iget = 0;
+        self->ringbuf.iput = 0;
     }
 
-    if (outgoing) {
+    self->packet_queued = false;
-        self->packet_queued = false;
+    self->pending_index = 0;
-        self->pending_index = 0;
+    self->pending_size = 0;
-        self->pending_size = 0;
+    self->outgoing[0] = outgoing_buffer1;
-        self->outgoing[0] = m_malloc(self->max_packet_size, false);
+    self->outgoing[1] = outgoing_buffer2;
-        self->outgoing[1] = m_malloc(self->max_packet_size, false);
-    } else {
-        self->outgoing[0] = NULL;
-        self->outgoing[1] = NULL;
-    }
 
     if (self->client) {
-        ble_drv_add_event_handler(packet_buffer_on_ble_client_evt, self);
+        if (static_handler_entry != NULL) {
+            ble_drv_add_event_handler_entry((ble_drv_evt_handler_entry_t *)static_handler_entry, packet_buffer_on_ble_client_evt, self);
+        } else {
+            ble_drv_add_event_handler(packet_buffer_on_ble_client_evt, self);
+        }
         if (incoming) {
             // Prefer notify if both are available.
             if (incoming & CHAR_PROP_NOTIFY) {
@@ -245,7 +248,11 @@ void common_hal_bleio_packet_buffer_construct(
             }
         }
     } else {
-        ble_drv_add_event_handler(packet_buffer_on_ble_server_evt, self);
+        if (static_handler_entry != NULL) {
+            ble_drv_add_event_handler_entry((ble_drv_evt_handler_entry_t *)static_handler_entry, packet_buffer_on_ble_server_evt, self);
+        } else {
+            ble_drv_add_event_handler(packet_buffer_on_ble_server_evt, self);
+        }
         if (outgoing) {
             self->write_type = BLE_GATT_HVX_INDICATION;
             if (outgoing & CHAR_PROP_NOTIFY) {
@@ -253,12 +260,44 @@ void common_hal_bleio_packet_buffer_construct(
             }
         }
     }
+
+}
+
+void common_hal_bleio_packet_buffer_construct(
+    bleio_packet_buffer_obj_t *self, bleio_characteristic_obj_t *characteristic,
+    size_t buffer_size, size_t max_packet_size) {
+
+    // Cap the packet size to our implementation limits.
+    max_packet_size = MIN(max_packet_size, BLE_GATTS_VAR_ATTR_LEN_MAX - 3);
+
+    bleio_characteristic_properties_t incoming = characteristic->props & (CHAR_PROP_WRITE_NO_RESPONSE | CHAR_PROP_WRITE);
+    bleio_characteristic_properties_t outgoing = characteristic->props & (CHAR_PROP_NOTIFY | CHAR_PROP_INDICATE);
+    if (characteristic->service->is_remote) {
+        // Swap if we're the client.
+        bleio_characteristic_properties_t temp = incoming;
+        incoming = outgoing;
+        outgoing = temp;
+    }
+    size_t incoming_buffer_size = 0;
+    uint32_t *incoming_buffer = NULL;
+    if (incoming) {
+        incoming_buffer_size = buffer_size * (sizeof(uint16_t) + max_packet_size);
+        incoming_buffer = m_malloc(incoming_buffer_size, false);
+    }
+
+    uint32_t *outgoing1 = NULL;
+    uint32_t *outgoing2 = NULL;
+    if (outgoing) {
+        outgoing1 = m_malloc(max_packet_size, false);
+        outgoing2 = m_malloc(max_packet_size, false);
+    }
+    _common_hal_bleio_packet_buffer_construct(self, characteristic,
+        incoming_buffer, incoming_buffer_size,
+        outgoing1, outgoing2, max_packet_size,
+        NULL);
 }
 
 mp_int_t common_hal_bleio_packet_buffer_readinto(bleio_packet_buffer_obj_t *self, uint8_t *data, size_t len) {
-    if (self->conn_handle == BLE_CONN_HANDLE_INVALID) {
-        mp_raise_ConnectionError(translate("Not connected"));
-    }
     if (ringbuf_num_filled(&self->ringbuf) < 2) {
         return 0;
     }
@@ -291,7 +330,7 @@ mp_int_t common_hal_bleio_packet_buffer_readinto(bleio_packet_buffer_obj_t *self
     return ret;
 }
 
-mp_int_t common_hal_bleio_packet_buffer_write(bleio_packet_buffer_obj_t *self, uint8_t *data, size_t len, uint8_t *header, size_t header_len) {
+mp_int_t common_hal_bleio_packet_buffer_write(bleio_packet_buffer_obj_t *self, const uint8_t *data, size_t len, uint8_t *header, size_t header_len) {
     if (self->outgoing[0] == NULL) {
         mp_raise_bleio_BluetoothError(translate("Writes not supported on Characteristic"));
     }
@@ -330,14 +369,14 @@ mp_int_t common_hal_bleio_packet_buffer_write(bleio_packet_buffer_obj_t *self, u
     uint8_t is_nested_critical_region;
     sd_nvic_critical_region_enter(&is_nested_critical_region);
 
-    uint8_t *pending = self->outgoing[self->pending_index];
+    uint32_t *pending = self->outgoing[self->pending_index];
 
     if (self->pending_size == 0) {
         memcpy(pending, header, header_len);
         self->pending_size += header_len;
         num_bytes_written += header_len;
     }
-    memcpy(pending + self->pending_size, data, len);
+    memcpy(((uint8_t *)pending) + self->pending_size, data, len);
     self->pending_size += len;
     num_bytes_written += len;
 
@@ -425,6 +464,15 @@ mp_int_t common_hal_bleio_packet_buffer_get_outgoing_packet_length(bleio_packet_
     return MIN(self->characteristic->max_length, self->max_packet_size);
 }
 
+void common_hal_bleio_packet_buffer_flush(bleio_packet_buffer_obj_t *self) {
+    while (self->pending_size != 0 &&
+           self->packet_queued &&
+           self->conn_handle != BLE_CONN_HANDLE_INVALID &&
+           !mp_hal_is_interrupted()) {
+        RUN_BACKGROUND_TASKS;
+    }
+}
+
 bool common_hal_bleio_packet_buffer_deinited(bleio_packet_buffer_obj_t *self) {
     return self->characteristic == NULL;
 }

ports/nrf/common-hal/_bleio/PacketBuffer.h

@@ -39,7 +39,7 @@ typedef struct {
     ringbuf_t ringbuf;
     // Two outgoing buffers to alternate between. One will be queued for transmission by the SD and
     // the other is waiting to be queued and can be extended.
-    uint8_t *outgoing[2];
+    uint32_t *outgoing[2];
     volatile uint16_t pending_size;
     // We remember the conn_handle so we can do a NOTIFY/INDICATE to a client.
     // We can find out the conn_handle on a Characteristic write or a CCCD write (but not a read).

ports/nrf/common-hal/_bleio/Service.c

@@ -34,6 +34,19 @@
 #include "shared-bindings/_bleio/Service.h"
 #include "shared-bindings/_bleio/Adapter.h"
 
+
+STATIC void _indicate_service_change(uint16_t start, uint16_t end) {
+    for (size_t i = 0; i < BLEIO_TOTAL_CONNECTION_COUNT; i++) {
+        bleio_connection_internal_t *connection = &bleio_connections[i];
+        uint16_t conn_handle = connection->conn_handle;
+        if (connection->conn_handle == BLE_CONN_HANDLE_INVALID) {
+            continue;
+        }
+
+        sd_ble_gatts_service_changed(conn_handle, start, end);
+    }
+}
+
 uint32_t _common_hal_bleio_service_construct(bleio_service_obj_t *self, bleio_uuid_obj_t *uuid, bool is_secondary, mp_obj_list_t *characteristic_list) {
     self->handle = 0xFFFF;
     self->uuid = uuid;
@@ -50,9 +63,13 @@ uint32_t _common_hal_bleio_service_construct(bleio_service_obj_t *self, bleio_uu
         service_type = BLE_GATTS_SRVC_TYPE_SECONDARY;
     }
 
-    vm_used_ble = true;
+    uint32_t result = sd_ble_gatts_service_add(service_type, &nordic_uuid, &self->handle);
+    // Do a service changed indication to all connected peers.
+    if (result == NRF_SUCCESS) {
+        _indicate_service_change(self->handle, self->handle);
+    }
 
-    return sd_ble_gatts_service_add(service_type, &nordic_uuid, &self->handle);
+    return result;
 }
 
 void common_hal_bleio_service_construct(bleio_service_obj_t *self, bleio_uuid_obj_t *uuid, bool is_secondary) {
@@ -85,6 +102,14 @@ bool common_hal_bleio_service_get_is_secondary(bleio_service_obj_t *self) {
     return self->is_secondary;
 }
 
+STATIC void _expand_range(uint16_t new_value, uint16_t *start, uint16_t *end) {
+    if (new_value == 0) {
+        return;
+    }
+    *start = MIN(*start, new_value);
+    *end = MAX(*end, new_value);
+}
+
 void common_hal_bleio_service_add_characteristic(bleio_service_obj_t *self,
     bleio_characteristic_obj_t *characteristic,
     mp_buffer_info_t *initial_value_bufinfo) {
@@ -124,14 +149,11 @@ void common_hal_bleio_service_add_characteristic(bleio_service_obj_t *self,
     char_attr_md.rd_auth = true;
     #endif
 
-    mp_buffer_info_t char_value_bufinfo;
-    mp_get_buffer_raise(characteristic->initial_value, &char_value_bufinfo, MP_BUFFER_READ);
-
     ble_gatts_attr_t char_attr = {
         .p_uuid = &char_uuid,
         .p_attr_md = &char_attr_md,
-        .init_len = char_value_bufinfo.len,
+        .init_len = characteristic->initial_value_len,
-        .p_value = char_value_bufinfo.buf,
+        .p_value = (uint8_t *)characteristic->initial_value,
         .init_offs = 0,
         .max_len = characteristic->max_length,
     };
@@ -144,6 +166,15 @@ void common_hal_bleio_service_add_characteristic(bleio_service_obj_t *self,
     characteristic->cccd_handle = char_handles.cccd_handle;
     characteristic->sccd_handle = char_handles.sccd_handle;
     characteristic->handle = char_handles.value_handle;
+
+    // Indicate that the attribute table has changed.
+    uint16_t start = char_handles.value_handle;
+    uint16_t end = char_handles.value_handle;
+    _expand_range(char_handles.cccd_handle, &start, &end);
+    _expand_range(char_handles.sccd_handle, &start, &end);
+    _expand_range(char_handles.user_desc_handle, &start, &end);
+    _indicate_service_change(start, end);
+
     #if CIRCUITPY_VERBOSE_BLE
     mp_printf(&mp_plat_print, "Char handle %x user %x cccd %x sccd %x\n", characteristic->handle, characteristic->user_desc_handle, characteristic->cccd_handle, characteristic->sccd_handle);
     #endif

ports/nrf/common-hal/_bleio/UUID.c

@@ -50,7 +50,6 @@ void common_hal_bleio_uuid_construct(bleio_uuid_obj_t *self, mp_int_t uuid16, co
 
         // Register this vendor-specific UUID. Bytes 12 and 13 will be zero.
         check_nrf_error(sd_ble_uuid_vs_add(&vs_uuid, &self->nrf_ble_uuid.type));
-        vm_used_ble = true;
     }
 }
 

ports/nrf/common-hal/_bleio/__init__.c

@@ -94,18 +94,13 @@ void check_sec_status(uint8_t sec_status) {
     }
 }
 
-bool vm_used_ble;
-
 // Turn off BLE on a reset or reload.
 void bleio_reset() {
     if (!common_hal_bleio_adapter_get_enabled(&common_hal_bleio_adapter_obj)) {
         return;
     }
+
     bleio_adapter_reset(&common_hal_bleio_adapter_obj);
-    if (!vm_used_ble) {
-        // No user-code BLE operations were done, so we can maintain the supervisor state.
-        return;
-    }
     common_hal_bleio_adapter_set_enabled(&common_hal_bleio_adapter_obj, false);
     bonding_reset();
     supervisor_start_bluetooth();

ports/nrf/common-hal/_bleio/__init__.h

@@ -45,7 +45,4 @@ void check_nrf_error(uint32_t err_code);
 void check_gatt_status(uint16_t gatt_status);
 void check_sec_status(uint8_t sec_status);
 
-// Track if the user code modified the BLE state to know if we need to undo it on reload.
-extern bool vm_used_ble;
-
 #endif // MICROPY_INCLUDED_NRF_COMMON_HAL_BLEIO_INIT_H

ports/nrf/common-hal/_bleio/bonding.c

@@ -139,6 +139,20 @@ STATIC bonding_block_t *find_existing_block(bool is_central, bonding_block_type_
     }
 }
 
+size_t bonding_peripheral_bond_count(void) {
+    bonding_block_t *block = NULL;
+    size_t count = 0;
+    while (1) {
+        block = next_block(block);
+        if (block == NULL) {
+            return count;
+        }
+        if (block->type != BLOCK_UNUSED && block->type != BLOCK_INVALID && !block->is_central) {
+            count++;
+        }
+    }
+}
+
 // 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_existing_block(true, BLOCK_UNUSED, EDIV_INVALID);
@@ -225,7 +239,7 @@ STATIC void write_keys_block(bleio_connection_internal_t *connection) {
         ? connection->bonding_keys.peer_enc.master_id.ediv
         : connection->bonding_keys.own_enc.master_id.ediv;
 
-    // Is there an existing keys block that matches?
+    // Is there an existing keys block that matches the ediv?
     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) &&
@@ -236,6 +250,21 @@ STATIC void write_keys_block(bleio_connection_internal_t *connection) {
         // Data doesn't match. Invalidate block and store a new one.
         invalidate_block(existing_block);
     }
+    // Invalidate any existing blocks that match the peer address.
+    existing_block = next_block(NULL);
+    while (existing_block != NULL) {
+        if (existing_block->type == BLOCK_KEYS && connection->is_central == existing_block->is_central &&
+            existing_block->data_length == sizeof(bonding_keys_t)) {
+            const ble_gap_addr_t *existing_peer = &((const bonding_keys_t *)existing_block->data)->peer_id.id_addr_info;
+            const ble_gap_addr_t *connecting_peer = &connection->bonding_keys.peer_id.id_addr_info;
+            if (memcmp(existing_peer->addr, connecting_peer->addr, 6) == 0 &&
+                memcmp(existing_block->data, &connection->bonding_keys, sizeof(bonding_keys_t)) != 0) {
+                // Mismatched block found. Invalidate it.
+                invalidate_block(existing_block);
+            }
+        }
+        existing_block = next_block(existing_block);
+    }
 
     bonding_block_t block_header = {
         .is_central = connection->is_central,
@@ -308,3 +337,40 @@ 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;
 }
+
+size_t bonding_load_identities(bool is_central, const ble_gap_id_key_t **keys, size_t max_length) {
+    bonding_block_t *block = NULL;
+    size_t len = 0;
+    while (len < max_length) {
+        block = next_block(block);
+        if (block == NULL) {
+            return len;
+        }
+        if (block->type != BLOCK_UNUSED &&
+            block->type != BLOCK_INVALID &&
+            block->is_central == is_central) {
+            if (sizeof(bonding_keys_t) != block->data_length) {
+                // bonding_keys_t is a fixed length, so lengths should match.
+                return len;
+            }
+            const bonding_keys_t *key_set = (const bonding_keys_t *)block->data;
+            keys[len] = &key_set->peer_id;
+            len++;
+        }
+    }
+    return len;
+}
+
+const ble_gap_enc_key_t *bonding_load_peer_encryption_key(bool is_central, const ble_gap_addr_t *peer) {
+    bonding_block_t *block = next_block(NULL);
+    while (block != NULL) {
+        if (block->type == BLOCK_KEYS && block->is_central == is_central) {
+            const bonding_keys_t *key_set = (const bonding_keys_t *)block->data;
+            if (memcmp(key_set->peer_id.id_addr_info.addr, peer->addr, 6) == 0) {
+                return &key_set->peer_enc;
+            }
+        }
+        block = next_block(block);
+    }
+    return NULL;
+}

ports/nrf/common-hal/_bleio/bonding.h

@@ -81,5 +81,8 @@ 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);
+const ble_gap_enc_key_t *bonding_load_peer_encryption_key(bool is_central, const ble_gap_addr_t *peer);
+size_t bonding_load_identities(bool is_central, const ble_gap_id_key_t **keys, size_t max_length);
+size_t bonding_peripheral_bond_count(void);
 
 #endif // MICROPY_INCLUDED_NRF_COMMON_HAL_BLEIO_BONDING_H

ports/nrf/mpconfigport.mk

@@ -47,6 +47,11 @@ CIRCUITPY_WATCHDOG ?= 1
 # Sleep and Wakeup
 CIRCUITPY_ALARM ?= 1
 
+# Turn on the BLE file service
+CIRCUITPY_BLE_FILE_SERVICE ?= 1
+
+CIRCUITPY_COMPUTED_GOTO_SAVE_SPACE ?= 1
+
 # nRF52840-specific
 
 ifeq ($(MCU_CHIP),nrf52840)

ports/nrf/supervisor/bluetooth.c

@@ -1,81 +0,0 @@
-/*
- * This file is part of the MicroPython project, http://micropython.org/
- *
- * The MIT License (MIT)
- *
- * Copyright (c) 2019 Scott Shawcroft for Adafruit Industries
- *
- * Permission is hereby granted, free of charge, to any person obtaining a copy
- * of this software and associated documentation files (the "Software"), to deal
- * in the Software without restriction, including without limitation the rights
- * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
- * copies of the Software, and to permit persons to whom the Software is
- * furnished to do so, subject to the following conditions:
- *
- * The above copyright notice and this permission notice shall be included in
- * all copies or substantial portions of the Software.
- *
- * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
- * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
- * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
- * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
- * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
- * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
- * THE SOFTWARE.
- */
-
-#include "supervisor/shared/bluetooth.h"
-#include "supervisor/bluetooth.h"
-
-// This happens in an interrupt so we need to be quick.
-bool supervisor_bluetooth_hook(ble_evt_t *ble_evt) {
-    #if CIRCUITPY_BLE_FILE_SERVICE
-    // Catch writes to filename or contents. Length is read-only.
-
-    bool done = false;
-    switch (ble_evt->header.evt_id) {
-        case BLE_GAP_EVT_CONNECTED:
-            // We run our background task even if it wasn't us connected to because we may want to
-            // advertise if the user code stopped advertising.
-            run_ble_background = true;
-            break;
-        case BLE_GAP_EVT_DISCONNECTED:
-            run_ble_background = true;
-            break;
-        case BLE_GATTS_EVT_WRITE: {
-            // A client wrote to a characteristic.
-
-            ble_gatts_evt_write_t *evt_write = &ble_evt->evt.gatts_evt.params.write;
-            // Event handle must match the handle for my characteristic.
-            if (evt_write->handle == supervisor_ble_contents_characteristic.handle) {
-                // Handle events
-                // write_to_ringbuf(self, evt_write->data, evt_write->len);
-                // First packet includes a uint16_t le for length at the start.
-                uint16_t current_length = ((uint16_t *)current_command)[0];
-                memcpy(((uint8_t *)current_command) + current_offset, evt_write->data, evt_write->len);
-                current_offset += evt_write->len;
-                current_length = ((uint16_t *)current_command)[0];
-                if (current_offset == current_length) {
-                    run_ble_background = true;
-                    done = true;
-                }
-            } else if (evt_write->handle == supervisor_ble_filename_characteristic.handle) {
-                new_filename = true;
-                run_ble_background = true;
-                done = true;
-            } else {
-                return done;
-            }
-            break;
-        }
-
-        default:
-            // For debugging.
-            // mp_printf(&mp_plat_print, "Unhandled peripheral event: 0x%04x\n", ble_evt->header.evt_id);
-            break;
-    }
-    return done;
-    #else
-    return false;
-    #endif
-}

ports/nrf/supervisor/bluetooth.h

@@ -1,36 +0,0 @@
-/*
- * This file is part of the MicroPython project, http://micropython.org/
- *
- * The MIT License (MIT)
- *
- * Copyright (c) 2019 Scott Shawcroft for Adafruit Industries
- *
- * Permission is hereby granted, free of charge, to any person obtaining a copy
- * of this software and associated documentation files (the "Software"), to deal
- * in the Software without restriction, including without limitation the rights
- * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
- * copies of the Software, and to permit persons to whom the Software is
- * furnished to do so, subject to the following conditions:
- *
- * The above copyright notice and this permission notice shall be included in
- * all copies or substantial portions of the Software.
- *
- * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
- * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
- * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
- * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
- * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
- * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
- * THE SOFTWARE.
- */
-
-#ifndef MICROPY_INCLUDED_NRF_SUPERVISOR_BLUETOOTH_H
-#define MICROPY_INCLUDED_NRF_SUPERVISOR_BLUETOOTH_H
-
-#include <stdbool.h>
-
-#include "ble.h"
-
-bool supervisor_bluetooth_hook(ble_evt_t *ble_evt);
-
-#endif // MICROPY_INCLUDED_NRF_SUPERVISOR_BLUETOOTH_H

shared-bindings/_bleio/Adapter.c

@@ -190,7 +190,10 @@ const mp_obj_property_t bleio_adapter_name_obj = {
                MP_ROM_NONE },
 };
 
-//|     def start_advertising(self, data: ReadableBuffer, *, scan_response: Optional[ReadableBuffer] = None, connectable: bool = True, anonymous: bool = False, timeout: int = 0, interval: float = 0.1, tx_power: int = 0) -> None:
+//|     def start_advertising(self, data: ReadableBuffer, *,
+//|                           scan_response: Optional[ReadableBuffer] = None, connectable: bool = True,
+//|                           anonymous: bool = False, timeout: int = 0, interval: float = 0.1,
+//|                           tx_power: int = 0, directed_to: Optional[Address] = None) -> None:
 //|         """Starts advertising until `stop_advertising` is called or if connectable, another device
 //|         connects to us.
 //|
@@ -206,13 +209,14 @@ const mp_obj_property_t bleio_adapter_name_obj = {
 //|         :param bool anonymous:  If `True` then this device's MAC address is randomized before advertising.
 //|         :param int timeout:  If set, we will only advertise for this many seconds. Zero means no timeout.
 //|         :param float interval:  advertising interval, in seconds
-//|         :param tx_power int: transmitter power while advertising in dBm"""
+//|         :param tx_power int: transmitter power while advertising in dBm
+//|         :param directed_to Address: peer to advertise directly to"""
 //|         ...
 //|
 STATIC mp_obj_t bleio_adapter_start_advertising(mp_uint_t n_args, const mp_obj_t *pos_args, mp_map_t *kw_args) {
     bleio_adapter_obj_t *self = MP_OBJ_TO_PTR(pos_args[0]);
 
-    enum { ARG_data, ARG_scan_response, ARG_connectable, ARG_anonymous, ARG_timeout, ARG_interval, ARG_tx_power };
+    enum { ARG_data, ARG_scan_response, ARG_connectable, ARG_anonymous, ARG_timeout, ARG_interval, ARG_tx_power, ARG_directed_to };
     static const mp_arg_t allowed_args[] = {
         { MP_QSTR_data, MP_ARG_REQUIRED | MP_ARG_OBJ },
         { MP_QSTR_scan_response, MP_ARG_KW_ONLY | MP_ARG_OBJ, {.u_obj = mp_const_none} },
@@ -221,6 +225,7 @@ STATIC mp_obj_t bleio_adapter_start_advertising(mp_uint_t n_args, const mp_obj_t
         { MP_QSTR_timeout, MP_ARG_KW_ONLY | MP_ARG_INT, {.u_int = 0} },
         { MP_QSTR_interval, MP_ARG_KW_ONLY | MP_ARG_OBJ, {.u_obj = MP_OBJ_NULL} },
         { MP_QSTR_tx_power, MP_ARG_KW_ONLY | MP_ARG_INT, {.u_int = 0} },
+        { MP_QSTR_directed_to, MP_ARG_KW_ONLY | MP_ARG_OBJ, {.u_obj = MP_OBJ_NULL} },
     };
 
     mp_arg_val_t args[MP_ARRAY_SIZE(allowed_args)];
@@ -252,8 +257,17 @@ STATIC mp_obj_t bleio_adapter_start_advertising(mp_uint_t n_args, const mp_obj_t
         mp_raise_bleio_BluetoothError(translate("Cannot have scan responses for extended, connectable advertisements."));
     }
 
+    bleio_address_obj_t *address = MP_OBJ_TO_PTR(args[ARG_directed_to].u_obj);
+    if (address != NULL && !connectable) {
+        mp_raise_bleio_BluetoothError(translate("Only connectable advertisements can be directed"));
+    }
+
+    if (address != NULL && !mp_obj_is_type(address, &bleio_address_type)) {
+        mp_raise_TypeError(translate("Expected an Address"));
+    }
+
     common_hal_bleio_adapter_start_advertising(self, connectable, anonymous, timeout, interval,
-        &data_bufinfo, &scan_response_bufinfo, args[ARG_tx_power].u_int);
+        &data_bufinfo, &scan_response_bufinfo, args[ARG_tx_power].u_int, address);
 
     return mp_const_none;
 }

shared-bindings/_bleio/Adapter.h

@@ -53,9 +53,17 @@ extern bool common_hal_bleio_adapter_set_address(bleio_adapter_obj_t *self, blei
 extern mp_obj_str_t *common_hal_bleio_adapter_get_name(bleio_adapter_obj_t *self);
 extern void common_hal_bleio_adapter_set_name(bleio_adapter_obj_t *self, const char *name);
 
-extern uint32_t _common_hal_bleio_adapter_start_advertising(bleio_adapter_obj_t *self, bool connectable, bool anonymous, uint32_t timeout, float interval, uint8_t *advertising_data, uint16_t advertising_data_len, uint8_t *scan_response_data, uint16_t scan_response_data_len, mp_int_t tx_power);
+extern uint32_t _common_hal_bleio_adapter_start_advertising(bleio_adapter_obj_t *self,
+    bool connectable, bool anonymous, uint32_t timeout, float interval,
+    const uint8_t *advertising_data, uint16_t advertising_data_len,
+    const uint8_t *scan_response_data, uint16_t scan_response_data_len,
+    mp_int_t tx_power, const bleio_address_obj_t *directed_to);
 
-extern void common_hal_bleio_adapter_start_advertising(bleio_adapter_obj_t *self, bool connectable, bool anonymous, uint32_t timeout, mp_float_t interval, mp_buffer_info_t *advertising_data_bufinfo, mp_buffer_info_t *scan_response_data_bufinfo, mp_int_t tx_power);
+extern void common_hal_bleio_adapter_start_advertising(bleio_adapter_obj_t *self,
+    bool connectable, bool anonymous, uint32_t timeout, mp_float_t interval,
+    mp_buffer_info_t *advertising_data_bufinfo,
+    mp_buffer_info_t *scan_response_data_bufinfo,
+    mp_int_t tx_power, const bleio_address_obj_t *directed_to);
 extern void common_hal_bleio_adapter_stop_advertising(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);
@@ -66,5 +74,6 @@ extern mp_obj_t common_hal_bleio_adapter_get_connections(bleio_adapter_obj_t *se
 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);
+extern bool common_hal_bleio_adapter_is_bonded_to_central(bleio_adapter_obj_t *self);
 
 #endif // MICROPY_INCLUDED_SHARED_BINDINGS_BLEIO_ADAPTER_H

shared-bindings/_bleio/Characteristic.h

@@ -28,6 +28,7 @@
 #ifndef MICROPY_INCLUDED_SHARED_BINDINGS_BLEIO_CHARACTERISTIC_H
 #define MICROPY_INCLUDED_SHARED_BINDINGS_BLEIO_CHARACTERISTIC_H
 
+#include "py/objtuple.h"
 #include "shared-bindings/_bleio/Attribute.h"
 #include "shared-bindings/_bleio/Descriptor.h"
 #include "shared-module/_bleio/Characteristic.h"

shared-bindings/_bleio/Connection.c

@@ -205,7 +205,7 @@ STATIC mp_obj_t bleio_connection_get_connection_interval(mp_obj_t self_in) {
 }
 STATIC MP_DEFINE_CONST_FUN_OBJ_1(bleio_connection_get_connection_interval_obj, bleio_connection_get_connection_interval);
 
-//|     attribute: int
+//|     max_packet_length: int
 //|     """The maximum number of data bytes that can be sent in a single transmission,
 //|     not including overhead bytes.
 //|

shared-bindings/_bleio/PacketBuffer.h

@@ -31,13 +31,20 @@
 
 extern const mp_obj_type_t bleio_packet_buffer_type;
 
-extern void common_hal_bleio_packet_buffer_construct(
+void common_hal_bleio_packet_buffer_construct(
     bleio_packet_buffer_obj_t *self, bleio_characteristic_obj_t *characteristic,
     size_t buffer_size, size_t max_packet_size);
-mp_int_t common_hal_bleio_packet_buffer_write(bleio_packet_buffer_obj_t *self, uint8_t *data, size_t len, uint8_t *header, size_t header_len);
+// Allocation free
+void _common_hal_bleio_packet_buffer_construct(
+    bleio_packet_buffer_obj_t *self, bleio_characteristic_obj_t *characteristic,
+    uint32_t *incoming_buffer, size_t incoming_buffer_size,
+    uint32_t *outgoing_buffer1, uint32_t *outgoing_buffer2, size_t outgoing_buffer_size,
+    void *static_handler_entry);
+mp_int_t common_hal_bleio_packet_buffer_write(bleio_packet_buffer_obj_t *self, const uint8_t *data, size_t len, uint8_t *header, size_t header_len);
 mp_int_t common_hal_bleio_packet_buffer_readinto(bleio_packet_buffer_obj_t *self, uint8_t *data, size_t len);
 mp_int_t common_hal_bleio_packet_buffer_get_incoming_packet_length(bleio_packet_buffer_obj_t *self);
 mp_int_t common_hal_bleio_packet_buffer_get_outgoing_packet_length(bleio_packet_buffer_obj_t *self);
+void common_hal_bleio_packet_buffer_flush(bleio_packet_buffer_obj_t *self);
 bool common_hal_bleio_packet_buffer_deinited(bleio_packet_buffer_obj_t *self);
 void common_hal_bleio_packet_buffer_deinit(bleio_packet_buffer_obj_t *self);
 

supervisor/shared/bluetooth.c

@@ -3,7 +3,7 @@
  *
  * The MIT License (MIT)
  *
- * Copyright (c) 2019 Scott Shawcroft for Adafruit Industries
+ * Copyright (c) 2019-2021 Scott Shawcroft for Adafruit Industries
  *
  * Permission is hereby granted, free of charge, to any person obtaining a copy
  * of this software and associated documentation files (the "Software"), to deal
@@ -25,6 +25,10 @@
  */
 
 #if !CIRCUITPY_BLE_FILE_SERVICE
+
+void supervisor_bluetooth_init(void) {
+}
+
 void supervisor_start_bluetooth(void) {
 }
 
@@ -35,66 +39,194 @@ void supervisor_bluetooth_background(void) {
 
 #include <string.h>
 
+#include "supervisor/shared/bluetooth.h"
+
 #include "extmod/vfs.h"
 #include "extmod/vfs_fat.h"
 
 #include "shared-bindings/_bleio/__init__.h"
 #include "shared-bindings/_bleio/Adapter.h"
 #include "shared-bindings/_bleio/Characteristic.h"
+#include "shared-bindings/_bleio/PacketBuffer.h"
 #include "shared-bindings/_bleio/Service.h"
 #include "shared-bindings/_bleio/UUID.h"
+#if defined(CIRCUITPY_BOOT_BUTTON)
+#include "shared-bindings/digitalio/DigitalInOut.h"
+#endif
+#include "shared-bindings/microcontroller/Processor.h"
+#include "shared-bindings/microcontroller/ResetReason.h"
+#include "shared-module/storage/__init__.h"
+
+#include "bluetooth/ble_drv.h"
 
 #include "common-hal/_bleio/__init__.h"
 
 #include "supervisor/shared/autoreload.h"
+#include "supervisor/shared/status_leds.h"
+#include "supervisor/shared/tick.h"
+#include "supervisor/usb.h"
 
 #include "py/mpstate.h"
 
-
-
 bleio_service_obj_t supervisor_ble_service;
 bleio_uuid_obj_t supervisor_ble_service_uuid;
 bleio_characteristic_obj_t supervisor_ble_version_characteristic;
 bleio_uuid_obj_t supervisor_ble_version_uuid;
-bleio_characteristic_obj_t supervisor_ble_filename_characteristic;
+bleio_characteristic_obj_t supervisor_ble_transfer_characteristic;
-bleio_uuid_obj_t supervisor_ble_filename_uuid;
+bleio_uuid_obj_t supervisor_ble_transfer_uuid;
-bleio_characteristic_obj_t supervisor_ble_length_characteristic;
-bleio_uuid_obj_t supervisor_ble_length_uuid;
-bleio_characteristic_obj_t supervisor_ble_contents_characteristic;
-bleio_uuid_obj_t supervisor_ble_contents_uuid;
 
-// This is the base UUID for CircuitPython services and characteristics.
+// This is the base UUID for the file transfer service.
-const uint8_t circuitpython_base_uuid[16] = {0x6e, 0x68, 0x74, 0x79, 0x50, 0x74, 0x69, 0x75, 0x63, 0x72, 0x69, 0x43, 0x00, 0x00, 0xaf, 0xad };
+const uint8_t file_transfer_base_uuid[16] = {0x72, 0x65, 0x66, 0x73, 0x6e, 0x61, 0x72, 0x54, 0x65, 0x6c, 0x69, 0x46, 0x00, 0x00, 0xaf, 0xad };
 // This standard advertisement advertises the CircuitPython editing service and a CIRCUITPY short name.
-uint8_t circuitpython_advertising_data[] = { 0x02, 0x01, 0x06, 0x02, 0x0a, 0x00, 0x11, 0x07, 0x6e, 0x68, 0x74, 0x79, 0x50, 0x74, 0x69, 0x75, 0x63, 0x72, 0x69, 0x43, 0x00, 0x01, 0xaf, 0xad, 0x06, 0x08, 0x43, 0x49, 0x52, 0x43, 0x55 };
+const uint8_t public_advertising_data[] = { 0x02, 0x01, 0x06, // 0-2 Flags
+                                            0x02, 0x0a, 0xd8,  // 3-5 TX power level -40
+                                            0x03, 0x02, 0xbb, 0xfe,  // 6 - 9 Incomplete service list (File Transfer service)
+                                            0x0e, 0xff, 0x22, 0x08,  // 10 - 13 Adafruit Manufacturer Data
+                                            0x0a, 0x04, 0x00,        // 14 - 16 Creator ID / Creation ID
+                                            CIRCUITPY_CREATOR_ID & 0xff,              // 17 - 20 Creator ID
+                                            (CIRCUITPY_CREATOR_ID >> 8) & 0xff,
+                                            (CIRCUITPY_CREATOR_ID >> 16) & 0xff,
+                                            (CIRCUITPY_CREATOR_ID >> 24) & 0xff,
+                                            CIRCUITPY_CREATION_ID & 0xff,              // 21 - 24 Creation ID
+                                            (CIRCUITPY_CREATION_ID >> 8) & 0xff,
+                                            (CIRCUITPY_CREATION_ID >> 16) & 0xff,
+                                            (CIRCUITPY_CREATION_ID >> 24) & 0xff,
+                                            0x05, 0x08, 0x43, 0x49, 0x52, 0x43  // 25 - 31 - Short name
+};
+const uint8_t private_advertising_data[] = { 0x02, 0x01, 0x06, // 0-2 Flags
+                                             0x02, 0x0a, 0x00 // 3-5 TX power level 0
+};
 // This scan response advertises the full CIRCUITPYXXXX device name.
 uint8_t circuitpython_scan_response_data[15] = {0x0e, 0x09, 0x43, 0x49, 0x52, 0x43, 0x55, 0x49, 0x54, 0x50, 0x59, 0x00, 0x00, 0x00, 0x00};
 mp_obj_list_t service_list;
 mp_obj_t service_list_items[1];
 mp_obj_list_t characteristic_list;
-mp_obj_t characteristic_list_items[4];
+mp_obj_t characteristic_list_items[2];
+// 2 * 10 ringbuf packets, 512 for a disk sector and 12 for the file transfer write header.
+#define PACKET_BUFFER_SIZE (2 * 10 + 512 + 12)
+// uint32_t so its aligned
+uint32_t _buffer[PACKET_BUFFER_SIZE / 4 + 1];
+uint32_t _outgoing1[BLE_GATTS_VAR_ATTR_LEN_MAX / 4];
+uint32_t _outgoing2[BLE_GATTS_VAR_ATTR_LEN_MAX / 4];
+ble_drv_evt_handler_entry_t static_handler_entry;
+bleio_packet_buffer_obj_t _transfer_packet_buffer;
+bool boot_in_discovery_mode = false;
+bool advertising = false;
 
 STATIC void supervisor_bluetooth_start_advertising(void) {
     bool is_connected = common_hal_bleio_adapter_get_connected(&common_hal_bleio_adapter_obj);
     if (is_connected) {
         return;
     }
-    // TODO: switch to Adafruit short UUID for the advertisement and add manufacturing data to distinguish ourselves from arduino.
+    bool bonded = common_hal_bleio_adapter_is_bonded_to_central(&common_hal_bleio_adapter_obj);
-    _common_hal_bleio_adapter_start_advertising(&common_hal_bleio_adapter_obj,
+    #if CIRCUITPY_USB
+    // Don't advertise when we have USB instead of BLE.
+    if (!bonded && !boot_in_discovery_mode) {
+        // mp_printf(&mp_plat_print, "skipping advertising\n");
+        return;
+    }
+    #endif
+    uint32_t timeout = 0;
+    float interval = 0.1f;
+    int tx_power = 0;
+    const uint8_t *adv = private_advertising_data;
+    size_t adv_len = sizeof(private_advertising_data);
+    const uint8_t *scan_response = NULL;
+    size_t scan_response_len = 0;
+    // Advertise with less power when doing so publicly to reduce who can hear us. This will make it
+    // harder for someone with bad intentions to pair from a distance.
+    if (!bonded) {
+        tx_power = -40;
+        adv = public_advertising_data;
+        adv_len = sizeof(public_advertising_data);
+        scan_response = circuitpython_scan_response_data;
+        scan_response_len = sizeof(circuitpython_scan_response_data);
+    }
+    uint32_t status = _common_hal_bleio_adapter_start_advertising(&common_hal_bleio_adapter_obj,
         true,
-        false, 0,
+        bonded, // Advertise anonymously if we are bonded
-        1.0,
+        timeout,
-        circuitpython_advertising_data,
+        interval,
-        sizeof(circuitpython_advertising_data),
+        adv,
-        circuitpython_scan_response_data,
+        adv_len,
-        sizeof(circuitpython_scan_response_data));
+        scan_response,
+        scan_response_len,
+        tx_power,
+        NULL);
+    // This may fail if we are already advertising.
+    advertising = status == NRF_SUCCESS;
+}
+
+#define BLE_DISCOVERY_DATA_GUARD 0xbb0000bb
+#define BLE_DISCOVERY_DATA_GUARD_MASK 0xff0000ff
+
+void supervisor_bluetooth_init(void) {
+    uint32_t reset_state = port_get_saved_word();
+    uint32_t ble_mode = 0;
+    if ((reset_state & BLE_DISCOVERY_DATA_GUARD_MASK) == BLE_DISCOVERY_DATA_GUARD) {
+        ble_mode = (reset_state & ~BLE_DISCOVERY_DATA_GUARD_MASK) >> 8;
+    }
+    const mcu_reset_reason_t reset_reason = common_hal_mcu_processor_get_reset_reason();
+    boot_in_discovery_mode = false;
+    if (reset_reason != RESET_REASON_POWER_ON &&
+        reset_reason != RESET_REASON_RESET_PIN &&
+        reset_reason != RESET_REASON_UNKNOWN &&
+        reset_reason != RESET_REASON_SOFTWARE) {
+        return;
+    }
+
+    // ble_mode = 1;
+
+    if (ble_mode == 0) {
+        port_set_saved_word(BLE_DISCOVERY_DATA_GUARD | (0x01 << 8));
+    }
+    // Wait for a while to allow for reset.
+
+    #ifdef CIRCUITPY_BOOT_BUTTON
+    digitalio_digitalinout_obj_t boot_button;
+    common_hal_digitalio_digitalinout_construct(&boot_button, CIRCUITPY_BOOT_BUTTON);
+    common_hal_digitalio_digitalinout_switch_to_input(&boot_button, PULL_UP);
+    #endif
+    uint64_t start_ticks = supervisor_ticks_ms64();
+    uint64_t diff = 0;
+    if (ble_mode != 0) {
+        #ifdef CIRCUITPY_STATUS_LED
+        new_status_color(0x0000ff);
+        #endif
+        common_hal_bleio_adapter_erase_bonding(&common_hal_bleio_adapter_obj);
+        boot_in_discovery_mode = true;
+        reset_state = 0x0;
+    }
+    while (diff < 1000) {
+        #ifdef CIRCUITPY_STATUS_LED
+        // Blink on for 100, off for 100, on for 100, off for 100 and on for 200
+        bool led_on = ble_mode != 0 || (diff % 150) <= 75;
+        if (led_on) {
+            new_status_color(0x0000ff);
+        } else {
+            new_status_color(BLACK);
+        }
+        #endif
+        #ifdef CIRCUITPY_BOOT_BUTTON
+        if (!common_hal_digitalio_digitalinout_get_value(&boot_button)) {
+            boot_in_discovery_mode = true;
+            break;
+        }
+        #endif
+        diff = supervisor_ticks_ms64() - start_ticks;
+    }
+    #if CIRCUITPY_STATUS_LED
+    new_status_color(BLACK);
+    status_led_deinit();
+    #endif
+    port_set_saved_word(reset_state);
 }
 
 void supervisor_start_bluetooth(void) {
     common_hal_bleio_adapter_set_enabled(&common_hal_bleio_adapter_obj, true);
 
     supervisor_ble_service_uuid.base.type = &bleio_uuid_type;
-    common_hal_bleio_uuid_construct(&supervisor_ble_service_uuid, 0x0100, circuitpython_base_uuid);
+    common_hal_bleio_uuid_construct(&supervisor_ble_service_uuid, 0xfebb, NULL);
 
     // We know we'll only be 1 characteristic so we can statically allocate it.
     characteristic_list.base.type = &mp_type_list;
@@ -105,9 +237,9 @@ void supervisor_start_bluetooth(void) {
 
     _common_hal_bleio_service_construct(&supervisor_ble_service, &supervisor_ble_service_uuid, false /* is secondary */, &characteristic_list);
 
-    // File length
+    // Version number
     supervisor_ble_version_uuid.base.type = &bleio_uuid_type;
-    common_hal_bleio_uuid_construct(&supervisor_ble_version_uuid, 0x0203, circuitpython_base_uuid);
+    common_hal_bleio_uuid_construct(&supervisor_ble_version_uuid, 0x0100, file_transfer_base_uuid);
     common_hal_bleio_characteristic_construct(&supervisor_ble_version_characteristic,
         &supervisor_ble_service,
         0,                                       // handle (for remote only)
@@ -126,183 +258,555 @@ void supervisor_start_bluetooth(void) {
     common_hal_bleio_characteristic_set_value(&supervisor_ble_version_characteristic, &bufinfo);
 
     // Active filename.
-    supervisor_ble_filename_uuid.base.type = &bleio_uuid_type;
+    supervisor_ble_transfer_uuid.base.type = &bleio_uuid_type;
-    common_hal_bleio_uuid_construct(&supervisor_ble_filename_uuid, 0x0200, circuitpython_base_uuid);
+    common_hal_bleio_uuid_construct(&supervisor_ble_transfer_uuid, 0x0200, file_transfer_base_uuid);
-    common_hal_bleio_characteristic_construct(&supervisor_ble_filename_characteristic,
+    common_hal_bleio_characteristic_construct(&supervisor_ble_transfer_characteristic,
         &supervisor_ble_service,
         0,                                       // handle (for remote only)
-        &supervisor_ble_filename_uuid,
+        &supervisor_ble_transfer_uuid,
-        CHAR_PROP_READ | CHAR_PROP_WRITE,
+        CHAR_PROP_READ | CHAR_PROP_WRITE_NO_RESPONSE | CHAR_PROP_NOTIFY,
-        SECURITY_MODE_OPEN,
+        SECURITY_MODE_ENC_NO_MITM,
-        SECURITY_MODE_OPEN,
+        SECURITY_MODE_ENC_NO_MITM,
-        500,                                       // max length
+        BLE_GATTS_VAR_ATTR_LEN_MAX,                  // max length
         false,                                       // fixed length
         NULL);                                       // no initial value
 
-    char code_py[] = "/code.py";
+    _common_hal_bleio_packet_buffer_construct(
-    bufinfo.buf = code_py;
+        &_transfer_packet_buffer, &supervisor_ble_transfer_characteristic,
-    bufinfo.len = sizeof(code_py);
+        _buffer, PACKET_BUFFER_SIZE,
-    common_hal_bleio_characteristic_set_value(&supervisor_ble_filename_characteristic, &bufinfo);
+        _outgoing1, _outgoing2, BLE_GATTS_VAR_ATTR_LEN_MAX,
+        &static_handler_entry);
 
-    // File length
+    // Kick off advertisments
-    supervisor_ble_length_uuid.base.type = &bleio_uuid_type;
+    supervisor_bluetooth_background();
-    common_hal_bleio_uuid_construct(&supervisor_ble_length_uuid, 0x0202, circuitpython_base_uuid);
-    common_hal_bleio_characteristic_construct(&supervisor_ble_length_characteristic,
-        &supervisor_ble_service,
-        0,                                       // handle (for remote only)
-        &supervisor_ble_length_uuid,
-        CHAR_PROP_NOTIFY | CHAR_PROP_READ,
-        SECURITY_MODE_OPEN,
-        SECURITY_MODE_NO_ACCESS,
-        4,                                       // max length
-        true,                                       // fixed length
-        NULL);                                       // no initial value
-
-    // File actions
-    supervisor_ble_contents_uuid.base.type = &bleio_uuid_type;
-    common_hal_bleio_uuid_construct(&supervisor_ble_contents_uuid, 0x0201, circuitpython_base_uuid);
-    common_hal_bleio_characteristic_construct(&supervisor_ble_contents_characteristic,
-        &supervisor_ble_service,
-        0,                                       // handle (for remote only)
-        &supervisor_ble_contents_uuid,
-        CHAR_PROP_NOTIFY | CHAR_PROP_WRITE_NO_RESPONSE | CHAR_PROP_WRITE,
-        SECURITY_MODE_OPEN,
-        SECURITY_MODE_OPEN,
-        500,                                       // max length
-        false,                                       // fixed length
-        NULL);                                       // no initial value
-
-    supervisor_bluetooth_start_advertising();
-    vm_used_ble = false;
 }
 
+#define COMMAND_SIZE 1024
+
+#define ANY_COMMAND 0x00
+#define THIS_COMMAND 0x01
+#define READ 0x10
+#define READ_DATA 0x11
+#define READ_PACING 0x12
+#define WRITE 0x20
+#define WRITE_PACING 0x21
+#define WRITE_DATA 0x22
+#define DELETE 0x30
+#define DELETE_STATUS 0x31
+#define MKDIR 0x40
+#define MKDIR_STATUS 0x41
+#define LISTDIR 0x50
+#define LISTDIR_ENTRY 0x51
+
+#define STATUS_OK 0x01
+#define STATUS_ERROR 0x02
+#define STATUS_ERROR_NO_FILE 0x03
+#define STATUS_ERROR_PROTOCOL 0x04
+
+// Used by read and write.
 FIL active_file;
-volatile bool new_filename;
-volatile bool run_ble_background;
-bool was_connected;
 
-STATIC void update_file_length(void) {
+struct read_command {
-    int32_t file_length = -1;
+    uint8_t command;
-    mp_buffer_info_t bufinfo;
+    uint8_t reserved;
-    bufinfo.buf = &file_length;
+    uint16_t path_length;
-    bufinfo.len = sizeof(file_length);
+    uint32_t chunk_offset;
-    if (active_file.obj.fs != 0) {
+    uint32_t chunk_size;
-        file_length = (int32_t)f_size(&active_file);
+    uint8_t path[];
-    }
+};
-    common_hal_bleio_characteristic_set_value(&supervisor_ble_length_characteristic, &bufinfo);
-}
 
-STATIC void open_current_file(void) {
+struct read_data {
-    if (active_file.obj.fs != 0) {
+    uint8_t command;
-        return;
+    uint8_t status;
+    uint16_t reserved;
+    uint32_t chunk_offset;
+    uint32_t total_length;
+    uint32_t data_size;
+    uint8_t data[];
+};
+
+struct read_pacing {
+    uint8_t command;
+    uint8_t status;
+    uint16_t reserved;
+    uint32_t chunk_offset;
+    uint32_t chunk_size;
+};
+
+uint8_t _process_read(const uint8_t *raw_buf, size_t command_len) {
+    struct read_command *command = (struct read_command *)raw_buf;
+    size_t header_size = 12;
+    size_t response_size = 16;
+    uint8_t data_buffer[response_size];
+    struct read_data response;
+    response.command = READ_DATA;
+    response.status = STATUS_OK;
+    if (command->path_length > (COMMAND_SIZE - response_size - 1)) { // -1 for the null we'll write
+        // TODO: throw away any more packets of path.
+        response.status = STATUS_ERROR;
+        common_hal_bleio_packet_buffer_write(&_transfer_packet_buffer, (const uint8_t *)&response, response_size, NULL, 0);
+        return ANY_COMMAND;
     }
-    uint16_t max_len = supervisor_ble_filename_characteristic.max_length;
+    // We need to receive another packet to have the full path.
-    uint8_t path[max_len];
+    if (command_len < header_size + command->path_length) {
-    size_t length = common_hal_bleio_characteristic_get_value(&supervisor_ble_filename_characteristic, path, max_len - 1);
+        return THIS_COMMAND;
-    path[length] = '\0';
+    }
+
+    char *path = (char *)((uint8_t *)command) + header_size;
+    path[command->path_length] = '\0';
 
     FATFS *fs = &((fs_user_mount_t *)MP_STATE_VM(vfs_mount_table)->obj)->fatfs;
-    f_open(fs, &active_file, (char *)path, FA_READ | FA_WRITE);
+    FRESULT result = f_open(fs, &active_file, path, FA_READ);
-
+    if (result != FR_OK) {
-    update_file_length();
+        response.status = STATUS_ERROR;
-}
+        common_hal_bleio_packet_buffer_write(&_transfer_packet_buffer, (const uint8_t *)&response, response_size, NULL, 0);
-
+        return ANY_COMMAND;
-STATIC void close_current_file(void) {
-    f_close(&active_file);
-}
-
-uint32_t current_command[1024 / sizeof(uint32_t)];
-volatile size_t current_offset;
-
-void supervisor_bluetooth_background(void) {
-    if (!run_ble_background) {
-        return;
     }
+    uint32_t total_length = f_size(&active_file);
+    // Write out the response header.
+    uint32_t offset = command->chunk_offset;
+    uint32_t chunk_size = command->chunk_size;
+    chunk_size = MIN(chunk_size, total_length - offset);
+    response.chunk_offset = offset;
+    response.total_length = total_length;
+    response.data_size = chunk_size;
+    common_hal_bleio_packet_buffer_write(&_transfer_packet_buffer, (const uint8_t *)&response, response_size, NULL, 0);
+    f_lseek(&active_file, offset);
+    // Write out the chunk contents. We can do this in small pieces because PacketBuffer
+    // will assemble them into larger packets of its own.
+    size_t chunk_end = offset + chunk_size;
+    while (offset < chunk_end) {
+        size_t quantity_read;
+        size_t read_amount = MIN(response_size, chunk_end - offset);
+        f_read(&active_file, data_buffer, read_amount, &quantity_read);
+        offset += quantity_read;
+        // TODO: Do something if the read fails
+        common_hal_bleio_packet_buffer_write(&_transfer_packet_buffer, data_buffer, quantity_read, NULL, 0);
+    }
+    if (offset >= total_length) {
+        f_close(&active_file);
+        return ANY_COMMAND;
+    }
+    return READ_PACING;
+}
+
+uint8_t _process_read_pacing(const uint8_t *command, size_t command_len) {
+    size_t response_size = 4 * sizeof(uint32_t);
+    uint32_t response[response_size / sizeof(uint32_t)];
+    uint8_t *response_bytes = (uint8_t *)response;
+    response_bytes[0] = READ_DATA;
+    response_bytes[1] = STATUS_OK;
+    uint32_t offset = ((uint32_t *)command)[1];
+    uint32_t chunk_size = ((uint32_t *)command)[2];
+    uint32_t total_length = f_size(&active_file);
+    // Write out the response header.
+    chunk_size = MIN(chunk_size, total_length - offset);
+    response[1] = offset;
+    response[2] = total_length;
+    response[3] = chunk_size;
+    common_hal_bleio_packet_buffer_write(&_transfer_packet_buffer, (const uint8_t *)&response, response_size, NULL, 0);
+    f_lseek(&active_file, offset);
+    // Write out the chunk contents. We can do this in small pieces because PacketBuffer
+    // will assemble them into larger packets of its own.
+    size_t chunk_offset = 0;
+    while (chunk_offset < chunk_size) {
+        size_t quantity_read;
+        size_t read_size = MIN(chunk_size - chunk_offset, response_size);
+        FRESULT result = f_read(&active_file, response, read_size, &quantity_read);
+        if (quantity_read == 0 || result != FR_OK) {
+            // TODO: If we can't read everything, then the file must have been shortened. Maybe we
+            // should return 0s to pad it out.
+            break;
+        }
+        common_hal_bleio_packet_buffer_write(&_transfer_packet_buffer, (const uint8_t *)&response, quantity_read, NULL, 0);
+        chunk_offset += quantity_read;
+    }
+    if ((offset + chunk_size) >= total_length) {
+        f_close(&active_file);
+        return ANY_COMMAND;
+    }
+    return READ_PACING;
+}
+
+// Used by write and write data to know when the write is complete.
+size_t total_write_length;
+
+struct write_command {
+    uint8_t command;
+    uint8_t reserved;
+    uint16_t path_length;
+    uint32_t offset;
+    uint32_t total_length;
+    uint8_t path[];
+};
+
+struct write_data {
+    uint8_t command;
+    uint8_t status;
+    uint16_t reserved;
+    uint32_t offset;
+    uint32_t data_size;
+    uint8_t data[];
+};
+
+struct write_pacing {
+    uint8_t command;
+    uint8_t status;
+    uint16_t reserved;
+    uint32_t offset;
+    uint32_t free_space;
+};
+
+uint8_t _process_write(const uint8_t *raw_buf, size_t command_len) {
+    struct write_command *command = (struct write_command *)raw_buf;
+    size_t header_size = 12;
+    struct write_pacing response;
+    response.command = WRITE_PACING;
+    response.status = STATUS_OK;
+    if (command->path_length > (COMMAND_SIZE - header_size - 1)) { // -1 for the null we'll write
+        // TODO: throw away any more packets of path.
+        response.status = STATUS_ERROR;
+        common_hal_bleio_packet_buffer_write(&_transfer_packet_buffer, (const uint8_t *)&response, sizeof(struct write_pacing), NULL, 0);
+        return ANY_COMMAND;
+    }
+    // We need to receive another packet to have the full path.
+    if (command_len < header_size + command->path_length) {
+        return THIS_COMMAND;
+    }
+    total_write_length = command->total_length;
+
+    char *path = (char *)command->path;
+    path[command->path_length] = '\0';
+
+    // Check to see if USB has already been mounted. If not, then we "eject" from USB until we're done.
+    #if CIRCUITPY_USB_MSC
+    if (storage_usb_enabled() && !usb_msc_lock()) {
+        response.status = STATUS_ERROR;
+        common_hal_bleio_packet_buffer_write(&_transfer_packet_buffer, (const uint8_t *)&response, sizeof(struct write_pacing), NULL, 0);
+        return ANY_COMMAND;
+    }
+    #endif
+
+    FATFS *fs = &((fs_user_mount_t *)MP_STATE_VM(vfs_mount_table)->obj)->fatfs;
+    FRESULT result = f_open(fs, &active_file, path, FA_WRITE | FA_OPEN_ALWAYS);
+    if (result != FR_OK) {
+        response.status = STATUS_ERROR;
+        common_hal_bleio_packet_buffer_write(&_transfer_packet_buffer, (const uint8_t *)&response, sizeof(struct write_pacing), NULL, 0);
+        #if CIRCUITPY_USB_MSC
+        usb_msc_unlock();
+        #endif
+        return ANY_COMMAND;
+    }
+    // Write out the pacing response.
+
+    // Align the next chunk to a sector boundary.
+    uint32_t offset = command->offset;
+    size_t chunk_size = MIN(total_write_length - offset, 512 - (offset % 512));
+    response.offset = offset;
+    response.free_space = chunk_size;
+    common_hal_bleio_packet_buffer_write(&_transfer_packet_buffer, (const uint8_t *)&response, sizeof(struct write_pacing), NULL, 0);
+    return WRITE_DATA;
+}
+
+uint8_t _process_write_data(const uint8_t *raw_buf, size_t command_len) {
+    struct write_data *command = (struct write_data *)raw_buf;
+    size_t header_size = 12;
+    struct write_pacing response;
+    response.command = WRITE_PACING;
+    response.status = STATUS_OK;
+    if (command->data_size > (COMMAND_SIZE - header_size - 1)) { // -1 for the null we'll write
+        // TODO: throw away any more packets of path.
+        response.status = STATUS_ERROR;
+        common_hal_bleio_packet_buffer_write(&_transfer_packet_buffer, (const uint8_t *)&response, sizeof(struct write_pacing), NULL, 0);
+        #if CIRCUITPY_USB_MSC
+        usb_msc_unlock();
+        #endif
+        return ANY_COMMAND;
+    }
+    // We need to receive another packet to have the full path.
+    if (command_len < header_size + command->data_size) {
+        return THIS_COMMAND;
+    }
+    uint32_t offset = command->offset;
+    f_lseek(&active_file, offset);
+    UINT actual;
+    f_write(&active_file, command->data, command->data_size, &actual);
+    if (actual < command->data_size) { // -1 for the null we'll write
+        // TODO: throw away any more packets of path.
+        response.status = STATUS_ERROR;
+        common_hal_bleio_packet_buffer_write(&_transfer_packet_buffer, (const uint8_t *)&response, sizeof(struct write_pacing), NULL, 0);
+        #if CIRCUITPY_USB_MSC
+        usb_msc_unlock();
+        #endif
+        return ANY_COMMAND;
+    }
+    offset += command->data_size;
+    // Align the next chunk to a sector boundary.
+    size_t chunk_size = MIN(total_write_length - offset, 512);
+    response.offset = offset;
+    response.free_space = chunk_size;
+    common_hal_bleio_packet_buffer_write(&_transfer_packet_buffer, (const uint8_t *)&response, sizeof(struct write_pacing), NULL, 0);
+    if (total_write_length == offset) {
+        f_truncate(&active_file);
+        f_close(&active_file);
+        #if CIRCUITPY_USB_MSC
+        usb_msc_unlock();
+        #endif
+        // Don't reload until everything is written out of the packet buffer.
+        common_hal_bleio_packet_buffer_flush(&_transfer_packet_buffer);
+        // Trigger an autoreload
+        autoreload_now();
+        return ANY_COMMAND;
+    }
+    return WRITE_DATA;
+}
+
+struct delete_command {
+    uint8_t command;
+    uint8_t reserved;
+    uint16_t path_length;
+    uint8_t path[];
+};
+
+struct delete_response {
+    uint8_t command;
+    uint8_t status;
+};
+
+uint8_t _process_delete(const uint8_t *raw_buf, size_t command_len) {
+    const struct delete_command *command = (struct delete_command *)raw_buf;
+    size_t header_size = 4;
+    struct delete_response response;
+    response.command = DELETE_STATUS;
+    response.status = STATUS_OK;
+    if (command->path_length > (COMMAND_SIZE - header_size - 1)) { // -1 for the null we'll write
+        // TODO: throw away any more packets of path.
+        response.status = STATUS_ERROR;
+        common_hal_bleio_packet_buffer_write(&_transfer_packet_buffer, (const uint8_t *)&response, sizeof(struct delete_response), NULL, 0);
+        return ANY_COMMAND;
+    }
+    // We need to receive another packet to have the full path.
+    if (command_len < header_size + command->path_length) {
+        return THIS_COMMAND;
+    }
+    FATFS *fs = &((fs_user_mount_t *)MP_STATE_VM(vfs_mount_table)->obj)->fatfs;
+    char *path = (char *)((uint8_t *)command) + header_size;
+    path[command->path_length] = '\0';
+    FRESULT result = f_unlink(fs, path);
+    if (result != FR_OK) {
+        response.status = STATUS_ERROR;
+    }
+    common_hal_bleio_packet_buffer_write(&_transfer_packet_buffer, (const uint8_t *)&response, sizeof(struct delete_response), NULL, 0);
+    return ANY_COMMAND;
+}
+
+struct mkdir_command {
+    uint8_t command;
+    uint8_t reserved;
+    uint16_t path_length;
+    uint8_t path[];
+};
+
+struct mkdir_response {
+    uint8_t command;
+    uint8_t status;
+};
+
+uint8_t _process_mkdir(const uint8_t *raw_buf, size_t command_len) {
+    const struct mkdir_command *command = (struct mkdir_command *)raw_buf;
+    size_t header_size = 4;
+    struct mkdir_response response;
+    response.command = MKDIR_STATUS;
+    response.status = STATUS_OK;
+    if (command->path_length > (COMMAND_SIZE - header_size - 1)) { // -1 for the null we'll write
+        // TODO: throw away any more packets of path.
+        response.status = STATUS_ERROR;
+        common_hal_bleio_packet_buffer_write(&_transfer_packet_buffer, (const uint8_t *)&response, sizeof(struct mkdir_response), NULL, 0);
+        return ANY_COMMAND;
+    }
+    // We need to receive another packet to have the full path.
+    if (command_len < header_size + command->path_length) {
+        return THIS_COMMAND;
+    }
+    FATFS *fs = &((fs_user_mount_t *)MP_STATE_VM(vfs_mount_table)->obj)->fatfs;
+    char *path = (char *)((uint8_t *)command) + header_size;
+    // TODO: Check that the final character is a `/`
+    path[command->path_length - 1] = '\0';
+    FRESULT result = f_mkdir(fs, path);
+    if (result != FR_OK) {
+        response.status = STATUS_ERROR;
+    }
+    common_hal_bleio_packet_buffer_write(&_transfer_packet_buffer, (const uint8_t *)&response, sizeof(struct mkdir_response), NULL, 0);
+    return ANY_COMMAND;
+}
+
+struct listdir_command {
+    uint8_t command;
+    uint8_t reserved;
+    uint16_t path_length;
+    uint8_t path[];
+};
+
+struct listdir_entry {
+    uint8_t command;
+    uint8_t status;
+    uint16_t path_length;
+    uint32_t entry_number;
+    uint32_t entry_count;
+    uint32_t flags;
+    uint32_t file_size;
+    uint8_t path[];
+};
+
+uint8_t _process_listdir(uint8_t *raw_buf, size_t command_len) {
+    const struct listdir_command *command = (struct listdir_command *)raw_buf;
+    struct listdir_entry *entry = (struct listdir_entry *)raw_buf;
+    size_t header_size = 4;
+    size_t response_size = 5 * sizeof(uint32_t);
+    // We reuse the command buffer so that we can produce long packets without
+    // making the stack large.
+    if (command->path_length > (COMMAND_SIZE - header_size - 1)) { // -1 for the null we'll write
+        // TODO: throw away any more packets of path.
+        entry->command = LISTDIR_ENTRY;
+        entry->status = STATUS_ERROR;
+        common_hal_bleio_packet_buffer_write(&_transfer_packet_buffer, (const uint8_t *)entry, response_size, NULL, 0);
+        return ANY_COMMAND;
+    }
+    // We need to receive another packet to have the full path.
+    if (command_len < header_size + command->path_length) {
+        return THIS_COMMAND;
+    }
+
+    FATFS *fs = &((fs_user_mount_t *)MP_STATE_VM(vfs_mount_table)->obj)->fatfs;
+    char *path = (char *)command->path;
+    // -1 because fatfs doesn't want a trailing /
+    path[command->path_length - 1] = '\0';
+    // mp_printf(&mp_plat_print, "list %s\n", path);
+    FF_DIR dir;
+    FRESULT res = f_opendir(fs, &dir, path);
+
+    entry->command = LISTDIR_ENTRY;
+    entry->status = STATUS_OK;
+    entry->path_length = 0;
+    entry->entry_number = 0;
+    entry->entry_count = 0;
+    entry->flags = 0;
+
+    if (res != FR_OK) {
+        entry->status = STATUS_ERROR_NO_FILE;
+        common_hal_bleio_packet_buffer_write(&_transfer_packet_buffer, (const uint8_t *)entry, response_size, NULL, 0);
+        return ANY_COMMAND;
+    }
+    FILINFO file_info;
+    res = f_readdir(&dir, &file_info);
+    char *fn = file_info.fname;
+    size_t total_entries = 0;
+    while (res == FR_OK && fn[0] != 0) {
+        res = f_readdir(&dir, &file_info);
+        total_entries += 1;
+    }
+    // Rewind the directory.
+    f_readdir(&dir, NULL);
+    entry->entry_count = total_entries;
+    for (size_t i = 0; i < total_entries; i++) {
+        res = f_readdir(&dir, &file_info);
+        entry->entry_number = i;
+        if ((file_info.fattrib & AM_DIR) != 0) {
+            entry->flags = 1; // Directory
+            entry->file_size = 0;
+        } else {
+            entry->flags = 0;
+            entry->file_size = file_info.fsize;
+        }
+
+        size_t name_length = strlen(file_info.fname);
+        entry->path_length = name_length;
+        common_hal_bleio_packet_buffer_write(&_transfer_packet_buffer, (const uint8_t *)entry, response_size, NULL, 0);
+        size_t fn_offset = 0;
+        while (fn_offset < name_length) {
+            size_t fn_size = MIN(name_length - fn_offset, 4);
+            common_hal_bleio_packet_buffer_write(&_transfer_packet_buffer, ((uint8_t *)file_info.fname) + fn_offset, fn_size, NULL, 0);
+            fn_offset += fn_size;
+        }
+    }
+    f_closedir(&dir);
+    entry->path_length = 0;
+    entry->entry_number = entry->entry_count;
+    entry->flags = 0;
+    entry->file_size = 0;
+    common_hal_bleio_packet_buffer_write(&_transfer_packet_buffer, (const uint8_t *)entry, response_size, NULL, 0);
+    return ANY_COMMAND;
+}
+
+// Background state that must live across background calls. After the _process
+// helpers to force them to not use them.
+uint8_t current_command[COMMAND_SIZE] __attribute__ ((aligned(4)));
+volatile size_t current_offset;
+uint8_t next_command;
+bool was_connected;
+void supervisor_bluetooth_background(void) {
     bool is_connected = common_hal_bleio_adapter_get_connected(&common_hal_bleio_adapter_obj);
-    if (!was_connected && is_connected) {
+    if (was_connected && !is_connected) {
-        open_current_file();
+        f_close(&active_file);
-    } else if (was_connected && !is_connected) {
-        close_current_file();
-        new_filename = false;
     }
     was_connected = is_connected;
-    run_ble_background = false;
     if (!is_connected) {
+        next_command = 0;
         supervisor_bluetooth_start_advertising();
         return;
     }
-    if (new_filename) {
-        close_current_file();
-        open_current_file();
 
-        new_filename = false;
+    mp_int_t size = 1;
-        // get length and set the characteristic for it
+    while (size > 0) {
-    }
+        size = common_hal_bleio_packet_buffer_readinto(&_transfer_packet_buffer, current_command + current_offset, COMMAND_SIZE - current_offset);
-    uint16_t current_length = ((uint16_t *)current_command)[0];
-    if (current_length > 0 && current_length == current_offset) {
-        uint16_t command = ((uint16_t *)current_command)[1];
 
-        if (command == 1) {
+        if (size == 0) {
-            uint16_t max_len = 20; // supervisor_ble_contents_characteristic.max_length;
+            break;
-            uint8_t buf[max_len];
+        }
-            mp_buffer_info_t bufinfo;
+        // TODO: If size < 0 return an error.
-            bufinfo.buf = buf;
+        current_offset += size;
-            f_lseek(&active_file, 0);
+        // mp_printf(&mp_plat_print, "buffer[:%d]:", current_offset);
-            while (f_read(&active_file, buf, max_len, &bufinfo.len) == FR_OK) {
+        // for (size_t i = 0; i < current_offset; i++) {
-                if (bufinfo.len == 0) {
+        //     mp_printf(&mp_plat_print, " (%x %c)", current_command[i], current_command[i]);
-                    break;
+        // }
-                }
+        // mp_printf(&mp_plat_print, "\n");
-                common_hal_bleio_characteristic_set_value(&supervisor_ble_contents_characteristic, &bufinfo);
+        uint8_t current_state = current_command[0];
-            }
+        // mp_printf(&mp_plat_print, "current command 0x%02x\n", current_state);
-        } else if (command == 2) { // patch
+        // Check for protocol error.
-            uint32_t offset = current_command[1];
+        if (next_command != ANY_COMMAND && next_command != THIS_COMMAND && ((current_state & 0xf) != 0) && current_state != next_command) {
-            uint32_t remove_length = current_command[2];
+            uint8_t response[2];
-            uint32_t insert_length = current_command[3];
+            response[0] = next_command;
-            uint32_t file_length = (int32_t)f_size(&active_file);
+            response[1] = STATUS_ERROR_PROTOCOL;
-            // uint32_t data_shift_length = fileLength - offset - remove_length;
+            common_hal_bleio_packet_buffer_write(&_transfer_packet_buffer, response, 2, NULL, 0);
-            int32_t data_shift = insert_length - remove_length;
+            break;
-            uint32_t new_length = file_length + data_shift;
+        }
-
+        switch (current_state) {
-            // TODO: Make these loops smarter to read and write on sector boundaries.
+            case READ:
-            if (data_shift < 0) {
+                next_command = _process_read(current_command, current_offset);
-                for (uint32_t shift_offset = offset + insert_length; shift_offset < new_length; shift_offset++) {
+                break;
-                    uint8_t data;
+            case READ_PACING:
-                    UINT actual;
+                next_command = _process_read_pacing(current_command, current_offset);
-                    f_lseek(&active_file, shift_offset - data_shift);
+                break;
-                    f_read(&active_file, &data, 1, &actual);
+            case WRITE:
-                    f_lseek(&active_file, shift_offset);
+                next_command = _process_write(current_command, current_offset);
-                    f_write(&active_file, &data, 1, &actual);
+                break;
-                }
+            case WRITE_DATA:
-                f_truncate(&active_file);
+                next_command = _process_write_data(current_command, current_offset);
-            } else if (data_shift > 0) {
+                break;
-                f_lseek(&active_file, file_length);
+            case DELETE:
-                // Fill end with 0xff so we don't need to erase.
+                next_command = _process_delete(current_command, current_offset);
-                uint8_t data = 0xff;
+                break;
-                for (size_t i = 0; i < (size_t)data_shift; i++) {
+            case MKDIR:
-                    UINT actual;
+                next_command = _process_mkdir(current_command, current_offset);
-                    f_write(&active_file, &data, 1, &actual);
+                break;
-                }
+            case LISTDIR:
-                for (uint32_t shift_offset = new_length - 1; shift_offset >= offset + insert_length; shift_offset--) {
+                next_command = _process_listdir(current_command, current_offset);
-                    UINT actual;
+                break;
-                    f_lseek(&active_file, shift_offset - data_shift);
+        }
-                    f_read(&active_file, &data, 1, &actual);
+        // Preserve the offset if we are waiting for more from this command.
-                    f_lseek(&active_file, shift_offset);
+        if (next_command != THIS_COMMAND) {
-                    f_write(&active_file, &data, 1, &actual);
+            current_offset = 0;
-                }
-            }
-
-            f_lseek(&active_file, offset);
-            uint8_t *data = (uint8_t *)(current_command + 4);
-            UINT written;
-            f_write(&active_file, data, insert_length, &written);
-            f_sync(&active_file);
-            // Notify the new file length.
-            update_file_length();
-
-            // Trigger an autoreload
-            autoreload_now();
         }
-        current_offset = 0;
     }
 }
 

supervisor/shared/bluetooth.h

@@ -27,7 +27,14 @@
 #ifndef MICROPY_INCLUDED_SUPERVISOR_SHARED_BLUETOOTH_H
 #define MICROPY_INCLUDED_SUPERVISOR_SHARED_BLUETOOTH_H
 
+#include <stdbool.h>
+
+#include "shared-bindings/_bleio/Characteristic.h"
+
 void supervisor_bluetooth_background(void);
+void supervisor_bluetooth_init(void);
 void supervisor_start_bluetooth(void);
 
+extern bleio_characteristic_obj_t supervisor_ble_transfer_characteristic;
+
 #endif // MICROPY_INCLUDED_SUPERVISOR_SHARED_BLUETOOTH_H

supervisor/shared/safe_mode.c

@@ -28,7 +28,7 @@
 
 #include "mphalport.h"
 
-#if defined(MICROPY_HW_LED_STATUS) || defined(CIRCUITPY_BOOT_BUTTON)
+#if defined(CIRCUITPY_BOOT_BUTTON)
 #include "shared-bindings/digitalio/DigitalInOut.h"
 #endif
 #include "shared-bindings/microcontroller/Processor.h"
@@ -79,8 +79,8 @@ safe_mode_t wait_for_safe_mode_reset(void) {
     bool boot_in_safe_mode = false;
     while (diff < 1000) {
         #ifdef CIRCUITPY_STATUS_LED
-        // Blink on for 100, off for 100, on for 100, off for 100 and on for 200
+        // Blink on for 100, off for 100
-        bool led_on = diff > 100 && diff / 100 != 2 && diff / 100 != 4;
+        bool led_on = (diff % 250) < 125;
         if (led_on) {
             new_status_color(SAFE_MODE);
         } else {
@@ -102,7 +102,8 @@ safe_mode_t wait_for_safe_mode_reset(void) {
     if (boot_in_safe_mode) {
         return USER_SAFE_MODE;
     }
-    port_set_saved_word(SAFE_MODE_DATA_GUARD);
+    // Restore the original state of the saved word if no reset occured during our wait period.
+    port_set_saved_word(reset_state);
     return NO_SAFE_MODE;
 }
 

supervisor/shared/tick.c

@@ -90,11 +90,6 @@ void supervisor_background_tasks(void *unused) {
     #endif
     filesystem_background();
 
-    #if CIRCUITPY_BLEIO
-    supervisor_bluetooth_background();
-    bleio_background();
-    #endif
-
     port_background_task();
 
     assert_heap_ok();

supervisor/shared/usb/usb_msc_flash.c

@@ -34,6 +34,7 @@
 #include "lib/oofatfs/ff.h"
 #include "py/mpstate.h"
 
+#include "shared-module/storage/__init__.h"
 #include "supervisor/filesystem.h"
 #include "supervisor/shared/autoreload.h"
 
@@ -41,16 +42,29 @@
 
 static bool ejected[1] = {true};
 
-void usb_msc_mount(void) {
+// Lock to track if something else is using the filesystem when USB is plugged in. If so, the drive
-    // Reset the ejection tracking every time we're plugged into USB. This allows for us to battery
+// will be made available once the lock is released.
-    // power the device, eject, unplug and plug it back in to get the drive.
+static bool _usb_msc_lock = false;
+static bool _usb_connected_while_locked = false;
+
+STATIC void _usb_msc_uneject(void) {
     for (uint8_t i = 0; i < sizeof(ejected); i++) {
         ejected[i] = false;
     }
 }
 
-void usb_msc_umount(void) {
+void usb_msc_mount(void) {
+    // Reset the ejection tracking every time we're plugged into USB. This allows for us to battery
+    // power the device, eject, unplug and plug it back in to get the drive.
+    if (_usb_msc_lock) {
+        _usb_connected_while_locked = true;
+        return;
+    }
+    _usb_msc_uneject();
+    _usb_connected_while_locked = false;
+}
 
+void usb_msc_umount(void) {
 }
 
 bool usb_msc_ejected(void) {
@@ -61,6 +75,25 @@ bool usb_msc_ejected(void) {
     return all_ejected;
 }
 
+bool usb_msc_lock(void) {
+    if ((storage_usb_enabled() && !usb_msc_ejected()) || _usb_msc_lock) {
+        return false;
+    }
+    _usb_msc_lock = true;
+    return true;
+}
+
+void usb_msc_unlock(void) {
+    if (!_usb_msc_lock) {
+        // Mismatched unlock.
+        return;
+    }
+    if (_usb_connected_while_locked) {
+        _usb_msc_uneject();
+    }
+    _usb_msc_lock = false;
+}
+
 // The root FS is always at the end of the list.
 static fs_user_mount_t *get_vfs(int lun) {
     // TODO(tannewt): Return the mount which matches the lun where 0 is the end

supervisor/supervisor.mk

@@ -28,7 +28,13 @@ SPI_FLASH_FILESYSTEM ?= 0
 CFLAGS += -DSPI_FLASH_FILESYSTEM=$(SPI_FLASH_FILESYSTEM)
 
 ifeq ($(CIRCUITPY_BLEIO),1)
-	SRC_SUPERVISOR += supervisor/shared/bluetooth.c supervisor/bluetooth.c
+	SRC_SUPERVISOR += supervisor/shared/bluetooth.c
+  ifeq ($(CIRCUITPY_BLE_FILE_SERVICE),1)
+    CIRCUITPY_CREATOR_ID ?= $(USB_VID)
+    CIRCUITPY_CREATION_ID ?= $(USB_PID)
+    CFLAGS += -DCIRCUITPY_CREATOR_ID=$(CIRCUITPY_CREATOR_ID)
+    CFLAGS += -DCIRCUITPY_CREATION_ID=$(CIRCUITPY_CREATION_ID)
+  endif
 endif
 
 # Choose which flash filesystem impl to use.

supervisor/usb.h

@@ -78,6 +78,11 @@ void usb_setup_with_vm(void);
 void usb_msc_mount(void);
 void usb_msc_umount(void);
 bool usb_msc_ejected(void);
+
+// Locking MSC prevents presenting the drive on plug-in when in use by something
+// else (likely BLE.)
+bool usb_msc_lock(void);
+void usb_msc_unlock(void);
 #endif
 
 #endif // MICROPY_INCLUDED_SUPERVISOR_USB_H