Merge pull request #3 from tcsullivan/stm32h7

Stm32h7

14 files changed, 862 insertions, 204 deletions

diff --git a/source/adc.cpp b/source/adc.cpp
index bdfeea8..b9fe34c 100644
--- a/source/adc.cpp
+++ b/source/adc.cpp
@@ -11,11 +11,26 @@
 
 #include "adc.hpp"
 
+#if defined(TARGET_PLATFORM_L4)
 ADCDriver *ADC::m_driver = &ADCD1;
-GPTDriver *ADC::m_timer = &GPTD6;
+ADCDriver *ADC::m_driver2 = &ADCD3;
+#else
+ADCDriver *ADC::m_driver = &ADCD3;
+//ADCDriver *ADC::m_driver2 = &ADCD1; // TODO
+#endif
 
 const ADCConfig ADC::m_config = {
-    .difsel = 0
+    .difsel = 0,
+#if defined(TARGET_PLATFORM_H7)
+    .calibration = 0,
+#endif
+};
+
+const ADCConfig ADC::m_config2 = {
+    .difsel = 0,
+#if defined(TARGET_PLATFORM_H7)
+    .calibration = 0,
+#endif
 };
 
 ADCConversionGroup ADC::m_group_config = {
@@ -24,49 +39,78 @@ 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_0 | ADC_CFGR2_OVSS_1, // Oversampling 2x
+    .cfgr2 = ADC_CFGR2_ROVSE | ADC_CFGR2_OVSR_1 | ADC_CFGR2_OVSS_0, // Oversampling 2x
+#if defined(TARGET_PLATFORM_H7)
+    .ccr = 0,
+    .pcsel = 0,
+    .ltr1 = 0, .htr1 = 4095,
+    .ltr2 = 0, .htr2 = 4095,
+    .ltr3 = 0, .htr3 = 4095,
+#else
     .tr1 = ADC_TR(0, 4095),
+    .tr2 = ADC_TR(0, 4095),
+    .tr3 = ADC_TR(0, 4095),
+    .awd2cr = 0,
+    .awd3cr = 0,
+#endif
     .smpr = {
         ADC_SMPR1_SMP_AN5(ADC_SMPR_SMP_12P5), 0
     },
     .sqr = {
         ADC_SQR1_SQ1_N(ADC_CHANNEL_IN5),
         0, 0, 0
-    }
+    },
 };
 
-const GPTConfig ADC::m_timer_config = {
-    .frequency = 36000000,
-    .callback = nullptr,
-    .cr2 = TIM_CR2_MMS_1, /* TRGO */
-    .dier = 0
+static bool readAltDone = false;
+static void readAltCallback(ADCDriver *)
+{
+    readAltDone = true;
+}
+ADCConversionGroup ADC::m_group_config2 = {
+    .circular = false,
+    .num_channels = 1,
+    .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
+#if defined(TARGET_PLATFORM_H7)
+    .ccr = 0,
+    .pcsel = 0,
+    .ltr1 = 0, .htr1 = 4095,
+    .ltr2 = 0, .htr2 = 4095,
+    .ltr3 = 0, .htr3 = 4095,
+#else
+    .tr1 = ADC_TR(0, 4095),
+    .tr2 = ADC_TR(0, 4095),
+    .tr3 = ADC_TR(0, 4095),
+    .awd2cr = 0,
+    .awd3cr = 0,
+#endif
+    .smpr = {
+        ADC_SMPR1_SMP_AN1(ADC_SMPR_SMP_12P5), 0
+    },
+    .sqr = {
+        ADC_SQR1_SQ1_N(ADC_CHANNEL_IN1),
+        0, 0, 0
+    },
 };
 
-std::array<std::array<uint32_t, 4>, 6> ADC::m_rate_presets = {{
-     // Rate   PLLSAI2N  R  OVERSAMPLE 2x?  GPT_DIV
-    {/* 8k  */ 16,       3, 1,              4500},
-    {/* 16k */ 32,       3, 1,              2250},
-    {/* 20k */ 40,       3, 1,              1800},
-    {/* 32k */ 64,       3, 1,              1125},
-    {/* 48k */ 24,       3, 0,               750},
-    {/* 96k */ 48,       3, 0,               375}
-}};
-
 adcsample_t *ADC::m_current_buffer = nullptr;
 size_t ADC::m_current_buffer_size = 0;
 ADC::Operation ADC::m_operation = nullptr;
 
-unsigned int ADC::m_timer_divisor = 2;
-
 void ADC::begin()
 {
-    palSetPadMode(GPIOA, 0, PAL_MODE_INPUT_ANALOG);
+#if defined(TARGET_PLATFORM_H7)
+    palSetPadMode(GPIOF, 3, PAL_MODE_INPUT_ANALOG);
+#else
+    palSetPadMode(GPIOA, 0, PAL_MODE_INPUT_ANALOG); // Algorithm in
+    palSetPadMode(GPIOC, 0, PAL_MODE_INPUT_ANALOG); // Potentiometer 1
+#endif
 
     adcStart(m_driver, &m_config);
-    adcSTM32EnableVREF(m_driver);
-    gptStart(m_timer, &m_timer_config);
-
-    setRate(Rate::R32K);
+    adcStart(m_driver2, &m_config2);
 }
 
 void ADC::start(adcsample_t *buffer, size_t count, Operation operation)
@@ -76,12 +120,12 @@ void ADC::start(adcsample_t *buffer, size_t count, Operation operation)
     m_operation = operation;
 
     adcStartConversion(m_driver, &m_group_config, buffer, count);
-    gptStartContinuous(m_timer, m_timer_divisor);
+    SClock::start();
 }
 
 void ADC::stop()
 {
-    gptStopTimer(m_timer);
+    SClock::stop();
     adcStopConversion(m_driver);
 
     m_current_buffer = nullptr;
@@ -89,15 +133,67 @@ void ADC::stop()
     m_operation = nullptr;
 }
 
