From 00660fb9143daf7fa04647bccd6047b9bf0f5e48 Mon Sep 17 00:00:00 2001
From: Christian Cunningham <cc@localhost>
Date: Fri, 21 Jan 2022 11:37:49 -0700
Subject: Scheduler Rewrite Start

---
 later/schedule.h | 159 +++++++++++++++++++++++++++++++++++++++++++++----------
 1 file changed, 130 insertions(+), 29 deletions(-)

(limited to 'later/schedule.h')

diff --git a/later/schedule.h b/later/schedule.h
index 77da7eb..e1cde57 100644
--- a/later/schedule.h
+++ b/later/schedule.h
@@ -1,43 +1,144 @@
 #ifndef SYS_SCHEDULE_H
 #define SYS_SCHEDULE_H
+#include <cpu.h>
+#include <lib/mem.h>
+#include <lib/ll.h>
+#include <sys/core.h>
 
-#define S_WAITING 0
-#define S_READY 1
+enum ThreadStatus {
+	THREAD_READY = 0,
+	THREAD_RUNNING = 1,
+	THREAD_WAITING = 2,
+	THREAD_WAITING_FOR_MUTEX = 3,
+	THREAD_FINISHED = 4,
+	THREAD_STACK_ERROR = 5,
+};
+
+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;
+};
 
-#define STACK_SIZE 0x800
-struct Task {
+struct ThreadData {
 	unsigned char priority;
-	unsigned char status;
+	unsigned char preempt_count;
+	unsigned short status;
+	void* mutex_waiting;
 	unsigned long pid;
-	void (*task)(void);
-	unsigned long reg[16];
-	unsigned long stacki;
-	void* stack;
+	struct cpu_context cpu_context;
 };
 
-#ifdef FLAT
-struct Scheduler {
-	unsigned long running_pid;
-	struct Task** tasks;
-};
-#elseif LL
-#include "../lib/ll.h"
-struct Scheduler {
-	unsigned long running_pid;
-	struct LL* tasks;
+struct Thread {
+	//void (*thread)(void);
+	void* thread;
+	void* stack;
+	void* stack_base;
+	struct ThreadData data;
 };
-#else
-#include "../lib/q.h"
+
+#define MAX_THREADS 0x100
+#define STACK_SIZE 0x1000
+#define PRIORITIES 6
 struct Scheduler {
-	unsigned long running_pid;
-	struct Q_base* tasks;
+	struct LL* rthread_ll;
+	struct cpu_context* ctx;
+	struct LL tlist[PRIORITIES];
 };
-#endif
 
-void add_fxn(void (*task)(void), unsigned char priority);
-unsigned char exists(void (*task)(void));
-void add_no_repeat(void (*task)(void), unsigned char priority);
-unsigned int get_task_length(void);
-void execute_task(void);
+void init_scheduler(void);
+void add_thread(void (*thread_fxn)(void), unsigned char priority);
+extern void schedule(void);
+void schedule_c(void);
+void schedule_irq(void);
+void cleanup(void);
+void sched_info(void);
+struct LL* get_next_thread(void);
+
+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() + 4*4; // Ignore 4 words pushed on by (schedule)
+	thread->stack = ssp;
+	// Restore mode
+	setmode(mode);
+}
+
+static inline void restore_stack(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 preserve_sys_stack(unsigned long* sp)
+{
+	if (*sp == 0) {
+		unsigned long mode = getmode();
+		setsvc();
+		*sp = (unsigned long)getsp();
+		setmode(mode);
+	}
+}
+
+static inline void restore_sys_stack(unsigned long* sp)
+{
+	if (*sp) {
+		unsigned long mode = getmode();
+		setsvc();
+		setsp((void*)*sp);
+		setmode(mode);
+		*sp = 0;
+	}
+}
+
+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 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
-- 
cgit v1.2.1