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
|
#ifndef SYS_SCHEDULE_H
#define SYS_SCHEDULE_H
#include <cpu.h>
#include <lib/mem.h>
#include <lib/ll.h>
#include <sys/core.h>
enum ThreadStatus {
THREAD_READY,
THREAD_WAITING,
THREAD_WAITING_FOR_MUTEX,
THREAD_STACK_ERROR,
THREAD_RUNNING,
THREAD_FINISHED,
};
struct ThreadData {
unsigned short status;
void* mutex_waiting;
unsigned long pid;
unsigned char priority;
};
struct Thread {
struct ThreadData data;
void (*thread)(void);
void* stack;
void* stack_base;
};
#define MAX_THREADS 0x100
#define STACK_SIZE 0x1000
#define PRIORITIES 6
struct Scheduler {
struct LL tlist[PRIORITIES];
struct LL* rthread_ll;
};
#ifndef SYS_SCHEDULE_C
#define SYS_SCHEDULE_C
extern struct Scheduler scheduler;
#endif
void init_scheduler(void);
void add_thread(void (*thread_fxn)(void), unsigned char priority);
void schedule(void);
void remove_running_thread(void);
static inline void preserveregs(struct Thread* thread)
{
// Preserve current stack pointer
void* sp = getsp();
// Get current mode
unsigned long mode = getmode();
// Set supervisor mode - "User mode"
setsvc();
void* ssp = getsp();
// Move stack to reserved register area
setsp(thread->stack_base - 0x1000 + 16*4);
// Push registers to the stack
asm volatile ("push {r0, r1, r2, r3, r4, r5, r6, r7, r8, r9, r10, r11, r12, lr}");
// Restore stack to previous
setsp(ssp);
// Restore mode
setmode(mode);
// Restore current stack pointer
setsp(sp);
}
static inline void preservestack(struct Thread* thread)
{
// Get current mode
unsigned long mode = getmode();
// Set supervisor mode - "User mode"
setsvc();
// Store the stack pointer
void* ssp = getsp();
thread->stack = ssp;
// Restore mode
setmode(mode);
}
static inline void restoreregs(struct Thread* thread)
{
// Preserve current stack pointer
void* sp = getsp();
// Get current mode
unsigned long mode = getmode();
// Set supervisor mode - "User mode"
setsvc();
void* ssp = getsp();
// Move stack to reserved register area
setsp(thread->stack_base - 0x1000 + 16*4 - 14*4);
// Restore registers on the stack
asm volatile ("pop {r0, r1, r2, r3, r4, r5, r6, r7, r8, r9, r10, r11, r12, lr}");
// Restore stack to previous
setsp(ssp);
// Restore mode
setmode(mode);
// Restore current stack pointer
setsp(sp);
}
static inline void restorestack(struct Thread* thread)
{
// Get current mode
unsigned long mode = getmode();
// Set supervisor mode - "User mode"
setsvc();
// Set stack pointer to thread's stack pointer
asm volatile("mov sp, %0" :: "r"(thread->stack));
// Restore mode
setmode(mode);
}
static inline void preservesysstack(unsigned long* sp)
{
if (*sp == 0) {
unsigned long mode = getmode();
setsvc();
*sp = (unsigned long)getsp();
setmode(mode);
}
}
static inline void restoresysstack(unsigned long* sp)
{
if (*sp) {
unsigned long mode = getmode();
setsvc();
setsp((void*)*sp);
setmode(mode);
*sp = 0;
}
}
static inline void preservepc(struct Thread* t)
{
asm volatile ("mov %0, lr" : "=r"(t->thread));
}
#endif
|
