extmod/modbluetooth: Allow discovery of svc/char by uuid.

In most situations this is a more efficient way of going straight to the
service and characteristic you need.

5 files changed, 81 insertions, 25 deletions

diff --git a/extmod/btstack/modbluetooth_btstack.c b/extmod/btstack/modbluetooth_btstack.c
index 625ccf4c4a..112c11a65e 100644
--- a/extmod/btstack/modbluetooth_btstack.c
+++ b/extmod/btstack/modbluetooth_btstack.c
@@ -677,12 +677,27 @@ int mp_bluetooth_gap_peripheral_connect(uint8_t addr_type, const uint8_t *addr,
     return btstack_error_to_errno(gap_connect(btstack_addr, addr_type));
 }
 
-int mp_bluetooth_gattc_discover_primary_services(uint16_t conn_handle) {
+int mp_bluetooth_gattc_discover_primary_services(uint16_t conn_handle, const mp_obj_bluetooth_uuid_t *uuid) {
     DEBUG_EVENT_printf("mp_bluetooth_gattc_discover_primary_services\n");
-    return btstack_error_to_errno(gatt_client_discover_primary_services(&btstack_packet_handler_discover_services, conn_handle));
+    uint8_t err;
+    if (uuid) {
+        if (uuid->type == MP_BLUETOOTH_UUID_TYPE_16) {
+            err = gatt_client_discover_primary_services_by_uuid16(&btstack_packet_handler_discover_services, conn_handle, get_uuid16(uuid));
+        } else if (uuid->type == MP_BLUETOOTH_UUID_TYPE_128) {
+            uint8_t buffer[16];
+            reverse_128(uuid->data, buffer);
+            err = gatt_client_discover_primary_services_by_uuid128(&btstack_packet_handler_discover_services, conn_handle, buffer);
+        } else {
+            DEBUG_EVENT_printf("  --> unknown UUID size\n");
+            return MP_EINVAL;
+        }
+    } else {
+        err = gatt_client_discover_primary_services(&btstack_packet_handler_discover_services, conn_handle);
+    }
+    return btstack_error_to_errno(err);
 }
 
-int mp_bluetooth_gattc_discover_characteristics(uint16_t conn_handle, uint16_t start_handle, uint16_t end_handle) {
+int mp_bluetooth_gattc_discover_characteristics(uint16_t conn_handle, uint16_t start_handle, uint16_t end_handle, const mp_obj_bluetooth_uuid_t *uuid) {
     DEBUG_EVENT_printf("mp_bluetooth_gattc_discover_characteristics\n");
     gatt_client_service_t service = {
         // Only start/end handles needed for gatt_client_discover_characteristics_for_service.
@@ -691,7 +706,22 @@ int mp_bluetooth_gattc_discover_characteristics(uint16_t conn_handle, uint16_t s
         .uuid16 = 0,
         .uuid128 = {0},
     };
-    return btstack_error_to_errno(gatt_client_discover_characteristics_for_service(&btstack_packet_handler_discover_characteristics, conn_handle, &service));
+    uint8_t err;
+    if (uuid) {
+        if (uuid->type == MP_BLUETOOTH_UUID_TYPE_16) {
+            err = gatt_client_discover_characteristics_for_service_by_uuid16(&btstack_packet_handler_discover_characteristics, conn_handle, &service, get_uuid16(uuid));
+        } else if (uuid->type == MP_BLUETOOTH_UUID_TYPE_128) {
+            uint8_t buffer[16];
+            reverse_128(uuid->data, buffer);
+            err = gatt_client_discover_characteristics_for_service_by_uuid128(&btstack_packet_handler_discover_characteristics, conn_handle, &service, buffer);
+        } else {
+            DEBUG_EVENT_printf("  --> unknown UUID size\n");
+            return MP_EINVAL;
+        }
+    } else {
+        err = gatt_client_discover_characteristics_for_service(&btstack_packet_handler_discover_characteristics, conn_handle, &service);
+    }
+    return btstack_error_to_errno(err);
 }
 
 int mp_bluetooth_gattc_discover_descriptors(uint16_t conn_handle, uint16_t start_handle, uint16_t end_handle) {
diff --git a/extmod/modbluetooth.c b/extmod/modbluetooth.c
index 7664e18295..3d66bc84c3 100644
--- a/extmod/modbluetooth.c
+++ b/extmod/modbluetooth.c
@@ -688,21 +688,27 @@ STATIC MP_DEFINE_CONST_FUN_OBJ_VAR_BETWEEN(bluetooth_ble_gatts_set_buffer_obj, 3
 
 #if MICROPY_PY_BLUETOOTH_ENABLE_CENTRAL_MODE
 
-STATIC mp_obj_t bluetooth_ble_gattc_discover_services(mp_obj_t self_in, mp_obj_t conn_handle_in) {
-    (void)self_in;
-    mp_int_t conn_handle = mp_obj_get_int(conn_handle_in);
-    return bluetooth_handle_errno(mp_bluetooth_gattc_discover_primary_services(conn_handle));
+STATIC mp_obj_t bluetooth_ble_gattc_discover_services(size_t n_args, const mp_obj_t *args) {
+    mp_int_t conn_handle = mp_obj_get_int(args[1]);
+    mp_obj_bluetooth_uuid_t *uuid = NULL;
+    if (n_args == 3) {
+        uuid = MP_OBJ_TO_PTR(args[2]);
+    }
+    return bluetooth_handle_errno(mp_bluetooth_gattc_discover_primary_services(conn_handle, uuid));
 }
-STATIC MP_DEFINE_CONST_FUN_OBJ_2(bluetooth_ble_gattc_discover_services_obj, bluetooth_ble_gattc_discover_services);
+STATIC MP_DEFINE_CONST_FUN_OBJ_VAR_BETWEEN(bluetooth_ble_gattc_discover_services_obj, 2, 3, bluetooth_ble_gattc_discover_services);
 
 STATIC mp_obj_t bluetooth_ble_gattc_discover_characteristics(size_t n_args, const mp_obj_t *args) {
-    (void)n_args;
     mp_int_t conn_handle = mp_obj_get_int(args[1]);
     mp_int_t start_handle = mp_obj_get_int(args[2]);
     mp_int_t end_handle = mp_obj_get_int(args[3]);
-    return bluetooth_handle_errno(mp_bluetooth_gattc_discover_characteristics(conn_handle, start_handle, end_handle));
+    mp_obj_bluetooth_uuid_t *uuid = NULL;
+    if (n_args == 3) {
+        uuid = MP_OBJ_TO_PTR(args[4]);
+    }
+    return bluetooth_handle_errno(mp_bluetooth_gattc_discover_characteristics(conn_handle, start_handle, end_handle, uuid));
 }
-STATIC MP_DEFINE_CONST_FUN_OBJ_VAR_BETWEEN(bluetooth_ble_gattc_discover_characteristics_obj, 4, 4, bluetooth_ble_gattc_discover_characteristics);
+STATIC MP_DEFINE_CONST_FUN_OBJ_VAR_BETWEEN(bluetooth_ble_gattc_discover_characteristics_obj, 4, 5, bluetooth_ble_gattc_discover_characteristics);
 
 STATIC mp_obj_t bluetooth_ble_gattc_discover_descriptors(size_t n_args, const mp_obj_t *args) {
     (void)n_args;
diff --git a/extmod/modbluetooth.h b/extmod/modbluetooth.h
index 2356223898..5d02d96e3c 100644
--- a/extmod/modbluetooth.h
+++ b/extmod/modbluetooth.h
@@ -221,10 +221,10 @@ int mp_bluetooth_gap_scan_stop(void);
 int mp_bluetooth_gap_peripheral_connect(uint8_t addr_type, const uint8_t *addr, int32_t duration_ms);
 
 // Find all primary services on the connected peripheral.
-int mp_bluetooth_gattc_discover_primary_services(uint16_t conn_handle);
+int mp_bluetooth_gattc_discover_primary_services(uint16_t conn_handle, const mp_obj_bluetooth_uuid_t *uuid);
 
 // Find all characteristics on the specified service on a connected peripheral.
-int mp_bluetooth_gattc_discover_characteristics(uint16_t conn_handle, uint16_t start_handle, uint16_t end_handle);
+int mp_bluetooth_gattc_discover_characteristics(uint16_t conn_handle, uint16_t start_handle, uint16_t end_handle, const mp_obj_bluetooth_uuid_t *uuid);
 
 // Find all descriptors on the specified characteristic on a connected peripheral.
 int mp_bluetooth_gattc_discover_descriptors(uint16_t conn_handle, uint16_t start_handle, uint16_t end_handle);
diff --git a/extmod/nimble/modbluetooth_nimble.c b/extmod/nimble/modbluetooth_nimble.c
index abe38d1f18..4b0f258481 100644
--- a/extmod/nimble/modbluetooth_nimble.c
+++ b/extmod/nimble/modbluetooth_nimble.c
@@ -74,19 +74,19 @@ STATIC int ble_hs_err_to_errno(int err) {
 }
 
 // Note: modbluetooth UUIDs store their data in LE.
-STATIC ble_uuid_t *create_nimble_uuid(const mp_obj_bluetooth_uuid_t *uuid) {
+STATIC ble_uuid_t *create_nimble_uuid(const mp_obj_bluetooth_uuid_t *uuid, ble_uuid_any_t *storage) {
     if (uuid->type == MP_BLUETOOTH_UUID_TYPE_16) {
-        ble_uuid16_t *result = m_new(ble_uuid16_t, 1);
+        ble_uuid16_t *result = storage ? &storage->u16 : m_new(ble_uuid16_t, 1);
         result->u.type = BLE_UUID_TYPE_16;
         result->value = (uuid->data[1] << 8) | uuid->data[0];
         return (ble_uuid_t *)result;
     } else if (uuid->type == MP_BLUETOOTH_UUID_TYPE_32) {
-        ble_uuid32_t *result = m_new(ble_uuid32_t, 1);
+        ble_uuid32_t *result = storage ? &storage->u32 : m_new(ble_uuid32_t, 1);
         result->u.type = BLE_UUID_TYPE_32;
         result->value = (uuid->data[1] << 24) | (uuid->data[1] << 16) | (uuid->data[1] << 8) | uuid->data[0];
         return (ble_uuid_t *)result;
     } else if (uuid->type == MP_BLUETOOTH_UUID_TYPE_128) {
-        ble_uuid128_t *result = m_new(ble_uuid128_t, 1);
+        ble_uuid128_t *result = storage ? &storage->u128 : m_new(ble_uuid128_t, 1);
         result->u.type = BLE_UUID_TYPE_128;
         memcpy(result->value, uuid->data, 16);
         return (ble_uuid_t *)result;
@@ -498,7 +498,7 @@ int mp_bluetooth_gatts_register_service(mp_obj_bluetooth_uuid_t *service_uuid, m
 
     struct ble_gatt_chr_def *characteristics = m_new(struct ble_gatt_chr_def, num_characteristics + 1);
     for (size_t i = 0; i < num_characteristics; ++i) {
-        characteristics[i].uuid = create_nimble_uuid(characteristic_uuids[i]);
+        characteristics[i].uuid = create_nimble_uuid(characteristic_uuids[i], NULL);
         characteristics[i].access_cb = characteristic_access_cb;
         characteristics[i].arg = NULL;
         characteristics[i].flags = characteristic_flags[i];
@@ -512,7 +512,7 @@ int mp_bluetooth_gatts_register_service(mp_obj_bluetooth_uuid_t *service_uuid, m
             struct ble_gatt_dsc_def *descriptors = m_new(struct ble_gatt_dsc_def, num_descriptors[i] + 1);
 
             for (size_t j = 0; j < num_descriptors[i]; ++j) {
-                descriptors[j].uuid = create_nimble_uuid(descriptor_uuids[descriptor_index]);
+                descriptors[j].uuid = create_nimble_uuid(descriptor_uuids[descriptor_index], NULL);
                 descriptors[j].access_cb = characteristic_access_cb;
                 descriptors[j].att_flags = descriptor_flags[descriptor_index];
                 descriptors[j].min_key_size = 0;
@@ -530,7 +530,7 @@ int mp_bluetooth_gatts_register_service(mp_obj_bluetooth_uuid_t *service_uuid, m
 
     struct ble_gatt_svc_def *service = m_new(struct ble_gatt_svc_def, 2);
     service[0].type = BLE_GATT_SVC_TYPE_PRIMARY;
-    service[0].uuid = create_nimble_uuid(service_uuid);
+    service[0].uuid = create_nimble_uuid(service_uuid, NULL);
     service[0].includes = NULL;
     service[0].characteristics = characteristics;
     service[1].type = 0; // no more services
@@ -769,11 +769,18 @@ STATIC int peripheral_discover_service_cb(uint16_t conn_handle, const struct ble
     return 0;
 }
 
-int mp_bluetooth_gattc_discover_primary_services(uint16_t conn_handle) {
+int mp_bluetooth_gattc_discover_primary_services(uint16_t conn_handle, const mp_obj_bluetooth_uuid_t *uuid) {
     if (!mp_bluetooth_is_active()) {
         return ERRNO_BLUETOOTH_NOT_ACTIVE;
     }
-    int err = ble_gattc_disc_all_svcs(conn_handle, &peripheral_discover_service_cb, NULL);
+    int err;
+    if (uuid) {
+        ble_uuid_any_t nimble_uuid;
+        create_nimble_uuid(uuid, &nimble_uuid);
+        err = ble_gattc_disc_svc_by_uuid(conn_handle, &nimble_uuid.u, &peripheral_discover_service_cb, NULL);
+    } else {
+        err = ble_gattc_disc_all_svcs(conn_handle, &peripheral_discover_service_cb, NULL);
+    }
     return ble_hs_err_to_errno(err);
 }
 
@@ -791,11 +798,18 @@ STATIC int ble_gatt_characteristic_cb(uint16_t conn_handle, const struct ble_gat
     return 0;
 }
 
-int mp_bluetooth_gattc_discover_characteristics(uint16_t conn_handle, uint16_t start_handle, uint16_t end_handle) {
+int mp_bluetooth_gattc_discover_characteristics(uint16_t conn_handle, uint16_t start_handle, uint16_t end_handle, const mp_obj_bluetooth_uuid_t *uuid) {
     if (!mp_bluetooth_is_active()) {
         return ERRNO_BLUETOOTH_NOT_ACTIVE;
     }
-    int err = ble_gattc_disc_all_chrs(conn_handle, start_handle, end_handle, &ble_gatt_characteristic_cb, NULL);
+    int err;
+    if (uuid) {
+        ble_uuid_any_t nimble_uuid;
+        create_nimble_uuid(uuid, &nimble_uuid);
+        err = ble_gattc_disc_chrs_by_uuid(conn_handle, start_handle, end_handle, &nimble_uuid.u, &ble_gatt_characteristic_cb, NULL);
+    } else {
+        err = ble_gattc_disc_all_chrs(conn_handle, start_handle, end_handle, &ble_gatt_characteristic_cb, NULL);
+    }
     return ble_hs_err_to_errno(err);
 }
 
diff --git a/extmod/nimble/nimble/npl_os.c b/extmod/nimble/nimble/npl_os.c
index 620dcb0aec..20c2604054 100644
--- a/extmod/nimble/nimble/npl_os.c
+++ b/extmod/nimble/nimble/npl_os.c
@@ -127,6 +127,12 @@ void *nimble_realloc(void *ptr, size_t size) {
     return ptr2;
 }
 
+// No-op implementation (only used by NimBLE logging).
+int nimble_sprintf(char *str, const char *fmt, ...) {
+    str[0] = 0;
+    return 0;
+}
+
 /******************************************************************************/
 // EVENTQ