-void ADC::setRate(ADC::Rate rate)
+adcsample_t ADC::readAlt(unsigned int id)
 {
+    if (id != 0)
+        return 0;
+    static adcsample_t result[32] = {};
+    readAltDone = false;
+    adcStartConversion(m_driver2, &m_group_config2, result, 32);
+    while (!readAltDone)
+        ;
+    adcStopConversion(m_driver2);
+    return result[0];
+}
+
+void ADC::setRate(SClock::Rate rate)
+{
+#if defined(TARGET_PLATFORM_H7)
+    std::array<std::array<uint32_t, 2>, 6> m_rate_presets = {{
+         // Rate   PLL N  PLL P
+        {/* 8k  */ 80,    20},
+        {/* 16k */ 80,    10},
+        {/* 20k */ 80,    8},
+        {/* 32k */ 80,    5},
+        {/* 48k */ 96,    4},
+        {/* 96k */ 288,   10}
+    }};
+
+    auto& preset = m_rate_presets[static_cast<unsigned int>(rate)];
+    auto pllbits = (preset[0] << RCC_PLL2DIVR_N2_Pos) |
+                   (preset[1] << RCC_PLL2DIVR_P2_Pos);
+
+    adcStop(m_driver);
+
+    // Adjust PLL2
+    RCC->CR &= ~(RCC_CR_PLL2ON);
+    while ((RCC->CR & RCC_CR_PLL2RDY) == RCC_CR_PLL2RDY);
+    auto pll2divr = RCC->PLL2DIVR &
+                    ~(RCC_PLL2DIVR_N2_Msk | RCC_PLL2DIVR_P2_Msk);
+    pll2divr |= pllbits;
+    RCC->PLL2DIVR = pll2divr;
+    RCC->CR |= RCC_CR_PLL2ON;
+    while ((RCC->CR & RCC_CR_PLL2RDY) != RCC_CR_PLL2RDY);
+
+    m_group_config.smpr[0] = rate != SClock::Rate::R96K ? ADC_SMPR1_SMP_AN5(ADC_SMPR_SMP_12P5)
+                                                        : ADC_SMPR1_SMP_AN5(ADC_SMPR_SMP_2P5);
+
+    adcStart(m_driver, &m_config);
+#elif defined(TARGET_PLATFORM_L4)
+    std::array<std::array<uint32_t, 3>, 6> m_rate_presets = {{
+        //  Rate   PLLSAI2N  R  OVERSAMPLE
+        {/* 8k  */ 16,       3, 1},
+        {/* 16k */ 32,       3, 1},
+        {/* 20k */ 40,       3, 1},
+        {/* 32k */ 64,       3, 1},
+        {/* 48k */ 24,       3, 0},
+        {/* 96k */ 48,       3, 0}
+    }};
+
     auto& preset = m_rate_presets[static_cast<int>(rate)];
     auto pllnr = (preset[0] << RCC_PLLSAI2CFGR_PLLSAI2N_Pos) |
                  (preset[1] << RCC_PLLSAI2CFGR_PLLSAI2R_Pos);
     bool oversample = preset[2] != 0;
-    m_timer_divisor = preset[3];
-
-    adcStop(m_driver);
 
     // Adjust PLLSAI2
     RCC->CR &= ~(RCC_CR_PLLSAI2ON);
@@ -108,8 +204,8 @@ void ADC::setRate(ADC::Rate rate)
 
     // Set 2x oversampling
     m_group_config.cfgr2 = oversample ? ADC_CFGR2_ROVSE | ADC_CFGR2_OVSR_0 | ADC_CFGR2_OVSS_1 : 0;
-
-    adcStart(m_driver, &m_config);
+    m_group_config2.cfgr2 = oversample ? ADC_CFGR2_ROVSE | ADC_CFGR2_OVSR_0 | ADC_CFGR2_OVSS_1 : 0;
+#endif
 }
 
 void ADC::setOperation(ADC::Operation operation)
