aboutsummaryrefslogtreecommitdiff
path: root/src/sys/schedule.c
blob: 1b0247630b26457d88028f02434467c313c1bfdd (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
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);
}