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
|
#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 = 0,
THREAD_RUNNING = 1,
THREAD_WAITING = 2,
THREAD_WAITING_FOR_MUTEX = 3,
THREAD_FINISHED = 4,
THREAD_STACK_ERROR = 5,
};
struct cpu_context {
unsigned long r4;
unsigned long r5;
unsigned long r6;
unsigned long r7;
unsigned long r8;
unsigned long r9;
unsigned long r10;
unsigned long r11;
unsigned long r12;
unsigned long lr;
};
struct ThreadData {
unsigned char priority;
unsigned char preempt_count;
unsigned short status;
void* mutex_waiting;
unsigned long pid;
struct cpu_context cpu_context;
};
struct Thread {
//void (*thread)(void);
void* thread;
void* stack;
void* stack_base;
struct ThreadData data;
};
#define MAX_THREADS 0x100
#define STACK_SIZE 0x1000
#define PRIORITIES 6
struct Scheduler {
struct LL* rthread_ll;
struct cpu_context* ctx;
struct LL tlist[PRIORITIES];
};
#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);
extern void schedule(void);
void schedule_c(void);
void schedule_irq(void);
void cleanup(void);
void sched_info(void);
struct LL* get_next_thread(void);
static inline void yield(void)
{
struct Thread* t = scheduler.rthread_ll->data;
t->data.status = THREAD_WAITING;
schedule();
}
static inline void preserve_stack(struct Thread* thread)
{
// Get current mode
unsigned long mode = getmode();
// Set supervisor mode - "User mode"
setsvc();
// Store the stack pointer
void* ssp = getsp() + 4*4; // Ignore 4 words pushed on by (schedule)
thread->stack = ssp;
// Restore mode
setmode(mode);
}
static inline void restore_stack(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 preserve_sys_stack(unsigned long* sp)
{
if (*sp == 0) {
unsigned long mode = getmode();
setsvc();
*sp = (unsigned long)getsp();
setmode(mode);
}
}
static inline void restore_sys_stack(unsigned long* sp)
{
if (*sp) {
unsigned long mode = getmode();
setsvc();
setsp((void*)*sp);
setmode(mode);
*sp = 0;
}
}
static inline void preserve_pc(struct Thread* t)
{
t->thread = (void*)t->data.cpu_context.lr;
}
static inline void preserve_ctx(struct cpu_context* cpuctx)
{
asm volatile ("mov %0, r4" : "=r"(cpuctx->r4));
asm volatile ("mov %0, r5" : "=r"(cpuctx->r5));
asm volatile ("mov %0, r6" : "=r"(cpuctx->r6));
asm volatile ("mov %0, r7" : "=r"(cpuctx->r7));
asm volatile ("mov %0, r8" : "=r"(cpuctx->r8));
asm volatile ("mov %0, r9" : "=r"(cpuctx->r9));
asm volatile ("mov %0, r10" : "=r"(cpuctx->r10));
asm volatile ("mov %0, r11" : "=r"(cpuctx->r11));
asm volatile ("mov %0, r12" : "=r"(cpuctx->r12));
asm volatile ("mov %0, lr" : "=r"(cpuctx->lr));
}
static inline void restore_ctx(struct cpu_context* cpuctx)
{
asm volatile ("mov r4, %0" :: "r"(cpuctx->r4));
asm volatile ("mov r5, %0" :: "r"(cpuctx->r5));
asm volatile ("mov r6, %0" :: "r"(cpuctx->r6));
asm volatile ("mov r7, %0" :: "r"(cpuctx->r7));
asm volatile ("mov r8, %0" :: "r"(cpuctx->r8));
asm volatile ("mov r9, %0" :: "r"(cpuctx->r9));
asm volatile ("mov r10, %0" :: "r"(cpuctx->r10));
asm volatile ("mov r11, %0" :: "r"(cpuctx->r11));
asm volatile ("mov r12, %0" :: "r"(cpuctx->r12));
asm volatile ("mov lr, %0" :: "r"(cpuctx->lr));
}
#endif
|
