/
irq_armv8mbl_common.S
301 lines (252 loc) · 12.8 KB
/
irq_armv8mbl_common.S
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
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
;/*
; * Copyright (c) 2016-2018 Arm Limited. All rights reserved.
; *
; * SPDX-License-Identifier: Apache-2.0
; *
; * Licensed under the Apache License, Version 2.0 (the License); you may
; * not use this file except in compliance with the License.
; * You may obtain a copy of the License at
; *
; * www.apache.org/licenses/LICENSE-2.0
; *
; * Unless required by applicable law or agreed to in writing, software
; * distributed under the License is distributed on an AS IS BASIS, WITHOUT
; * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
; * See the License for the specific language governing permissions and
; * limitations under the License.
; *
; * -----------------------------------------------------------------------------
; *
; * Project: CMSIS-RTOS RTX
; * Title: ARMv8M Baseline Exception handlers
; *
; * -----------------------------------------------------------------------------
; */
#ifndef DOMAIN_NS
#define DOMAIN_NS 0
#endif
I_T_RUN_OFS EQU 20 ; osRtxInfo.thread.run offset
TCB_SM_OFS EQU 48 ; TCB.stack_mem offset
TCB_SP_OFS EQU 56 ; TCB.SP offset
TCB_SF_OFS EQU 34 ; TCB.stack_frame offset
TCB_TZM_OFS EQU 64 ; TCB.tz_memory offset
PRESERVE8
SECTION .rodata:DATA:NOROOT(2)
EXPORT irqRtxLib
irqRtxLib DCB 0 ; Non weak library reference
SECTION .text:CODE:NOROOT(2)
THUMB
SVC_Handler
EXPORT SVC_Handler
IMPORT osRtxUserSVC
IMPORT osRtxInfo
#if (DOMAIN_NS == 1)
IMPORT TZ_LoadContext_S
IMPORT TZ_StoreContext_S
#endif
MOV R0,LR
LSRS R0,R0,#3 ; Determine return stack from EXC_RETURN bit 2
BCC SVC_MSP ; Branch if return stack is MSP
MRS R0,PSP ; Get PSP
SVC_Number
LDR R1,[R0,#24] ; Load saved PC from stack
SUBS R1,R1,#2 ; Point to SVC instruction
LDRB R1,[R1] ; Load SVC number
CMP R1,#0
BNE SVC_User ; Branch if not SVC 0
PUSH {R0,LR} ; Save SP and EXC_RETURN
LDM R0,{R0-R3} ; Load function parameters from stack
BLX R7 ; Call service function
POP {R2,R3} ; Restore SP and EXC_RETURN
STMIA R2!,{R0-R1} ; Store function return values
MOV LR,R3 ; Set EXC_RETURN
SVC_Context
LDR R3,=osRtxInfo+I_T_RUN_OFS; Load address of osRtxInfo.run
LDMIA R3!,{R1,R2} ; Load osRtxInfo.thread.run: curr & next
CMP R1,R2 ; Check if thread switch is required
BEQ SVC_Exit ; Branch when threads are the same
CBZ R1,SVC_ContextSwitch ; Branch if running thread is deleted
SVC_ContextSave
#if (DOMAIN_NS == 1)
LDR R0,[R1,#TCB_TZM_OFS] ; Load TrustZone memory identifier
CBZ R0,SVC_ContextSave1 ; Branch if there is no secure context
PUSH {R1,R2,R3,R7} ; Save registers
MOV R7,LR ; Get EXC_RETURN
BL TZ_StoreContext_S ; Store secure context
MOV LR,R7 ; Set EXC_RETURN
POP {R1,R2,R3,R7} ; Restore registers
#endif
SVC_ContextSave1
MRS R0,PSP ; Get PSP
SUBS R0,R0,#32 ; Calculate SP
STR R0,[R1,#TCB_SP_OFS] ; Store SP
STMIA R0!,{R4-R7} ; Save R4..R7
MOV R4,R8
MOV R5,R9
MOV R6,R10
MOV R7,R11
STMIA R0!,{R4-R7} ; Save R8..R11
SVC_ContextSave2
MOV R0,LR ; Get EXC_RETURN
ADDS R1,R1,#TCB_SF_OFS ; Adjust address
STRB R0,[R1] ; Store stack frame information
SVC_ContextSwitch
SUBS R3,R3,#8 ; Adjust address
STR R2,[R3] ; osRtxInfo.thread.run: curr = next
SVC_ContextRestore
#if (DOMAIN_NS == 1)
LDR R0,[R2,#TCB_TZM_OFS] ; Load TrustZone memory identifier
CBZ R0,SVC_ContextRestore1 ; Branch if there is no secure context
PUSH {R2,R3} ; Save registers
BL TZ_LoadContext_S ; Load secure context
POP {R2,R3} ; Restore registers
#endif
SVC_ContextRestore1
MOV R1,R2
ADDS R1,R1,#TCB_SF_OFS ; Adjust address
LDRB R0,[R1] ; Load stack frame information
MOVS R1,#0xFF
MVNS R1,R1 ; R1=0xFFFFFF00
ORRS R0,R1
MOV LR,R0 ; Set EXC_RETURN
#if (DOMAIN_NS == 1)
LSLS R0,R0,#25 ; Check domain of interrupted thread
BPL SVC_ContextRestore2 ; Branch if non-secure
LDR R0,[R2,#TCB_SP_OFS] ; Load SP
MSR PSP,R0 ; Set PSP
BX LR ; Exit from handler
#else
LDR R0,[R2,#TCB_SM_OFS] ; Load stack memory base
MSR PSPLIM,R0 ; Set PSPLIM
#endif
SVC_ContextRestore2
LDR R0,[R2,#TCB_SP_OFS] ; Load SP
ADDS R0,R0,#16 ; Adjust address
LDMIA R0!,{R4-R7} ; Restore R8..R11
MOV R8,R4
MOV R9,R5
MOV R10,R6
MOV R11,R7
MSR PSP,R0 ; Set PSP
SUBS R0,R0,#32 ; Adjust address
LDMIA R0!,{R4-R7} ; Restore R4..R7
SVC_Exit
BX LR ; Exit from handler
SVC_MSP
MRS R0,MSP ; Get MSP
B SVC_Number
SVC_User
LDR R2,=osRtxUserSVC ; Load address of SVC table
LDR R3,[R2] ; Load SVC maximum number
CMP R1,R3 ; Check SVC number range
BHI SVC_Exit ; Branch if out of range
PUSH {R0,LR} ; Save SP and EXC_RETURN
LSLS R1,R1,#2
LDR R3,[R2,R1] ; Load address of SVC function
MOV R12,R3
LDMIA R0,{R0-R3} ; Load function parameters from stack
BLX R12 ; Call service function
POP {R2,R3} ; Restore SP and EXC_RETURN
STR R0,[R2] ; Store function return value
MOV LR,R3 ; Set EXC_RETURN
BX LR ; Return from handler
PendSV_Handler
EXPORT PendSV_Handler
IMPORT osRtxPendSV_Handler
PUSH {R0,LR} ; Save EXC_RETURN
BL osRtxPendSV_Handler ; Call osRtxPendSV_Handler
POP {R0,R1} ; Restore EXC_RETURN
MOV LR,R1 ; Set EXC_RETURN
B Sys_Context
SysTick_Handler
EXPORT SysTick_Handler
IMPORT osRtxTick_Handler
PUSH {R0,LR} ; Save EXC_RETURN
BL osRtxTick_Handler ; Call osRtxTick_Handler
POP {R0,R1} ; Restore EXC_RETURN
MOV LR,R1 ; Set EXC_RETURN
B Sys_Context
Sys_Context
EXPORT Sys_Context
IMPORT osRtxInfo
#if (DOMAIN_NS == 1)
IMPORT TZ_LoadContext_S
IMPORT TZ_StoreContext_S
#endif
LDR R3,=osRtxInfo+I_T_RUN_OFS; Load address of osRtxInfo.run
LDM R3!,{R1,R2} ; Load osRtxInfo.thread.run: curr & next
CMP R1,R2 ; Check if thread switch is required
BEQ Sys_ContextExit ; Branch when threads are the same
Sys_ContextSave
#if (DOMAIN_NS == 1)
LDR R0,[R1,#TCB_TZM_OFS] ; Load TrustZone memory identifier
CBZ R0,Sys_ContextSave1 ; Branch if there is no secure context
PUSH {R1,R2,R3,R7} ; Save registers
MOV R7,LR ; Get EXC_RETURN
BL TZ_StoreContext_S ; Store secure context
MOV LR,R7 ; Set EXC_RETURN
POP {R1,R2,R3,R7} ; Restore registers
MOV R0,LR ; Get EXC_RETURN
LSLS R0,R0,#25 ; Check domain of interrupted thread
BPL Sys_ContextSave1 ; Branch if non-secure
MRS R0,PSP ; Get PSP
STR R0,[R1,#TCB_SP_OFS] ; Store SP
B Sys_ContextSave2
#endif
Sys_ContextSave1
MRS R0,PSP ; Get PSP
SUBS R0,R0,#32 ; Adjust address
STR R0,[R1,#TCB_SP_OFS] ; Store SP
STMIA R0!,{R4-R7} ; Save R4..R7
MOV R4,R8
MOV R5,R9
MOV R6,R10
MOV R7,R11
STMIA R0!,{R4-R7} ; Save R8..R11
Sys_ContextSave2
MOV R0,LR ; Get EXC_RETURN
ADDS R1,R1,#TCB_SF_OFS ; Adjust address
STRB R0,[R1] ; Store stack frame information
Sys_ContextSwitch
SUBS R3,R3,#8 ; Adjust address
STR R2,[R3] ; osRtxInfo.run: curr = next
Sys_ContextRestore
#if (DOMAIN_NS == 1)
LDR R0,[R2,#TCB_TZM_OFS] ; Load TrustZone memory identifier
CBZ R0,Sys_ContextRestore1 ; Branch if there is no secure context
PUSH {R2,R3} ; Save registers
BL TZ_LoadContext_S ; Load secure context
POP {R2,R3} ; Restore registers
#endif
Sys_ContextRestore1
MOV R1,R2
ADDS R1,R1,#TCB_SF_OFS ; Adjust offset
LDRB R0,[R1] ; Load stack frame information
MOVS R1,#0xFF
MVNS R1,R1 ; R1=0xFFFFFF00
ORRS R0,R1
MOV LR,R0 ; Set EXC_RETURN
#if (DOMAIN_NS == 1)
LSLS R0,R0,#25 ; Check domain of interrupted thread
BPL Sys_ContextRestore2 ; Branch if non-secure
LDR R0,[R2,#TCB_SP_OFS] ; Load SP
MSR PSP,R0 ; Set PSP
BX LR ; Exit from handler
#else
LDR R0,[R2,#TCB_SM_OFS] ; Load stack memory base
MSR PSPLIM,R0 ; Set PSPLIM
#endif
Sys_ContextRestore2
LDR R0,[R2,#TCB_SP_OFS] ; Load SP
ADDS R0,R0,#16 ; Adjust address
LDMIA R0!,{R4-R7} ; Restore R8..R11
MOV R8,R4
MOV R9,R5
MOV R10,R6
MOV R11,R7
MSR PSP,R0 ; Set PSP
SUBS R0,R0,#32 ; Adjust address
LDMIA R0!,{R4-R7} ; Restore R4..R7
Sys_ContextExit
BX LR ; Exit from handler
END