1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
|
#include "handlers.hpp"
#include "adc.hpp"
#include "conversion.hpp"
#include "cordic.hpp"
#include "runstatus.hpp"
extern "C" {
time_measurement_t conversion_time_measurement;
__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"
|
