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
|
#include <task.h>
#include <heap.h>
#include <stm32l476xx.h>
typedef struct {
uint32_t *sp;
uint8_t use;
uint32_t *stack;
void (*code)(void);
} task_t;
#define MAX_TASKS 6
static task_t tasks[MAX_TASKS];
static volatile int next_idx = 0;
static uint8_t task_enable = 0;
void task_init(void (*init)(void))
{
for (int i = 0; i < MAX_TASKS; i++)
tasks[i].use = 0;
task_start(init, 4096);
asm("\
msr psp, %0; \
mrs r0, control; \
orr r0, r0, #3; \
msr control, r0; \
isb; \
" :: "r" (tasks[0].sp));
task_enable = 1;
init();
}
extern void _exit(int);
void task_exit(void)
{
// TODO free stack?
asm("cpsid i"); // hope to catch next_idx
tasks[next_idx].use = 0;
asm("cpsie i");
while (1); // bye
}
void task_start(void (*task)(void), uint16_t stackSize)
{
asm("cpsid i"); // just to be safe
for (int i = 0; i < MAX_TASKS; i++) {
if (tasks[i].use == 0) {
tasks[i].stack = malloc(stackSize);
tasks[i].sp = tasks[i].stack + stackSize - 16;
tasks[i].sp[13] = (uint32_t)task_exit;
tasks[i].sp[14] = (uint32_t)task;
tasks[i].sp[15] = 0x01000000;
tasks[i].use = 1;
tasks[i].code = task;
asm("cpsie i");
return;
}
}
// TODO handle error
}
__attribute__ ((naked))
void PendSV_Handler(void)
{
if (task_enable == 0)
asm("bx lr");
// save state
asm("\
cpsid i; \
isb; \
dsb; \
mrs r0, psp; \
stmdb r0!, {r4-r11}; \
mov %0, r0; \
" : "=r" (tasks[next_idx].sp));
// find next task (round-robin style)
do {
if (++next_idx == MAX_TASKS) {
next_idx = 0;
break; // task 0 better exist
}
} while (tasks[next_idx].use == 0);
// restore
asm("\
mov r0, %0; \
ldmia r0!, {r4-r11}; \
msr psp, r0; \
isb; \
dsb; \
" :: "r" (tasks[next_idx].sp));
// end
asm("\
mov r0, #0xFFFFFFFD; \
cpsie i; \
bx lr; \
");
}
|
