extmod/modbluetooth: Add discover complete events for svc/char/desc.

Without this it's difficult to implement a state machine correctly if the
desired services are not found.

5 files changed, 100 insertions, 28 deletions

diff --git a/examples/bluetooth/ble_temperature_central.py b/examples/bluetooth/ble_temperature_central.py
index 8b679b9b8..19dc93409 100644
--- a/examples/bluetooth/ble_temperature_central.py
+++ b/examples/bluetooth/ble_temperature_central.py
@@ -80,6 +80,8 @@ class BLETemperatureCentral:
 
         # Connected device.
         self._conn_handle = None
+        self._start_handle = None
+        self._end_handle = None
         self._value_handle = None
 
     def _irq(self, event, data):
@@ -124,18 +126,31 @@ class BLETemperatureCentral:
             # Connected device returned a service.
             conn_handle, start_handle, end_handle, uuid = data
             if conn_handle == self._conn_handle and uuid == _ENV_SENSE_UUID:
+                self._start_handle, self._end_handle = start_handle, end_handle
+
+        elif event == _IRQ_GATTC_SERVICES_DONE:
+            # Service query complete.
+            if self._start_handle and self._end_handle:
                 self._ble.gattc_discover_characteristics(
-                    self._conn_handle, start_handle, end_handle
+                    self._conn_handle, self._start_handle, self._end_handle
                 )
+            else:
+                print("Failed to find environmental sensing service.")
 
         elif event == _IRQ_GATTC_CHARACTERISTIC_RESULT:
             # Connected device returned a characteristic.
             conn_handle, def_handle, value_handle, properties, uuid = data
             if conn_handle == self._conn_handle and uuid == _TEMP_UUID:
                 self._value_handle = value_handle
+
+        elif event == _IRQ_GATTC_CHARACTERISTICS_DONE:
+            # Characteristic query complete.
+            if self._value_handle:
                 # We've finished connecting and discovering device, fire the connect callback.
                 if self._conn_callback:
                     self._conn_callback()
+            else:
+                print("Failed to find temperature characteristic.")
 
         elif event == _IRQ_GATTC_READ_RESULT:
             # A read completed successfully.
