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 +++++++++++++++++++++++++++++++++++++++++++++++++ 5 files changed, 217 insertions(+), 13 deletions(-) create mode 100644 include/cpu.h create mode 100644 include/sys/schedule.h (limited to 'include') 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 -- cgit v1.2.1