merge in branch devel

1 files changed, 16 insertions, 655 deletions

diff --git a/source/main.cpp b/source/main.cpp
index e151a3d..9a22a73 100644
--- a/source/main.cpp
+++ b/source/main.cpp
@@ -2,7 +2,7 @@
  * @file main.cpp
  * @brief Program entry point.
  *
- * Copyright (C) 2020 Clyne Sullivan
+ * Copyright (C) 2021 Clyne Sullivan
  *
  * Distributed under the GNU GPL v3 or later. You should have received a copy of
  * the GNU General Public License along with this program.
@@ -12,122 +12,32 @@
 #include "ch.h"
 #include "hal.h"
 
-static_assert(sizeof(adcsample_t) == sizeof(uint16_t));
-static_assert(sizeof(dacsample_t) == sizeof(uint16_t));
-
 #include "adc.hpp"
 #include "cordic.hpp"
 #include "dac.hpp"
-#include "elf_load.hpp"
 #include "error.hpp"
-#include "samplebuffer.hpp"
 #include "sclock.hpp"
 #include "usbserial.hpp"
 
-#include <array>
-
-constexpr unsigned int MAX_ELF_FILE_SIZE = 16 * 1024;
-
-enum class RunStatus : char
-{
-    Idle = '1',
-    Running,
-    Recovering
-};
-static RunStatus run_status = RunStatus::Idle;
-
-#define MSG_CONVFIRST          (1)
-#define MSG_CONVSECOND         (2)
-#define MSG_CONVFIRST_MEASURE  (3)
-#define MSG_CONVSECOND_MEASURE (4)
-
-#define MSG_FOR_FIRST(m)   (m & 1)
-#define MSG_FOR_MEASURE(m) (m > 2)
-
-static ErrorManager EM;
-
-static msg_t conversionMBBuffer[2];
-static MAILBOX_DECL(conversionMB, conversionMBBuffer, 2);
-
-// Thread for LED status and wakeup hold
-__attribute__((section(".stacks")))
-static THD_WORKING_AREA(monitorThreadWA, 256);
-static THD_FUNCTION(monitorThread, arg);
-
-// Thread for managing the conversion task
-__attribute__((section(".stacks")))
-static THD_WORKING_AREA(conversionThreadMonitorWA, 1024);
-static THD_FUNCTION(conversionThreadMonitor, arg);
-static thread_t *conversionThreadHandle = nullptr;
-
-// Thread for unprivileged algorithm execution
-__attribute__((section(".stacks")))
-static THD_WORKING_AREA(conversionThreadWA, 128); // All we do is enter unprivileged mode.
-static THD_FUNCTION(conversionThread, arg);
-constexpr unsigned int conversionThreadUPWASize = 
-#if defined(TARGET_PLATFORM_H7)
-                                                  62 * 1024;
-#else
-                                                  15 * 1024;
-#endif
-__attribute__((section(".convdata")))
-static THD_WORKING_AREA(conversionThreadUPWA, conversionThreadUPWASize);
-__attribute__((section(".convdata")))
-static thread_t *conversionThreadMonitorHandle = nullptr;
-
-// Thread for USB monitoring
-__attribute__((section(".stacks")))
-static THD_WORKING_AREA(communicationThreadWA, 4096);
-static THD_FUNCTION(communicationThread, arg);
+#include "runstatus.hpp"
+RunStatus run_status = RunStatus::Idle;
 
-static time_measurement_t conversion_time_measurement;
-#if defined(TARGET_PLATFORM_H7)
-__attribute__((section(".convdata")))
-static SampleBuffer samplesIn  (reinterpret_cast<Sample *>(0x38000000)); // 16k
-__attribute__((section(".convdata")))
-static SampleBuffer samplesOut (reinterpret_cast<Sample *>(0x30004000)); // 16k
-static SampleBuffer samplesSigGen (reinterpret_cast<Sample *>(0x30000000)); // 16k
-#else
-__attribute__((section(".convdata")))
-static SampleBuffer samplesIn  (reinterpret_cast<Sample *>(0x20008000)); // 16k
-__attribute__((section(".convdata")))
-static SampleBuffer samplesOut (reinterpret_cast<Sample *>(0x2000C000)); // 16k
-static SampleBuffer samplesSigGen (reinterpret_cast<Sample *>(0x20010000)); // 16k
-#endif
+// Other variables
+//
+//static char userMessageBuffer[128];
+//static unsigned char userMessageSize = 0;
 
