From 159ba190d626f049512c3ebda70772c03e6737a7 Mon Sep 17 00:00:00 2001
From: Christian Cunningham <cc@localhost>
Date: Thu, 6 Jan 2022 16:02:43 -0800
Subject: Scheduling

---
 include/sys/schedule.h | 44 ++------------------------------------------
 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
-- 
cgit v1.2.1