From d05a7dc7e9fcf0e013b4494cde905ae5f8065a59 Mon Sep 17 00:00:00 2001 From: Robert Rostohar Date: Wed, 25 Oct 2017 13:55:27 +0200 Subject: [PATCH] RTX5: Optimization (#230): Stack setup for Cortex-M replaced with stack detection in SVC_Handler --- CMSIS/RTOS2/RTX/Source/ARM/irq_armv8mbl.s | 35 ++- CMSIS/RTOS2/RTX/Source/ARM/irq_armv8mml.s | 43 ++-- CMSIS/RTOS2/RTX/Source/ARM/irq_cm0.s | 35 ++- CMSIS/RTOS2/RTX/Source/ARM/irq_cm3.s | 35 +-- CMSIS/RTOS2/RTX/Source/ARM/irq_cm4f.s | 40 +-- CMSIS/RTOS2/RTX/Source/GCC/irq_armv8mbl.S | 35 ++- CMSIS/RTOS2/RTX/Source/GCC/irq_armv8mml.S | 36 +-- CMSIS/RTOS2/RTX/Source/GCC/irq_cm0.S | 37 +-- CMSIS/RTOS2/RTX/Source/GCC/irq_cm3.S | 34 +-- CMSIS/RTOS2/RTX/Source/GCC/irq_cm4f.S | 37 +-- CMSIS/RTOS2/RTX/Source/IAR/irq_armv8mbl.s | 5 +- .../RTX/Source/IAR/irq_armv8mbl_common.s | 35 ++- CMSIS/RTOS2/RTX/Source/IAR/irq_armv8mbl_ns.s | 5 +- CMSIS/RTOS2/RTX/Source/IAR/irq_armv8mml.s | 7 +- .../RTX/Source/IAR/irq_armv8mml_common.s | 40 +-- CMSIS/RTOS2/RTX/Source/IAR/irq_armv8mml_ns.s | 7 +- CMSIS/RTOS2/RTX/Source/IAR/irq_cm0.s | 41 ++-- CMSIS/RTOS2/RTX/Source/IAR/irq_cm3.s | 40 +-- CMSIS/RTOS2/RTX/Source/IAR/irq_cm4f.s | 40 +-- CMSIS/RTOS2/RTX/Source/rtx_core_ca.h | 18 -- CMSIS/RTOS2/RTX/Source/rtx_core_cm.h | 230 ------------------ CMSIS/RTOS2/RTX/Source/rtx_evflags.c | 14 +- CMSIS/RTOS2/RTX/Source/rtx_kernel.c | 4 +- CMSIS/RTOS2/RTX/Source/rtx_mempool.c | 18 +- CMSIS/RTOS2/RTX/Source/rtx_msgqueue.c | 20 +- CMSIS/RTOS2/RTX/Source/rtx_mutex.c | 12 +- CMSIS/RTOS2/RTX/Source/rtx_semaphore.c | 12 +- CMSIS/RTOS2/RTX/Source/rtx_thread.c | 2 +- CMSIS/RTOS2/RTX/Source/rtx_timer.c | 12 +- 29 files changed, 381 insertions(+), 548 deletions(-) diff --git a/CMSIS/RTOS2/RTX/Source/ARM/irq_armv8mbl.s b/CMSIS/RTOS2/RTX/Source/ARM/irq_armv8mbl.s index 1586a95549..a824eeb694 100644 --- a/CMSIS/RTOS2/RTX/Source/ARM/irq_armv8mbl.s +++ b/CMSIS/RTOS2/RTX/Source/ARM/irq_armv8mbl.s @@ -56,17 +56,22 @@ SVC_Handler PROC 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 PSP and EXC_RETURN + 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 PSP and EXC_RETURN + POP {R2,R3} ; Restore SP and EXC_RETURN STMIA R2!,{R0-R1} ; Store function return values MOV LR,R3 ; Set EXC_RETURN @@ -91,7 +96,7 @@ SVC_ContextSave SVC_ContextSave1 MRS R0,PSP ; Get PSP - SUBS R0,R0,#32 ; Adjust 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 @@ -153,23 +158,27 @@ SVC_ContextRestore2 SVC_Exit BX LR ; Exit from handler +SVC_MSP + MRS R0,MSP ; Get MSP + B SVC_Number + 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 + BHI SVC_Exit ; Branch if out of range + PUSH {R0,LR} ; Save SP and EXC_RETURN LSLS R1,R1,#2 - LDR R4,[R2,R1] ; 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 + 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 -SVC_Done - POP {R4,PC} ; Return from handler + BX LR ; Return from handler ALIGN ENDP diff --git a/CMSIS/RTOS2/RTX/Source/ARM/irq_armv8mml.s b/CMSIS/RTOS2/RTX/Source/ARM/irq_armv8mml.s index b8b25a1a32..df0b656cb8 100644 --- a/CMSIS/RTOS2/RTX/Source/ARM/irq_armv8mml.s +++ b/CMSIS/RTOS2/RTX/Source/ARM/irq_armv8mml.s @@ -62,22 +62,27 @@ SVC_Handler PROC IMPORT TZ_StoreContext_S ENDIF - MRS R0,PSP ; Get PSP + TST LR,#0x04 ; Determine return stack from EXC_RETURN bit 2 + ITE EQ + MRSEQ R0,MSP ; Get MSP if return stack is MSP + MRSNE R0,PSP ; Get PSP if return stack is 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 + PUSH {R0,LR} ; Save SP 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 + POP {R12,LR} ; Restore SP 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 + IT EQ BXEQ LR ; Exit when threads are the same IF __FPU_USED = 1 @@ -86,7 +91,7 @@ SVC_Context BNE SVC_ContextSwitch LDR R1,=0xE000EF34 ; FPCCR Address LDR R0,[R1] ; Load FPCCR - BIC R0,#1 ; Clear LSPACT (Lazy state) + BIC R0,R0,#1 ; Clear LSPACT (Lazy state) STR R0,[R1] ; Store FPCCR B SVC_ContextSwitch ELSE @@ -107,6 +112,7 @@ SVC_ContextSave1 STMDB R0!,{R4-R11} ; Save R4..R11 IF __FPU_USED = 1 TST LR,#0x10 ; Check if extended stack frame + IT EQ VSTMDBEQ R0!,{S16-S31} ; Save VFP S16.S31 ENDIF @@ -140,6 +146,7 @@ SVC_ContextRestore1 IF __FPU_USED = 1 TST LR,#0x10 ; Check if extended stack frame + IT EQ VLDMIAEQ R0!,{S16-S31} ; Restore VFP S16..S31 ENDIF LDMIA R0!,{R4-R11} ; Restore R4..R11 @@ -151,21 +158,19 @@ 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 + BHI SVC_Exit ; Branch if out of range + PUSH {R0,LR} ; Save SP and EXC_RETURN + LDR R12,[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 + BLX R12 ; Call service function + POP {R12,LR} ; Restore SP and EXC_RETURN + STR R0,[R12] ; Store function return value -SVC_Done - POP {R4,PC} ; Return from handler + BX LR ; Return from handler ALIGN ENDP @@ -175,9 +180,9 @@ PendSV_Handler PROC EXPORT PendSV_Handler IMPORT osRtxPendSV_Handler - PUSH {R4,LR} ; Save EXC_RETURN + PUSH {R0,LR} ; Save EXC_RETURN BL osRtxPendSV_Handler ; Call osRtxPendSV_Handler - POP {R4,LR} ; Restore EXC_RETURN + POP {R0,LR} ; Restore EXC_RETURN B Sys_Context ALIGN @@ -188,9 +193,9 @@ SysTick_Handler PROC EXPORT SysTick_Handler IMPORT osRtxTick_Handler - PUSH {R4,LR} ; Save EXC_RETURN + PUSH {R0,LR} ; Save EXC_RETURN BL osRtxTick_Handler ; Call osRtxTick_Handler - POP {R4,LR} ; Restore EXC_RETURN + POP {R0,LR} ; Restore EXC_RETURN B Sys_Context ALIGN @@ -208,6 +213,7 @@ Sys_Context PROC 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 + IT EQ BXEQ LR ; Exit when threads are the same Sys_ContextSave @@ -218,6 +224,7 @@ Sys_ContextSave 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 + IT NE MRSNE R0,PSP ; Get PSP BNE Sys_ContextSave2 ; Branch if secure ENDIF @@ -227,6 +234,7 @@ Sys_ContextSave1 STMDB R0!,{R4-R11} ; Save R4..R11 IF __FPU_USED = 1 TST LR,#0x10 ; Check if extended stack frame + IT EQ VSTMDBEQ R0!,{S16-S31} ; Save VFP S16.S31 ENDIF @@ -260,6 +268,7 @@ Sys_ContextRestore1 IF __FPU_USED = 1 TST LR,#0x10 ; Check if extended stack frame + IT EQ VLDMIAEQ R0!,{S16-S31} ; Restore VFP S16..S31 ENDIF LDMIA R0!,{R4-R11} ; Restore R4..R11 diff --git a/CMSIS/RTOS2/RTX/Source/ARM/irq_cm0.s b/CMSIS/RTOS2/RTX/Source/ARM/irq_cm0.s index 8fab32aabe..83bffd762e 100644 --- a/CMSIS/RTOS2/RTX/Source/ARM/irq_cm0.s +++ b/CMSIS/RTOS2/RTX/Source/ARM/irq_cm0.s @@ -45,17 +45,22 @@ SVC_Handler PROC IMPORT osRtxUserSVC IMPORT osRtxInfo + 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 PSP and EXC_RETURN + PUSH {R0,LR} ; Save SP and EXC_RETURN LDMIA R0,{R0-R3} ; Load function parameters from stack BLX R7 ; Call service function - POP {R2,R3} ; Restore PSP and EXC_RETURN + POP {R2,R3} ; Restore SP and EXC_RETURN STMIA R2!,{R0-R1} ; Store function return values MOV LR,R3 ; Set EXC_RETURN @@ -70,7 +75,7 @@ SVC_Context SVC_ContextSave MRS R0,PSP ; Get PSP - SUBS R0,R0,#32 ; Adjust address + SUBS R0,R0,#32 ; Calculate SP STR R0,[R1,#TCB_SP_OFS] ; Store SP STMIA R0!,{R4-R7} ; Save R4..R7 MOV R4,R8 @@ -80,7 +85,7 @@ SVC_ContextSave STMIA R0!,{R4-R7} ; Save R8..R11 SVC_ContextSwitch - SUBS R3,R3,#8 + SUBS R3,R3,#8 ; Adjust address STR R2,[R3] ; osRtxInfo.thread.run: curr = next SVC_ContextRestore @@ -99,26 +104,30 @@ SVC_ContextRestore MVNS R0,R0 ; Set EXC_RETURN value BX R0 ; Exit from handler +SVC_MSP + MRS R0,MSP ; Get MSP + B SVC_Number + 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 + BHI SVC_Exit ; Branch if out of range + PUSH {R0,LR} ; Save SP and EXC_RETURN LSLS R1,R1,#2 - LDR R4,[R2,R1] ; Load address of SVC function - + LDR R3,[R2,R1] ; Load address of SVC function + MOV R12,R3 LDMIA R0,{R0-R3} ; Load function parameters from stack - BLX R4 ; Call service function - MRS R4,PSP ; Get PSP - STMIA R4!,{R0-R3} ; Store function return values + 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 -SVC_Done - POP {R4,PC} ; Return from handler + BX LR ; Return from handler ALIGN ENDP diff --git a/CMSIS/RTOS2/RTX/Source/ARM/irq_cm3.s b/CMSIS/RTOS2/RTX/Source/ARM/irq_cm3.s index 2fd6618f03..b0cd40980d 100644 --- a/CMSIS/RTOS2/RTX/Source/ARM/irq_cm3.s +++ b/CMSIS/RTOS2/RTX/Source/ARM/irq_cm3.s @@ -45,21 +45,26 @@ SVC_Handler PROC IMPORT osRtxUserSVC IMPORT osRtxInfo - MRS R0,PSP ; Get PSP + TST LR,#0x04 ; Determine return stack from EXC_RETURN bit 2 + ITE EQ + MRSEQ R0,MSP ; Get MSP if return stack is MSP + MRSNE R0,PSP ; Get PSP if return stack is PSP + LDR R1,[R0,#24] ; Load saved PC from stack LDRB R1,[R1,#-2] ; Load SVC number CBNZ R1,SVC_User ; Branch if not SVC 0 - PUSH {R0,LR} ; Save PSP and EXC_RETURN + PUSH {R0,LR} ; Save SP 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 + POP {R12,LR} ; Restore SP 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 + IT EQ BXEQ LR ; Exit when threads are the same CBZ R1,SVC_ContextSwitch ; Branch if running thread is deleted @@ -82,21 +87,19 @@ 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 + BHI SVC_Exit ; Branch if out of range + PUSH {R0,LR} ; Save SP and EXC_RETURN + LDR R12,[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 + BLX R12 ; Call service function + POP {R12,LR} ; Restore SP and EXC_RETURN + STR R0,[R12] ; Store function return value -SVC_Done - POP {R4,PC} ; Return from handler + BX LR ; Return from handler ALIGN ENDP @@ -106,9 +109,9 @@ PendSV_Handler PROC EXPORT PendSV_Handler IMPORT osRtxPendSV_Handler - PUSH {R4,LR} ; Save EXC_RETURN + PUSH {R0,LR} ; Save EXC_RETURN BL osRtxPendSV_Handler ; Call osRtxPendSV_Handler - POP {R4,LR} ; Restore EXC_RETURN + POP {R0,LR} ; Restore EXC_RETURN MRS R12,PSP B SVC_Context @@ -120,9 +123,9 @@ SysTick_Handler PROC EXPORT SysTick_Handler IMPORT osRtxTick_Handler - PUSH {R4,LR} ; Save EXC_RETURN + PUSH {R0,LR} ; Save EXC_RETURN BL osRtxTick_Handler ; Call osRtxTick_Handler - POP {R4,LR} ; Restore EXC_RETURN + POP {R0,LR} ; Restore EXC_RETURN MRS R12,PSP B SVC_Context diff --git a/CMSIS/RTOS2/RTX/Source/ARM/irq_cm4f.s b/CMSIS/RTOS2/RTX/Source/ARM/irq_cm4f.s index f0df8aaa04..b462868bb6 100644 --- a/CMSIS/RTOS2/RTX/Source/ARM/irq_cm4f.s +++ b/CMSIS/RTOS2/RTX/Source/ARM/irq_cm4f.s @@ -46,21 +46,26 @@ SVC_Handler PROC IMPORT osRtxUserSVC IMPORT osRtxInfo - MRS R0,PSP ; Get PSP + TST LR,#0x04 ; Determine return stack from EXC_RETURN bit 2 + ITE EQ + MRSEQ R0,MSP ; Get MSP if return stack is MSP + MRSNE R0,PSP ; Get PSP if return stack is PSP + LDR R1,[R0,#24] ; Load saved PC from stack LDRB R1,[R1,#-2] ; Load SVC number CBNZ R1,SVC_User ; Branch if not SVC 0 - PUSH {R0,LR} ; Save PSP and EXC_RETURN + PUSH {R0,LR} ; Save SP 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 + POP {R12,LR} ; Restore SP 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 + IT EQ BXEQ LR ; Exit when threads are the same CBNZ R1,SVC_ContextSave ; Branch if running thread is not deleted @@ -68,15 +73,15 @@ SVC_Context BNE SVC_ContextSwitch LDR R1,=0xE000EF34 ; FPCCR Address LDR R0,[R1] ; Load FPCCR - BIC R0,#1 ; Clear LSPACT (Lazy state) + BIC R0,R0,#1 ; Clear LSPACT (Lazy state) STR R0,[R1] ; Store FPCCR B SVC_ContextSwitch SVC_ContextSave STMDB R12!,{R4-R11} ; Save R4..R11 TST LR,#0x10 ; Check if extended stack frame + IT EQ VSTMDBEQ R12!,{S16-S31} ; Save VFP S16.S31 - STR R12,[R1,#TCB_SP_OFS] ; Store SP STRB LR, [R1,#TCB_SF_OFS] ; Store stack frame information @@ -89,6 +94,7 @@ SVC_ContextRestore ORR LR,R1,#0xFFFFFF00 ; Set EXC_RETURN TST LR,#0x10 ; Check if extended stack frame + IT EQ VLDMIAEQ R0!,{S16-S31} ; Restore VFP S16..S31 LDMIA R0!,{R4-R11} ; Restore R4..R11 MSR PSP,R0 ; Set PSP @@ -97,21 +103,19 @@ 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 + BHI SVC_Exit ; Branch if out of range + PUSH {R0,LR} ; Save SP and EXC_RETURN + LDR R12,[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 + BLX R12 ; Call service function + POP {R12,LR} ; Restore SP and EXC_RETURN + STR R0,[R12] ; Store function return value -SVC_Done - POP {R4,PC} ; Return from handler + BX LR ; Return from handler ALIGN ENDP @@ -121,9 +125,9 @@ PendSV_Handler PROC EXPORT PendSV_Handler IMPORT osRtxPendSV_Handler - PUSH {R4,LR} ; Save EXC_RETURN + PUSH {R0,LR} ; Save EXC_RETURN BL osRtxPendSV_Handler ; Call osRtxPendSV_Handler - POP {R4,LR} ; Restore EXC_RETURN + POP {R0,LR} ; Restore EXC_RETURN MRS R12,PSP B SVC_Context @@ -135,9 +139,9 @@ SysTick_Handler PROC EXPORT SysTick_Handler IMPORT osRtxTick_Handler - PUSH {R4,LR} ; Save EXC_RETURN + PUSH {R0,LR} ; Save EXC_RETURN BL osRtxTick_Handler ; Call osRtxTick_Handler - POP {R4,LR} ; Restore EXC_RETURN + POP {R0,LR} ; Restore EXC_RETURN MRS R12,PSP B SVC_Context diff --git a/CMSIS/RTOS2/RTX/Source/GCC/irq_armv8mbl.S b/CMSIS/RTOS2/RTX/Source/GCC/irq_armv8mbl.S index 497e5c917f..63d2978ce0 100644 --- a/CMSIS/RTOS2/RTX/Source/GCC/irq_armv8mbl.S +++ b/CMSIS/RTOS2/RTX/Source/GCC/irq_armv8mbl.S @@ -55,17 +55,22 @@ irqRtxLib: .cantunwind SVC_Handler: + 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 PSP and EXC_RETURN + 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 PSP and EXC_RETURN + POP {R2,R3} // Restore SP and EXC_RETURN STMIA R2!,{R0-R1} // Store function return values MOV LR,R3 // Set EXC_RETURN @@ -90,7 +95,7 @@ SVC_ContextSave: SVC_ContextSave1: MRS R0,PSP // Get PSP - SUBS R0,R0,#32 // Adjust 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 @@ -152,23 +157,27 @@ SVC_ContextRestore2: SVC_Exit: BX LR // Exit from handler +SVC_MSP: + MRS R0,MSP // Get MSP + B SVC_Number + 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 + BHI SVC_Exit // Branch if out of range + PUSH {R0,LR} // Save SP and EXC_RETURN LSLS R1,R1,#2 - LDR R4,[R2,R1] // 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 + 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 -SVC_Done: - POP {R4,PC} // Return from handler + BX LR // Return from handler .fnend .size SVC_Handler, .-SVC_Handler diff --git a/CMSIS/RTOS2/RTX/Source/GCC/irq_armv8mml.S b/CMSIS/RTOS2/RTX/Source/GCC/irq_armv8mml.S index 3248b9eb3b..1815e18c32 100644 --- a/CMSIS/RTOS2/RTX/Source/GCC/irq_armv8mml.S +++ b/CMSIS/RTOS2/RTX/Source/GCC/irq_armv8mml.S @@ -59,16 +59,20 @@ irqRtxLib: .cantunwind SVC_Handler: - MRS R0,PSP // Get PSP + TST LR,#0x04 // Determine return stack from EXC_RETURN bit 2 + ITE EQ + MRSEQ R0,MSP // Get MSP if return stack is MSP + MRSNE R0,PSP // Get PSP if return stack is 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 + PUSH {R0,LR} // Save SP 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 + POP {R12,LR} // Restore SP and EXC_RETURN STM R12,{R0-R1} // Store function return values SVC_Context: @@ -84,7 +88,7 @@ SVC_Context: BNE SVC_ContextSwitch LDR R1,=0xE000EF34 // FPCCR Address LDR R0,[R1] // Load FPCCR - BIC R0,#1 // Clear LSPACT (Lazy state) + BIC R0,R0,#1 // Clear LSPACT (Lazy state) STR R0,[R1] // Store FPCCR B SVC_ContextSwitch .else @@ -151,21 +155,19 @@ 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 + BHI SVC_Exit // Branch if out of range + PUSH {R0,LR} // Save SP and EXC_RETURN + LDR R12,[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 + BLX R12 // Call service function + POP {R12,LR} // Restore SP and EXC_RETURN + STR R0,[R12] // Store function return value -SVC_Done: - POP {R4,PC} // Return from handler + BX LR // Return from handler .fnend .size SVC_Handler, .-SVC_Handler @@ -178,9 +180,9 @@ SVC_Done: .cantunwind PendSV_Handler: - PUSH {R4,LR} // Save EXC_RETURN + PUSH {R0,LR} // Save EXC_RETURN BL osRtxPendSV_Handler // Call osRtxPendSV_Handler - POP {R4,LR} // Restore EXC_RETURN + POP {R0,LR} // Restore EXC_RETURN B Sys_Context .fnend @@ -194,9 +196,9 @@ PendSV_Handler: .cantunwind SysTick_Handler: - PUSH {R4,LR} // Save EXC_RETURN + PUSH {R0,LR} // Save EXC_RETURN BL osRtxTick_Handler // Call osRtxTick_Handler - POP {R4,LR} // Restore EXC_RETURN + POP {R0,LR} // Restore EXC_RETURN B Sys_Context .fnend diff --git a/CMSIS/RTOS2/RTX/Source/GCC/irq_cm0.S b/CMSIS/RTOS2/RTX/Source/GCC/irq_cm0.S index 59288e2433..ce30b0b1a1 100644 --- a/CMSIS/RTOS2/RTX/Source/GCC/irq_cm0.S +++ b/CMSIS/RTOS2/RTX/Source/GCC/irq_cm0.S @@ -48,17 +48,22 @@ irqRtxLib: .cantunwind SVC_Handler: + 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 PSP and EXC_RETURN + PUSH {R0,LR} // Save SP and EXC_RETURN LDMIA R0,{R0-R3} // Load function parameters from stack BLX R7 // Call service function - POP {R2,R3} // Restore PSP and EXC_RETURN + POP {R2,R3} // Restore SP and EXC_RETURN STMIA R2!,{R0-R1} // Store function return values MOV LR,R3 // Set EXC_RETURN @@ -73,8 +78,8 @@ SVC_Context: SVC_ContextSave: MRS R0,PSP // Get PSP - SUBS R0,R0,#32 // Adjust address - STR R0,[R1,#TCB_SP_OFS]; // Store SP + 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 @@ -83,7 +88,7 @@ SVC_ContextSave: STMIA R0!,{R4-R7} // Save R8..R11 SVC_ContextSwitch: - SUBS R3,R3,#8 + SUBS R3,R3,#8 // Adjust address STR R2,[R3] // osRtxInfo.thread.run: curr = next SVC_ContextRestore: @@ -102,26 +107,30 @@ SVC_ContextRestore: MVNS R0,R0 // Set EXC_RETURN value BX R0 // Exit from handler +SVC_MSP: + MRS R0,MSP // Get MSP + B SVC_Number + 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 + BHI SVC_Exit // Branch if out of range + PUSH {R0,LR} // Save SP and EXC_RETURN LSLS R1,R1,#2 - LDR R4,[R2,R1] // Load address of SVC function - + LDR R3,[R2,R1] // Load address of SVC function + MOV R12,R3 LDMIA R0,{R0-R3} // Load function parameters from stack - BLX R4 // Call service function - MRS R4,PSP // Get PSP - STMIA R4!,{R0-R3} // Store function return values + 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 -SVC_Done: - POP {R4,PC} // Return from handler + BX LR // Return from handler .fnend .size SVC_Handler, .-SVC_Handler diff --git a/CMSIS/RTOS2/RTX/Source/GCC/irq_cm3.S b/CMSIS/RTOS2/RTX/Source/GCC/irq_cm3.S index 0bdf091d31..bb3b9ae3d4 100644 --- a/CMSIS/RTOS2/RTX/Source/GCC/irq_cm3.S +++ b/CMSIS/RTOS2/RTX/Source/GCC/irq_cm3.S @@ -48,15 +48,19 @@ irqRtxLib: .cantunwind SVC_Handler: - MRS R0,PSP // Get PSP + TST LR,#0x04 // Determine return stack from EXC_RETURN bit 2 + ITE EQ + MRSEQ R0,MSP // Get MSP if return stack is MSP + MRSNE R0,PSP // Get PSP if return stack is PSP + LDR R1,[R0,#24] // Load saved PC from stack LDRB R1,[R1,#-2] // Load SVC number CBNZ R1,SVC_User // Branch if not SVC 0 - PUSH {R0,LR} // Save PSP and EXC_RETURN + PUSH {R0,LR} // Save SP 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 + POP {R12,LR} // Restore SP and EXC_RETURN STM R12,{R0-R1} // Store function return values SVC_Context: @@ -85,21 +89,19 @@ 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 + BHI SVC_Exit // Branch if out of range + PUSH {R0,LR} // Save SP and EXC_RETURN + LDR R12,[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 + BLX R12 // Call service function + POP {R12,LR} // Restore SP and EXC_RETURN + STR R0,[R12] // Store function return value -SVC_Done: - POP {R4,PC} // Return from handler + BX LR // Return from handler .fnend .size SVC_Handler, .-SVC_Handler @@ -112,9 +114,9 @@ SVC_Done: .cantunwind PendSV_Handler: - PUSH {R4,LR} // Save EXC_RETURN + PUSH {R0,LR} // Save EXC_RETURN BL osRtxPendSV_Handler // Call osRtxPendSV_Handler - POP {R4,LR} // Restore EXC_RETURN + POP {R0,LR} // Restore EXC_RETURN MRS R12,PSP B SVC_Context @@ -129,9 +131,9 @@ PendSV_Handler: .cantunwind SysTick_Handler: - PUSH {R4,LR} // Save EXC_RETURN + PUSH {R0,LR} // Save EXC_RETURN BL osRtxTick_Handler // Call osRtxTick_Handler - POP {R4,LR} // Restore EXC_RETURN + POP {R0,LR} // Restore EXC_RETURN MRS R12,PSP B SVC_Context diff --git a/CMSIS/RTOS2/RTX/Source/GCC/irq_cm4f.S b/CMSIS/RTOS2/RTX/Source/GCC/irq_cm4f.S index 8d10213d11..c775ded702 100644 --- a/CMSIS/RTOS2/RTX/Source/GCC/irq_cm4f.S +++ b/CMSIS/RTOS2/RTX/Source/GCC/irq_cm4f.S @@ -49,15 +49,19 @@ irqRtxLib: .cantunwind SVC_Handler: - MRS R0,PSP // Get PSP + TST LR,#0x04 // Determine return stack from EXC_RETURN bit 2 + ITE EQ + MRSEQ R0,MSP // Get MSP if return stack is MSP + MRSNE R0,PSP // Get PSP if return stack is PSP + LDR R1,[R0,#24] // Load saved PC from stack LDRB R1,[R1,#-2] // Load SVC number CBNZ R1,SVC_User // Branch if not SVC 0 - PUSH {R0,LR} // Save PSP and EXC_RETURN + PUSH {R0,LR} // Save SP 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 + POP {R12,LR} // Restore SP and EXC_RETURN STM R12,{R0-R1} // Store function return values SVC_Context: @@ -72,7 +76,7 @@ SVC_Context: BNE SVC_ContextSwitch LDR R1,=0xE000EF34 // FPCCR Address LDR R0,[R1] // Load FPCCR - BIC R0,#1 // Clear LSPACT (Lazy state) + BIC R0,R0,#1 // Clear LSPACT (Lazy state) STR R0,[R1] // Store FPCCR B SVC_ContextSwitch @@ -81,7 +85,6 @@ SVC_ContextSave: TST LR,#0x10 // Check if extended stack frame IT EQ VSTMDBEQ R12!,{S16-S31} // Save VFP S16.S31 - STR R12,[R1,#TCB_SP_OFS] // Store SP STRB LR, [R1,#TCB_SF_OFS] // Store stack frame information @@ -103,21 +106,19 @@ 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 + BHI SVC_Exit // Branch if out of range + PUSH {R0,LR} // Save SP and EXC_RETURN + LDR R12,[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 + BLX R12 // Call service function + POP {R12,LR} // Restore SP and EXC_RETURN + STR R0,[R12] // Store function return value -SVC_Done: - POP {R4,PC} // Return from handler + BX LR // Return from handler .fnend .size SVC_Handler, .-SVC_Handler @@ -130,9 +131,9 @@ SVC_Done: .cantunwind PendSV_Handler: - PUSH {R4,LR} // Save EXC_RETURN + PUSH {R0,LR} // Save EXC_RETURN BL osRtxPendSV_Handler // Call osRtxPendSV_Handler - POP {R4,LR} // Restore EXC_RETURN + POP {R0,LR} // Restore EXC_RETURN MRS R12,PSP B SVC_Context @@ -147,9 +148,9 @@ PendSV_Handler: .cantunwind SysTick_Handler: - PUSH {R4,LR} // Save EXC_RETURN + PUSH {R0,LR} // Save EXC_RETURN BL osRtxTick_Handler // Call osRtxTick_Handler - POP {R4,LR} // Restore EXC_RETURN + POP {R0,LR} // Restore EXC_RETURN MRS R12,PSP B SVC_Context diff --git a/CMSIS/RTOS2/RTX/Source/IAR/irq_armv8mbl.s b/CMSIS/RTOS2/RTX/Source/IAR/irq_armv8mbl.s index 3b4e9f3652..7e77ded3b9 100644 --- a/CMSIS/RTOS2/RTX/Source/IAR/irq_armv8mbl.s +++ b/CMSIS/RTOS2/RTX/Source/IAR/irq_armv8mbl.s @@ -1,5 +1,4 @@ - NAME irq_armv8mbl.s + NAME irq_armv8mbl.s #define __DOMAIN_NS 0 - INCLUDE irq_armv8mbl_common.s - + INCLUDE irq_armv8mbl_common.s END diff --git a/CMSIS/RTOS2/RTX/Source/IAR/irq_armv8mbl_common.s b/CMSIS/RTOS2/RTX/Source/IAR/irq_armv8mbl_common.s index 7cce8f567f..18c1f9ef46 100644 --- a/CMSIS/RTOS2/RTX/Source/IAR/irq_armv8mbl_common.s +++ b/CMSIS/RTOS2/RTX/Source/IAR/irq_armv8mbl_common.s @@ -57,17 +57,22 @@ SVC_Handler 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 PSP and EXC_RETURN + 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 PSP and EXC_RETURN + POP {R2,R3} ; Restore SP and EXC_RETURN STMIA R2!,{R0-R1} ; Store function return values MOV LR,R3 ; Set EXC_RETURN @@ -92,7 +97,7 @@ SVC_ContextSave SVC_ContextSave1 MRS R0,PSP ; Get PSP - SUBS R0,R0,#32 ; Adjust 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 @@ -154,23 +159,27 @@ SVC_ContextRestore2 SVC_Exit BX LR ; Exit from handler +SVC_MSP + MRS R0,MSP ; Get MSP + B SVC_Number + 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 + BHI SVC_Exit ; Branch if out of range + PUSH {R0,LR} ; Save SP and EXC_RETURN LSLS R1,R1,#2 - LDR R4,[R2,R1] ; 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 + 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 -SVC_Done - POP {R4,PC} ; Return from handler + BX LR ; Return from handler PendSV_Handler diff --git a/CMSIS/RTOS2/RTX/Source/IAR/irq_armv8mbl_ns.s b/CMSIS/RTOS2/RTX/Source/IAR/irq_armv8mbl_ns.s index 7ee9d0465d..833f5b6741 100644 --- a/CMSIS/RTOS2/RTX/Source/IAR/irq_armv8mbl_ns.s +++ b/CMSIS/RTOS2/RTX/Source/IAR/irq_armv8mbl_ns.s @@ -1,5 +1,4 @@ - NAME irq_armv8mbl_ns.s + NAME irq_armv8mbl_ns.s #define __DOMAIN_NS 1 - INCLUDE irq_armv8mbl_common.s - + INCLUDE irq_armv8mbl_common.s END diff --git a/CMSIS/RTOS2/RTX/Source/IAR/irq_armv8mml.s b/CMSIS/RTOS2/RTX/Source/IAR/irq_armv8mml.s index 8a8bd8a0f1..490168b459 100644 --- a/CMSIS/RTOS2/RTX/Source/IAR/irq_armv8mml.s +++ b/CMSIS/RTOS2/RTX/Source/IAR/irq_armv8mml.s @@ -1,5 +1,4 @@ - NAME irq_armv8mml.s + NAME irq_armv8mml.s #define __DOMAIN_NS 0 - INCLUDE irq_armv8mml_common.s - END - + INCLUDE irq_armv8mml_common.s + END diff --git a/CMSIS/RTOS2/RTX/Source/IAR/irq_armv8mml_common.s b/CMSIS/RTOS2/RTX/Source/IAR/irq_armv8mml_common.s index 5edbf07bcf..d7826cd470 100644 --- a/CMSIS/RTOS2/RTX/Source/IAR/irq_armv8mml_common.s +++ b/CMSIS/RTOS2/RTX/Source/IAR/irq_armv8mml_common.s @@ -63,16 +63,20 @@ SVC_Handler IMPORT TZ_StoreContext_S #endif - MRS R0,PSP ; Get PSP + TST LR,#0x04 ; Determine return stack from EXC_RETURN bit 2 + ITE EQ + MRSEQ R0,MSP ; Get MSP if return stack is MSP + MRSNE R0,PSP ; Get PSP if return stack is 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 + PUSH {R0,LR} ; Save SP 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 + POP {R12,LR} ; Restore SP and EXC_RETURN STM R12,{R0-R1} ; Store function return values SVC_Context @@ -88,7 +92,7 @@ SVC_Context BNE SVC_ContextSwitch LDR R1,=0xE000EF34 ; FPCCR Address LDR R0,[R1] ; Load FPCCR - BIC R0,#1 ; Clear LSPACT (Lazy state) + BIC R0,R0,#1 ; Clear LSPACT (Lazy state) STR R0,[R1] ; Store FPCCR B SVC_ContextSwitch #else @@ -109,6 +113,7 @@ SVC_ContextSave1 STMDB R0!,{R4-R11} ; Save R4..R11 #if (__FPU_USED == 1) TST LR,#0x10 ; Check if extended stack frame + IT EQ VSTMDBEQ R0!,{S16-S31} ; Save VFP S16.S31 #endif @@ -142,6 +147,7 @@ SVC_ContextRestore1 #if (__FPU_USED == 1) TST LR,#0x10 ; Check if extended stack frame + IT EQ VLDMIAEQ R0!,{S16-S31} ; Restore VFP S16..S31 #endif LDMIA R0!,{R4-R11} ; Restore R4..R11 @@ -153,30 +159,28 @@ 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 + BHI SVC_Exit ; Branch if out of range + PUSH {R0,LR} ; Save SP and EXC_RETURN + LDR R12,[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 + BLX R12 ; Call service function + POP {R12,LR} ; Restore SP and EXC_RETURN + STR R0,[R12] ; Store function return value -SVC_Done - POP {R4,PC} ; Return from handler + BX LR ; Return from handler PendSV_Handler EXPORT PendSV_Handler IMPORT osRtxPendSV_Handler - PUSH {R4,LR} ; Save EXC_RETURN + PUSH {R0,LR} ; Save EXC_RETURN BL osRtxPendSV_Handler ; Call osRtxPendSV_Handler - POP {R4,LR} ; Restore EXC_RETURN + POP {R0,LR} ; Restore EXC_RETURN B Sys_Context @@ -184,9 +188,9 @@ SysTick_Handler EXPORT SysTick_Handler IMPORT osRtxTick_Handler - PUSH {R4,LR} ; Save EXC_RETURN + PUSH {R0,LR} ; Save EXC_RETURN BL osRtxTick_Handler ; Call osRtxTick_Handler - POP {R4,LR} ; Restore EXC_RETURN + POP {R0,LR} ; Restore EXC_RETURN B Sys_Context @@ -223,6 +227,7 @@ Sys_ContextSave1 STMDB R0!,{R4-R11} ; Save R4..R11 #if (__FPU_USED == 1) TST LR,#0x10 ; Check if extended stack frame + IT EQ VSTMDBEQ R0!,{S16-S31} ; Save VFP S16.S31 #endif @@ -256,6 +261,7 @@ Sys_ContextRestore1 #if (__FPU_USED == 1) TST LR,#0x10 ; Check if extended stack frame + IT EQ VLDMIAEQ R0!,{S16-S31} ; Restore VFP S16..S31 #endif LDMIA R0!,{R4-R11} ; Restore R4..R11 diff --git a/CMSIS/RTOS2/RTX/Source/IAR/irq_armv8mml_ns.s b/CMSIS/RTOS2/RTX/Source/IAR/irq_armv8mml_ns.s index 827abb4920..9d9681261f 100644 --- a/CMSIS/RTOS2/RTX/Source/IAR/irq_armv8mml_ns.s +++ b/CMSIS/RTOS2/RTX/Source/IAR/irq_armv8mml_ns.s @@ -1,5 +1,4 @@ - NAME irq_armv8mml_ns.s + NAME irq_armv8mml_ns.s #define __DOMAIN_NS 1 - INCLUDE irq_armv8mml_common.s - END - + INCLUDE irq_armv8mml_common.s + END diff --git a/CMSIS/RTOS2/RTX/Source/IAR/irq_cm0.s b/CMSIS/RTOS2/RTX/Source/IAR/irq_cm0.s index 9d11699931..f8fd2e4458 100644 --- a/CMSIS/RTOS2/RTX/Source/IAR/irq_cm0.s +++ b/CMSIS/RTOS2/RTX/Source/IAR/irq_cm0.s @@ -43,22 +43,27 @@ irqRtxLib DCB 0 ; Non weak library reference SECTION .text:CODE:NOROOT(2) -SVC_Handler +SVC_Handler EXPORT SVC_Handler IMPORT osRtxUserSVC IMPORT osRtxInfo + 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 PSP and EXC_RETURN + PUSH {R0,LR} ; Save SP and EXC_RETURN LDMIA R0,{R0-R3} ; Load function parameters from stack BLX R7 ; Call service function - POP {R2,R3} ; Restore PSP and EXC_RETURN + POP {R2,R3} ; Restore SP and EXC_RETURN STMIA R2!,{R0-R1} ; Store function return values MOV LR,R3 ; Set EXC_RETURN @@ -73,7 +78,7 @@ SVC_Context SVC_ContextSave MRS R0,PSP ; Get PSP - SUBS R0,R0,#32 ; Adjust address + SUBS R0,R0,#32 ; Calculate SP STR R0,[R1,#TCB_SP_OFS] ; Store SP STMIA R0!,{R4-R7} ; Save R4..R7 MOV R4,R8 @@ -83,7 +88,7 @@ SVC_ContextSave STMIA R0!,{R4-R7} ; Save R8..R11 SVC_ContextSwitch - SUBS R3,R3,#8 + SUBS R3,R3,#8 ; Adjust address STR R2,[R3] ; osRtxInfo.thread.run: curr = next SVC_ContextRestore @@ -102,29 +107,33 @@ SVC_ContextRestore MVNS R0,R0 ; Set EXC_RETURN value BX R0 ; Exit from handler +SVC_MSP + MRS R0,MSP ; Get MSP + B SVC_Number + 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 + BHI SVC_Exit ; Branch if out of range + PUSH {R0,LR} ; Save SP and EXC_RETURN LSLS R1,R1,#2 - LDR R4,[R2,R1] ; Load address of SVC function - + LDR R3,[R2,R1] ; Load address of SVC function + MOV R12,R3 LDMIA R0,{R0-R3} ; Load function parameters from stack - BLX R4 ; Call service function - MRS R4,PSP ; Get PSP - STMIA R4!,{R0-R3} ; Store function return values + 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 -SVC_Done - POP {R4,PC} ; Return from handler + BX LR ; Return from handler -PendSV_Handler +PendSV_Handler EXPORT PendSV_Handler IMPORT osRtxPendSV_Handler @@ -135,7 +144,7 @@ PendSV_Handler B SVC_Context -SysTick_Handler +SysTick_Handler EXPORT SysTick_Handler IMPORT osRtxTick_Handler diff --git a/CMSIS/RTOS2/RTX/Source/IAR/irq_cm3.s b/CMSIS/RTOS2/RTX/Source/IAR/irq_cm3.s index 0bffcb3468..821280a4e3 100644 --- a/CMSIS/RTOS2/RTX/Source/IAR/irq_cm3.s +++ b/CMSIS/RTOS2/RTX/Source/IAR/irq_cm3.s @@ -43,20 +43,24 @@ irqRtxLib DCB 0 ; Non weak library reference SECTION .text:CODE:NOROOT(2) -SVC_Handler +SVC_Handler EXPORT SVC_Handler IMPORT osRtxUserSVC IMPORT osRtxInfo - MRS R0,PSP ; Get PSP + TST LR,#0x04 ; Determine return stack from EXC_RETURN bit 2 + ITE EQ + MRSEQ R0,MSP ; Get MSP if return stack is MSP + MRSNE R0,PSP ; Get PSP if return stack is PSP + LDR R1,[R0,#24] ; Load saved PC from stack LDRB R1,[R1,#-2] ; Load SVC number CBNZ R1,SVC_User ; Branch if not SVC 0 - PUSH {R0,LR} ; Save PSP and EXC_RETURN + PUSH {R0,LR} ; Save SP 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 + POP {R12,LR} ; Restore SP and EXC_RETURN STM R12,{R0-R1} ; Store function return values SVC_Context @@ -86,41 +90,39 @@ 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 + BHI SVC_Exit ; Branch if out of range + PUSH {R0,LR} ; Save SP and EXC_RETURN + LDR R12,[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 + BLX R12 ; Call service function + POP {R12,LR} ; Restore SP and EXC_RETURN + STR R0,[R12] ; Store function return value -SVC_Done - POP {R4,PC} ; Return from handler + BX LR ; Return from handler -PendSV_Handler +PendSV_Handler EXPORT PendSV_Handler IMPORT osRtxPendSV_Handler - PUSH {R4,LR} ; Save EXC_RETURN + PUSH {R0,LR} ; Save EXC_RETURN BL osRtxPendSV_Handler ; Call osRtxPendSV_Handler - POP {R4,LR} ; Restore EXC_RETURN + POP {R0,LR} ; Restore EXC_RETURN MRS R12,PSP B SVC_Context -SysTick_Handler +SysTick_Handler EXPORT SysTick_Handler IMPORT osRtxTick_Handler - PUSH {R4,LR} ; Save EXC_RETURN + PUSH {R0,LR} ; Save EXC_RETURN BL osRtxTick_Handler ; Call osRtxTick_Handler - POP {R4,LR} ; Restore EXC_RETURN + POP {R0,LR} ; Restore EXC_RETURN MRS R12,PSP B SVC_Context diff --git a/CMSIS/RTOS2/RTX/Source/IAR/irq_cm4f.s b/CMSIS/RTOS2/RTX/Source/IAR/irq_cm4f.s index 0d84409b39..b59f455bbc 100644 --- a/CMSIS/RTOS2/RTX/Source/IAR/irq_cm4f.s +++ b/CMSIS/RTOS2/RTX/Source/IAR/irq_cm4f.s @@ -43,20 +43,25 @@ irqRtxLib DCB 0 ; Non weak library reference THUMB SECTION .text:CODE:NOROOT(2) + SVC_Handler EXPORT SVC_Handler IMPORT osRtxUserSVC IMPORT osRtxInfo - MRS R0,PSP ; Get PSP + TST LR,#0x04 ; Determine return stack from EXC_RETURN bit 2 + ITE EQ + MRSEQ R0,MSP ; Get MSP if return stack is MSP + MRSNE R0,PSP ; Get PSP if return stack is PSP + LDR R1,[R0,#24] ; Load saved PC from stack LDRB R1,[R1,#-2] ; Load SVC number CBNZ R1,SVC_User ; Branch if not SVC 0 - PUSH {R0,LR} ; Save PSP and EXC_RETURN + PUSH {R0,LR} ; Save SP 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 + POP {R12,LR} ; Restore SP and EXC_RETURN STM R12,{R0-R1} ; Store function return values SVC_Context @@ -80,7 +85,6 @@ SVC_ContextSave TST LR,#0x10 ; Check if extended stack frame IT EQ VSTMDBEQ R12!,{S16-S31} ; Save VFP S16.S31 - STR R12,[R1,#TCB_SP_OFS] ; Store SP STRB LR, [R1,#TCB_SF_OFS] ; Store stack frame information @@ -102,41 +106,39 @@ 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 + BHI SVC_Exit ; Branch if out of range + PUSH {R0,LR} ; Save SP and EXC_RETURN + LDR R12,[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 + BLX R12 ; Call service function + POP {R12,LR} ; Restore SP and EXC_RETURN + STR R0,[R12] ; Store function return value -SVC_Done - POP {R4,PC} ; Return from handler + BX LR ; Return from handler -PendSV_Handler +PendSV_Handler EXPORT PendSV_Handler IMPORT osRtxPendSV_Handler - PUSH {R4,LR} ; Save EXC_RETURN + PUSH {R0,LR} ; Save EXC_RETURN BL osRtxPendSV_Handler ; Call osRtxPendSV_Handler - POP {R4,LR} ; Restore EXC_RETURN + POP {R0,LR} ; Restore EXC_RETURN MRS R12,PSP B SVC_Context -SysTick_Handler +SysTick_Handler EXPORT SysTick_Handler IMPORT osRtxTick_Handler - PUSH {R4,LR} ; Save EXC_RETURN + PUSH {R0,LR} ; Save EXC_RETURN BL osRtxTick_Handler ; Call osRtxTick_Handler - POP {R4,LR} ; Restore EXC_RETURN + POP {R0,LR} ; Restore EXC_RETURN MRS R12,PSP B SVC_Context diff --git a/CMSIS/RTOS2/RTX/Source/rtx_core_ca.h b/CMSIS/RTOS2/RTX/Source/rtx_core_ca.h index 0f9109fae9..0b8e69bab9 100644 --- a/CMSIS/RTOS2/RTX/Source/rtx_core_ca.h +++ b/CMSIS/RTOS2/RTX/Source/rtx_core_ca.h @@ -161,12 +161,6 @@ __STATIC_INLINE t __svc##f (t1 a1, t2 a2, t3 a3, t4 a4) { \ return svc##f(svcRtx##f,a1,a2,a3,a4); \ } -#define SVC0_0M SVC0_0 -#define SVC0_1M SVC0_1 -#define SVC0_2M SVC0_2 -#define SVC0_3M SVC0_3 -#define SVC0_4M SVC0_4 - #elif defined(__ICCARM__) #define SVC_Setup(f) \ @@ -241,12 +235,6 @@ __STATIC_INLINE t __svc##f (t1 a1, t2 a2, t3 a3, t4 a4) { \ return svc##f(a1,a2,a3,a4); \ } -#define SVC0_0M SVC0_0 -#define SVC0_1M SVC0_1 -#define SVC0_2M SVC0_2 -#define SVC0_3M SVC0_3 -#define SVC0_4M SVC0_4 - #else // !(defined(__CC_ARM) || defined(__ICCARM__)) #define SVC_RegF "r12" @@ -343,12 +331,6 @@ __STATIC_INLINE t __svc##f (t1 a1, t2 a2, t3 a3, t4 a4) { \ return (t) __r0; \ } -#define SVC0_0M SVC0_0 -#define SVC0_1M SVC0_1 -#define SVC0_2M SVC0_2 -#define SVC0_3M SVC0_3 -#define SVC0_4M SVC0_4 - #endif diff --git a/CMSIS/RTOS2/RTX/Source/rtx_core_cm.h b/CMSIS/RTOS2/RTX/Source/rtx_core_cm.h index 3f2e8d1a4b..ef0b66456b 100644 --- a/CMSIS/RTOS2/RTX/Source/rtx_core_cm.h +++ b/CMSIS/RTOS2/RTX/Source/rtx_core_cm.h @@ -100,20 +100,6 @@ #define __SVC_INDIRECT(n) __svc_indirect_r7(n) #endif -#if (__FPU_USED == 1U) -#define SVC_SETUP_PSP \ - uint32_t control = __get_CONTROL(); \ - if ((control & 2U) == 0U) { \ - __set_PSP((__get_MSP() - ((control & 4U) ? 104U : 32U)) & ~7U); \ - } -#else -#define SVC_SETUP_PSP \ - uint32_t control = __get_CONTROL(); \ - if ((control & 2U) == 0U) { \ - __set_PSP((__get_MSP() - 32U) & ~7U); \ - } -#endif - #define SVC0_0N(f,t) \ __SVC_INDIRECT(0) t svc##f (t(*)()); \ t svcRtx##f (void); \ @@ -130,15 +116,6 @@ __STATIC_INLINE t __svc##f (void) { \ return svc##f(svcRtx##f); \ } -#define SVC0_0M(f,t) \ -__SVC_INDIRECT(0) t svc##f (t(*)()); \ - t svcRtx##f (void); \ -__attribute__((always_inline)) \ -__STATIC_INLINE t __svc##f (void) { \ - SVC_SETUP_PSP \ - return svc##f(svcRtx##f); \ -} - #define SVC0_1N(f,t,t1) \ __SVC_INDIRECT(0) t svc##f (t(*)(t1),t1); \ t svcRtx##f (t1 a1); \ @@ -155,15 +132,6 @@ __STATIC_INLINE t __svc##f (t1 a1) { \ return svc##f(svcRtx##f,a1); \ } -#define SVC0_1M(f,t,t1) \ -__SVC_INDIRECT(0) t svc##f (t(*)(t1),t1); \ - t svcRtx##f (t1 a1); \ -__attribute__((always_inline)) \ -__STATIC_INLINE t __svc##f (t1 a1) { \ - SVC_SETUP_PSP \ - return svc##f(svcRtx##f,a1); \ -} - #define SVC0_2(f,t,t1,t2) \ __SVC_INDIRECT(0) t svc##f (t(*)(t1,t2),t1,t2); \ t svcRtx##f (t1 a1, t2 a2); \ @@ -172,15 +140,6 @@ __STATIC_INLINE t __svc##f (t1 a1, t2 a2) { \ return svc##f(svcRtx##f,a1,a2); \ } -#define SVC0_2M(f,t,t1,t2) \ -__SVC_INDIRECT(0) t svc##f (t(*)(t1,t2),t1,t2); \ - t svcRtx##f (t1 a1, t2 a2); \ -__attribute__((always_inline)) \ -__STATIC_INLINE t __svc##f (t1 a1, t2 a2) { \ - SVC_SETUP_PSP \ - return svc##f(svcRtx##f,a1,a2); \ -} - #define SVC0_3(f,t,t1,t2,t3) \ __SVC_INDIRECT(0) t svc##f (t(*)(t1,t2,t3),t1,t2,t3); \ t svcRtx##f (t1 a1, t2 a2, t3 a3); \ @@ -189,15 +148,6 @@ __STATIC_INLINE t __svc##f (t1 a1, t2 a2, t3 a3) { \ return svc##f(svcRtx##f,a1,a2,a3); \ } -#define SVC0_3M(f,t,t1,t2,t3) \ -__SVC_INDIRECT(0) t svc##f (t(*)(t1,t2,t3),t1,t2,t3); \ - t svcRtx##f (t1 a1, t2 a2, t3 a3); \ -__attribute__((always_inline)) \ -__STATIC_INLINE t __svc##f (t1 a1, t2 a2, t3 a3) { \ - SVC_SETUP_PSP \ - return svc##f(svcRtx##f,a1,a2,a3); \ -} - #define SVC0_4(f,t,t1,t2,t3,t4) \ __SVC_INDIRECT(0) t svc##f (t(*)(t1,t2,t3,t4),t1,t2,t3,t4); \ t svcRtx##f (t1 a1, t2 a2, t3 a3, t4 a4); \ @@ -206,15 +156,6 @@ __STATIC_INLINE t __svc##f (t1 a1, t2 a2, t3 a3, t4 a4) { \ return svc##f(svcRtx##f,a1,a2,a3,a4); \ } -#define SVC0_4M(f,t,t1,t2,t3,t4) \ -__SVC_INDIRECT(0) t svc##f (t(*)(t1,t2,t3,t4),t1,t2,t3,t4); \ - t svcRtx##f (t1 a1, t2 a2, t3 a3, t4 a4); \ -__attribute__((always_inline)) \ -__STATIC_INLINE t __svc##f (t1 a1, t2 a2, t3 a3, t4 a4) { \ - SVC_SETUP_PSP \ - return svc##f(svcRtx##f,a1,a2,a3,a4); \ -} - #elif defined(__ICCARM__) #if ((__ARM_ARCH_7M__ == 1U) || \ @@ -237,20 +178,6 @@ __STATIC_INLINE t __svc##f (t1 a1, t2 a2, t3 a3, t4 a4) { \ #define STRINGIFY(a) #a #define __SVC_INDIRECT(n) _Pragma(STRINGIFY(swi_number = n)) __swi -#if (__FPU_USED == 1U) -#define SVC_SETUP_PSP \ - uint32_t control = __get_CONTROL(); \ - if ((control & 2U) == 0U) { \ - __set_PSP((__get_MSP() - ((control & 4U) ? 104U : 32U)) & ~7U); \ - } -#else -#define SVC_SETUP_PSP \ - uint32_t control = __get_CONTROL(); \ - if ((control & 2U) == 0U) { \ - __set_PSP((__get_MSP() - 32U) & ~7U); \ - } -#endif - #define SVC0_0N(f,t) \ __SVC_INDIRECT(0) t svc##f (); \ t svcRtx##f (void); \ @@ -269,16 +196,6 @@ __STATIC_INLINE t __svc##f (void) { \ return svc##f(); \ } -#define SVC0_0M(f,t) \ -__SVC_INDIRECT(0) t svc##f (); \ - t svcRtx##f (void); \ -__attribute__((always_inline)) \ -__STATIC_INLINE t __svc##f (void) { \ - SVC_SETUP_PSP \ - SVC_Setup(svcRtx##f); \ - return svc##f(); \ -} - #define SVC0_1N(f,t,t1) \ __SVC_INDIRECT(0) t svc##f (t1 a1); \ t svcRtx##f (t1 a1); \ @@ -297,16 +214,6 @@ __STATIC_INLINE t __svc##f (t1 a1) { \ return svc##f(a1); \ } -#define SVC0_1M(f,t,t1) \ -__SVC_INDIRECT(0) t svc##f (t1 a1); \ - t svcRtx##f (t1 a1); \ -__attribute__((always_inline)) \ -__STATIC_INLINE t __svc##f (t1 a1) { \ - SVC_SETUP_PSP \ - SVC_Setup(svcRtx##f); \ - return svc##f(a1); \ -} - #define SVC0_2(f,t,t1,t2) \ __SVC_INDIRECT(0) t svc##f (t1 a1, t2 a2); \ t svcRtx##f (t1 a1, t2 a2); \ @@ -316,16 +223,6 @@ __STATIC_INLINE t __svc##f (t1 a1, t2 a2) { \ return svc##f(a1,a2); \ } -#define SVC0_2M(f,t,t1,t2) \ -__SVC_INDIRECT(0) t svc##f (t1 a1, t2 a2); \ - t svcRtx##f (t1 a1, t2 a2); \ -__attribute__((always_inline)) \ -__STATIC_INLINE t __svc##f (t1 a1, t2 a2) { \ - SVC_SETUP_PSP \ - SVC_Setup(svcRtx##f); \ - return svc##f(a1,a2); \ -} - #define SVC0_3(f,t,t1,t2,t3) \ __SVC_INDIRECT(0) t svc##f (t1 a1, t2 a2, t3 a3); \ t svcRtx##f (t1 a1, t2 a2, t3 a3); \ @@ -335,16 +232,6 @@ __STATIC_INLINE t __svc##f (t1 a1, t2 a2, t3 a3) { \ return svc##f(a1,a2,a3); \ } -#define SVC0_3M(f,t,t1,t2,t3) \ -__SVC_INDIRECT(0) t svc##f (t1 a1, t2 a2, t3 a3); \ - t svcRtx##f (t1 a1, t2 a2, t3 a3); \ -__attribute__((always_inline)) \ -__STATIC_INLINE t __svc##f (t1 a1, t2 a2, t3 a3) { \ - SVC_SETUP_PSP \ - SVC_Setup(svcRtx##f); \ - return svc##f(a1,a2,a3); \ -} - #define SVC0_4(f,t,t1,t2,t3,t4) \ __SVC_INDIRECT(0) t svc##f (t1 a1, t2 a2, t3 a3, t4 a4); \ t svcRtx##f (t1 a1, t2 a2, t3 a3, t4 a4); \ @@ -354,16 +241,6 @@ __STATIC_INLINE t __svc##f (t1 a1, t2 a2, t3 a3, t4 a4) { \ return svc##f(a1,a2,a3,a4); \ } -#define SVC0_4M(f,t,t1,t2,t3,t4) \ -__SVC_INDIRECT(0) t svc##f (t1 a1, t2 a2, t3 a3, t4 a4); \ - t svcRtx##f (t1 a1, t2 a2, t3 a3, t4 a4); \ -__attribute__((always_inline)) \ -__STATIC_INLINE t __svc##f (t1 a1, t2 a2, t3 a3, t4 a4) { \ - SVC_SETUP_PSP \ - SVC_Setup(svcRtx##f); \ - return svc##f(a1,a2,a3,a4); \ -} - #else // !(defined(__CC_ARM) || defined(__ICCARM__)) #if ((__ARM_ARCH_7M__ == 1U) || \ @@ -401,62 +278,6 @@ register uint32_t __rf __ASM(SVC_RegF) = (uint32_t)f #define SVC_Call0(in, out, cl) \ __ASM volatile ("svc 0" : out : in : cl) -#if ((__ARM_ARCH_7M__ == 1U) || \ - (__ARM_ARCH_7EM__ == 1U) || \ - (__ARM_ARCH_8M_MAIN__ == 1U)) -#if (__FPU_USED == 1U) -#define SVC_Call0M(in, out, cl) \ - register uint32_t val; \ - __ASM volatile ( \ - ".syntax unified\n\t" \ - "mrs %[val],control\n\t" \ - "tst %[val],#2\n\t" \ - "bne 0f\n\t" \ - "tst %[val],#4\n\t" \ - "mrs %[val],msp\n\t" \ - "ite eq\n\t" \ - "subeq %[val],#32\n\t" \ - "subne %[val],#104\n\t" \ - "bic %[val],#7\n\t" \ - "msr psp,%[val]\n\t" \ - "0:\n\t" \ - "svc 0" \ - : out, [val] "=&l" (val) : in : cl) -#else -#define SVC_Call0M(in, out, cl) \ - register uint32_t val; \ - __ASM volatile ( \ - ".syntax unified\n\t" \ - "mrs %[val],control\n\t" \ - "tst %[val],#2\n\t" \ - "bne 0f\n\t" \ - "mrs %[val],msp\n\t" \ - "subs %[val],#32\n\t" \ - "bic %[val],#7\n\t" \ - "msr psp,%[val]\n\t" \ - "0:\n\t" \ - "svc 0" \ - : out, [val] "=&l" (val) : in : cl) -#endif -#elif ((__ARM_ARCH_6M__ == 1U) || \ - (__ARM_ARCH_8M_BASE__ == 1U)) -#define SVC_Call0M(in, out, cl) \ - register uint32_t val; \ - __ASM volatile ( \ - ".syntax unified\n\t" \ - "mrs %[val],control\n\t" \ - "lsls %[val],#30\n\t" \ - "bmi 0f\n\t" \ - "mrs %[val],msp\n\t" \ - "subs %[val],#32\n\t" \ - "lsrs %[val],#3\n\t" \ - "lsls %[val],#3\n\t" \ - "msr psp,%[val]\n\t" \ - "0:\n\t" \ - "svc 0" \ - : out, [val] "=&l" (val) : in : cl) -#endif - #define SVC0_0N(f,t) \ __attribute__((always_inline)) \ __STATIC_INLINE t __svc##f (void) { \ @@ -473,15 +294,6 @@ __STATIC_INLINE t __svc##f (void) { \ return (t) __r0; \ } -#define SVC0_0M(f,t) \ -__attribute__((always_inline)) \ -__STATIC_INLINE t __svc##f (void) { \ - SVC_ArgN(0); \ - SVC_ArgF(svcRtx##f); \ - SVC_Call0M(SVC_In0, SVC_Out1, SVC_CL1); \ - return (t) __r0; \ -} - #define SVC0_1N(f,t,t1) \ __attribute__((always_inline)) \ __STATIC_INLINE t __svc##f (t1 a1) { \ @@ -499,15 +311,6 @@ __STATIC_INLINE t __svc##f (t1 a1) { \ return (t) __r0; \ } -#define SVC0_1M(f,t,t1) \ -__attribute__((always_inline)) \ -__STATIC_INLINE t __svc##f (t1 a1) { \ - SVC_ArgR(0,a1); \ - SVC_ArgF(svcRtx##f); \ - SVC_Call0M(SVC_In1, SVC_Out1, SVC_CL1); \ - return (t) __r0; \ -} - #define SVC0_2(f,t,t1,t2) \ __attribute__((always_inline)) \ __STATIC_INLINE t __svc##f (t1 a1, t2 a2) { \ @@ -518,16 +321,6 @@ __STATIC_INLINE t __svc##f (t1 a1, t2 a2) { \ return (t) __r0; \ } -#define SVC0_2M(f,t,t1,t2) \ -__attribute__((always_inline)) \ -__STATIC_INLINE t __svc##f (t1 a1, t2 a2) { \ - SVC_ArgR(0,a1); \ - SVC_ArgR(1,a2); \ - SVC_ArgF(svcRtx##f); \ - SVC_Call0M(SVC_In2, SVC_Out1, SVC_CL0); \ - return (t) __r0; \ -} - #define SVC0_3(f,t,t1,t2,t3) \ __attribute__((always_inline)) \ __STATIC_INLINE t __svc##f (t1 a1, t2 a2, t3 a3) { \ @@ -539,17 +332,6 @@ __STATIC_INLINE t __svc##f (t1 a1, t2 a2, t3 a3) { \ return (t) __r0; \ } -#define SVC0_3M(f,t,t1,t2,t3) \ -__attribute__((always_inline)) \ -__STATIC_INLINE t __svc##f (t1 a1, t2 a2, t3 a3) { \ - SVC_ArgR(0,a1); \ - SVC_ArgR(1,a2); \ - SVC_ArgR(2,a3); \ - SVC_ArgF(svcRtx##f); \ - SVC_Call0M(SVC_In3, SVC_Out1, SVC_CL0); \ - return (t) __r0; \ -} - #define SVC0_4(f,t,t1,t2,t3,t4) \ __attribute__((always_inline)) \ __STATIC_INLINE t __svc##f (t1 a1, t2 a2, t3 a3, t4 a4) { \ @@ -562,18 +344,6 @@ __STATIC_INLINE t __svc##f (t1 a1, t2 a2, t3 a3, t4 a4) { \ return (t) __r0; \ } -#define SVC0_4M(f,t,t1,t2,t3,t4) \ -__attribute__((always_inline)) \ -__STATIC_INLINE t __svc##f (t1 a1, t2 a2, t3 a3, t4 a4) { \ - SVC_ArgR(0,a1); \ - SVC_ArgR(1,a2); \ - SVC_ArgR(2,a3); \ - SVC_ArgR(3,a4); \ - SVC_ArgF(svcRtx##f); \ - SVC_Call0M(SVC_In4, SVC_Out1, SVC_CL0); \ - return (t) __r0; \ -} - #endif diff --git a/CMSIS/RTOS2/RTX/Source/rtx_evflags.c b/CMSIS/RTOS2/RTX/Source/rtx_evflags.c index 1c534d5ca3..c1d07186f2 100644 --- a/CMSIS/RTOS2/RTX/Source/rtx_evflags.c +++ b/CMSIS/RTOS2/RTX/Source/rtx_evflags.c @@ -157,13 +157,13 @@ void osRtxEventFlagsPostProcess (os_event_flags_t *ef) { // ==== Service Calls ==== // Service Calls definitions -SVC0_1M(EventFlagsNew, osEventFlagsId_t, const osEventFlagsAttr_t *) -SVC0_1 (EventFlagsGetName, const char *, osEventFlagsId_t) -SVC0_2 (EventFlagsSet, uint32_t, osEventFlagsId_t, uint32_t) -SVC0_2 (EventFlagsClear, uint32_t, osEventFlagsId_t, uint32_t) -SVC0_1 (EventFlagsGet, uint32_t, osEventFlagsId_t) -SVC0_4 (EventFlagsWait, uint32_t, osEventFlagsId_t, uint32_t, uint32_t, uint32_t) -SVC0_1 (EventFlagsDelete, osStatus_t, osEventFlagsId_t) +SVC0_1(EventFlagsNew, osEventFlagsId_t, const osEventFlagsAttr_t *) +SVC0_1(EventFlagsGetName, const char *, osEventFlagsId_t) +SVC0_2(EventFlagsSet, uint32_t, osEventFlagsId_t, uint32_t) +SVC0_2(EventFlagsClear, uint32_t, osEventFlagsId_t, uint32_t) +SVC0_1(EventFlagsGet, uint32_t, osEventFlagsId_t) +SVC0_4(EventFlagsWait, uint32_t, osEventFlagsId_t, uint32_t, uint32_t, uint32_t) +SVC0_1(EventFlagsDelete, osStatus_t, osEventFlagsId_t) /// Create and Initialize an Event Flags object. /// \note API identical to osEventFlagsNew diff --git a/CMSIS/RTOS2/RTX/Source/rtx_kernel.c b/CMSIS/RTOS2/RTX/Source/rtx_kernel.c index fad31603bf..d97f38f96d 100644 --- a/CMSIS/RTOS2/RTX/Source/rtx_kernel.c +++ b/CMSIS/RTOS2/RTX/Source/rtx_kernel.c @@ -65,9 +65,9 @@ static void KernelUnblock (void) { // ==== Service Calls ==== // Service Calls definitions -SVC0_0M(KernelInitialize, osStatus_t) +SVC0_0 (KernelInitialize, osStatus_t) SVC0_3 (KernelGetInfo, osStatus_t, osVersion_t *, char *, uint32_t) -SVC0_0M(KernelStart, osStatus_t) +SVC0_0 (KernelStart, osStatus_t) SVC0_0 (KernelLock, int32_t) SVC0_0 (KernelUnlock, int32_t) SVC0_1 (KernelRestoreLock, int32_t, int32_t) diff --git a/CMSIS/RTOS2/RTX/Source/rtx_mempool.c b/CMSIS/RTOS2/RTX/Source/rtx_mempool.c index 63bb32a4e3..923352ba9d 100644 --- a/CMSIS/RTOS2/RTX/Source/rtx_mempool.c +++ b/CMSIS/RTOS2/RTX/Source/rtx_mempool.c @@ -177,15 +177,15 @@ void osRtxMemoryPoolPostProcess (os_memory_pool_t *mp) { // ==== Service Calls ==== // Service Calls definitions -SVC0_3M(MemoryPoolNew, osMemoryPoolId_t, uint32_t, uint32_t, const osMemoryPoolAttr_t *) -SVC0_1 (MemoryPoolGetName, const char *, osMemoryPoolId_t) -SVC0_2 (MemoryPoolAlloc, void *, osMemoryPoolId_t, uint32_t) -SVC0_2 (MemoryPoolFree, osStatus_t, osMemoryPoolId_t, void *) -SVC0_1 (MemoryPoolGetCapacity, uint32_t, osMemoryPoolId_t) -SVC0_1 (MemoryPoolGetBlockSize, uint32_t, osMemoryPoolId_t) -SVC0_1 (MemoryPoolGetCount, uint32_t, osMemoryPoolId_t) -SVC0_1 (MemoryPoolGetSpace, uint32_t, osMemoryPoolId_t) -SVC0_1 (MemoryPoolDelete, osStatus_t, osMemoryPoolId_t) +SVC0_3(MemoryPoolNew, osMemoryPoolId_t, uint32_t, uint32_t, const osMemoryPoolAttr_t *) +SVC0_1(MemoryPoolGetName, const char *, osMemoryPoolId_t) +SVC0_2(MemoryPoolAlloc, void *, osMemoryPoolId_t, uint32_t) +SVC0_2(MemoryPoolFree, osStatus_t, osMemoryPoolId_t, void *) +SVC0_1(MemoryPoolGetCapacity, uint32_t, osMemoryPoolId_t) +SVC0_1(MemoryPoolGetBlockSize, uint32_t, osMemoryPoolId_t) +SVC0_1(MemoryPoolGetCount, uint32_t, osMemoryPoolId_t) +SVC0_1(MemoryPoolGetSpace, uint32_t, osMemoryPoolId_t) +SVC0_1(MemoryPoolDelete, osStatus_t, osMemoryPoolId_t) /// Create and Initialize a Memory Pool object. /// \note API identical to osMemoryPoolNew diff --git a/CMSIS/RTOS2/RTX/Source/rtx_msgqueue.c b/CMSIS/RTOS2/RTX/Source/rtx_msgqueue.c index ae33f641f8..1fb981d4c3 100644 --- a/CMSIS/RTOS2/RTX/Source/rtx_msgqueue.c +++ b/CMSIS/RTOS2/RTX/Source/rtx_msgqueue.c @@ -225,16 +225,16 @@ void osRtxMessageQueuePostProcess (os_message_t *msg) { // ==== Service Calls ==== -SVC0_3M(MessageQueueNew, osMessageQueueId_t, uint32_t, uint32_t, const osMessageQueueAttr_t *) -SVC0_1 (MessageQueueGetName, const char *, osMessageQueueId_t) -SVC0_4 (MessageQueuePut, osStatus_t, osMessageQueueId_t, const void *, uint8_t, uint32_t) -SVC0_4 (MessageQueueGet, osStatus_t, osMessageQueueId_t, void *, uint8_t *, uint32_t) -SVC0_1 (MessageQueueGetCapacity, uint32_t, osMessageQueueId_t) -SVC0_1 (MessageQueueGetMsgSize, uint32_t, osMessageQueueId_t) -SVC0_1 (MessageQueueGetCount, uint32_t, osMessageQueueId_t) -SVC0_1 (MessageQueueGetSpace, uint32_t, osMessageQueueId_t) -SVC0_1 (MessageQueueReset, osStatus_t, osMessageQueueId_t) -SVC0_1 (MessageQueueDelete, osStatus_t, osMessageQueueId_t) +SVC0_3(MessageQueueNew, osMessageQueueId_t, uint32_t, uint32_t, const osMessageQueueAttr_t *) +SVC0_1(MessageQueueGetName, const char *, osMessageQueueId_t) +SVC0_4(MessageQueuePut, osStatus_t, osMessageQueueId_t, const void *, uint8_t, uint32_t) +SVC0_4(MessageQueueGet, osStatus_t, osMessageQueueId_t, void *, uint8_t *, uint32_t) +SVC0_1(MessageQueueGetCapacity, uint32_t, osMessageQueueId_t) +SVC0_1(MessageQueueGetMsgSize, uint32_t, osMessageQueueId_t) +SVC0_1(MessageQueueGetCount, uint32_t, osMessageQueueId_t) +SVC0_1(MessageQueueGetSpace, uint32_t, osMessageQueueId_t) +SVC0_1(MessageQueueReset, osStatus_t, osMessageQueueId_t) +SVC0_1(MessageQueueDelete, osStatus_t, osMessageQueueId_t) /// Create and Initialize a Message Queue object. /// \note API identical to osMessageQueueNew diff --git a/CMSIS/RTOS2/RTX/Source/rtx_mutex.c b/CMSIS/RTOS2/RTX/Source/rtx_mutex.c index 2151c0251d..4b04ff555c 100644 --- a/CMSIS/RTOS2/RTX/Source/rtx_mutex.c +++ b/CMSIS/RTOS2/RTX/Source/rtx_mutex.c @@ -65,12 +65,12 @@ void osRtxMutexOwnerRelease (os_mutex_t *mutex_list) { // ==== Service Calls ==== // Service Calls definitions -SVC0_1M(MutexNew, osMutexId_t, const osMutexAttr_t *) -SVC0_1 (MutexGetName, const char *, osMutexId_t) -SVC0_2 (MutexAcquire, osStatus_t, osMutexId_t, uint32_t) -SVC0_1 (MutexRelease, osStatus_t, osMutexId_t) -SVC0_1 (MutexGetOwner, osThreadId_t, osMutexId_t) -SVC0_1 (MutexDelete, osStatus_t, osMutexId_t) +SVC0_1(MutexNew, osMutexId_t, const osMutexAttr_t *) +SVC0_1(MutexGetName, const char *, osMutexId_t) +SVC0_2(MutexAcquire, osStatus_t, osMutexId_t, uint32_t) +SVC0_1(MutexRelease, osStatus_t, osMutexId_t) +SVC0_1(MutexGetOwner, osThreadId_t, osMutexId_t) +SVC0_1(MutexDelete, osStatus_t, osMutexId_t) /// Create and Initialize a Mutex object. /// \note API identical to osMutexNew diff --git a/CMSIS/RTOS2/RTX/Source/rtx_semaphore.c b/CMSIS/RTOS2/RTX/Source/rtx_semaphore.c index f0a3c40cdf..bdfbcc0ce7 100644 --- a/CMSIS/RTOS2/RTX/Source/rtx_semaphore.c +++ b/CMSIS/RTOS2/RTX/Source/rtx_semaphore.c @@ -122,12 +122,12 @@ void osRtxSemaphorePostProcess (os_semaphore_t *semaphore) { // ==== Service Calls ==== // Service Calls definitions -SVC0_3M(SemaphoreNew, osSemaphoreId_t, uint32_t, uint32_t, const osSemaphoreAttr_t *) -SVC0_1 (SemaphoreGetName, const char *, osSemaphoreId_t) -SVC0_2 (SemaphoreAcquire, osStatus_t, osSemaphoreId_t, uint32_t) -SVC0_1 (SemaphoreRelease, osStatus_t, osSemaphoreId_t) -SVC0_1 (SemaphoreGetCount, uint32_t, osSemaphoreId_t) -SVC0_1 (SemaphoreDelete, osStatus_t, osSemaphoreId_t) +SVC0_3(SemaphoreNew, osSemaphoreId_t, uint32_t, uint32_t, const osSemaphoreAttr_t *) +SVC0_1(SemaphoreGetName, const char *, osSemaphoreId_t) +SVC0_2(SemaphoreAcquire, osStatus_t, osSemaphoreId_t, uint32_t) +SVC0_1(SemaphoreRelease, osStatus_t, osSemaphoreId_t) +SVC0_1(SemaphoreGetCount, uint32_t, osSemaphoreId_t) +SVC0_1(SemaphoreDelete, osStatus_t, osSemaphoreId_t) /// Create and Initialize a Semaphore object. /// \note API identical to osSemaphoreNew diff --git a/CMSIS/RTOS2/RTX/Source/rtx_thread.c b/CMSIS/RTOS2/RTX/Source/rtx_thread.c index 26e4c38c16..9b45b9722d 100644 --- a/CMSIS/RTOS2/RTX/Source/rtx_thread.c +++ b/CMSIS/RTOS2/RTX/Source/rtx_thread.c @@ -542,7 +542,7 @@ void osRtxThreadPostProcess (os_thread_t *thread) { // ==== Service Calls ==== // Service Calls definitions -SVC0_3M(ThreadNew, osThreadId_t, osThreadFunc_t, void *, const osThreadAttr_t *) +SVC0_3 (ThreadNew, osThreadId_t, osThreadFunc_t, void *, const osThreadAttr_t *) SVC0_1 (ThreadGetName, const char *, osThreadId_t) SVC0_0 (ThreadGetId, osThreadId_t) SVC0_1 (ThreadGetState, osThreadState_t, osThreadId_t) diff --git a/CMSIS/RTOS2/RTX/Source/rtx_timer.c b/CMSIS/RTOS2/RTX/Source/rtx_timer.c index 90bd795fa7..73ddc53313 100644 --- a/CMSIS/RTOS2/RTX/Source/rtx_timer.c +++ b/CMSIS/RTOS2/RTX/Source/rtx_timer.c @@ -132,12 +132,12 @@ __WEAK void osRtxTimerThread (void *argument) { // ==== Service Calls ==== // Service Calls definitions -SVC0_4M(TimerNew, osTimerId_t, osTimerFunc_t, osTimerType_t, void *, const osTimerAttr_t *) -SVC0_1 (TimerGetName, const char *, osTimerId_t) -SVC0_2 (TimerStart, osStatus_t, osTimerId_t, uint32_t) -SVC0_1 (TimerStop, osStatus_t, osTimerId_t) -SVC0_1 (TimerIsRunning, uint32_t, osTimerId_t) -SVC0_1 (TimerDelete, osStatus_t, osTimerId_t) +SVC0_4(TimerNew, osTimerId_t, osTimerFunc_t, osTimerType_t, void *, const osTimerAttr_t *) +SVC0_1(TimerGetName, const char *, osTimerId_t) +SVC0_2(TimerStart, osStatus_t, osTimerId_t, uint32_t) +SVC0_1(TimerStop, osStatus_t, osTimerId_t) +SVC0_1(TimerIsRunning, uint32_t, osTimerId_t) +SVC0_1(TimerDelete, osStatus_t, osTimerId_t) /// Create and Initialize a timer. /// \note API identical to osTimerNew