From 0350992141676a3f0e404f28c6deacb5da97e6bd Mon Sep 17 00:00:00 2001 From: Clyne Sullivan Date: Fri, 26 Mar 2021 09:23:22 -0400 Subject: [PATCH] algorithm fault recovery --- source/main.cpp | 68 ++++++++++++++++++++++++++++--------------------- 1 file changed, 39 insertions(+), 29 deletions(-) diff --git a/source/main.cpp b/source/main.cpp index 0ec69b8..6cd7091 100644 --- a/source/main.cpp +++ b/source/main.cpp @@ -446,18 +446,25 @@ void conversion_unprivileged_main() if (elf_entry) { if (!MSG_FOR_MEASURE(message)) { + // Remember the stack pointer in case the algorithm messes things up. + uint32_t sp; + asm("mov %0, sp" : "=r" (sp)); samples = elf_entry(samples, size); + asm("mov sp, %0" :: "r" (sp)); } else { - asm("eor r0, r0; svc 2"); // start measurement + uint32_t sp; + asm("mov %0, sp; eor r0, r0; svc 2" : "=r" (sp)); // start measurement samples = elf_entry(samples, size); - asm("mov r0, #1; svc 2"); // stop measurement + asm("mov r0, #1; svc 2; mov sp, %0" :: "r" (sp)); // stop measurement } } - if (MSG_FOR_FIRST(message)) - samplesOut.modify(samples, size); - else - samplesOut.midmodify(samples, size); + if (samples != nullptr) { + if (MSG_FOR_FIRST(message)) + samplesOut.modify(samples, size); + else + samplesOut.midmodify(samples, size); + } } } } @@ -509,12 +516,8 @@ void mpu_setup() void conversion_abort() { elf_entry = nullptr; - DAC::stop(0); - ADC::stop(); EM.add(Error::ConversionAborted); - - chMBReset(&conversionMB); - run_status = RunStatus::Idle; + run_status = RunStatus::Recovering; } void signal_operate(adcsample_t *buffer, size_t) @@ -524,6 +527,7 @@ void signal_operate(adcsample_t *buffer, size_t) if (chMBGetUsedCountI(&conversionMB) > 1) { chSysUnlockFromISR(); conversion_abort(); + chMBReset(&conversionMB); } else { if (buffer == samplesIn.data()) { samplesIn.setModified(); @@ -622,16 +626,31 @@ void port_syscall(struct port_extctx *ctxp, uint32_t n) __attribute__((naked)) void MemManage_Handler() { - while (1); + // 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)); + + // 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. + + // 3. Return. + asm("mov lr, %0; bx lr" :: "r" (lr)); } __attribute__((naked)) void HardFault_Handler() { - // Below not working (yet) - while (1); - - // 1. Get the stack pointer + // Get the stack pointer. uint32_t *stack; uint32_t lr; asm("\ @@ -642,20 +661,11 @@ void HardFault_Handler() mov %1, lr; \ " : "=r" (stack), "=r" (lr)); - // 2. Only attempt to recover from failed algorithm code - if ((lr & 4) == 0 || run_status != RunStatus::Running) - while (1); + // If coming from the algorithm, attempt to recover; otherwise, give up. + if (run_status != RunStatus::Running && (lr & 4) != 0) + MemManage_Handler(); - // 3. Post the failure and unload algorithm - elf_entry = nullptr; - EM.add(Error::ConversionAborted); - run_status = RunStatus::Recovering; - - // 4. Make this exception return to point after algorithm exec. - stack[6] = stack[5]; - stack[7] |= (1 << 24); // Ensure Thumb mode stays enabled - - asm("mov lr, %0; bx lr" :: "r" (lr)); + while (1); } } // extern "C"