@@ -117,21 +213,6 @@ void ADC::setOperation(ADC::Operation operation)
     m_operation = operation;
 }
 
-int ADC::getRate()
-{
-    for (unsigned int i = 0; i < m_rate_presets.size(); i++) {
-        if (m_timer_divisor == m_rate_presets[i][3])
-            return i;
-    }
-
-    return -1;
-}
-
-unsigned int ADC::getTimerDivisor()
-{
-    return m_timer_divisor;
-}
- 
 void ADC::conversionCallback(ADCDriver *driver)
 {
     if (m_operation != nullptr) {
diff --git a/source/adc.hpp b/source/adc.hpp
index 5f9dc23..24a7fff 100644
--- a/source/adc.hpp
+++ b/source/adc.hpp
@@ -13,6 +13,7 @@
 #define STMDSP_ADC_HPP_
 
 #include "hal.h"
+#include "sclock.hpp"
 
 #include <array>
 
@@ -21,42 +22,30 @@ class ADC
 public:
     using Operation = void (*)(adcsample_t *buffer, size_t count);
 
-    enum class Rate : int {
-        R8K = 0,
-        R16K,
-        R20K,
-        R32K,
-        R48K,
-        R96K
-    };
-    
     static void begin();
 
     static void start(adcsample_t *buffer, size_t count, Operation operation);
     static void stop();
 
-    static void setRate(Rate rate);
-    static void setOperation(Operation operation);
+    static adcsample_t readAlt(unsigned int id);
 
-    static int getRate();
-    static unsigned int getTimerDivisor();
+    static void setRate(SClock::Rate rate);
+    static void setOperation(Operation operation);
 
 private:
     static ADCDriver *m_driver;
-    static GPTDriver *m_timer;
+    static ADCDriver *m_driver2;
 
     static const ADCConfig m_config;
-    static /*const*/ ADCConversionGroup m_group_config;
-    static const GPTConfig m_timer_config;
-
-    static std::array<std::array<uint32_t, 4>, 6> m_rate_presets;
+    static const ADCConfig m_config2;
+    static ADCConversionGroup m_group_config;
+    static ADCConversionGroup m_group_config2;
 
     static adcsample_t *m_current_buffer;
     static size_t m_current_buffer_size;
     static Operation m_operation;
 
-    static unsigned int m_timer_divisor;
-
+public:
     static void conversionCallback(ADCDriver *);
 };
 
diff --git a/source/common.hpp b/source/common.hpp
deleted file mode 100644
index 1045b32..0000000
--- a/source/common.hpp
+++ /dev/null
@@ -1,56 +0,0 @@
-#include <array>
-#include <cstdint>
-
-constexpr unsigned int MAX_SAMPLE_BUFFER_SIZE = 6000;
-
-using Sample = uint16_t;
-
-class SampleBuffer
-{
-public:
-    void clear() {
-        m_buffer.fill(0);
-    }
-    void modify(Sample *data, unsigned int srcsize) {
-        auto size = srcsize < m_size ? srcsize : m_size;
-        std::copy(data, data + size, m_buffer.data());
-        m_modified = m_buffer.data();
-    }
-    void midmodify(Sample *data, unsigned int srcsize) {
-        auto size = srcsize < m_size / 2 ? srcsize : m_size / 2;
-        std::copy(data, data + size, middata());
-        m_modified = middata();
-    }
-
-    void setSize(unsigned int size) {
-        m_size = size < MAX_SAMPLE_BUFFER_SIZE ? size : MAX_SAMPLE_BUFFER_SIZE;
-    }
-
-    Sample *data() /*const*/ {
-        return m_buffer.data();
-    }
-    Sample *middata() /*const*/ {
-        return m_buffer.data() + m_size / 2;
-    }
-    uint8_t *bytedata() /*const*/ {
-        return reinterpret_cast<uint8_t *>(m_buffer.data());
-    }
-
-    Sample *modified() {
-        auto m = m_modified;
-        m_modified = nullptr;
-        return m;
-    }
-    unsigned int size() const {
-        return m_size;
-    }
-    unsigned int bytesize() const {
-        return m_size * sizeof(Sample);
-    }
-
-private:
-    std::array<Sample, MAX_SAMPLE_BUFFER_SIZE> m_buffer;
-    unsigned int m_size = MAX_SAMPLE_BUFFER_SIZE;
-    Sample *m_modified = nullptr;
-};
-
diff --git a/source/cordic.cpp b/source/cordic.cpp
new file mode 100644
index 0000000..29ee068
--- /dev/null
+++ b/source/cordic.cpp
@@ -0,0 +1,113 @@
+#include "cordic.hpp"
+#include "hal.h"
+
+#if !defined(TARGET_PLATFORM_L4)
+namespace cordic {
+
+void init()
+{
+    RCC->AHB2ENR |= RCC_AHB2ENR_CORDICEN;
+}
+
+static void prepare() {
+    while (CORDIC->CSR & CORDIC_CSR_RRDY)
+        asm("mov r0, %0" :: "r" (CORDIC->RDATA));
+}
+
+static uint32_t dtoq(double) {
+    uint32_t res;
+    asm("vcvt.s32.f64 d0, d0, #31;"
+        "vmov %0, r5, d0"
+    : "=r" (res));
+    return res;
+}
+__attribute__((naked))
+static double qtod(uint32_t) {
+    asm("eor r1, r1;"
+        "vmov d0, r0, r1;"
+        "vcvt.f64.s32 d0, d0, #31;"
+        "bx lr");
+    return 0;
+}
+__attribute__((naked))
+double mod(double, double) {
+    asm("vdiv.f64   d2, d0, d1;"
+        "vrintz.f64 d2;"
+        "vmul.f64   d1, d1, d2;"
+        "vsub.f64   d0, d0, d1;"
+        "bx lr");
+    return 0;
+}
+
+double cos(double x) {
+    x = mod(x, 2 * math::PI) / math::PI;
+    auto input = dtoq(x > 1. ? x - 2 : x);
+
+    prepare();
+    CORDIC->CSR = CORDIC_CSR_NARGS | CORDIC_CSR_NRES |
+                  (6 << CORDIC_CSR_PRECISION_Pos) |
+                  (0 << CORDIC_CSR_FUNC_Pos);
+
+    CORDIC->WDATA = input;
+    CORDIC->WDATA = input;
+    while (!(CORDIC->CSR & CORDIC_CSR_RRDY));
+
+    double cosx = qtod(CORDIC->RDATA) / x;
+    [[maybe_unused]] auto sinx = CORDIC->RDATA;
+    return cosx;
+}
+
+double sin(double x) {
+    x = mod(x, 2 * math::PI) / math::PI;
+    auto input = dtoq(x > 1. ? x - 2 : x);
+
+    prepare();
+    CORDIC->CSR = CORDIC_CSR_NARGS | CORDIC_CSR_NRES |
+                  (6 << CORDIC_CSR_PRECISION_Pos) |
+                  (1 << CORDIC_CSR_FUNC_Pos);
+
+    CORDIC->WDATA = input;
+    CORDIC->WDATA = input;
+    while (!(CORDIC->CSR & CORDIC_CSR_RRDY));
+
+    double sinx = qtod(CORDIC->RDATA) / x;
+    [[maybe_unused]] auto cosx = CORDIC->RDATA;
+    return sinx;
+}
+
+double tan(double x) {
+    x = mod(x, 2 * math::PI) / math::PI;
+    auto input = dtoq(x > 1. ? x - 2 : x);
+
+    prepare();
+    CORDIC->CSR = CORDIC_CSR_NARGS | CORDIC_CSR_NRES |
+                  (6 << CORDIC_CSR_PRECISION_Pos) |
+                  (1 << CORDIC_CSR_FUNC_Pos);
+
+    CORDIC->WDATA = input;
+    CORDIC->WDATA = input;
+    while (!(CORDIC->CSR & CORDIC_CSR_RRDY));
+
+    double sinx = qtod(CORDIC->RDATA) / x;
+    double tanx = sinx * x / qtod(CORDIC->RDATA);
+    return tanx;
+}
+
+}
+#else // L4
+#include <cmath>
+namespace cordic {
+
+void init() {}
+
+float mod(float a, float b) {
+    return a - (b * std::floor(a / b));
+}
+
+float cos(float x) { return std::cos(x); }
+float sin(float x) { return std::sin(x); }
+float tan(float x) { return std::tan(x); }
+
+}
+#endif
+
diff --git a/source/cordic.hpp b/source/cordic.hpp
new file mode 100644
index 0000000..5d640cc
--- /dev/null
+++ b/source/cordic.hpp
@@ -0,0 +1,25 @@
+#ifndef CORDIC_HPP_
+#define CORDIC_HPP_
+
+namespace cordic {
+    constexpr double PI = 3.1415926535L;
+
+    void init();
+
+#if !defined(TARGET_PLATFORM_L4)
+    double mod(double n, double d);
+
+    double cos(double x);
+    double sin(double x);
+    double tan(double x);
+#else
+    float mod(float n, float d);
+
+    float cos(float x);
+    float sin(float x);
+    float tan(float x);
+#endif
+}
+
+#endif // CORDIC_HPP_
+
diff --git a/source/dac.cpp b/source/dac.cpp
index ed08461..2116dcb 100644
--- a/source/dac.cpp
+++ b/source/dac.cpp
@@ -9,14 +9,12 @@
  * If not, see <https://www.gnu.org/licenses/>.
  */
 
-#include "adc.hpp" // ADC::getTimerDivisor
 #include "dac.hpp"
+#include "sclock.hpp"
 
 DACDriver *DAC::m_driver[2] = {
     &DACD1, &DACD2
 };
-GPTDriver *DAC::m_timer = &GPTD7;
-int DAC::m_timer_user_count = 0;
 
 const DACConfig DAC::m_config = {
     .init = 0,
@@ -28,14 +26,7 @@ const DACConversionGroup DAC::m_group_config = {
     .num_channels = 1,
     .end_cb = nullptr,
     .error_cb = nullptr,
-    .trigger = DAC_TRG(2)
-};
-
-const GPTConfig DAC::m_timer_config = {
-    .frequency = 36000000,
-    .callback = nullptr,
-    .cr2 = TIM_CR2_MMS_1, /* TRGO */
-    .dier = 0
+    .trigger = 5 // TIM6_TRGO
 };
 
 void DAC::begin()
@@ -45,17 +36,13 @@ void DAC::begin()
 
     dacStart(m_driver[0], &m_config);
     dacStart(m_driver[1], &m_config);
-    gptStart(m_timer, &m_timer_config);
 }
 
 void DAC::start(int channel, dacsample_t *buffer, size_t count)
 {
     if (channel >= 0 && channel < 2) {
         dacStartConversion(m_driver[channel], &m_group_config, buffer, count);
-
-        if (m_timer_user_count == 0)
-            gptStartContinuous(m_timer, ADC::getTimerDivisor());
-        m_timer_user_count++;
+        SClock::start();
     }
 }
 
@@ -63,9 +50,7 @@ void DAC::stop(int channel)
 {
     if (channel >= 0 && channel < 2) {
         dacStopConversion(m_driver[channel]);
-
-        if (--m_timer_user_count == 0)
-            gptStopTimer(m_timer);
+        SClock::stop();
     }
 }
 
diff --git a/source/dac.hpp b/source/dac.hpp
index 542b4a1..e305c4b 100644
--- a/source/dac.hpp
+++ b/source/dac.hpp
@@ -25,12 +25,9 @@ public:
 
 private:
     static DACDriver *m_driver[2];
-    static GPTDriver *m_timer;
-    static int m_timer_user_count;
 
     static const DACConfig m_config;
     static const DACConversionGroup m_group_config;
-    static const GPTConfig m_timer_config;
 };
 
 #endif // STMDSP_DAC_HPP_
