1 files changed, 40 insertions, 1 deletions
diff --git a/src/exceptions/svc.S b/src/exceptions/svc.S
index 48d03a5..fc4653a 100644
--- a/src/exceptions/svc.S
+++ b/src/exceptions/svc.S
@@ -5,8 +5,12 @@ svc:
 	stmfd sp!, {r0-r12,lr}
 	ldr r0, [lr, #-4]
 	bic r0, #0xFF000000
-	cmp r0, #3
+	cmp r0, #5
 	bgt svc_exit
+	beq svc_000005
+	cmp r0, #4
+	beq svc_000004
+	cmp r0, #3
 	beq svc_000003
 	cmp r0, #2
 	beq svc_000002
@@ -42,6 +46,41 @@ svc_000003: // Clean task stack
 	mov r0, r2
 	bl kfree
 	b svc_exit
+svc_000004: // Lock Mutex (usr_r0 = struct Mutex*)
+	ldr r3, =scheduler
+	ldr r2, [r3, #0] // struct Thread* rthread
+	ldr r1, [r2, #0x10] // unsigned long pid
+	ldr r0, [sp, #0] // struct Mutex* m
+	add r0, #4 // Point to pid
+1:	clrex
+	ldrex r2, [r0, #0]
+	cmp r2, #0
+	bne svc_000004_delay_mutex
+	strexeq r2, r1, [r0, #0]
+	teq r2, #0
+	bne 1b
+	dmb
+	b svc_exit
+svc_000004_delay_mutex:
+	// r0 = struct Mutex* m
+	sub r0, #4
+	bl sched_mutex_yield
+	ldmfd sp!, {r0-r12,lr}
+	b schedule
+svc_000005: // Release Mutex
+	ldr r0, [sp, #0] // struct Mutex* m
+	add r0, #4
+	mov r1, #0
+	dmb
+	str r1, [r0, #0]
+	dsb
+	sev
+	// TODO: Branch to scheduler to awake threads awaiting mutex
+	sub r0, #4
+	bl sched_mutex_resurrect
+	ldmfd sp!, {r0-r12,lr}
+	b schedule
+	b svc_exit
 svc_exit:
 	ldmfd sp!, {r0-r12,pc}^