error recv. draft; add trig/math funcs

4 files changed, 162 insertions, 50 deletions

diff --git a/Makefile b/Makefile
index a609c42..e560cfc 100644
--- a/Makefile
+++ b/Makefile
@@ -148,7 +148,7 @@ CPPWARN = -Wall -Wextra -Wundef
 #

 

 # List all user C define here, like -D_DEBUG=1

-UDEFS = -DCORTEX_ENABLE_WFI_IDLE=FALSE

+UDEFS = -DCORTEX_ENABLE_WFI_IDLE=FALSE -DPORT_USE_SYSCALL=TRUE

 

 # Define ASM defines here

 UADEFS =

diff --git a/source/cordic.cpp b/source/cordic.cpp
new file mode 100644
index 0000000..2a44015
--- /dev/null
+++ b/source/cordic.cpp
@@ -0,0 +1,94 @@
+#include "cordic.hpp"
+#include "hal.h"
+
+namespace math {
+
+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 in) {
+    double res = in * 0x7FFFFFFF;
+    int32_t resi = res;
+    return resi;
+}
+static double qtod(uint32_t in) {
+    int32_t ini = in;
+    double res = ini;
+    return res / 0x7FFFFFFF;
+}
+
+__attribute__((naked))
+double mod(double n, double) {
+    asm("vdiv.f64   d2, d0, d1;"
+        "vrintz.f64 d2;"
+        "vmul.f64   d1, d1, d2;"
+        "vsub.f64   d0, d0, d1;"
+        "bx lr");
+    return n;
+}
+
+double cos(double x) {
+    x = (mod(x, 2 * math::PI) - math::PI) / math::PI;
+    prepare();
+    CORDIC->CSR = CORDIC_CSR_NARGS | CORDIC_CSR_NRES |
+                  (6 << CORDIC_CSR_PRECISION_Pos) |
+                  (0 << CORDIC_CSR_FUNC_Pos);
+
+    auto in = dtoq(x);
+    CORDIC->WDATA = in;
+    CORDIC->WDATA = in & 0x7FFFFFFF;
+    while (!(CORDIC->CSR & CORDIC_CSR_RRDY));
+
+    double cosx = qtod(CORDIC->RDATA) / x;
+    in = CORDIC->RDATA;
+    return cosx;
+}
+
+double sin(double x) {
+    x = (mod(x, 2 * math::PI) - math::PI) / math::PI;
+    prepare();
+    CORDIC->CSR = CORDIC_CSR_NARGS | CORDIC_CSR_NRES |
+                  (6 << CORDIC_CSR_PRECISION_Pos) |
+                  (1 << CORDIC_CSR_FUNC_Pos);
+
+    auto in = dtoq(x);
+    CORDIC->WDATA = in;
+    CORDIC->WDATA = in & 0x7FFFFFFF;
+    while (!(CORDIC->CSR & CORDIC_CSR_RRDY));
+
+    double sinx = qtod(CORDIC->RDATA) / x;
+    in = CORDIC->RDATA;
+    return sinx;
+}
+
+double tan(double x) {
+    x = (mod(x, 2 * math::PI) - math::PI) / math::PI;
+    prepare();
+    CORDIC->CSR = CORDIC_CSR_NARGS | CORDIC_CSR_NRES |
+                  (6 << CORDIC_CSR_PRECISION_Pos) |
+                  (1 << CORDIC_CSR_FUNC_Pos);
+
+    auto in = dtoq(x);
+    CORDIC->WDATA = in;
+    CORDIC->WDATA = in & 0x7FFFFFFF;
+    while (!(CORDIC->CSR & CORDIC_CSR_RRDY));
+
+    double sinx = qtod(CORDIC->RDATA) / x;
+    double tanx = sinx * x / qtod(CORDIC->RDATA);
+    return tanx;
+}
+
+__attribute__((naked))
+double sqrt(double x) {
+    asm("vsqrt.f64 d0, d0; bx lr");
+    return x;
+}
+
+}
+
diff --git a/source/cordic.hpp b/source/cordic.hpp
new file mode 100644
index 0000000..755fddb
--- /dev/null
+++ b/source/cordic.hpp
@@ -0,0 +1,18 @@
+#ifndef CORDIC_HPP_
+#define CORDIC_HPP_
+
+namespace math {
+    constexpr double PI = 3.1415926535L;
+
+    void init();
+
+    double mod(double n, double d);
+
+    double cos(double x);
+    double sin(double x);
+    double tan(double x);
+    double sqrt(double x);
+}
+
+#endif // CORDIC_HPP_
+
diff --git a/source/main.cpp b/source/main.cpp
index 5afed63..a682cfe 100644
--- a/source/main.cpp
+++ b/source/main.cpp
@@ -19,6 +19,7 @@ static_assert(sizeof(dacsample_t) == sizeof(uint16_t));
 #include "error.hpp"
 
 #include "adc.hpp"
