Merge pull request #3 from tcsullivan/stm32h7

Stm32h7

1 files changed, 375 insertions, 57 deletions

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"