Improve Bluetooth de-initialization

bricks/_common/micropython.c

@@ -314,7 +314,7 @@ static void run_user_program(void) {
             // The global scope is preserved to facilitate debugging, but we
             // stop active resources like motors and sounds. They are stopped
             // but not reset so the user can restart them in the REPL.
-            pbio_stop_all(false);
+            pbio_main_soft_stop();
 
             // Enter REPL.
             run_repl();
@@ -420,11 +420,9 @@ void pbsys_main_run_program(pbsys_main_program_t *program) {
             run_user_program();
             break;
     }
+}
 
-    // De-init bluetooth resources (including flushing stdout) that may use
-    // memory allocated by MicroPython before we wipe it.
-    pb_package_pybricks_deinit();
-
+void pbsys_main_run_program_cleanup(void) {
     gc_sweep_all();
     mp_deinit();
 }

bricks/virtualhub/mp_port.c

@@ -86,8 +86,6 @@ void pb_virtualhub_port_init(void) {
 
 // MICROPY_PORT_DEINIT_FUNC
 void pb_virtualhub_port_deinit(void) {
-
-    pb_package_pybricks_deinit();
 }
 
 // Implementation for MICROPY_EVENT_POLL_HOOK

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>
@@ -296,7 +297,12 @@ pbio_error_t pbdrv_bluetooth_await_peripheral_command(pbio_os_state_t *state, vo
     // of interest and will be cancelled if the active function supports it.
     pbio_os_timer_set(&peri->watchdog, 10);
 
-    return peripheral_singleton.func ? peripheral_singleton.err : PBIO_SUCCESS;
+    return peri->func ? peri->err : PBIO_SUCCESS;
+}
+
+void pbdrv_bluetooth_cancel_operation_request(void) {
+    // Only some peripheral actions support cancellation.
+    peripheral_singleton.cancel = true;
 }
 
 //
@@ -406,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
@@ -429,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;
@@ -442,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));
@@ -458,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.
@@ -529,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.
  *
@@ -421,6 +426,15 @@ pbio_error_t pbdrv_bluetooth_peripheral_write_characteristic(uint16_t handle, co
  */
 pbio_error_t pbdrv_bluetooth_await_peripheral_command(pbio_os_state_t *state, void *context);
 
+/**
+ * Requests active Bluetooth tasks to be cancelled. It is up to the task
+ * implementation to respect or ignore it. The task should still be awaited
+ * with ::pbdrv_bluetooth_await_advertise_or_scan_command or
+ * with ::pbdrv_bluetooth_await_peripheral_command. Cancelling just allows
+ * some commands to exit earlier.
+ */
+void pbdrv_bluetooth_cancel_operation_request(void);
+
 //
 // Functions related to advertising and scanning.
 //
@@ -494,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;
 }
@@ -565,6 +582,9 @@ static inline pbio_error_t pbdrv_bluetooth_await_peripheral_command(pbio_os_stat
     return PBIO_ERROR_NOT_SUPPORTED;
 }
 
+static inline void pbdrv_bluetooth_cancel_operation_request(void) {
+}
+
 static inline pbio_error_t pbdrv_bluetooth_start_advertising(bool start) {
     return PBIO_ERROR_NOT_SUPPORTED;
 }

lib/pbio/include/pbio/main.h

@@ -10,6 +10,7 @@
 
 void pbio_init(bool start_processes);
 void pbio_deinit(void);
-void pbio_stop_all(bool reset);
+void pbio_main_stop_application_resources();
+void pbio_main_soft_stop(void);
 
 #endif // _PBIO_MAIN_H_

lib/pbio/include/pbsys/main.h

@@ -110,6 +110,15 @@ pbio_error_t pbsys_main_program_validate(pbsys_main_program_t *program);
  */
 void pbsys_main_run_program(pbsys_main_program_t *program);
 
+/**
+ * Cleans up after running main application program, such as wiping application
+ * heap.
+ *
+ * This is separate from ::pbsys_main_run_program so that the system can
+ * safely close resources and unset callbacks before this cleanup runs.
+ */
+void pbsys_main_run_program_cleanup(void);
+
 /**
  * Stops (cancels) the main application program.
  *

lib/pbio/src/main.c

@@ -8,9 +8,12 @@
 
 #include <stdbool.h>
 
+#include <pbdrv/bluetooth.h>
+#include <pbdrv/display.h>
 #include <pbdrv/sound.h>
 
 #include <pbio/battery.h>
+#include <pbio/image.h>
 #include <pbio/imu.h>
 #include <pbio/motor_process.h>
 #include <pbio/port_interface.h>
@@ -51,26 +54,62 @@ void pbio_deinit(void) {
 }
 
 /**
- * Stops all user-level background processes. Called when the user application
- * completes to get these modules back into their default state. Drivers and
- * OS-level processes continue running.
+ * Stops resources like motors or sounds or peripheral procedures that take a
+ * long time.
  *
- * @param [in]  reset  Whether to reset all user-level processes to a clean
- *                     state (true), or whether to only stop active outputs
- *                     like sound or motors (false). The latter is useful to
- *                     preserve the state for debugging, without sound or
- *                     movement getting in the way, or out of control.
+ * Useful to get the system in a safe state for the user without doing a full
+ * reset. Applications can all this to enter a user debug mode like the
+ * MicroPython REPL.
  */
