pbio/drv/bluetooth: Support multiple peripherals.

This completes the recent overhaul to clean up references to peripheral singletons.

We keep supporting only one on Move Hub, City Hub, and Technic Hub due to hardware limitations with their Bluetooth chips.

Author: laurensvalk

lib/pbio/drv/bluetooth/bluetooth.c

@@ -147,31 +147,29 @@ pbio_error_t pbdrv_bluetooth_send_event_notification(pbio_os_state_t *state, pbi
 // Functions related to connections to peripherals.
 //
 
-pbdrv_bluetooth_peripheral_t peripheral_singleton;
-
 pbio_error_t pbdrv_bluetooth_peripheral_get_available(pbdrv_bluetooth_peripheral_t **peripheral, void *user) {
-    // Only a single peripheral instance supported for now.
-    *peripheral = &peripheral_singleton;
-
-    if (/* Already connected. */
-        pbdrv_bluetooth_is_connected(PBDRV_BLUETOOTH_CONNECTION_PERIPHERAL) ||
-        /* A different user is still claiming this resource. */
-        (*peripheral)->user ||
-        /* Busy. Could be connecting but not connected yet. */
-        (*peripheral)->func) {
-        return PBIO_ERROR_BUSY;
-    }
 
-    // Claim this device for new user.
-    (*peripheral)->user = user;
+    for (uint8_t i = 0; i < PBDRV_CONFIG_BLUETOOTH_NUM_PERIPHERALS; i++) {
+        pbdrv_bluetooth_peripheral_t *peri = pbdrv_bluetooth_peripheral_get_by_index(i);
 
-    return PBIO_SUCCESS;
+        // Test if not already in use, not connected, and not busy.
+        if (!pbdrv_bluetooth_peripheral_is_connected(peri) && !peri->user && !peri->func) {
+            // Claim this device for new user.
+            peri->user = user;
+            *peripheral = peri;
+            return PBIO_SUCCESS;
+        }
+    }
+
+    // All instances are in use.
+    *peripheral = NULL;
+    return PBIO_ERROR_BUSY;
 }
 
 void pbdrv_bluetooth_peripheral_release(pbdrv_bluetooth_peripheral_t *peripheral, void *user) {
     // Only release if the user matches. A new user may have already safely
     // claimed it, and this call to release may come from an orphaned user.
-    if (peripheral->user != user) {
+    if (!peripheral || peripheral->user != user) {
         return;
     }
     peripheral->user = NULL;
@@ -188,7 +186,7 @@ pbio_error_t pbdrv_bluetooth_peripheral_scan_and_connect(pbdrv_bluetooth_periphe
     }
 
     // Can't connect if already connected or already busy.
-    if (pbdrv_bluetooth_is_connected(PBDRV_BLUETOOTH_CONNECTION_PERIPHERAL) || peri->func) {
+    if (pbdrv_bluetooth_peripheral_is_connected(peri) || peri->func) {
         return PBIO_ERROR_BUSY;
     }
 
@@ -219,7 +217,7 @@ pbio_error_t pbdrv_bluetooth_peripheral_disconnect(pbdrv_bluetooth_peripheral_t
     }
 
     // Pass silently for already disconnected.
-    if (!pbdrv_bluetooth_is_connected(PBDRV_BLUETOOTH_CONNECTION_PERIPHERAL)) {
+    if (!pbdrv_bluetooth_peripheral_is_connected(peri)) {
         peri->err = PBIO_SUCCESS;
         return PBIO_SUCCESS;
     }
@@ -233,7 +231,7 @@ pbio_error_t pbdrv_bluetooth_peripheral_disconnect(pbdrv_bluetooth_peripheral_t
 
 pbio_error_t pbdrv_bluetooth_peripheral_discover_characteristic(pbdrv_bluetooth_peripheral_t *peri, pbdrv_bluetooth_peripheral_char_t *characteristic) {
 
-    if (!pbdrv_bluetooth_is_connected(PBDRV_BLUETOOTH_CONNECTION_PERIPHERAL)) {
+    if (!pbdrv_bluetooth_peripheral_is_connected(peri)) {
         return PBIO_ERROR_NO_DEV;
     }
     if (peri->func) {
@@ -251,7 +249,7 @@ pbio_error_t pbdrv_bluetooth_peripheral_discover_characteristic(pbdrv_bluetooth_
 
 pbio_error_t pbdrv_bluetooth_peripheral_read_characteristic(pbdrv_bluetooth_peripheral_t *peri, pbdrv_bluetooth_peripheral_char_t *characteristic) {
 
-    if (!pbdrv_bluetooth_is_connected(PBDRV_BLUETOOTH_CONNECTION_PERIPHERAL)) {
+    if (!pbdrv_bluetooth_peripheral_is_connected(peri)) {
         return PBIO_ERROR_NO_DEV;
     }
     if (peri->func) {
@@ -272,7 +270,7 @@ size_t pbdrv_bluetooth_char_write_size;
 
 pbio_error_t pbdrv_bluetooth_peripheral_write_characteristic(pbdrv_bluetooth_peripheral_t *peri, uint16_t handle, const uint8_t *data, size_t size) {
 
-    if (!pbdrv_bluetooth_is_connected(PBDRV_BLUETOOTH_CONNECTION_PERIPHERAL)) {
+    if (!pbdrv_bluetooth_peripheral_is_connected(peri)) {
         return PBIO_ERROR_NO_DEV;
     }
     if (peri->func) {
@@ -307,7 +305,10 @@ pbio_error_t pbdrv_bluetooth_await_peripheral_command(pbio_os_state_t *state, vo
 void pbdrv_bluetooth_cancel_operation_request(void) {
     // Only some peripheral actions support cancellation.
     DEBUG_PRINT("Bluetooth operation cancel requested.\n");
-    peripheral_singleton.cancel = true;
+    for (uint8_t i = 0; i < PBDRV_CONFIG_BLUETOOTH_NUM_PERIPHERALS; i++) {
+        pbdrv_bluetooth_peripheral_t *peri = pbdrv_bluetooth_peripheral_get_by_index(i);
+        peri->cancel = true;
+    }
 }
 
 //
@@ -523,7 +524,10 @@ pbio_error_t pbdrv_bluetooth_process_thread(pbio_os_state_t *state, void *contex
 
     // Shorthand notation accessible throughout.
     bool can_send = pbdrv_bluetooth_is_connected(PBDRV_BLUETOOTH_CONNECTION_PYBRICKS);
-    pbdrv_bluetooth_peripheral_t *peri = &peripheral_singleton;
+
+    // For looping over peripherals.
+    static uint8_t peri_index;
+    static pbdrv_bluetooth_peripheral_t *peri;
 
     PBIO_OS_ASYNC_BEGIN(state);
 
@@ -565,18 +569,21 @@ pbio_error_t pbdrv_bluetooth_process_thread(pbio_os_state_t *state, void *contex
             advertising_or_scan_func = NULL;
         }
 
-        // Handle pending peripheral task, if any.
-        if (peri->func) {
+        // Handle pending peripheral tasks, one at a time.
+        for (peri_index = 0; peri_index < PBDRV_CONFIG_BLUETOOTH_NUM_PERIPHERALS; peri_index++) {
+            peri = pbdrv_bluetooth_peripheral_get_by_index(peri_index);
+            if (peri->func) {
 
-            // If currently observing, stop if we need to scan for a peripheral.
-            if (pbdrv_bluetooth_is_observing && peri->func == pbdrv_bluetooth_peripheral_scan_and_connect_func) {
-                PBIO_OS_AWAIT(state, &sub, pbdrv_bluetooth_stop_observing_func(&sub, NULL));
-                observe_restart_requested = true;
-            }
+                // If currently observing, stop if we need to scan for a peripheral.
+                if (pbdrv_bluetooth_is_observing && peri->func == pbdrv_bluetooth_peripheral_scan_and_connect_func) {
+                    PBIO_OS_AWAIT(state, &sub, pbdrv_bluetooth_stop_observing_func(&sub, NULL));
+                    observe_restart_requested = true;
+                }
 
-            PBIO_OS_AWAIT(state, &sub, peri->err = peri->func(&sub, peri));
-            peri->func = NULL;
-            peri->cancel = false;
+                PBIO_OS_AWAIT(state, &sub, peri->err = peri->func(&sub, peri));
+                peri->func = NULL;
+                peri->cancel = false;
+            }
         }
 
         // Restart if we stopped it temporarily to scan for a peripheral or
@@ -606,21 +613,32 @@ pbio_error_t pbdrv_bluetooth_close_user_tasks(pbio_os_state_t *state, pbio_os_ti
 
     static pbio_os_state_t sub;
 
+    // For looping over peripherals.
+    static uint8_t peri_index;
+    static pbdrv_bluetooth_peripheral_t *peri;
+
     if (pbio_os_timer_is_expired(timer)) {
         return PBIO_ERROR_TIMEDOUT;
     }
 
     PBIO_OS_ASYNC_BEGIN(state);
 
+    // Requests peripheral operations to cancel, if they support it.
     pbdrv_bluetooth_cancel_operation_request();
 
-    // Let ongoing user tasks finish first.
-    PBIO_OS_AWAIT(state, &sub, pbdrv_bluetooth_await_peripheral_command(&sub, &peripheral_singleton));
-    PBIO_OS_AWAIT(state, &sub, pbdrv_bluetooth_await_advertise_or_scan_command(&sub, NULL));
+    for (peri_index = 0; peri_index < PBDRV_CONFIG_BLUETOOTH_NUM_PERIPHERALS; peri_index++) {
+        peri = pbdrv_bluetooth_peripheral_get_by_index(peri_index);
+
+        // Await ongoing peripheral user task.
+        PBIO_OS_AWAIT(state, &sub, pbdrv_bluetooth_await_peripheral_command(&sub, peri));
+
+        // Disconnect peripheral.
+        pbdrv_bluetooth_peripheral_disconnect(peri);
+        PBIO_OS_AWAIT(state, &sub, pbdrv_bluetooth_await_peripheral_command(&sub, peri));
+    }
 
-    // Disconnect peripheral.
-    pbdrv_bluetooth_peripheral_disconnect(&peripheral_singleton);
-    PBIO_OS_AWAIT(state, &sub, pbdrv_bluetooth_await_peripheral_command(&sub, &peripheral_singleton));
+    // Let ongoing user task finish first.
+    PBIO_OS_AWAIT(state, &sub, pbdrv_bluetooth_await_advertise_or_scan_command(&sub, NULL));
 
     // Stop scanning.
     pbdrv_bluetooth_start_observing(NULL);
@@ -643,7 +661,16 @@ void pbdrv_bluetooth_deinit(void) {
     // Under normal operation ::pbdrv_bluetooth_close_user_tasks completes
     // normally and there should be no user activity at this point. If there
     // is, a task got stuck, so exit forcefully.
-    if (advertising_or_scan_err == PBIO_ERROR_AGAIN || peripheral_singleton.err == PBIO_ERROR_AGAIN) {
+    bool failed_to_stop = advertising_or_scan_err == PBIO_ERROR_AGAIN;
+    for (uint8_t i = 0; i < PBDRV_CONFIG_BLUETOOTH_NUM_PERIPHERALS; i++) {
+        pbdrv_bluetooth_peripheral_t *peri = pbdrv_bluetooth_peripheral_get_by_index(i);
+        if (peri->err == PBIO_ERROR_AGAIN) {
+            failed_to_stop = true;
+            break;
+        }
+    }
+
+    if (failed_to_stop) {
         // Hard reset without waiting on completion of any process.
         DEBUG_PRINT("Bluetooth deinit: forcing hard reset due to busy tasks.\n");
         pbdrv_bluetooth_controller_reset_hard();

lib/pbio/drv/bluetooth/bluetooth.h

@@ -27,22 +27,22 @@ pbio_error_t pbdrv_bluetooth_controller_reset(pbio_os_state_t *state, pbio_os_ti
 pbio_error_t pbdrv_bluetooth_controller_initialize(pbio_os_state_t *state, pbio_os_timer_t *timer);
 
 pbio_error_t pbdrv_bluetooth_start_broadcasting_func(pbio_os_state_t *state, void *context);
+pbio_error_t pbdrv_bluetooth_start_advertising_func(pbio_os_state_t *state, void *context);
+pbio_error_t pbdrv_bluetooth_stop_advertising_func(pbio_os_state_t *state, void *context);
+pbio_error_t pbdrv_bluetooth_start_observing_func(pbio_os_state_t *state, void *context);
+pbio_error_t pbdrv_bluetooth_stop_observing_func(pbio_os_state_t *state, void *context);
+
+pbdrv_bluetooth_peripheral_t *pbdrv_bluetooth_peripheral_get_by_index(uint8_t index);
 pbio_error_t pbdrv_bluetooth_peripheral_disconnect_func(pbio_os_state_t *state, void *context);
 pbio_error_t pbdrv_bluetooth_peripheral_discover_characteristic_func(pbio_os_state_t *state, void *context);
 pbio_error_t pbdrv_bluetooth_peripheral_read_characteristic_func(pbio_os_state_t *state, void *context);
 pbio_error_t pbdrv_bluetooth_peripheral_scan_and_connect_func(pbio_os_state_t *state, void *context);
 pbio_error_t pbdrv_bluetooth_peripheral_write_characteristic_func(pbio_os_state_t *state, void *context);
-pbio_error_t pbdrv_bluetooth_start_advertising_func(pbio_os_state_t *state, void *context);
-pbio_error_t pbdrv_bluetooth_stop_advertising_func(pbio_os_state_t *state, void *context);
-pbio_error_t pbdrv_bluetooth_start_observing_func(pbio_os_state_t *state, void *context);
-pbio_error_t pbdrv_bluetooth_stop_observing_func(pbio_os_state_t *state, void *context);
 
 pbio_error_t pbdrv_bluetooth_send_pybricks_value_notification(pbio_os_state_t *state, const uint8_t *data, uint16_t size);
 
 extern pbdrv_bluetooth_receive_handler_t pbdrv_bluetooth_receive_handler;
 
-extern pbdrv_bluetooth_peripheral_t peripheral_singleton;
-
 extern uint16_t pbdrv_bluetooth_char_write_handle;
 extern uint8_t pbdrv_bluetooth_char_write_data[PBDRV_BLUETOOTH_MAX_CHAR_SIZE];
 extern size_t pbdrv_bluetooth_char_write_size;

lib/pbio/drv/bluetooth/bluetooth_btstack.c

@@ -89,7 +89,17 @@ static struct _pbdrv_bluetooth_peripheral_platform_state_t {
      *  are set up such that only one char is discovered at a time
      */
     gatt_client_characteristic_t current_char;
-} peripheral_platform_state;
+} peripheral_platform_state[PBDRV_CONFIG_BLUETOOTH_NUM_PERIPHERALS];
+
+static pbdrv_bluetooth_peripheral_t _peripherals[PBDRV_CONFIG_BLUETOOTH_NUM_PERIPHERALS];
+
+pbdrv_bluetooth_peripheral_t *pbdrv_bluetooth_peripheral_get_by_index(uint8_t index) {
+    if (index >= PBDRV_CONFIG_BLUETOOTH_NUM_PERIPHERALS) {
+        return NULL;
+    }
+    return &_peripherals[index];
+}
+
 #endif // PBDRV_CONFIG_BLUETOOTH_BTSTACK_LE
 
 // hub name goes in special section so that it can be modified when flashing firmware
@@ -183,7 +193,7 @@ static void pbdrv_bluetooth_peripheral_disconnect_now(pbdrv_bluetooth_peripheral
 /**
  * Checks if the given peripheral is connected.
  */
-static bool pbdrv_bluetooth_peripheral_is_connected(pbdrv_bluetooth_peripheral_t *peri) {
+bool pbdrv_bluetooth_peripheral_is_connected(pbdrv_bluetooth_peripheral_t *peri) {
     return peri->con_handle != HCI_CON_HANDLE_INVALID;
 }
 
@@ -236,9 +246,6 @@ bool pbdrv_bluetooth_is_connected(pbdrv_bluetooth_connection_t connection) {
         return true;
     }
 
-    if (connection == PBDRV_BLUETOOTH_CONNECTION_PERIPHERAL && peripheral_singleton.con_handle != HCI_CON_HANDLE_INVALID) {
-        return true;
-    }
     #endif // PBDRV_CONFIG_BLUETOOTH_BTSTACK_LE
 
     return false;
@@ -307,10 +314,6 @@ static void packet_handler(uint8_t packet_type, uint16_t channel, uint8_t *packe
     // Platform-specific platform handler has priority.
     pbdrv_bluetooth_btstack_platform_packet_handler(packet_type, channel, packet, size);
 
-    #if PBDRV_CONFIG_BLUETOOTH_BTSTACK_LE
-    pbdrv_bluetooth_peripheral_t *peri = &peripheral_singleton;
-    #endif // PBDRV_CONFIG_BLUETOOTH_BTSTACK_LE
-
     switch (hci_event_packet_get_type(packet)) {
         case HCI_EVENT_COMMAND_COMPLETE: {
             const uint8_t *rp = hci_event_command_complete_get_return_parameters(packet);
@@ -343,17 +346,19 @@ static void packet_handler(uint8_t packet_type, uint16_t channel, uint8_t *packe
         case GATT_EVENT_QUERY_COMPLETE:
             DEBUG_PRINT("GATT_EVENT_QUERY_COMPLETE\n");
             break;
-        case GATT_EVENT_NOTIFICATION: {
-            if (gatt_event_notification_get_handle(packet) != peri->con_handle) {
-                break;
-            }
-            if (peri->config->notification_handler) {
-                uint16_t length = gatt_event_notification_get_value_length(packet);
-                const uint8_t *value = gatt_event_notification_get_value(packet);
-                peri->config->notification_handler(peri->user, value, length);
+        case GATT_EVENT_NOTIFICATION:
+            for (uint8_t i = 0; i < PBDRV_CONFIG_BLUETOOTH_NUM_PERIPHERALS; i++) {
+                pbdrv_bluetooth_peripheral_t *peri = pbdrv_bluetooth_peripheral_get_by_index(i);
+                if (!peri || !peri->config || !peri->config->notification_handler) {
+                    continue;
+                }
+                if (gatt_event_notification_get_handle(packet) == peri->con_handle) {
+                    uint16_t length = gatt_event_notification_get_value_length(packet);
+                    const uint8_t *value = gatt_event_notification_get_value(packet);
+                    peri->config->notification_handler(peri->user, value, length);
+                }
             }
             break;
-        }
         case HCI_EVENT_LE_META:
             if (hci_event_le_meta_get_subevent_code(packet) != HCI_SUBEVENT_LE_CONNECTION_COMPLETE) {
                 break;
@@ -374,10 +379,15 @@ static void packet_handler(uint8_t packet_type, uint16_t channel, uint8_t *packe
                 le_con_handle = HCI_CON_HANDLE_INVALID;
                 pybricks_con_handle = HCI_CON_HANDLE_INVALID;
                 uart_con_handle = HCI_CON_HANDLE_INVALID;
-            } else if (hci_event_disconnection_complete_get_connection_handle(packet) == peri->con_handle) {
-                DEBUG_PRINT("Peripheral disconnected\n");
-                gatt_client_stop_listening_for_characteristic_value_updates(&peri->platform_state->notification);
-                peri->con_handle = HCI_CON_HANDLE_INVALID;
+            } else {
+                for (uint8_t i = 0; i < PBDRV_CONFIG_BLUETOOTH_NUM_PERIPHERALS; i++) {
+                    pbdrv_bluetooth_peripheral_t *peri = pbdrv_bluetooth_peripheral_get_by_index(i);
+                    if (peri && hci_event_disconnection_complete_get_connection_handle(packet) == peri->con_handle) {
+                        DEBUG_PRINT("Peripheral %u with handle %u disconnected\n", i, peri->con_handle);
+                        gatt_client_stop_listening_for_characteristic_value_updates(&peri->platform_state->notification);
+                        peri->con_handle = HCI_CON_HANDLE_INVALID;
+                    }
+                }
             }
             break;
 
@@ -1065,6 +1075,12 @@ pbio_error_t pbdrv_bluetooth_stop_observing_func(pbio_os_state_t *state, void *c
 pbio_error_t pbdrv_bluetooth_start_broadcasting_func(pbio_os_state_t *state, void *context) {
     return PBIO_ERROR_NOT_SUPPORTED;
 }
+bool pbdrv_bluetooth_peripheral_is_connected(pbdrv_bluetooth_peripheral_t *peri) {
+    return false;
+}
+pbdrv_bluetooth_peripheral_t *pbdrv_bluetooth_peripheral_get_by_index(uint8_t index) {
+    return NULL;
+}
 pbio_error_t pbdrv_bluetooth_peripheral_disconnect_func(pbio_os_state_t *state, void *context) {
     return PBIO_ERROR_NOT_SUPPORTED;
 }
@@ -1264,9 +1280,12 @@ void pbdrv_bluetooth_init_hci(void) {
     static btstack_packet_callback_registration_t hci_event_callback_registration;
 
     #if PBDRV_CONFIG_BLUETOOTH_BTSTACK_LE
-    // don't need to init the whole struct, so doing this here
-    peripheral_singleton.platform_state = &peripheral_platform_state;
-    peripheral_singleton.con_handle = HCI_CON_HANDLE_INVALID;
+    // Attach btstack platform state to peripherals.
+    for (uint8_t i = 0; i < PBDRV_CONFIG_BLUETOOTH_NUM_PERIPHERALS; i++) {
+        pbdrv_bluetooth_peripheral_t *peri = pbdrv_bluetooth_peripheral_get_by_index(i);
+        peri->platform_state = &peripheral_platform_state[i];
+        peri->con_handle = HCI_CON_HANDLE_INVALID;
+    }
     #endif
 
     btstack_memory_init();

lib/pbio/drv/bluetooth/bluetooth_stm32_bluenrg.c

@@ -124,6 +124,19 @@ static const pbdrv_gpio_t mosi_gpio = { .bank = GPIOC, .pin = 3 };
 static const pbdrv_gpio_t miso_gpio = { .bank = GPIOC, .pin = 2 };
 static const pbdrv_gpio_t sck_gpio = { .bank = GPIOB, .pin = 13 };
 
+static pbdrv_bluetooth_peripheral_t peripheral_singleton;
+
+pbdrv_bluetooth_peripheral_t *pbdrv_bluetooth_peripheral_get_by_index(uint8_t index) {
+    // This platform supports only a single peripheral instance. Some of its
+    // states are global variables listed above. This single instance is used
+    // troughout the event handler.
+    return &peripheral_singleton;
+}
+
+bool pbdrv_bluetooth_peripheral_is_connected(pbdrv_bluetooth_peripheral_t *peri) {
+    return peri == &peripheral_singleton && peri->con_handle != 0;
+}
+
 /**
  * Converts a BlueNRG-MS error code to a PBIO error code.
  * @param [in]  status  The BlueNRG-MS error code.
@@ -307,10 +320,6 @@ bool pbdrv_bluetooth_is_connected(pbdrv_bluetooth_connection_t connection) {
         return true;
     }
 
-    if (connection == PBDRV_BLUETOOTH_CONNECTION_PERIPHERAL && peripheral_singleton.con_handle) {
-        return true;
-    }
-
     return false;
 }
 
@@ -950,7 +959,6 @@ static void handle_event(hci_event_pckt *event) {
 
                 case EVT_BLUE_GATT_NOTIFICATION: {
                     evt_gatt_attr_notification *subevt = (void *)evt->data;
-                    pbdrv_bluetooth_peripheral_t *peri = &peripheral_singleton;
                     if (peri->config->notification_handler) {
                         peri->config->notification_handler(peri->user, subevt->attr_value, subevt->event_data_length - 2);
                     }