diff --git a/source/error.hpp b/source/error.hpp
index 699c746..6911792 100644
--- a/source/error.hpp
+++ b/source/error.hpp
@@ -27,6 +27,10 @@ public:
         return condition;
     }
 
+    bool hasError() {
+        return m_index > 0;
+    }
+
     Error pop() {
         return m_index == 0 ? Error::None : m_queue[--m_index];
     }
diff --git a/source/main.cpp b/source/main.cpp
index b77bd2f..0ec69b8 100644
--- a/source/main.cpp
+++ b/source/main.cpp
@@ -15,22 +15,24 @@
 static_assert(sizeof(adcsample_t) == sizeof(uint16_t));
 static_assert(sizeof(dacsample_t) == sizeof(uint16_t));
 
-#include "common.hpp"
-#include "error.hpp"
-
 #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 = 8 * 1024;
+constexpr unsigned int MAX_ELF_FILE_SIZE = 16 * 1024;
 
 enum class RunStatus : char
 {
     Idle = '1',
-    Running
+    Running,
+    Recovering
 };
 static RunStatus run_status = RunStatus::Idle;
 
@@ -42,26 +44,70 @@ static RunStatus run_status = RunStatus::Idle;
 #define MSG_FOR_FIRST(m)   (m & 1)
 #define MSG_FOR_MEASURE(m) (m > 2)
 
