3 files changed, 17 insertions, 6 deletions
diff --git a/src/sys/kernel.S b/src/sys/kernel.S
index 9184c76..4432ccd 100644
--- a/src/sys/kernel.S
+++ b/src/sys/kernel.S
@@ -20,6 +20,12 @@ kernel_main:
 	wfe
 	b 2b
 
+testf:
+	push {lr}
+	mrs r0, cpsr
+	bl uart_hexn
+	pop {pc}
+
 .section .data
 ttbr_msg:
 	.asciz "MMU Initialized!"
diff --git a/src/sys/schedule.S b/src/sys/schedule.S
index 6b3d3e1..a47252c 100644
--- a/src/sys/schedule.S
+++ b/src/sys/schedule.S
@@ -41,3 +41,13 @@ cleanup:
 kernel_usr_task_loop:
 	wfe
 	b kernel_usr_task_loop
+
+.globl add_thread
+add_thread:
+	mrs r3, cpsr
+	and r3, #0x1F
+	cmp r3, #0x10
+	beq 1f
+	b svc_add_thread
+1:	svc #3
+	bx lr
diff --git a/src/sys/schedule.c b/src/sys/schedule.c
index 00935c9..c3c549f 100644
--- a/src/sys/schedule.c
+++ b/src/sys/schedule.c
@@ -250,7 +250,7 @@ unsigned char add_thread_without_duplicate(void* pc, void* arg, unsigned char pr
 	return 1;
 }
 
-unsigned char add_thread(void* pc, void* arg, unsigned char priority)
+unsigned char svc_add_thread(void* pc, void* arg, unsigned char priority)
 {
 	struct Entry* thread_entry = get_unused_thread();
 	// The only point-of-failure is not having a thread available
@@ -283,11 +283,6 @@ unsigned char add_thread(void* pc, void* arg, unsigned char priority)
 	thread->preempt = 0;
 	/// Add Thread to Scheduler
 	push_thread_to_queue(thread, THREAD_READY, thread->priority);
-	// Schedule if this was called in usermode
-	unsigned long mode = getmode() & 0x1F;
-	if (mode == 0x10) {
-		sys0(SYS_YIELD_HIGH);
-	}
 	return 0;
 }