From a61201b8047ebe278cfb281723a4bf6c82556472 Mon Sep 17 00:00:00 2001 From: Christian Cunningham Date: Thu, 6 Jan 2022 15:51:48 -0800 Subject: Scheduling --- include/cpu.h | 69 ++++++++++++++++++++++++ include/lib/ll.h | 4 +- include/lib/q.h | 5 +- include/sys/core.h | 10 ---- include/sys/schedule.h | 142 +++++++++++++++++++++++++++++++++++++++++++++++++ src/lib/ll.c | 27 +++++++++- src/lib/q.c | 14 +++++ src/sys/core.c | 28 +++++++++- src/sys/schedule.c | 140 ++++++++++++++++++++++++++++++++++++++++++++++++ src/util/mutex.c | 22 +++++++- src/util/status.c | 28 ++++++---- 11 files changed, 461 insertions(+), 28 deletions(-) create mode 100644 include/cpu.h create mode 100644 include/sys/schedule.h create mode 100644 src/sys/schedule.c diff --git a/include/cpu.h b/include/cpu.h new file mode 100644 index 0000000..c93b3b5 --- /dev/null +++ b/include/cpu.h @@ -0,0 +1,69 @@ +#ifndef CPU_H +#define CPU_H + +static inline unsigned long getmode(void) +{ + unsigned long mode; + asm volatile ("mrs %0, cpsr" : "=r"(mode)); + return mode; +} + +static inline void setsvc(void) +{ + unsigned long mode; + asm volatile ( + "mrs %0, cpsr\n" + "bic %0, %0, #0x1F\n" + "orr %0, %0, #0x13\n" + "msr cpsr_c, %0" + : "=r"(mode)); +} + +static inline void setmode(unsigned long mode) +{ + asm volatile ("msr cpsr_c, %0" :: "r"(mode)); +} + +static inline void* getsvcstack(void) +{ + void* sp; + asm volatile ( + "mrs r0, cpsr\n" + "bic r1, r0, #0x1F\n" + "orr r1, r1, #0x13\n" + "msr cpsr_c, r1\n" + "mov %0, sp\n" + "msr cpsr_c, r0" + : "=r"(sp)); + return sp; +} + +static inline void* getfiqstack(void) +{ + void* sp; + asm volatile ( + "mrs r0, cpsr\n" + "bic r1, r0, #0x1F\n" + "orr r1, r1, #0x11\n" + "msr cpsr_c, r1\n" + "mov %0, sp\n" + "msr cpsr_c, r0" + : "=r"(sp)); + return sp; +} + +static inline void* getirqstack(void) +{ + void* sp; + asm volatile ( + "mrs r0, cpsr\n" + "bic r1, r0, #0x1F\n" + "orr r1, r1, #0x12\n" + "msr cpsr_c, r1\n" + "mov %0, sp\n" + "msr cpsr_c, r0" + : "=r"(sp)); + return sp; +} + +#endif diff --git a/include/lib/ll.h b/include/lib/ll.h index ab4148d..a9c3722 100644 --- a/include/lib/ll.h +++ b/include/lib/ll.h @@ -5,11 +5,13 @@ struct LL { struct LL* prev; struct LL* next; void* data; -}; +} __attribute__((packed)); struct LL* new_ll(void* val); void push_ll(struct LL* l, void* val); +void pop_ll(struct LL* l); void remove_ll(struct LL* l, unsigned long idx); +unsigned long length_ll(struct LL* l); #define show_ll(L, TYPE) { \ struct LL* t = L; \ diff --git a/include/lib/q.h b/include/lib/q.h index cf75c6d..11d7ab7 100644 --- a/include/lib/q.h +++ b/include/lib/q.h @@ -4,16 +4,17 @@ struct Q_base { struct Q* next; struct Q* last; -}; +} __attribute__((packed)); struct Q { struct Q* next; void* data; -}; +} __attribute__((packed)); struct Q_base* new_q(); void push_q(struct Q_base* qb, void* val); void pop_q(struct Q_base* qb); +unsigned long length_q(struct Q_base* qb); #define show_q(QQ, TYPE) { \ if (QQ->next != 0) { \ diff --git a/include/sys/core.h b/include/sys/core.h index 2d611b3..361ffb1 100644 --- a/include/sys/core.h +++ b/include/sys/core.h @@ -21,16 +21,6 @@ static inline void delay(unsigned long cycles) : "=r"(cycles): [cycles]"0"(cycles) : "cc"); } -static inline void preserveregs(void) -{ - asm volatile("push {r0, r1, r2, r3, r4, r5, r6, r7, r8, r9, r10, r11}"); -} - -static inline void restoreregs(void) -{ - asm volatile("pop {r0, r1, r2, r3, r4, r5, r6, r7, r8, r9, r10, r11}"); -} - static inline void* getsp(void) { void* out; diff --git a/include/sys/schedule.h b/include/sys/schedule.h new file mode 100644 index 0000000..cced851 --- /dev/null +++ b/include/sys/schedule.h @@ -0,0 +1,142 @@ +#ifndef SYS_SCHEDULE_H +#define SYS_SCHEDULE_H +#include +#include +#include +#include + +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 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 #include #include -#include #include #include #include @@ -10,6 +9,7 @@ #include #include #include +#include #include #include #include @@ -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 +#include +#include +#include + +#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(®loc) + 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(®loc); + 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 #include +#include #include 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 #include #include #include @@ -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; -- cgit v1.2.1