You cannot select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
139 lines
3.8 KiB
C++
139 lines
3.8 KiB
C++
#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<mathcall>(cordic::sin),
|
|
reinterpret_cast<mathcall>(cordic::cos),
|
|
reinterpret_cast<mathcall>(cordic::tan),
|
|
};
|
|
#if defined(PLATFORM_H7)
|
|
asm("vmov.f64 d0, %0, %1" :: "r" (ctxp->r1), "r" (ctxp->r2));
|
|
if (ctxp->r0 < 3) {
|
|
funcs[ctxp->r0]();
|
|
asm("vmov.f64 %0, %1, d0" : "=r" (ctxp->r1), "=r" (ctxp->r2));
|
|
} else {
|
|
asm("eor r0, r0; vmov.f64 d0, r0, r0");
|
|
}
|
|
#else
|
|
asm("vmov.f32 s0, %0" :: "r" (ctxp->r1));
|
|
if (ctxp->r0 < 3) {
|
|
funcs[ctxp->r0]();
|
|
asm("vmov.f32 %0, s0" : "=r" (ctxp->r1));
|
|
} else {
|
|
asm("eor r0, r0; vmov.f32 s0, r0");
|
|
}
|
|
#endif
|
|
}
|
|
break;
|
|
|
|
// Starts or stops precise cycle time measurement.
|
|
// Used to measure algorithm execution time.
|
|
case 2:
|
|
if (ctxp->r0 == 0) {
|
|
chTMStartMeasurementX(&conversion_time_measurement);
|
|
} else {
|
|
chTMStopMeasurementX(&conversion_time_measurement);
|
|
// Subtract measurement overhead from the result.
|
|
// Running an empty algorithm ("bx lr") takes 196 cycles as of 2/4/21.
|
|
// Only measures algorithm code time (loading args/storing result takes 9 cycles).
|
|
constexpr rtcnt_t measurement_overhead = 196 - 1;
|
|
if (conversion_time_measurement.last > measurement_overhead)
|
|
conversion_time_measurement.last -= measurement_overhead;
|
|
}
|
|
break;
|
|
|
|
// Reads one of the analog inputs made available for algorithm run-time input.
|
|
case 3:
|
|
ctxp->r0 = ADC::readAlt(ctxp->r0);
|
|
break;
|
|
|
|
//case 4:
|
|
// {
|
|
// const char *str = reinterpret_cast<const char *>(ctxp->r0);
|
|
// auto src = str;
|
|
// auto dst = userMessageBuffer;
|
|
// while (*src)
|
|
// *dst++ = *src++;
|
|
// *dst = '\0';
|
|
// userMessageSize = src - str;
|
|
// }
|
|
// break;
|
|
default:
|
|
while (1);
|
|
break;
|
|
}
|
|
|
|
asm("svc 0");
|
|
while (1);
|
|
}
|
|
|
|
__attribute__((naked))
|
|
void MemManage_Handler()
|
|
{
|
|
// 1. Get the stack pointer.
|
|
uint32_t lr;
|
|
asm("mov %0, lr" : "=r" (lr));
|
|
|
|
// 2. Recover from the fault.
|
|
ConversionManager::abort();
|
|
|
|
// 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"
|
|
|