1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
|
.section ".text.exceptions"
.globl svc
svc:
cpsid aif
stmfd sp!, {r0-r12,lr}
// Get the SVC Exception #
ldr r0, [lr, #-4]
bic r0, #0xFF000000
// Check it is within our defined SVC
cmp r0, #7
adrle r3, svc_table_1
ldrle pc, [r3, r0, LSL #2]
sub r0, #8
cmp r0, #3
bgt svc_exit
//// Jump to the appropriate Call
adr r3, svc_table_2
ldr pc, [r3, r0, LSL #2]
svc_000000: // SYS_YIELD
bl yield
ldmfd sp!, {r0-r12,lr}
b schedule
svc_000001: // SYS_TIME
movw r2, #0x3004
movt r2, #0x3F00
ldr r0, [r2, #4] // <- SYS_TIMER_CLO
ldr r1, [r2, #0] // <- SYS_TIMER_CHI
str r0, [sp] // Return value
str r1, [sp, #4] // Return value hi
b svc_exit
svc_000002: // Run Schedule
ldmfd sp!, {r0-r12,lr}
b schedule
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
ldr r1, [r2, #0x10] // unsigned long pid
ldr r0, [sp, #0] // struct Lock* m
1: clrex
ldrex r2, [r0, #0]
cmp r2, #0
// If it is not available, wait-queue the thread
bne svc_000004_delay_mutex
// Otherwise lock it
strexeq r2, r1, [r0, #0]
teq r2, #0
bne 1b
dmb
b svc_exit
svc_000004_delay_mutex: // Wait-queue the current thread
// r0 = struct Lock* m
bl sched_mutex_yield
ldmfd sp!, {r0-r12,lr}
sub lr, #4
b schedule
svc_000005: // Release Lock
ldr r0, [sp, #0] // struct Lock* m
mov r1, #0
dmb
// Unlock
str r1, [r0, #0]
dsb
sev
// Awake any threads waiting for this lock
bl sched_mutex_resurrect
cmp r0, #0
beq 1f
ldmfd sp!, {r0-r12,lr}
b schedule
1: b svc_exit
svc_000006: // Semaphore decrease
ldr r0, [sp, #0] // struct Semaphore* s
1: clrex
ldrex r2, [r0, #0]
cmp r2, #0
beq svc_000006_delay_semaphore
sub r1, r2, #1
strex r2, r1, [r0, #0]
teq r2, #0
bne 1b
dmb
b svc_exit
svc_000006_delay_semaphore:
bl sched_semaphore_yield
ldmfd sp!, {r0-r12,lr}
sub lr, #4
b schedule
b svc_exit
svc_000007: // Semaphore increase
ldr r0, [sp, #0] // struct Semaphore* s
1: clrex
ldrex r2, [r0, #0]
add r1, r2, #1
strexeq r2, r1, [r0, #0]
teq r2, #0
bne 1b
dmb
cmp r1, #1
bne svc_exit
mov r1, #1
bl sched_semaphore_resurrect
cmp r0, #0
beq 1f
ldmfd sp!, {r0-r12,lr}
b schedule
1: b svc_exit
svc_000008: // Semaphore add #
ldr r0, [sp, #0] // struct Semaphore* s
ldr r3, [sp, #1] // unsigned long # times to increase
1: clrex
ldrex r2, [r0, #0]
add r1, r2, #1
strexeq r2, r1, [r0, #0]
teq r2, #0
bne 1b
dmb
mov r1, r3
bl sched_semaphore_resurrect
cmp r0, #0
beq 1f
ldmfd sp!, {r0-r12,lr}
b schedule
1: b svc_exit
svc_000009: // SYS_TIME_2
mrc p15, 0, r0, c9, c13, 0
str r0, [sp, #0]
b svc_exit
svc_00000A: // SYS_CNTV_ENABLE
ldr r0, =cntfrq
ldr r0, [r0]
mcr p15, 0, r0, c14, c3, 0
mov r0, #1
mcr p15, 0, r0, c14, c3, 1
b svc_exit
svc_00000B: // SYS_CNTV_DISABLE
mov r0, #0
mcr p15, 0, r0, c14, c3, 1
b svc_exit
svc_exit:
ldmfd sp!, {r0-r12,pc}^
svc_table_1:
.word svc_000000
.word svc_000001
.word svc_000002
.word svc_000003
.word svc_000004
.word svc_000005
.word svc_000006
.word svc_000007
svc_table_2:
.word svc_000008
.word svc_000009
.word svc_00000A
.word svc_00000B
|
