Added SVC jumps

1 files changed, 18 insertions, 11 deletions

diff --git a/src/boot.S b/src/boot.S
index 5c157e0..1faf860 100644
--- a/src/boot.S
+++ b/src/boot.S
@@ -136,30 +136,37 @@ svc:
 	// SVC #0 returns to supervisor mode
 	// TODO: Make supervisor mode return to a specific location
 	//  (rather than to a user location) such as the kernel loop
-	cmp r0, #0
-	bne 3f
-	cps #0x13
-	b   1f
-3:
+	cmp r0, #3
+	bgt svc_exit
+	beq svc_000003
 	cmp r0, #2
-	bne 2f
+	beq svc_000002
+	cmp r0, #1
+	beq svc_000001
+	cmp r0, #1
+	beq svc_000000
+svc_000000:
+	cps #0x13
+	b svc_exit
+svc_000001:
+	b svc_exit
+svc_000002:
 	ldmfd sp!, {r0-r12,lr}
 	b schedule
-2:
-	cmp r0, #3
-	bne 1f
+svc_000003:
 	ldr r3, =scheduler
 	ldr r2, [r3, #0]
 	ldr r1, [r2, #8] // sp_base
 	cmp r1, #-1
-	beq 1f
+	beq svc_exit
 	ldr r3, =stacks_table
 	mov r0, #0
 	strb r0, [r3, r1]
 	// Free the thread after freeing the stack
 	mov r0, r2
 	bl free
-1:
+	b svc_exit
+svc_exit:
 	ldmfd sp!, {r0-r12,pc}^
 prefetch:
 	stmfd sp!, {r0-r12,lr}