pbio/drv/bluetooth: Shut down gracefully.

Before the Bluetooth overhaul, we were just powering down. In the recent commits, this was not yet implemented, so the hub would stay connected or keep advertising once turned off and charging.

Poweroff is now back, but we also gracefully disconnect first to be nice to Pybricks Code.

Author: laurensvalk

lib/pbio/drv/bluetooth/bluetooth.c

@@ -11,6 +11,7 @@
 
 #include <pbdrv/bluetooth.h>
 
+#include <pbio/busy_count.h>
 #include <pbio/error.h>
 #include <pbio/os.h>
 #include <pbio/protocol.h>
@@ -411,6 +412,8 @@ pbio_error_t pbdrv_bluetooth_await_advertise_or_scan_command(pbio_os_state_t *st
     return advertising_or_scan_func ? advertising_or_scan_err : PBIO_SUCCESS;
 }
 
+static bool shutting_down;
+
 /**
  * This is the main high level pbdrv/bluetooth thread. It is driven forward by
  * the platform-specific HCI process whenever there is new data to process or
@@ -434,7 +437,9 @@ pbio_error_t pbdrv_bluetooth_process_thread(pbio_os_state_t *state, void *contex
 
     PBIO_OS_ASYNC_BEGIN(state);
 
-    for (;;) {
+    while (!shutting_down) {
+
+        DEBUG_PRINT("Bluetooth disable requested.\n");
 
         pbdrv_bluetooth_is_broadcasting = false;
         pbdrv_bluetooth_is_observing = false;
@@ -447,7 +452,10 @@ pbio_error_t pbdrv_bluetooth_process_thread(pbio_os_state_t *state, void *contex
         pbio_os_request_poll();
 
         // Bluetooth is now disabled. Await system processes to ask for enable.
-        PBIO_OS_AWAIT_UNTIL(state, power_on_requested);
+        PBIO_OS_AWAIT_UNTIL(state, power_on_requested || shutting_down);
+        if (shutting_down) {
+            break;
+        }
         DEBUG_PRINT("Bluetooth enable requested.\n");
 
         PBIO_OS_AWAIT(state, &sub, err = pbdrv_bluetooth_controller_initialize(&sub, &timer));
@@ -463,9 +471,11 @@ pbio_error_t pbdrv_bluetooth_process_thread(pbio_os_state_t *state, void *contex
         pbio_os_timer_set(&status_timer, PBDRV_BLUETOOTH_STATUS_UPDATE_INTERVAL);
 
         // Service scheduled tasks as long as Bluetooth is enabled.
-        while (power_on_requested) {
+        while (power_on_requested && !shutting_down) {
 
-            // REVISIT: Only needed if there is nothing to do
+            // In principle, this wait is only needed if there is nothing to do.
+            // In practice, leaving it here helps rather than hurts since it
+            // allows short stdout messages to be queued rather than sent separately.
             PBIO_OS_AWAIT_MS(state, &timer, 1);
 
             // Handle pending status update, if any.
@@ -534,11 +544,26 @@ pbio_error_t pbdrv_bluetooth_process_thread(pbio_os_state_t *state, void *contex
                 observe_restart_requested = false;
             }
         }
-
-        DEBUG_PRINT("Bluetooth disable requested.\n");
     }
 
+    DEBUG_PRINT("Shutdown requested.\n");
+
+    // Power down the chip. This will disconnect from the host first.
+    // The peripheral has already been disconnected in the cleanup that runs after
+    // every program. If we change that behavior, we can do the disconnect here.
+
+    PBIO_OS_AWAIT(state, &sub, pbdrv_bluetooth_controller_reset(&sub, &timer));
+
+    pbio_busy_count_down();
+
     PBIO_OS_ASYNC_END(PBIO_SUCCESS);
 }
 
+void pbdrv_bluetooth_deinit(void) {
+    pbio_busy_count_up();
+    pbdrv_bluetooth_cancel_operation_request();
+    shutting_down = true;
+    pbio_os_request_poll();
+}
+
 #endif // PBDRV_CONFIG_BLUETOOTH_STM32_CC2640

lib/pbio/drv/bluetooth/bluetooth_btstack.c

@@ -858,7 +858,11 @@ pbio_error_t pbdrv_bluetooth_controller_reset(pbio_os_state_t *state, pbio_os_ti
 
     PBIO_OS_ASYNC_BEGIN(state);
 
-    // TODO: Reset state
+    // Disconnect gracefully if connected to host.
+    if (le_con_handle != HCI_CON_HANDLE_INVALID) {
+        gap_disconnect(le_con_handle);
+        PBIO_OS_AWAIT_UNTIL(state, le_con_handle == HCI_CON_HANDLE_INVALID);
+    }
 
     hci_power_control(HCI_POWER_OFF);
     PBIO_OS_AWAIT_UNTIL(state, hci_get_state() == HCI_STATE_OFF);

lib/pbio/drv/bluetooth/bluetooth_stm32_bluenrg.c

@@ -1240,6 +1240,15 @@ static pbio_error_t init_uart_service(pbio_os_state_t *state, void *context) {
 pbio_error_t pbdrv_bluetooth_controller_reset(pbio_os_state_t *state, pbio_os_timer_t *timer) {
     PBIO_OS_ASYNC_BEGIN(state);
 
+    // Disconnect gracefully if connected to host.
+    if (conn_handle) {
+        PBIO_OS_AWAIT_WHILE(state, write_xfer_size);
+        aci_gap_terminate_begin(conn_handle, HCI_OE_USER_ENDED_CONNECTION);
+        PBIO_OS_AWAIT_UNTIL(state, hci_command_status);
+        aci_gap_terminate_end();
+        PBIO_OS_AWAIT_UNTIL(state, conn_handle == 0);
+    }
+
     pybricks_notify_en = uart_tx_notify_en = false;
     conn_handle = peripheral_singleton.con_handle = 0;
 

lib/pbio/drv/bluetooth/bluetooth_stm32_cc2640.c

@@ -1840,6 +1840,13 @@ static pbio_error_t init_uart_service(pbio_os_state_t *state, void *context) {
 pbio_error_t pbdrv_bluetooth_controller_reset(pbio_os_state_t *state, pbio_os_timer_t *timer) {
     PBIO_OS_ASYNC_BEGIN(state);
 
+    // Disconnect gracefully if connected to host.
+    if (conn_handle != NO_CONNECTION) {
+        PBIO_OS_AWAIT_WHILE(state, write_xfer_size);
+        GAP_TerminateLinkReq(conn_handle, 0x13);
+        PBIO_OS_AWAIT_UNTIL(state, conn_handle == NO_CONNECTION);
+    }
+
     pybricks_notify_en = uart_tx_notify_en = false;
     conn_handle = peripheral_singleton.con_handle = NO_CONNECTION;
 

lib/pbio/drv/core.c

@@ -101,6 +101,7 @@ void pbdrv_init(void) {
 void pbdrv_deinit(void) {
 
     pbdrv_imu_deinit();
+    pbdrv_bluetooth_deinit();
 
     while (pbio_busy_count_busy()) {
         pbio_os_run_processes_once();

lib/pbio/include/pbdrv/bluetooth.h

@@ -232,6 +232,11 @@ typedef void (*pbdrv_bluetooth_start_observing_callback_t)(pbdrv_bluetooth_ad_ty
  */
 void pbdrv_bluetooth_init(void);
 
+/**
+ * Deinitializes the Bluetooth driver.
+ */
+void pbdrv_bluetooth_deinit(void);
+
 /**
  * Turns the Bluetooth chip power on or off. Await the operation.
  *
@@ -503,6 +508,9 @@ pbio_error_t pbdrv_bluetooth_await_advertise_or_scan_command(pbio_os_state_t *st
 static inline void pbdrv_bluetooth_init(void) {
 }
 
+static inline void pbdrv_bluetooth_deinit(void) {
+}
+
 static inline pbio_error_t pbdrv_bluetooth_power_on(pbio_os_state_t *state, bool on) {
     return PBIO_ERROR_NOT_SUPPORTED;
 }