1 files changed, 37 insertions, 3 deletions
diff --git a/software/mpu/main.c b/software/mpu/main.c
index 461c071..a737d9b 100644
--- a/software/mpu/main.c
+++ b/software/mpu/main.c
@@ -36,6 +36,17 @@
 #include "heartbeat.h"
 #include "usb-cdc-shell.h"
 
+static void reboot(void)
+{
+  NVIC_SystemReset();
+}
+
+static void reset2bootloader(void)
+{
+  NVIC_SystemReset();
+      /* TODO: not just reset but go to bootloader */
+}
+
 /*
  * Application entry point.
  */
@@ -52,8 +63,31 @@ int main(void)
   chSysInit();
 
   heartbeatInit();
-  usbCDCShellInit();
-  while (TRUE) {
-    usbCDCShellRun();
+  int8_t ret = usbCDCShellRun();
+
+  /* the debug shell returned - we have to halt, reboot or goto bootloader */
+
+  /*  Safely stop critical threads */
+     /* nothing here. */
+
+  /*  Invoke the xxxStop() method on all the active device drivers */
+  chSysDisable();
+
+  /* Stop the system timer whose service routine invokes chSysTimerHandlerI(). */
+  SysTick->CTRL &= ~(SysTick_CTRL_ENABLE_Msk);
+
+  /* Disable any other interrupt source that may invoke OS APIs */
+     /* nothing here. */
+
+  /* Perform any application related de-initialization. */
+     /* nothing here. */
+
+  chSysEnable();
+  /* the OS is stopped now */
+
+  switch(ret) {
+    case MPU_BOOTLOADER: reset2bootloader(); break;
+    case MPU_REBOOT: reboot(); break;
   }
+  for(;;); /* halting system, aka loop endlessly */
 }