merge in branch devel

36 files changed, 1342 insertions, 752 deletions

diff --git a/source/board/board_h7.c b/source/board/board_h7.c
index 2868726..74285cf 100644
--- a/source/board/board_h7.c
+++ b/source/board/board_h7.c
@@ -262,5 +262,26 @@ bool mmc_lld_is_write_protected(MMCDriver *mmcp) {
  * @note    You can add your board-specific code here.

  */

 void boardInit(void) {

+    // Enable the FPU (floating-point unit)

+    SCB->CPACR |= 0xF << 20;

 

+    // Setup the MPU (memory protection unit):

+    //     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);

 }

diff --git a/source/board/board_l4.c b/source/board/board_l4.c
index cd16e43..55af697 100644
--- a/source/board/board_l4.c
+++ b/source/board/board_l4.c
@@ -277,5 +277,31 @@ bool mmc_lld_is_write_protected(MMCDriver *mmcp) {
  * @note    You can add your board-specific code here.

  */

 void boardInit(void) {

+    palSetLineMode(LINE_LED_RED, PAL_MODE_OUTPUT_PUSHPULL);

+    palSetLineMode(LINE_LED_GREEN, PAL_MODE_OUTPUT_PUSHPULL);

+    palSetLineMode(LINE_LED_BLUE, PAL_MODE_OUTPUT_PUSHPULL);

+    palClearLine(LINE_LED_RED);

+    palClearLine(LINE_LED_GREEN);

+    palClearLine(LINE_LED_BLUE);

 

+    SCB->CPACR |= 0xF << 20; // Enable FPU

+

+    // 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);

 }

diff --git a/source/board/l4/board.h b/source/board/l4/board.h
index 8291664..4b2642a 100644
--- a/source/board/l4/board.h
+++ b/source/board/l4/board.h
@@ -1502,4 +1502,8 @@ extern "C" {
 #endif

 #endif /* _FROM_ASM_ */

 

+#define LINE_LED_RED   PAL_LINE(GPIOC_BASE, 10U)

+#define LINE_LED_GREEN PAL_LINE(GPIOC_BASE, 11U)

+#define LINE_LED_BLUE  PAL_LINE(GPIOC_BASE, 12U)

+

 #endif /* BOARD_H */

diff --git a/source/cfg/mcuconf_l4.h b/source/cfg/mcuconf_l4.h
index 438e0be..bf19f7a 100644
--- a/source/cfg/mcuconf_l4.h
+++ b/source/cfg/mcuconf_l4.h
@@ -47,11 +47,11 @@
 #define STM32_HSE_ENABLED                   FALSE

 #define STM32_LSE_ENABLED                   FALSE

 #define STM32_MSIPLL_ENABLED                FALSE

-#define STM32_MSIRANGE                      STM32_MSIRANGE_8M

+#define STM32_MSIRANGE                      STM32_MSIRANGE_4M

 #define STM32_MSISRANGE                     STM32_MSISRANGE_4M

 #define STM32_SW                            STM32_SW_PLL

 #define STM32_PLLSRC                        STM32_PLLSRC_MSI

-#define STM32_PLLM_VALUE                    2

+#define STM32_PLLM_VALUE                    1

 #define STM32_PLLN_VALUE                    72

 #define STM32_PLLP_VALUE                    7

 #define STM32_PLLQ_VALUE                    6

diff --git a/source/communication.cpp b/source/communication.cpp
new file mode 100644
index 0000000..b5ee28e
--- /dev/null
+++ b/source/communication.cpp
@@ -0,0 +1,295 @@
+#include "communication.hpp"
+
+#include "ch.h"
+#include "hal.h"
+
+#include "periph/adc.hpp"
+#include "periph/dac.hpp"
+#include "periph/usbserial.hpp"
+#include "elfload.hpp"
+#include "error.hpp"
+#include "conversion.hpp"
+#include "runstatus.hpp"
+#include "samples.hpp"
+
+#include <algorithm>
+#include <tuple>
+
+__attribute__((section(".stacks")))
+std::array<char, 4096> CommunicationManager::m_thread_stack = {};
+
+void CommunicationManager::begin()
+{
+    chThdCreateStatic(m_thread_stack.data(),
+                      m_thread_stack.size(),
+                      NORMALPRIO,
+                      threadComm,
+                      nullptr);
+}
+
+static void writeADCBuffer(unsigned char *);
+static void setBufferSize(unsigned char *);
+static void updateGenerator(unsigned char *);
+static void loadAlgorithm(unsigned char *);
+static void readStatus(unsigned char *);
+static void startConversionMeasure(unsigned char *);
+static void startConversion(unsigned char *);
+static void stopConversion(unsigned char *);
+static void startGenerator(unsigned char *);
+static void readADCBuffer(unsigned char *);
+static void readDACBuffer(unsigned char *);
+static void unloadAlgorithm(unsigned char *);
+static void readIdentifier(unsigned char *);
+static void readExecTime(unsigned char *);
+static void sampleRate(unsigned char *);
+static void readConversionResults(unsigned char *);
+static void readConversionInput(unsigned char *);
+static void readMessage(unsigned char *);
+static void stopGenerator(unsigned char *);
+
+static const std::array<std::pair<char, void (*)(unsigned char *)>, 19> commandTable {{
+    {'A', writeADCBuffer},
+    {'B', setBufferSize},
+    {'D', updateGenerator},
+    {'E', loadAlgorithm},
+    {'I', readStatus},
+    {'M', startConversionMeasure},
+    {'R', startConversion},
+    {'S', stopConversion},
+    {'W', startGenerator},
+    {'a', readADCBuffer},
+    {'d', readDACBuffer},
+    {'e', unloadAlgorithm},
+    {'i', readIdentifier},
+    {'m', readExecTime},
+    {'r', sampleRate},
+    {'s', readConversionResults},
+    {'t', readConversionInput},
+    {'u', readMessage},
+    {'w', stopGenerator}
+}};
+
+void CommunicationManager::threadComm(void *)
+{
+	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
+                auto func = std::find_if(commandTable.cbegin(), commandTable.cend(),
+                                         [&cmd](const auto& f) { return f.first == cmd[0]; });
+                if (func != commandTable.cend())
+                    func->second(cmd);
+            }
+        }
+
+		chThdSleepMicroseconds(100);
+    }
+}
+
+void writeADCBuffer(unsigned char *)
+{
+    USBSerial::read(Samples::In.bytedata(), Samples::In.bytesize());
+}
+
+void setBufferSize(unsigned char *cmd)
+{
+    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)) {
+            Samples::In.setSize(count);
+            Samples::Out.setSize(count);
+        }
+    }
+}
+
+void updateGenerator(unsigned char *cmd)
+{
+    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)) {
+            if (!DAC::isSigGenRunning()) {
+                Samples::Generator.setSize(count);
+                USBSerial::read(
+                    reinterpret_cast<uint8_t *>(Samples::Generator.data()),
+                    Samples::Generator.bytesize());
+            } else {
+                const int more = DAC::sigGenWantsMore();
+                if (more == -1) {
+                    USBSerial::write(reinterpret_cast<const uint8_t *>("\0"), 1);
+                } else {
+                    USBSerial::write(reinterpret_cast<const uint8_t *>("\1"), 1);
+
+                    // Receive streamed samples in half-buffer chunks.
+                    USBSerial::read(reinterpret_cast<uint8_t *>(
+                        more == 0 ? Samples::Generator.data() : Samples::Generator.middata()),
+                        Samples::Generator.bytesize() / 2);
+                }
+            }
+        }
+    }
+}
+
+void loadAlgorithm(unsigned char *cmd)
+{
+    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 < MAX_ELF_FILE_SIZE, Error::BadUserCodeSize)) {
+            USBSerial::read(ELFManager::fileBuffer(), size);
+            auto success = ELFManager::loadFromInternalBuffer();
+            EM.assert(success, Error::BadUserCodeLoad);
+        }
+    }
+}
+
+void readStatus(unsigned char *)
+{
+    unsigned char buf[2] = {
+        static_cast<unsigned char>(run_status),
+        static_cast<unsigned char>(EM.pop())
+    };
+
+    USBSerial::write(buf, sizeof(buf));
+}
+
+void startConversionMeasure(unsigned char *)
+{
+    if (EM.assert(run_status == RunStatus::Idle, Error::NotIdle)) {
+        run_status = RunStatus::Running;
+        ConversionManager::startMeasured();
+    }
+}
+
+void startConversion(unsigned char *)
+{
+    if (EM.assert(run_status == RunStatus::Idle, Error::NotIdle)) {
+        run_status = RunStatus::Running;
+        ConversionManager::start();
+    }
+}
+
+void stopConversion(unsigned char *)
+{
+    if (run_status == RunStatus::Running) {
+        ConversionManager::stop();
+        run_status = RunStatus::Idle;
+    }
+}
+
+void startGenerator(unsigned char *)
+{
+    DAC::start(1, Samples::Generator.data(), Samples::Generator.size());
+}
+
+void readADCBuffer(unsigned char *)
+{
+    USBSerial::write(Samples::In.bytedata(), Samples::In.bytesize());
+}
+
+void readDACBuffer(unsigned char *)
+{
+
+    USBSerial::write(Samples::Out.bytedata(), Samples::Out.bytesize());
+}
+
+void unloadAlgorithm(unsigned char *)
+{
+    ELFManager::unload();
+}
+
+void readIdentifier(unsigned char *)
+{
+#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
+}
+
+void readExecTime(unsigned char *)
+{
+    // Stores the measured execution time.
+    extern time_measurement_t conversion_time_measurement;
+    USBSerial::write(reinterpret_cast<uint8_t *>(&conversion_time_measurement.last),
+                     sizeof(rtcnt_t));
+}
+
+void sampleRate(unsigned char *cmd)
+{
+    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);
+        }
+    }
+}
+
+void readConversionResults(unsigned char *)
+{
+    if (auto samps = Samples::Out.modified(); samps != nullptr) {
+        unsigned char buf[2] = {
+            static_cast<unsigned char>(Samples::Out.size() / 2 & 0xFF),
+            static_cast<unsigned char>(((Samples::Out.size() / 2) >> 8) & 0xFF)
+        };
+        USBSerial::write(buf, 2);
+        unsigned int total = Samples::Out.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);
+    }
+}
+
+void readConversionInput(unsigned char *)
+{
+    if (auto samps = Samples::In.modified(); samps != nullptr) {
+        unsigned char buf[2] = {
+            static_cast<unsigned char>(Samples::In.size() / 2 & 0xFF),
+            static_cast<unsigned char>(((Samples::In.size() / 2) >> 8) & 0xFF)
+        };
+        USBSerial::write(buf, 2);
+        unsigned int total = Samples::In.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);
+    }
+}
+
+void readMessage(unsigned char *)
+{
+    //USBSerial::write(reinterpret_cast<uint8_t *>(userMessageBuffer), userMessageSize);
+}
+
+void stopGenerator(unsigned char *)
+{
+    DAC::stop(1);
+}
+
diff --git a/source/communication.hpp b/source/communication.hpp
new file mode 100644
index 0000000..03220b8
--- /dev/null
+++ b/source/communication.hpp
@@ -0,0 +1,29 @@
+/**
+ * @file communication.hpp
+ * @brief Manages communication with the host computer.
+ *
+ * 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.
+ * If not, see <https://www.gnu.org/licenses/>.
+ */
+
+#ifndef STMDSP_COMMUNICATION_HPP
+#define STMDSP_COMMUNICATION_HPP
+
+#include <array>
+
+class CommunicationManager
+{
+public:
+    static void begin();
+
+private:
+    static void threadComm(void *);
+
+    static std::array<char, 4096> m_thread_stack;
+};
+
+#endif // STMDSP_COMMUNICATION_HPP
+
diff --git a/source/conversion.cpp b/source/conversion.cpp
new file mode 100644
index 0000000..6fdea07
--- /dev/null
+++ b/source/conversion.cpp
@@ -0,0 +1,218 @@
+/**
+ * @file conversion.cpp
+ * @brief Manages algorithm application (converts input samples to output).
+ *
+ * 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.
+ * If not, see <https://www.gnu.org/licenses/>.
+ */
+
+#include "conversion.hpp"
+
+#include "periph/adc.hpp"
+#include "periph/dac.hpp"
+#include "elfload.hpp"
+#include "error.hpp"
+#include "runstatus.hpp"
+#include "samples.hpp"
+
+// MSG_* things below are macros rather than constexpr
+// to ensure inlining.
+
+#define MSG_CONVFIRST          (1)
+#define MSG_CONVSECOND         (2)
+#define MSG_CONVFIRST_MEASURE  (3)
+#define MSG_CONVSECOND_MEASURE (4)
+
+#define MSG_FOR_FIRST(msg)   (msg & 1)
+#define MSG_FOR_MEASURE(msg) (msg > 2)
+
+__attribute__((section(".convdata")))
+thread_t *ConversionManager::m_thread_monitor = nullptr;
+thread_t *ConversionManager::m_thread_runner = nullptr;
+
+__attribute__((section(".stacks")))
+std::array<char, 1024> ConversionManager::m_thread_monitor_stack = {};
+__attribute__((section(".stacks")))
+std::array<char, THD_WORKING_AREA_SIZE(128)> ConversionManager::m_thread_runner_entry_stack = {};
+__attribute__((section(".convdata")))
+std::array<char, CONVERSION_THREAD_STACK_SIZE> ConversionManager::m_thread_runner_stack = {};
+
+std::array<msg_t, 2> ConversionManager::m_mailbox_buffer;
+mailbox_t ConversionManager::m_mailbox = _MAILBOX_DATA(m_mailbox, m_mailbox_buffer.data(), m_mailbox_buffer.size());
+
+void ConversionManager::begin()
+{
+    m_thread_monitor = chThdCreateStatic(m_thread_monitor_stack.data(),
+                                         m_thread_monitor_stack.size(),
+                                         NORMALPRIO + 1,
+                                         threadMonitor,
+                                         nullptr);
+    auto runner_stack_end = &m_thread_runner_stack[CONVERSION_THREAD_STACK_SIZE];
+    m_thread_runner = chThdCreateStatic(m_thread_runner_entry_stack.data(),
+                                        m_thread_runner_entry_stack.size(),
+                                        HIGHPRIO,
+                                        threadRunnerEntry,
+                                        runner_stack_end);
+}
+
+void ConversionManager::start()
+{
+    Samples::Out.clear();
+    ADC::start(Samples::In.data(), Samples::In.size(), adcReadHandler);
+    DAC::start(0, Samples::Out.data(), Samples::Out.size());
+}
+
+void ConversionManager::startMeasured()
+{
+    Samples::Out.clear();
+    ADC::start(Samples::In.data(), Samples::In.size(), adcReadHandlerMeasure);
+    DAC::start(0, Samples::Out.data(), Samples::Out.size());
+}
+
+void ConversionManager::stop()
+{
+    DAC::stop(0);
+    ADC::stop();
+}
+
+thread_t *ConversionManager::getMonitorHandle()
+{
+    return m_thread_monitor;
+}
+
+void ConversionManager::abort(bool fpu_stacked)
+{
+    ELFManager::unload();
+    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));
+
+    bool isRunnerStack = 
+           (uint32_t)psp >= reinterpret_cast<uint32_t>(m_thread_runner_stack.data()) &&
+           (uint32_t)psp <= reinterpret_cast<uint32_t>(m_thread_runner_stack.data() +
+                                                       m_thread_runner_stack.size());
+
+    if (isRunnerStack)
+    {
+        // 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.
+        auto newpsp = reinterpret_cast<uint32_t *>(m_thread_runner_stack.data() + 
+                                                   m_thread_runner_stack.size() -
+                                                   (fpu_stacked ? 26 : 8) * sizeof(uint32_t));
+        // Set the LR register to the thread's entry point.
+        newpsp[5] = reinterpret_cast<uint32_t>(threadRunner);
+        // 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 ConversionManager::threadMonitor(void *)
+{
+    while (1) {
+        msg_t message;
+        msg_t fetch = chMBFetchTimeout(&m_mailbox, &message, TIME_INFINITE);
+        if (fetch == MSG_OK)
+            chMsgSend(m_thread_runner, message);
+    }
+}
+
+void ConversionManager::threadRunnerEntry(void *stack)
+{
+    ELFManager::unload();
+    port_unprivileged_jump(reinterpret_cast<uint32_t>(threadRunner),
+                           reinterpret_cast<uint32_t>(stack));
+}
+
+__attribute__((section(".convcode")))
+void ConversionManager::threadRunner(void *)
+{
+    while (1) {
+        // Sleep until we receive a mailbox message.
+        msg_t message;
+        asm("svc 0; mov %0, r0" : "=r" (message));
+
+        if (message != 0) {
+            auto samples = MSG_FOR_FIRST(message) ? Samples::In.data()
+                                                  : Samples::In.middata();
+            auto size = Samples::In.size() / 2;
+
+            auto entry = ELFManager::loadedElf();
+            if (entry) {
+                // Below, we remember the stack pointer just in case the
+                // loaded algorithm messes things up.
+                uint32_t sp;
+
+                if (!MSG_FOR_MEASURE(message)) {
+                    asm("mov %0, sp" : "=r" (sp));
+                    samples = entry(samples, size);
+                    asm("mov sp, %0" :: "r" (sp));
+                } else {
+                    // Start execution timer:
+                    asm("mov %0, sp; eor r0, r0; svc 2" : "=r" (sp));
+                    samples = entry(samples, size);
+                    // Stop execution timer:
+                    asm("mov r0, #1; svc 2; mov sp, %0" :: "r" (sp));
+                } 
+            }
+
+            // Update the sample out buffer with the transformed samples.
+            if (samples != nullptr) {
+                if (MSG_FOR_FIRST(message))
+                    Samples::Out.modify(samples, size);
+                else
+                    Samples::Out.midmodify(samples, size);
+            }
+        }
+    }
+}
+
+void ConversionManager::adcReadHandler(adcsample_t *buffer, size_t)
+{
+    chSysLockFromISR();
+
+    // If previous request hasn't been handled, then we're going too slow.
+    // We'll need to abort.
+    if (chMBGetUsedCountI(&m_mailbox) > 1) {
+        chMBResetI(&m_mailbox);
+        chMBResumeX(&m_mailbox);
+        chSysUnlockFromISR();
+        abort();
+    } else {
+        // Mark the modified samples as 'fresh' or ready for manipulation.
+        if (buffer == Samples::In.data()) {
+            Samples::In.setModified();
+            chMBPostI(&m_mailbox, MSG_CONVFIRST);
+        } else {
+            Samples::In.setMidmodified();
+            chMBPostI(&m_mailbox, MSG_CONVSECOND);
+        }
+        chSysUnlockFromISR();
+    }
+}
+
+void ConversionManager::adcReadHandlerMeasure(adcsample_t *buffer, size_t)
+{
+    chSysLockFromISR();
+    if (buffer == Samples::In.data()) {
+        Samples::In.setModified();
+        chMBPostI(&m_mailbox, MSG_CONVFIRST_MEASURE);
+    } else {
+        Samples::In.setMidmodified();
+        chMBPostI(&m_mailbox, MSG_CONVSECOND_MEASURE);
+    }
+    chSysUnlockFromISR();
+
+    ADC::setOperation(adcReadHandler);
+}
+
diff --git a/source/conversion.hpp b/source/conversion.hpp
new file mode 100644
index 0000000..a26dd19
--- /dev/null
+++ b/source/conversion.hpp
@@ -0,0 +1,64 @@
+/**
+ * @file conversion.hpp
+ * @brief Manages algorithm application (converts input samples to output).
+ *
+ * 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.
+ * If not, see <https://www.gnu.org/licenses/>.
+ */
+
+#ifndef STMDSP_CONVERSION_HPP
+#define STMDSP_CONVERSION_HPP
+
+#include "ch.h"
+#include "hal.h"
+
+#include <array>
+
+constexpr unsigned int CONVERSION_THREAD_STACK_SIZE = 
+#if defined(TARGET_PLATFORM_H7)
+                                                  62 * 1024;
+#else
+                                                  15 * 1024;
+#endif
+
+class ConversionManager
+{
+public:
+    static void begin();
+
+    // Begins sample conversion.
+    static void start();
+    // Begins conversion with execution time measured.
+    static void startMeasured();
+    // Stops conversion.
+    static void stop();
+
+    static thread_t *getMonitorHandle();
+
+    // Internal only: Aborts a running conversion.
+    static void abort(bool fpu_stacked = true);
+
+private:
+    static void threadMonitor(void *);
+    static void threadRunnerEntry(void *stack);
+
+    static void threadRunner(void *);
+    static void adcReadHandler(adcsample_t *buffer, size_t);
+    static void adcReadHandlerMeasure(adcsample_t *buffer, size_t);
+
+    static thread_t *m_thread_monitor;
+    static thread_t *m_thread_runner;
+
+    static std::array<char, 1024> m_thread_monitor_stack;
+    static std::array<char, THD_WORKING_AREA_SIZE(128)> m_thread_runner_entry_stack;
+    static std::array<char, CONVERSION_THREAD_STACK_SIZE> m_thread_runner_stack;
+
+    static std::array<msg_t, 2> m_mailbox_buffer;
+    static mailbox_t m_mailbox;
+};
+
+#endif // STMDSP_CONVERSION_HPP
+
diff --git a/source/elf_format.hpp b/source/elf.h
index d4feb99..998356d 100644
--- a/source/elf_format.hpp
+++ b/source/elf.h
@@ -1,12 +1,12 @@
 /**
- * @file elf_format.cpp
+ * @file elf.h
  * @brief Defines ELF binary format info.
  *
  * Free to use, written by Clyne Sullivan.
  */
 
-#ifndef STMDSP_ELF_FORMAT_HPP_
-#define STMDSP_ELF_FORMAT_HPP_
+#ifndef STMDSP_ELF_HPP
+#define STMDSP_ELF_HPP
 
 #include <cstdint>
 
@@ -96,5 +96,5 @@ typedef struct {
 	Elf32_Word p_align;
 } __attribute__((packed)) Elf32_Phdr;
 
-#endif // STMDSP_ELF_FORMAT_HPP_
+#endif // STMDSP_ELF_HPP
 
diff --git a/source/elf_load.hpp b/source/elf_load.hpp
deleted file mode 100644
index 619dada..0000000
--- a/source/elf_load.hpp
+++ /dev/null
@@ -1,26 +0,0 @@
-/**
- * @file elf_load.hpp
- * @brief Loads ELF binary data into memory for execution.
- *
- * Copyright (C) 2020 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.
- * If not, see <https://www.gnu.org/licenses/>.
- */
-
-#ifndef ELF_LOAD_HPP_
-#define ELF_LOAD_HPP_
-
-#include <cstddef>
-#include <cstdint>
-
-namespace ELF
-{
-    using Entry = uint16_t *(*)(uint16_t *, size_t);
-
-    Entry load(void *elf_data);
-}
-
-#endif // ELF_LOAD_HPP_
-
diff --git a/source/elf_load.cpp b/source/elfload.cpp
index 0e41d6a..87461e4 100644
--- a/source/elf_load.cpp
+++ b/source/elfload.cpp
@@ -1,36 +1,58 @@
 /**
- * @file elf_load.cpp
+ * @file elfload.cpp
  * @brief Loads ELF binary data into memory for execution.
  *
- * 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.
  * If not, see <https://www.gnu.org/licenses/>.
  */
 
-#include "elf_load.hpp"
-#include "elf_format.hpp"
+#include "elfload.hpp"
+#include "elf.h"
 
 #include <algorithm>
 #include <cstring>
 
+__attribute__((section(".convdata")))
+ELFManager::EntryFunc ELFManager::m_entry = nullptr;
+std::array<unsigned char, MAX_ELF_FILE_SIZE> ELFManager::m_file_buffer = {};
+
 static const unsigned char elf_header[] = { '\177', 'E', 'L', 'F' };
 
+__attribute__((section(".convcode")))
+ELFManager::EntryFunc ELFManager::loadedElf()
+{
+    return m_entry;
+}
+
+unsigned char *ELFManager::fileBuffer()
+{
+    return m_file_buffer.data();
+}
+
+void ELFManager::unload()
+{
+    m_entry = nullptr;
+}
+
 template<typename T>
 constexpr static auto ptr_from_offset(void *base, uint32_t offset)
 {
     return reinterpret_cast<T>(reinterpret_cast<uint8_t *>(base) + offset);
 }
 
-namespace ELF {
-
-Entry load(void *elf_data)
+bool ELFManager::loadFromInternalBuffer()
 {
+    m_entry = nullptr;
+
+    auto elf_data = m_file_buffer.data();
+
     // Check the ELF's header signature
     auto ehdr = reinterpret_cast<Elf32_Ehdr *>(elf_data);
     if (!std::equal(ehdr->e_ident, ehdr->e_ident + 4, elf_header))
-        return nullptr;
+        return false;
 
     // Iterate through program header LOAD sections
     bool loaded = false;
@@ -54,8 +76,9 @@ Entry load(void *elf_data)
     }
 
 
-    return loaded ? reinterpret_cast<Entry>(ehdr->e_entry) : nullptr;
-}
+    if (loaded)
+        m_entry = reinterpret_cast<ELFManager::EntryFunc>(ehdr->e_entry);
 
-} // namespace ELF
+    return loaded;
+}
 
