From 123cc4c756cc8a22f66351ab65595c5a20e53e27 Mon Sep 17 00:00:00 2001 From: Clyne Sullivan Date: Sat, 31 Jul 2021 10:47:00 -0400 Subject: [PATCH] reorganized source, wip --- Makefile | 7 +- openocd.cfg | 2 +- source/board/board_h7.c | 21 + source/board/board_l4.c | 23 + source/board/l4/board.h | 4 + source/communication.cpp | 291 +++++++++++ source/communication.hpp | 29 ++ source/conversion.cpp | 206 ++++++++ source/conversion.hpp | 67 +++ source/{elf_format.hpp => elf.h} | 8 +- source/elf_load.hpp | 26 - source/{elf_load.cpp => elfload.cpp} | 20 +- source/elfload.hpp | 48 ++ source/error.cpp | 38 ++ source/error.hpp | 44 +- source/handlers.cpp | 138 ++++++ source/handlers.hpp | 31 ++ source/main.cpp | 712 +-------------------------- source/monitor.cpp | 80 +++ source/monitor.hpp | 29 ++ source/{ => periph}/adc.cpp | 6 +- source/{ => periph}/adc.hpp | 0 source/{ => periph}/cordic.cpp | 0 source/{ => periph}/cordic.hpp | 0 source/{ => periph}/dac.cpp | 0 source/{ => periph}/dac.hpp | 0 source/{ => periph}/usbcfg.c | 0 source/{ => periph}/usbcfg.h | 0 source/{ => periph}/usbserial.cpp | 0 source/{ => periph}/usbserial.hpp | 0 source/runstatus.hpp | 11 + source/samples.cpp | 35 ++ source/samples.hpp | 26 + 33 files changed, 1137 insertions(+), 765 deletions(-) create mode 100644 source/communication.cpp create mode 100644 source/communication.hpp create mode 100644 source/conversion.cpp create mode 100644 source/conversion.hpp rename source/{elf_format.hpp => elf.h} (94%) delete mode 100644 source/elf_load.hpp rename source/{elf_load.cpp => elfload.cpp} (79%) create mode 100644 source/elfload.hpp create mode 100644 source/error.cpp create mode 100644 source/handlers.cpp create mode 100644 source/handlers.hpp create mode 100644 source/monitor.cpp create mode 100644 source/monitor.hpp rename source/{ => periph}/adc.cpp (98%) rename source/{ => periph}/adc.hpp (100%) rename source/{ => periph}/cordic.cpp (100%) rename source/{ => periph}/cordic.hpp (100%) rename source/{ => periph}/dac.cpp (100%) rename source/{ => periph}/dac.hpp (100%) rename source/{ => periph}/usbcfg.c (100%) rename source/{ => periph}/usbcfg.h (100%) rename source/{ => periph}/usbserial.cpp (100%) rename source/{ => periph}/usbserial.hpp (100%) create mode 100644 source/runstatus.hpp create mode 100644 source/samples.cpp create mode 100644 source/samples.hpp diff --git a/Makefile b/Makefile index bec5309..e308e1e 100644 --- a/Makefile +++ b/Makefile @@ -128,8 +128,8 @@ include $(CHIBIOS)/os/rt/rt.mk include $(CHIBIOS)/os/common/ports/ARMCMx/compilers/GCC/mk/port_v7m.mk # Auto-build files in ./source recursively. #include $(CHIBIOS)/tools/mk/autobuild.mk -ALLCSRC += $(wildcard source/*.c) -ALLCPPSRC += $(wildcard source/*.cpp) +ALLCSRC += $(wildcard source/*.c) $(wildcard source/periph/*.c) +ALLCPPSRC += $(wildcard source/*.cpp) $(wildcard source/periph/*.cpp) ALLASMSRC += $(wildcard source/*.s) # Other files (optional). #include $(CHIBIOS)/test/lib/test.mk @@ -158,7 +158,8 @@ ASMSRC = $(ALLASMSRC) ASMXSRC = $(ALLXASMSRC) # Inclusion directories. -INCDIR = $(CONFDIR) $(ALLINC) $(TESTINC) +INCDIR = $(CONFDIR) $(ALLINC) $(TESTINC) \ + source source/periph # Define C warning options here. CWARN = -Wall -Wextra -Wundef -Wstrict-prototypes -pedantic diff --git a/openocd.cfg b/openocd.cfg index 0599165..47e6c7d 100644 --- a/openocd.cfg +++ b/openocd.cfg @@ -1,3 +1,3 @@ source [find interface/stlink.cfg] -source [find target/stm32h7x.cfg] +source [find target/stm32l4x.cfg] 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..31d1d51 100644 --- a/source/board/board_l4.c +++ b/source/board/board_l4.c @@ -277,5 +277,28 @@ bool mmc_lld_is_write_protected(MMCDriver *mmcp) { * @note You can add your board-specific code here. */ void boardInit(void) { + palSetLineMode(LINE_LED_GREEN, PAL_MODE_OUTPUT_PUSHPULL); + palSetLineMode(LINE_LED_YELLOW, PAL_MODE_OUTPUT_PUSHPULL); + palSetLineMode(LINE_LED_RED, PAL_MODE_OUTPUT_PUSHPULL); + 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..e4dcf03 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_GREEN PAL_LINE(GPIOC_BASE, 10U) +#define LINE_LED_YELLOW PAL_LINE(GPIOC_BASE, 11U) +#define LINE_LED_RED PAL_LINE(GPIOC_BASE, 12U) + #endif /* BOARD_H */ diff --git a/source/communication.cpp b/source/communication.cpp new file mode 100644 index 0000000..5835aa5 --- /dev/null +++ b/source/communication.cpp @@ -0,0 +1,291 @@ +#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 +#include + +__attribute__((section(".stacks"))) +std::array 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, 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 (run_status == RunStatus::Idle) { + Samples::Generator.setSize(count); + USBSerial::read( + reinterpret_cast(Samples::Generator.data()), + Samples::Generator.bytesize()); + } else if (run_status == RunStatus::Running) { + int more; + do { + chThdSleepMicroseconds(10); + more = DAC::sigGenWantsMore(); + } while (more == -1); + + USBSerial::read(reinterpret_cast( + 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 entry = ELFManager::loadFromInternalBuffer(); + EM.assert(entry != nullptr, Error::BadUserCodeLoad); + } + } +} + +void readStatus(unsigned char *) +{ + unsigned char buf[2] = { + static_cast(run_status), + static_cast(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("stmdsph"), 7); +#else + USBSerial::write(reinterpret_cast("stmdspl"), 7); +#endif +} + +void readExecTime(unsigned char *) +{ + USBSerial::write(reinterpret_cast(&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(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(Samples::Out.size() / 2 & 0xFF), + static_cast(((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(samps) + offset, 512); + while (USBSerial::read(&unused, 1) == 0); + offset += 512; + total -= 512; + } + USBSerial::write(reinterpret_cast(samps) + offset, total); + while (USBSerial::read(&unused, 1) == 0); + } else { + USBSerial::write(reinterpret_cast("\0\0"), 2); + } +} + +void readConversionInput(unsigned char *) +{ + if (auto samps = Samples::In.modified(); samps != nullptr) { + unsigned char buf[2] = { + static_cast(Samples::In.size() / 2 & 0xFF), + static_cast(((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(samps) + offset, 512); + while (USBSerial::read(&unused, 1) == 0); + offset += 512; + total -= 512; + } + USBSerial::write(reinterpret_cast(samps) + offset, total); + while (USBSerial::read(&unused, 1) == 0); + } else { + USBSerial::write(reinterpret_cast("\0\0"), 2); + } +} + +void readMessage(unsigned char *) +{ + //USBSerial::write(reinterpret_cast(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 . + */ + +#ifndef STMDSP_COMMUNICATION_HPP +#define STMDSP_COMMUNICATION_HPP + +#include + +class CommunicationManager +{ +public: + static void begin(); + +private: + static void threadComm(void *); + + static std::array m_thread_stack; +}; + +#endif // STMDSP_COMMUNICATION_HPP + diff --git a/source/conversion.cpp b/source/conversion.cpp new file mode 100644 index 0000000..27954be --- /dev/null +++ b/source/conversion.cpp @@ -0,0 +1,206 @@ +#include "conversion.hpp" + +#include "periph/adc.hpp" +#include "periph/dac.hpp" +#include "elfload.hpp" +#include "error.hpp" +#include "runstatus.hpp" +#include "samples.hpp" + +constexpr msg_t MSG_CONVFIRST = 1; +constexpr msg_t MSG_CONVSECOND = 2; +constexpr msg_t MSG_CONVFIRST_MEASURE = 3; +constexpr msg_t MSG_CONVSECOND_MEASURE = 4; + +constexpr auto MSG_FOR_FIRST = [](msg_t m) { return m & 1; }; +constexpr auto MSG_FOR_MEASURE = [](msg_t m) { return m > 2; }; + +time_measurement_t conversion_time_measurement; + +__attribute__((section(".convdata"))) +thread_t *ConversionManager::m_thread_monitor = nullptr; +thread_t *ConversionManager::m_thread_runner = nullptr; + +__attribute__((section(".stacks"))) +std::array ConversionManager::m_thread_monitor_stack = {}; +__attribute__((section(".stacks"))) +std::array ConversionManager::m_thread_runner_entry_stack = {}; +__attribute__((section(".convdata"))) +std::array ConversionManager::m_thread_runner_stack = {}; + +std::array 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() +{ + 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(m_thread_runner_stack.data()) && + (uint32_t)psp <= reinterpret_cast(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(m_thread_runner_stack.data() + + m_thread_runner_stack.size() - + 8 * sizeof(uint32_t)); + // Set the LR register to the thread's entry point. + newpsp[5] = reinterpret_cast(threadRunner); + // Overwrite the instruction we'll return to with "bx lr" (jump to address in LR). + newpsp[6] = psp[6]; + *reinterpret_cast(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(threadRunner), + reinterpret_cast(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..ad949dc --- /dev/null +++ b/source/conversion.hpp @@ -0,0 +1,67 @@ +/** + * @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 . + */ + +#ifndef STMDSP_CONVERSION_HPP +#define STMDSP_CONVERSION_HPP + +#include "ch.h" +#include "hal.h" + +#include + +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(); + +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 m_thread_monitor_stack; + static std::array m_thread_runner_entry_stack; + static std::array m_thread_runner_stack; + + static std::array m_mailbox_buffer; + static mailbox_t m_mailbox; +}; + +// Stores the measured execution time. +extern time_measurement_t conversion_time_measurement; + +#endif // STMDSP_CONVERSION_HPP + diff --git a/source/elf_format.hpp b/source/elf.h similarity index 94% rename from source/elf_format.hpp rename to 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 @@ -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 ae7265b..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) 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 . - */ - -#ifndef ELF_LOAD_HPP_ -#define ELF_LOAD_HPP_ - -#include -#include - -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 similarity index 79% rename from source/elf_load.cpp rename to source/elfload.cpp index e161206..a430ad2 100644 --- a/source/elf_load.cpp +++ b/source/elfload.cpp @@ -1,5 +1,5 @@ /** - * @file elf_load.cpp + * @file elfload.cpp * @brief Loads ELF binary data into memory for execution. * * Copyright (C) 2021 Clyne Sullivan @@ -9,12 +9,16 @@ * If not, see . */ -#include "elf_load.hpp" -#include "elf_format.hpp" +#include "elfload.hpp" +#include "elf.h" #include #include +__attribute__((section(".convdata"))) +ELFManager::EntryFunc ELFManager::m_entry = nullptr; +std::array ELFManager::m_file_buffer = {}; + static const unsigned char elf_header[] = { '\177', 'E', 'L', 'F' }; template @@ -23,10 +27,10 @@ constexpr static auto ptr_from_offset(void *base, uint32_t offset) return reinterpret_cast(reinterpret_cast(base) + offset); } -namespace ELF { - -Entry load(void *elf_data) +ELFManager::EntryFunc ELFManager::loadFromInternalBuffer() { + auto elf_data = m_file_buffer.data(); + // Check the ELF's header signature auto ehdr = reinterpret_cast(elf_data); if (!std::equal(ehdr->e_ident, ehdr->e_ident + 4, elf_header)) @@ -54,8 +58,6 @@ Entry load(void *elf_data) } - return loaded ? reinterpret_cast(ehdr->e_entry) : nullptr; + return loaded ? reinterpret_cast(ehdr->e_entry) : nullptr; } -} // namespace ELF - diff --git a/source/elfload.hpp b/source/elfload.hpp new file mode 100644 index 0000000..ada35e5 --- /dev/null +++ b/source/elfload.hpp @@ -0,0 +1,48 @@ +/** + * @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 . + */ + +#ifndef ELF_LOAD_HPP_ +#define ELF_LOAD_HPP_ + +#include "samplebuffer.hpp" + +#include +#include + +constexpr unsigned int MAX_ELF_FILE_SIZE = 16 * 1024; + +class ELFManager +{ +public: + using EntryFunc = Sample *(*)(Sample *, size_t); + + static EntryFunc loadFromInternalBuffer(); + + static EntryFunc loadedElf() { + return m_entry; + } + + static unsigned char *fileBuffer() { + return m_file_buffer.data(); + } + + static void unload() { + m_entry = nullptr; + } + +private: + static EntryFunc m_entry; + + static std::array 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 . + */ + +#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 +/** + * @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 . + */ + +#ifndef STMDSP_ERROR_HPP +#define STMDSP_ERROR_HPP -constexpr unsigned int MAX_ERROR_QUEUE_SIZE = 8; +#include 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 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..4b0e3eb --- /dev/null +++ b/source/handlers.cpp @@ -0,0 +1,138 @@ +#include "handlers.hpp" + +#include "adc.hpp" +#include "conversion.hpp" +#include "cordic.hpp" +#include "runstatus.hpp" + +extern "C" { + +__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(cordic::sin), + reinterpret_cast(cordic::cos), + reinterpret_cast(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(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(); + + // 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 . + */ + +#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 e9c33ef..9a22a73 100644 --- a/source/main.cpp +++ b/source/main.cpp @@ -12,90 +12,24 @@ #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 - -// Pin definitions -// -#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 - -// Run status -// -enum class RunStatus : char -{ - Idle = '1', - Running, - Recovering -}; -static RunStatus run_status = RunStatus::Idle; - -// Conversion threads messaging -// -#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 msg_t conversionMBBuffer[2]; -static MAILBOX_DECL(conversionMB, conversionMBBuffer, 2); - -// Sample input and output buffers -// -#if defined(TARGET_PLATFORM_H7) -__attribute__((section(".convdata"))) -static SampleBuffer samplesIn (reinterpret_cast(0x38000000)); // 16k -__attribute__((section(".convdata"))) -static SampleBuffer samplesOut (reinterpret_cast(0x30004000)); // 16k -static SampleBuffer samplesSigGen (reinterpret_cast(0x30000000)); // 16k -#else -__attribute__((section(".convdata"))) -static SampleBuffer samplesIn (reinterpret_cast(0x20008000)); // 16k -__attribute__((section(".convdata"))) -static SampleBuffer samplesOut (reinterpret_cast(0x2000C000)); // 16k -static SampleBuffer samplesSigGen (reinterpret_cast(0x20010000)); // 16k -#endif - -// Algorithm binary storage -// -constexpr unsigned int MAX_ELF_FILE_SIZE = 16 * 1024; -static unsigned char elf_file_store[MAX_ELF_FILE_SIZE]; -__attribute__((section(".convdata"))) -static ELF::Entry elf_entry = nullptr; +#include "runstatus.hpp" +RunStatus run_status = RunStatus::Idle; // Other variables // -static ErrorManager EM; -static time_measurement_t conversion_time_measurement; -static char userMessageBuffer[128]; -static unsigned char userMessageSize = 0; +//static char userMessageBuffer[128]; +//static unsigned char userMessageSize = 0; -// Functions -// -__attribute__((section(".convcode"))) -static void conversion_unprivileged_main(); -static void startThreads(); -static void mpuSetup(); -static void abortAlgorithmFromISR(); -static void signalOperate(adcsample_t *buffer, size_t count); -static void signalOperateMeasure(adcsample_t *buffer, size_t count); +#include "conversion.hpp" +#include "communication.hpp" +#include "monitor.hpp" int main() { @@ -103,15 +37,7 @@ int main() halInit(); chSysInit(); - SCB->CPACR |= 0xF << 20; // Enable FPU - mpuSetup(); - -#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(); @@ -121,624 +47,12 @@ int main() SClock::setRate(SClock::Rate::R32K); ADC::setRate(SClock::Rate::R32K); - startThreads(); + // Start our threads. + ConversionManager::begin(); + CommunicationManager::begin(); + Monitor::begin(); + chThdExit(0); return 0; } -static THD_FUNCTION(monitorThread, arg); // Runs status LEDs and allows debug halt. -static THD_FUNCTION(conversionThreadMonitor, arg); // Monitors and manages algo. thread. -static THD_FUNCTION(conversionThread, arg); // Algorithm thread (unprivileged). -static THD_FUNCTION(communicationThread, arg); // Manages USB communications. - -// Need to hold some thread handles for mailbox usage. -static thread_t *conversionThreadHandle = nullptr; -__attribute__((section(".convdata"))) -static thread_t *conversionThreadMonitorHandle = nullptr; - -// The more stack for the algorithm, the merrier. -constexpr unsigned int conversionThreadUPWASize = -#if defined(TARGET_PLATFORM_H7) - 62 * 1024; -#else - 15 * 1024; -#endif - -__attribute__((section(".stacks"))) -static THD_WORKING_AREA(monitorThreadWA, 256); -__attribute__((section(".stacks"))) -static THD_WORKING_AREA(conversionThreadMonitorWA, 1024); -__attribute__((section(".stacks"))) -static THD_WORKING_AREA(conversionThreadWA, 128); // For entering unprivileged mode. -__attribute__((section(".convdata"))) -static THD_WORKING_AREA(conversionThreadUPWA, conversionThreadUPWASize); -__attribute__((section(".stacks"))) -static THD_WORKING_AREA(communicationThreadWA, 4096); - -void startThreads() -{ - chThdCreateStatic( - monitorThreadWA, sizeof(monitorThreadWA), - LOWPRIO, - monitorThread, nullptr); - conversionThreadMonitorHandle = chThdCreateStatic( - conversionThreadMonitorWA, sizeof(conversionThreadMonitorWA), - NORMALPRIO + 1, - conversionThreadMonitor, nullptr); - auto conversionThreadUPWAEnd = - reinterpret_cast(conversionThreadUPWA) + conversionThreadUPWASize; - conversionThreadHandle = chThdCreateStatic( - conversionThreadWA, sizeof(conversionThreadWA), - HIGHPRIO, - conversionThread, - reinterpret_cast(conversionThreadUPWAEnd)); - chThdCreateStatic( - communicationThreadWA, sizeof(communicationThreadWA), - NORMALPRIO, - communicationThread, nullptr); -} - -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. - // 'u' - Get user message. - // '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)) { - if (run_status == RunStatus::Idle) { - samplesSigGen.setSize(count * 2); - USBSerial::read( - reinterpret_cast(samplesSigGen.middata()), - samplesSigGen.bytesize() / 2); - } else if (run_status == RunStatus::Running) { - int more; - do { - chThdSleepMicroseconds(10); - more = DAC::sigGenWantsMore(); - } while (more == -1); - - USBSerial::read(reinterpret_cast( - more == 0 ? samplesSigGen.data() : samplesSigGen.middata()), - samplesSigGen.bytesize() / 2); - } - } - } - 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("stmdsph"), 7); -#else - USBSerial::write(reinterpret_cast("stmdspl"), 7); -#endif - break; - - // 'I' - Sends the current run status. - case 'I': - { - unsigned char buf[2] = { - static_cast(run_status), - static_cast(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(), signalOperateMeasure); - 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(&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(), signalOperate); - 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(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(samplesOut.size() / 2 & 0xFF), - static_cast(((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(samps) + offset, 512); - while (USBSerial::read(&unused, 1) == 0); - offset += 512; - total -= 512; - } - USBSerial::write(reinterpret_cast(samps) + offset, total); - while (USBSerial::read(&unused, 1) == 0); - } else { - USBSerial::write(reinterpret_cast("\0\0"), 2); - } - break; - case 't': - if (auto samps = samplesIn.modified(); samps != nullptr) { - unsigned char buf[2] = { - static_cast(samplesIn.size() / 2 & 0xFF), - static_cast(((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(samps) + offset, 512); - while (USBSerial::read(&unused, 1) == 0); - offset += 512; - total -= 512; - } - USBSerial::write(reinterpret_cast(samps) + offset, total); - while (USBSerial::read(&unused, 1) == 0); - } else { - USBSerial::write(reinterpret_cast("\0\0"), 2); - } - break; - - case 'u': - USBSerial::write(reinterpret_cast(userMessageBuffer), userMessageSize); - 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(conversion_unprivileged_main), - reinterpret_cast(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 mpuSetup() -{ - // 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( - (char *)conversionThreadUPWA + - conversionThreadUPWASize - 8 * sizeof(uint32_t)); - // Set the LR register to the thread's entry point. - newpsp[5] = reinterpret_cast(conversion_unprivileged_main); - // Overwrite the instruction we'll return to with "bx lr" (jump to address in LR). - newpsp[6] = psp[6]; - *reinterpret_cast(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 signalOperate(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 signalOperateMeasure(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(signalOperate); -} - -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(cordic::sin), - reinterpret_cast(cordic::cos), - reinterpret_cast(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(ctxp->r0); - break; - case 4: - { - const char *str = reinterpret_cast(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. - 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..335a1eb --- /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 . + */ + +#include "monitor.hpp" + +#include "error.hpp" +#include "runstatus.hpp" + +#include "ch.h" +#include "hal.h" + +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 + }; + + 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); + } + } +} + +__attribute__((section(".stacks"))) +std::array Monitor::m_thread_stack = {}; + diff --git a/source/monitor.hpp b/source/monitor.hpp new file mode 100644 index 0000000..44545c3 --- /dev/null +++ b/source/monitor.hpp @@ -0,0 +1,29 @@ +/** + * @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 . + */ + +#ifndef STMDSP_MONITOR_HPP +#define STMDSP_MONITOR_HPP + +#include + +class Monitor +{ +public: + static void begin(); + +private: + static void threadMonitor(void *); + + static std::array m_thread_stack; +}; + +#endif // STMDSP_MONITOR_HPP + diff --git a/source/adc.cpp b/source/periph/adc.cpp similarity index 98% rename from source/adc.cpp rename to source/periph/adc.cpp index 2a4fa38..00438f2 100644 --- a/source/adc.cpp +++ b/source/periph/adc.cpp @@ -88,7 +88,7 @@ ADCConversionGroup ADC::m_group_config2 = { .awd3cr = 0, #endif .smpr = { - ADC_SMPR1_SMP_AN1(ADC_SMPR_SMP_12P5) | ADC_SMPR1_SMP_AN2(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_SQ2_N(ADC_CHANNEL_IN2), @@ -141,8 +141,8 @@ adcsample_t ADC::readAlt(unsigned int id) static adcsample_t result[16] = {}; readAltDone = false; adcStartConversion(m_driver2, &m_group_config2, result, 8); - while (!readAltDone) - __WFI(); + while (!readAltDone); + //__WFI(); adcStopConversion(m_driver2); return result[id]; } diff --git a/source/adc.hpp b/source/periph/adc.hpp similarity index 100% rename from source/adc.hpp rename to source/periph/adc.hpp diff --git a/source/cordic.cpp b/source/periph/cordic.cpp similarity index 100% rename from source/cordic.cpp rename to source/periph/cordic.cpp diff --git a/source/cordic.hpp b/source/periph/cordic.hpp similarity index 100% rename from source/cordic.hpp rename to source/periph/cordic.hpp diff --git a/source/dac.cpp b/source/periph/dac.cpp similarity index 100% rename from source/dac.cpp rename to source/periph/dac.cpp diff --git a/source/dac.hpp b/source/periph/dac.hpp similarity index 100% rename from source/dac.hpp rename to source/periph/dac.hpp diff --git a/source/usbcfg.c b/source/periph/usbcfg.c similarity index 100% rename from source/usbcfg.c rename to source/periph/usbcfg.c diff --git a/source/usbcfg.h b/source/periph/usbcfg.h similarity index 100% rename from source/usbcfg.h rename to source/periph/usbcfg.h diff --git a/source/usbserial.cpp b/source/periph/usbserial.cpp similarity index 100% rename from source/usbserial.cpp rename to source/periph/usbserial.cpp diff --git a/source/usbserial.hpp b/source/periph/usbserial.hpp similarity index 100% rename from source/usbserial.hpp rename to source/periph/usbserial.hpp 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/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 . + */ + +#include "samples.hpp" + +#include "ch.h" +#include "hal.h" + +#include + +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(0x38000000)); // 16k +__attribute__((section(".convdata"))) +SampleBuffer Samples::Out (reinterpret_cast(0x30004000)); // 16k +SampleBuffer Samples::SigGen (reinterpret_cast(0x30000000)); // 16k +#else +__attribute__((section(".convdata"))) +SampleBuffer Samples::In (reinterpret_cast(0x20008000)); // 16k +__attribute__((section(".convdata"))) +SampleBuffer Samples::Out (reinterpret_cast(0x2000C000)); // 16k +SampleBuffer Samples::Generator (reinterpret_cast(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 . + */ + +#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 +