diff --git a/extmod/btstack/modbluetooth_btstack.c b/extmod/btstack/modbluetooth_btstack.c
index 772fe4bed..625ccf4c4 100644
--- a/extmod/btstack/modbluetooth_btstack.c
+++ b/extmod/btstack/modbluetooth_btstack.c
@@ -56,10 +56,17 @@ volatile int mp_bluetooth_btstack_state = MP_BLUETOOTH_BTSTACK_STATE_OFF;
 STATIC btstack_packet_callback_registration_t hci_event_callback_registration;
 
 STATIC int btstack_error_to_errno(int err) {
+    DEBUG_EVENT_printf("  --> btstack error: %d\n", err);
     if (err == ERROR_CODE_SUCCESS) {
         return 0;
-    } else if (err == BTSTACK_ACL_BUFFERS_FULL) {
+    } else if (err == BTSTACK_ACL_BUFFERS_FULL || err == BTSTACK_MEMORY_ALLOC_FAILED) {
         return MP_ENOMEM;
+    } else if (err == GATT_CLIENT_IN_WRONG_STATE) {
+        return MP_EALREADY;
+    } else if (err == GATT_CLIENT_BUSY) {
+        return MP_EBUSY;
+    } else if (err == GATT_CLIENT_NOT_CONNECTED) {
+        return MP_ENOTCONN;
     } else {
         return MP_EINVAL;
     }
@@ -78,10 +85,8 @@ STATIC mp_obj_bluetooth_uuid_t create_mp_uuid(uint16_t uuid16, const uint8_t *uu
     return result;
 }
 
-STATIC void btstack_packet_handler(uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size) {
-    (void)channel;
-    (void)size;
-    DEBUG_EVENT_printf("btstack_packet_handler(packet_type=%u, channel=%u, packet=%p, size=%u)\n", packet_type, channel, packet, size);
+STATIC void btstack_packet_handler(uint8_t packet_type, uint8_t *packet, uint8_t irq) {
+    DEBUG_EVENT_printf("btstack_packet_handler(packet_type=%u, packet=%p)\n", packet_type, packet);
     if (packet_type != HCI_EVENT_PACKET) {
         return;
     }
@@ -157,6 +162,18 @@ STATIC void btstack_packet_handler(uint8_t packet_type, uint16_t channel, uint8_
     #if MICROPY_PY_BLUETOOTH_ENABLE_CENTRAL_MODE
     } else if (event_type == GATT_EVENT_QUERY_COMPLETE) {
         DEBUG_EVENT_printf("  --> gatt query complete\n");
+        uint16_t conn_handle = gatt_event_query_complete_get_handle(packet);
+        uint16_t status = gatt_event_query_complete_get_att_status(packet);
+        if (irq == MP_BLUETOOTH_IRQ_GATTC_READ_DONE || irq == MP_BLUETOOTH_IRQ_GATTC_WRITE_DONE) {
+            // TODO there is no value_handle available to pass here
+            mp_bluetooth_gattc_on_read_write_status(irq, conn_handle, 0, status);
+        } else if (irq == MP_BLUETOOTH_IRQ_GATTC_SERVICE_DONE ||
+                   irq == MP_BLUETOOTH_IRQ_GATTC_CHARACTERISTIC_DONE ||
+                   irq == MP_BLUETOOTH_IRQ_GATTC_DESCRIPTOR_DONE) {
+            mp_bluetooth_gattc_on_discover_complete(irq, conn_handle, status);
+        } else {
+            // Note that query complete is fired for other operations like query value too.
+        }
     } else if (event_type == GATT_EVENT_SERVICE_QUERY_RESULT) {
         DEBUG_EVENT_printf("  --> gatt service query result\n");
         uint16_t conn_handle = gatt_event_service_query_result_get_handle(packet);
@@ -208,31 +225,50 @@ STATIC void btstack_packet_handler(uint8_t packet_type, uint16_t channel, uint8_
         len = mp_bluetooth_gattc_on_data_available_start(MP_BLUETOOTH_IRQ_GATTC_INDICATE, conn_handle, value_handle, len, &atomic_state);
         mp_bluetooth_gattc_on_data_available_chunk(data, len);
         mp_bluetooth_gattc_on_data_available_end(atomic_state);
-    #endif
+    #endif // MICROPY_PY_BLUETOOTH_ENABLE_CENTRAL_MODE
     } else {
         DEBUG_EVENT_printf("  --> hci event type: unknown (0x%02x)\n", event_type);
     }
 }
 
+// Because the packet handler callbacks don't support an argument, we use a specific
+// handler when we need to provide additional state to the handler (in the "irq" parameter).
+// This is the generic handler for when you don't need extra state.
+STATIC void btstack_packet_handler_generic(uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size) {
+    (void)channel;
+    (void)size;
+    btstack_packet_handler(packet_type, packet, 0);
+}
+
 #if MICROPY_PY_BLUETOOTH_ENABLE_CENTRAL_MODE
-STATIC void btstack_packet_handler_write_with_response(uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size) {
+// For when the handler is being used for service discovery.
+STATIC void btstack_packet_handler_discover_services(uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size) {
     (void)channel;
     (void)size;
-    DEBUG_EVENT_printf("btstack_packet_handler_write_with_response(packet_type=%u, channel=%u, packet=%p, size=%u)\n", packet_type, channel, packet, size);
-    if (packet_type != HCI_EVENT_PACKET) {
-        return;
-    }
+    btstack_packet_handler(packet_type, packet, MP_BLUETOOTH_IRQ_GATTC_SERVICE_DONE);
+}
 
-    uint8_t event_type = hci_event_packet_get_type(packet);
-    if (event_type == GATT_EVENT_QUERY_COMPLETE) {
-        DEBUG_EVENT_printf("  --> gatt query complete\n");
-        uint16_t conn_handle = gatt_event_query_complete_get_handle(packet);
-        uint8_t status = gatt_event_query_complete_get_att_status(packet);
-        // TODO there is no value_handle to pass here
-        mp_bluetooth_gattc_on_write_status(conn_handle, 0, status);
-    }
+// For when the handler is being used for characteristic discovery.
+STATIC void btstack_packet_handler_discover_characteristics(uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size) {
+    (void)channel;
+    (void)size;
+    btstack_packet_handler(packet_type, packet, MP_BLUETOOTH_IRQ_GATTC_CHARACTERISTIC_DONE);
 }
-#endif
+
+// For when the handler is being used for descriptor discovery.
+STATIC void btstack_packet_handler_discover_descriptors(uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size) {
+    (void)channel;
+    (void)size;
+    btstack_packet_handler(packet_type, packet, MP_BLUETOOTH_IRQ_GATTC_DESCRIPTOR_DONE);
+}
+
+// For when the handler is being used for write-with-response.
+STATIC void btstack_packet_handler_write_with_response(uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size) {
+    (void)channel;
+    (void)size;
+    btstack_packet_handler(packet_type, packet, MP_BLUETOOTH_IRQ_GATTC_WRITE_DONE);
+}
+#endif // MICROPY_PY_BLUETOOTH_ENABLE_CENTRAL_MODE
 
 STATIC btstack_timer_source_t btstack_init_deinit_timeout;
 
@@ -285,7 +321,7 @@ int mp_bluetooth_init(void) {
     #endif
 
     // Register for HCI events.
-    hci_event_callback_registration.callback = &btstack_packet_handler;
+    hci_event_callback_registration.callback = &btstack_packet_handler_generic;
     hci_add_event_handler(&hci_event_callback_registration);
 
     // Set a timeout for HCI initialisation.
@@ -314,7 +350,7 @@ int mp_bluetooth_init(void) {
 
     #if MICROPY_PY_BLUETOOTH_ENABLE_CENTRAL_MODE
     // Enable GATT_EVENT_NOTIFICATION/GATT_EVENT_INDICATION for all connections and handles.
-    gatt_client_listen_for_characteristic_value_updates(&MP_STATE_PORT(bluetooth_btstack_root_pointers)->notification, &btstack_packet_handler, GATT_CLIENT_ANY_CONNECTION, NULL);
+    gatt_client_listen_for_characteristic_value_updates(&MP_STATE_PORT(bluetooth_btstack_root_pointers)->notification, &btstack_packet_handler_generic, GATT_CLIENT_ANY_CONNECTION, NULL);
     #endif
 
     return 0;
@@ -643,12 +679,11 @@ int mp_bluetooth_gap_peripheral_connect(uint8_t addr_type, const uint8_t *addr,
 
 int mp_bluetooth_gattc_discover_primary_services(uint16_t conn_handle) {
     DEBUG_EVENT_printf("mp_bluetooth_gattc_discover_primary_services\n");
-    return btstack_error_to_errno(gatt_client_discover_primary_services(&btstack_packet_handler, conn_handle));
+    return btstack_error_to_errno(gatt_client_discover_primary_services(&btstack_packet_handler_discover_services, conn_handle));
 }
 
 int mp_bluetooth_gattc_discover_characteristics(uint16_t conn_handle, uint16_t start_handle, uint16_t end_handle) {
     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.
         .start_group_handle = start_handle,
@@ -656,7 +691,7 @@ 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, conn_handle, &service));
+    return btstack_error_to_errno(gatt_client_discover_characteristics_for_service(&btstack_packet_handler_discover_characteristics, conn_handle, &service));
 }
 
 int mp_bluetooth_gattc_discover_descriptors(uint16_t conn_handle, uint16_t start_handle, uint16_t end_handle) {
@@ -670,12 +705,12 @@ int mp_bluetooth_gattc_discover_descriptors(uint16_t conn_handle, uint16_t start
         .uuid16 = 0,
         .uuid128 = {0},
     };
-    return btstack_error_to_errno(gatt_client_discover_characteristic_descriptors(&btstack_packet_handler, conn_handle, &characteristic));
+    return btstack_error_to_errno(gatt_client_discover_characteristic_descriptors(&btstack_packet_handler_discover_descriptors, conn_handle, &characteristic));
 }
 
 int mp_bluetooth_gattc_read(uint16_t conn_handle, uint16_t value_handle) {
     DEBUG_EVENT_printf("mp_bluetooth_gattc_read\n");
-    return btstack_error_to_errno(gatt_client_read_value_of_characteristic_using_value_handle(&btstack_packet_handler, conn_handle, value_handle));
+    return btstack_error_to_errno(gatt_client_read_value_of_characteristic_using_value_handle(&btstack_packet_handler_generic, conn_handle, value_handle));
 }
 
 int mp_bluetooth_gattc_write(uint16_t conn_handle, uint16_t value_handle, const uint8_t *value, size_t *value_len, unsigned int mode) {
diff --git a/extmod/modbluetooth.c b/extmod/modbluetooth.c
index b43bec017..7664e1829 100644
--- a/extmod/modbluetooth.c
+++ b/extmod/modbluetooth.c
@@ -886,6 +886,9 @@ STATIC mp_obj_t bluetooth_ble_invoke_irq(mp_obj_t none_in) {
         } else if (event == MP_BLUETOOTH_IRQ_GATTC_DESCRIPTOR_RESULT) {
             // conn_handle, handle, uuid
             ringbuf_extract(&o->ringbuf, data_tuple, 2, 0, NULL, 0, &o->irq_data_uuid, NULL);
+        } else if (event == MP_BLUETOOTH_IRQ_GATTC_SERVICE_DONE || event == MP_BLUETOOTH_IRQ_GATTC_CHARACTERISTIC_DONE || event == MP_BLUETOOTH_IRQ_GATTC_DESCRIPTOR_DONE) {
+            // conn_handle, status
+            ringbuf_extract(&o->ringbuf, data_tuple, 2, 0, NULL, 0, NULL, NULL);
         } else if (event == MP_BLUETOOTH_IRQ_GATTC_READ_RESULT || event == MP_BLUETOOTH_IRQ_GATTC_NOTIFY || event == MP_BLUETOOTH_IRQ_GATTC_INDICATE) {
             // conn_handle, value_handle, data
             ringbuf_extract(&o->ringbuf, data_tuple, 2, 0, NULL, 0, NULL, &o->irq_data_data);
@@ -1045,6 +1048,16 @@ void mp_bluetooth_gattc_on_descriptor_result(uint16_t conn_handle, uint16_t hand
     schedule_ringbuf(atomic_state);
 }
 
+void mp_bluetooth_gattc_on_discover_complete(uint8_t event, uint16_t conn_handle, uint16_t status) {
+    MICROPY_PY_BLUETOOTH_ENTER
+    mp_obj_bluetooth_ble_t *o = MP_OBJ_TO_PTR(MP_STATE_VM(bluetooth));
+    if (enqueue_irq(o, 2 + 2, event)) {
+        ringbuf_put16(&o->ringbuf, conn_handle);
+        ringbuf_put16(&o->ringbuf, status);
+    }
+    schedule_ringbuf(atomic_state);
+}
+
 size_t mp_bluetooth_gattc_on_data_available_start(uint8_t event, uint16_t conn_handle, uint16_t value_handle, size_t data_len, mp_uint_t *atomic_state_out) {
     MICROPY_PY_BLUETOOTH_ENTER
     *atomic_state_out = atomic_state;
diff --git a/extmod/modbluetooth.h b/extmod/modbluetooth.h
index 091a62c84..235622389 100644
--- a/extmod/modbluetooth.h
+++ b/extmod/modbluetooth.h
@@ -266,6 +266,9 @@ void mp_bluetooth_gattc_on_characteristic_result(uint16_t conn_handle, uint16_t
 // Notify modbluetooth that a descriptor was found.
 void mp_bluetooth_gattc_on_descriptor_result(uint16_t conn_handle, uint16_t handle, mp_obj_bluetooth_uuid_t *descriptor_uuid);
 
+// Notify modbluetooth that service, characteristic or descriptor discovery has finished.
+void mp_bluetooth_gattc_on_discover_complete(uint8_t event, uint16_t conn_handle, uint16_t status);
+
 // Notify modbluetooth that a read has completed with data (or notify/indicate data available, use `event` to disambiguate).
 // Note: these functions are to be called in a group protected by MICROPY_PY_BLUETOOTH_ENTER/EXIT.
 // _start returns the number of bytes to submit to the calls to _chunk, followed by a call to _end.
diff --git a/extmod/nimble/modbluetooth_nimble.c b/extmod/nimble/modbluetooth_nimble.c
index 656b99a56..abe38d1f1 100644
--- a/extmod/nimble/modbluetooth_nimble.c
+++ b/extmod/nimble/modbluetooth_nimble.c
@@ -763,6 +763,8 @@ STATIC int peripheral_discover_service_cb(uint16_t conn_handle, const struct ble
     if (error->status == 0) {
         mp_obj_bluetooth_uuid_t service_uuid = create_mp_uuid(&service->uuid);
         mp_bluetooth_gattc_on_primary_service_result(conn_handle, service->start_handle, service->end_handle, &service_uuid);
+    } else {
+        mp_bluetooth_gattc_on_discover_complete(MP_BLUETOOTH_IRQ_GATTC_SERVICE_DONE, conn_handle, error->status);
     }
     return 0;
 }
@@ -783,6 +785,8 @@ STATIC int ble_gatt_characteristic_cb(uint16_t conn_handle, const struct ble_gat
     if (error->status == 0) {
         mp_obj_bluetooth_uuid_t characteristic_uuid = create_mp_uuid(&characteristic->uuid);
         mp_bluetooth_gattc_on_characteristic_result(conn_handle, characteristic->def_handle, characteristic->val_handle, characteristic->properties, &characteristic_uuid);
+    } else {
+        mp_bluetooth_gattc_on_discover_complete(MP_BLUETOOTH_IRQ_GATTC_CHARACTERISTIC_DONE, conn_handle, error->status);
     }
     return 0;
 }
@@ -803,6 +807,8 @@ STATIC int ble_gatt_descriptor_cb(uint16_t conn_handle, const struct ble_gatt_er
     if (error->status == 0) {
         mp_obj_bluetooth_uuid_t descriptor_uuid = create_mp_uuid(&descriptor->uuid);
         mp_bluetooth_gattc_on_descriptor_result(conn_handle, descriptor->handle, &descriptor_uuid);
+    } else {
+        mp_bluetooth_gattc_on_discover_complete(MP_BLUETOOTH_IRQ_GATTC_DESCRIPTOR_DONE, conn_handle, error->status);
     }
     return 0;
 }