blob: 38c6632fd050d3cfe68a1a7804bbe7c25d41b3c7 (
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
|
#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;
unsigned char preempt_count;
};
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 schedule_irq(void);
void remove_running_thread(void);
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 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
|
