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
141
142
143
144
145
146
147
148
149
150
151
152
153
|
#include "regs.h"
struct tsk {
uint8_t stack_h;
uint8_t stack_l;
struct tsk *next;
};
struct tsk *cur = NULL;
struct tsk *tasks = NULL;
// Setup a timer running every 10 ms
void timersetup()
{
noInterrupts();
TCNT1 = 0;
// Enable CTC and set prescaler = 1024.
// This will give a freq of 15625
TCCR1B = (1 << WGM12) | (1 << CS12) | (1 << CS10);
// Clear on match
//TCCR1A = (1 << COM1A1);
TCCR1A = 0;
// Setup compare. Must be around 156 to create 10ms
OCR1A = 156;
// Enable interrupt on compare
TIMSK1 |= (1 << OCF1A);
interrupts();
}
inline struct tsk *get_next() {
if (cur->next) {
return cur->next;
}
return tasks;
}
void create_tsk(struct tsk *t, void (*f)(void), uint8_t *stack, size_t len) {
// Move to the end of stack
uint8_t *s = stack + len - 1;
// Push load address
*(s--) = (uint16_t)f & 0xFF;
*(s--) = ((uint16_t)f >> 8) & 0xFF;
// Push all the stuff. Reg + SREG
for (int i = 0; i < 32 + 1; i++) {
*(s--) = 0;
}
// Save the stack pointer
t->stack_l = (uint16_t)s & 0xFF;
t->stack_h = ((uint16_t)s >> 8) & 0xFF;
// Put it on the list
t->next = tasks;
tasks = t;
}
void run() {
cur = tasks;
// Put it back
SPL = cur->stack_l;
SPH = cur->stack_h;
POPREGS();
__asm__("RETI");
}
void func(void) {
for(;;) {
Serial.println(1);
}
}
void func3(void) {
for(;;) {
Serial.println(3);
}
}
void func4(void) {
for(;;) {
Serial.println(4);
}
}
void func2(void) {
bool state = false;
for(;;) {
digitalWrite(13, state);
state = !state;
delay(1000);
}
}
ISR(TIMER1_COMPA_vect, ISR_NAKED)
{
PUSHREGS();
if (!cur) {
goto exit;
}
// Save stack space
cur->stack_l = SPL;
cur->stack_h = SPH;
// Switch
cur = get_next();
// Put it back
SPL = cur->stack_l;
SPH = cur->stack_h;
exit:
POPREGS();
__asm__("RETI");
}
void setup()
{
pinMode(13, OUTPUT);
Serial.begin(115200);
Serial.println("Starting stuff");
struct tsk task1, task2, task4, task3;
uint8_t stack[100], stack2[100], stack3[100], stack4[100];
timersetup();
create_tsk(&task1, func, stack, 100);
create_tsk(&task2, func2, stack2, 100);
create_tsk(&task3, func3, stack3, 100);
create_tsk(&task4, func4, stack4, 100);
run();
}
void loop()
{
}
|
