pbio/drv: Add deinit exit point.

Allows graceful exit of selected drivers. As an example, stop the IMU driver loop.

Fixes pybricks/support#2253

Author: laurensvalk

lib/pbio/drv/core.c

@@ -90,3 +90,16 @@ void pbdrv_init(void) {
     #endif
     pbdrv_ioport_enable_vcc(true);
 }
+
+/**
+ * Deinitializes selected drivers that are not needed after soft-shutdown.
+ */
+void pbdrv_deinit(void) {
+
+    pbdrv_imu_deinit();
+
+    while (pbdrv_init_busy()) {
+        pbio_os_run_processes_once();
+    }
+
+}

lib/pbio/drv/imu/imu.h

@@ -11,10 +11,12 @@
 #include <pbdrv/imu.h>
 
 void pbdrv_imu_init(void);
+void pbdrv_imu_deinit(void);
 
 #else // PBDRV_CONFIG_IMU
 
 #define pbdrv_imu_init()
+#define pbdrv_imu_deinit()
 
 #endif // PBDRV_CONFIG_IMU
 

lib/pbio/drv/imu/imu_lsm6ds3tr_c_stm32.c

@@ -367,7 +367,8 @@ static pbio_error_t pbdrv_imu_lsm6ds3tr_c_stm32_process_thread(pbio_os_state_t *
     // value each time we want to read new data. This saves CPU usage since
     // we have fewer interrupts per sample.
 
-    for (;;) {
+    while (!(pbdrv_imu_lsm6ds3tr_c_stm32_process.request & PBIO_OS_PROCESS_REQUEST_TYPE_CANCEL)) {
+
         PBIO_OS_AWAIT_UNTIL(state, atomic_exchange(&imu_dev->int1, false));
 
         imu_dev->ctx.read_write_done = false;
@@ -403,8 +404,10 @@ static pbio_error_t pbdrv_imu_lsm6ds3tr_c_stm32_process_thread(pbio_os_state_t *
         }
     }
 
-    // Unreachable
-    PBIO_OS_ASYNC_END(PBIO_ERROR_FAILED);
+    // Cancellation complete.
+    pbdrv_imu_lsm6ds3tr_c_stm32_i2c_reset(hi2c);
+    pbdrv_init_busy_down();
+    PBIO_OS_ASYNC_END(PBIO_ERROR_CANCELED);
 }
 
 // internal driver interface implementation
@@ -414,6 +417,11 @@ void pbdrv_imu_init(void) {
     pbio_os_process_start(&pbdrv_imu_lsm6ds3tr_c_stm32_process, pbdrv_imu_lsm6ds3tr_c_stm32_process_thread, NULL);
 }
 
+void pbdrv_imu_deinit(void) {
+    pbdrv_init_busy_up();
+    pbio_os_process_make_request(&pbdrv_imu_lsm6ds3tr_c_stm32_process, PBIO_OS_PROCESS_REQUEST_TYPE_CANCEL);
+}
+
 // public driver interface implementation
 
 pbio_error_t pbdrv_imu_get_imu(pbdrv_imu_dev_t **imu_dev, pbdrv_imu_config_t **config) {

lib/pbio/include/pbdrv/core.h

@@ -13,6 +13,7 @@
 #define _PBDRV_CORE_H_
 
 void pbdrv_init(void);
+void pbdrv_deinit(void);
 
 #endif // _PBDRV_CORE_H_
 

lib/pbio/sys/main.c

@@ -7,6 +7,7 @@
 
 #include <stdint.h>
 
+#include <pbdrv/core.h>
 #include <pbdrv/reset.h>
 #include <pbdrv/usb.h>
 #include <pbio/main.h>
@@ -140,6 +141,7 @@ int main(int argc, char **argv) {
     pbsys_deinit();
 
     // Now lower-level processes may shutdown and/or power off.
+    pbdrv_deinit();
     pbsys_status_set(PBIO_PYBRICKS_STATUS_SHUTDOWN);
 
     // The power could be held on due to someone pressing the center button