+#include "cordic.hpp"
 #include "dac.hpp"
 #include "elf_load.hpp"
 #include "sclock.hpp"
@@ -31,7 +32,8 @@ constexpr unsigned int MAX_ELF_FILE_SIZE = 8 * 1024;
 enum class RunStatus : char
 {
     Idle = '1',
-    Running
+    Running,
+    Recovering
 };
 static RunStatus run_status = RunStatus::Idle;
 
@@ -80,6 +82,7 @@ int main()
     DAC::begin();
     SClock::begin();
     USBSerial::begin();
+    math::init();
 
     SClock::setRate(SClock::Rate::R32K);
     ADC::setRate(SClock::Rate::R32K);
@@ -89,25 +92,6 @@ int main()
     chThdCreateStatic(conversionThreadWA, sizeof(conversionThreadWA),
                       NORMALPRIO, conversionThread, nullptr);
 
-    // TEST (success!)
-    /*double input = 0.25L;
-
-    RCC->AHB2ENR |= RCC_AHB2ENR_CORDICEN;
-    uint32_t dummy = 0;
-    while (CORDIC->CSR & CORDIC_CSR_RRDY)
-        dummy = CORDIC->RDATA;
-
-    CORDIC->CSR = (3 << CORDIC_CSR_PRECISION_Pos) |
-                  (9 << CORDIC_CSR_FUNC_Pos); // 1 arg, 3 iterations, sqrt
-
-    input *= 0x7FFFFFFF;
-    dummy = input;
-    CORDIC->WDATA = dummy;
-    while (!(CORDIC->CSR & CORDIC_CSR_RRDY));
-    // result: 
-    dummy = CORDIC->RDATA; // m cos()
-    double output = (double)dummy / 0x7FFFFFFF;*/
-
     main_loop();
 }
 
@@ -287,6 +271,9 @@ void conversion_abort()
     DAC::stop(0);
     ADC::stop();
     EM.add(Error::ConversionAborted);
+
+    chMBReset(&conversionMB);
+    run_status = RunStatus::Idle;
 }
 
 THD_FUNCTION(conversionThread, arg)
@@ -294,14 +281,26 @@ THD_FUNCTION(conversionThread, 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) {
-            auto samples = MSG_FOR_FIRST(message) ? samplesIn.data() : samplesIn.middata();
-            auto size = samplesIn.size() / 2;
+            static Sample *samples = nullptr;
+            static unsigned int size = 0;
+            samples = MSG_FOR_FIRST(message) ? samplesIn.data() : samplesIn.middata();
+            size = samplesIn.size() / 2;
 
             if (elf_entry) {
                 if (!MSG_FOR_MEASURE(message)) {
-                    samples = elf_entry(samples, size);
+                    //asm("cpsid i");
+                    //mpuDisable();
+                    //port_unprivileged_jump((uint32_t)+[] {
+                        samples = elf_entry(samples, size);
+                    //}, 0xF800);
+                    //mpuEnable(MPU_CTRL_PRIVDEFENA);
+                    //asm("cpsie i");
                 } else {
                     chTMStartMeasurementX(&conversion_time_measurement);
                     samples = elf_entry(samples, size);
@@ -369,33 +368,34 @@ extern "C" {
 __attribute__((naked))
 void HardFault_Handler()
 {
+    // Below not working (yet)
     while (1);
-//    //asm("push {lr}");
-//
-//    uint32_t *stack;
-//    uint32_t lr;
-//	asm("\
-//		tst lr, #4; \
-//		ite eq; \
-//		mrseq %0, msp; \
-//		mrsne %0, psp; \
-//        mov %1, lr; \
-//	" : "=r" (stack), "=r" (lr));
-//    //stack++;
-//    stack[7] |= (1 << 24); // Keep Thumb mode enabled
-//
-//    conversion_abort();
-//
-//    // TODO test lr and decide how to recover
-//
-//    //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
-//    //}
-//
-//    //asm("pop {lr}; bx lr");
-//    asm("bx lr");
+
+    // 1. Get the stack pointer
+    uint32_t *stack;
+    uint32_t lr;
+	asm("\
+		tst lr, #4; \
+		ite eq; \
+		mrseq %0, msp; \
+		mrsne %0, psp; \
+        mov %1, lr; \
+	" : "=r" (stack), "=r" (lr));
+
+    // 2. Only attempt to recover from failed algorithm code
+    if ((lr & 4) == 0 || run_status != RunStatus::Running)
+        while (1);
+
+    // 3. Post the failure and unload algorithm
+    elf_entry = nullptr;
+    EM.add(Error::ConversionAborted);
+    run_status = RunStatus::Recovering;
+
+    // 4. Make this exception return to point after algorithm exec.
+    stack[6] = stack[5];
+    stack[7] |= (1 << 24); // Ensure Thumb mode stays enabled
+
+    asm("mov lr, %0; bx lr" :: "r" (lr));
 }
 
 } // extern "C"