bleio: Don't register the services until needed

Because of the very specific way nRF requires service registration
(characteristics can be added only to last added service), we would
have to write the Python code in a specific way. With this patch the
user has more freedom.

6 files changed, 41 insertions, 29 deletions

diff --git a/ports/nrf/common-hal/bleio/Device.c b/ports/nrf/common-hal/bleio/Device.c
index fd772c04d..c03db0ab9 100644
--- a/ports/nrf/common-hal/bleio/Device.c
+++ b/ports/nrf/common-hal/bleio/Device.c
@@ -490,6 +490,36 @@ STATIC void on_ble_evt(ble_evt_t *ble_evt, void *device_in) {
     }
 }
 
+void common_hal_bleio_device_add_service(bleio_device_obj_t *device, bleio_service_obj_t *service) {
+    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;
+    }
+
+    common_hal_bleio_adapter_set_enabled(true);
+
+    const uint32_t err_code = sd_ble_gatts_service_add(service_type, &uuid, &service->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));
+    }
+
+    const mp_obj_list_t *char_list = MP_OBJ_TO_PTR(service->char_list);
+    for (size_t i = 0; i < char_list->len; ++i) {
+        bleio_characteristic_obj_t *characteristic = char_list->items[i];
+        common_hal_bleio_service_add_characteristic(service, characteristic);
+    }
+}
+
 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);
diff --git a/ports/nrf/common-hal/bleio/Service.c b/ports/nrf/common-hal/bleio/Service.c
index 93588d156..ee2624073 100644
--- a/ports/nrf/common-hal/bleio/Service.c
+++ b/ports/nrf/common-hal/bleio/Service.c
@@ -30,30 +30,6 @@
 #include "shared-bindings/bleio/Service.h"
 #include "shared-bindings/bleio/Adapter.h"
 
-void common_hal_bleio_service_construct(bleio_service_obj_t *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) {
     ble_gatts_char_md_t char_md = {
         .char_props.broadcast      = characteristic->props.broadcast,
diff --git a/shared-bindings/bleio/Device.c b/shared-bindings/bleio/Device.c
index 756a5e05d..e89bc45a2 100644
--- a/shared-bindings/bleio/Device.c
+++ b/shared-bindings/bleio/Device.c
@@ -291,6 +291,14 @@ STATIC mp_obj_t bleio_device_start_advertising(mp_uint_t n_args, const mp_obj_t
         mp_get_buffer_raise(args[ARG_data].u_obj, &bufinfo, MP_BUFFER_READ);
     }
 
+    const 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 = service_list->items[i];
+        if (service->handle == 0xFFFF) {
+            common_hal_bleio_device_add_service(self, service);
+        }
+    }
+
     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 f989736dd..aebf1d639 100644
--- a/shared-bindings/bleio/Device.h
+++ b/shared-bindings/bleio/Device.h
@@ -29,9 +29,11 @@
 
 #include "shared-module/bleio/AdvertisementData.h"
 #include "shared-module/bleio/Device.h"
+#include "shared-module/bleio/Service.h"
 
 extern const mp_obj_type_t bleio_device_type;
 
+extern void common_hal_bleio_device_add_service(bleio_device_obj_t *device, bleio_service_obj_t *service);
 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);
diff --git a/shared-bindings/bleio/Service.c b/shared-bindings/bleio/Service.c
index a5043a346..6e112880a 100644
--- a/shared-bindings/bleio/Service.c
+++ b/shared-bindings/bleio/Service.c
@@ -76,6 +76,7 @@ STATIC mp_obj_t bleio_service_make_new(const mp_obj_type_t *type, size_t n_args,
     self->base.type = &bleio_service_type;
     self->device = NULL;
     self->char_list = mp_obj_new_list(0, NULL);
+    self->handle = 0xFFFF;
 
     mp_map_t kw_args;
     mp_map_init_fixed_table(&kw_args, n_kw, pos_args + n_args);
@@ -104,8 +105,6 @@ STATIC mp_obj_t bleio_service_make_new(const mp_obj_type_t *type, size_t n_args,
                   "Invalid UUID parameter"));
     }
 
-    common_hal_bleio_service_construct(self);
-
     return MP_OBJ_FROM_PTR(self);
 }
 
@@ -118,8 +117,6 @@ STATIC mp_obj_t bleio_service_add_characteristic(mp_obj_t self_in, mp_obj_t char
         characteristic->uuid->uuid_vs_idx = self->uuid->uuid_vs_idx;
     }
 
-    common_hal_bleio_service_add_characteristic(self, characteristic);
-
     characteristic->service = self;
 
     mp_obj_list_append(self->char_list, characteristic);
diff --git a/shared-bindings/bleio/Service.h b/shared-bindings/bleio/Service.h
index 6e3079cf9..77c751829 100644
--- a/shared-bindings/bleio/Service.h
+++ b/shared-bindings/bleio/Service.h
@@ -32,7 +32,6 @@
 
 const mp_obj_type_t bleio_service_type;
 
-extern void common_hal_bleio_service_construct(bleio_service_obj_t *self);
 extern void common_hal_bleio_service_add_characteristic(bleio_service_obj_t *self, bleio_characteristic_obj_t *characteristic);
 
 #endif // MICROPY_INCLUDED_SHARED_BINDINGS_BLEIO_SERVICE_H