diff options
| -rw-r--r-- | include/sys/core.h | 14 | ||||
| -rw-r--r-- | include/sys/schedule.h | 75 | ||||
| -rw-r--r-- | src/boot.S | 49 | ||||
| -rw-r--r-- | src/sys/core.c | 36 | ||||
| -rw-r--r-- | src/sys/schedule.S | 126 | ||||
| -rw-r--r-- | src/sys/schedule.c | 120 |
6 files changed, 352 insertions, 68 deletions
diff --git a/include/sys/core.h b/include/sys/core.h index 361ffb1..eb1f1e6 100644 --- a/include/sys/core.h +++ b/include/sys/core.h @@ -21,6 +21,20 @@ static inline void delay(unsigned long cycles) : "=r"(cycles): [cycles]"0"(cycles) : "cc"); } +static inline void* getlr(void) +{ + void* out; + asm volatile ("mov %0, lr" : "=r"(out)); + return out; +} + +static inline void* getpc(void) +{ + void* out; + asm volatile ("mov %0, pc" : "=r"(out)); + return out; +} + static inline void* getsp(void) { void* out; diff --git a/include/sys/schedule.h b/include/sys/schedule.h index 38c6632..c3c748b 100644 --- a/include/sys/schedule.h +++ b/include/sys/schedule.h @@ -14,27 +14,43 @@ enum ThreadStatus { THREAD_FINISHED, }; +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; - unsigned char priority; - unsigned char preempt_count; + struct cpu_context cpu_context; }; struct Thread { - struct ThreadData data; - void (*thread)(void); + //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 tlist[PRIORITIES]; struct LL* rthread_ll; + struct cpu_context* ctx; + struct LL tlist[PRIORITIES]; }; #ifndef SYS_SCHEDULE_C @@ -44,24 +60,27 @@ extern struct Scheduler scheduler; void init_scheduler(void); void add_thread(void (*thread_fxn)(void), unsigned char priority); -void schedule(void); +extern void schedule(void); +void schedule_c(void); void schedule_irq(void); -void remove_running_thread(void); +void cleanup(void); +void sched_info(void); +void yield(void); -static inline void preservestack(struct Thread* thread) +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(); + void* ssp = getsp() + 4*4; // Ignore 4 words pushed on by (schedule) thread->stack = ssp; // Restore mode setmode(mode); } -static inline void restorestack(struct Thread* thread) +static inline void restore_stack(struct Thread* thread) { // Get current mode unsigned long mode = getmode(); @@ -73,7 +92,7 @@ static inline void restorestack(struct Thread* thread) setmode(mode); } -static inline void preservesysstack(unsigned long* sp) +static inline void preserve_sys_stack(unsigned long* sp) { if (*sp == 0) { unsigned long mode = getmode(); @@ -83,7 +102,7 @@ static inline void preservesysstack(unsigned long* sp) } } -static inline void restoresysstack(unsigned long* sp) +static inline void restore_sys_stack(unsigned long* sp) { if (*sp) { unsigned long mode = getmode(); @@ -94,9 +113,37 @@ static inline void restoresysstack(unsigned long* sp) } } -static inline void preservepc(struct Thread* t) +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 %0, lr" : "=r"(t->thread)); + 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 @@ -66,6 +66,37 @@ io_halt: wfi b io_halt +io_halt_undefined: + ldr r0, =undefined_msg + ldr r0, [r0] + bl uart_char + b io_halt +io_halt_swi: + ldr r0, =swi_msg + ldr r0, [r0] + bl uart_char + b io_halt +io_halt_prefetch: + ldr r0, =prefetch_msg + ldr r0, [r0] + bl uart_char + b io_halt +io_halt_data: + ldr r0, =data_msg + ldr r0, [r0] + bl uart_char + b io_halt +io_halt_unused: + ldr r0, =unused_msg + ldr r0, [r0] + bl uart_char + b io_halt +io_halt_fiq: + ldr r0, =fiq_msg + ldr r0, [r0] + bl uart_char + b io_halt + .align 5 vector: ldr pc, reset_handler @@ -78,10 +109,16 @@ vector: ldr pc, fiq_handler reset_handler: .word reset -undefined_handler: .word io_halt -swi_handler: .word io_halt -prefetch_handler: .word io_halt -data_handler: .word io_halt -unused_handler: .word io_halt +undefined_handler: .word io_halt_undefined +swi_handler: .word io_halt_swi +prefetch_handler: .word io_halt_prefetch +data_handler: .word io_halt_data +unused_handler: .word io_halt_unused irq_handler: .word irq -fiq_handler: .word io_halt // fiq +fiq_handler: .word io_halt_fiq +undefined_msg: .asciz "Undefined\n" +swi_msg: .asciz "SWI\n" +prefetch_msg: .asciz "Prefetch\n" +data_msg: .asciz "Data\n" +unused_msg: .asciz "unused\n" +fiq_msg: .asciz "FIQ\n" diff --git a/src/sys/core.c b/src/sys/core.c index 204eb55..302e629 100644 --- a/src/sys/core.c +++ b/src/sys/core.c @@ -62,21 +62,41 @@ void sysinit(void) add_thread(testlocal, 0); add_thread(testlocal, 5); add_thread(testlocal, 8); - delay(0x80000000); + delay(0x20000000); schedule(); } struct Mutex testm = {.addr = (void*)0xDEADBEEF, .pid = NULL_PID}; +void testlocal1(void) +{ + unsigned long a = 5; + struct Thread* t = scheduler.rthread_ll->data; + uart_string("vRan Thread "); + uart_10(t->data.pid); + uart_string(" Pri. "); + uart_10(t->data.priority); + uart_string(" ...\n"); + add_thread(testlocal, 0); + schedule(); + a += t->data.pid; + uart_10(a); + uart_string(" Done!\n"); +} 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'); + uart_string(" Pri. "); + uart_10(t->data.priority); + uart_string(" ...\n"); + //delay(0x80000000); + if (t->data.pid == 6) { + add_thread(testlocal, 0); + } else if (t->data.pid == 5) { + add_thread(testlocal1, 1); + schedule(); + sched_info(); + } + uart_string("Done!\n"); } diff --git a/src/sys/schedule.S b/src/sys/schedule.S new file mode 100644 index 0000000..18130d0 --- /dev/null +++ b/src/sys/schedule.S @@ -0,0 +1,126 @@ +.section .text +.globl schedule +// Implemented the scheduler in Assembly since the C defined was messing around with the program stacks +// This way, I can be confident that the stacks will be unchanged +// +// TODO: Mark threads as READY and RUNNING +// +schedule: + ldr r3, =scheduler + // r3 = struct Scheduler* + // Preserve context + //add r0, r3, #4 // struct cpu_context* ctx + ldr r0, [r3, #4] + // r0 = struct cpu_context* + str r4, [r0, #0x00] + str r5, [r0, #0x04] + str r6, [r0, #0x08] + str r7, [r0, #0x0c] + str r8, [r0, #0x10] + str r9, [r0, #0x14] + str r10, [r0, #0x18] + str r11, [r0, #0x1c] + str r12, [r0, #0x20] + str lr, [r0, #0x24] + // Get the next available thread + push {r3, lr} + bl get_next_thread + // r0 = struct LL* next_thread_ll + pop {r3, lr} + ldr r1, [r3, #0] + // r1 = struct LL* current_thread_ll + // Check if there is a valid currently running thread + cmp r1, #0 + beq schedule.current_thread_nexists +schedule.current_thread_exists: + cmp r0, r1 + beq schedule.run_current + // Next is not the same as the current + // Preserve stack of current + ldr r2, [r1, #0x8] // struct Thread* current + str sp, [r2, #0x4] // void* stack // Preserve stack + // Preserve program counter of current + str lr, [r2, #0x0] // void* thread // Preserve pc + ldr r2, [r0, #0x8] // struct Thread* next + // Set new stack pointer + ldr sp, [r2, #0x4] + add r2, r2, #0x18 + // Set new running thread + str r0, [r3, #0x0] // struct LL* next_thread_ll // Set new running thread + // Set new context + str r2, [r3, #0x4] // struct cpu_context* ctx // Set new context + b schedule.run_current +schedule.current_thread_nexists: + // r0 = struct LL* next_thread_ll + // r1 = 0 = struct LL* current_thread_ll + cmp r0, #0 + beq schedule.no_next_thread + ldr r1, [r0, #0x8] + // r1 = struct Thread* next_thread + // Store system stack pointer + ldr r2, =syssp + push {r1} + ldr r1, [r2] + cmp r1, #0 + pop {r1} + bne schedule.dont_overwrite_sys_stack + // Store if zero system stack + str sp, [r2] +schedule.dont_overwrite_sys_stack: + // Move stack to next thread's stack pointer + ldr sp, [r1, #0x4] // void* stack + // Store the running thread ll entry + str r0, [r3, #0x0] // struct LL* rthread_ll + // Set context + add r1, r1, #0x18 // struct cpu_context* + str r1, [r3, #0x4] // store to scheduler.ctx +schedule.run_current: + // Restore context + ldr r2, [r3, #0x4] // struct cpu_context* ctx // Set new context + ldr r4, [r2, #0x00] + ldr r5, [r2, #0x04] + ldr r6, [r2, #0x08] + ldr r7, [r2, #0x0c] + ldr r8, [r2, #0x10] + ldr r9, [r2, #0x14] + ldr r10, [r2, #0x18] + ldr r11, [r2, #0x1c] + ldr r12, [r2, #0x20] + ldr lr, [r2, #0x24] + // Run + ldr r1, [r3, #0] + // r1 = struct LL* rthread_ll + ldr r1, [r1, #0x8] + // r1 = struct Thread* rthread + ldr r0, [r1, #0x0] + // r0 = void* thread + bx r0 +schedule.no_next_thread: + // r0 = 0 = struct LL* next_thread_ll + // r1 = 0 = struct LL* current_thread_ll + // No thread to run + // Restore sys context + ldr r0, =syscpu + str r0, [r3, #0x4] // Store context + ldr r0, =syssp + ldr r1, [r0] + cmp r1, #0 + beq schedule.exit + mov sp, r1 // Restore stack pointer + mov r1, #0 + str r1, [r0] // Clear stack pointer +schedule.exit: + // Restore context + ldr r2, [r3, #0x4] // struct cpu_context* ctx // Set new context + // Restore register context + ldr r4, [r2, #0x00] + ldr r5, [r2, #0x04] + ldr r6, [r2, #0x08] + ldr r7, [r2, #0x0c] + ldr r8, [r2, #0x10] + ldr r9, [r2, #0x14] + ldr r10, [r2, #0x18] + ldr r11, [r2, #0x1c] + ldr r12, [r2, #0x20] + ldr lr, [r2, #0x24] + bx lr diff --git a/src/sys/schedule.c b/src/sys/schedule.c index 1bb02c2..4b071c3 100644 --- a/src/sys/schedule.c +++ b/src/sys/schedule.c @@ -1,3 +1,4 @@ +#include <cpu/irq.h> #include <drivers/uart.h> #include <sys/core.h> #include <sys/schedule.h> @@ -15,6 +16,12 @@ struct Scheduler scheduler = { }, .rthread_ll = 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) { @@ -24,6 +31,7 @@ void init_scheduler(void) scheduler.tlist[i].data = 0; } scheduler.rthread_ll = 0; + scheduler.ctx = &syscpu; } unsigned char stacks_table[MAX_THREADS] = {0, }; @@ -42,12 +50,12 @@ void* get_stack(void) static unsigned long nextpid = 3; void add_thread(void (*thread_fxn)(void), unsigned char priority) { - struct Thread* thread = (struct Thread*)malloc(sizeof(struct Thread)); + 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 = get_stack(); - thread->stack_base = thread->stack; + 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; @@ -57,6 +65,8 @@ void add_thread(void (*thread_fxn)(void), unsigned char priority) 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; @@ -81,70 +91,100 @@ struct LL* get_next_thread(void) return 0; } -unsigned long syssp = 0; -void schedule(void) +void schedule_c(void) { - // Preserve current process's registers - // in the current stack - asm volatile ("push {r0, r1, r2, r3, r4, r5, r6, r7, r8, r9, r10, r11, r12, lr}"); + // 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) { + 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; - preservestack(current_thread); - preservepc(current_thread); - restorestack(next_thread); - //restoreregs(next_thread); + 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) { + else if (next_thread_ll != 0) { struct Thread* next_thread = next_thread_ll->data; - preservesysstack(&syssp); - //preservesysregs(®loc) - restorestack(next_thread); - //restoreregs(next_thread); + 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 process's registers - asm volatile ("pop {r0, r1, r2, r3, r4, r5, r6, r7, r8, r9, r10, r11, r12, lr}"); - // Run the thread - i.e. jump to the pc - asm volatile ("blx lr"); - //rthread->thread(); - // Remove the currently running thread after completion - remove_running_thread(); - // Schedule the next thread - schedule(); + restore_ctx(scheduler.ctx); + asm volatile ("bx %0" :: "r"(rthread->thread)); } else { - //restoresysregs(®loc); - restoresysstack(&syssp); + scheduler.ctx = &syscpu; + restore_sys_stack(&syssp); + restore_ctx(scheduler.ctx); } } -void remove_running_thread(void) +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; - 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); - } + struct LL* prev = ll->prev; + struct LL* next = ll->next; + prev->next = ll->next; + next->prev = ll->prev; + free(ll); scheduler.rthread_ll = 0; } + // Schedule next thread + //uart_string("Scheduling from cleanup!\n"); + //sched_info(); + //schedule(); + schedule(); +} + +void sched_info(void) +{ + disableirq(); + 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'); + } + enableirq(); } |