service discovery works; need to work on char and descriptor discovery

9 files changed, 44 insertions, 22 deletions

diff --git a/devices/ble_hci/common-hal/_bleio/Characteristic.c b/devices/ble_hci/common-hal/_bleio/Characteristic.c
index e2cdac08c..2f4a81f9a 100644
--- a/devices/ble_hci/common-hal/_bleio/Characteristic.c
+++ b/devices/ble_hci/common-hal/_bleio/Characteristic.c
@@ -149,6 +149,8 @@ void common_hal_bleio_characteristic_add_descriptor(bleio_characteristic_obj_t *
     }
 
     descriptor->handle = bleio_adapter_add_attribute(&common_hal_bleio_adapter_obj, MP_OBJ_TO_PTR(descriptor));
+    // Include this desriptor in the service handles range.
+    self->service->end_handle = descriptor->handle;
 
     // Link together all the descriptors for this characteristic.
     descriptor->next = self->descriptor_list;
diff --git a/devices/ble_hci/common-hal/_bleio/Service.c b/devices/ble_hci/common-hal/_bleio/Service.c
index c68355791..abccfd5c4 100644
--- a/devices/ble_hci/common-hal/_bleio/Service.c
+++ b/devices/ble_hci/common-hal/_bleio/Service.c
@@ -42,6 +42,8 @@ uint32_t _common_hal_bleio_service_construct(bleio_service_obj_t *self, bleio_uu
     vm_used_ble = true;
 
     self->handle = bleio_adapter_add_attribute(&common_hal_bleio_adapter_obj, MP_OBJ_TO_PTR(self));
+    self->start_handle = self->handle;
+    self->end_handle = self->handle;
     if (self->handle == BLE_GATT_HANDLE_INVALID) {
         return 1;
     }
@@ -90,10 +92,12 @@ void common_hal_bleio_service_add_characteristic(bleio_service_obj_t *self,
     }
     characteristic->decl_handle = bleio_adapter_add_attribute(
         &common_hal_bleio_adapter_obj, MP_OBJ_TO_PTR(characteristic));
-    // This is the value handle
+    // This is the value handle.
     characteristic->handle = bleio_adapter_add_attribute(
         &common_hal_bleio_adapter_obj, MP_OBJ_TO_PTR(characteristic));
 
+    self->end_handle = characteristic->handle;
+
     if (characteristic->props & (CHAR_PROP_NOTIFY | CHAR_PROP_INDICATE)) {
         // We need a CCCD.
         bleio_descriptor_obj_t *cccd = m_new_obj(bleio_descriptor_obj_t);
@@ -107,6 +111,8 @@ void common_hal_bleio_service_add_characteristic(bleio_service_obj_t *self,
         cccd->handle = cccd_handle;
         characteristic->cccd_handle = cccd_handle;
         common_hal_bleio_characteristic_add_descriptor(characteristic, cccd);
+
+        self->end_handle = cccd_handle;
     }
 
     // #if CIRCUITPY_VERBOSE_BLE
diff --git a/devices/ble_hci/common-hal/_bleio/Service.h b/devices/ble_hci/common-hal/_bleio/Service.h
index bb8bc61ed..11e7d1c96 100644
--- a/devices/ble_hci/common-hal/_bleio/Service.h
+++ b/devices/ble_hci/common-hal/_bleio/Service.h
@@ -43,7 +43,7 @@ typedef struct bleio_service_obj {
     // A local service doesn't know the connection.
     mp_obj_t connection;
     mp_obj_list_t *characteristic_list;
-    // Range of attribute handles of this remote service.
+    // Range of attribute handles of this service.
     uint16_t start_handle;
     uint16_t end_handle;
     struct bleio_service_obj* next;
diff --git a/devices/ble_hci/common-hal/_bleio/__init__.c b/devices/ble_hci/common-hal/_bleio/__init__.c
index 7139e6193..ee953cfeb 100644
--- a/devices/ble_hci/common-hal/_bleio/__init__.c
+++ b/devices/ble_hci/common-hal/_bleio/__init__.c
@@ -38,6 +38,8 @@
 #include "shared-bindings/_bleio/UUID.h"
 #include "supervisor/shared/bluetooth.h"
 
+bool vm_used_ble;
+
 void check_hci_error(hci_result_t result) {
     switch (result) {
         case HCI_OK:
@@ -110,6 +112,8 @@ void check_hci_error(hci_result_t result) {
 
 // Turn off BLE on a reset or reload.
 void bleio_reset() {
+    bleio_hci_reset();
+
     if (!common_hal_bleio_adapter_get_enabled(&common_hal_bleio_adapter_obj)) {
         return;
     }
diff --git a/devices/ble_hci/common-hal/_bleio/__init__.h b/devices/ble_hci/common-hal/_bleio/__init__.h
index 18b4289e9..5873675af 100644
--- a/devices/ble_hci/common-hal/_bleio/__init__.h
+++ b/devices/ble_hci/common-hal/_bleio/__init__.h
@@ -57,6 +57,6 @@ void check_gatt_status(uint16_t gatt_status);
 void check_sec_status(uint8_t sec_status);
 
 // Track if the user code modified the BLE state to know if we need to undo it on reload.
-bool vm_used_ble;
+extern bool vm_used_ble;
 
 #endif // MICROPY_INCLUDED_BLE_HCI_COMMON_HAL_INIT_H
diff --git a/devices/ble_hci/common-hal/_bleio/att.c b/devices/ble_hci/common-hal/_bleio/att.c
index 08324c37e..ba6c7c3d0 100644
--- a/devices/ble_hci/common-hal/_bleio/att.c
+++ b/devices/ble_hci/common-hal/_bleio/att.c
@@ -116,7 +116,7 @@ STATIC void check_and_save_expected_rsp(uint16_t conn_handle, uint8_t opcode, ui
     }
 }
 
-void att_init(void) {
+void bleio_att_reset(void) {
     max_mtu = BT_ATT_DEFAULT_LE_MTU;
     timeout = 5000;
     long_write_handle = BLE_GATT_HANDLE_INVALID;
@@ -884,6 +884,8 @@ void process_read_by_group_req(uint16_t conn_handle, uint16_t mtu, uint8_t dlen,
     struct bt_att_read_group_req *req = (struct bt_att_read_group_req *) data;
     uint16_t type_uuid = req->uuid[0] | (req->uuid[1] << 8);
 
+    // We only support returning services for BT_ATT_OP_READ_GROUP_REQ, which is typically used
+    // for service discovery.
     if (dlen != sizeof(struct bt_att_read_group_req) + sizeof(type_uuid) ||
         (type_uuid != BLE_TYPE_PRIMARY_SERVICE &&
          type_uuid != BLE_TYPE_SECONDARY_SERVICE)) {
@@ -897,7 +899,7 @@ void process_read_by_group_req(uint16_t conn_handle, uint16_t mtu, uint8_t dlen,
     } rsp_t;
 
     uint8_t rsp_bytes[mtu];
-    rsp_t *rsp = (rsp_t *) &rsp_bytes;
+    rsp_t *rsp = (rsp_t *) rsp_bytes;
     rsp->h.code = BT_ATT_OP_READ_GROUP_RSP;
     rsp->r.len = 0;
 
@@ -907,13 +909,22 @@ void process_read_by_group_req(uint16_t conn_handle, uint16_t mtu, uint8_t dlen,
     bool no_data = true;
 
     // All the data chunks must have uuid's that are the same size.
-    // Keep track fo the first one to make sure.
+    // Keep track of the first one to make sure.
     size_t sizeof_first_service_uuid = 0;
+
+    // Size of a single bt_att_group_data chunk. Start with the intial size, and
+    // add the uuid size in the loop below.
+    size_t data_length = sizeof(struct bt_att_group_data);
+
     const uint16_t max_attribute_handle = bleio_adapter_max_attribute_handle(&common_hal_bleio_adapter_obj);
     for (uint16_t handle = req->start_handle;
          handle <= max_attribute_handle && handle <= req->end_handle;
          handle++) {
-        no_data = false;
+
+        if (rsp_length + data_length > mtu) {
+            // The next possible bt_att_group_data chunk won't fit. The response is full.
+            break;
+        }
 
         mp_obj_t *attribute_obj = bleio_adapter_get_attribute(&common_hal_bleio_adapter_obj, handle);
         if (type_uuid != bleio_attribute_type_uuid(attribute_obj)) {
@@ -925,33 +936,28 @@ void process_read_by_group_req(uint16_t conn_handle, uint16_t mtu, uint8_t dlen,
 
         // Is this a 16-bit or a 128-bit uuid? It must match in size with any previous attribute
         // in this transmission.
-        const uint8_t sizeof_service_uuid = common_hal_bleio_uuid_get_size(service->uuid) / 8;
+        const uint32_t sizeof_service_uuid = common_hal_bleio_uuid_get_size(service->uuid) / 8;
         if (sizeof_first_service_uuid == 0) {
             sizeof_first_service_uuid = sizeof_service_uuid;
+            data_length += sizeof_service_uuid;
         } else if (sizeof_first_service_uuid != sizeof_service_uuid) {
-            // Mismatched sizes. Transmit just what we have so far in this batch.
+            // Mismatched sizes, which can't be in the same batch.
+            // Transmit just what we have so far in this batch.
             break;
         }
 
-        // Size of bt_att_group_data chunk with uuid.
-        const uint16_t data_length = sizeof(struct bt_att_group_data) + sizeof_service_uuid;
-
-        if (rsp_length + data_length > mtu) {
-            // No room for another bt_att_group_data chunk.
-            break;
-        }
 
         // Pass the length of ONE bt_att_group_data chunk. There may be multiple ones in this transmission.
         rsp->r.len = data_length;
 
-        uint8_t group_data_bytes[data_length];
-        struct bt_att_group_data *group_data = (struct bt_att_group_data *) group_data_bytes;
+        struct bt_att_group_data *group_data = (struct bt_att_group_data *) &rsp_bytes[rsp_length];
 
         group_data->start_handle = service->start_handle;
         group_data->end_handle = service->end_handle;
         common_hal_bleio_uuid_pack_into(service->uuid, group_data->value);
 
         rsp_length += data_length;
+        no_data = false;
     }
 
     if (no_data) {
diff --git a/devices/ble_hci/common-hal/_bleio/att.h b/devices/ble_hci/common-hal/_bleio/att.h
index 6cf9b305b..48f7836e0 100644
--- a/devices/ble_hci/common-hal/_bleio/att.h
+++ b/devices/ble_hci/common-hal/_bleio/att.h
@@ -30,6 +30,8 @@
 #include "hci_include/att.h"
 #include "hci_include/att_internal.h"
 
+void bleio_att_reset(void);
+
 //FIX BLEDevice att_central(void);
 //FIX BLERemoteDevice* att_device(uint8_t address_type, const uint8_t address[6]);
 //FIX void att_set_event_handler(BLEDeviceEvent event, BLEDeviceEventHandler eventHandler);
diff --git a/devices/ble_hci/common-hal/_bleio/hci.c b/devices/ble_hci/common-hal/_bleio/hci.c
index 7b4adaa79..52452a26d 100644
--- a/devices/ble_hci/common-hal/_bleio/hci.c
+++ b/devices/ble_hci/common-hal/_bleio/hci.c
@@ -272,18 +272,20 @@ STATIC void process_evt_pkt(size_t pkt_len, uint8_t pkt_data[])
     }
 }
 
-void hci_init(void) {
+void bleio_hci_reset(void) {
     rx_idx = 0;
     pending_pkt = 0;
     hci_poll_in_progress = false;
+
+    bleio_att_reset();
 }
 
 hci_result_t hci_poll_for_incoming_pkt_timeout(uint32_t timeout_msecs) {
     uint64_t start = supervisor_ticks_ms64();
 
-    hci_result_t result;
+    hci_result_t result = HCI_OK;
 
-    while (supervisor_ticks_ms64() -start < timeout_msecs) {
+    while (supervisor_ticks_ms64() - start < timeout_msecs) {
         result = hci_poll_for_incoming_pkt();
         RUN_BACKGROUND_TASKS;
     }
diff --git a/devices/ble_hci/common-hal/_bleio/hci.h b/devices/ble_hci/common-hal/_bleio/hci.h
index 89a2b3304..d907a84b7 100644
--- a/devices/ble_hci/common-hal/_bleio/hci.h
+++ b/devices/ble_hci/common-hal/_bleio/hci.h
@@ -39,7 +39,7 @@ typedef int hci_result_t;
 #define HCI_WRITE_ERROR (-5)
 #define HCI_ATT_ERROR (-6)
 
-void hci_init(void);
+void bleio_hci_reset(void);
 
 hci_result_t hci_disconnect(uint16_t handle);