blob: 927072700263993f702205aa371d5a53baece5aa (
plain)
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
|
#ifndef THREAD_H
#define THREAD_H
#include <stdint.h>
#include <util/intrusive_list.h>
#include "time.h"
class Thread {
friend void switch_context();
private:
struct int_frame_t {
// Software saved.
uint32_t r4;
uint32_t r5;
uint32_t r6;
uint32_t r7;
uint32_t r8;
uint32_t r9;
uint32_t r10;
uint32_t r11;
uint32_t lr_ex;
// Hardware saved.
uint32_t r0;
uint32_t r1;
uint32_t r2;
uint32_t r3;
uint32_t r12;
uint32_t lr;
uint32_t pc;
uint32_t psr;
};
int_frame_t* sp;
enum State {
Running,
Sleeping,
};
State state;
uint32_t sleep_deadline;
ListHandle<Thread> queue_handle;
static List<Thread> ready_queue;
static List<Thread> sleep_queue;
static Thread* active_thread;
static Thread main_thread;
static bool reschedule() {
uint32_t now = Time::time();
// Go through sleep queue.
for(auto h : sleep_queue) {
// Move to ready queue if deadline has expired.
if(h->p->sleep_deadline <= now) {
h->p->state = Running;
ready_queue.append(*h);
// Moving one element makes the iterator unusable, so break here.
break;
}
}
// Check whether active thread still is ready.
if(active_thread->state == Running) {
// Move thread to end of ready queue.
ready_queue.append(active_thread->queue_handle);
}
// Check whether any threads are ready to run.
if(ready_queue.empty()) {
return false;
}
// Set front of ready queue as active.
active_thread = ready_queue.begin()->p;
return true;
}
Thread() : queue_handle(this) {
ready_queue.append(queue_handle);
}
public:
Thread(void* stack, uint32_t stack_size, void (*func)()) : queue_handle(this) {
sp = (int_frame_t*)((uint8_t*)stack + stack_size - sizeof(int_frame_t));
sp->lr_ex = 0xfffffff9;
// frame->lr = thread exit handler
sp->pc = (uint32_t)func & ~1;
sp->psr = 0x01000000;
}
void start() {
ready_queue.append(queue_handle);
}
void set_sleeping(uint32_t until) {
sleep_deadline = until;
state = Sleeping;
// Search for thread with later deadline.
for(auto h : sleep_queue) {
// Insert before if found.
if(h->p->sleep_deadline > sleep_deadline) {
queue_handle.insert_before(*h);
return;
}
}
// Otherwise append to end of queue.
sleep_queue.append(queue_handle);
}
static void sleep(uint32_t delay) {
active_thread->set_sleeping(Time::time() + delay);
yield();
}
static inline void yield() {
asm volatile("svc 0" ::: "memory");
}
};
#endif
|
