aboutsummaryrefslogtreecommitdiff
path: root/src
diff options
context:
space:
mode:
Diffstat (limited to 'src')
-rw-r--r--src/lib/ll.c27
-rw-r--r--src/lib/q.c14
-rw-r--r--src/sys/core.c28
-rw-r--r--src/sys/schedule.c140
-rw-r--r--src/util/mutex.c22
-rw-r--r--src/util/status.c28
6 files changed, 244 insertions, 15 deletions
diff --git a/src/lib/ll.c b/src/lib/ll.c
index 4eaa291..4918194 100644
--- a/src/lib/ll.c
+++ b/src/lib/ll.c
@@ -13,13 +13,24 @@ struct LL* new_ll(void* val)
void push_ll(struct LL* l, void* val)
{
struct LL* ll = (struct LL*)malloc(sizeof(struct LL));
+ l->prev->next = ll;
ll->prev = l->prev;
ll->next = l;
- ll->prev->next = ll;
l->prev = ll;
ll->data = val;
}
+void pop_ll(struct LL* l)
+{
+ if ((l->prev == l->next) && (l->prev == l))
+ l->data = 0;
+ else {
+ l->prev->next = l->next;
+ l->next->prev = l->prev;
+ free(l);
+ }
+}
+
void remove_ll(struct LL* l, unsigned long idx)
{
struct LL* t = l;
@@ -30,3 +41,17 @@ void remove_ll(struct LL* l, unsigned long idx)
t->next->prev = t->prev;
free(t);
}
+
+unsigned long length_ll(struct LL* l)
+{
+ struct LL* t = l;
+ unsigned long len = 0;
+ while (1) {
+ if (t->next == l) {
+ return len;
+ }
+ len++;
+ t = t->next;
+ }
+ return len;
+}
diff --git a/src/lib/q.c b/src/lib/q.c
index ebae2b9..2ade143 100644
--- a/src/lib/q.c
+++ b/src/lib/q.c
@@ -38,3 +38,17 @@ void pop_q(struct Q_base* qb)
free(t->data);
free(t);
}
+
+unsigned long length_q(struct Q_base* qb)
+{
+ unsigned long length = 0;
+ if(qb->next == 0)
+ return length;
+ length++;
+ struct Q* q = qb->next;
+ while (q != qb->last) {
+ length++;
+ q = q->next;
+ }
+ return length;
+}
diff --git a/src/sys/core.c b/src/sys/core.c
index 6bd2abf..204eb55 100644
--- a/src/sys/core.c
+++ b/src/sys/core.c
@@ -2,7 +2,6 @@
#include <drivers/uart.h>
#include <graphics/drawer.h>
#include <graphics/lfb.h>
-#include <lib/ll.h>
#include <lib/mem.h>
#include <lib/q.h>
#include <lib/strings.h>
@@ -10,6 +9,7 @@
#include <sys/core.h>
#include <sys/kernel.h>
#include <sys/power.h>
+#include <sys/schedule.h>
#include <sys/timer.h>
#include <util/mutex.h>
#include <util/status.h>
@@ -22,6 +22,8 @@ char* os_info_v = "?";
char* os_info_v = VERSION;
#endif
+void testlocal(void);
+
// Initialize IRQs
void sysinit(void)
{
@@ -53,4 +55,28 @@ void sysinit(void)
enablefiq();
// Start Scheduler
+ init_scheduler();
+ add_thread(testlocal, 0);
+ add_thread(testlocal, 1);
+ add_thread(testlocal, 3);
+ add_thread(testlocal, 0);
+ add_thread(testlocal, 5);
+ add_thread(testlocal, 8);
+ delay(0x80000000);
+ schedule();
+}
+
+struct Mutex testm = {.addr = (void*)0xDEADBEEF, .pid = NULL_PID};
+void testlocal(void)
+{
+ unsigned char testd = 0xDE;
+ struct Thread* t = scheduler.rthread_ll->data;
+ delay(0x04000000);
+ testd -= 50;
+ uart_string("Ran Thread ");
+ delay(0x04000000);
+ uart_10(t->data.pid);
+ uart_char(' ');
+ uart_10(testd);
+ uart_char('\n');
}
diff --git a/src/sys/schedule.c b/src/sys/schedule.c
new file mode 100644
index 0000000..bde12bc
--- /dev/null
+++ b/src/sys/schedule.c
@@ -0,0 +1,140 @@
+#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,
+};
+
+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;
+}
+
+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*)malloc(sizeof(struct Thread));
+ // Set the program counter to the entry
+ thread->thread = thread_fxn;
+ // Get a stack frame
+ thread->stack = get_stack();
+ thread->stack_base = thread->stack;
+ // 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++;
+ 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;
+}
+
+unsigned long syssp = 0;
+void schedule(void)
+{
+ struct LL* current_thread_ll = scheduler.rthread_ll;
+ struct LL* next_thread_ll = get_next_thread();
+ if (current_thread_ll) {
+ 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;
+ //preserveregs(current_thread);
+ preservestack(current_thread);
+ preservepc(current_thread);
+ restorestack(next_thread);
+ //restoreregs(next_thread);
+ scheduler.rthread_ll = next_thread_ll;
+ }
+ }
+ else if (next_thread_ll) {
+ struct Thread* next_thread = next_thread_ll->data;
+ preservesysstack(&syssp);
+ //preservesysregs(&regloc)
+ restorestack(next_thread);
+ //restoreregs(next_thread);
+ scheduler.rthread_ll = next_thread_ll;
+ }
+ if (scheduler.rthread_ll) {
+ struct Thread* rthread = scheduler.rthread_ll->data;
+ // Run the thread - i.e. jump to the pc
+ rthread->thread();
+ // Remove the currently running thread after completion
+ remove_running_thread();
+ // Schedule the next thread
+ schedule();
+ } else {
+ //restoresysregs(&regloc);
+ restoresysstack(&syssp);
+ }
+}
+
+void remove_running_thread(void)
+{
+ if (scheduler.rthread_ll != 0) {
+ struct LL* ll = scheduler.rthread_ll;
+ if ((ll->next == ll->prev) && (ll->next == ll)) {
+ ll->data = 0;
+ }
+ else {
+ struct LL* prev = ll->prev;
+ struct LL* next = ll->next;
+ prev->next = ll->next;
+ next->prev = ll->prev;
+ free(ll);
+ }
+ scheduler.rthread_ll = 0;
+ }
+}
diff --git a/src/util/mutex.c b/src/util/mutex.c
index ade0be3..de7a515 100644
--- a/src/util/mutex.c
+++ b/src/util/mutex.c
@@ -1,13 +1,23 @@
#include <cpu/atomic/swap.h>
#include <lib/mem.h>
+#include <sys/schedule.h>
#include <util/mutex.h>
unsigned char lock_mutex(struct Mutex* m, unsigned long pid)
{
if (m->pid == NULL_PID) {
- atm_lock(pid, &m->pid);
+ // Use currently running thread's PID if no pid given
+ if (pid == 0) {
+ struct Thread* thread = scheduler.rthread_ll->data;
+ atm_lock(thread->data.pid, &m->pid);
+ } else {
+ atm_lock(pid, &m->pid);
+ }
return 0;
}
+ struct Thread* thread = scheduler.rthread_ll->data;
+ thread->data.status = THREAD_WAITING_FOR_MUTEX;
+ thread->data.mutex_waiting = m;
return 1;
}
@@ -16,7 +26,15 @@ unsigned char lock_mutex(struct Mutex* m, unsigned long pid)
// mutex's pid lock
unsigned char release_mutex(struct Mutex* m, unsigned long pid)
{
- if (m->pid == pid) {
+ // Use current thread's PID if no pid
+ if (pid == 0) {
+ struct Thread* thread = scheduler.rthread_ll->data;
+ if (m->pid == thread->data.pid) {
+ atm_release(&m->pid);
+ return 0;
+ }
+ }
+ else if (m->pid == pid) {
atm_release(&m->pid);
return 0;
}
diff --git a/src/util/status.c b/src/util/status.c
index f59ede6..be19287 100644
--- a/src/util/status.c
+++ b/src/util/status.c
@@ -1,3 +1,4 @@
+#include <cpu.h>
#include <graphics/drawer.h>
#include <graphics/lfb.h>
#include <symbols.h>
@@ -129,19 +130,24 @@ void status(void)
write_char(&g_Drawer, ' ');
g_Drawer.x = 0;
g_Drawer.y = 9;
- /*
- struct Q* q = scheduler.tasks->next;
- while (q != 0) {
- struct Task* t = q->data;
- write_hex32(&g_Drawer, (unsigned long)t->task);
- write_char(&g_Drawer, ' ');
- q = q->next;
- }
- write_char(&g_Drawer, '\n');
- */
- unsigned long sp = (unsigned long)getsp();
+ unsigned long sp = (unsigned long)getsvcstack();
+ write_hex32(&g_Drawer, sp);
+ write_char(&g_Drawer, ' ');
+ sp = (unsigned long)getirqstack();
write_hex32(&g_Drawer, sp);
+ write_char(&g_Drawer, ' ');
+ sp = (unsigned long)getfiqstack();
+ write_hex32(&g_Drawer, sp);
+ write_char(&g_Drawer, '\n');
+ for(unsigned long i = 1; i <= 14; i++) {
+ write_hex32(&g_Drawer, *(unsigned long*)(0x4000 - i*4));
+ if(i % 6 == 0) {
+ write_char(&g_Drawer, '\n');
+ } else {
+ write_char(&g_Drawer, ' ');
+ }
+ }
g_Drawer.x = x;
g_Drawer.y = y;
generated by cgit v1.2.1 (git 2.18.0) at 2024-12-26 06:16:48 +0000