Skip to content

Commit 8c44554

Browse files
Deepikaadbridge
Deepika
authored and
adbridge
committed
Added RTX5 context switcher files
1 parent 651e3fc commit 8c44554

File tree

3 files changed

+543
-31
lines changed

3 files changed

+543
-31
lines changed

‎rtos/TARGET_CORTEX/rtx5/TARGET_M33/TOOLCHAIN_ARM/irq_armv8mml.S

Lines changed: 267 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,267 @@
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 Mainline Exception handlers
22+
; *
23+
; * -----------------------------------------------------------------------------
24+
; */
25+
26+
27+
I_T_RUN_OFS EQU 28 ; osRtxInfo.thread.run offset
28+
TCB_SM_OFS EQU 48 ; TCB.stack_mem offset
29+
TCB_SP_OFS EQU 56 ; TCB.SP offset
30+
TCB_SF_OFS EQU 34 ; TCB.stack_frame offset
31+
TCB_TZM_OFS EQU 64 ; TCB.tz_memory offset
32+
33+
34+
PRESERVE8
35+
THUMB
36+
37+
38+
AREA |.constdata|, DATA, READONLY
39+
EXPORT irqRtxLib
40+
irqRtxLib DCB 0 ; Non weak library reference
41+
42+
43+
AREA |.text|, CODE, READONLY
44+
45+
46+
SVC_Handler PROC
47+
EXPORT SVC_Handler
48+
IMPORT osRtxUserSVC
49+
IMPORT osRtxInfo
50+
#ifdef __DOMAIN_NS
51+
IMPORT TZ_LoadContext_S
52+
IMPORT TZ_StoreContext_S
53+
#endif
54+
55+
MRS R0,PSP ; Get PSP
56+
LDR R1,[R0,#24] ; Load saved PC from stack
57+
LDRB R1,[R1,#-2] ; 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,R12} ; Load function parameters and address from stack
63+
BLX R12 ; Call service function
64+
POP {R12,LR} ; Restore PSP and EXC_RETURN
65+
STM R12,{R0-R1} ; Store function return values
66+
67+
SVC_Context
68+
LDR R3,=osRtxInfo+I_T_RUN_OFS; Load address of osRtxInfo.run
69+
LDM R3,{R1,R2} ; Load osRtxInfo.thread.run: curr & next
70+
CMP R1,R2 ; Check if thread switch is required
71+
BXEQ LR ; Exit when threads are the same
72+
73+
#ifdef __FPU_USED
74+
CBNZ R1,SVC_ContextSave ; Branch if running thread is not deleted
75+
TST LR,#0x10 ; Check if extended stack frame
76+
BNE SVC_ContextSwitch
77+
LDR R1,=0xE000EF34 ; FPCCR Address
78+
LDR R0,[R1] ; Load FPCCR
79+
BIC R0,#1 ; Clear LSPACT (Lazy state)
80+
STR R0,[R1] ; Store FPCCR
81+
B SVC_ContextSwitch
82+
#else
83+
CBZ R1,SVC_ContextSwitch ; Branch if running thread is deleted
84+
#endif
85+
86+
SVC_ContextSave
87+
#ifdef __DOMAIN_NS
88+
LDR R0,[R1,#TCB_TZM_OFS] ; Load TrustZone memory identifier
89+
CBZ R0,SVC_ContextSave1 ; Branch if there is no secure context
90+
PUSH {R1,R2,R3,LR} ; Save registers and EXC_RETURN
91+
BL TZ_StoreContext_S ; Store secure context
92+
POP {R1,R2,R3,LR} ; Restore registers and EXC_RETURN
93+
#endif
94+
95+
SVC_ContextSave1
96+
MRS R0,PSP ; Get PSP
97+
STMDB R0!,{R4-R11} ; Save R4..R11
98+
#ifdef __FPU_USED
99+
TST LR,#0x10 ; Check if extended stack frame
100+
VSTMDBEQ R0!,{S16-S31} ; Save VFP S16.S31
101+
#endif
102+
103+
SVC_ContextSave2
104+
STR R0,[R1,#TCB_SP_OFS] ; Store SP
105+
STRB LR,[R1,#TCB_SF_OFS] ; Store stack frame information
106+
107+
SVC_ContextSwitch
108+
STR R2,[R3] ; osRtxInfo.thread.run: curr = next
109+
110+
SVC_ContextRestore
111+
#ifdef __DOMAIN_NS
112+
LDR R0,[R2,#TCB_TZM_OFS] ; Load TrustZone memory identifier
113+
CBZ R0,SVC_ContextRestore1 ; Branch if there is no secure context
114+
PUSH {R2,R3} ; Save registers
115+
BL TZ_LoadContext_S ; Load secure context
116+
POP {R2,R3} ; Restore registers
117+
#endif
118+
119+
SVC_ContextRestore1
120+
LDR R0,[R2,#TCB_SM_OFS] ; Load stack memory base
121+
LDRB R1,[R2,#TCB_SF_OFS] ; Load stack frame information
122+
MSR PSPLIM,R0 ; Set PSPLIM
123+
LDR R0,[R2,#TCB_SP_OFS] ; Load SP
124+
ORR LR,R1,#0xFFFFFF00 ; Set EXC_RETURN
125+
126+
#ifdef __DOMAIN_NS
127+
TST LR,#0x40 ; Check domain of interrupted thread
128+
BNE SVC_ContextRestore2 ; Branch if secure
129+
#endif
130+
131+
#ifdef __FPU_USED
132+
TST LR,#0x10 ; Check if extended stack frame
133+
VLDMIAEQ R0!,{S16-S31} ; Restore VFP S16..S31
134+
#endif
135+
LDMIA R0!,{R4-R11} ; Restore R4..R11
136+
137+
SVC_ContextRestore2
138+
MSR PSP,R0 ; Set PSP
139+
140+
SVC_Exit
141+
BX LR ; Exit from handler
142+
143+
SVC_User
144+
PUSH {R4,LR} ; Save registers
145+
LDR R2,=osRtxUserSVC ; Load address of SVC table
146+
LDR R3,[R2] ; Load SVC maximum number
147+
CMP R1,R3 ; Check SVC number range
148+
BHI SVC_Done ; Branch if out of range
149+
150+
LDR R4,[R2,R1,LSL #2] ; Load address of SVC function
151+
152+
LDM R0,{R0-R3} ; Load function parameters from stack
153+
BLX R4 ; Call service function
154+
MRS R4,PSP ; Get PSP
155+
STR R0,[R4] ; Store function return value
156+
157+
SVC_Done
158+
POP {R4,PC} ; Return from handler
159+
160+
ALIGN
161+
ENDP
162+
163+
164+
PendSV_Handler PROC
165+
EXPORT PendSV_Handler
166+
IMPORT osRtxPendSV_Handler
167+
168+
PUSH {R4,LR} ; Save EXC_RETURN
169+
BL osRtxPendSV_Handler ; Call osRtxPendSV_Handler
170+
POP {R4,LR} ; Restore EXC_RETURN
171+
B Sys_Context
172+
173+
ALIGN
174+
ENDP
175+
176+
177+
SysTick_Handler PROC
178+
EXPORT SysTick_Handler
179+
IMPORT osRtxTick_Handler
180+
181+
PUSH {R4,LR} ; Save EXC_RETURN
182+
BL osRtxTick_Handler ; Call osRtxTick_Handler
183+
POP {R4,LR} ; Restore EXC_RETURN
184+
B Sys_Context
185+
186+
ALIGN
187+
ENDP
188+
189+
190+
Sys_Context PROC
191+
EXPORT Sys_Context
192+
IMPORT osRtxInfo
193+
#ifdef __DOMAIN_NS
194+
IMPORT TZ_LoadContext_S
195+
IMPORT TZ_StoreContext_S
196+
#endif
197+
198+
LDR R3,=osRtxInfo+I_T_RUN_OFS; Load address of osRtxInfo.run
199+
LDM R3,{R1,R2} ; Load osRtxInfo.thread.run: curr & next
200+
CMP R1,R2 ; Check if thread switch is required
201+
BXEQ LR ; Exit when threads are the same
202+
203+
Sys_ContextSave
204+
#ifdef __DOMAIN_NS
205+
LDR R0,[R1,#TCB_TZM_OFS] ; Load TrustZone memory identifier
206+
CBZ R0,Sys_ContextSave1 ; Branch if there is no secure context
207+
PUSH {R1,R2,R3,LR} ; Save registers and EXC_RETURN
208+
BL TZ_StoreContext_S ; Store secure context
209+
POP {R1,R2,R3,LR} ; Restore registers and EXC_RETURN
210+
TST LR,#0x40 ; Check domain of interrupted thread
211+
MRSNE R0,PSP ; Get PSP
212+
BNE Sys_ContextSave2 ; Branch if secure
213+
#endif
214+
215+
Sys_ContextSave1
216+
MRS R0,PSP ; Get PSP
217+
STMDB R0!,{R4-R11} ; Save R4..R11
218+
#ifdef __FPU_USED
219+
TST LR,#0x10 ; Check if extended stack frame
220+
VSTMDBEQ R0!,{S16-S31} ; Save VFP S16.S31
221+
#endif
222+
223+
Sys_ContextSave2
224+
STR R0,[R1,#TCB_SP_OFS] ; Store SP
225+
STRB LR,[R1,#TCB_SF_OFS] ; Store stack frame information
226+
227+
Sys_ContextSwitch
228+
STR R2,[R3] ; osRtxInfo.run: curr = next
229+
230+
Sys_ContextRestore
231+
#ifdef __DOMAIN_NS
232+
LDR R0,[R2,#TCB_TZM_OFS] ; Load TrustZone memory identifier
233+
CBZ R0,Sys_ContextRestore1 ; Branch if there is no secure context
234+
PUSH {R2,R3} ; Save registers
235+
BL TZ_LoadContext_S ; Load secure context
236+
POP {R2,R3} ; Restore registers
237+
#endif
238+
239+
Sys_ContextRestore1
240+
LDR R0,[R2,#TCB_SM_OFS] ; Load stack memory base
241+
LDRB R1,[R2,#TCB_SF_OFS] ; Load stack frame information
242+
MSR PSPLIM,R0 ; Set PSPLIM
243+
LDR R0,[R2,#TCB_SP_OFS] ; Load SP
244+
ORR LR,R1,#0xFFFFFF00 ; Set EXC_RETURN
245+
246+
#ifdef __DOMAIN_NS
247+
TST LR,#0x40 ; Check domain of interrupted thread
248+
BNE Sys_ContextRestore2 ; Branch if secure
249+
#endif
250+
251+
#ifdef __FPU_USED
252+
TST LR,#0x10 ; Check if extended stack frame
253+
VLDMIAEQ R0!,{S16-S31} ; Restore VFP S16..S31
254+
#endif
255+
LDMIA R0!,{R4-R11} ; Restore R4..R11
256+
257+
Sys_ContextRestore2
258+
MSR PSP,R0 ; Set PSP
259+
260+
Sys_ContextExit
261+
BX LR ; Exit from handler
262+
263+
ALIGN
264+
ENDP
265+
266+
267+
END

0 commit comments

Comments
 (0)