1+ ;/*
2+ ; * Copyright (c) 2016-2017 ARM Limited. All rights reserved.
3+ ; *
4+ ; * SPDX-License-Identifier: Apache-2.0
5+ ; *
6+ ; * Licensed under the Apache License, Version 2.0 (the License); you may
7+ ; * not use this file except in compliance with the License.
8+ ; * You may obtain a copy of the License at
9+ ; *
10+ ; * www.apache.org/licenses/LICENSE-2.0
11+ ; *
12+ ; * Unless required by applicable law or agreed to in writing, software
13+ ; * distributed under the License is distributed on an AS IS BASIS, WITHOUT
14+ ; * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
15+ ; * See the License for the specific language governing permissions and
16+ ; * limitations under the License.
17+ ; *
18+ ; * -----------------------------------------------------------------------------
19+ ; *
20+ ; * Project: CMSIS-RTOS RTX
21+ ; * Title: ARMv8M Baseline Exception handlers
22+ ; *
23+ ; * -----------------------------------------------------------------------------
24+ ; */
25+
26+ I_T_RUN_OFS EQU 28 ; osInfo.thread.run offset
27+ TCB_SM_OFS EQU 48 ; TCB.stack_mem offset
28+ TCB_SP_OFS EQU 56 ; TCB.SP offset
29+ TCB_SF_OFS EQU 34 ; TCB.stack_frame offset
30+ TCB_TZM_OFS EQU 64 ; TCB.tz_memory offset
31+
32+
33+ PRESERVE8
34+ THUMB
35+
36+
37+ AREA |.constdata| , DATA , READONLY
38+ EXPORT irqRtxLib
39+ irqRtxLib DCB 0 ; Non weak library reference
40+
41+
42+ AREA |.text| , CODE , READONLY
43+
44+
45+ SVC_Handler PROC
46+ EXPORT SVC_Handler
47+ IMPORT osRtxUserSVC
48+ IMPORT osRtxInfo
49+ #ifdef __DOMAIN_NS
50+ IMPORT TZ_LoadContext_S
51+ IMPORT TZ_StoreContext_S
52+ #endif
53+
54+ MRS R0 , PSP ; Get PSP
55+ LDR R1 ,[ R0 , # 24 ] ; Load saved PC from stack
56+ SUBS R1 , R1 , # 2 ; Point to SVC instruction
57+ LDRB R1 ,[ R1 ] ; Load SVC number
58+ CMP R1 , # 0
59+ BNE SVC_User ; Branch if not SVC 0
60+
61+ PUSH {R0 , LR} ; Save PSP and EXC_RETURN
62+ LDM R0 , {R0 - R3} ; Load function parameters from stack
63+ BLX R7 ; Call service function
64+ POP {R2 , R3} ; Restore PSP and EXC_RETURN
65+ STMIA R2! , {R0 - R1} ; Store function return values
66+ MOV LR , R3 ; Set EXC_RETURN
67+
68+ SVC_Context
69+ LDR R3 , =osRtxInfo + I_T_RUN_OFS ; Load address of osRtxInfo.run
70+ LDMIA R3! , {R1 , R2} ; Load osRtxInfo.thread.run: curr & next
71+ CMP R1 , R2 ; Check if thread switch is required
72+ BEQ SVC_Exit ; Branch when threads are the same
73+
74+ CBZ R1 , SVC_ContextSwitch ; Branch if running thread is deleted
75+
76+ SVC_ContextSave
77+ #ifdef __DOMAIN_NS
78+ LDR R0 ,[ R1 , #TCB_TZM_OFS ] ; Load TrustZone memory identifier
79+ CBZ R0 , SVC_ContextSave1 ; Branch if there is no secure context
80+ PUSH {R1 , R2 , R3 , R7} ; Save registers
81+ MOV R7 , LR ; Get EXC_RETURN
82+ BL TZ_StoreContext_S ; Store secure context
83+ MOV LR , R7 ; Set EXC_RETURN
84+ POP {R1 , R2 , R3 , R7} ; Restore registers
85+ #endif
86+
87+ SVC_ContextSave1
88+ MRS R0 , PSP ; Get PSP
89+ SUBS R0 , R0 , # 32 ; Adjust PSP
90+ STR R0 ,[ R1 , #TCB_SP_OFS ] ; Store SP
91+ STMIA R0! , {R4 - R7} ; Save R4..R7
92+ MOV R4 , R8
93+ MOV R5 , R9
94+ MOV R6 , R10
95+ MOV R7 , R11
96+ STMIA R0! , {R4 - R7} ; Save R8..R11
97+
98+ SVC_ContextSave2
99+ MOV R0 , LR ; Get EXC_RETURN
100+ ADDS R1 , R1 , #TCB_SF_OFS ; Adjust address
101+ STRB R0 ,[ R1 ] ; Store stack frame information
102+
103+ SVC_ContextSwitch
104+ SUBS R3 , R3 , # 8 ; Adjust address
105+ STR R2 ,[ R3 ] ; osRtxInfo.thread.run: curr = next
106+
107+ SVC_ContextRestore
108+ #ifdef __DOMAIN_NS
109+ LDR R0 ,[ R2 , #TCB_TZM_OFS ] ; Load TrustZone memory identifier
110+ CBZ R0 , SVC_ContextRestore1 ; Branch if there is no secure context
111+ PUSH {R2 , R3} ; Save registers
112+ BL TZ_LoadContext_S ; Load secure context
113+ POP {R2 , R3} ; Restore registers
114+ #endif
115+
116+ SVC_ContextRestore1
117+ MOV R1 , R2
118+ ADDS R1 , R1 , #TCB_SF_OFS ; Adjust address
119+ LDRB R0 ,[ R1 ] ; Load stack frame information
120+ MOVS R1 , # 0xFF
121+ MVNS R1 , R1 ; R1=0xFFFFFF00
122+ ORRS R0 , R1
123+ MOV LR , R0 ; Set EXC_RETURN
124+
125+ #ifdef __DOMAIN_NS
126+ LSLS R0 , R0 , # 25 ; Check domain of interrupted thread
127+ BPL SVC_ContextRestore2 ; Branch if non-secure
128+ LDR R0 ,[ R2 , #TCB_SP_OFS ] ; Load SP
129+ MSR PSP , R0 ; Set PSP
130+ BX LR ; Exit from handler
131+ #else
132+ LDR R0 ,[ R2 , #TCB_SM_OFS ] ; Load stack memory base
133+ MSR PSPLIM , R0 ; Set PSPLIM
134+ #endif
135+
136+ SVC_ContextRestore2
137+ LDR R0 ,[ R2 , #TCB_SP_OFS ] ; Load SP
138+ ADDS R0 , R0 , # 16 ; Adjust address
139+ LDMIA R0! , {R4 - R7} ; Restore R8..R11
140+ MOV R8 , R4
141+ MOV R9 , R5
142+ MOV R10 , R6
143+ MOV R11 , R7
144+ MSR PSP , R0 ; Set PSP
145+ SUBS R0 , R0 , # 32 ; Adjust address
146+ LDMIA R0! , {R4 - R7} ; Restore R4..R7
147+
148+ SVC_Exit
149+ BX LR ; Exit from handler
150+
151+ SVC_User
152+ PUSH {R4 , LR} ; Save registers
153+ LDR R2 , =osRtxUserSVC ; Load address of SVC table
154+ LDR R3 ,[ R2 ] ; Load SVC maximum number
155+ CMP R1 , R3 ; Check SVC number range
156+ BHI SVC_Done ; Branch if out of range
157+
158+ LSLS R1 , R1 , # 2
159+ LDR R4 ,[ R2 , R1 ] ; Load address of SVC function
160+
161+ LDM R0 , {R0 - R3} ; Load function parameters from stack
162+ BLX R4 ; Call service function
163+ MRS R4 , PSP ; Get PSP
164+ STR R0 ,[ R4 ] ; Store function return value
165+
166+ SVC_Done
167+ POP {R4 , PC} ; Return from handler
168+
169+ ALIGN
170+ ENDP
171+
172+
173+ PendSV_Handler PROC
174+ EXPORT PendSV_Handler
175+ IMPORT osRtxPendSV_Handler
176+
177+ PUSH {R0 , LR} ; Save EXC_RETURN
178+ BL osRtxPendSV_Handler ; Call osRtxPendSV_Handler
179+ POP {R0 , R1} ; Restore EXC_RETURN
180+ MOV LR , R1 ; Set EXC_RETURN
181+ B Sys_Context
182+
183+ ALIGN
184+ ENDP
185+
186+
187+ SysTick_Handler PROC
188+ EXPORT SysTick_Handler
189+ IMPORT osRtxTick_Handler
190+
191+ PUSH {R0 , LR} ; Save EXC_RETURN
192+ BL osRtxTick_Handler ; Call osRtxTick_Handler
193+ POP {R0 , R1} ; Restore EXC_RETURN
194+ MOV LR , R1 ; Set EXC_RETURN
195+ B Sys_Context
196+
197+ ALIGN
198+ ENDP
199+
200+
201+ Sys_Context PROC
202+ EXPORT Sys_Context
203+ IMPORT osRtxInfo
204+ #ifdef __DOMAIN_NS
205+ IMPORT TZ_LoadContext_S
206+ IMPORT TZ_StoreContext_S
207+ #endif
208+
209+ LDR R3 , =osRtxInfo + I_T_RUN_OFS ; Load address of osRtxInfo.run
210+ LDM R3! , {R1 , R2} ; Load osRtxInfo.thread.run: curr & next
211+ CMP R1 , R2 ; Check if thread switch is required
212+ BEQ Sys_ContextExit ; Branch when threads are the same
213+
214+ Sys_ContextSave
215+ #ifdef __DOMAIN_NS
216+ LDR R0 ,[ R1 , #TCB_TZM_OFS ] ; Load TrustZone memory identifier
217+ CBZ R0 , Sys_ContextSave1 ; Branch if there is no secure context
218+ PUSH {R1 , R2 , R3 , R7} ; Save registers
219+ MOV R7 , LR ; Get EXC_RETURN
220+ BL TZ_StoreContext_S ; Store secure context
221+ MOV LR , R7 ; Set EXC_RETURN
222+ POP {R1 , R2 , R3 , R7} ; Restore registers
223+ LSLS R7 , R7 , # 25 ; Check domain of interrupted thread
224+ BMI Sys_ContextSave1 ; Branch if secure
225+ MRS R0 , PSP ; Get PSP
226+ STR R0 ,[ R1 , #TCB_SP_OFS ] ; Store SP
227+ B Sys_ContextSave2
228+ #endif
229+
230+ Sys_ContextSave1
231+ MRS R0 , PSP ; Get PSP
232+ SUBS R0 , R0 , # 32 ; Adjust address
233+ STR R0 ,[ R1 , #TCB_SP_OFS ] ; Store SP
234+ STMIA R0! , {R4 - R7} ; Save R4..R7
235+ MOV R4 , R8
236+ MOV R5 , R9
237+ MOV R6 , R10
238+ MOV R7 , R11
239+ STMIA R0! , {R4 - R7} ; Save R8..R11
240+
241+ Sys_ContextSave2
242+ MOV R0 , LR ; Get EXC_RETURN
243+ ADDS R1 , R1 , #TCB_SF_OFS ; Adjust address
244+ STRB R0 ,[ R1 ] ; Store stack frame information
245+
246+ Sys_ContextSwitch
247+ SUBS R3 , R3 , # 8 ; Adjust address
248+ STR R2 ,[ R3 ] ; osRtxInfo.run: curr = next
249+
250+ Sys_ContextRestore
251+ #ifdef __DOMAIN_NS
252+ LDR R0 ,[ R2 , #TCB_TZM_OFS ] ; Load TrustZone memory identifier
253+ CBZ R0 , Sys_ContextRestore1 ; Branch if there is no secure context
254+ PUSH {R2 , R3} ; Save registers
255+ BL TZ_LoadContext_S ; Load secure context
256+ POP {R2 , R3} ; Restore registers
257+ #endif
258+
259+ Sys_ContextRestore1
260+ MOV R1 , R2
261+ ADDS R1 , R1 , #TCB_SF_OFS ; Adjust offset
262+ LDRB R0 ,[ R1 ] ; Load stack frame information
263+ MOVS R1 , # 0xFF
264+ MVNS R1 , R1 ; R1=0xFFFFFF00
265+ ORRS R0 , R1
266+ MOV LR , R0 ; Set EXC_RETURN
267+
268+ #ifdef __DOMAIN_NS
269+ LSLS R0 , R0 , # 25 ; Check domain of interrupted thread
270+ BPL Sys_ContextRestore2 ; Branch if non-secure
271+ LDR R0 ,[ R2 , #TCB_SP_OFS ] ; Load SP
272+ MSR PSP , R0 ; Set PSP
273+ BX LR ; Exit from handler
274+ #else
275+ LDR R0 ,[ R2 , #TCB_SM_OFS ] ; Load stack memory base
276+ MSR PSPLIM , R0 ; Set PSPLIM
277+ #endif
278+
279+ Sys_ContextRestore2
280+ LDR R0 ,[ R2 , #TCB_SP_OFS ] ; Load SP
281+ ADDS R0 , R0 , # 16 ; Adjust address
282+ LDMIA R0! , {R4 - R7} ; Restore R8..R11
283+ MOV R8 , R4
284+ MOV R9 , R5
285+ MOV R10 , R6
286+ MOV R11 , R7
287+ MSR PSP , R0 ; Set PSP
288+ SUBS R0 , R0 , # 32 ; Adjust address
289+ LDMIA R0! , {R4 - R7} ; Restore R4..R7
290+
291+ Sys_ContextExit
292+ BX LR ; Exit from handler
293+
294+ ALIGN
295+ ENDP
296+
297+
298+ END
0 commit comments