diff --git a/rtos/TARGET_CORTEX/rtx5/TARGET_M33/TOOLCHAIN_ARM/irq_armv8mml.S b/rtos/TARGET_CORTEX/rtx5/TARGET_M33/TOOLCHAIN_ARM/irq_armv8mml.S new file mode 100644 index 0000000000..1adeffa557 --- /dev/null +++ b/rtos/TARGET_CORTEX/rtx5/TARGET_M33/TOOLCHAIN_ARM/irq_armv8mml.S @@ -0,0 +1,267 @@ +;/* +; * Copyright (c) 2016-2017 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 Mainline Exception handlers +; * +; * ----------------------------------------------------------------------------- +; */ + + +I_T_RUN_OFS EQU 28 ; 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 + THUMB + + + AREA |.constdata|, DATA, READONLY + EXPORT irqRtxLib +irqRtxLib DCB 0 ; Non weak library reference + + + AREA |.text|, CODE, READONLY + + +SVC_Handler PROC + EXPORT SVC_Handler + IMPORT osRtxUserSVC + IMPORT osRtxInfo +#ifdef __DOMAIN_NS + IMPORT TZ_LoadContext_S + IMPORT TZ_StoreContext_S +#endif + + MRS R0,PSP ; Get PSP + LDR R1,[R0,#24] ; Load saved PC from stack + LDRB R1,[R1,#-2] ; Load SVC number + CMP R1,#0 + BNE SVC_User ; Branch if not SVC 0 + + PUSH {R0,LR} ; Save PSP and EXC_RETURN + LDM R0,{R0-R3,R12} ; Load function parameters and address from stack + BLX R12 ; Call service function + POP {R12,LR} ; Restore PSP and EXC_RETURN + STM R12,{R0-R1} ; Store function return values + +SVC_Context + 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 + BXEQ LR ; Exit when threads are the same + +#ifdef __FPU_USED + CBNZ R1,SVC_ContextSave ; Branch if running thread is not deleted + TST LR,#0x10 ; Check if extended stack frame + BNE SVC_ContextSwitch + LDR R1,=0xE000EF34 ; FPCCR Address + LDR R0,[R1] ; Load FPCCR + BIC R0,#1 ; Clear LSPACT (Lazy state) + STR R0,[R1] ; Store FPCCR + B SVC_ContextSwitch +#else + CBZ R1,SVC_ContextSwitch ; Branch if running thread is deleted +#endif + +SVC_ContextSave +#ifdef __DOMAIN_NS + 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,LR} ; Save registers and EXC_RETURN + BL TZ_StoreContext_S ; Store secure context + POP {R1,R2,R3,LR} ; Restore registers and EXC_RETURN +#endif + +SVC_ContextSave1 + MRS R0,PSP ; Get PSP + STMDB R0!,{R4-R11} ; Save R4..R11 +#ifdef __FPU_USED + TST LR,#0x10 ; Check if extended stack frame + VSTMDBEQ R0!,{S16-S31} ; Save VFP S16.S31 +#endif + +SVC_ContextSave2 + STR R0,[R1,#TCB_SP_OFS] ; Store SP + STRB LR,[R1,#TCB_SF_OFS] ; Store stack frame information + +SVC_ContextSwitch + STR R2,[R3] ; osRtxInfo.thread.run: curr = next + +SVC_ContextRestore +#ifdef __DOMAIN_NS + 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 + LDR R0,[R2,#TCB_SM_OFS] ; Load stack memory base + LDRB R1,[R2,#TCB_SF_OFS] ; Load stack frame information + MSR PSPLIM,R0 ; Set PSPLIM + LDR R0,[R2,#TCB_SP_OFS] ; Load SP + ORR LR,R1,#0xFFFFFF00 ; Set EXC_RETURN + +#ifdef __DOMAIN_NS + TST LR,#0x40 ; Check domain of interrupted thread + BNE SVC_ContextRestore2 ; Branch if secure +#endif + +#ifdef __FPU_USED + TST LR,#0x10 ; Check if extended stack frame + VLDMIAEQ R0!,{S16-S31} ; Restore VFP S16..S31 +#endif + LDMIA R0!,{R4-R11} ; Restore R4..R11 + +SVC_ContextRestore2 + MSR PSP,R0 ; Set PSP + +SVC_Exit + BX LR ; Exit from handler + +SVC_User + PUSH {R4,LR} ; Save registers + LDR R2,=osRtxUserSVC ; Load address of SVC table + LDR R3,[R2] ; Load SVC maximum number + CMP R1,R3 ; Check SVC number range + BHI SVC_Done ; Branch if out of range + + LDR R4,[R2,R1,LSL #2] ; Load address of SVC function + + LDM R0,{R0-R3} ; Load function parameters from stack + BLX R4 ; Call service function + MRS R4,PSP ; Get PSP + STR R0,[R4] ; Store function return value + +SVC_Done + POP {R4,PC} ; Return from handler + + ALIGN + ENDP + + +PendSV_Handler PROC + EXPORT PendSV_Handler + IMPORT osRtxPendSV_Handler + + PUSH {R4,LR} ; Save EXC_RETURN + BL osRtxPendSV_Handler ; Call osRtxPendSV_Handler + POP {R4,LR} ; Restore EXC_RETURN + B Sys_Context + + ALIGN + ENDP + + +SysTick_Handler PROC + EXPORT SysTick_Handler + IMPORT osRtxTick_Handler + + PUSH {R4,LR} ; Save EXC_RETURN + BL osRtxTick_Handler ; Call osRtxTick_Handler + POP {R4,LR} ; Restore EXC_RETURN + B Sys_Context + + ALIGN + ENDP + + +Sys_Context PROC + EXPORT Sys_Context + IMPORT osRtxInfo +#ifdef __DOMAIN_NS + 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 + BXEQ LR ; Exit when threads are the same + +Sys_ContextSave +#ifdef __DOMAIN_NS + 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,LR} ; Save registers and EXC_RETURN + BL TZ_StoreContext_S ; Store secure context + POP {R1,R2,R3,LR} ; Restore registers and EXC_RETURN + TST LR,#0x40 ; Check domain of interrupted thread + MRSNE R0,PSP ; Get PSP + BNE Sys_ContextSave2 ; Branch if secure +#endif + +Sys_ContextSave1 + MRS R0,PSP ; Get PSP + STMDB R0!,{R4-R11} ; Save R4..R11 +#ifdef __FPU_USED + TST LR,#0x10 ; Check if extended stack frame + VSTMDBEQ R0!,{S16-S31} ; Save VFP S16.S31 +#endif + +Sys_ContextSave2 + STR R0,[R1,#TCB_SP_OFS] ; Store SP + STRB LR,[R1,#TCB_SF_OFS] ; Store stack frame information + +Sys_ContextSwitch + STR R2,[R3] ; osRtxInfo.run: curr = next + +Sys_ContextRestore +#ifdef __DOMAIN_NS + 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 + LDR R0,[R2,#TCB_SM_OFS] ; Load stack memory base + LDRB R1,[R2,#TCB_SF_OFS] ; Load stack frame information + MSR PSPLIM,R0 ; Set PSPLIM + LDR R0,[R2,#TCB_SP_OFS] ; Load SP + ORR LR,R1,#0xFFFFFF00 ; Set EXC_RETURN + +#ifdef __DOMAIN_NS + TST LR,#0x40 ; Check domain of interrupted thread + BNE Sys_ContextRestore2 ; Branch if secure +#endif + +#ifdef __FPU_USED + TST LR,#0x10 ; Check if extended stack frame + VLDMIAEQ R0!,{S16-S31} ; Restore VFP S16..S31 +#endif + LDMIA R0!,{R4-R11} ; Restore R4..R11 + +Sys_ContextRestore2 + MSR PSP,R0 ; Set PSP + +Sys_ContextExit + BX LR ; Exit from handler + + ALIGN + ENDP + + + END \ No newline at end of file diff --git a/rtos/TARGET_CORTEX/rtx5/TARGET_M33/TOOLCHAIN_GCC/irq_armv8mml.S b/rtos/TARGET_CORTEX/rtx5/TARGET_M33/TOOLCHAIN_GCC/irq_armv8mml.S index c01721b152..49f80cf9b1 100644 --- a/rtos/TARGET_CORTEX/rtx5/TARGET_M33/TOOLCHAIN_GCC/irq_armv8mml.S +++ b/rtos/TARGET_CORTEX/rtx5/TARGET_M33/TOOLCHAIN_GCC/irq_armv8mml.S @@ -27,14 +27,6 @@ .file "irq_armv8mml.S" .syntax unified - .ifndef __DOMAIN_NS - .equ __DOMAIN_NS, 0 - .endif - - .ifndef __FPU_USED - .equ __FPU_USED, 0 - .endif - .equ I_T_RUN_OFS, 28 // osRtxInfo.thread.run offset .equ TCB_SM_OFS, 48 // TCB.stack_mem offset .equ TCB_SP_OFS, 56 // TCB.SP offset @@ -78,7 +70,7 @@ SVC_Context: IT EQ BXEQ LR // Exit when threads are the same - .if __FPU_USED == 1 +#ifdef __FPU_USED CBNZ R1,SVC_ContextSave // Branch if running thread is not deleted TST LR,#0x10 // Check if extended stack frame BNE SVC_ContextSwitch @@ -87,27 +79,27 @@ SVC_Context: BIC R0,#1 // Clear LSPACT (Lazy state) STR R0,[R1] // Store FPCCR B SVC_ContextSwitch - .else +#else CBZ R1,SVC_ContextSwitch // Branch if running thread is deleted - .endif +#endif SVC_ContextSave: - .if __DOMAIN_NS == 1 +#ifdef __DOMAIN_NS 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,LR} // Save registers and EXC_RETURN BL TZ_StoreContext_S // Store secure context POP {R1,R2,R3,LR} // Restore registers and EXC_RETURN - .endif +#endif SVC_ContextSave1: MRS R0,PSP // Get PSP STMDB R0!,{R4-R11} // Save R4..R11 - .if __FPU_USED == 1 +#ifdef __FPU_USED TST LR,#0x10 // Check if extended stack frame IT EQ VSTMDBEQ R0!,{S16-S31} // Save VFP S16.S31 - .endif +#endif SVC_ContextSave2: STR R0,[R1,#TCB_SP_OFS] // Store SP @@ -117,13 +109,13 @@ SVC_ContextSwitch: STR R2,[R3] // osRtxInfo.thread.run: curr = next SVC_ContextRestore: - .if __DOMAIN_NS == 1 +#ifdef __DOMAIN_NS 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 +#endif SVC_ContextRestore1: LDR R0,[R2,#TCB_SM_OFS] // Load stack memory base @@ -132,16 +124,16 @@ SVC_ContextRestore1: LDR R0,[R2,#TCB_SP_OFS] // Load SP ORR LR,R1,#0xFFFFFF00 // Set EXC_RETURN - .if __DOMAIN_NS == 1 +#ifdef __DOMAIN_NS TST LR,#0x40 // Check domain of interrupted thread BNE SVC_ContextRestore2 // Branch if secure - .endif +#endif - .if __FPU_USED == 1 +#ifdef __FPU_USED TST LR,#0x10 // Check if extended stack frame IT EQ VLDMIAEQ R0!,{S16-S31} // Restore VFP S16..S31 - .endif +#endif LDMIA R0!,{R4-R11} // Restore R4..R11 SVC_ContextRestore2: @@ -217,7 +209,7 @@ Sys_Context: BXEQ LR // Exit when threads are the same Sys_ContextSave: - .if __DOMAIN_NS == 1 +#ifdef __DOMAIN_NS 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,LR} // Save registers and EXC_RETURN @@ -227,16 +219,16 @@ Sys_ContextSave: IT NE MRSNE R0,PSP // Get PSP BNE Sys_ContextSave2 // Branch if secure - .endif +#endif Sys_ContextSave1: MRS R0,PSP // Get PSP STMDB R0!,{R4-R11} // Save R4..R11 - .if __FPU_USED == 1 +#ifdef __FPU_USED TST LR,#0x10 // Check if extended stack frame IT EQ VSTMDBEQ R0!,{S16-S31} // Save VFP S16.S31 - .endif +#endif Sys_ContextSave2: STR R0,[R1,#TCB_SP_OFS] // Store SP @@ -246,13 +238,13 @@ Sys_ContextSwitch: STR R2,[R3] // osRtxInfo.run: curr = next Sys_ContextRestore: - .if __DOMAIN_NS == 1 +#ifdef __DOMAIN_NS 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 +#endif Sys_ContextRestore1: LDR R0,[R2,#TCB_SM_OFS] // Load stack memory base @@ -261,16 +253,16 @@ Sys_ContextRestore1: LDR R0,[R2,#TCB_SP_OFS] // Load SP ORR LR,R1,#0xFFFFFF00 // Set EXC_RETURN - .if __DOMAIN_NS == 1 +#ifdef __DOMAIN_NS TST LR,#0x40 // Check domain of interrupted thread BNE Sys_ContextRestore2 // Branch if secure - .endif +#endif - .if __FPU_USED == 1 +#ifdef __FPU_USED TST LR,#0x10 // Check if extended stack frame IT EQ VLDMIAEQ R0!,{S16-S31} // Restore VFP S16..S31 - .endif +#endif LDMIA R0!,{R4-R11} // Restore R4..R11 Sys_ContextRestore2: diff --git a/rtos/TARGET_CORTEX/rtx5/TARGET_M33/TOOLCHAIN_IAR/irq_armv8mml.S b/rtos/TARGET_CORTEX/rtx5/TARGET_M33/TOOLCHAIN_IAR/irq_armv8mml.S new file mode 100644 index 0000000000..20c041119f --- /dev/null +++ b/rtos/TARGET_CORTEX/rtx5/TARGET_M33/TOOLCHAIN_IAR/irq_armv8mml.S @@ -0,0 +1,253 @@ +;/* +; * Copyright (c) 2016-2017 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 Mainline Exception handlers +; * +; * ----------------------------------------------------------------------------- +; */ + + NAME irq_armv8mml.S + +I_T_RUN_OFS EQU 28 ; 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 + + THUMB + SECTION .text:CODE:NOROOT(2) + + +SVC_Handler + EXPORT SVC_Handler + IMPORT osRtxUserSVC + IMPORT osRtxInfo +#ifdef __DOMAIN_NS + IMPORT TZ_LoadContext_S + IMPORT TZ_StoreContext_S +#endif + + MRS R0,PSP ; Get PSP + LDR R1,[R0,#24] ; Load saved PC from stack + LDRB R1,[R1,#-2] ; Load SVC number + CMP R1,#0 + BNE SVC_User ; Branch if not SVC 0 + + PUSH {R0,LR} ; Save PSP and EXC_RETURN + LDM R0,{R0-R3,R12} ; Load function parameters and address from stack + BLX R12 ; Call service function + POP {R12,LR} ; Restore PSP and EXC_RETURN + STM R12,{R0-R1} ; Store function return values + +SVC_Context + 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 + BXEQ LR ; Exit when threads are the same + +#ifdef __FPU_USED + CBNZ R1,SVC_ContextSave ; Branch if running thread is not deleted + TST LR,#0x10 ; Check if extended stack frame + BNE SVC_ContextSwitch + LDR R1,=0xE000EF34 ; FPCCR Address + LDR R0,[R1] ; Load FPCCR + BIC R0,#1 ; Clear LSPACT (Lazy state) + STR R0,[R1] ; Store FPCCR + B SVC_ContextSwitch +#else + CBZ R1,SVC_ContextSwitch ; Branch if running thread is deleted +#endif + +SVC_ContextSave +#ifdef __DOMAIN_NS + 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,LR} ; Save registers and EXC_RETURN + BL TZ_StoreContext_S ; Store secure context + POP {R1,R2,R3,LR} ; Restore registers and EXC_RETURN +#endif + +SVC_ContextSave1 + MRS R0,PSP ; Get PSP + STMDB R0!,{R4-R11} ; Save R4..R11 +#ifdef __FPU_USED + TST LR,#0x10 ; Check if extended stack frame + VSTMDBEQ R0!,{S16-S31} ; Save VFP S16.S31 +#endif + +SVC_ContextSave2 + STR R0,[R1,#TCB_SP_OFS] ; Store SP + STRB LR,[R1,#TCB_SF_OFS] ; Store stack frame information + +SVC_ContextSwitch + STR R2,[R3] ; osRtxInfo.thread.run: curr = next + +SVC_ContextRestore +#ifdef __DOMAIN_NS + 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 + LDR R0,[R2,#TCB_SM_OFS] ; Load stack memory base + LDRB R1,[R2,#TCB_SF_OFS] ; Load stack frame information + MSR PSPLIM,R0 ; Set PSPLIM + LDR R0,[R2,#TCB_SP_OFS] ; Load SP + ORR LR,R1,#0xFFFFFF00 ; Set EXC_RETURN + +#ifdef __DOMAIN_NS + TST LR,#0x40 ; Check domain of interrupted thread + BNE SVC_ContextRestore2 ; Branch if secure +#endif + +#ifdef __FPU_USED + TST LR,#0x10 ; Check if extended stack frame + VLDMIAEQ R0!,{S16-S31} ; Restore VFP S16..S31 +#endif + LDMIA R0!,{R4-R11} ; Restore R4..R11 + +SVC_ContextRestore2 + MSR PSP,R0 ; Set PSP + +SVC_Exit + BX LR ; Exit from handler + +SVC_User + PUSH {R4,LR} ; Save registers + LDR R2,=osRtxUserSVC ; Load address of SVC table + LDR R3,[R2] ; Load SVC maximum number + CMP R1,R3 ; Check SVC number range + BHI SVC_Done ; Branch if out of range + + LDR R4,[R2,R1,LSL #2] ; Load address of SVC function + + LDM R0,{R0-R3} ; Load function parameters from stack + BLX R4 ; Call service function + MRS R4,PSP ; Get PSP + STR R0,[R4] ; Store function return value + +SVC_Done + POP {R4,PC} ; Return from handler + + +PendSV_Handler + EXPORT PendSV_Handler + IMPORT osRtxPendSV_Handler + + PUSH {R4,LR} ; Save EXC_RETURN + BL osRtxPendSV_Handler ; Call osRtxPendSV_Handler + POP {R4,LR} ; Restore EXC_RETURN + B Sys_Context + + +SysTick_Handler + EXPORT SysTick_Handler + IMPORT osRtxTick_Handler + + PUSH {R4,LR} ; Save EXC_RETURN + BL osRtxTick_Handler ; Call osRtxTick_Handler + POP {R4,LR} ; Restore EXC_RETURN + B Sys_Context + + +Sys_Context + EXPORT Sys_Context + IMPORT osRtxInfo +#ifdef __DOMAIN_NS + 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 + BXEQ LR ; Exit when threads are the same + +Sys_ContextSave +#ifdef __DOMAIN_NS + 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,LR} ; Save registers and EXC_RETURN + BL TZ_StoreContext_S ; Store secure context + POP {R1,R2,R3,LR} ; Restore registers and EXC_RETURN + TST LR,#0x40 ; Check domain of interrupted thread + MRSNE R0,PSP ; Get PSP + BNE Sys_ContextSave2 ; Branch if secure +#endif + +Sys_ContextSave1 + MRS R0,PSP ; Get PSP + STMDB R0!,{R4-R11} ; Save R4..R11 +#ifdef __FPU_USED + TST LR,#0x10 ; Check if extended stack frame + VSTMDBEQ R0!,{S16-S31} ; Save VFP S16.S31 +#endif + +Sys_ContextSave2 + STR R0,[R1,#TCB_SP_OFS] ; Store SP + STRB LR,[R1,#TCB_SF_OFS] ; Store stack frame information + +Sys_ContextSwitch + STR R2,[R3] ; osRtxInfo.run: curr = next + +Sys_ContextRestore +#ifdef __DOMAIN_NS + 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 + LDR R0,[R2,#TCB_SM_OFS] ; Load stack memory base + LDRB R1,[R2,#TCB_SF_OFS] ; Load stack frame information + MSR PSPLIM,R0 ; Set PSPLIM + LDR R0,[R2,#TCB_SP_OFS] ; Load SP + ORR LR,R1,#0xFFFFFF00 ; Set EXC_RETURN + +#ifdef __DOMAIN_NS + TST LR,#0x40 ; Check domain of interrupted thread + BNE Sys_ContextRestore2 ; Branch if secure +#endif + +#ifdef __FPU_USED + TST LR,#0x10 ; Check if extended stack frame + VLDMIAEQ R0!,{S16-S31} ; Restore VFP S16..S31 +#endif + LDMIA R0!,{R4-R11} ; Restore R4..R11 + +Sys_ContextRestore2 + MSR PSP,R0 ; Set PSP + +Sys_ContextExit + BX LR ; Exit from handler + + END \ No newline at end of file