5 files changed, 27 insertions, 9 deletions
diff --git a/include/cpu.h b/include/cpu.h
index 393fe2f..202c6f5 100644
--- a/include/cpu.h
+++ b/include/cpu.h
@@ -91,7 +91,7 @@ static inline void* getirqstack(void)
 #define SYS_TIME        1
 #define SYS_SCHED       2
 #define SYS_YIELD_HIGH  2
-#define SYS_FREE_STACK  3
+#define SYS_ADD_THREAD  3
 #define SYS_LOCK        4
 #define SYS_UNLOCK      5
 #define SYS_SEMAPHORE_P 6
diff --git a/src/exceptions/svc.S b/src/exceptions/svc.S
index 8494281..d14344b 100644
--- a/src/exceptions/svc.S
+++ b/src/exceptions/svc.S
@@ -27,8 +27,15 @@ svc_000001: // SYS_TIME
 svc_000002: // Run Schedule
 	ldmfd sp!, {r0-r12,lr}
 	b schedule
-svc_000003: // Unused
-	b svc_exit
+svc_000003: // Add Thread
+	ldr r0, [sp, #0]
+	ldr r1, [sp, #4]
+	ldr r2, [sp, #8]
+	and r2, #0xFF
+	bl svc_add_thread
+	str r0, [sp, #0]
+	ldmfd sp!, {r0-r12,lr}
+	b schedule
 svc_000004: // Lock Lock (usr_r0 = struct Lock*)
 	ldr r3, =scheduler
 	ldr r2, [r3, #0] // struct Thread* rthread
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;
 }