-void pbio_stop_all(bool reset) {
-    #if PBIO_CONFIG_LIGHT
-    if (reset) {
-        pbio_light_animation_stop_all();
-    }
-    #endif
+void pbio_main_soft_stop(void) {
 
-    pbio_port_stop_user_actions(reset);
+    pbio_port_stop_user_actions(false);
 
     pbdrv_sound_stop();
+
+    pbdrv_bluetooth_cancel_operation_request();
+}
+
+static void wait_for_bluetooth(void) {
+    pbio_os_state_t unused;
+    while (pbdrv_bluetooth_await_advertise_or_scan_command(&unused, NULL) == PBIO_ERROR_AGAIN ||
+           pbdrv_bluetooth_await_peripheral_command(&unused, NULL) == PBIO_ERROR_AGAIN) {
+
+        // Run event loop until Bluetooth is idle.
+        pbio_os_run_processes_and_wait_for_event();
+    }
+}
+
+/**
+ * Stops all application-level background processes. Called when the user
+ * application completes to get these modules back into their default state.
+ * Drivers and OS-level processes continue running.
+ */
+void pbio_main_stop_application_resources() {
+
+    pbio_main_soft_stop();
+
+    // Let ongoing task finish first.
+    wait_for_bluetooth();
+
+    // Stop broadcasting, observing and disconnect peripheral.
+    pbdrv_bluetooth_start_broadcasting(NULL, 0);
+    wait_for_bluetooth();
+
+    pbdrv_bluetooth_start_observing(NULL);
+    wait_for_bluetooth();
+
+    pbdrv_bluetooth_peripheral_disconnect();
+    wait_for_bluetooth();
+
+    #if PBIO_CONFIG_LIGHT
+    pbio_light_animation_stop_all();
+    #endif
+
+    #if PBDRV_CONFIG_DISPLAY
+    pbio_image_fill(pbdrv_display_get_image(), 0);
+    pbdrv_display_update();
+    #endif
 }
 
 /** @} */

lib/pbio/sys/light_matrix.c

@@ -153,28 +153,21 @@ static uint32_t pbsys_hub_light_matrix_user_program_animation_next(pbio_light_an
 }
 
 /**
- * Updates light matrix behavior when program is started or stopped.
- *
- * @param start   @c true for start or @c false for stop.
+ * Updates light matrix behavior when program is started.
  */
-void pbsys_hub_light_matrix_handle_user_program_start(bool start) {
+void pbsys_hub_light_matrix_handle_user_program_start(void) {
 
     #if PBSYS_CONFIG_HUB_LIGHT_MATRIX_DISPLAY
     pbio_image_fill(pbdrv_display_get_image(), 0);
     pbdrv_display_update();
     return;
     #endif
 
-    if (start) {
-        // The user animation updates only a subset of pixels to save time,
-        // so the rest must be cleared before it starts.
-        pbsys_hub_light_matrix_user_program_animation_clear();
-        pbio_light_animation_init(&pbsys_hub_light_matrix->animation, pbsys_hub_light_matrix_user_program_animation_next);
-        pbio_light_animation_start(&pbsys_hub_light_matrix->animation);
-    } else {
-        // If the user program has ended, show stop sign and selected slot.
-        pbsys_hub_light_matrix_show_idle_ui(100);
-    }
+    // The user animation updates only a subset of pixels to save time,
+    // so the rest must be cleared before it starts.
+    pbsys_hub_light_matrix_user_program_animation_clear();
+    pbio_light_animation_init(&pbsys_hub_light_matrix->animation, pbsys_hub_light_matrix_user_program_animation_next);
+    pbio_light_animation_start(&pbsys_hub_light_matrix->animation);
 }
 
 #endif // PBSYS_CONFIG_HUB_LIGHT_MATRIX

lib/pbio/sys/light_matrix.h

@@ -11,12 +11,12 @@
 #if PBSYS_CONFIG_HUB_LIGHT_MATRIX
 void pbsys_hub_light_matrix_init(void);
 void pbsys_hub_light_matrix_deinit(void);
-void pbsys_hub_light_matrix_handle_user_program_start(bool start);
+void pbsys_hub_light_matrix_handle_user_program_start(void);
 void pbsys_hub_light_matrix_update_program_slot(void);
 #else
 #define pbsys_hub_light_matrix_init()
 #define pbsys_hub_light_matrix_deinit()
-#define pbsys_hub_light_matrix_handle_user_program_start(start)
+#define pbsys_hub_light_matrix_handle_user_program_start()
 #define pbsys_hub_light_matrix_update_program_slot()
 #endif