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
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
|
#include <cpu/irq.h>
#include <drivers/uart.h>
#include <sys/core.h>
#include <sys/schedule.h>
#include <util/mutex.h>
#define SYS_SCHEDULE_C
struct Scheduler scheduler = {
.tlist = {
{.prev = 0, .next = 0, .data = 0},
{.prev = 0, .next = 0, .data = 0},
{.prev = 0, .next = 0, .data = 0},
{.prev = 0, .next = 0, .data = 0},
{.prev = 0, .next = 0, .data = 0},
{.prev = 0, .next = 0, .data = 0},
},
.rthread_ll = 0,
.ctx = 0,
};
unsigned long syssp = 0;
struct cpu_context syscpu = {
.r4 = 0, .r5 = 0, .r6 = 0, .r7 = 0,
.r8 = 0, .r9 = 0, .r10 = 0, .r11 = 0,
.r12 = 0, .lr = 0,
};
void init_scheduler(void)
{
for(int i = 0; i < PRIORITIES; i++) {
scheduler.tlist[i].prev = &scheduler.tlist[i];
scheduler.tlist[i].next = &scheduler.tlist[i];
scheduler.tlist[i].data = 0;
}
scheduler.rthread_ll = 0;
scheduler.ctx = &syscpu;
}
unsigned char stacks_table[MAX_THREADS] = {0, };
void* get_stack(void)
{
for (int i = 0; i < MAX_THREADS; i++) {
if (stacks_table[i] == 0) {
stacks_table[i] = 1;
return (void*)heap_end() - STACK_SIZE*i;
}
}
return 0;
}
static unsigned long nextpid = 3;
void add_thread(void (*thread_fxn)(void), unsigned char priority)
{
struct Thread* thread = (struct Thread*)malloca(sizeof(struct Thread), 4);
// Set the program counter to the entry
thread->thread = thread_fxn;
// Get a stack frame
thread->stack_base = get_stack();
thread->stack = thread->stack_base;
// Put in error state for no stack
if(thread->stack == 0)
thread->data.status = THREAD_STACK_ERROR;
else
thread->data.status = THREAD_READY;
// Doesn't wait for mutex at start
thread->data.mutex_waiting = 0;
// Set PID
thread->data.pid = nextpid++;
thread->data.preempt_count = 0;
thread->data.cpu_context.lr = (unsigned long)cleanup;
unsigned char p = priority;
if (p >= PRIORITIES) {
p = PRIORITIES - 1;
}
thread->data.priority = p;
push_ll(&scheduler.tlist[p], thread);
}
struct LL* get_next_thread(void)
{
for(unsigned long i = 0; i < PRIORITIES; i++) {
struct LL* thread_ll = scheduler.tlist[i].next;
if (thread_ll == &scheduler.tlist[i])
continue;
do {
struct Thread* thread = thread_ll->data;
if((thread->data.status == THREAD_RUNNING) || (thread->data.status == THREAD_READY))
return thread_ll;
thread_ll = thread_ll->next;
} while(thread_ll != &scheduler.tlist[i]);
}
return 0;
}
void schedule_c(void)
{
// Preserve registers in current context
preserve_ctx(scheduler.ctx);
// Get current thread
struct LL* current_thread_ll = scheduler.rthread_ll;
// Get next thread
struct LL* next_thread_ll = get_next_thread();
// If there is a current thread
if (current_thread_ll != 0) {
// If we are switching the thread
if (current_thread_ll != next_thread_ll) {
// Context switch
struct Thread* current_thread = current_thread_ll->data;
struct Thread* next_thread = next_thread_ll->data;
preserve_stack(current_thread);
//preserve_pc(current_thread);
current_thread->thread = (void*)current_thread->data.cpu_context.lr;
restore_stack(next_thread);
scheduler.rthread_ll = next_thread_ll;
scheduler.ctx = &next_thread->data.cpu_context;
}
}
else if (next_thread_ll != 0) {
struct Thread* next_thread = next_thread_ll->data;
preserve_sys_stack(&syssp);
restore_stack(next_thread);
scheduler.rthread_ll = next_thread_ll;
scheduler.ctx = &next_thread->data.cpu_context;
}
if (scheduler.rthread_ll) {
struct Thread* rthread = scheduler.rthread_ll->data;
restore_ctx(scheduler.ctx);
asm volatile ("bx %0" :: "r"(rthread->thread));
} else {
scheduler.ctx = &syscpu;
restore_sys_stack(&syssp);
restore_ctx(scheduler.ctx);
}
}
void cleanup(void)
{
if (scheduler.rthread_ll != 0) {
// Mark the thread as finished
struct Thread* t = scheduler.rthread_ll->data;
//uart_string("Cleaning up thread ");
//uart_10(t->data.pid);
//uart_char('\n');
t->data.status = THREAD_FINISHED;
// Mark the stack space as free
unsigned long sidx = (unsigned long)(heap_end() - t->stack_base)/STACK_SIZE;
stacks_table[sidx] = 0;
// Remove the thread
struct LL* ll = scheduler.rthread_ll;
struct LL* prev = ll->prev;
struct LL* next = ll->next;
prev->next = ll->next;
next->prev = ll->prev;
free(ll->data);
free(ll);
scheduler.rthread_ll = 0;
}
// Schedule next thread
schedule();
}
void sched_info(void)
{
uart_string("Scheduler Information\n");
for(unsigned long i = 0; i < PRIORITIES; i++) {
struct LL* ll = scheduler.tlist[i].next;
uart_string("Queue ");
uart_10(i);
while (ll != &scheduler.tlist[i]) {
uart_string("\nThread ");
struct Thread* t = ll->data;
uart_hex((unsigned long)t->thread);uart_char(' ');
uart_hex((unsigned long)t->stack);uart_char(' ');
uart_hex((unsigned long)t->stack_base);uart_char(' ');
uart_10(t->data.priority);uart_char(' ');
uart_10(t->data.preempt_count);uart_char(' ');
uart_10(t->data.status);uart_char(' ');
uart_hex((unsigned long)t->data.mutex_waiting);uart_char(' ');
uart_10(t->data.pid);uart_char('\n');
memshow32((unsigned long*)&t->data.cpu_context, 10);
ll = ll->next;
}
uart_char('\n');
}
uart_string("Stacks:\n");
memshow32((unsigned long*)stacks_table, 6);
}
|