diff --git a/source/elfload.hpp b/source/elfload.hpp
new file mode 100644
index 0000000..10d95d7
--- /dev/null
+++ b/source/elfload.hpp
@@ -0,0 +1,39 @@
+/**
+ * @file elfload.hpp
+ * @brief Loads ELF binary data into memory for execution.
+ *
+ * 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.
+ * If not, see <https://www.gnu.org/licenses/>.
+ */
+
+#ifndef ELF_LOAD_HPP_
+#define ELF_LOAD_HPP_
+
+#include "samplebuffer.hpp"
+
+#include <array>
+#include <cstddef>
+
+constexpr unsigned int MAX_ELF_FILE_SIZE = 16 * 1024;
+
+class ELFManager
+{
+public:
+    using EntryFunc = Sample *(*)(Sample *, size_t);
+    
+    static bool loadFromInternalBuffer();
+    static EntryFunc loadedElf();
+    static unsigned char *fileBuffer();
+    static void unload();
+
+private:
+    static EntryFunc m_entry;
+
+    static std::array<unsigned char, MAX_ELF_FILE_SIZE> m_file_buffer;
+};
+
+#endif // ELF_LOAD_HPP_
+
diff --git a/source/error.cpp b/source/error.cpp
new file mode 100644
index 0000000..9f2e98f
--- /dev/null
+++ b/source/error.cpp
@@ -0,0 +1,38 @@
+/**
+ * @file error.cpp
+ * @brief Tracks and reports non-critical errors.
+ *
+ * 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.
+ * If not, see <https://www.gnu.org/licenses/>.
+ */
+
+#include "error.hpp"
+
+ErrorManager EM;
+
+void ErrorManager::add(Error error)
+{ 
+    if (m_index < m_queue.size())
+        m_queue[m_index++] = error;
+}
+
+bool ErrorManager::assert(bool condition, Error error)
+{
+    if (!condition)
+        add(error);
+    return condition;
+}
+
+bool ErrorManager::hasError()
+{
+    return m_index > 0;
+}
+
+Error ErrorManager::pop()
+{
+    return m_index == 0 ? Error::None : m_queue[--m_index];
+}
+
diff --git a/source/error.hpp b/source/error.hpp
index 6911792..9bbbe2c 100644
--- a/source/error.hpp
+++ b/source/error.hpp
@@ -1,6 +1,18 @@
-#include <array>
+/**
+ * @file error.hpp
+ * @brief Tracks and reports non-critical errors.
+ *
+ * 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.
+ * If not, see <https://www.gnu.org/licenses/>.
+ */
+
+#ifndef STMDSP_ERROR_HPP
+#define STMDSP_ERROR_HPP
 
