diff options
| -rw-r--r-- | include/sys/schedule.h | 44 | ||||
| -rw-r--r-- | src/sys/schedule.c | 14 |
2 files changed, 14 insertions, 44 deletions
diff --git a/include/sys/schedule.h b/include/sys/schedule.h index cced851..38c6632 100644 --- a/include/sys/schedule.h +++ b/include/sys/schedule.h @@ -19,6 +19,7 @@ struct ThreadData { void* mutex_waiting; unsigned long pid; unsigned char priority; + unsigned char preempt_count; }; struct Thread { @@ -44,29 +45,9 @@ extern struct Scheduler scheduler; 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 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 @@ -80,27 +61,6 @@ static inline void preservestack(struct Thread* thread) 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 diff --git a/src/sys/schedule.c b/src/sys/schedule.c index bde12bc..1bb02c2 100644 --- a/src/sys/schedule.c +++ b/src/sys/schedule.c @@ -84,14 +84,21 @@ struct LL* get_next_thread(void) unsigned long syssp = 0; void schedule(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}"); + // 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 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; - //preserveregs(current_thread); preservestack(current_thread); preservepc(current_thread); restorestack(next_thread); @@ -109,8 +116,11 @@ void schedule(void) } 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 - rthread->thread(); + asm volatile ("blx lr"); + //rthread->thread(); // Remove the currently running thread after completion remove_running_thread(); // Schedule the next thread |