-static unsigned char elf_file_store[MAX_ELF_FILE_SIZE];
-__attribute__((section(".convdata")))
-static ELF::Entry elf_entry = nullptr;
-
-__attribute__((section(".convcode")))
-static void conversion_unprivileged_main();
-
-static void mpu_setup();
-static void abortAlgorithmFromISR();
-static void signal_operate(adcsample_t *buffer, size_t count);
-static void signal_operate_measure(adcsample_t *buffer, size_t count);
-
-#if defined(TARGET_PLATFORM_L4)
-constexpr auto LINE_LED_GREEN = PAL_LINE(GPIOC_BASE, 10U);
-constexpr auto LINE_LED_YELLOW = PAL_LINE(GPIOC_BASE, 11U);
-constexpr auto LINE_LED_RED = PAL_LINE(GPIOC_BASE, 12U);
-#endif
+#include "conversion.hpp"
+#include "communication.hpp"
+#include "monitor.hpp"
 
 int main()
 {
-    // Initialize the RTOS
+    // Initialize ChibiOS
     halInit();
     chSysInit();
 
-    SCB->CPACR |= 0xF << 20; // Enable FPU
-    mpu_setup();
-
-#if defined(TARGET_PLATFORM_L4)
-    palSetLineMode(LINE_LED_GREEN, PAL_MODE_OUTPUT_PUSHPULL);
-    palSetLineMode(LINE_LED_YELLOW, PAL_MODE_OUTPUT_PUSHPULL);
-    palSetLineMode(LINE_LED_RED, PAL_MODE_OUTPUT_PUSHPULL);
-#endif
-
+    // Init peripherials
     ADC::begin();
     DAC::begin();
     SClock::begin();
@@ -137,561 +47,12 @@ int main()
     SClock::setRate(SClock::Rate::R32K);
     ADC::setRate(SClock::Rate::R32K);
 
-    chTMObjectInit(&conversion_time_measurement);
-    chThdCreateStatic(
-        monitorThreadWA, sizeof(monitorThreadWA),
-        LOWPRIO,
-        monitorThread, nullptr);
-    conversionThreadMonitorHandle = chThdCreateStatic(
-        conversionThreadMonitorWA, sizeof(conversionThreadMonitorWA),
-        NORMALPRIO + 1,
-        conversionThreadMonitor, nullptr);
-    conversionThreadHandle = chThdCreateStatic(
-        conversionThreadWA, sizeof(conversionThreadWA),
-        HIGHPRIO,
-        conversionThread,
-        reinterpret_cast<void *>(reinterpret_cast<uint32_t>(conversionThreadUPWA) +
-                                 conversionThreadUPWASize));
-    chThdCreateStatic(
-        communicationThreadWA, sizeof(communicationThreadWA),
-        NORMALPRIO,
-        communicationThread, nullptr);
+    // Start our threads.
+    ConversionManager::begin();
+    CommunicationManager::begin();
+    Monitor::begin();
 
     chThdExit(0);
     return 0;
 }
 
