better algorithm fault recovery

1 files changed, 34 insertions, 24 deletions

diff --git a/source/main.cpp b/source/main.cpp
index 6cd7091..fbadd23 100644
--- a/source/main.cpp
+++ b/source/main.cpp
@@ -105,7 +105,7 @@ __attribute__((section(".convcode")))
 static void conversion_unprivileged_main();
 
 static void mpu_setup();
-static void conversion_abort();
+static void abortAlgorithmFromISR();
 static void signal_operate(adcsample_t *buffer, size_t count);
 static void signal_operate_measure(adcsample_t *buffer, size_t count);
 
@@ -375,10 +375,6 @@ THD_FUNCTION(conversionThreadMonitor, arg)
 {
     (void)arg;
     while (1) {
-        // Recover from algorithm fault if necessary
-        //if (run_status == RunStatus::Recovering)
-        //    conversion_abort();
-
         msg_t message;
         if (chMBFetchTimeout(&conversionMB, &message, TIME_INFINITE) == MSG_OK)
             chMsgSend(conversionThreadHandle, message);
@@ -513,11 +509,33 @@ void mpu_setup()
 #endif
 }
 
-void conversion_abort()
+void abortAlgorithmFromISR()
 {
     elf_entry = nullptr;
     EM.add(Error::ConversionAborted);
     run_status = RunStatus::Recovering;
+
+    // Confirm that the exception return thread is the algorithm...
+    uint32_t *psp;
+	asm("mrs %0, psp" : "=r" (psp));
+    if ((uint32_t)psp >= (uint32_t)conversionThreadUPWA &&
+        (uint32_t)psp <= (uint32_t)conversionThreadUPWA + conversionThreadUPWASize)
+    {
+        // If it is, we can force the algorithm to exit by "resetting" its thread.
+        // We do this by rebuilding the thread's stacked exception return.
+        uint32_t *newpsp = reinterpret_cast<uint32_t *>(
+            (char *)conversionThreadUPWA +
+            conversionThreadUPWASize - 8 * sizeof(uint32_t));
+        // Set the LR register to the thread's entry point.
+        newpsp[5] = reinterpret_cast<uint32_t>(conversion_unprivileged_main);
+        // Overwrite the instruction we'll return to with "bx lr" (jump to address in LR).
+        newpsp[6] = psp[6];
+        *reinterpret_cast<uint16_t *>(newpsp[6]) = 0x4770; // "bx lr"
+        // Keep PSR contents (bit set forces Thumb mode, just in case).
+        newpsp[7] = psp[7] | (1 << 24);
+        // Set the new stack pointer.
+	    asm("msr psp, %0" :: "r" (newpsp));
+    }
 }
 
 void signal_operate(adcsample_t *buffer, size_t)
@@ -525,9 +543,10 @@ void signal_operate(adcsample_t *buffer, size_t)
     chSysLockFromISR();
 
     if (chMBGetUsedCountI(&conversionMB) > 1) {
+        chMBResetI(&conversionMB);
+        chMBResumeX(&conversionMB);
         chSysUnlockFromISR();
-        conversion_abort();
-        chMBReset(&conversionMB);
+        abortAlgorithmFromISR();
     } else {
         if (buffer == samplesIn.data()) {
             samplesIn.setModified();
@@ -627,21 +646,11 @@ __attribute__((naked))
 void MemManage_Handler()
 {
     // 1. Get the stack pointer.
-    uint32_t *stack;
     uint32_t lr;
-	asm("\
-		tst lr, #4; \
-		ite eq; \
-		mrseq %0, msp; \
-		mrsne %0, psp; \
-        mov %1, lr; \
-	" : "=r" (stack), "=r" (lr));
+	asm("mov %0, lr" : "=r" (lr));
 
-    // 2. Recover from the fault:
-    conversion_abort();             // Unload algorithm and indicate error.
-    stack[0] = 0;                   // Force algo. to return nullptr (DAC buffer will not update).
-    stack[6] = stack[5];            // Skip remainder of algo. code.
-    stack[7] |= (1 << 24);          // Ensure Thumb mode stays enabled.
+    // 2. Recover from the fault.
+    abortAlgorithmFromISR();
 
     // 3. Return.
     asm("mov lr, %0; bx lr" :: "r" (lr));
@@ -651,15 +660,16 @@ __attribute__((naked))
 void HardFault_Handler()
 {
     // Get the stack pointer.
-    uint32_t *stack;
+    //uint32_t *stack;
     uint32_t lr;
-	asm("\
+	asm("mov %0, lr" : "=r" (lr));
+	/*asm("\
 		tst lr, #4; \
 		ite eq; \
 		mrseq %0, msp; \
 		mrsne %0, psp; \
         mov %1, lr; \
-	" : "=r" (stack), "=r" (lr));
+	" : "=r" (stack), "=r" (lr));*/
 
     // If coming from the algorithm, attempt to recover; otherwise, give up.
     if (run_status != RunStatus::Running && (lr & 4) != 0)