nrf: Cleanup of the ble driver

Moved the functions to classes that they belong to.

18 files changed, 1024 insertions, 1141 deletions

diff --git a/ports/nrf/common-hal/bleio/Adapter.c b/ports/nrf/common-hal/bleio/Adapter.c
index bee9bd395..dd2fd00dd 100644
--- a/ports/nrf/common-hal/bleio/Adapter.c
+++ b/ports/nrf/common-hal/bleio/Adapter.c
@@ -28,32 +28,133 @@
 #include <stdio.h>
 #include <string.h>
 
+#include "ble.h"
 #include "ble_drv.h"
-#include "nrfx.h"
-#include "nrf_error.h"
+#include "nrfx_power.h"
+#include "nrf_nvic.h"
 #include "nrf_sdm.h"
 #include "py/nlr.h"
-#include "shared-module/bleio/Address.h"
+#include "shared-bindings/bleio/Adapter.h"
+
+STATIC void softdevice_assert_handler(uint32_t id, uint32_t pc, uint32_t info) {
+    nlr_raise(mp_obj_new_exception_msg_varg(&mp_type_AssertionError,
+         "Soft device assert, id: 0x%08lX, pc: 0x%08lX", id, pc));
+}
+
+STATIC uint32_t ble_stack_enable(void) {
+    nrf_clock_lf_cfg_t clock_config = {
+        .source = NRF_CLOCK_LF_SRC_XTAL,
+#if (BLE_API_VERSION == 4)
+        .accuracy = NRF_CLOCK_LF_ACCURACY_20_PPM
+#else
+        .xtal_accuracy = NRF_CLOCK_LF_XTAL_ACCURACY_20_PPM
+#endif
+    };
+
+#if (BLUETOOTH_SD == 140)
+    // The SD takes over the POWER IRQ and will fail if the IRQ is already in use
+    nrfx_power_uninit();
+#endif
+
+    uint32_t err_code = sd_softdevice_enable(&clock_config, softdevice_assert_handler);
+    if (err_code != NRF_SUCCESS)
+        return err_code;
+
+    err_code = sd_nvic_EnableIRQ(SD_EVT_IRQn);
+    if (err_code != NRF_SUCCESS)
+        return err_code;
+
+    uint32_t app_ram_start;
+#if (BLE_API_VERSION == 2)
+    ble_enable_params_t ble_enable_params = {
+        .gatts_enable_params.attr_tab_size = BLE_GATTS_ATTR_TAB_SIZE_DEFAULT,
+        .gap_enable_params.central_conn_count = 1,
+        .gap_enable_params.periph_conn_count = 1,
+    };
+
+    app_ram_start = 0x200039c0;
+    err_code = sd_ble_enable(&ble_enable_params, &app_ram_start);
+#else
+    app_ram_start = 0x20004000;
+
+    ble_cfg_t ble_conf;
+    ble_conf.conn_cfg.conn_cfg_tag = BLE_CONN_CFG_TAG_CUSTOM;
+    ble_conf.conn_cfg.params.gap_conn_cfg.conn_count = BLE_GAP_CONN_COUNT_DEFAULT;
+    ble_conf.conn_cfg.params.gap_conn_cfg.event_length = BLE_GAP_EVENT_LENGTH_DEFAULT;
+    err_code = sd_ble_cfg_set(BLE_CONN_CFG_GAP, &ble_conf, app_ram_start);
+    if (err_code != NRF_SUCCESS)
+        return err_code;
+
+    memset(&ble_conf, 0, sizeof(ble_conf));
+    ble_conf.gap_cfg.role_count_cfg.periph_role_count = 1;
+    ble_conf.gap_cfg.role_count_cfg.central_role_count = 1;
+    err_code = sd_ble_cfg_set(BLE_GAP_CFG_ROLE_COUNT, &ble_conf, app_ram_start);
+    if (err_code != NRF_SUCCESS)
+        return err_code;
+
+    memset(&ble_conf, 0, sizeof(ble_conf));
+    ble_conf.conn_cfg.conn_cfg_tag = BLE_CONN_CFG_TAG_CUSTOM;
+    ble_conf.conn_cfg.params.gatts_conn_cfg.hvn_tx_queue_size = MAX_TX_IN_PROGRESS;
+    err_code = sd_ble_cfg_set(BLE_CONN_CFG_GATTS, &ble_conf, app_ram_start);
+    if (err_code != NRF_SUCCESS)
+        return err_code;
+
+    err_code = sd_ble_enable(&app_ram_start);
+#endif
+
+    return err_code;
+}
 
 void common_hal_bleio_adapter_set_enabled(bool enabled) {
-    uint32_t err_code;
+    const bool is_enabled = common_hal_bleio_adapter_get_enabled();
 
+    // Don't enable or disable twice
+    if ((is_enabled && enabled) || (!is_enabled && !enabled)) {
+        return;
+    }
+
+    uint32_t err_code;
     if (enabled) {
-        err_code = ble_drv_stack_enable();
+        err_code = ble_stack_enable();
     } else {
         err_code = sd_softdevice_disable();
     }
 
     if (err_code != NRF_SUCCESS) {
         nlr_raise(mp_obj_new_exception_msg_varg(&mp_type_OSError,
-                  "Failed to change softdevice status, error: 0x%08lX", err_code));
+             "Failed to change softdevice state, error: 0x%08lX", err_code));
     }
 }
 
 bool common_hal_bleio_adapter_get_enabled(void) {
-    return ble_drv_stack_enabled();
+    uint8_t is_enabled;
+
+    const uint32_t err_code = sd_softdevice_is_enabled(&is_enabled);
+    if (err_code != NRF_SUCCESS) {
+        nlr_raise(mp_obj_new_exception_msg_varg(&mp_type_OSError,
+            "Failed to get softdevice state, error: 0x%08lX", err_code));
+    }
+
+    return is_enabled;
 }
 
 void common_hal_bleio_adapter_get_address(bleio_address_obj_t *address) {
-    ble_drv_address_get(address);
+    ble_gap_addr_t local_address;
+    uint32_t err_code;
+
+    common_hal_bleio_adapter_set_enabled(true);
+
+#if (BLE_API_VERSION == 2)
+    err_code = sd_ble_gap_address_get(&local_address);
+#else
+    err_code = sd_ble_gap_addr_get(&local_address);
+#endif
+
+    if (err_code != NRF_SUCCESS) {
+        nlr_raise(mp_obj_new_exception_msg_varg(&mp_type_OSError,
+            "Failed to get local address, error: 0x%08lX", err_code));
+    }
+
+    address->type = local_address.addr_type;
+    memcpy(address->value, local_address.addr, BLEIO_ADDRESS_BYTES);
 }
diff --git a/ports/nrf/common-hal/bleio/Characteristic.c b/ports/nrf/common-hal/bleio/Characteristic.c
index 35b4f5e24..040cacc3f 100644
--- a/ports/nrf/common-hal/bleio/Characteristic.c
+++ b/ports/nrf/common-hal/bleio/Characteristic.c
@@ -24,26 +24,173 @@
  * THE SOFTWARE.
  */
 
+#include <string.h>
+#include <stdio.h>
+
 #include "ble_drv.h"
+#include "ble_gatts.h"
+#include "nrf_soc.h"
+#include "py/nlr.h"
 #include "shared-module/bleio/Characteristic.h"
-#include "shared-module/bleio/Device.h"
+
+static volatile bleio_characteristic_obj_t *m_read_characteristic;
+static volatile uint8_t m_tx_in_progress;
+static nrf_mutex_t *m_write_mutex;
+//static volatile bool m_write_done;
+
+STATIC void gatts_write(bleio_characteristic_obj_t *characteristic, mp_buffer_info_t *bufinfo) {
+    bleio_device_obj_t *device = characteristic->service->device;
+    const uint16_t conn_handle = device->conn_handle;
+
+    ble_gatts_value_t gatts_value = {
+        .p_value = bufinfo->buf,
+        .len = bufinfo->len,
+    };
+
+    const uint32_t err_code = sd_ble_gatts_value_set(conn_handle, characteristic->handle, &gatts_value);
+    if (err_code != NRF_SUCCESS) {
+        nlr_raise(mp_obj_new_exception_msg_varg(&mp_type_OSError,
+             "Failed to write gatts value, status: 0x%08lX", err_code));
+    }
+}
+
+STATIC void gatts_notify(bleio_characteristic_obj_t *characteristic, mp_buffer_info_t *bufinfo) {
+    bleio_device_obj_t *device = characteristic->service->device;
+    uint16_t hvx_len = bufinfo->len;
+
+    ble_gatts_hvx_params_t hvx_params = {
+        .handle = characteristic->handle,
+        .type = BLE_GATT_HVX_NOTIFICATION,
+        .p_len = &hvx_len,
+        .p_data = bufinfo->buf,
+    };
+
+    while (m_tx_in_progress > MAX_TX_IN_PROGRESS) {
+#ifdef MICROPY_VM_HOOK_LOOP
+    MICROPY_VM_HOOK_LOOP
+#endif
+    }
+
+    const uint32_t err_code = sd_ble_gatts_hvx(device->conn_handle, &hvx_params);
+    if (err_code != NRF_SUCCESS) {
+        nlr_raise(mp_obj_new_exception_msg_varg(&mp_type_OSError,
+            "Failed to notify attribute value, status: 0x%08lX", err_code));
+    }
+
+    m_tx_in_progress += 1;
+}
+
+STATIC void gattc_read(bleio_characteristic_obj_t *characteristic) {
+    bleio_service_obj_t *service = characteristic->service;
+    bleio_device_obj_t *device = service->device;
+
+    m_read_characteristic = characteristic;
+
+    const uint32_t err_code = sd_ble_gattc_read(device->conn_handle, characteristic->handle, 0);
+    if (err_code != NRF_SUCCESS) {
+        nlr_raise(mp_obj_new_exception_msg_varg(&mp_type_OSError,
+                  "Failed to read attribute value, status: 0x%08lX", err_code));
+    }
+
+    while (m_read_characteristic != NULL) {
+#ifdef MICROPY_VM_HOOK_LOOP
+    MICROPY_VM_HOOK_LOOP
+#endif
+    }
+}
+
+STATIC void gattc_write(bleio_characteristic_obj_t *characteristic, mp_buffer_info_t *bufinfo) {
+    bleio_device_obj_t *device = characteristic->service->device;
+    uint16_t conn_handle = device->conn_handle;
+    uint32_t err_code;
+
+    ble_gattc_write_params_t write_params;
+    write_params.write_op = BLE_GATT_OP_WRITE_REQ;
+
+    if (characteristic->props.write_wo_resp) {
+        write_params.write_op = BLE_GATT_OP_WRITE_CMD;
+    }
+
+    write_params.flags    = BLE_GATT_EXEC_WRITE_FLAG_PREPARED_CANCEL;
+    write_params.handle   = characteristic->handle;
+    write_params.offset   = 0;
+    write_params.len      = bufinfo->len;
+    write_params.p_value  = bufinfo->buf;
+
+    if (write_params.write_op == BLE_GATT_OP_WRITE_CMD) {
+        err_code = sd_mutex_acquire(m_write_mutex);
+        if (err_code != NRF_SUCCESS) {
+            nlr_raise(mp_obj_new_exception_msg_varg(&mp_type_OSError,
+                      "Failed to acquire mutex, status: 0x%08lX", err_code));
+        }
+    }
+
+    err_code = sd_ble_gattc_write(conn_handle, &write_params);
+    if (err_code != 0) {
+        nlr_raise(mp_obj_new_exception_msg_varg(&mp_type_OSError,
+            "Failed to write attribute value, status: 0x%08lX", err_code));
+    }
+
+    while (sd_mutex_acquire(m_write_mutex) == NRF_ERROR_SOC_MUTEX_ALREADY_TAKEN) {
+#ifdef MICROPY_VM_HOOK_LOOP
+    MICROPY_VM_HOOK_LOOP
+#endif
+    }
+
+    err_code = sd_mutex_release(m_write_mutex);
+    if (err_code != NRF_SUCCESS) {
+        nlr_raise(mp_obj_new_exception_msg_varg(&mp_type_OSError,
+                  "Failed to release mutex, status: 0x%08lX", err_code));
+    }
+}
+
+STATIC void on_ble_evt(ble_evt_t *ble_evt, void *param) {
+    switch (ble_evt->header.evt_id) {
+#if (BLE_API_VERSION == 4)
+        case BLE_GATTS_EVT_HVN_TX_COMPLETE:
+            m_tx_in_progress -= ble_evt->evt.gatts_evt.params.hvn_tx_complete.count;
+            break;
+#else
+        case BLE_EVT_TX_COMPLETE:
+            m_tx_in_progress -= ble_evt->evt.common_evt.params.tx_complete.count;
+            break;
+#endif
+
+        case BLE_GATTC_EVT_READ_RSP:
+        {
+            ble_gattc_evt_read_rsp_t *response = &ble_evt->evt.gattc_evt.params.read_rsp;
+            m_read_characteristic->value_data = mp_obj_new_bytearray(response->len, response->data);
+            m_read_characteristic = NULL;
+            break;
+        }
+
+        case BLE_GATTC_EVT_WRITE_RSP:
+            sd_mutex_release(m_write_mutex);
+//            m_write_done = true;
+            break;
+    }
+}
+
+void common_hal_bleio_characteristic_construct(bleio_characteristic_obj_t *self) {
+    ble_drv_add_event_handler(on_ble_evt, NULL);
+}
 
 void common_hal_bleio_characteristic_read_value(bleio_characteristic_obj_t *self) {
-    ble_drv_attr_c_read(self);
+    gattc_read(self);
 }
 
 void common_hal_bleio_characteristic_write_value(bleio_characteristic_obj_t *self, mp_buffer_info_t *bufinfo) {
-    const bleio_device_obj_t *device = MP_OBJ_TO_PTR(self->service->device);
+    const bleio_device_obj_t *device = self->service->device;
 
     if (device->is_peripheral) {
         // TODO: Add indications
         if (self->props.notify) {
-            ble_drv_attr_s_notify(self, bufinfo);
+            gatts_notify(self, bufinfo);
         } else {
-            ble_drv_attr_s_write(self, bufinfo);
+            gatts_write(self, bufinfo);
         }
     } else {
-        ble_drv_attr_c_write(self, bufinfo);
+        gattc_write(self, bufinfo);
     }
 
 }
diff --git a/ports/nrf/common-hal/bleio/Device.c b/ports/nrf/common-hal/bleio/Device.c
index 9e7b382dd..195b6cd32 100644
--- a/ports/nrf/common-hal/bleio/Device.c
+++ b/ports/nrf/common-hal/bleio/Device.c
@@ -24,49 +24,528 @@
  * THE SOFTWARE.
  */
 
+#include <string.h>
 #include <stdio.h>
 
+#include "ble.h"
 #include "ble_drv.h"
-#include "ble_gap.h"
-#include "ble_gatt.h"
-#include "ble_types.h"
+#include "ble_hci.h"
+#include "nrf_soc.h"
+#include "py/objstr.h"
 #include "py/runtime.h"