-static msg_t conversionMBBuffer[4];
-static MAILBOX_DECL(conversionMB, conversionMBBuffer, 4);
+static ErrorManager EM;
 
-static THD_WORKING_AREA(conversionThreadWA, 2048);
+static msg_t conversionMBBuffer[2];
+static MAILBOX_DECL(conversionMB, conversionMBBuffer, 2);
+
+// Thread for LED status and wakeup hold
+#if defined(TARGET_PLATFORM_H7)
+__attribute__((section(".stacks")))
+static THD_WORKING_AREA(monitorThreadWA, 1024);
+static THD_FUNCTION(monitorThread, arg);
+#endif
+
+// 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);
 
 static time_measurement_t conversion_time_measurement;
-
-static ErrorManager EM;
-
-static SampleBuffer samplesIn;
-static SampleBuffer samplesOut;
-static SampleBuffer samplesSigGen;
+#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
 
 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 conversion_abort();
 static void signal_operate(adcsample_t *buffer, size_t count);
 static void signal_operate_measure(adcsample_t *buffer, size_t count);
-static void main_loop();
 
 int main()
 {
@@ -69,29 +115,48 @@ int main()
     halInit();
     chSysInit();
 
-    // Enable FPU
-    SCB->CPACR |= 0xF << 20;
-
-    // Prepare LED
-    palSetPadMode(GPIOA, 5,  PAL_MODE_OUTPUT_PUSHPULL);
-    palClearPad(GPIOA, 5);
+    SCB->CPACR |= 0xF << 20; // Enable FPU
+    mpu_setup();
 
+    palSetLineMode(LINE_BUTTON, PAL_MODE_INPUT);
     ADC::begin();
     DAC::begin();
+    SClock::begin();
     USBSerial::begin();
+    cordic::init();
 
-    // Start the conversion manager thread
-    chTMObjectInit(&conversion_time_measurement);
-    chThdCreateStatic(conversionThreadWA, sizeof(conversionThreadWA),
-                      NORMALPRIO,
-                      conversionThread, nullptr);
+    SClock::setRate(SClock::Rate::R32K);
+    ADC::setRate(SClock::Rate::R32K);
 
-    main_loop();
+    chTMObjectInit(&conversion_time_measurement);
+#if defined(TARGET_PLATFORM_H7)
+    chThdCreateStatic(
+        monitorThreadWA, sizeof(monitorThreadWA),
+        LOWPRIO,
+        monitorThread, nullptr);
+#endif
+    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);
+
+    chThdExit(0);
+    return 0;
 }
 
