From 3ace36ab03f40db05a7876013b7d814a3e5ca171 Mon Sep 17 00:00:00 2001
From: Christian Cunningham <cc@localhost>
Date: Sat, 22 Jan 2022 23:18:22 -0700
Subject: Added SVC call for clean stack

---
 Common.mk              |  2 +-
 include/sys/schedule.h |  7 ++++++-
 src/boot.S             | 17 ++++++++++++++---
 src/cpu/irq.c          | 18 +++++++-----------
 src/sys/schedule.S     |  2 ++
 src/sys/schedule.c     | 25 ++++++++++++++-----------
 6 files changed, 44 insertions(+), 27 deletions(-)

diff --git a/Common.mk b/Common.mk
index b60db40..04bf399 100644
--- a/Common.mk
+++ b/Common.mk
@@ -26,7 +26,7 @@ ifeq ($(BSP),2)
 	CFLAGS += -DBSP23
 endif
 
-CFLAGS += -DVERSION="\"0.0e\""
+CFLAGS += -DVERSION="\"0.0s\""
 
 .PHONY: clean run run-debug debug export tree
 
diff --git a/include/sys/schedule.h b/include/sys/schedule.h
index d5f3519..53475de 100644
--- a/include/sys/schedule.h
+++ b/include/sys/schedule.h
@@ -15,10 +15,15 @@ enum ThreadStatus {
 	THREAD_SERROR = 4, // Stack Error
 };
 
+struct RStack {
+	void* sp;
+	unsigned long idx;
+};
+
 struct Thread {
 	void* pc;
 	void* sp; // Store r0-r12,lr on stack
-	void* sp_base;
+	unsigned long sp_base;
 	void* mptr;
 	unsigned long pid;
 	unsigned char priority;
diff --git a/src/boot.S b/src/boot.S
index 94de57f..aedc957 100644
--- a/src/boot.S
+++ b/src/boot.S
@@ -136,14 +136,25 @@ svc:
 	// TODO: Make supervisor mode return to a specific location
 	//  (rather than to a user location) such as the kernel loop
 	cmp r0, #0
-	bne 2f
+	bne 3f
 	cps #0x13
 	b   1f
-2:
+3:
 	cmp r0, #2
-	bne 1f
+	bne 2f
 	ldmfd sp!, {r0-r12,lr}
 	b schedule
+2:
+	cmp r0, #3
+	bne 1f
+	ldr r3, =scheduler
+	ldr r2, [r3, #0]
+	ldr r1, [r2, #8] // sp_base
+	cmp r1, #-1
+	beq 1f
+	ldr r3, =stacks_table
+	mov r0, #0
+	strb r0, [r3, r1]
 1:
 	ldmfd sp!, {r0-r12,pc}^
 io_halt_prefetch:
diff --git a/src/cpu/irq.c b/src/cpu/irq.c
index 0ddba2b..729a58d 100644
--- a/src/cpu/irq.c
+++ b/src/cpu/irq.c
@@ -219,11 +219,16 @@ unsigned long c_fiq_handler(void)
 		}
 	} else if (source & (1 << 3)) {
 		c_timer();
-		if (counter++ >= 0x10) {
+		counter++;
+		if (counter % 0x60 == 0) {
+			add_thread(localtest, 0, 0);
+		} else if (counter % 0x6000 == 0) {
 			counter = 0;
+		}
+		if (counter % 0x10 == 0) {
 			//uart_scheduler();
 			return 1;
-		}
+		} 
 		return 0;
 	}
 	return 0;
@@ -231,13 +236,4 @@ unsigned long c_fiq_handler(void)
 
 void localtest(void)
 {
-	//struct Thread* t = scheduler.rthread_ll->data;
-	uart_string("Running IRQ Task...\n");
-	//uart_scheduler();
-	//uart_10(t->data.pid);
-	//uart_char('\n');
-	uart_string("Finished!\n");
-	//uart_10(t->data.pid);
-	//uart_char('\n');
-	//sched_info();
 }
diff --git a/src/sys/schedule.S b/src/sys/schedule.S
index a4c07c4..8f8ae52 100644
--- a/src/sys/schedule.S
+++ b/src/sys/schedule.S
@@ -58,6 +58,8 @@ cleanup:
 	ldr r1, [r0, #0]
 	add r1, #1
 	str r1, [r0, #0]
+	// cleanup stack
+	svc #3
 	// usrloop -> rthread
 	ldr r2, =usrloopthread
 	str r2, [r3, #0]
diff --git a/src/sys/schedule.c b/src/sys/schedule.c
index 5b311a9..ea5b465 100644
--- a/src/sys/schedule.c
+++ b/src/sys/schedule.c
@@ -4,7 +4,6 @@
 #include <util/mutex.h>
 
 extern void kernel_usr_task_loop(void);
-extern void cleanup(void);
 
 void init_scheduler(void)
 {
@@ -12,7 +11,7 @@ void init_scheduler(void)
 	usrloopthread.pc = (void*)kernel_usr_task_loop;
 	usrloopthread.sp = (void*)0x5FC8;
 	*(unsigned long**)usrloopthread.sp = (unsigned long*)kernel_usr_task_loop;
-	usrloopthread.sp_base = (void*)0x6000;
+	usrloopthread.sp_base = -1;
 	usrloopthread.mptr = 0;
 	usrloopthread.pid = -1;
 	usrloopthread.priority = -1;
@@ -35,25 +34,29 @@ void init_scheduler(void)
 	nextpid = SCHED_PID + 1;
 }
 
-void* get_stack(void)
+struct RStack get_stack(void)
 {
+	struct RStack r = {.sp = 0, .idx = -1};
 	for (int i = 0; i < MAX_THREADS; i++) {
 		if (stacks_table[i] == 0) {
 			stacks_table[i] = 1;
-			return (void*)0x20000000 - STACK_SIZE*i;
+			r.idx = i;
+			r.sp = (void*)0x20000000 - STACK_SIZE*i;
+			return r;
 		}
 	}
-	return 0;
+	return r;
 }
 
 void add_thread(void* pc, void* arg, unsigned char priority)
 {
-	void* sp = get_stack();
+	//void* sp = get_stack();
+	struct RStack r = get_stack();
 	struct Thread* thread = (struct Thread*)malloca(sizeof(struct Thread), 4);
 	thread->pc = pc;
-	if (sp) {
-		thread->sp_base = sp;
-		unsigned long* argp = sp;
+	if (r.sp) {
+		thread->sp_base = r.idx;
+		unsigned long* argp = r.sp;
 		argp -= 1;
 		*argp = (unsigned long)arg; // Set r0 to the argument
 		argp -= 13;
@@ -61,8 +64,8 @@ void add_thread(void* pc, void* arg, unsigned char priority)
 		thread->sp = (void*)argp;
 		thread->status = THREAD_READY;
 	} else {
-		thread->sp_base = 0;
-		thread->sp = 0;
+		thread->sp_base = r.idx;
+		thread->sp = r.sp;
 		thread->status = THREAD_SERROR;
 	}
 	thread->mptr = (void*)0;
-- 
cgit v1.2.1