From 7d3884b5cfc282dbdfe5d9a22d8b2b343280b8af Mon Sep 17 00:00:00 2001
From: Christian Cunningham <cc@localhost>
Date: Tue, 1 Feb 2022 20:36:30 -0700
Subject: Thread yield control to any other threads

---
 include/cpu.h          |  5 +++--
 include/sys/schedule.h |  2 +-
 src/cpu/irq.c          | 12 ++++++++++--
 src/exceptions/svc.S   | 13 +++++++------
 src/sys/schedule.c     | 13 +++++++++++++
 5 files changed, 34 insertions(+), 11 deletions(-)

diff --git a/include/cpu.h b/include/cpu.h
index c15c086..bac80e8 100644
--- a/include/cpu.h
+++ b/include/cpu.h
@@ -91,11 +91,12 @@ __attribute__((always_inline)) static inline unsigned long long get_sys_time(voi
 		}s;
 		unsigned long long llv;
 	}t;
-	asm volatile("svc #1\nmov %0, r0\nmov %0, r1" : "=r"(t.s.lo), "=r"(t.s.hi));
+	asm volatile("svc #1\nmov %0, r1\nmov %1, r0" : "=r"(t.s.lo), "=r"(t.s.hi));
 	return t.llv;
 }
 
-#define SYS_SCHED 2
+#define SYS_YIELD 0
 #define SYS_TIME  1
+#define SYS_SCHED 2
 
 #endif
diff --git a/include/sys/schedule.h b/include/sys/schedule.h
index 4d331a6..cb784c5 100644
--- a/include/sys/schedule.h
+++ b/include/sys/schedule.h
@@ -64,6 +64,6 @@ struct Thread* next_thread(void);
 ///  to user mode then calls the SVC call
 extern void schedule(void);
 extern void cleanup(void);
-// void yield(void);
+void yield(void);
 
 #endif
diff --git a/src/cpu/irq.c b/src/cpu/irq.c
index 612b6d8..2197067 100644
--- a/src/cpu/irq.c
+++ b/src/cpu/irq.c
@@ -140,14 +140,15 @@ void testfxn2(void)
 
 void testfxn(void)
 {
-	unsigned int i = 0x69420;
+	unsigned long long ti = get_sys_time();
+	unsigned int i = 0xDEADBEEF;
 	void* a = malloc(5);
 	void* b = malloc(3);
 	void* c = malloc(4);
 	void* d = malloc(4);
 	uart_string("Start\n");
 	add_thread(testfxn2, 0, 0);
-	delay(0x20000000);
+	sys0(SYS_YIELD);
 	uart_string("Freeing B\n");
 	free(b);
 	uart_string("Freeing A\n");
@@ -160,4 +161,11 @@ void testfxn(void)
 	delay(0x20000000);
 	uart_hexn(i);
 	uart_string("End\n");
+	unsigned long long tf = get_sys_time();
+	unsigned long long dt = tf-ti;
+	unsigned long thi, tlo;
+	thi = dt >> 32;
+	tlo = dt;
+	uart_hex(thi);
+	uart_hexn(tlo);
 }
diff --git a/src/exceptions/svc.S b/src/exceptions/svc.S
index 51c6479..7e6a327 100644
--- a/src/exceptions/svc.S
+++ b/src/exceptions/svc.S
@@ -12,12 +12,13 @@ svc:
 	beq svc_000002
 	cmp r0, #1
 	beq svc_000001
-	cmp r0, #1
+	cmp r0, #0
 	beq svc_000000
 svc_000000:
-	cps #0x13
-	b svc_exit
-svc_000001:
+	bl yield
+	ldmfd sp!, {r0-r12,lr}
+	b schedule
+svc_000001: // Get time
 	mov r2, #0x3004
 	movt r2, #0x3F00
 	ldr r0, [r2, #4] // <- SYS_TIMER_CLO
@@ -25,10 +26,10 @@ svc_000001:
 	str r0, [sp]	 // Return value
 	str r1, [sp, #4] // Return value hi
 	b svc_exit
-svc_000002:
+svc_000002: // Run Schedule
 	ldmfd sp!, {r0-r12,lr}
 	b schedule
-svc_000003:
+svc_000003: // Clean task stack
 	ldr r3, =scheduler
 	ldr r2, [r3, #0] // struct Thread* rthread
 	ldr r1, [r2, #8] // sp_base
diff --git a/src/sys/schedule.c b/src/sys/schedule.c
index 6f2a72d..70aebb9 100644
--- a/src/sys/schedule.c
+++ b/src/sys/schedule.c
@@ -156,3 +156,16 @@ void* get_rthread_roffset(void)
 {
 	return &scheduler.thread_queues[scheduler.rthread->priority].ready.roffset;
 }
+
+void yield(void)
+{
+	struct Thread* rthread = scheduler.rthread;
+	if (rthread == &usrloopthread)
+		return;
+	unsigned char priority = rthread->priority;
+	struct ThreadRotBuffer* trb = &scheduler.thread_queues[priority].ready;
+	trb->roffset += 1;
+	trb->roffset %= TQUEUE_MAX;
+	trb->queue[trb->woffset++] = rthread;
+	trb->roffset %= TQUEUE_MAX;
+}
-- 
cgit v1.2.1