-void main_loop()
+THD_FUNCTION(communicationThread, arg)
 {
-
+    (void)arg;
 	while (1) {
         if (USBSerial::isActive()) {
             // Attempt to receive a command packet
@@ -99,6 +164,25 @@ void main_loop()
                 // 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;
@@ -110,6 +194,8 @@ void main_loop()
                     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);
@@ -154,7 +240,11 @@ void main_loop()
 
                 // 'i' - Sends an identifying string to confirm that this is the stmdsp device.
                 case 'i':
-                    USBSerial::write(reinterpret_cast<const uint8_t *>("stmdsp"), 6);
+#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.
@@ -200,10 +290,12 @@ void main_loop()
                 case 'r':
                     if (EM.assert(USBSerial::read(&cmd[1], 1) == 1, Error::BadParamSize)) {
                         if (cmd[1] == 0xFF) {
-                            unsigned char r = static_cast<unsigned char>(ADC::getRate());
+                            unsigned char r = SClock::getRate();
                             USBSerial::write(&r, 1);
                         } else {
-                            ADC::setRate(static_cast<ADC::Rate>(cmd[1]));
+                            auto r = static_cast<SClock::Rate>(cmd[1]);
+                            SClock::setRate(r);
+                            ADC::setRate(r);
                         }
                     }
                     break;
@@ -239,6 +331,28 @@ void main_loop()
                         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());
@@ -257,21 +371,76 @@ void main_loop()
 	}
 }
 
-void conversion_abort()
+THD_FUNCTION(conversionThreadMonitor, arg)
+{
+    (void)arg;
+    while (1) {
+        // Recover from algorithm fault if necessary
+        //if (run_status == RunStatus::Recovering)
+        //    conversion_abort();
+
+        msg_t message;
+        if (chMBFetchTimeout(&conversionMB, &message, TIME_INFINITE) == MSG_OK)
+            chMsgSend(conversionThreadHandle, message);
+    }
+}
+
+THD_FUNCTION(conversionThread, stack)
 {
     elf_entry = nullptr;
-    DAC::stop(0);
-    ADC::stop();
-    EM.add(Error::ConversionAborted);
+    port_unprivileged_jump(reinterpret_cast<uint32_t>(conversion_unprivileged_main),
+                           reinterpret_cast<uint32_t>(stack));
 }
 
-THD_FUNCTION(conversionThread, arg)
+#if defined(TARGET_PLATFORM_H7)
+THD_FUNCTION(monitorThread, arg)
 {
     (void)arg;
 
+    palSetLineMode(LINE_BUTTON, PAL_MODE_INPUT_PULLUP);
+
+    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 && palReadLine(LINE_BUTTON)) {
+            palSetLine(LINE_LED_RED);
+            palSetLine(LINE_LED_YELLOW);
+            chSysLock();
+            while (palReadLine(LINE_BUTTON))
+                asm("nop");
+            while (!palReadLine(LINE_BUTTON))
+                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);
+        }
+    }
+}
+#endif
+
+void conversion_unprivileged_main()
+{
     while (1) {
         msg_t message;
-        if (chMBFetchTimeout(&conversionMB, &message, TIME_INFINITE) == MSG_OK) {
+        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;
 
@@ -279,9 +448,9 @@ THD_FUNCTION(conversionThread, arg)
                 if (!MSG_FOR_MEASURE(message)) {
                     samples = elf_entry(samples, size);
                 } else {
-                    chTMStartMeasurementX(&conversion_time_measurement);
+                    asm("eor r0, r0; svc 2"); // start measurement
                     samples = elf_entry(samples, size);
-                    chTMStopMeasurementX(&conversion_time_measurement);
+                    asm("mov r0, #1; svc 2"); // stop measurement
                 } 
             }
 
@@ -293,27 +462,176 @@ THD_FUNCTION(conversionThread, arg)
     }
 }
 
