aboutsummaryrefslogtreecommitdiff
path: root/src/exceptions
diff options
context:
space:
mode:
Diffstat (limited to 'src/exceptions')
-rw-r--r--src/exceptions/svc.S41
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}^