-THD_FUNCTION(communicationThread, arg)
-{
-    (void)arg;
-	while (1) {
-        if (USBSerial::isActive()) {
-            // Attempt to receive a command packet
-            if (unsigned char cmd[3]; USBSerial::read(&cmd[0], 1) > 0) {
-                // Packet received, first byte represents the desired command/action
-                switch (cmd[0]) {
-
-                // 'a' - Read contents of ADC buffer.
-                // 'A' - Write contents of ADC buffer.
-                // 'B' - Set ADC/DAC buffer size.
-                // 'd' - Read contents of DAC buffer.
-                // 'D' - Set siggen size and write to its buffer.
-                // 'E' - Load algorithm binary.
-                // 'e' - Unload algorithm.
-                // 'i' - Read "stmdsp" identifier string.
-                // 'I' - Read status information.
-                // 'M' - Begin conversion, measure algorithm execution time.
-                // 'm' - Read last algorithm execution time.
-                // 'R' - Begin conversion.
-                // 'r' - Read or write sample rate.
-                // 'S' - Stop conversion.
-                // 's' - Get latest block of conversion results.
-                // 't' - Get latest block of conversion input.
-                // 'W' - Start signal generator (siggen).
-                // 'w' - Stop siggen.
-
-                case 'a':
-                    USBSerial::write(samplesIn.bytedata(), samplesIn.bytesize());
-                    break;
-                case 'A':
-                    USBSerial::read(samplesIn.bytedata(), samplesIn.bytesize());
-                    break;
-
-                case 'B':
-                    if (EM.assert(run_status == RunStatus::Idle, Error::NotIdle) &&
-                        EM.assert(USBSerial::read(&cmd[1], 2) == 2, Error::BadParamSize))
-                    {
-                        // count is multiplied by two since this command receives size of buffer
-                        // for each algorithm application.
-                        unsigned int count = (cmd[1] | (cmd[2] << 8)) * 2;
-                        if (EM.assert(count <= MAX_SAMPLE_BUFFER_SIZE, Error::BadParam)) {
-                            samplesIn.setSize(count);
-                            samplesOut.setSize(count);
-                        }
-                    }
-                    break;
-
-                case 'd':
-                    USBSerial::write(samplesOut.bytedata(), samplesOut.bytesize());
-                    break;
-                case 'D':
-                    if (EM.assert(USBSerial::read(&cmd[1], 2) == 2, Error::BadParamSize)) {
-                        unsigned int count = cmd[1] | (cmd[2] << 8);
-                        if (EM.assert(count <= MAX_SAMPLE_BUFFER_SIZE, Error::BadParam)) {
-                            samplesSigGen.setSize(count);
-                            USBSerial::read(samplesSigGen.bytedata(), samplesSigGen.bytesize());
-                        }
-                    }
-                    break;
-
-                // 'E' - Reads in and loads the compiled conversion code binary from USB.
-                case 'E':
-                    if (EM.assert(run_status == RunStatus::Idle, Error::NotIdle) &&
-                        EM.assert(USBSerial::read(&cmd[1], 2) == 2, Error::BadParamSize))
-                    {
-                        // Only load the binary if it can fit in the memory reserved for it.
-                        unsigned int size = cmd[1] | (cmd[2] << 8);
-                        if (EM.assert(size < sizeof(elf_file_store), Error::BadUserCodeSize)) {
-                            USBSerial::read(elf_file_store, size);
-                            elf_entry = ELF::load(elf_file_store);
-
-                            EM.assert(elf_entry != nullptr, Error::BadUserCodeLoad);
-                        }
-                    }
-                    break;
-
-                // 'e' - Unloads the currently loaded conversion code
-                case 'e':
-                    elf_entry = nullptr;
-                    break;
-
-                // 'i' - Sends an identifying string to confirm that this is the stmdsp device.
-                case 'i':
-#if defined(TARGET_PLATFORM_H7)
-                    USBSerial::write(reinterpret_cast<const uint8_t *>("stmdsph"), 7);
-#else
-                    USBSerial::write(reinterpret_cast<const uint8_t *>("stmdspl"), 7);
-#endif
-                    break;
-
-                // 'I' - Sends the current run status.
-                case 'I':
-                    {
-                        unsigned char buf[2] = {
-                            static_cast<unsigned char>(run_status),
-                            static_cast<unsigned char>(EM.pop())
-                        };
-                        USBSerial::write(buf, sizeof(buf));
-                    }
-                    break;
-
-                // 'M' - Begins continuous sampling, but measures the execution time of the first
-                //       sample processing. This duration can be later read through 'm'.
-                case 'M':
-                    if (EM.assert(run_status == RunStatus::Idle, Error::NotIdle)) {
-                        run_status = RunStatus::Running;
-                        samplesOut.clear();
-                        ADC::start(samplesIn.data(), samplesIn.size(), signal_operate_measure);
-                        DAC::start(0, samplesOut.data(), samplesOut.size());
-                    }
-                    break;
-
-                // 'm' - Returns the last measured sample processing time, presumably in processor
-                //       ticks.
-                case 'm':
-                    USBSerial::write(reinterpret_cast<uint8_t *>(&conversion_time_measurement.last),
-                                     sizeof(rtcnt_t));
-                    break;
-
-                // 'R' - Begin continuous sampling/conversion of the ADC. Samples will go through
-                //       the conversion code, and will be sent out over the DAC.
-                case 'R':
-                    if (EM.assert(run_status == RunStatus::Idle, Error::NotIdle)) {
-                        run_status = RunStatus::Running;
-                        samplesOut.clear();
-                        ADC::start(samplesIn.data(), samplesIn.size(), signal_operate);
-                        DAC::start(0, samplesOut.data(), samplesOut.size());
-                    }
-                    break;
-
-                case 'r':
-                    if (EM.assert(USBSerial::read(&cmd[1], 1) == 1, Error::BadParamSize)) {
-                        if (cmd[1] == 0xFF) {
-                            unsigned char r = SClock::getRate();
-                            USBSerial::write(&r, 1);
-                        } else {
-                            auto r = static_cast<SClock::Rate>(cmd[1]);
-                            SClock::setRate(r);
-                            ADC::setRate(r);
-                        }
-                    }
-                    break;
-
-                // 'S' - Stops the continuous sampling/conversion.
-                case 'S':
-                    if (run_status == RunStatus::Running) {
-                        DAC::stop(0);
-                        ADC::stop();
-                        run_status = RunStatus::Idle;
-                    }
-                    break;
-
-                case 's':
-                    if (auto samps = samplesOut.modified(); samps != nullptr) {
-                        unsigned char buf[2] = {
-                            static_cast<unsigned char>(samplesOut.size() / 2 & 0xFF),
-                            static_cast<unsigned char>(((samplesOut.size() / 2) >> 8) & 0xFF)
-                        };
-                        USBSerial::write(buf, 2);
-                        unsigned int total = samplesOut.bytesize() / 2;
-                        unsigned int offset = 0;
-                        unsigned char unused;
-                        while (total > 512) {
-                            USBSerial::write(reinterpret_cast<uint8_t *>(samps) + offset, 512);
-                            while (USBSerial::read(&unused, 1) == 0);
-                            offset += 512;
-                            total -= 512;
-                        }
-                        USBSerial::write(reinterpret_cast<uint8_t *>(samps) + offset, total);
-                        while (USBSerial::read(&unused, 1) == 0);
-                    } else {
-                        USBSerial::write(reinterpret_cast<const uint8_t *>("\0\0"), 2);
-                    }
-                    break;
-                case 't':
-                    if (auto samps = samplesIn.modified(); samps != nullptr) {
-                        unsigned char buf[2] = {
-                            static_cast<unsigned char>(samplesIn.size() / 2 & 0xFF),
-                            static_cast<unsigned char>(((samplesIn.size() / 2) >> 8) & 0xFF)
-                        };
-                        USBSerial::write(buf, 2);
-                        unsigned int total = samplesIn.bytesize() / 2;
-                        unsigned int offset = 0;
-                        unsigned char unused;
-                        while (total > 512) {
-                            USBSerial::write(reinterpret_cast<uint8_t *>(samps) + offset, 512);
-                            while (USBSerial::read(&unused, 1) == 0);
-                            offset += 512;
-                            total -= 512;
-                        }
-                        USBSerial::write(reinterpret_cast<uint8_t *>(samps) + offset, total);
-                        while (USBSerial::read(&unused, 1) == 0);
-                    } else {
-                        USBSerial::write(reinterpret_cast<const uint8_t *>("\0\0"), 2);
-                    }
-                    break;
-
-                case 'W':
-                    DAC::start(1, samplesSigGen.data(), samplesSigGen.size());
-                    break;
-                case 'w':
-                    DAC::stop(1);
-                    break;
-
-                default:
-                    break;
-                }
-            }
-        }
-
-		chThdSleepMicroseconds(100);
-	}
-}
-
-THD_FUNCTION(conversionThreadMonitor, arg)
-{
-    (void)arg;
-    while (1) {
-        msg_t message;
-        if (chMBFetchTimeout(&conversionMB, &message, TIME_INFINITE) == MSG_OK)
-            chMsgSend(conversionThreadHandle, message);
-    }
-}
-
-THD_FUNCTION(conversionThread, stack)
-{
-    elf_entry = nullptr;
-    port_unprivileged_jump(reinterpret_cast<uint32_t>(conversion_unprivileged_main),
-                           reinterpret_cast<uint32_t>(stack));
-}
-
-THD_FUNCTION(monitorThread, arg)
-{
-    (void)arg;
-
-    palSetLineMode(LINE_BUTTON, PAL_MODE_INPUT_PULLUP);
-    auto readButton = [] {
-#if defined(TARGET_PLATFORM_L4)
-            return !palReadLine(LINE_BUTTON);
-#else
-            return palReadLine(LINE_BUTTON);
-#endif
-        };
-
-    palClearLine(LINE_LED_RED);
-    palClearLine(LINE_LED_YELLOW);
-
-    while (1) {
-        bool isidle = run_status == RunStatus::Idle;
-        auto led = isidle ? LINE_LED_GREEN : LINE_LED_YELLOW;
-        auto delay = isidle ? 500 : 250;
-
-        palSetLine(led);
-        chThdSleepMilliseconds(delay);
-        palClearLine(led);
-        chThdSleepMilliseconds(delay);
-
-        if (run_status == RunStatus::Idle && readButton()) {
-            palSetLine(LINE_LED_RED);
-            palSetLine(LINE_LED_YELLOW);
-            chSysLock();
-            while (readButton())
-                asm("nop");
-            while (!readButton())
-                asm("nop");
-            chSysUnlock();
-            palClearLine(LINE_LED_RED);
-            palClearLine(LINE_LED_YELLOW);
-            chThdSleepMilliseconds(500);
-        }
-
-        static bool erroron = false;
-        if (auto err = EM.hasError(); err ^ erroron) {
-            erroron = err;
-            if (err)
-                palSetLine(LINE_LED_RED);
-            else
-                palClearLine(LINE_LED_RED);
-        }
-    }
-}
-
-void conversion_unprivileged_main()
-{
-    while (1) {
-        msg_t message;
-        asm("svc 0; mov %0, r0" : "=r" (message)); // sleep until next message
-        if (message != 0) {
-            auto samples = MSG_FOR_FIRST(message) ? samplesIn.data() : samplesIn.middata();
-            auto size = samplesIn.size() / 2;
-
-            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 {
-                    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; mov sp, %0" :: "r" (sp)); // stop measurement
-                } 
-            }
-
-            if (samples != nullptr) {
-                if (MSG_FOR_FIRST(message))
-                    samplesOut.modify(samples, size);
-                else
-                    samplesOut.midmodify(samples, size);
-            }
-        }
-    }
-}
-
-void mpu_setup()
-{
-    // Set up MPU for user algorithm
-#if defined(TARGET_PLATFORM_H7)
-    // Region 2: Data for algorithm thread
-    // Region 3: Code for algorithm thread
-    // Region 4: User algorithm code
-    mpuConfigureRegion(MPU_REGION_2,
-                       0x20000000,
-                       MPU_RASR_ATTR_AP_RW_RW | MPU_RASR_ATTR_NON_CACHEABLE |
-                       MPU_RASR_SIZE_64K |
-                       MPU_RASR_ENABLE);
-    mpuConfigureRegion(MPU_REGION_3,
-                       0x0807F800,
-                       MPU_RASR_ATTR_AP_RO_RO | MPU_RASR_ATTR_NON_CACHEABLE |
-                       MPU_RASR_SIZE_2K |
-                       MPU_RASR_ENABLE);
-    mpuConfigureRegion(MPU_REGION_4,
-                       0x00000000,
-                       MPU_RASR_ATTR_AP_RW_RW | MPU_RASR_ATTR_NON_CACHEABLE |
-                       MPU_RASR_SIZE_64K |
-                       MPU_RASR_ENABLE);
-#else
-    // Region 2: Data for algorithm thread and ADC/DAC buffers
-    // Region 3: Code for algorithm thread
-    // Region 4: User algorithm code
-    mpuConfigureRegion(MPU_REGION_2,
-                       0x20008000,
-                       MPU_RASR_ATTR_AP_RW_RW | MPU_RASR_ATTR_NON_CACHEABLE |
-                       MPU_RASR_SIZE_128K|
-                       MPU_RASR_ENABLE);
-    mpuConfigureRegion(MPU_REGION_3,
-                       0x0807F800,
-                       MPU_RASR_ATTR_AP_RO_RO | MPU_RASR_ATTR_NON_CACHEABLE |
-                       MPU_RASR_SIZE_2K |
-                       MPU_RASR_ENABLE);
-    mpuConfigureRegion(MPU_REGION_4,
-                       0x10000000,
-                       MPU_RASR_ATTR_AP_RW_RW | MPU_RASR_ATTR_NON_CACHEABLE |
-                       MPU_RASR_SIZE_32K |
-                       MPU_RASR_ENABLE);
-#endif
-}
-
-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)
-{
-    chSysLockFromISR();
-
-    if (chMBGetUsedCountI(&conversionMB) > 1) {
-        chMBResetI(&conversionMB);
-        chMBResumeX(&conversionMB);
-        chSysUnlockFromISR();
-        abortAlgorithmFromISR();
-    } else {
-        if (buffer == samplesIn.data()) {
-            samplesIn.setModified();
-            chMBPostI(&conversionMB, MSG_CONVFIRST);
-        } else {
-            samplesIn.setMidmodified();
-            chMBPostI(&conversionMB, MSG_CONVSECOND);
-        }
-        chSysUnlockFromISR();
-    }
-}
-
-void signal_operate_measure(adcsample_t *buffer, [[maybe_unused]] size_t count)
-{
-    chSysLockFromISR();
-    if (buffer == samplesIn.data()) {
-        samplesIn.setModified();
-        chMBPostI(&conversionMB, MSG_CONVFIRST_MEASURE);
-    } else {
-        samplesIn.setMidmodified();
-        chMBPostI(&conversionMB, MSG_CONVSECOND_MEASURE);
-    }
-    chSysUnlockFromISR();
-
-    ADC::setOperation(signal_operate);
-}
-
-extern "C" {
-
-__attribute__((naked))
-void port_syscall(struct port_extctx *ctxp, uint32_t n)
-{
-    switch (n) {
-    case 0:
-        {
-            chSysLock();
-            chMsgWaitS();
-            auto msg = chMsgGet(conversionThreadMonitorHandle);
-            chMsgReleaseS(conversionThreadMonitorHandle, MSG_OK);
-            chSysUnlock();
-            ctxp->r0 = msg;
-        }
-        break;
-    case 1:
-        {
-            using mathcall = void (*)();
-            static mathcall funcs[3] = {
-                reinterpret_cast<mathcall>(cordic::sin),
-                reinterpret_cast<mathcall>(cordic::cos),
-                reinterpret_cast<mathcall>(cordic::tan),
-            };
-#if defined(PLATFORM_H7)
-            asm("vmov.f64 d0, %0, %1" :: "r" (ctxp->r1), "r" (ctxp->r2));
-            if (ctxp->r0 < 3) {
-                funcs[ctxp->r0]();
-                asm("vmov.f64 %0, %1, d0" : "=r" (ctxp->r1), "=r" (ctxp->r2));
-            } else {
-                asm("eor r0, r0; vmov.f64 d0, r0, r0");
-            }
-#else
-            asm("vmov.f32 s0, %0" :: "r" (ctxp->r1));
-            if (ctxp->r0 < 3) {
-                funcs[ctxp->r0]();
-                asm("vmov.f32 %0, s0" : "=r" (ctxp->r1));
-            } else {
-                asm("eor r0, r0; vmov.f32 s0, r0");
-            }
-#endif
-        }
-        break;
-    case 2:
-        if (ctxp->r0 == 0) {
-            chTMStartMeasurementX(&conversion_time_measurement);
-        } else {
-            chTMStopMeasurementX(&conversion_time_measurement);
-            // Subtract measurement overhead from the result.
-            // Running an empty algorithm ("bx lr") takes 196 cycles as of 2/4/21.
-            // Only measures algorithm code time (loading args/storing result takes 9 cycles).
-            constexpr rtcnt_t measurement_overhead = 196 - 1;
-            if (conversion_time_measurement.last > measurement_overhead)
-                conversion_time_measurement.last -= measurement_overhead;
-        }
-        break;
-    case 3:
-        ctxp->r0 = ADC::readAlt(0);
-        break;
-    default:
-        while (1);
-        break;
-    }
-
-    asm("svc 0");
-    while (1);
-}
-
-__attribute__((naked))
-void MemManage_Handler()
-{
-    // 1. Get the stack pointer.
-    uint32_t lr;
-	asm("mov %0, lr" : "=r" (lr));
-
-    // 2. Recover from the fault.
-    abortAlgorithmFromISR();
-
-    // 3. Return.
-    asm("mov lr, %0; bx lr" :: "r" (lr));
-}
-
-__attribute__((naked))
-void HardFault_Handler()
-{
-    // Get the stack pointer.
-    //uint32_t *stack;
-    uint32_t lr;
-	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));*/
-
-    // If coming from the algorithm, attempt to recover; otherwise, give up.
-    if (run_status != RunStatus::Running && (lr & 4) != 0)
-        MemManage_Handler();
-
-    while (1);
-}
-
-} // extern "C"
-