clyne/stmdsp
1
0
Fork 0
Code Issues Pull Requests Projects Releases 1 Wiki Activity

better algorithm fault recovery

Browse Source
This commit is contained in:
Clyne 2021-03-28 20:28:04 -04:00
parent 0350992141
commit 20eb697de8
1 changed files with 34 additions and 24 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

58
source/main.cpp
View File

@ -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)