-constexpr unsigned int MAX_ERROR_QUEUE_SIZE = 8;
+#include <array>
 
 enum class Error : char
 {
@@ -15,28 +27,20 @@ enum class Error : char
 
 class ErrorManager
 {
-public:
-    void add(Error error) {
-        if (m_index < m_queue.size())
-            m_queue[m_index++] = error;
-    }
-
-    bool assert(bool condition, Error error) {
-        if (!condition)
-            add(error);
-        return condition;
-    }
+    constexpr static unsigned int MAX_ERROR_QUEUE_SIZE = 8;
 
-    bool hasError() {
-        return m_index > 0;
-    }
-
-    Error pop() {
-        return m_index == 0 ? Error::None : m_queue[--m_index];
-    }
+public:
+    void add(Error error);
+    bool assert(bool condition, Error error);
+    bool hasError();
+    Error pop();
 
 private:
     std::array<Error, MAX_ERROR_QUEUE_SIZE> m_queue;
     unsigned int m_index = 0;
 };
 
+extern ErrorManager EM;
+
+#endif // STMDSP_ERROR_HPP
+
diff --git a/source/handlers.cpp b/source/handlers.cpp
new file mode 100644
index 0000000..43e65c3
--- /dev/null
+++ b/source/handlers.cpp
@@ -0,0 +1,140 @@
+#include "handlers.hpp"
+
+#include "adc.hpp"
+#include "conversion.hpp"
+#include "cordic.hpp"
+#include "runstatus.hpp"
+
+extern "C" {
+
+time_measurement_t conversion_time_measurement;
+
+__attribute__((naked))
+void port_syscall(struct port_extctx *ctxp, uint32_t n)
+{
+    switch (n) {
+
+    // Sleeps the current thread until a message is received.
+    // Used the algorithm runner to wait for new data.
+    case 0:
+        {
+            chSysLock();
+            chMsgWaitS();
+            auto monitor = ConversionManager::getMonitorHandle();
+            auto msg = chMsgGet(monitor);
+            chMsgReleaseS(monitor, MSG_OK);
+            chSysUnlock();
+            ctxp->r0 = msg;
+        }
+        break;
+
+    // Provides access to advanced math functions.
+    // A service call like this is required for some hardware targets that
+    // provide hardware-accelerated math computations (e.g. CORDIC).
+    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;
+
+    // Starts or stops precise cycle time measurement.
+    // Used to measure algorithm execution time.
+    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;
+
+    // Reads one of the analog inputs made available for algorithm run-time input.
+    case 3:
+        ctxp->r0 = ADC::readAlt(ctxp->r0);
+        break;
+
+    //case 4:
+    //    {
+    //        const char *str = reinterpret_cast<const char *>(ctxp->r0);
+    //        auto src = str;
+    //        auto dst = userMessageBuffer;
+    //        while (*src)
+    //            *dst++ = *src++;
+    //        *dst = '\0';
+    //        userMessageSize = src - str;
+    //    }
+    //    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.
+    ConversionManager::abort((lr & (1 << 4)) ? false : true);
+
+    // 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"
+
diff --git a/source/handlers.hpp b/source/handlers.hpp
new file mode 100644
index 0000000..fd7e10c
--- /dev/null
+++ b/source/handlers.hpp
@@ -0,0 +1,31 @@
+/**
+ * @file handlers.hpp
+ * @brief Interrupt service routine handlers.
+ *
+ * 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.
+ * If not, see <https://www.gnu.org/licenses/>.
+ */
+
+#ifndef STMDSP_HANDLERS_HPP
+#define STMDSP_HANDLERS_HPP
+
+#include "ch.h"
+
+extern "C" {
+
+__attribute__((naked))
+void port_syscall(struct port_extctx *ctxp, uint32_t n);
+
+__attribute__((naked))
+void MemManage_Handler();
+
+__attribute__((naked))
+void HardFault_Handler();
+
+}
+
+#endif // STMDSP_HANDLERS_HPP
+
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"
-
diff --git a/source/monitor.cpp b/source/monitor.cpp
new file mode 100644
index 0000000..6ef97e9
--- /dev/null
+++ b/source/monitor.cpp
@@ -0,0 +1,80 @@
+/**
+ * @file monitor.cpp
+ * @brief Manages the device monitoring thread (status LEDs, etc.).
+ *
+ * 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.
+ * If not, see <https://www.gnu.org/licenses/>.
+ */
+
+#include "monitor.hpp"
+
+#include "error.hpp"
+#include "runstatus.hpp"
+
+#include "hal.h"
+
+__attribute__((section(".stacks")))
+std::array<char, THD_WORKING_AREA_SIZE(256)> Monitor::m_thread_stack = {};
+
+void Monitor::begin()
+{
+    chThdCreateStatic(m_thread_stack.data(),
+                      m_thread_stack.size(),
+                      LOWPRIO,
+                      threadMonitor,
+                      nullptr);
+}
+
+void Monitor::threadMonitor(void *)
+{
+    palSetLineMode(LINE_BUTTON, PAL_MODE_INPUT_PULLUP);
+    auto readButton = [] {
+#ifdef TARGET_PLATFORM_L4
+        return !palReadLine(LINE_BUTTON);
+#else
+        return palReadLine(LINE_BUTTON);
+#endif
+    };
+
+    palSetLine(LINE_LED_RED);
+    palSetLine(LINE_LED_GREEN);
+    palSetLine(LINE_LED_BLUE);
+
+    while (1) {
+        bool isidle = run_status == RunStatus::Idle;
+        auto led = isidle ? LINE_LED_GREEN : LINE_LED_BLUE;
+        auto delay = isidle ? 500 : 250;
+
+        palToggleLine(led);
+        chThdSleepMilliseconds(delay);
+        palToggleLine(led);
+        chThdSleepMilliseconds(delay);
+
+        if (isidle && readButton()) {
+            palClearLine(LINE_LED_GREEN);
+            palClearLine(LINE_LED_BLUE);
+            chSysLock();
+            while (readButton())
+                asm("nop");
+            while (!readButton())
+                asm("nop");
+            chSysUnlock();
+            palSetLine(LINE_LED_GREEN);
+            palSetLine(LINE_LED_BLUE);
+            chThdSleepMilliseconds(500);
+        }
+
+        static bool erroron = false;
+        if (auto err = EM.hasError(); err ^ erroron) {
+            erroron = err;
+            if (err)
+                palClearLine(LINE_LED_RED);
+            else
+                palSetLine(LINE_LED_RED);
+        }
+    }
+}
+
diff --git a/source/monitor.hpp b/source/monitor.hpp
new file mode 100644
index 0000000..93f75e3
--- /dev/null
+++ b/source/monitor.hpp
@@ -0,0 +1,31 @@
+/**
+ * @file monitor.hpp
+ * @brief Manages the device monitoring thread (status LEDs, etc.).
+ *
+ * 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.
+ * If not, see <https://www.gnu.org/licenses/>.
+ */
+
+#ifndef STMDSP_MONITOR_HPP
+#define STMDSP_MONITOR_HPP
+
+#include "ch.h"
+
+#include <array>
+
+class Monitor
+{
+public:
+    static void begin();
+
+private:
+    static void threadMonitor(void *);
+
+    static std::array<char, THD_WORKING_AREA_SIZE(256)> m_thread_stack;
+};
+
+#endif // STMDSP_MONITOR_HPP
+
diff --git a/source/adc.cpp b/source/periph/adc.cpp
index 3ab5430..4667307 100644
--- a/source/adc.cpp
+++ b/source/periph/adc.cpp
@@ -2,7 +2,7 @@
  * @file adc.cpp
  * @brief Manages signal reading through the ADC.
  *
- * 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.
@@ -39,7 +39,7 @@ ADCConversionGroup ADC::m_group_config = {
     .end_cb = ADC::conversionCallback,
     .error_cb = nullptr,
     .cfgr = ADC_CFGR_EXTEN_RISING | ADC_CFGR_EXTSEL_SRC(13),  /* TIM6_TRGO */
-    .cfgr2 = ADC_CFGR2_ROVSE | ADC_CFGR2_OVSR_1 | ADC_CFGR2_OVSS_0, // Oversampling 2x
+    .cfgr2 = 0,//ADC_CFGR2_ROVSE | ADC_CFGR2_OVSR_1 | ADC_CFGR2_OVSS_0, // Oversampling 2x
 #if defined(TARGET_PLATFORM_H7)
     .ccr = 0,
     .pcsel = 0,
@@ -69,11 +69,11 @@ static void readAltCallback(ADCDriver *)
 }
 ADCConversionGroup ADC::m_group_config2 = {
     .circular = false,
-    .num_channels = 1,
+    .num_channels = 2,
     .end_cb = readAltCallback,
     .error_cb = nullptr,
     .cfgr = ADC_CFGR_EXTEN_RISING | ADC_CFGR_EXTSEL_SRC(13),  /* TIM6_TRGO */
-    .cfgr2 = ADC_CFGR2_ROVSE | ADC_CFGR2_OVSR_1 | ADC_CFGR2_OVSS_0, // Oversampling 2x
+    .cfgr2 = 0,//ADC_CFGR2_ROVSE | ADC_CFGR2_OVSR_1 | ADC_CFGR2_OVSS_0, // Oversampling 2x
 #if defined(TARGET_PLATFORM_H7)
     .ccr = 0,
     .pcsel = 0,
@@ -88,10 +88,10 @@ ADCConversionGroup ADC::m_group_config2 = {
     .awd3cr = 0,
 #endif
     .smpr = {
-        ADC_SMPR1_SMP_AN1(ADC_SMPR_SMP_12P5), 0
+        ADC_SMPR1_SMP_AN1(ADC_SMPR_SMP_2P5) | ADC_SMPR1_SMP_AN2(ADC_SMPR_SMP_2P5), 0
     },
     .sqr = {
-        ADC_SQR1_SQ1_N(ADC_CHANNEL_IN1),
+        ADC_SQR1_SQ1_N(ADC_CHANNEL_IN1) | ADC_SQR1_SQ2_N(ADC_CHANNEL_IN2),
         0, 0, 0
     },
 };
@@ -107,6 +107,7 @@ void ADC::begin()
 #else
     palSetPadMode(GPIOA, 0, PAL_MODE_INPUT_ANALOG); // Algorithm in
     palSetPadMode(GPIOC, 0, PAL_MODE_INPUT_ANALOG); // Potentiometer 1
+    palSetPadMode(GPIOC, 1, PAL_MODE_INPUT_ANALOG); // Potentiometer 2
 #endif
 
     adcStart(m_driver, &m_config);
@@ -135,15 +136,15 @@ void ADC::stop()
 
 adcsample_t ADC::readAlt(unsigned int id)
 {
-    if (id != 0)
+    if (id > 1)
         return 0;
-    static adcsample_t result[32] = {};
+    static adcsample_t result[16] = {};
     readAltDone = false;
-    adcStartConversion(m_driver2, &m_group_config2, result, 32);
-    while (!readAltDone)
-        ;
+    adcStartConversion(m_driver2, &m_group_config2, result, 8);
+    while (!readAltDone);
+        //__WFI();
     adcStopConversion(m_driver2);
-    return result[0];
+    return result[id];
 }
 
 void ADC::setRate(SClock::Rate rate)
@@ -181,13 +182,29 @@ void ADC::setRate(SClock::Rate rate)
     adcStart(m_driver, &m_config);
 #elif defined(TARGET_PLATFORM_L4)
     std::array<std::array<uint32_t, 3>, 6> m_rate_presets = {{
+        // PLLSAI2 sources MSI of 4MHz, divided by PLLM of /1 = 4MHz.
+        // 4MHz is then multiplied by PLLSAI2N (x8 to x86), with result
+        // between 64 and 344 MHz.
+        //
+        // SAI2N MUST BE AT LEAST 16 TO MAKE 64MHz MINIMUM.
+        //
+        // That is then divided by PLLSAI2R:
+        //     R of 0 = /2; 1 = /4, 2 = /6, 3 = /8.
+        // PLLSAI2 then feeds into the ADC, which has a prescaler of /10.
+        // Finally, the ADC's SMP value produces the desired sample rate.
+        //
+        // 4MHz * N / R / 10 / SMP = sample rate.
+        //
+        // With oversampling, must create faster clock
+        // (x2 oversampling requires x2 sample rate clock).
+        //
         //  Rate   PLLSAI2N  R  SMPR
-        {/* 8k  */ 8,        1, ADC_SMPR_SMP_12P5},
-        {/* 16k */ 16,       1, ADC_SMPR_SMP_12P5},
-        {/* 20k */ 20,       1, ADC_SMPR_SMP_12P5},
-        {/* 32k */ 32,       1, ADC_SMPR_SMP_12P5},
-        {/* 48k */ 24,       0, ADC_SMPR_SMP_12P5},
-        {/* 96k */ 73,       1, ADC_SMPR_SMP_6P5}   // Technically 96.05263kS/s
+        {/* 8k  */ 16,       1, ADC_SMPR_SMP_12P5}, // R3=32k (min), R1=64k
+        {/* 16k */ 16,       0, ADC_SMPR_SMP_12P5},
+        {/* 20k */ 20,       0, ADC_SMPR_SMP_12P5},
+        {/* 32k */ 32,       0, ADC_SMPR_SMP_12P5},
+        {/* 48k */ 48,       0, ADC_SMPR_SMP_12P5},
+        {/* 96k */ 73,       0, ADC_SMPR_SMP_6P5}   // Technically 96.05263kS/s
     }};
 
     auto& preset = m_rate_presets[static_cast<int>(rate)];
@@ -204,9 +221,9 @@ void ADC::setRate(SClock::Rate rate)
 
     m_group_config.smpr[0] = ADC_SMPR1_SMP_AN5(smpr);
 
-    // Set 2x oversampling
-    m_group_config.cfgr2 = ADC_CFGR2_ROVSE | ADC_CFGR2_OVSR_0 | ADC_CFGR2_OVSS_1;
-    m_group_config2.cfgr2 = ADC_CFGR2_ROVSE | ADC_CFGR2_OVSR_0 | ADC_CFGR2_OVSS_1;
+    // 8x oversample
+    m_group_config.cfgr2 = ADC_CFGR2_ROVSE | (2 << ADC_CFGR2_OVSR_Pos) | (3 << ADC_CFGR2_OVSS_Pos);
+    m_group_config2.cfgr2 = ADC_CFGR2_ROVSE | (2 << ADC_CFGR2_OVSR_Pos) | (3 << ADC_CFGR2_OVSS_Pos);
 #endif
 }
 
diff --git a/source/adc.hpp b/source/periph/adc.hpp
index 24a7fff..5f7fa08 100644
--- a/source/adc.hpp
+++ b/source/periph/adc.hpp
@@ -2,7 +2,7 @@
  * @file adc.hpp
  * @brief Manages signal reading through the ADC.
  *
- * 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.
diff --git a/source/cordic.cpp b/source/periph/cordic.cpp
index 29ee068..29ee068 100644
--- a/source/cordic.cpp
+++ b/source/periph/cordic.cpp
diff --git a/source/cordic.hpp b/source/periph/cordic.hpp
index 5d640cc..5d640cc 100644
--- a/source/cordic.hpp
+++ b/source/periph/cordic.hpp
diff --git a/source/dac.cpp b/source/periph/dac.cpp
index ce9c465..35c2908 100644
--- a/source/dac.cpp
+++ b/source/periph/dac.cpp
@@ -2,7 +2,7 @@
  * @file dac.cpp
  * @brief Manages signal creation using the DAC.
  *
- * 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.
@@ -17,14 +17,21 @@ DACDriver *DAC::m_driver[2] = {
 };
 
 const DACConfig DAC::m_config = {
-    .init = 0,
+    .init = 2048,
     .datamode = DAC_DHRM_12BIT_RIGHT,
     .cr = 0
 };
 
+static int dacIsDone = -1;
+static void dacEndCallback(DACDriver *dacd)
+{
+    if (dacd == &DACD2)
+        dacIsDone = dacIsBufferComplete(dacd) ? 1 : 0;
+}
+
 const DACConversionGroup DAC::m_group_config = {
     .num_channels = 1,
-    .end_cb = nullptr,
+    .end_cb = dacEndCallback,
     .error_cb = nullptr,
 #if defined(TARGET_PLATFORM_H7)
     .trigger = 5 // TIM6_TRGO
@@ -45,11 +52,29 @@ void DAC::begin()
 void DAC::start(int channel, dacsample_t *buffer, size_t count)
 {
     if (channel >= 0 && channel < 2) {
+        if (channel == 1)
+            dacIsDone = -1;
         dacStartConversion(m_driver[channel], &m_group_config, buffer, count);
         SClock::start();
     }
 }
 
+int DAC::sigGenWantsMore()
+{
+    if (dacIsDone != -1) {
+        int tmp = dacIsDone;
+        dacIsDone = -1;
+        return tmp;
+    } else {
+        return -1;
+    }
+}
+
+int DAC::isSigGenRunning()
+{
+    return m_driver[1]->state == DAC_ACTIVE;
+}
+
 void DAC::stop(int channel)
 {
     if (channel >= 0 && channel < 2) {
diff --git a/source/dac.hpp b/source/periph/dac.hpp
index e305c4b..7250a52 100644
--- a/source/dac.hpp
+++ b/source/periph/dac.hpp
@@ -2,7 +2,7 @@
  * @file dac.hpp
  * @brief Manages signal creation using the DAC.
  *
- * 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.
@@ -23,6 +23,9 @@ public:
     static void start(int channel, dacsample_t *buffer, size_t count);
     static void stop(int channel);
 
+    static int sigGenWantsMore();
+    static int isSigGenRunning();
+
 private:
     static DACDriver *m_driver[2];
 
diff --git a/source/usbcfg.c b/source/periph/usbcfg.c
index b726e23..b726e23 100644
--- a/source/usbcfg.c
+++ b/source/periph/usbcfg.c
diff --git a/source/usbcfg.h b/source/periph/usbcfg.h
index 2fceccb..2fceccb 100644
--- a/source/usbcfg.h
+++ b/source/periph/usbcfg.h
diff --git a/source/usbserial.cpp b/source/periph/usbserial.cpp
index c24be2f..775a911 100644
--- a/source/usbserial.cpp
+++ b/source/periph/usbserial.cpp
@@ -2,7 +2,7 @@
  * @file usbserial.cpp
  * @brief Wrapper for ChibiOS's SerialUSBDriver.
  *
- * 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.
@@ -32,7 +32,7 @@ bool USBSerial::isActive()
 {
     if (auto config = m_driver->config; config != nullptr) {
         if (auto usbp = config->usbp; usbp != nullptr)
-            return usbp->state == USB_ACTIVE;
+            return usbp->state == USB_ACTIVE && !ibqIsEmptyI(&m_driver->ibqueue);
     }
 
     return false;
diff --git a/source/usbserial.hpp b/source/periph/usbserial.hpp
index 828fc56..58113c9 100644
--- a/source/usbserial.hpp
+++ b/source/periph/usbserial.hpp
@@ -2,7 +2,7 @@
  * @file usbserial.hpp
  * @brief Wrapper for ChibiOS's SerialUSBDriver.
  *
- * 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.
diff --git a/source/runstatus.hpp b/source/runstatus.hpp
new file mode 100644
index 0000000..ab269b4
--- /dev/null
+++ b/source/runstatus.hpp
@@ -0,0 +1,11 @@
+// Run status
+//
+enum class RunStatus : char
+{
+    Idle = '1',
+    Running,
+    Recovering
+};
+
+extern RunStatus run_status;
+
diff --git a/source/samplebuffer.cpp b/source/samplebuffer.cpp
index 24cc424..74c6778 100644
--- a/source/samplebuffer.cpp
+++ b/source/samplebuffer.cpp
@@ -1,3 +1,14 @@
+/**
+ * @file samplebuffer.cpp
+ * @brief Manages ADC/DAC buffer data.
+ *
+ * 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.
+ * If not, see <https://www.gnu.org/licenses/>.
+ */
+
 #include "samplebuffer.hpp"
 
 SampleBuffer::SampleBuffer(Sample *buffer) :
@@ -9,16 +20,62 @@ void SampleBuffer::clear() {
 __attribute__((section(".convcode")))
 void SampleBuffer::modify(Sample *data, unsigned int srcsize) {
     auto size = srcsize < m_size ? srcsize : m_size;
+    size = (size + 15) & (~15);
+
     m_modified = m_buffer;
-    for (Sample *d = m_buffer, *s = data; s != data + size;)
-        *d++ = *s++;
+    const int *src = reinterpret_cast<const int *>(data);
+    const int * const srcend = src + (size / 2);
+    int *dst = reinterpret_cast<int *>(m_buffer);
+    do {
+        int a = src[0];
+        int b = src[1];
+        int c = src[2];
+        int d = src[3];
+        int e = src[4];
+        int f = src[5];
+        int g = src[6];
+        int h = src[7];
+        dst[0] = a;
+        dst[1] = b;
+        dst[2] = c;
+        dst[3] = d;
+        dst[4] = e;
+        dst[5] = f;
+        dst[6] = g;
+        dst[7] = h;
+        src += 8;
+        dst += 8;
+    } while (src < srcend);
 }
 __attribute__((section(".convcode")))
 void SampleBuffer::midmodify(Sample *data, unsigned int srcsize) {
     auto size = srcsize < m_size / 2 ? srcsize : m_size / 2;
+    size = (size + 15) & (~15);
+
     m_modified = middata();
-    for (Sample *d = middata(), *s = data; s != data + size;)
-        *d++ = *s++;
+    const int *src = reinterpret_cast<const int *>(data);
+    const int * const srcend = src + (size / 2);
+    int *dst = reinterpret_cast<int *>(middata());
+    do {
+        int a = src[0];
+        int b = src[1];
+        int c = src[2];
+        int d = src[3];
+        int e = src[4];
+        int f = src[5];
+        int g = src[6];
+        int h = src[7];
+        dst[0] = a;
+        dst[1] = b;
+        dst[2] = c;
+        dst[3] = d;
+        dst[4] = e;
+        dst[5] = f;
+        dst[6] = g;
+        dst[7] = h;
+        src += 8;
+        dst += 8;
+    } while (src < srcend);
 }
 
 void SampleBuffer::setModified() {
diff --git a/source/samplebuffer.hpp b/source/samplebuffer.hpp
index 6d17d2a..d13023a 100644
--- a/source/samplebuffer.hpp
+++ b/source/samplebuffer.hpp
@@ -1,3 +1,14 @@
+/**
+ * @file samplebuffer.hpp
+ * @brief Manages ADC/DAC buffer data.
+ *
+ * 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.
+ * If not, see <https://www.gnu.org/licenses/>.
+ */
+
 #ifndef SAMPLEBUFFER_HPP_
 #define SAMPLEBUFFER_HPP_
 
diff --git a/source/samples.cpp b/source/samples.cpp
new file mode 100644
index 0000000..cfbf835
--- /dev/null
+++ b/source/samples.cpp
@@ -0,0 +1,35 @@
+/**
+ * @file samples.cpp
+ * @brief Provides sample buffers for inputs and outputs.
+ *
+ * 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.
+ * If not, see <https://www.gnu.org/licenses/>.
+ */
+
+#include "samples.hpp"
+
+#include "ch.h"
+#include "hal.h"
+
+#include <cstdint>
+
+static_assert(sizeof(adcsample_t) == sizeof(uint16_t));
+static_assert(sizeof(dacsample_t) == sizeof(uint16_t));
+
+#if defined(TARGET_PLATFORM_H7)
+__attribute__((section(".convdata")))
+SampleBuffer Samples::In (reinterpret_cast<Sample *>(0x38000000)); // 16k
+__attribute__((section(".convdata")))
+SampleBuffer Samples::Out (reinterpret_cast<Sample *>(0x30004000)); // 16k
+SampleBuffer Samples::SigGen (reinterpret_cast<Sample *>(0x30000000)); // 16k
+#else
+__attribute__((section(".convdata")))
+SampleBuffer Samples::In (reinterpret_cast<Sample *>(0x20008000)); // 16k
+__attribute__((section(".convdata")))
+SampleBuffer Samples::Out (reinterpret_cast<Sample *>(0x2000C000)); // 16k
+SampleBuffer Samples::Generator (reinterpret_cast<Sample *>(0x20010000)); // 16k
+#endif
+
diff --git a/source/samples.hpp b/source/samples.hpp
new file mode 100644
index 0000000..6551e25
--- /dev/null
+++ b/source/samples.hpp
@@ -0,0 +1,26 @@
+/**
+ * @file samples.hpp
+ * @brief Provides sample buffers for inputs and outputs.
+ *
+ * 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.
+ * If not, see <https://www.gnu.org/licenses/>.
+ */
+
+#ifndef STMDSP_SAMPLES_HPP
+#define STMDSP_SAMPLES_HPP
+
+#include "samplebuffer.hpp"
+
+class Samples
+{
+public:
+    static SampleBuffer In;
+    static SampleBuffer Out;
+    static SampleBuffer Generator;
+};
+
+#endif // STMDSP_SAMPLES_HPP
+
diff --git a/source/sclock.cpp b/source/sclock.cpp
index 198c684..6660f95 100644
--- a/source/sclock.cpp
+++ b/source/sclock.cpp
@@ -1,3 +1,14 @@
+/**
+ * @file sclock.cpp
+ * @brief Manages sampling rate clock speeds.
+ *
+ * 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.
+ * If not, see <https://www.gnu.org/licenses/>.
+ */
+
 #include "sclock.hpp"
 
 GPTDriver *SClock::m_timer = &GPTD6;
@@ -24,7 +35,12 @@ const std::array<unsigned int, 6> SClock::m_rate_divs = {{
     /* 48k */ 100,
     /* 96k */ 50
 #else
-    4500, 2250, 1800, 1125, 750, 375
+    /* 8k  */ 4500,
+    /* 16k */ 2250,
+    /* 20k */ 1800,
+    /* 32k */ 1125,
+    /* 48k */ 750,
+    /* 96k */ 375
 #endif
 }};
 
diff --git a/source/sclock.hpp b/source/sclock.hpp
index 960d9e3..d5b93df 100644
--- a/source/sclock.hpp
+++ b/source/sclock.hpp
@@ -1,3 +1,14 @@
+/**
+ * @file sclock.hpp
+ * @brief Manages sampling rate clock speeds.
+ *
+ * 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.
+ * If not, see <https://www.gnu.org/licenses/>.
+ */
+
 #ifndef SCLOCK_HPP_
 #define SCLOCK_HPP_