-void signal_operate(adcsample_t *buffer, [[maybe_unused]] size_t count)
+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 conversion_abort()
 {
-    if (chMBGetUsedCountI(&conversionMB) > 1)
+    elf_entry = nullptr;
+    DAC::stop(0);
+    ADC::stop();
+    EM.add(Error::ConversionAborted);
+
+    chMBReset(&conversionMB);
+    run_status = RunStatus::Idle;
+}
+
+void signal_operate(adcsample_t *buffer, size_t)
+{
+    chSysLockFromISR();
+
+    if (chMBGetUsedCountI(&conversionMB) > 1) {
+        chSysUnlockFromISR();
         conversion_abort();
-    else
-        chMBPostI(&conversionMB, buffer == samplesIn.data() ? MSG_CONVFIRST : MSG_CONVSECOND);
+    } 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)
 {
-    chMBPostI(&conversionMB, buffer == samplesIn.data() ? MSG_CONVFIRST_MEASURE : MSG_CONVSECOND_MEASURE);
+    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()
+{
+    while (1);
+}
+
+__attribute__((naked))
 void HardFault_Handler()
 {
-    //asm("push {lr}");
+    // Below not working (yet)
+    while (1);
 
+    // 1. Get the stack pointer
     uint32_t *stack;
     uint32_t lr;
 	asm("\
@@ -323,21 +641,21 @@ void HardFault_Handler()
 		mrsne %0, psp; \
         mov %1, lr; \
 	" : "=r" (stack), "=r" (lr));
-    //stack++;
-    stack[7] |= (1 << 24); // Keep Thumb mode enabled
 
-    conversion_abort();
+    // 2. Only attempt to recover from failed algorithm code
+    if ((lr & 4) == 0 || run_status != RunStatus::Running)
+        while (1);
 
-    // TODO test lr and decide how to recover
+    // 3. Post the failure and unload algorithm
+    elf_entry = nullptr;
+    EM.add(Error::ConversionAborted);
+    run_status = RunStatus::Recovering;
 
-    //if (run_status == RunStatus::Converting) {
-        stack[6] = stack[5];   // Escape from elf_entry code
-    //} else /*if (run_status == RunStatus::Recovered)*/ {
-    //    stack[6] = (uint32_t)main_loop & ~1; // Return to safety
-    //}
+    // 4. Make this exception return to point after algorithm exec.
+    stack[6] = stack[5];
+    stack[7] |= (1 << 24); // Ensure Thumb mode stays enabled
 
-    //asm("pop {lr}; bx lr");
-    asm("bx lr");
+    asm("mov lr, %0; bx lr" :: "r" (lr));
 }
 
 } // extern "C"
diff --git a/source/samplebuffer.cpp b/source/samplebuffer.cpp
new file mode 100644
index 0000000..24cc424
--- /dev/null
+++ b/source/samplebuffer.cpp
@@ -0,0 +1,60 @@
+#include "samplebuffer.hpp"
+
+SampleBuffer::SampleBuffer(Sample *buffer) :
+    m_buffer(buffer) {}
+
+void SampleBuffer::clear() {
+    std::fill(m_buffer, m_buffer + m_size, 2048);
+}
+__attribute__((section(".convcode")))
+void SampleBuffer::modify(Sample *data, unsigned int srcsize) {
+    auto size = srcsize < m_size ? srcsize : m_size;
+    m_modified = m_buffer;
+    for (Sample *d = m_buffer, *s = data; s != data + size;)
+        *d++ = *s++;
+}
+__attribute__((section(".convcode")))
+void SampleBuffer::midmodify(Sample *data, unsigned int srcsize) {
+    auto size = srcsize < m_size / 2 ? srcsize : m_size / 2;
+    m_modified = middata();
+    for (Sample *d = middata(), *s = data; s != data + size;)
+        *d++ = *s++;
+}
+
+void SampleBuffer::setModified() {
+    m_modified = m_buffer;
+}
+
+void SampleBuffer::setMidmodified() {
+    m_modified = middata();
+}
+
+void SampleBuffer::setSize(unsigned int size) {
+    m_size = size < MAX_SAMPLE_BUFFER_SIZE ? size : MAX_SAMPLE_BUFFER_SIZE;
+}
+
+__attribute__((section(".convcode")))
+Sample *SampleBuffer::data() {
+    return m_buffer;
+}
+__attribute__((section(".convcode")))
+Sample *SampleBuffer::middata() {
+    return m_buffer + m_size / 2;
+}
+uint8_t *SampleBuffer::bytedata() {
+    return reinterpret_cast<uint8_t *>(m_buffer);
+}
+
+Sample *SampleBuffer::modified() {
+    auto m = m_modified;
+    m_modified = nullptr;
+    return m;
+}
+__attribute__((section(".convcode")))
+unsigned int SampleBuffer::size() const {
+    return m_size;
+}
+unsigned int SampleBuffer::bytesize() const {
+    return m_size * sizeof(Sample);
+}
+
diff --git a/source/samplebuffer.hpp b/source/samplebuffer.hpp
new file mode 100644
index 0000000..6d17d2a
--- /dev/null
+++ b/source/samplebuffer.hpp
@@ -0,0 +1,40 @@
+#ifndef SAMPLEBUFFER_HPP_
+#define SAMPLEBUFFER_HPP_
+
+#include <array>
+#include <cstdint>
+
+using Sample = uint16_t;
+
+constexpr unsigned int MAX_SAMPLE_BUFFER_BYTESIZE = sizeof(Sample) * 8192;
+constexpr unsigned int MAX_SAMPLE_BUFFER_SIZE = MAX_SAMPLE_BUFFER_BYTESIZE / sizeof(Sample);
+
+class SampleBuffer
+{
+public:
+    SampleBuffer(Sample *buffer);
+
+    void clear();
+
+    void modify(Sample *data, unsigned int srcsize);
+    void midmodify(Sample *data, unsigned int srcsize);
+    void setModified();
+    void setMidmodified();
+    Sample *modified();
+
+    Sample *data();
+    Sample *middata();
+    uint8_t *bytedata();
+
+    void setSize(unsigned int size);
+    unsigned int size() const;
+    unsigned int bytesize() const;
+
+private:
+    Sample *m_buffer = nullptr;
+    unsigned int m_size = MAX_SAMPLE_BUFFER_SIZE;
+    Sample *m_modified = nullptr;
+};
+
+#endif // SAMPLEBUFFER_HPP_
+
diff --git a/source/sclock.cpp b/source/sclock.cpp
new file mode 100644
index 0000000..198c684
--- /dev/null
+++ b/source/sclock.cpp
@@ -0,0 +1,62 @@
+#include "sclock.hpp"
+
+GPTDriver *SClock::m_timer = &GPTD6;
+unsigned int SClock::m_div = 1;
+unsigned int SClock::m_runcount = 0;
+
+const GPTConfig SClock::m_timer_config = {
+#if defined(TARGET_PLATFORM_H7)
+    .frequency = 4800000,
+#else
+    .frequency = 36000000,
+#endif
+    .callback = nullptr,
+    .cr2 = TIM_CR2_MMS_1, /* TRGO */
+    .dier = 0
+};
+
+const std::array<unsigned int, 6> SClock::m_rate_divs = {{
+#if defined(TARGET_PLATFORM_H7)
+    /* 8k  */ 600,
+    /* 16k */ 300,
+    /* 20k */ 240,
+    /* 32k */ 150,
+    /* 48k */ 100,
+    /* 96k */ 50
+#else
+    4500, 2250, 1800, 1125, 750, 375
+#endif
+}};
+
+void SClock::begin()
+{
+    gptStart(m_timer, &m_timer_config);
+}
+
+void SClock::start()
+{
+    if (m_runcount++ == 0)
+        gptStartContinuous(m_timer, m_div);
+}
+
+void SClock::stop()
+{
+    if (--m_runcount == 0)
+        gptStopTimer(m_timer);
+}
+
+void SClock::setRate(SClock::Rate rate)
+{
+    m_div = m_rate_divs[static_cast<unsigned int>(rate)];
+}
+
+unsigned int SClock::getRate()
+{
+    for (unsigned int i = 0; i < m_rate_divs.size(); ++i) {
+        if (m_rate_divs[i] == m_div)
+            return i;
+    }
+
+    return static_cast<unsigned int>(-1);
+}
+
diff --git a/source/sclock.hpp b/source/sclock.hpp
new file mode 100644
index 0000000..960d9e3
--- /dev/null
+++ b/source/sclock.hpp
@@ -0,0 +1,36 @@
+#ifndef SCLOCK_HPP_
+#define SCLOCK_HPP_
+
+#include "hal.h"
+
+#include <array>
+
+class SClock
+{
+public:
+    enum class Rate : unsigned int {
+        R8K = 0,
+        R16K,
+        R20K,
+        R32K,
+        R48K,
+        R96K
+    };
+
+    static void begin();
+    static void start();
+    static void stop();
+
+    static void setRate(Rate rate);
+    static unsigned int getRate();
+
+private:
+    static GPTDriver *m_timer;
+    static unsigned int m_div;
+    static unsigned int m_runcount;
+    static const GPTConfig m_timer_config;
+    static const std::array<unsigned int, 6> m_rate_divs;
+};
+
+#endif // SCLOCK_HPP_
+
diff --git a/source/usbcfg.c b/source/usbcfg.c
index 4c5809a..b726e23 100644
--- a/source/usbcfg.c
+++ b/source/usbcfg.c
@@ -335,7 +335,11 @@ const USBConfig usbcfg = {
  * Serial over USB driver configuration.

  */

 const SerialUSBConfig serusbcfg = {

+#if defined(TARGET_PLATFORM_H7)

+  &USBD2,

+#else

   &USBD1,

+#endif

   USBD1_DATA_REQUEST_EP,

   USBD1_DATA_AVAILABLE_EP,

   USBD1_INTERRUPT_REQUEST_EP