nrf: Cleanup of the ble driver

Moved the functions to classes that they belong to.

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 "nrfx_power.h"
-#include "nrf_error.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);
 }

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);
     }
 
 }

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_hci.h"
-#include "ble_gatt.h"
+#include "nrf_soc.h"
-#include "ble_types.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) {
+#define BLE_MIN_CONN_INTERVAL        MSEC_TO_UNITS(15, UNIT_0_625_MS)
-    if (event_id == BLE_GAP_EVT_CONNECTED) {
+#define BLE_MAX_CONN_INTERVAL        MSEC_TO_UNITS(300, UNIT_0_625_MS)
-        device->conn_handle = conn_handle;
+#define BLE_SLAVE_LATENCY            0
-    } else if (event_id == BLE_GAP_EVT_DISCONNECTED) {
+#define BLE_CONN_SUP_TIMEOUT         MSEC_TO_UNITS(4000, UNIT_10_MS)
-        device->conn_handle = BLE_CONN_HANDLE_INVALID;
+
+#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 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;
+}
+
+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));
+    }
+
+    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 gatts_event_handler(bleio_device_obj_t *device, uint16_t event_id, uint16_t attr_handle, uint16_t length, uint8_t * data) {
+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;
 
-void common_hal_bleio_device_start_advertising(bleio_device_obj_t *device, bleio_advertisement_data_t *adv_data) {
+        bleio_uuid_obj_t *uuid = m_new_obj(bleio_uuid_obj_t);
-    if (adv_data->connectable) {
+        uuid->base.type = &bleio_uuid_type;
-        ble_drv_gap_event_handler_set(device, gap_event_handler);
+        uuid->type = (gattc_char->uuid.type == BLE_UUID_TYPE_BLE) ? UUID_TYPE_16BIT : UUID_TYPE_128BIT;
-        ble_drv_gatts_event_handler_set(device, gatts_event_handler);
+        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));
     }
 
-    ble_drv_advertise_data(adv_data);
+    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;
 
-    ble_drv_advertise_stop();
+#if (BLUETOOTH_SD == 140)
+    if (m_adv_handle == BLE_GAP_ADV_SET_HANDLE_NOT_SET)
+        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,
+            "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);
 }

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_drv_scan_start(self->interval, self->window);
+    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
+    };
 
-    mp_hal_delay_ms(timeout);
+    common_hal_bleio_adapter_set_enabled(true);
 
-    ble_drv_scan_stop();
+    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));
+    }
+
+    if (timeout > 0) {
+        mp_hal_delay_ms(timeout);
+        sd_ble_gap_scan_stop();
+    }
 }

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)) {
+    ble_gatts_char_md_t char_md = {
-        characteristic->service = self;
+        .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;
 }

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];
+            ble_uuid128_t vs_uuid;
-            buffer[0] = xdigit_8b_value(str_data[35], str_data[34]);
+            vs_uuid.uuid128[0] = xdigit_8b_value(str_data[35], str_data[34]);
-            buffer[1] = xdigit_8b_value(str_data[33], str_data[32]);
+            vs_uuid.uuid128[1] = xdigit_8b_value(str_data[33], str_data[32]);
-            buffer[2] = xdigit_8b_value(str_data[31], str_data[30]);
+            vs_uuid.uuid128[2] = xdigit_8b_value(str_data[31], str_data[30]);
-            buffer[3] = xdigit_8b_value(str_data[29], str_data[28]);
+            vs_uuid.uuid128[3] = xdigit_8b_value(str_data[29], str_data[28]);
-            buffer[4] = xdigit_8b_value(str_data[27], str_data[26]);
+            vs_uuid.uuid128[4] = xdigit_8b_value(str_data[27], str_data[26]);
-            buffer[5] = xdigit_8b_value(str_data[25], str_data[24]);
+            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]);
+            vs_uuid.uuid128[6] = xdigit_8b_value(str_data[22], str_data[21]);
-            buffer[7] = xdigit_8b_value(str_data[20], str_data[19]);
+            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]);
+            vs_uuid.uuid128[8] = xdigit_8b_value(str_data[17], str_data[16]);
-            buffer[9] = xdigit_8b_value(str_data[15], str_data[14]);
+            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]);
+            vs_uuid.uuid128[10] = xdigit_8b_value(str_data[12], str_data[11]);
-            buffer[11] = xdigit_8b_value(str_data[10], str_data[9]);
+            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]);
+            vs_uuid.uuid128[14] = xdigit_8b_value(str_data[3], str_data[2]);
-            buffer[15] = xdigit_8b_value(str_data[1], str_data[0]);
+            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);
     }
 }
 

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"
+#if (BLUETOOTH_SD == 132) && (BLE_API_VERSION == 2)
-#include "shared-bindings/bleio/Scanner.h"
+#define NRF52
-#include "shared-bindings/bleio/Service.h"
+#endif
-#include "shared-module/bleio/ScanEntry.h"
 
-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);
+#define MAX_TX_IN_PROGRESS 10
-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);
 
-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);
+void ble_drv_add_event_handler(ble_drv_evt_handler_t func, void *param);
-
-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
 
 #endif // BLUETOOTH_LE_DRIVER_H__

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__

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);
 }
 

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);
 

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
+    mp_buffer_info_t bufinfo = { 0 };
-    bleio_advertisement_data_t adv_data = {
+    if (args[ARG_data].u_obj != mp_const_none) {
-        .device_name = self->name,
+        mp_get_buffer_raise(args[ARG_data].u_obj, &bufinfo, MP_BUFFER_READ);
-        .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]);
-        }
     }
 
-    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;
 }

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);

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

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;

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;

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;

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;