+#include "shared-bindings/bleio/Adapter.h"
 #include "shared-bindings/bleio/Characteristic.h"
 #include "shared-bindings/bleio/Device.h"
 #include "shared-bindings/bleio/Service.h"
 #include "shared-bindings/bleio/UUID.h"
 
-STATIC void gap_event_handler(bleio_device_obj_t *device, uint16_t event_id, uint16_t conn_handle, uint16_t length, uint8_t * data) {
-    if (event_id == BLE_GAP_EVT_CONNECTED) {
-        device->conn_handle = conn_handle;
-    } else if (event_id == BLE_GAP_EVT_DISCONNECTED) {
-        device->conn_handle = BLE_CONN_HANDLE_INVALID;
+#define BLE_MIN_CONN_INTERVAL        MSEC_TO_UNITS(15, UNIT_0_625_MS)
+#define BLE_MAX_CONN_INTERVAL        MSEC_TO_UNITS(300, UNIT_0_625_MS)
+#define BLE_SLAVE_LATENCY            0
+#define BLE_CONN_SUP_TIMEOUT         MSEC_TO_UNITS(4000, UNIT_10_MS)
+
+#define BLE_ADV_LENGTH_FIELD_SIZE   1
+#define BLE_ADV_AD_TYPE_FIELD_SIZE  1
+#define BLE_AD_TYPE_FLAGS_DATA_SIZE 1
+
+#ifndef BLE_GAP_ADV_MAX_SIZE
+#define BLE_GAP_ADV_MAX_SIZE         31
+#endif
+
+static bleio_service_obj_t *m_char_discovery_service;
+static volatile bool m_discovery_successful;
+static nrf_mutex_t *m_discovery_mutex;
+
+#if (BLUETOOTH_SD == 140)
+static uint8_t m_adv_handle = BLE_GAP_ADV_SET_HANDLE_NOT_SET;
+
+static uint8_t m_scan_buffer_data[BLE_GAP_SCAN_BUFFER_MIN];
+
+static ble_data_t m_scan_buffer = {
+    .p_data = m_scan_buffer_data,
+    .len = BLE_GAP_SCAN_BUFFER_MIN
+};
+#endif
+
+STATIC uint32_t set_advertisement_data(bleio_device_obj_t *device, bool connectable, mp_buffer_info_t *raw_data) {
+    common_hal_bleio_adapter_set_enabled(true);
+
+    uint8_t adv_data[BLE_GAP_ADV_MAX_SIZE];
+    uint8_t byte_pos = 0;
+    uint32_t err_code;
+
+#define ADD_FIELD(field, len) \
+    do { \
+        if (byte_pos + (len) > BLE_GAP_ADV_MAX_SIZE) { \
+            nlr_raise(mp_obj_new_exception_msg_varg(&mp_type_OSError, \
+                "Can not fit data into the advertisment packet")); \
+        } \
+        adv_data[byte_pos] = (field); \
+        byte_pos += (len); \
+    } while (0)
+
+    GET_STR_DATA_LEN(device->name, name_data, name_len);
+    if (name_len > 0) {
+        ble_gap_conn_sec_mode_t sec_mode;
+        BLE_GAP_CONN_SEC_MODE_SET_OPEN(&sec_mode);
+
+        err_code = sd_ble_gap_device_name_set(&sec_mode, name_data, name_len);
+        if (err_code != NRF_SUCCESS) {
+            return err_code;
+        }
+
+        // TODO: Shorten if too long
+
+        ADD_FIELD(BLE_ADV_AD_TYPE_FIELD_SIZE + name_len, BLE_ADV_LENGTH_FIELD_SIZE);
+        ADD_FIELD(BLE_GAP_AD_TYPE_COMPLETE_LOCAL_NAME, BLE_ADV_AD_TYPE_FIELD_SIZE);
+
+        memcpy(&adv_data[byte_pos], name_data, name_len);
+        byte_pos += name_len;
+    }
+
+    // set flags, default to disc mode
+    if (raw_data->len == 0) {
+        ADD_FIELD(BLE_ADV_AD_TYPE_FIELD_SIZE + BLE_AD_TYPE_FLAGS_DATA_SIZE, BLE_ADV_LENGTH_FIELD_SIZE);
+        ADD_FIELD(BLE_GAP_AD_TYPE_FLAGS, BLE_AD_TYPE_FLAGS_DATA_SIZE);
+        ADD_FIELD(BLE_GAP_ADV_FLAGS_LE_ONLY_GENERAL_DISC_MODE, BLE_AD_TYPE_FLAGS_DATA_SIZE);
+    } else  {
+        if (byte_pos + raw_data->len > BLE_GAP_ADV_MAX_SIZE) {
+            nlr_raise(mp_obj_new_exception_msg_varg(&mp_type_OSError,
+                "Can not fit data into the advertisment packet"));
+        }
+
+        memcpy(&adv_data[byte_pos], raw_data->buf, raw_data->len);
+        byte_pos += raw_data->len;
+    }
+
+    const mp_obj_list_t *service_list = MP_OBJ_TO_PTR(device->service_list);
+    if (service_list->len > 0) {
+        bool has_128bit_services = false;
+        bool has_16bit_services = false;
+
+        for (size_t i = 0; i < service_list->len; ++i) {
+            const bleio_service_obj_t *service = MP_OBJ_TO_PTR(service_list->items[i]);
+
+            if (service->is_secondary) {
+                continue;
+            }
+
+            if (service->uuid->type == UUID_TYPE_16BIT) {
+                has_16bit_services = true;
+            }
+
+            if (service->uuid->type == UUID_TYPE_128BIT) {
+                has_128bit_services = true;
+            }
+        }
+
+        if (has_16bit_services) {
+            const uint8_t size_byte_pos = byte_pos;
+            uint8_t uuid_total_size = 0;
+
+            // skip length byte for now, apply total length post calculation
+            byte_pos += BLE_ADV_LENGTH_FIELD_SIZE;
+
+            ADD_FIELD(BLE_GAP_AD_TYPE_16BIT_SERVICE_UUID_COMPLETE, BLE_ADV_AD_TYPE_FIELD_SIZE);
+
+            for (size_t i = 0; i < service_list->len; ++i) {
+                const bleio_service_obj_t *service = MP_OBJ_TO_PTR(service_list->items[i]);
+                uint8_t encoded_size = 0;
+
+                if ((service->uuid->type != UUID_TYPE_16BIT) || service->is_secondary) {
+                    continue;
+                }
+
+                ble_uuid_t uuid;
+                uuid.type = BLE_UUID_TYPE_BLE;
+                uuid.uuid = service->uuid->value[0] | (service->uuid->value[1] << 8);
+
+                err_code = sd_ble_uuid_encode(&uuid, &encoded_size, &adv_data[byte_pos]);
+                if (err_code != NRF_SUCCESS) {
+                    return err_code;
+                }
+
+                uuid_total_size += encoded_size;
+                byte_pos += encoded_size;
+            }
+
+            adv_data[size_byte_pos] = (BLE_ADV_AD_TYPE_FIELD_SIZE + uuid_total_size);
+        }
+
+        if (has_128bit_services) {
+            const uint8_t size_byte_pos = byte_pos;
+            uint8_t uuid_total_size = 0;
+
+            // skip length byte for now, apply total length post calculation
+            byte_pos += BLE_ADV_LENGTH_FIELD_SIZE;
+
+            ADD_FIELD(BLE_GAP_AD_TYPE_128BIT_SERVICE_UUID_COMPLETE, BLE_ADV_AD_TYPE_FIELD_SIZE);
+
+            for (size_t i = 0; i < service_list->len; ++i) {
+                const bleio_service_obj_t *service = MP_OBJ_TO_PTR(service_list->items[i]);
+                uint8_t encoded_size = 0;
+
+                if ((service->uuid->type != UUID_TYPE_128BIT) || service->is_secondary) {
+                    continue;
+                }
+
+                ble_uuid_t uuid;
+                uuid.type = service->uuid->uuid_vs_idx;
+                uuid.uuid = service->uuid->value[0] | (service->uuid->value[1] << 8);
+
+                err_code = sd_ble_uuid_encode(&uuid, &encoded_size, &adv_data[byte_pos]);
+                if (err_code != NRF_SUCCESS) {
+                    return err_code;
+                }
+
+                uuid_total_size += encoded_size;
+                byte_pos += encoded_size;
+            }
+
+            adv_data[size_byte_pos] = (BLE_ADV_AD_TYPE_FIELD_SIZE + uuid_total_size);
+        }
+    }
+
+#if (BLUETOOTH_SD == 132)
+    err_code = sd_ble_gap_adv_data_set(adv_data, byte_pos, NULL, 0);
+    if (err_code != NRF_SUCCESS) {
+        return err_code;
+    }
+#endif
+
+    static ble_gap_adv_params_t m_adv_params = {
+        .interval = MSEC_TO_UNITS(100, UNIT_0_625_MS),
+#if (BLUETOOTH_SD == 140)
+        .properties.type = BLE_GAP_ADV_TYPE_CONNECTABLE_SCANNABLE_UNDIRECTED,
+        .duration = BLE_GAP_ADV_TIMEOUT_GENERAL_UNLIMITED,
+        .filter_policy = BLE_GAP_ADV_FP_ANY,
+        .primary_phy = BLE_GAP_PHY_1MBPS,
+#else
+        .type = BLE_GAP_ADV_TYPE_ADV_IND,
+        .fp = BLE_GAP_ADV_FP_ANY,
+#endif
+    };
+
+    if (!connectable) {
+#if (BLUETOOTH_SD == 140)
+        m_adv_params.properties.type = BLE_GAP_ADV_TYPE_NONCONNECTABLE_NONSCANNABLE_UNDIRECTED;
+#else
+        m_adv_params.type = BLE_GAP_ADV_TYPE_ADV_NONCONN_IND;
+#endif
     }
+
+    common_hal_bleio_device_stop_advertising(device);
+
+#if (BLUETOOTH_SD == 140)
+    const ble_gap_adv_data_t ble_gap_adv_data = {
+        .adv_data.p_data = adv_data,
+        .adv_data.len = byte_pos,
+    };
+
+    err_code = sd_ble_gap_adv_set_configure(&m_adv_handle, &ble_gap_adv_data, &m_adv_params);
+    if (err_code != NRF_SUCCESS) {
+        return err_code;
+    }
+
+    err_code = sd_ble_gap_adv_start(m_adv_handle, BLE_CONN_CFG_TAG_CUSTOM);
+#elif (BLUETOOTH_SD == 132 && BLE_API_VERSION == 4)
+    err_code = sd_ble_gap_adv_start(&m_adv_params, BLE_CONN_CFG_TAG_CUSTOM);
+#else
+    err_code = sd_ble_gap_adv_start(&m_adv_params);
+#endif
+
+    return err_code;
 }
 
-STATIC void gatts_event_handler(bleio_device_obj_t *device, uint16_t event_id, uint16_t attr_handle, uint16_t length, uint8_t * data) {
+STATIC bool discover_services(bleio_device_obj_t *device, uint16_t start_handle) {
+    m_discovery_successful = false;
+
+    uint32_t err_code = sd_ble_gattc_primary_services_discover(device->conn_handle, start_handle, NULL);
+    if (err_code != NRF_SUCCESS) {
+        nlr_raise(mp_obj_new_exception_msg_varg(&mp_type_OSError,
+                  "Failed to discover serivices, status: 0x%08lX", err_code));
+    }
+
+    err_code = sd_mutex_acquire(m_discovery_mutex);
+    if (err_code != NRF_SUCCESS) {
+        nlr_raise(mp_obj_new_exception_msg_varg(&mp_type_OSError,
+                  "Failed to acquire mutex, status: 0x%08lX", err_code));
+    }
+
+    while (sd_mutex_acquire(m_discovery_mutex) == NRF_ERROR_SOC_MUTEX_ALREADY_TAKEN) {
+#ifdef MICROPY_VM_HOOK_LOOP
+    MICROPY_VM_HOOK_LOOP
+#endif
+    }
+
+    err_code = sd_mutex_release(m_discovery_mutex);
+    if (err_code != NRF_SUCCESS) {
+        nlr_raise(mp_obj_new_exception_msg_varg(&mp_type_OSError,
+                  "Failed to release mutex, status: 0x%08lX", err_code));
+    }
 
+    return m_discovery_successful;
 }
 
-void common_hal_bleio_device_start_advertising(bleio_device_obj_t *device, bleio_advertisement_data_t *adv_data) {
-    if (adv_data->connectable) {
-        ble_drv_gap_event_handler_set(device, gap_event_handler);
-        ble_drv_gatts_event_handler_set(device, gatts_event_handler);
+STATIC bool discover_characteristics(bleio_device_obj_t *device, bleio_service_obj_t *service, uint16_t start_handle) {
+    m_char_discovery_service = service;
+
+    ble_gattc_handle_range_t handle_range;
+    handle_range.start_handle = start_handle;
+    handle_range.end_handle = service->end_handle;
+
+    m_discovery_successful = false;
+
+    uint32_t err_code = sd_ble_gattc_characteristics_discover(device->conn_handle, &handle_range);
+    if (err_code != 0) {
+        return false;
+    }
+
+    err_code = sd_mutex_acquire(m_discovery_mutex);
+    if (err_code != NRF_SUCCESS) {
+        nlr_raise(mp_obj_new_exception_msg_varg(&mp_type_OSError,
+                  "Failed to acquire mutex, status: 0x%08lX", err_code));
     }
 
-    ble_drv_advertise_data(adv_data);
+    while (sd_mutex_acquire(m_discovery_mutex) == NRF_ERROR_SOC_MUTEX_ALREADY_TAKEN) {
+#ifdef MICROPY_VM_HOOK_LOOP
+    MICROPY_VM_HOOK_LOOP
+#endif
+    }
+
+    err_code = sd_mutex_release(m_discovery_mutex);
+    if (err_code != NRF_SUCCESS) {
+        nlr_raise(mp_obj_new_exception_msg_varg(&mp_type_OSError,
+                  "Failed to release mutex, status: 0x%08lX", err_code));
+    }
+
+    return m_discovery_successful;
+}
+
+STATIC void on_primary_srv_discovery_rsp(ble_gattc_evt_prim_srvc_disc_rsp_t *response, bleio_device_obj_t *device) {
+    for (size_t i = 0; i < response->count; ++i) {
+        const ble_gattc_service_t *gattc_service = &response->services[i];
+
+        bleio_service_obj_t *service = m_new_obj(bleio_service_obj_t);
+        service->base.type = &bleio_service_type;
+        service->device = device;
+        service->char_list = mp_obj_new_list(0, NULL);
+        service->start_handle = gattc_service->handle_range.start_handle;
+        service->end_handle = gattc_service->handle_range.end_handle;
+        service->handle = gattc_service->handle_range.start_handle;
+
+        bleio_uuid_obj_t *uuid = m_new_obj(bleio_uuid_obj_t);
+        uuid->base.type = &bleio_uuid_type;
+        uuid->type = (gattc_service->uuid.type == BLE_UUID_TYPE_BLE) ? UUID_TYPE_16BIT : UUID_TYPE_128BIT;
+        uuid->value[0] = gattc_service->uuid.uuid & 0xFF;
+        uuid->value[1] = gattc_service->uuid.uuid >> 8;
+        service->uuid = uuid;
+
+        mp_obj_list_append(device->service_list, service);
+    }
+
+    if (response->count > 0) {
+        m_discovery_successful = true;
+    }
+
+    const uint32_t err_code = sd_mutex_release(m_discovery_mutex);
+    if (err_code != NRF_SUCCESS) {
+        nlr_raise(mp_obj_new_exception_msg_varg(&mp_type_OSError,
+                  "Failed to release mutex, status: 0x%08lX", err_code));
+    }
+}
+
+STATIC void on_char_discovery_rsp(ble_gattc_evt_char_disc_rsp_t *response, bleio_device_obj_t *device) {
+    for (size_t i = 0; i < response->count; ++i) {
+        const ble_gattc_char_t *gattc_char = &response->chars[i];
+
+        bleio_characteristic_obj_t *characteristic = m_new_obj(bleio_characteristic_obj_t);
+        characteristic->base.type = &bleio_characteristic_type;
+
+        bleio_uuid_obj_t *uuid = m_new_obj(bleio_uuid_obj_t);
+        uuid->base.type = &bleio_uuid_type;
+        uuid->type = (gattc_char->uuid.type == BLE_UUID_TYPE_BLE) ? UUID_TYPE_16BIT : UUID_TYPE_128BIT;
+        uuid->value[0] = gattc_char->uuid.uuid & 0xFF;
+        uuid->value[1] = gattc_char->uuid.uuid >> 8;
+        characteristic->uuid = uuid;
+
+        characteristic->props.broadcast = gattc_char->char_props.broadcast;
+        characteristic->props.indicate = gattc_char->char_props.indicate;
+        characteristic->props.notify = gattc_char->char_props.notify;
+        characteristic->props.read = gattc_char->char_props.read;
+        characteristic->props.write = gattc_char->char_props.write;
+        characteristic->props.write_wo_resp = gattc_char->char_props.write_wo_resp;
+        characteristic->handle = gattc_char->handle_value;
+
+        characteristic->service_handle = m_char_discovery_service->handle;
+        characteristic->service = m_char_discovery_service;
+
+        mp_obj_list_append(m_char_discovery_service->char_list, MP_OBJ_FROM_PTR(characteristic));
+    }
+
+    if (response->count > 0) {
+        m_discovery_successful = true;
+    }
+
+    const uint32_t err_code = sd_mutex_release(m_discovery_mutex);
+    if (err_code != NRF_SUCCESS) {
+        nlr_raise(mp_obj_new_exception_msg_varg(&mp_type_OSError,
+                  "Failed to release mutex, status: 0x%08lX", err_code));
+    }
+}
+
+STATIC void on_adv_report(ble_gap_evt_adv_report_t *report, bleio_device_obj_t *device) {
+    uint32_t err_code;
+
+    if (memcmp(report->peer_addr.addr, device->address.value, BLEIO_ADDRESS_BYTES) != 0) {
+#if (BLUETOOTH_SD == 140)
+        err_code = sd_ble_gap_scan_start(NULL, &m_scan_buffer);
+        if (err_code != NRF_SUCCESS) {
+            nlr_raise(mp_obj_new_exception_msg_varg(&mp_type_OSError,
+                "Failed to continue scanning, status: 0x%0xlX", err_code));
+        }
+#endif
+        return;
+    }
+
+    ble_gap_scan_params_t scan_params = {
+        .active = 1,
+        .interval = MSEC_TO_UNITS(100, UNIT_0_625_MS),
+        .window = MSEC_TO_UNITS(100, UNIT_0_625_MS),
+    };
+
+    ble_gap_addr_t addr;
+    memset(&addr, 0, sizeof(addr));
+
+    addr.addr_type = report->peer_addr.addr_type;
+    memcpy(addr.addr, report->peer_addr.addr, BLEIO_ADDRESS_BYTES);
+
+    ble_gap_conn_params_t conn_params = {
+        .min_conn_interval = BLE_MIN_CONN_INTERVAL,
+        .max_conn_interval = BLE_MAX_CONN_INTERVAL,
+        .conn_sup_timeout = BLE_CONN_SUP_TIMEOUT,
+        .slave_latency = BLE_SLAVE_LATENCY,
+    };
+
+#if (BLE_API_VERSION == 2)
+    err_code = sd_ble_gap_connect(&addr, &scan_params, &conn_params);
+#else
+    err_code = sd_ble_gap_connect(&addr, &scan_params, &conn_params, BLE_CONN_CFG_TAG_CUSTOM);
+#endif
+
+    if (err_code != NRF_SUCCESS) {
+        nlr_raise(mp_obj_new_exception_msg_varg(&mp_type_OSError,
+            "Failed to connect, status: 0x%08lX", err_code));
+    }
+}
+
+STATIC void on_ble_evt(ble_evt_t *ble_evt, void *device_in) {
+    bleio_device_obj_t *device = (bleio_device_obj_t*)device_in;
+
+    switch (ble_evt->header.evt_id) {
+        case BLE_GAP_EVT_CONNECTED:
+        {
+            ble_gap_conn_params_t conn_params;
+            device->conn_handle = ble_evt->evt.gap_evt.conn_handle;
+
+            sd_ble_gap_ppcp_get(&conn_params);
+            sd_ble_gap_conn_param_update(ble_evt->evt.gap_evt.conn_handle, &conn_params);
+            break;
+        }
+
+        case BLE_GAP_EVT_DISCONNECTED:
+            device->conn_handle = BLE_CONN_HANDLE_INVALID;
+            break;
+
+        case BLE_GAP_EVT_ADV_REPORT:
+            on_adv_report(&ble_evt->evt.gap_evt.params.adv_report, device);
+            break;
+
+        case BLE_GATTC_EVT_PRIM_SRVC_DISC_RSP:
+            on_primary_srv_discovery_rsp(&ble_evt->evt.gattc_evt.params.prim_srvc_disc_rsp, device);
+            break;
+
+        case BLE_GATTC_EVT_CHAR_DISC_RSP:
+            on_char_discovery_rsp(&ble_evt->evt.gattc_evt.params.char_disc_rsp, device);
+            break;
+
+        case BLE_GATTS_EVT_SYS_ATTR_MISSING:
+            sd_ble_gatts_sys_attr_set(ble_evt->evt.gatts_evt.conn_handle, NULL, 0, 0);
+            break;
+
+#if (BLE_API_VERSION == 4)
+        case BLE_GATTS_EVT_EXCHANGE_MTU_REQUEST:
+            sd_ble_gatts_exchange_mtu_reply(device->conn_handle, BLE_GATT_ATT_MTU_DEFAULT);
+            break;
+#endif
+
+        case BLE_GAP_EVT_SEC_PARAMS_REQUEST:
+            sd_ble_gap_sec_params_reply(device->conn_handle, BLE_GAP_SEC_STATUS_PAIRING_NOT_SUPP, NULL, NULL);
+            break;
+
+        case BLE_GAP_EVT_CONN_PARAM_UPDATE_REQUEST:
+        {
+            ble_gap_evt_conn_param_update_request_t *request = &ble_evt->evt.gap_evt.params.conn_param_update_request;
+            sd_ble_gap_conn_param_update(device->conn_handle, &request->conn_params);
+            break;
+        }
+    }
+}
+
+void common_hal_bleio_device_start_advertising(bleio_device_obj_t *device, bool connectable, mp_buffer_info_t *raw_data) {
+    if (connectable) {
+        ble_drv_add_event_handler(on_ble_evt, device);
+    }
+
+    const uint32_t err_code = set_advertisement_data(device, connectable, raw_data);
+    if (err_code != NRF_SUCCESS) {
+        nlr_raise(mp_obj_new_exception_msg_varg(&mp_type_OSError,
+            "Failed to start advertisment, status: 0x%08lX", err_code));
+    }
 }
 
 void common_hal_bleio_device_stop_advertising(bleio_device_obj_t *device) {
-    (void)device;
+    uint32_t err_code;
+
+#if (BLUETOOTH_SD == 140)
+    if (m_adv_handle == BLE_GAP_ADV_SET_HANDLE_NOT_SET)
+        return;
 
-    ble_drv_advertise_stop();
+    err_code = sd_ble_gap_adv_stop(m_adv_handle);
+#else
+    err_code = sd_ble_gap_adv_stop();
+#endif
+
+    if ((err_code != NRF_SUCCESS) && (err_code != NRF_ERROR_INVALID_STATE)) {
+        nlr_raise(mp_obj_new_exception_msg_varg(&mp_type_OSError,
+            "Failed to stop advertisment, status: 0x%08lX", err_code));
+    }
 }
 
 void common_hal_bleio_device_connect(bleio_device_obj_t *device) {
-    ble_drv_gap_event_handler_set(device, gap_event_handler);
+    ble_drv_add_event_handler(on_ble_evt, device);
 
-    ble_drv_connect(device);
+    ble_gap_scan_params_t scan_params = {
+        .interval = MSEC_TO_UNITS(100, UNIT_0_625_MS),
+        .window = MSEC_TO_UNITS(100, UNIT_0_625_MS),
+#if (BLUETOOTH_SD == 140)
+        .scan_phys = BLE_GAP_PHY_1MBPS,
+#endif
+    };
+
+    common_hal_bleio_adapter_set_enabled(true);
+
+    uint32_t err_code;
+#if (BLUETOOTH_SD == 140)
+    err_code = sd_ble_gap_scan_start(&scan_params, &m_scan_buffer);
+#else
+    err_code = sd_ble_gap_scan_start(&scan_params);
+#endif
+
+    if (err_code != NRF_SUCCESS) {
+        nlr_raise(mp_obj_new_exception_msg_varg(&mp_type_OSError,
+            "Failed to start scanning, status: 0x%0xlX", err_code));
+    }
 
     while (device->conn_handle == BLE_CONN_HANDLE_INVALID) {
 #ifdef MICROPY_VM_HOOK_LOOP
@@ -76,13 +555,23 @@ void common_hal_bleio_device_connect(bleio_device_obj_t *device) {
 
     // TODO: read name
 
+    if (m_discovery_mutex == NULL) {
+        m_discovery_mutex = m_new_ll(nrf_mutex_t, 1);
+
+        err_code = sd_mutex_new(m_discovery_mutex);
+        if (err_code != NRF_SUCCESS) {
+            nlr_raise(mp_obj_new_exception_msg_varg(&mp_type_OSError,
+                "Failed to create mutex, status: 0x%0xlX", err_code));
+        }
+    }
+
     // find services
-    bool found_service = ble_drv_discover_services(device, BLE_GATT_HANDLE_START);
+    bool found_service = discover_services(device, BLE_GATT_HANDLE_START);
     while (found_service) {
         const mp_obj_list_t *service_list = MP_OBJ_TO_PTR(device->service_list);
         const bleio_service_obj_t *service = service_list->items[service_list->len - 1];
 
-        found_service = ble_drv_discover_services(device, service->end_handle + 1);
+        found_service = discover_services(device, service->end_handle + 1);
     }
 
     // find characteristics in each service
@@ -90,7 +579,7 @@ void common_hal_bleio_device_connect(bleio_device_obj_t *device) {
     for (size_t i = 0; i < service_list->len; ++i) {
         bleio_service_obj_t *service = service_list->items[i];
 
-        bool found_char = ble_drv_discover_characteristic(device, service, service->start_handle);
+        bool found_char = discover_characteristics(device, service, service->start_handle);
         while (found_char) {
             const mp_obj_list_t *char_list = MP_OBJ_TO_PTR(service->char_list);
             const bleio_characteristic_obj_t *characteristic = char_list->items[char_list->len - 1];
@@ -100,11 +589,11 @@ void common_hal_bleio_device_connect(bleio_device_obj_t *device) {
                 break;
             }
 
-            found_char = ble_drv_discover_characteristic(device, service, next_handle);
+            found_char = discover_characteristics(device, service, next_handle);
         }
     }
 }
 
 void common_hal_bleio_device_disconnect(bleio_device_obj_t *device) {
-    ble_drv_disconnect(device);
+    sd_ble_gap_disconnect(device->conn_handle, BLE_HCI_REMOTE_USER_TERMINATED_CONNECTION);
 }
diff --git a/ports/nrf/common-hal/bleio/Scanner.c b/ports/nrf/common-hal/bleio/Scanner.c
index 150c417a3..17af49621 100644
--- a/ports/nrf/common-hal/bleio/Scanner.c
+++ b/ports/nrf/common-hal/bleio/Scanner.c
@@ -28,25 +28,83 @@
 #include <string.h>
 
 #include "ble_drv.h"
+#include "ble_gap.h"
 #include "py/mphal.h"
+#include "py/nlr.h"
+#include "shared-bindings/bleio/Adapter.h"
 #include "shared-bindings/bleio/ScanEntry.h"
 #include "shared-bindings/bleio/Scanner.h"
 #include "shared-module/bleio/ScanEntry.h"
 
-STATIC void adv_event_handler(bleio_scanner_obj_t *self, bleio_scanentry_obj_t *entry) {
+#if (BLUETOOTH_SD == 140)
+static uint8_t m_scan_buffer_data[BLE_GAP_SCAN_BUFFER_MIN];
+
+static ble_data_t m_scan_buffer = {
+    m_scan_buffer_data,
+    BLE_GAP_SCAN_BUFFER_MIN
+};
+#endif
+
+STATIC void on_ble_evt(ble_evt_t *ble_evt, void *scanner_in) {
+    bleio_scanner_obj_t *scanner = (bleio_scanner_obj_t*)scanner_in;
+    ble_gap_evt_adv_report_t *report = &ble_evt->evt.gap_evt.params.adv_report;
+
+    if (ble_evt->header.evt_id != BLE_GAP_EVT_ADV_REPORT) {
+        return;
+    }
+
     // TODO: Don't add new entry for each item, group by address and update
+    bleio_scanentry_obj_t *entry = m_new_obj(bleio_scanentry_obj_t);
+    entry->base.type = &bleio_scanentry_type;
+    entry->rssi = report->rssi;
 
-    mp_obj_list_append(self->adv_reports, entry);
+    entry->address.type = report->peer_addr.addr_type;
+    memcpy(entry->address.value, report->peer_addr.addr, BLEIO_ADDRESS_BYTES);
 
-    ble_drv_scan_continue();
+#if (BLUETOOTH_SD == 140)
+    entry->data = mp_obj_new_bytearray(report->data.len, report->data.p_data);
+#else
+    entry->data = mp_obj_new_bytearray(report->dlen, report->data);
+#endif
+
+    mp_obj_list_append(scanner->adv_reports, entry);
+
+#if (BLUETOOTH_SD == 140)
+    const uint32_t err_code = sd_ble_gap_scan_start(NULL, &m_scan_buffer);
+    if (err_code != NRF_SUCCESS) {
+        nlr_raise(mp_obj_new_exception_msg_varg(&mp_type_OSError,
+            "Failed to continue scanning, status: 0x%0xlX", err_code));
+    }
+#endif
 }
 
 void common_hal_bleio_scanner_scan(bleio_scanner_obj_t *self, mp_int_t timeout) {
-    ble_drv_adv_report_handler_set(self, adv_event_handler);
+    ble_drv_add_event_handler(on_ble_evt, self);
+
+    ble_gap_scan_params_t scan_params = {
+        .interval = MSEC_TO_UNITS(self->interval, UNIT_0_625_MS),
+        .window = MSEC_TO_UNITS(self->window, UNIT_0_625_MS),
+#if (BLUETOOTH_SD == 140)
+        .scan_phys = BLE_GAP_PHY_1MBPS,
+#endif
+    };
+
+    common_hal_bleio_adapter_set_enabled(true);
 
-    ble_drv_scan_start(self->interval, self->window);
+    uint32_t err_code;
+#if (BLUETOOTH_SD == 140)
+    err_code = sd_ble_gap_scan_start(&scan_params, &m_scan_buffer);
+#else
+    err_code = sd_ble_gap_scan_start(&scan_params);
+#endif
 
-    mp_hal_delay_ms(timeout);
+    if (err_code != NRF_SUCCESS) {
+        nlr_raise(mp_obj_new_exception_msg_varg(&mp_type_OSError,
+            "Failed to start scanning, status: 0x%0xlX", err_code));
+    }
 
-    ble_drv_scan_stop();
+    if (timeout > 0) {
+        mp_hal_delay_ms(timeout);
+        sd_ble_gap_scan_stop();
+    }
 }
diff --git a/ports/nrf/common-hal/bleio/Service.c b/ports/nrf/common-hal/bleio/Service.c
index b6e0595b1..9e376b333 100644
--- a/ports/nrf/common-hal/bleio/Service.c
+++ b/ports/nrf/common-hal/bleio/Service.c
@@ -25,14 +25,91 @@
  */
 
 #include "ble_drv.h"
-#include "shared-module/bleio/Service.h"
+#include "ble.h"
+#include "py/nlr.h"
+#include "shared-bindings/bleio/Service.h"
+#include "shared-bindings/bleio/Adapter.h"
 
 void common_hal_bleio_service_construct(bleio_service_obj_t *self) {
-    ble_drv_service_add(self);
+    ble_uuid_t uuid = {
+        .type = BLE_UUID_TYPE_BLE,
+        .uuid = self->uuid->value[0] | (self->uuid->value[1] << 8)
+    };
+
+    if (self->uuid->type == UUID_TYPE_128BIT) {
+        uuid.type = self->uuid->uuid_vs_idx;
+    }
+
+    uint8_t service_type = BLE_GATTS_SRVC_TYPE_PRIMARY;
+    if (self->is_secondary) {
+        service_type = BLE_GATTS_SRVC_TYPE_SECONDARY;
+    }
+
+    common_hal_bleio_adapter_set_enabled(true);
+
+    const uint32_t err_code = sd_ble_gatts_service_add(service_type, &uuid, &self->handle);
+    if (err_code != NRF_SUCCESS) {
+        nlr_raise(mp_obj_new_exception_msg_varg(&mp_type_OSError,
+             "Failed to add service, status: 0x%08lX", err_code));
+    }
 }
 
 void common_hal_bleio_service_add_characteristic(bleio_service_obj_t *self, bleio_characteristic_obj_t *characteristic) {
-    if (ble_drv_characteristic_add(characteristic)) {
-        characteristic->service = self;
+    ble_gatts_char_md_t char_md = {
+        .char_props.broadcast      = characteristic->props.broadcast,
+        .char_props.read           = characteristic->props.read,
+        .char_props.write_wo_resp  = characteristic->props.write_wo_resp,
+        .char_props.write          = characteristic->props.write,
+        .char_props.notify         = characteristic->props.notify,
+        .char_props.indicate       = characteristic->props.indicate,
+    };
+
+    ble_gatts_attr_md_t cccd_md = {
+        .vloc = BLE_GATTS_VLOC_STACK,
+    };
+
+    if (char_md.char_props.notify || char_md.char_props.indicate) {
+        BLE_GAP_CONN_SEC_MODE_SET_OPEN(&cccd_md.read_perm);
+        BLE_GAP_CONN_SEC_MODE_SET_OPEN(&cccd_md.write_perm);
+
+        char_md.p_cccd_md = &cccd_md;
     }
+
+    ble_uuid_t uuid = {
+        .type = BLE_UUID_TYPE_BLE,
+        .uuid = characteristic->uuid->value[0] | (characteristic->uuid->value[1] << 8),
+    };
+
+    if (characteristic->uuid->type == UUID_TYPE_128BIT)
+        uuid.type = characteristic->uuid->uuid_vs_idx;
+
+    ble_gatts_attr_md_t attr_md = {
+        .vloc = BLE_GATTS_VLOC_STACK,
+        .vlen = 1,
+    };
+
+    BLE_GAP_CONN_SEC_MODE_SET_OPEN(&attr_md.read_perm);
+    BLE_GAP_CONN_SEC_MODE_SET_OPEN(&attr_md.write_perm);
+
+    ble_gatts_attr_t attr_char_value = {
+        .p_uuid = &uuid,
+        .p_attr_md = &attr_md,
+        .init_len = sizeof(uint8_t),
+        .max_len = (BLE_GATT_ATT_MTU_DEFAULT - 3),
+    };
+
+    ble_gatts_char_handles_t handles;
+
+    uint32_t err_code;
+    err_code = sd_ble_gatts_characteristic_add(characteristic->service_handle, &char_md, &attr_char_value, &handles);
+    if (err_code != NRF_SUCCESS) {
+        nlr_raise(mp_obj_new_exception_msg_varg(&mp_type_OSError,
+            "Failed to add characteristic, status: 0x%08lX", err_code));
+    }
+
+    characteristic->user_desc_handle = handles.user_desc_handle;
+    characteristic->cccd_handle = handles.cccd_handle;
+    characteristic->sccd_handle = handles.sccd_handle;
+    characteristic->handle = handles.value_handle;
+    characteristic->service = self;
 }
diff --git a/ports/nrf/common-hal/bleio/UUID.c b/ports/nrf/common-hal/bleio/UUID.c
index e655589a6..4d4c8c9fb 100644
--- a/ports/nrf/common-hal/bleio/UUID.c
+++ b/ports/nrf/common-hal/bleio/UUID.c
@@ -25,10 +25,13 @@
  * THE SOFTWARE.
  */
 
+#include "ble.h"
 #include "ble_drv.h"
 #include "common-hal/bleio/UUID.h"
+#include "nrf_error.h"
 #include "py/objstr.h"
 #include "py/runtime.h"
+#include "shared-bindings/bleio/Adapter.h"
 #include "shared-bindings/bleio/UUID.h"
 
 #define UUID_STR_16BIT_LEN 6
@@ -59,37 +62,44 @@ void common_hal_bleio_uuid_construct(bleio_uuid_obj_t *self, const mp_obj_t *uui
         } else if (str_len == UUID_STR_128BIT_LEN) {
             self->type = UUID_TYPE_128BIT;
 
-            uint8_t buffer[16];
-            buffer[0] = xdigit_8b_value(str_data[35], str_data[34]);
-            buffer[1] = xdigit_8b_value(str_data[33], str_data[32]);
-            buffer[2] = xdigit_8b_value(str_data[31], str_data[30]);
-            buffer[3] = xdigit_8b_value(str_data[29], str_data[28]);
-            buffer[4] = xdigit_8b_value(str_data[27], str_data[26]);
-            buffer[5] = xdigit_8b_value(str_data[25], str_data[24]);
+            ble_uuid128_t vs_uuid;
+            vs_uuid.uuid128[0] = xdigit_8b_value(str_data[35], str_data[34]);
+            vs_uuid.uuid128[1] = xdigit_8b_value(str_data[33], str_data[32]);
+            vs_uuid.uuid128[2] = xdigit_8b_value(str_data[31], str_data[30]);
+            vs_uuid.uuid128[3] = xdigit_8b_value(str_data[29], str_data[28]);
+            vs_uuid.uuid128[4] = xdigit_8b_value(str_data[27], str_data[26]);
+            vs_uuid.uuid128[5] = xdigit_8b_value(str_data[25], str_data[24]);
 
             // 23 '-'
-            buffer[6] = xdigit_8b_value(str_data[22], str_data[21]);
-            buffer[7] = xdigit_8b_value(str_data[20], str_data[19]);
+            vs_uuid.uuid128[6] = xdigit_8b_value(str_data[22], str_data[21]);
+            vs_uuid.uuid128[7] = xdigit_8b_value(str_data[20], str_data[19]);
 
             // 18 '-'
-            buffer[8] = xdigit_8b_value(str_data[17], str_data[16]);
-            buffer[9] = xdigit_8b_value(str_data[15], str_data[14]);
+            vs_uuid.uuid128[8] = xdigit_8b_value(str_data[17], str_data[16]);
+            vs_uuid.uuid128[9] = xdigit_8b_value(str_data[15], str_data[14]);
 
             // 13 '-'
-            buffer[10] = xdigit_8b_value(str_data[12], str_data[11]);
-            buffer[11] = xdigit_8b_value(str_data[10], str_data[9]);
+            vs_uuid.uuid128[10] = xdigit_8b_value(str_data[12], str_data[11]);
+            vs_uuid.uuid128[11] = xdigit_8b_value(str_data[10], str_data[9]);
 
             // 8 '-'
             self->value[0] = xdigit_8b_value(str_data[7], str_data[6]);
             self->value[1] = xdigit_8b_value(str_data[5], str_data[4]);
 
-            buffer[14] = xdigit_8b_value(str_data[3], str_data[2]);
-            buffer[15] = xdigit_8b_value(str_data[1], str_data[0]);
+            vs_uuid.uuid128[14] = xdigit_8b_value(str_data[3], str_data[2]);
+            vs_uuid.uuid128[15] = xdigit_8b_value(str_data[1], str_data[0]);
+
+            common_hal_bleio_adapter_set_enabled(true);
+
+            const uint32_t err_code = sd_ble_uuid_vs_add(&vs_uuid, &self->uuid_vs_idx);
+            if (err_code != NRF_SUCCESS) {
+                nlr_raise(mp_obj_new_exception_msg_varg(&mp_type_OSError,
+                    "Failed to add Vendor Specific UUID, status: 0x%08lX", err_code));
+            }
 
-            ble_drv_uuid_add_vs(buffer, &self->uuid_vs_idx);
         } else {
             nlr_raise(mp_obj_new_exception_msg_varg(&mp_type_ValueError,
-                      "Invalid UUID string length"));
+                "Invalid UUID string length"));
         }
 
         return;
@@ -113,10 +123,10 @@ void common_hal_bleio_uuid_construct(bleio_uuid_obj_t *self, const mp_obj_t *uui
 void common_hal_bleio_uuid_print(bleio_uuid_obj_t *self, const mp_print_t *print) {
     if (self->type == UUID_TYPE_16BIT) {
         mp_printf(print, "UUID(uuid: 0x" HEX2_FMT HEX2_FMT ")",
-                  self->value[1], self->value[0]);
+            self->value[1], self->value[0]);
     } else {
         mp_printf(print, "UUID(uuid: 0x" HEX2_FMT HEX2_FMT ", VS idx: " HEX2_FMT ")",
-                  self->value[1], self->value[0], self->uuid_vs_idx);
+            self->value[1], self->value[0], self->uuid_vs_idx);
     }
 }
 
diff --git a/ports/nrf/drivers/bluetooth/ble_drv.c b/ports/nrf/drivers/bluetooth/ble_drv.c
index 6d32ff6f9..0b6daf711 100644
--- a/ports/nrf/drivers/bluetooth/ble_drv.c
+++ b/ports/nrf/drivers/bluetooth/ble_drv.c
@@ -25,1027 +25,73 @@
  * THE SOFTWARE.
  */
 
-#if BLUETOOTH_SD
-
-#include <stdio.h>
-#include <string.h>
 #include <stdbool.h>
+#include <stdio.h>
 
-#if (BLUETOOTH_SD == 132)
-#define NRF52 // Needed for SD132 v2
-#endif
-
-#include "shared-bindings/bleio/Characteristic.h"
-#include "shared-bindings/bleio/ScanEntry.h"
-#include "shared-bindings/bleio/Service.h"
-#include "shared-bindings/bleio/UUID.h"
-#include "shared-module/bleio/Device.h"
-#include "py/objstr.h"
-#include "py/runtime.h"
-#include "supervisor/shared/translate.h"
+#include "ble.h"
 #include "ble_drv.h"
 #include "nrf_nvic.h"
 #include "nrf_sdm.h"
-#include "nrfx_power.h"
-#include "py/objstr.h"
-#include "py/runtime.h"
-#include "shared-bindings/bleio/Characteristic.h"
-#include "shared-bindings/bleio/ScanEntry.h"
-#include "shared-bindings/bleio/UUID.h"
-
-#define BLE_DRIVER_LOG printf
-
-#define BLE_ADV_LENGTH_FIELD_SIZE   1
-#define BLE_ADV_AD_TYPE_FIELD_SIZE  1
-#define BLE_AD_TYPE_FLAGS_DATA_SIZE 1
-
-#define MSEC_TO_UNITS(TIME, RESOLUTION) (((TIME) * 1000) / (RESOLUTION))
-#define UNIT_0_625_MS (625)
-#define UNIT_10_MS    (10000)
-
-#define BLE_MIN_CONN_INTERVAL        MSEC_TO_UNITS(15, UNIT_0_625_MS)
-#define BLE_MAX_CONN_INTERVAL        MSEC_TO_UNITS(300, UNIT_0_625_MS)
-#define BLE_SLAVE_LATENCY            0
-#define BLE_CONN_SUP_TIMEOUT         MSEC_TO_UNITS(4000, UNIT_10_MS)
-
-#ifndef BLE_GAP_ADV_MAX_SIZE
-#define BLE_GAP_ADV_MAX_SIZE            31
-#endif
-
-#define SD_TEST_OR_ENABLE() \
-if (ble_drv_stack_enabled() == 0) { \
-    (void)ble_drv_stack_enable(); \
-}
-
-static ble_drv_adv_evt_callback_t adv_event_handler;
-static ble_drv_gatts_evt_callback_t gatts_event_handler;
-static ble_drv_gap_evt_callback_t gap_event_handler;
-
-static bleio_characteristic_obj_t *mp_gattc_char_data_observer;
-static bleio_device_obj_t *mp_gattc_disc_service_observer;
-static bleio_service_obj_t *mp_gattc_disc_char_observer;
-static bleio_address_obj_t *mp_connect_address;
-static bleio_device_obj_t *mp_gatts_observer;
-static bleio_scanner_obj_t *mp_adv_observer;
-static bleio_device_obj_t *mp_gap_observer;
-
-static volatile bool m_primary_service_found;
-static volatile bool m_characteristic_found;
-static volatile bool m_tx_in_progress;
-static volatile bool m_write_done;
+#include "py/misc.h"
 
 nrf_nvic_state_t nrf_nvic_state = { 0 };
 
-#if (BLUETOOTH_SD == 140)
-static uint8_t m_adv_handle = BLE_GAP_ADV_SET_HANDLE_NOT_SET;
-static uint8_t m_scan_buffer_data[BLE_GAP_SCAN_BUFFER_MIN];
-
-static ble_data_t m_scan_buffer =
-{
-    m_scan_buffer_data,
-    BLE_GAP_SCAN_BUFFER_MIN
-};
-#endif
-
-void softdevice_assert_handler(uint32_t id, uint32_t pc, uint32_t info) {
-    BLE_DRIVER_LOG("ERROR: SoftDevice assert!!!");
-}
-
-uint32_t ble_drv_stack_enable(void) {
-    nrf_clock_lf_cfg_t clock_config = {
-        .source = NRF_CLOCK_LF_SRC_XTAL,
-        .rc_ctiv = 0,
-        .rc_temp_ctiv = 0,
-#if (BLE_API_VERSION == 4)
-        .accuracy = NRF_CLOCK_LF_ACCURACY_20_PPM
-#else
-        .xtal_accuracy = NRF_CLOCK_LF_XTAL_ACCURACY_20_PPM
-#endif
-    };
-
-#if (BLUETOOTH_SD == 140)
-    // The SD takes over the POWER IRQ and will fail if the IRQ is already in use
-    nrfx_power_uninit();
-#endif
-
-    uint32_t err_code = sd_softdevice_enable(&clock_config, softdevice_assert_handler);
-    if (err_code != NRF_SUCCESS)
-        BLE_DRIVER_LOG("SoftDevice enable status: " UINT_FMT "\n", (uint16_t)err_code);
-
-    err_code = sd_nvic_EnableIRQ(SD_EVT_IRQn);
-    if (err_code != NRF_SUCCESS)
-        BLE_DRIVER_LOG("IRQ enable status: " UINT_FMT "\n", (uint16_t)err_code);
-
-    // Enable BLE stack.
-    uint32_t app_ram_start;
-#if (BLE_API_VERSION == 2)
-    ble_enable_params_t ble_enable_params;
-    memset(&ble_enable_params, 0x00, sizeof(ble_enable_params));
-    ble_enable_params.gatts_enable_params.attr_tab_size = BLE_GATTS_ATTR_TAB_SIZE_DEFAULT;
-    ble_enable_params.gatts_enable_params.service_changed  = 0;
-    ble_enable_params.gap_enable_params.periph_conn_count  = 1;
-    ble_enable_params.gap_enable_params.central_conn_count = 1;
-
-    app_ram_start = 0x200039c0;
-    err_code = sd_ble_enable(&ble_enable_params, &app_ram_start); // 8K SD headroom from linker script.
-#else
-    app_ram_start = 0x20004000;
-    err_code = sd_ble_enable(&app_ram_start);
-#endif
-
-    if (err_code != NRF_SUCCESS) {
-        BLE_DRIVER_LOG("BLE ram size: " UINT_FMT "\n", (uint16_t)app_ram_start);
-        BLE_DRIVER_LOG("BLE enable status: " UINT_FMT "\n", (uint16_t)err_code);
-    }
-
-    return err_code;
-}
-
-uint8_t ble_drv_stack_enabled(void) {
-    uint8_t is_enabled;
-    uint32_t err_code = sd_softdevice_is_enabled(&is_enabled);
-
-    if (err_code != NRF_SUCCESS) {
-        BLE_DRIVER_LOG("Is enabled status: " UINT_FMT "\n", (uint16_t)err_code);
-    }
-
-    return is_enabled;
-}
-
-void ble_drv_address_get(bleio_address_obj_t *address) {
-    SD_TEST_OR_ENABLE();
-
-    ble_gap_addr_t local_ble_addr;
-#if (BLE_API_VERSION == 2)
-    uint32_t err_code = sd_ble_gap_address_get(&local_ble_addr);
-#else
-    uint32_t err_code = sd_ble_gap_addr_get(&local_ble_addr);
-#endif
-
-    if (err_code != 0) {
-        nlr_raise(mp_obj_new_exception_msg_varg(&mp_type_OSError,
-                  translate("Can not query for the device address.")));
-    }
-
-    BLE_DRIVER_LOG("ble address, type: " HEX2_FMT ", " \
-                   "address: " HEX2_FMT ":" HEX2_FMT ":" HEX2_FMT ":" \
-                               HEX2_FMT ":" HEX2_FMT ":" HEX2_FMT "\n", \
-                   local_ble_addr.addr_type, \
-                   local_ble_addr.addr[5], local_ble_addr.addr[4], local_ble_addr.addr[3], \
-                   local_ble_addr.addr[2], local_ble_addr.addr[1], local_ble_addr.addr[0]);
-
-    address->type = local_ble_addr.addr_type;
-    memcpy(address->value, local_ble_addr.addr, BLEIO_ADDRESS_BYTES);
-}
-
-bool ble_drv_uuid_add_vs(uint8_t *uuid, uint8_t *idx) {
-    SD_TEST_OR_ENABLE();
-
-    if (sd_ble_uuid_vs_add((ble_uuid128_t const *)uuid, idx) != 0) {
-        nlr_raise(mp_obj_new_exception_msg_varg(&mp_type_OSError,
-                  translate("Can not add Vendor Specific 128-bit UUID.")));
-    }
-
-    return true;
-}
-
-void ble_drv_service_add(bleio_service_obj_t *service) {
-    SD_TEST_OR_ENABLE();
-
-    ble_uuid_t uuid = {
-        .type = BLE_UUID_TYPE_BLE,
-        .uuid = service->uuid->value[0] | (service->uuid->value[1] << 8)
-    };
-
-    if (service->uuid->type == UUID_TYPE_128BIT) {
-        uuid.type = service->uuid->uuid_vs_idx;
-    }
-
-    uint8_t service_type = BLE_GATTS_SRVC_TYPE_PRIMARY;
-    if (service->is_secondary) {
-        service_type = BLE_GATTS_SRVC_TYPE_SECONDARY;
-    }
-
-    if (sd_ble_gatts_service_add(service_type, &uuid, &service->handle) != 0) {
-        nlr_raise(mp_obj_new_exception_msg_varg(&mp_type_OSError,
-                  translate("Can not add Service.")));
-    }
-}
-
-bool ble_drv_characteristic_add(bleio_characteristic_obj_t *characteristic) {
-    ble_gatts_char_md_t char_md;
-    ble_gatts_attr_md_t cccd_md;
-    ble_gatts_attr_t    attr_char_value;
-    ble_uuid_t          uuid;
-    ble_gatts_attr_md_t attr_md;
-
-    memset(&char_md, 0, sizeof(char_md));
-
-    char_md.char_props.broadcast      = characteristic->props.broadcast;
-    char_md.char_props.read           = characteristic->props.read;
-    char_md.char_props.write_wo_resp  = characteristic->props.write_wo_resp;
-    char_md.char_props.write          = characteristic->props.write;
-    char_md.char_props.notify         = characteristic->props.notify;
-    char_md.char_props.indicate       = characteristic->props.indicate;
-
-    char_md.p_char_user_desc  = NULL;
-    char_md.p_char_pf         = NULL;
-    char_md.p_user_desc_md    = NULL;
-    char_md.p_sccd_md         = NULL;
-
-    if (characteristic->props.notify || characteristic->props.notify) {
-        memset(&cccd_md, 0, sizeof(cccd_md));
-        BLE_GAP_CONN_SEC_MODE_SET_OPEN(&cccd_md.read_perm);
-        BLE_GAP_CONN_SEC_MODE_SET_OPEN(&cccd_md.write_perm);
-        cccd_md.vloc = BLE_GATTS_VLOC_STACK;
-        char_md.p_cccd_md = &cccd_md;
-    } else {
-        char_md.p_cccd_md = NULL;
-    }
-
-    uuid.type = BLE_UUID_TYPE_BLE;
-    if (characteristic->uuid->type == UUID_TYPE_128BIT)
-        uuid.type = characteristic->uuid->uuid_vs_idx;
-
-    uuid.uuid  = characteristic->uuid->value[0];
-    uuid.uuid += characteristic->uuid->value[1] << 8;
-
-    memset(&attr_md, 0, sizeof(attr_md));
-
-    BLE_GAP_CONN_SEC_MODE_SET_OPEN(&attr_md.read_perm);
-    BLE_GAP_CONN_SEC_MODE_SET_OPEN(&attr_md.write_perm);
-
-    attr_md.vloc    = BLE_GATTS_VLOC_STACK;
-    attr_md.rd_auth = 0;
-    attr_md.wr_auth = 0;
-    attr_md.vlen    = 1;
-
-    memset(&attr_char_value, 0, sizeof(attr_char_value));
-
-    attr_char_value.p_uuid    = &uuid;
-    attr_char_value.p_attr_md = &attr_md;
-    attr_char_value.init_len  = sizeof(uint8_t);
-    attr_char_value.init_offs = 0;
-#if (BLE_API_VERSION == 2)
-    attr_char_value.max_len   = (GATT_MTU_SIZE_DEFAULT - 3);
-#else
-    attr_char_value.max_len   = (BLE_GATT_ATT_MTU_DEFAULT - 3);
-#endif
-
-    ble_gatts_char_handles_t handles;
-
-    uint32_t err_code = sd_ble_gatts_characteristic_add(characteristic->service_handle,
-                                                        &char_md,
-                                                        &attr_char_value,
-                                                        &handles);
-    if (err_code != 0) {
-        nlr_raise(mp_obj_new_exception_msg_varg(&mp_type_OSError,
-                  translate("Can not add Characteristic. status: 0x%08lX"), err_code));
-    }
-
-    // apply handles to object instance
-    characteristic->handle           = handles.value_handle;
-    characteristic->user_desc_handle = handles.user_desc_handle;
-    characteristic->cccd_handle      = handles.cccd_handle;
-    characteristic->sccd_handle      = handles.sccd_handle;
-
-    return true;
-}
-
-// TODO: Replace with just bleio_device_obj_t + data
-bool ble_drv_advertise_data(bleio_advertisement_data_t *adv_params) {
-    SD_TEST_OR_ENABLE();
-
-    uint8_t byte_pos = 0;
-    uint8_t adv_data[BLE_GAP_ADV_MAX_SIZE];
-
-    GET_STR_DATA_LEN(adv_params->device_name, name_data, name_len);
-
-    if (name_len > 0) {
-        ble_gap_conn_sec_mode_t sec_mode;
-        BLE_GAP_CONN_SEC_MODE_SET_OPEN(&sec_mode);
-
-        if (sd_ble_gap_device_name_set(&sec_mode,
-                                       name_data,
-                                       name_len) != 0) {
-            nlr_raise(mp_obj_new_exception_msg_varg(&mp_type_OSError,
-	              translate("Can not apply device name in the stack.")));
-        }
-
-        adv_data[byte_pos] = (BLE_ADV_AD_TYPE_FIELD_SIZE + name_len);
-        byte_pos += BLE_ADV_LENGTH_FIELD_SIZE;
-
-        // TODO: Shorten if too long
-        adv_data[byte_pos] = BLE_GAP_AD_TYPE_COMPLETE_LOCAL_NAME;
-        byte_pos += BLE_ADV_AD_TYPE_FIELD_SIZE;
-
-        memcpy(&adv_data[byte_pos], name_data, name_len);
-
-        byte_pos += name_len;
-    }
-
-    mp_buffer_info_t bufinfo;
-    mp_get_buffer_raise(adv_params->data, &bufinfo, MP_BUFFER_WRITE);
-
-    // set flags, default to disc mode
-    if (bufinfo.len == 0) {
-        adv_data[byte_pos] = (BLE_ADV_AD_TYPE_FIELD_SIZE + BLE_AD_TYPE_FLAGS_DATA_SIZE);
-        byte_pos += BLE_ADV_LENGTH_FIELD_SIZE;
-
-        adv_data[byte_pos] = BLE_GAP_AD_TYPE_FLAGS;
-        byte_pos += BLE_AD_TYPE_FLAGS_DATA_SIZE;
-
-        adv_data[byte_pos] = BLE_GAP_ADV_FLAGS_LE_ONLY_GENERAL_DISC_MODE;
-        byte_pos += 1;
-    }
-
-    const mp_obj_list_t *service_list = MP_OBJ_TO_PTR(adv_params->services);
-    if (service_list->len > 0) {
-        bool type_16bit_present  = false;
-        bool type_128bit_present = false;
-
-        for (size_t i = 0; i < service_list->len; ++i) {
-            const bleio_service_obj_t *service = MP_OBJ_TO_PTR(service_list->items[i]);
-
-            if (service->uuid->type == UUID_TYPE_16BIT) {
-                type_16bit_present = true;
-            }
-
-            if (service->uuid->type == UUID_TYPE_128BIT) {
-                type_128bit_present = true;
-            }
-        }
-
-        if (type_16bit_present) {
-            uint8_t size_byte_pos = byte_pos;
-
-            // skip length byte for now, apply total length post calculation
-            byte_pos += BLE_ADV_LENGTH_FIELD_SIZE;
+__attribute__((aligned(4)))
+static uint8_t m_ble_evt_buf[sizeof(ble_evt_t) + (BLE_GATT_ATT_MTU_DEFAULT)];
 
-            adv_data[byte_pos] = BLE_GAP_AD_TYPE_16BIT_SERVICE_UUID_COMPLETE;
-            byte_pos += BLE_ADV_AD_TYPE_FIELD_SIZE;
+typedef struct event_handler {
+    struct event_handler *next;
+    void *param;
+    ble_drv_evt_handler_t func;
+} event_handler_t;
 
-            uint8_t uuid_total_size = 0;
-            uint8_t encoded_size    = 0;
+static event_handler_t *m_event_handlers;
 
-            for (size_t i = 0; i < service_list->len; ++i) {
-                const bleio_service_obj_t *service = MP_OBJ_TO_PTR(service_list->items[i]);
-
-                if (service->uuid->type != UUID_TYPE_16BIT) {
-                    continue;
-                }
-
-                ble_uuid_t uuid;
-                uuid.type = BLE_UUID_TYPE_BLE;
-                uuid.uuid = service->uuid->value[0] | (service->uuid->value[1] << 8);
-
-                // calculate total size of uuids
-                if (sd_ble_uuid_encode(&uuid, &encoded_size, NULL) != 0) {
-                    nlr_raise(mp_obj_new_exception_msg_varg(&mp_type_OSError,
-                              translate("Can not encode UUID, to check length.")));
-                }
-
-                // do encoding into the adv buffer
-                if (sd_ble_uuid_encode(&uuid, &encoded_size, &adv_data[byte_pos]) != 0) {
-                    nlr_raise(mp_obj_new_exception_msg_varg(&mp_type_OSError,
-                              translate("Can encode UUID into the advertisement packet.")));
-                }
-
-                uuid_total_size += encoded_size; // size of entry
-                byte_pos        += encoded_size; // relative to adv data packet
-            }
-
-            adv_data[size_byte_pos] = (BLE_ADV_AD_TYPE_FIELD_SIZE + uuid_total_size);
-        }
-
-        if (type_128bit_present) {
-            uint8_t size_byte_pos = byte_pos;
-
-            // skip length byte for now, apply total length post calculation
-            byte_pos += BLE_ADV_LENGTH_FIELD_SIZE;
-
-            adv_data[byte_pos] = BLE_GAP_AD_TYPE_128BIT_SERVICE_UUID_COMPLETE;
-            byte_pos += BLE_ADV_AD_TYPE_FIELD_SIZE;
-
-            uint8_t uuid_total_size = 0;
-            uint8_t encoded_size    = 0;
-
-            for (size_t i = 0; i < service_list->len; ++i) {
-                const bleio_service_obj_t *service = MP_OBJ_TO_PTR(service_list->items[i]);
-
-                if (service->uuid->type != UUID_TYPE_128BIT) {
-                    continue;
-                }
-
-                ble_uuid_t uuid;
-                uuid.type = service->uuid->uuid_vs_idx;
-                uuid.uuid = service->uuid->value[0] | (service->uuid->value[1] << 8);
-
-                // calculate total size of uuids
-                if (sd_ble_uuid_encode(&uuid, &encoded_size, NULL) != 0) {
-                    nlr_raise(mp_obj_new_exception_msg_varg(&mp_type_OSError,
-                              translate("Can not encode UUID, to check length.")));
-                }
-
-                // do encoding into the adv buffer
-                if (sd_ble_uuid_encode(&uuid, &encoded_size, &adv_data[byte_pos]) != 0) {
-                    nlr_raise(mp_obj_new_exception_msg_varg(&mp_type_OSError,
-                              translate("Can encode UUID into the advertisement packet.")));
-                }
-
-                uuid_total_size += encoded_size; // size of entry
-                byte_pos        += encoded_size; // relative to adv data packet
-            }
+void ble_drv_add_event_handler(ble_drv_evt_handler_t func, void *param) {
+    event_handler_t *handler = m_new_ll(event_handler_t, 1);
+    handler->next = NULL;
+    handler->param = param;
+    handler->func = func;
 
-            adv_data[size_byte_pos] = (BLE_ADV_AD_TYPE_FIELD_SIZE + uuid_total_size);
-        }
-    }
-
-    if (bufinfo.len > 0) {
-        if (byte_pos + bufinfo.len > BLE_GAP_ADV_MAX_SIZE) {
-            nlr_raise(mp_obj_new_exception_msg_varg(&mp_type_OSError,
-                      translate("Can not fit data into the advertisement packet.")));
-        }
-
-        memcpy(adv_data, bufinfo.buf, bufinfo.len);
-        byte_pos += bufinfo.len;
-    }
-
-    uint32_t err_code;
-#if (BLUETOOTH_SD == 132)
-    if ((err_code = sd_ble_gap_adv_data_set(adv_data, byte_pos, NULL, 0)) != 0) {
-        nlr_raise(mp_obj_new_exception_msg_varg(&mp_type_OSError,
-                  translate("Can not apply advertisement data. status: 0x%02x"), (uint16_t)err_code));
-    }
-#endif
-
-    static ble_gap_adv_params_t m_adv_params;
-
-    // initialize advertising params
-    memset(&m_adv_params, 0, sizeof(m_adv_params));
-    if (adv_params->connectable) {
-#if (BLUETOOTH_SD == 140)
-        m_adv_params.properties.type = BLE_GAP_ADV_TYPE_CONNECTABLE_SCANNABLE_UNDIRECTED;
-#else
-        m_adv_params.type        = BLE_GAP_ADV_TYPE_ADV_IND;
-#endif
-    } else {
-#if (BLUETOOTH_SD == 140)
-        m_adv_params.properties.type = BLE_GAP_ADV_TYPE_NONCONNECTABLE_NONSCANNABLE_UNDIRECTED;
-#else
-        m_adv_params.type        = BLE_GAP_ADV_TYPE_ADV_NONCONN_IND;
-#endif
-    }
-
-    m_adv_params.p_peer_addr = NULL;                                // undirected advertisement
-    m_adv_params.interval    = MSEC_TO_UNITS(100, UNIT_0_625_MS);   // approx 8 ms
-#if (BLUETOOTH_SD == 140)
-    m_adv_params.duration              = BLE_GAP_ADV_TIMEOUT_GENERAL_UNLIMITED;
-    m_adv_params.filter_policy         = BLE_GAP_ADV_FP_ANY;
-    m_adv_params.primary_phy           = BLE_GAP_PHY_1MBPS;
-#else
-    m_adv_params.fp          = BLE_GAP_ADV_FP_ANY;
-    m_adv_params.timeout     = 0;                                   // infinite advertisement
-#endif
-
-    ble_drv_advertise_stop();
-
-#if (BLUETOOTH_SD == 140)
-    const ble_gap_adv_data_t ble_gap_adv_data = {
-        .adv_data = {
-            .p_data = adv_data,
-            .len = byte_pos
-        }
-    };
-
-    if ((err_code = sd_ble_gap_adv_set_configure(&m_adv_handle, &ble_gap_adv_data, &m_adv_params)) != 0) {
-        nlr_raise(mp_obj_new_exception_msg_varg(&mp_type_OSError,
-                  translate("Can not apply advertisement data. status: 0x%02x"), (uint16_t)err_code));
-    }
-    err_code = sd_ble_gap_adv_start(m_adv_handle, BLE_CONN_CFG_TAG_DEFAULT);
-#elif (BLUETOOTH_SD == 132 && BLE_API_VERSION == 4)
-    err_code = sd_ble_gap_adv_start(&m_adv_params, BLE_CONN_CFG_TAG_DEFAULT);
-#else
-    err_code = sd_ble_gap_adv_start(&m_adv_params);
-#endif
-    if (err_code != 0) {
-        nlr_raise(mp_obj_new_exception_msg_varg(&mp_type_OSError,
-                  translate("Can not start advertisement. status: 0x%02x"), (uint16_t)err_code));
-    }
-
-    return true;
-}
-
-void ble_drv_advertise_stop(void) {
-    uint32_t err_code;
-
-#if (BLUETOOTH_SD == 140)
-    if (m_adv_handle == BLE_GAP_ADV_SET_HANDLE_NOT_SET)
+    if (m_event_handlers == NULL) {
+        m_event_handlers = handler;
         return;
-
-    err_code = sd_ble_gap_adv_stop(m_adv_handle);
-#else
-    err_code = sd_ble_gap_adv_stop();
-#endif
-
-    if ((err_code != NRF_SUCCESS) && (err_code != NRF_ERROR_INVALID_STATE)) {
-        nlr_raise(mp_obj_new_exception_msg_varg(&mp_type_OSError,
-                                        translate("Can not stop advertisement. status: 0x%02x"), (uint16_t)err_code));
-    }
-}
-
-void ble_drv_attr_s_read(uint16_t conn_handle, uint16_t handle, uint16_t len, uint8_t * p_data) {
-    ble_gatts_value_t gatts_value;
-    memset(&gatts_value, 0, sizeof(gatts_value));
-
-    gatts_value.len     = len;
-    gatts_value.offset  = 0;
-    gatts_value.p_value = p_data;
-
-    uint32_t err_code = sd_ble_gatts_value_get(conn_handle,
-                                               handle,
-                                               &gatts_value);
-    if (err_code != 0) {
-        nlr_raise(mp_obj_new_exception_msg_varg(&mp_type_OSError,
-                  translate("Can not read attribute value. status: 0x%02x"), (uint16_t)err_code));
     }
 
-}
-
-void ble_drv_attr_s_write(bleio_characteristic_obj_t *characteristic, mp_buffer_info_t *bufinfo) {
-    bleio_device_obj_t *device = MP_OBJ_TO_PTR(characteristic->service->device);
-    uint16_t conn_handle = device->conn_handle;
-    ble_gatts_value_t gatts_value;
-
-    memset(&gatts_value, 0, sizeof(gatts_value));
-
-    gatts_value.len     = bufinfo->len;
-    gatts_value.offset  = 0;
-    gatts_value.p_value = bufinfo->buf;
-
-    uint32_t err_code = sd_ble_gatts_value_set(conn_handle, characteristic->handle, &gatts_value);
-
-    if (err_code != 0) {
-        nlr_raise(mp_obj_new_exception_msg_varg(&mp_type_OSError,
-                  translate("Can not write attribute value. status: 0x%02x"), (uint16_t)err_code));
-    }
-}
-
-void ble_drv_attr_s_notify(bleio_characteristic_obj_t *characteristic, mp_buffer_info_t *bufinfo) {
-    bleio_device_obj_t *device = MP_OBJ_TO_PTR(characteristic->service->device);
-    uint16_t conn_handle = device->conn_handle;
-    ble_gatts_hvx_params_t hvx_params;
-    uint16_t hvx_len = bufinfo->len;
-
-    memset(&hvx_params, 0, sizeof(hvx_params));
-
-    hvx_params.handle = characteristic->handle;
-    hvx_params.type   = BLE_GATT_HVX_NOTIFICATION;
-    hvx_params.offset = 0;
-    hvx_params.p_len  = &hvx_len;
-    hvx_params.p_data = bufinfo->buf;
-
-    while (m_tx_in_progress) {
-#ifdef MICROPY_VM_HOOK_LOOP
-    MICROPY_VM_HOOK_LOOP
-#endif
-    }
-
-    m_tx_in_progress = true;
-    uint32_t err_code;
-    if ((err_code = sd_ble_gatts_hvx(conn_handle, &hvx_params)) != 0) {
-        nlr_raise(mp_obj_new_exception_msg_varg(&mp_type_OSError,
-                  translate("Can not notify attribute value. status: 0x%02x"), (uint16_t)err_code));
-    }
-}
-
-void ble_drv_gap_event_handler_set(bleio_device_obj_t *device, ble_drv_gap_evt_callback_t evt_handler) {
-    mp_gap_observer = device;
-    gap_event_handler = evt_handler;
-}
-
-void ble_drv_gatts_event_handler_set(bleio_device_obj_t *device, ble_drv_gatts_evt_callback_t evt_handler) {
-    mp_gatts_observer = device;
-    gatts_event_handler = evt_handler;
-}
-
-void ble_drv_adv_report_handler_set(bleio_scanner_obj_t *device, ble_drv_adv_evt_callback_t evt_handler) {
-    mp_adv_observer = device;
-    adv_event_handler = evt_handler;
-}
-
-void ble_drv_attr_c_read(bleio_characteristic_obj_t *characteristic) {
-    bleio_service_obj_t *service = characteristic->service;
-    bleio_device_obj_t *device = MP_OBJ_TO_PTR(service->device);
-
-    mp_gattc_char_data_observer = characteristic;
-
-    const uint32_t err_code = sd_ble_gattc_read(device->conn_handle, characteristic->handle, 0);
-    if (err_code != 0) {
-        nlr_raise(mp_obj_new_exception_msg_varg(&mp_type_OSError,
-                  translate("Can not read attribute value. status: 0x%02x"), (uint16_t)err_code));
-    }
-
-    while (mp_gattc_char_data_observer != NULL) {
-#ifdef MICROPY_VM_HOOK_LOOP
-    MICROPY_VM_HOOK_LOOP
-#endif
-    }
-}
-
-void ble_drv_attr_c_write(bleio_characteristic_obj_t *characteristic, mp_buffer_info_t *bufinfo) {
-    bleio_device_obj_t *device = MP_OBJ_TO_PTR(characteristic->service->device);
-    uint16_t conn_handle = device->conn_handle;
-
-    ble_gattc_write_params_t write_params;
-    write_params.write_op = BLE_GATT_OP_WRITE_REQ;
-
-    if (characteristic->props.write_wo_resp) {
-        write_params.write_op = BLE_GATT_OP_WRITE_CMD;
-    }
-
-    write_params.flags    = BLE_GATT_EXEC_WRITE_FLAG_PREPARED_CANCEL;
-    write_params.handle   = characteristic->handle;
-    write_params.offset   = 0;
-    write_params.len      = bufinfo->len;
-    write_params.p_value  = bufinfo->buf;
-
-    m_write_done = (write_params.write_op == BLE_GATT_OP_WRITE_CMD);
-
-    uint32_t err_code = sd_ble_gattc_write(conn_handle, &write_params);
-    if (err_code != 0) {
-        nlr_raise(mp_obj_new_exception_msg_varg(&mp_type_OSError,
-            translate("Can not write attribute value. status: 0x%02x"), (uint16_t)err_code));
-    }
-
-    while (m_write_done != true) {   
-#ifdef MICROPY_VM_HOOK_LOOP
-    MICROPY_VM_HOOK_LOOP
-#endif
-    }
-}
-
-void ble_drv_scan_start(uint16_t interval, uint16_t window) {
-    SD_TEST_OR_ENABLE();
-
-    ble_gap_scan_params_t scan_params;
-    memset(&scan_params, 0, sizeof(ble_gap_scan_params_t));
-
-    scan_params.active   = 1;
-    scan_params.interval = MSEC_TO_UNITS(interval, UNIT_0_625_MS);
-    scan_params.window   = MSEC_TO_UNITS(window, UNIT_0_625_MS);
-#if (BLUETOOTH_SD == 140)
-    scan_params.scan_phys          = BLE_GAP_PHY_1MBPS;
-#endif
-    scan_params.timeout  = 0; // Infinite
-
-    uint32_t err_code;
-#if (BLUETOOTH_SD == 140)
-    if ((err_code = sd_ble_gap_scan_start(&scan_params, &m_scan_buffer)) != 0) {
-#else
-    if ((err_code = sd_ble_gap_scan_start(&scan_params)) != 0) {
-#endif
-        nlr_raise(mp_obj_new_exception_msg_varg(&mp_type_OSError,
-                  translate("Can not start scanning. status: 0x%02x"), (uint16_t)err_code));
-    }
-}
-
-void ble_drv_scan_continue(void) {
-    SD_TEST_OR_ENABLE();
-
-#if (BLUETOOTH_SD == 140)
-    uint32_t err_code;
-    if ((err_code = sd_ble_gap_scan_start(NULL, &m_scan_buffer)) != 0) {
-        nlr_raise(mp_obj_new_exception_msg_varg(&mp_type_OSError,
-                  "Can not continue scanning. status: 0x" HEX2_FMT, (uint16_t)err_code));
-    }
-#endif
-}
-
-void ble_drv_scan_stop(void) {
-    sd_ble_gap_scan_stop();
-}
-
-STATIC void ble_drv_connect_scan_callback(bleio_scanner_obj_t *scanner, bleio_scanentry_obj_t *entry) {
-    if (memcmp(entry->address.value, mp_connect_address->value, BLEIO_ADDRESS_BYTES) == 0) {
-        ble_drv_adv_report_handler_set(NULL, NULL);
-
-        ble_gap_scan_params_t scan_params;
-        memset(&scan_params, 0, sizeof(scan_params));
-
-        scan_params.active = 1;
-        scan_params.interval = MSEC_TO_UNITS(100, UNIT_0_625_MS);
-        scan_params.window = MSEC_TO_UNITS(100, UNIT_0_625_MS);
-        scan_params.timeout = 0;
-
-        ble_gap_addr_t addr;
-        memset(&addr, 0, sizeof(addr));
-
-        addr.addr_type = entry->address.type;
-        memcpy(addr.addr, entry->address.value, BLEIO_ADDRESS_BYTES);
-
-        BLE_DRIVER_LOG("GAP CONNECTING: "HEX2_FMT":"HEX2_FMT":"HEX2_FMT":"HEX2_FMT":"HEX2_FMT":"HEX2_FMT", type: %d\n",
-                       addr.addr[5], addr.addr[4], addr.addr[3], addr.addr[2], addr.addr[1], addr.addr[0], addr.addr_type);
-
-        ble_gap_conn_params_t conn_params = {
-            .min_conn_interval = BLE_MIN_CONN_INTERVAL,
-            .max_conn_interval = BLE_MAX_CONN_INTERVAL,
-            .conn_sup_timeout = BLE_CONN_SUP_TIMEOUT,
-            .slave_latency = BLE_SLAVE_LATENCY,
-        };
-
-        uint32_t err_code;
-    #if (BLE_API_VERSION == 2)
-        if ((err_code = sd_ble_gap_connect(&addr, &scan_params, &conn_params)) != 0) {
-    #else
-        if ((err_code = sd_ble_gap_connect(&addr, &scan_params, &conn_params, BLE_CONN_CFG_TAG_DEFAULT)) != 0) {
-    #endif
-            nlr_raise(mp_obj_new_exception_msg_varg(&mp_type_OSError,
-                      "Can not connect. status: 0x" HEX2_FMT, (uint16_t)err_code));
+    event_handler_t *it = m_event_handlers;
+    while (it->next != NULL) {
+        if ((it->func == func) && (it->param == param)) {
+            m_free(handler);
+            return;
         }
-    }
-}
-
-void ble_drv_connect(bleio_device_obj_t *device) {
-    SD_TEST_OR_ENABLE();
-
-    mp_connect_address = &device->address;
-    ble_drv_adv_report_handler_set(NULL, ble_drv_connect_scan_callback);
-
-    ble_drv_scan_start(100, 100);
-}
-
-void ble_drv_disconnect(bleio_device_obj_t *device) {
-    sd_ble_gap_disconnect(device->conn_handle, BLE_HCI_REMOTE_USER_TERMINATED_CONNECTION);
-}
-
-bool ble_drv_discover_services(bleio_device_obj_t *device, uint16_t start_handle) {
-    mp_gattc_disc_service_observer = device;
-
-    m_primary_service_found = false;
-
-    uint32_t err_code = sd_ble_gattc_primary_services_discover(device->conn_handle, start_handle, NULL);
-    if (err_code != NRF_SUCCESS) {
-        nlr_raise(mp_obj_new_exception_msg_varg(&mp_type_OSError,
-                  "Failed to discover serivices. status: 0x" HEX2_FMT, (uint16_t)err_code));
-    }
 
-    while (mp_gattc_disc_service_observer != NULL) {
-#ifdef MICROPY_VM_HOOK_LOOP
-    MICROPY_VM_HOOK_LOOP
-#endif
+        it = it->next;
     }
 
-    return m_primary_service_found;
+    it->next = handler;
 }
 
-bool ble_drv_discover_characteristic(bleio_device_obj_t *device, bleio_service_obj_t *service, uint16_t start_handle) {
-    BLE_DRIVER_LOG("Discover characteristicts. Conn handle: 0x" HEX2_FMT "\n", device->conn_handle);
-
-    mp_gattc_disc_char_observer = service;
-
-    ble_gattc_handle_range_t handle_range;
-    handle_range.start_handle = start_handle;
-    handle_range.end_handle = service->end_handle;
-
-    m_characteristic_found = false;
-
-    uint32_t err_code = sd_ble_gattc_characteristics_discover(device->conn_handle, &handle_range);
-    if (err_code != 0) {
-        return false;
-    }
-
-    while (mp_gattc_disc_char_observer != NULL) {
-#ifdef MICROPY_VM_HOOK_LOOP
-    MICROPY_VM_HOOK_LOOP
-#endif
-    }
-
-    return m_characteristic_found;
-}
-
-void ble_drv_discover_descriptors(void) {
-
-}
-
-STATIC void on_adv_report(ble_gap_evt_adv_report_t *report) {
-    bleio_scanentry_obj_t *entry = m_new_obj(bleio_scanentry_obj_t);
-    entry->base.type = &bleio_scanentry_type;
-
-    entry->rssi = report->rssi;
-
-    entry->address.type = report->peer_addr.addr_type;
-    memcpy(entry->address.value, report->peer_addr.addr, BLEIO_ADDRESS_BYTES);
-
-#if (BLUETOOTH_SD == 140)
-    entry->data = mp_obj_new_bytearray(report->data.len, report->data.p_data);
-#else
-    entry->data = mp_obj_new_bytearray(report->dlen, report->data);
-#endif
-
-    if (adv_event_handler != NULL) {
-        adv_event_handler(mp_adv_observer, entry);
-    }
-}
-
-STATIC void on_primary_srv_discovery_rsp(ble_gattc_evt_prim_srvc_disc_rsp_t *response) {
-    BLE_DRIVER_LOG(">>> service count: %d\n", response->count);
-
-    for (size_t i = 0; i < response->count; ++i) {
-        const ble_gattc_service_t *gattc_service = &response->services[i];
-
-        bleio_service_obj_t *service = m_new_obj(bleio_service_obj_t);
-        service->base.type = &bleio_service_type;
-        service->char_list = mp_obj_new_list(0, NULL);
-        service->start_handle = gattc_service->handle_range.start_handle;
-        service->end_handle = gattc_service->handle_range.end_handle;
-        service->handle = gattc_service->handle_range.start_handle;
-        service->device = mp_gattc_disc_service_observer;
-
-        bleio_uuid_obj_t *uuid = m_new_obj(bleio_uuid_obj_t);
-        uuid->base.type = &bleio_uuid_type;
-        uuid->type = (gattc_service->uuid.type == BLE_UUID_TYPE_BLE) ? UUID_TYPE_16BIT : UUID_TYPE_128BIT;
-        uuid->value[0] = gattc_service->uuid.uuid & 0xFF;
-        uuid->value[1] = gattc_service->uuid.uuid >> 8;
-        service->uuid = uuid;
-
-        mp_obj_list_append(mp_gattc_disc_service_observer->service_list, service);
-    }
-
-    if (response->count > 0) {
-        m_primary_service_found = true;
-    }
-
-    // mark end of service discovery
-    mp_gattc_disc_service_observer = NULL;
-}
-
-STATIC void on_characteristic_discovery_rsp(ble_gattc_evt_char_disc_rsp_t *response) {
-    BLE_DRIVER_LOG(">>> characteristic count: %d\n", response->count);
-
-    for (size_t i = 0; i < response->count; ++i) {
-        const ble_gattc_char_t *gattc_char = &response->chars[i];
-
-        bleio_characteristic_obj_t *characteristic = m_new_obj(bleio_characteristic_obj_t);
-        characteristic->base.type = &bleio_characteristic_type;
-
-        bleio_uuid_obj_t *uuid = m_new_obj(bleio_uuid_obj_t);
-        uuid->base.type = &bleio_uuid_type;
-        uuid->type = (gattc_char->uuid.type == BLE_UUID_TYPE_BLE) ? UUID_TYPE_16BIT : UUID_TYPE_128BIT;
-        uuid->value[0] = gattc_char->uuid.uuid & 0xFF;
-        uuid->value[1] = gattc_char->uuid.uuid >> 8;
-        characteristic->uuid = uuid;
-
-        characteristic->props.broadcast = gattc_char->char_props.broadcast;
-        characteristic->props.indicate = gattc_char->char_props.indicate;
-        characteristic->props.notify = gattc_char->char_props.notify;
-        characteristic->props.read = gattc_char->char_props.read;
-        characteristic->props.write = gattc_char->char_props.write;
-        characteristic->props.write_wo_resp = gattc_char->char_props.write_wo_resp;
-        characteristic->handle = gattc_char->handle_value;
-
-        characteristic->service_handle = mp_gattc_disc_char_observer->handle;
-        characteristic->service = mp_gattc_disc_char_observer;
-
-        mp_obj_list_append(mp_gattc_disc_char_observer->char_list, MP_OBJ_FROM_PTR(characteristic));
-    }
-
-    if (response->count > 0) {
-        m_characteristic_found = true;
+void SD_EVT_IRQHandler(void) {
+    uint32_t evt_id;
+    while (sd_evt_get(&evt_id) != NRF_ERROR_NOT_FOUND) {
+//        sd_evt_handler(evt_id);
     }
 
-    // mark end of characteristic discovery
-    mp_gattc_disc_char_observer = NULL;
-}
-
-STATIC void on_read_rsp(ble_gattc_evt_read_rsp_t *response) {
-    mp_gattc_char_data_observer->value_data = mp_obj_new_bytearray(response->len, response->data);
-
-    mp_gattc_char_data_observer = NULL;
-}
-
-STATIC void ble_evt_handler(ble_evt_t *p_ble_evt) {
-    printf("%s - 0x%02X\r\n", __func__, p_ble_evt->header.evt_id);
-
-    switch (p_ble_evt->header.evt_id) {
-        case BLE_GAP_EVT_CONNECTED:
-            BLE_DRIVER_LOG("GAP CONNECT\n");
-            gap_event_handler(mp_gap_observer, p_ble_evt->header.evt_id, p_ble_evt->evt.gap_evt.conn_handle, p_ble_evt->header.evt_len - (2 * sizeof(uint16_t)), NULL);
-
-            ble_gap_conn_params_t conn_params;
-            (void)sd_ble_gap_ppcp_get(&conn_params);
-            (void)sd_ble_gap_conn_param_update(p_ble_evt->evt.gap_evt.conn_handle, &conn_params);
-            break;
-
-        case BLE_GAP_EVT_DISCONNECTED:
-            BLE_DRIVER_LOG("GAP DISCONNECT\n");
-            gap_event_handler(mp_gap_observer, p_ble_evt->header.evt_id, p_ble_evt->evt.gap_evt.conn_handle, p_ble_evt->header.evt_len - (2 * sizeof(uint16_t)), NULL);
-            break;
-
-        case BLE_GATTS_EVT_HVC:
-            gatts_event_handler(mp_gatts_observer, p_ble_evt->header.evt_id, p_ble_evt->evt.gatts_evt.params.hvc.handle, p_ble_evt->header.evt_len - (2 * sizeof(uint16_t)), NULL);
-            break;
-
-        case BLE_GATTS_EVT_WRITE:
-            BLE_DRIVER_LOG("GATTS write\n");
-
-            uint16_t  handle   = p_ble_evt->evt.gatts_evt.params.write.handle;
-            uint16_t  data_len = p_ble_evt->evt.gatts_evt.params.write.len;
-            uint8_t * p_data   = &p_ble_evt->evt.gatts_evt.params.write.data[0];
-
-            gatts_event_handler(mp_gatts_observer, p_ble_evt->header.evt_id, handle, data_len, p_data);
-            break;
-
-        case BLE_GAP_EVT_CONN_PARAM_UPDATE:
-            BLE_DRIVER_LOG("GAP CONN PARAM UPDATE\n");
-            break;
-
-        case BLE_GATTS_EVT_SYS_ATTR_MISSING:
-            // No system attributes have been stored.
-            (void)sd_ble_gatts_sys_attr_set(p_ble_evt->evt.gatts_evt.conn_handle, NULL, 0, 0);
-            break;
-
-#if (BLE_API_VERSION == 4)
-        case BLE_GATTS_EVT_EXCHANGE_MTU_REQUEST:
-            BLE_DRIVER_LOG("GATTS EVT EXCHANGE MTU REQUEST\n");
-            (void)sd_ble_gatts_exchange_mtu_reply(p_ble_evt->evt.gatts_evt.conn_handle, 23); // MAX MTU size
-            break;
-#endif
-
-#if (BLE_API_VERSION == 4)
-        case BLE_GATTS_EVT_HVN_TX_COMPLETE:
-#else
-        case BLE_EVT_TX_COMPLETE:
-#endif
-            BLE_DRIVER_LOG("BLE EVT TX COMPLETE\n");
-            m_tx_in_progress = false;
-            break;
-
-        case BLE_GAP_EVT_SEC_PARAMS_REQUEST:
-            BLE_DRIVER_LOG("BLE EVT SEC PARAMS REQUEST\n");
-            // pairing not supported
-            (void)sd_ble_gap_sec_params_reply(p_ble_evt->evt.gatts_evt.conn_handle,
-                                              BLE_GAP_SEC_STATUS_PAIRING_NOT_SUPP,
-                                              NULL, NULL);
-            break;
-
-        case BLE_GAP_EVT_ADV_REPORT:
-            on_adv_report(&p_ble_evt->evt.gap_evt.params.adv_report);
-            break;
-
-        case BLE_GAP_EVT_CONN_PARAM_UPDATE_REQUEST:
-            BLE_DRIVER_LOG("BLE EVT CONN PARAM UPDATE REQUEST\n");
-
-            (void)sd_ble_gap_conn_param_update(p_ble_evt->evt.gap_evt.conn_handle,
-                                               &p_ble_evt->evt.gap_evt.params.conn_param_update_request.conn_params);
-            break;
-
-        case BLE_GATTC_EVT_PRIM_SRVC_DISC_RSP:
-            on_primary_srv_discovery_rsp(&p_ble_evt->evt.gattc_evt.params.prim_srvc_disc_rsp);
-            break;
-
-        case BLE_GATTC_EVT_CHAR_DISC_RSP:
-            on_characteristic_discovery_rsp(&p_ble_evt->evt.gattc_evt.params.char_disc_rsp);
-            break;
-
-        case BLE_GATTC_EVT_READ_RSP:
-            on_read_rsp(&p_ble_evt->evt.gattc_evt.params.read_rsp);
-            break;
-
-        case BLE_GATTC_EVT_WRITE_RSP:
-            BLE_DRIVER_LOG("BLE EVT WRITE RESPONSE\n");
-            m_write_done = true;
-            break;
+    while (1) {
+        uint16_t evt_len = sizeof(m_ble_evt_buf);
+        const uint32_t err_code = sd_ble_evt_get(m_ble_evt_buf, &evt_len);
+        if (err_code != NRF_SUCCESS) {
+            if (err_code == NRF_ERROR_DATA_SIZE) {
+                printf("NRF_ERROR_DATA_SIZE\n");
+            }
 
-        case BLE_GATTC_EVT_HVX:
-            BLE_DRIVER_LOG("BLE EVT HVX RESPONSE\n");
             break;
+        }
 
-        default:
-            BLE_DRIVER_LOG(">>> unhandled evt: 0x" HEX2_FMT "\n", p_ble_evt->header.evt_id);
-            break;
+        event_handler_t *it = m_event_handlers;
+        while (it != NULL) {
+            it->func((ble_evt_t *)m_ble_evt_buf, it->param);
+            it = it->next;
+        }
     }
 }
-
-#if (BLE_API_VERSION == 2)
-static uint8_t m_ble_evt_buf[sizeof(ble_evt_t) + (GATT_MTU_SIZE_DEFAULT)] __attribute__ ((aligned (4)));
-#else
-static uint8_t m_ble_evt_buf[sizeof(ble_evt_t) + (BLE_GATT_ATT_MTU_DEFAULT)] __attribute__ ((aligned (4)));
-#endif
-
-void SD_EVT_IRQHandler(void) {
-    uint32_t evt_id;
-    uint32_t err_code;
-    do {
-        err_code = sd_evt_get(&evt_id);
-        // TODO: handle non ble events
-    } while (err_code != NRF_ERROR_NOT_FOUND && err_code != NRF_SUCCESS);
-
-    uint16_t evt_len = sizeof(m_ble_evt_buf);
-    do {
-        err_code = sd_ble_evt_get(m_ble_evt_buf, &evt_len);
-        ble_evt_handler((ble_evt_t *)m_ble_evt_buf);
-    } while (err_code != NRF_ERROR_NOT_FOUND && err_code != NRF_SUCCESS);
-}
-
-#endif // BLUETOOTH_SD
diff --git a/ports/nrf/drivers/bluetooth/ble_drv.h b/ports/nrf/drivers/bluetooth/ble_drv.h
index 3b065c7a9..426354a19 100644
--- a/ports/nrf/drivers/bluetooth/ble_drv.h
+++ b/ports/nrf/drivers/bluetooth/ble_drv.h
@@ -4,6 +4,7 @@
  * The MIT License (MIT)
  *
  * Copyright (c) 2016 Glenn Ruben Bakke
+ * Copyright (c) 2018 Artur Pacholec
  *
  * Permission is hereby granted, free of charge, to any person obtaining a copy
  * of this software and associated documentation files (the "Software"), to deal
@@ -27,65 +28,26 @@
 #ifndef BLUETOOTH_LE_DRIVER_H__
 #define BLUETOOTH_LE_DRIVER_H__
 
-#if BLUETOOTH_SD
+#include "ble.h"
 
-#include "shared-bindings/bleio/Device.h"
-#include "shared-bindings/bleio/Scanner.h"
-#include "shared-bindings/bleio/Service.h"
-#include "shared-module/bleio/ScanEntry.h"
+#if (BLUETOOTH_SD == 132) && (BLE_API_VERSION == 2)
+#define NRF52
+#endif
 
-typedef void (*ble_drv_gap_evt_callback_t)(bleio_device_obj_t *device, uint16_t event_id, uint16_t conn_handle, uint16_t length, uint8_t * data);
-typedef void (*ble_drv_gatts_evt_callback_t)(bleio_device_obj_t *device, uint16_t event_id, uint16_t attr_handle, uint16_t length, uint8_t * data);
-typedef void (*ble_drv_adv_evt_callback_t)(bleio_scanner_obj_t *scanner, bleio_scanentry_obj_t *entry);
+#define MAX_TX_IN_PROGRESS 10
 
-uint32_t ble_drv_stack_enable(void);
+#ifndef BLE_GATT_ATT_MTU_DEFAULT
+    #define BLE_GATT_ATT_MTU_DEFAULT GATT_MTU_SIZE_DEFAULT
+#endif
 
-uint8_t ble_drv_stack_enabled(void);
+#define BLE_CONN_CFG_TAG_CUSTOM 1
 
-void ble_drv_address_get(bleio_address_obj_t *address);
+#define MSEC_TO_UNITS(TIME, RESOLUTION) (((TIME) * 1000) / (RESOLUTION))
+#define UNIT_0_625_MS (625)
+#define UNIT_10_MS    (10000)
 
-bool ble_drv_uuid_add_vs(uint8_t * p_uuid, uint8_t * idx);
+typedef void (*ble_drv_evt_handler_t)(ble_evt_t*, void*);
 
-void ble_drv_service_add(bleio_service_obj_t *service);
-
-bool ble_drv_characteristic_add(bleio_characteristic_obj_t *characteristic);
-
-bool ble_drv_advertise_data(bleio_advertisement_data_t *p_adv_params);
-
-void ble_drv_advertise_stop(void);
-
-void ble_drv_gap_event_handler_set(bleio_device_obj_t *device, ble_drv_gap_evt_callback_t evt_handler);
-
-void ble_drv_gatts_event_handler_set(bleio_device_obj_t *device, ble_drv_gatts_evt_callback_t evt_handler);
-
-void ble_drv_attr_s_read(uint16_t conn_handle, uint16_t handle, uint16_t len, uint8_t * p_data);
-
-void ble_drv_attr_c_read(bleio_characteristic_obj_t *characteristic);
-
-void ble_drv_attr_s_write(bleio_characteristic_obj_t *characteristic, mp_buffer_info_t *bufinfo);
-
-void ble_drv_attr_s_notify(bleio_characteristic_obj_t *characteristic, mp_buffer_info_t *bufinfo);
-
-void ble_drv_attr_c_write(bleio_characteristic_obj_t *characteristic, mp_buffer_info_t *bufinfo);
-
-void ble_drv_scan_start(uint16_t interval, uint16_t window);
-
-void ble_drv_scan_continue(void);
-
-void ble_drv_scan_stop(void);
-
-void ble_drv_adv_report_handler_set(bleio_scanner_obj_t *self, ble_drv_adv_evt_callback_t evt_handler);
-
-void ble_drv_connect(bleio_device_obj_t *device);
-
-void ble_drv_disconnect(bleio_device_obj_t *device);
-
-bool ble_drv_discover_services(bleio_device_obj_t *device, uint16_t start_handle);
-
-bool ble_drv_discover_characteristic(bleio_device_obj_t *device, bleio_service_obj_t *service, uint16_t start_handle);
-
-void ble_drv_discover_descriptors(void);
-
-#endif // BLUETOOTH_SD
+void ble_drv_add_event_handler(ble_drv_evt_handler_t func, void *param);
 
 #endif // BLUETOOTH_LE_DRIVER_H__
diff --git a/ports/nrf/drivers/bluetooth/ble_uart.h b/ports/nrf/drivers/bluetooth/ble_uart.h
index 336624cd3..b57a75229 100644
--- a/ports/nrf/drivers/bluetooth/ble_uart.h
+++ b/ports/nrf/drivers/bluetooth/ble_uart.h
@@ -27,7 +27,7 @@
 #ifndef BLUETOOTH_LE_UART_H__
 #define BLUETOOTH_LE_UART_H__
 
-#if BLUETOOTH_SD
+#include <stdbool.h>
 
 #include "ble_drv.h"
 
@@ -36,6 +36,4 @@ void ble_uart_advertise(void);
 bool ble_uart_connected(void);
 bool ble_uart_enabled(void);
 
-#endif // BLUETOOTH_SD
-
 #endif // BLUETOOTH_LE_UART_H__
diff --git a/shared-bindings/bleio/Characteristic.c b/shared-bindings/bleio/Characteristic.c
index 905babecb..3563d3895 100644
--- a/shared-bindings/bleio/Characteristic.c
+++ b/shared-bindings/bleio/Characteristic.c
@@ -125,6 +125,8 @@ STATIC mp_obj_t bleio_characteristic_make_new(const mp_obj_type_t *type, size_t
                   "Invalid UUID parameter"));
     }
 
+    common_hal_bleio_characteristic_construct(self);
+
     return MP_OBJ_FROM_PTR(self);
 }
 
diff --git a/shared-bindings/bleio/Characteristic.h b/shared-bindings/bleio/Characteristic.h
index dd67170d9..0e99e97cc 100644
--- a/shared-bindings/bleio/Characteristic.h
+++ b/shared-bindings/bleio/Characteristic.h
@@ -31,6 +31,7 @@
 
 extern const mp_obj_type_t bleio_characteristic_type;
 
+extern void common_hal_bleio_characteristic_construct(bleio_characteristic_obj_t *self);
 extern void common_hal_bleio_characteristic_read_value(bleio_characteristic_obj_t *self);
 extern void common_hal_bleio_characteristic_write_value(bleio_characteristic_obj_t *self, mp_buffer_info_t *bufinfo);
 
diff --git a/shared-bindings/bleio/Device.c b/shared-bindings/bleio/Device.c
index 386261683..8c7cef95f 100644
--- a/shared-bindings/bleio/Device.c
+++ b/shared-bindings/bleio/Device.c
@@ -199,7 +199,7 @@ STATIC mp_obj_t bleio_device_add_service(mp_obj_t self_in, mp_obj_t service_in)
                   "Can't add services in Central mode"));
     }
 
-    service->device = self_in;
+    service->device = self;
 
     mp_obj_list_append(self->service_list, service);
 
@@ -266,31 +266,21 @@ STATIC mp_obj_t bleio_device_start_advertising(mp_uint_t n_args, const mp_obj_t
                   "Can't advertise in Central mode"));
     }
 
-    enum { ARG_connectable };
+    enum { ARG_connectable, ARG_data };
     static const mp_arg_t allowed_args[] = {
         { MP_QSTR_connectable, MP_ARG_KW_ONLY | MP_ARG_BOOL, {.u_bool = true} },
+        { MP_QSTR_data, MP_ARG_KW_ONLY | MP_ARG_OBJ, {.u_obj = mp_const_none} },
     };
 
     mp_arg_val_t args[MP_ARRAY_SIZE(allowed_args)];
     mp_arg_parse_all(n_args - 1, pos_args + 1, kw_args, MP_ARRAY_SIZE(allowed_args), allowed_args, args);
 
-    // TODO: data
-    bleio_advertisement_data_t adv_data = {
-        .device_name = self->name,
-        .services = mp_obj_new_list(0, NULL),
-        .data = mp_obj_new_bytearray(0, NULL),
-        .connectable = args[ARG_connectable].u_bool
-    };
-
-    mp_obj_list_t *service_list = MP_OBJ_TO_PTR(self->service_list);
-    for (size_t i = 0; i < service_list->len; ++i) {
-        bleio_service_obj_t *service = MP_OBJ_TO_PTR(service_list->items[i]);
-        if (!service->is_secondary) {
-            mp_obj_list_append(adv_data.services, service_list->items[i]);
-        }
+    mp_buffer_info_t bufinfo = { 0 };
+    if (args[ARG_data].u_obj != mp_const_none) {
+        mp_get_buffer_raise(args[ARG_data].u_obj, &bufinfo, MP_BUFFER_READ);
     }
 
-    common_hal_bleio_device_start_advertising(self, &adv_data);
+    common_hal_bleio_device_start_advertising(self, args[ARG_connectable].u_bool, &bufinfo);
 
     return mp_const_none;
 }
diff --git a/shared-bindings/bleio/Device.h b/shared-bindings/bleio/Device.h
index d099e5c43..f989736dd 100644
--- a/shared-bindings/bleio/Device.h
+++ b/shared-bindings/bleio/Device.h
@@ -32,7 +32,7 @@
 
 extern const mp_obj_type_t bleio_device_type;
 
-extern void common_hal_bleio_device_start_advertising(bleio_device_obj_t *device, bleio_advertisement_data_t *adv_data);
+extern void common_hal_bleio_device_start_advertising(bleio_device_obj_t *device, bool connectable, mp_buffer_info_t *raw_data);
 extern void common_hal_bleio_device_stop_advertising(bleio_device_obj_t *device);
 extern void common_hal_bleio_device_connect(bleio_device_obj_t *device);
 extern void common_hal_bleio_device_disconnect(bleio_device_obj_t *device);
diff --git a/shared-bindings/bleio/Scanner.h b/shared-bindings/bleio/Scanner.h
index 03db5cd8b..9bd071747 100644
--- a/shared-bindings/bleio/Scanner.h
+++ b/shared-bindings/bleio/Scanner.h
@@ -34,5 +34,6 @@
 extern const mp_obj_type_t bleio_scanner_type;
 
 extern void common_hal_bleio_scanner_scan(bleio_scanner_obj_t *self, mp_int_t timeout);
+extern void common_hal_bleio_scanner_stop(bleio_scanner_obj_t *self);
 
 #endif // MICROPY_INCLUDED_SHARED_BINDINGS_BLEIO_SCANNER_H
diff --git a/shared-bindings/bleio/Service.c b/shared-bindings/bleio/Service.c
index 338269a5f..a2f9b9899 100644
--- a/shared-bindings/bleio/Service.c
+++ b/shared-bindings/bleio/Service.c
@@ -74,7 +74,7 @@ STATIC mp_obj_t bleio_service_make_new(const mp_obj_type_t *type, size_t n_args,
     mp_arg_check_num(n_args, n_kw, 1, 1, true);
     bleio_service_obj_t *self = m_new_obj(bleio_service_obj_t);
     self->base.type = &bleio_service_type;
-    self->device = mp_const_none;
+    self->device = NULL;
     self->char_list = mp_obj_new_list(0, NULL);
 
     mp_map_t kw_args;
diff --git a/shared-module/bleio/Characteristic.h b/shared-module/bleio/Characteristic.h
index 2347c9c30..13f75b2c9 100644
--- a/shared-module/bleio/Characteristic.h
+++ b/shared-module/bleio/Characteristic.h
@@ -33,7 +33,7 @@
 typedef struct {
     mp_obj_base_t base;
     bleio_service_obj_t *service;
-    uint16_t service_handle;
+    uint16_t service_handle; // TODO: Is this needed?
     bleio_uuid_obj_t *uuid;
     mp_obj_t value_data;
     uint16_t handle;
diff --git a/shared-module/bleio/Device.h b/shared-module/bleio/Device.h
index afbd8f063..8d9ece5ef 100644
--- a/shared-module/bleio/Device.h
+++ b/shared-module/bleio/Device.h
@@ -36,7 +36,7 @@ typedef struct {
     bool is_peripheral;
     mp_obj_t name;
     bleio_address_obj_t address;
-    uint16_t conn_handle;
+    volatile uint16_t conn_handle;
     mp_obj_t service_list;
     mp_obj_t notif_handler;
     mp_obj_t conn_handler;
diff --git a/shared-module/bleio/Service.h b/shared-module/bleio/Service.h
index 1e3f09119..ff506d3f3 100644
--- a/shared-module/bleio/Service.h
+++ b/shared-module/bleio/Service.h
@@ -28,13 +28,14 @@
 #define MICROPY_INCLUDED_SHARED_MODULE_BLEIO_SERVICE_H
 
 #include "common-hal/bleio/UUID.h"
+#include "shared-module/bleio/Device.h"
 
 typedef struct {
     mp_obj_base_t base;
     uint16_t handle;
     bool is_secondary;
     bleio_uuid_obj_t *uuid;
-    mp_obj_t device;
+    bleio_device_obj_t *device;
     mp_obj_t char_list;
     uint16_t start_handle;
     uint16_t end_handle;