From 461c215636acf3e1c6a1e8974deb21dc6f59c315 Mon Sep 17 00:00:00 2001 From: Robert Rostohar Date: Fri, 27 Oct 2017 10:44:59 +0200 Subject: [PATCH] RTX5: Cortex-A exception handlers updated (VFP register count detection) --- .../TOOLCHAIN_ARM/TARGET_CORTEX_A/irq_ca.S | 27 ++-- .../TOOLCHAIN_GCC/TARGET_CORTEX_A/irq_ca.S | 119 +++++++++--------- .../TOOLCHAIN_IAR/TARGET_CORTEX_A/irq_ca.S | 36 +++--- 3 files changed, 89 insertions(+), 93 deletions(-) diff --git a/rtos/TARGET_CORTEX/rtx5/RTX/Source/TOOLCHAIN_ARM/TARGET_CORTEX_A/irq_ca.S b/rtos/TARGET_CORTEX/rtx5/RTX/Source/TOOLCHAIN_ARM/TARGET_CORTEX_A/irq_ca.S index 7963fdefc0..97b43d89bb 100644 --- a/rtos/TARGET_CORTEX/rtx5/RTX/Source/TOOLCHAIN_ARM/TARGET_CORTEX_A/irq_ca.S +++ b/rtos/TARGET_CORTEX/rtx5/RTX/Source/TOOLCHAIN_ARM/TARGET_CORTEX_A/irq_ca.S @@ -384,19 +384,18 @@ osRtxContextSave VMRS R2, FPSCR STMDB R3!, {R2,R12} ; Push FPSCR, maintain 8-byte alignment - IF {TARGET_FEATURE_EXTENSION_REGISTER_COUNT} == 16 - VSTMDB R3!, {D0-D15} - LDRB R2, [R0, #TCB_SP_FRAME] ; Record in TCB that VFP/D16 state is stacked - ORR R2, R2, #2 - STRB R2, [R0, #TCB_SP_FRAME] - ENDIF + + VSTMDB R3!, {D0-D15} ; Save D0-D15 IF {TARGET_FEATURE_EXTENSION_REGISTER_COUNT} == 32 - VSTMDB R3!, {D0-D15} - VSTMDB R3!, {D16-D31} - LDRB R2, [R0, #TCB_SP_FRAME] ; Record in TCB that NEON/D32 state is stacked - ORR R2, R2, #4 - STRB R2, [R0, #TCB_SP_FRAME] + VSTMDB R3!, {D16-D31} ; Save D16-D31 ENDIF + LDRB R2, [R0, #TCB_SP_FRAME] + IF {TARGET_FEATURE_EXTENSION_REGISTER_COUNT} == 32 + ORR R2, R2, #4 ; NEON state + ELSE + ORR R2, R2, #2 ; VFP state + ENDIF + STRB R2, [R0, #TCB_SP_FRAME] ; Record VFP/NEON state osRtxContextSave1 STR R3, [R0, #TCB_SP_OFS] ; Store user sp to osRtxInfo.thread.run.curr @@ -414,11 +413,11 @@ osRtxContextRestore BEQ osRtxContextRestore1 ; No VFP ISB ; Only sync if we enabled VFP, otherwise we will context switch before next VFP instruction anyway IF {TARGET_FEATURE_EXTENSION_REGISTER_COUNT} == 32 - VLDMIA R3!, {D16-D31} + VLDMIA R3!, {D16-D31} ; Restore D16-D31 ENDIF - VLDMIA R3!, {D0-D15} + VLDMIA R3!, {D0-D15} ; Restore D0-D15 LDR R2, [R3] - VMSR FPSCR, R2 + VMSR FPSCR, R2 ; Restore FPSCR ADD R3, R3, #8 osRtxContextRestore1 diff --git a/rtos/TARGET_CORTEX/rtx5/RTX/Source/TOOLCHAIN_GCC/TARGET_CORTEX_A/irq_ca.S b/rtos/TARGET_CORTEX/rtx5/RTX/Source/TOOLCHAIN_GCC/TARGET_CORTEX_A/irq_ca.S index aa40e0d364..07162747d2 100644 --- a/rtos/TARGET_CORTEX/rtx5/RTX/Source/TOOLCHAIN_GCC/TARGET_CORTEX_A/irq_ca.S +++ b/rtos/TARGET_CORTEX/rtx5/RTX/Source/TOOLCHAIN_GCC/TARGET_CORTEX_A/irq_ca.S @@ -361,93 +361,92 @@ osRtxContextSwitch: // R0 = osRtxInfo.thread.run.curr // R1 = osRtxInfo.thread.run.next - // R12 = &osRtxInfo.thread.run + // R12 = &osRtxInfo.thread.run - CMP R0, #0 // Is osRtxInfo.thread.run.curr == 0 - ADDEQ SP, SP, #32 // Equal, curr deleted, adjust current SP - BEQ osRtxContextRestore // Restore context, run.curr = run.next; + CMP R0, #0 // Is osRtxInfo.thread.run.curr == 0 + ADDEQ SP, SP, #32 // Equal, curr deleted, adjust current SP + BEQ osRtxContextRestore // Restore context, run.curr = run.next; osRtxContextSave: SUB SP, SP, #4 - STM SP, {SP}^ // Save SP_usr to current stack - POP {R3} // Pop SP_usr into R3 + STM SP, {SP}^ // Save SP_usr to current stack + POP {R3} // Pop SP_usr into R3 - SUB R3, R3, #64 // Adjust user sp to end of basic frame (R4) - STMIA R3!, {R4-R11} // Save R4-R11 to user - POP {R4-R8} // Pop current R0-R12 into R4-R8 - STMIA R3!, {R4-R8} // Store them to user stack - STM R3, {LR}^ // Store LR_usr directly - ADD R3, R3, #4 // Adjust user sp to PC - POP {R4-R6} // Pop current LR, PC, CPSR - STMIA R3!, {R5-R6} // Restore user PC and CPSR + SUB R3, R3, #64 // Adjust user sp to end of basic frame (R4) + STMIA R3!, {R4-R11} // Save R4-R11 to user + POP {R4-R8} // Pop current R0-R12 into R4-R8 + STMIA R3!, {R4-R8} // Store them to user stack + STM R3, {LR}^ // Store LR_usr directly + ADD R3, R3, #4 // Adjust user sp to PC + POP {R4-R6} // Pop current LR, PC, CPSR + STMIA R3!, {R5-R6} // Restore user PC and CPSR - SUB R3, R3, #64 // Adjust user sp to R4 + SUB R3, R3, #64 // Adjust user sp to R4 // Check if VFP state need to be saved - MRC p15, 0, R2, c1, c0, 2 // VFP/NEON access enabled? (CPACR) + MRC p15, 0, R2, c1, c0, 2 // VFP/NEON access enabled? (CPACR) AND R2, R2, #0x00F00000 CMP R2, #0x00F00000 - BNE osRtxContextSave1 // Continue, no VFP + BNE osRtxContextSave1 // Continue, no VFP VMRS R2, FPSCR - STMDB R3!, {R2,R12} // Push FPSCR, maintain 8-byte alignment - #if TARGET_FEATURE_EXTENSION_REGISTER_COUNT == 16 - VSTMDB R3!, {D0-D15} - LDRB R2, [R0, #TCB_SP_FRAME] // Record in TCB that VFP/D16 state is stacked - ORR R2, R2, #2 - STRB R2, [R0, #TCB_SP_FRAME] + STMDB R3!, {R2,R12} // Push FPSCR, maintain 8-byte alignment + + VSTMDB R3!, {D0-D15} // Save D0-D15 + #if __ARM_NEON == 1 + VSTMDB R3!, {D16-D31} // Save D16-D31 #endif - #if TARGET_FEATURE_EXTENSION_REGISTER_COUNT == 32 - VSTMDB R3!, {D0-D15} - VSTMDB R3!, {D16-D31} - LDRB R2, [R0, #TCB_SP_FRAME] // Record in TCB that NEON/D32 state is stacked - ORR R2, R2, #4 - STRB R2, [R0, #TCB_SP_FRAME] + LDRB R2, [R0, #TCB_SP_FRAME] + #if __ARM_NEON == 1 + ORR R2, R2, #4 // NEON state + #else + ORR R2, R2, #2 // VFP state #endif + STRB R2, [R0, #TCB_SP_FRAME] // Record VFP/NEON state osRtxContextSave1: - STR R3, [R0, #TCB_SP_OFS] // Store user sp to osRtxInfo.thread.run.curr + STR R3, [R0, #TCB_SP_OFS] // Store user sp to osRtxInfo.thread.run.curr osRtxContextRestore: - STR R1, [R12] // Store run.next to run.curr - LDR R3, [R1, #TCB_SP_OFS] // Load next osRtxThread_t.sp - LDRB R2, [R1, #TCB_SP_FRAME] // Load next osRtxThread_t.stack_frame + STR R1, [R12] // Store run.next to run.curr + LDR R3, [R1, #TCB_SP_OFS] // Load next osRtxThread_t.sp + LDRB R2, [R1, #TCB_SP_FRAME] // Load next osRtxThread_t.stack_frame - ANDS R2, R2, #0x6 // Check stack frame for VFP context - MRC p15, 0, R2, c1, c0, 2 // Read CPACR - ANDEQ R2, R2, #0xFF0FFFFF // Disable VFP/NEON access if incoming task does not have stacked VFP/NEON state - ORRNE R2, R2, #0x00F00000 // Enable VFP/NEON access if incoming task does have stacked VFP/NEON state - MCR p15, 0, R2, c1, c0, 2 // Write CPACR - BEQ osRtxContextRestore1 // No VFP - ISB // Only sync if we enabled VFP, otherwise we will context switch before next VFP instruction anyway - #if TARGET_FEATURE_EXTENSION_REGISTER_COUNT == 32 - VLDMIA R3!, {D16-D31} + ANDS R2, R2, #0x6 // Check stack frame for VFP context + MRC p15, 0, R2, c1, c0, 2 // Read CPACR + ANDEQ R2, R2, #0xFF0FFFFF // Disable VFP/NEON access if incoming task does not have stacked VFP/NEON state + ORRNE R2, R2, #0x00F00000 // Enable VFP/NEON access if incoming task does have stacked VFP/NEON state + MCR p15, 0, R2, c1, c0, 2 // Write CPACR + BEQ osRtxContextRestore1 // No VFP + ISB // Only sync if we enabled VFP, otherwise we will context switch before next VFP instruction anyway + #if __ARM_NEON == 1 + VLDMIA R3!, {D16-D31} // Restore D16-D31 #endif - VLDMIA R3!, {D0-D15} + VLDMIA R3!, {D0-D15} // Restore D0-D15 LDR R2, [R3] - VMSR FPSCR, R2 + VMSR FPSCR, R2 // Restore FPSCR ADD R3, R3, #8 osRtxContextRestore1: - LDMIA R3!, {R4-R11} // Restore R4-R11 - MOV R12, R3 // Move sp pointer to R12 - ADD R3, R3, #32 // Adjust sp - PUSH {R3} // Push sp onto stack - LDMIA SP, {SP}^ // Restore SP_usr - ADD SP, SP, #4 // Adjust SP_svc - LDMIA R12!, {R0-R3} // Restore User R0-R3 - LDR LR, [R12, #12] // Load SPSR into LR - MSR SPSR_cxsf, LR // Restore SPSR - ADD R12, R12, #4 // Adjust pointer to LR - LDM R12, {LR}^ // Restore LR_usr directly into LR - LDR LR, [R12, #4] // Restore LR - LDR R12, [R12, #-4] // Restore R12 + LDMIA R3!, {R4-R11} // Restore R4-R11 + MOV R12, R3 // Move sp pointer to R12 + ADD R3, R3, #32 // Adjust sp + PUSH {R3} // Push sp onto stack + LDMIA SP, {SP}^ // Restore SP_usr + ADD SP, SP, #4 // Adjust SP_svc + LDMIA R12!, {R0-R3} // Restore User R0-R3 + LDR LR, [R12, #12] // Load SPSR into LR + MSR SPSR_cxsf, LR // Restore SPSR + ADD R12, R12, #4 // Adjust pointer to LR + LDM R12, {LR}^ // Restore LR_usr directly into LR + LDR LR, [R12, #4] // Restore LR + LDR R12, [R12, #-4] // Restore R12 - MOVS PC, LR // Return from exception + MOVS PC, LR // Return from exception osRtxContextExit: - POP {R0-R3, R12, LR} // Restore stacked APCS registers - RFEFD SP! // Return from exception + POP {R0-R3, R12, LR} // Restore stacked APCS registers + RFEFD SP! // Return from exception .fnend .size osRtxContextSwitch, .-osRtxContextSwitch diff --git a/rtos/TARGET_CORTEX/rtx5/RTX/Source/TOOLCHAIN_IAR/TARGET_CORTEX_A/irq_ca.S b/rtos/TARGET_CORTEX/rtx5/RTX/Source/TOOLCHAIN_IAR/TARGET_CORTEX_A/irq_ca.S index acb2b12344..159214520f 100644 --- a/rtos/TARGET_CORTEX/rtx5/RTX/Source/TOOLCHAIN_IAR/TARGET_CORTEX_A/irq_ca.S +++ b/rtos/TARGET_CORTEX/rtx5/RTX/Source/TOOLCHAIN_IAR/TARGET_CORTEX_A/irq_ca.S @@ -42,7 +42,6 @@ TCB_SP_OFS EQU 56 ; osRtxThread_t.sp offset PRESERVE8 - ARM SECTION .rodata:DATA:NOROOT(2) @@ -370,19 +369,18 @@ osRtxContextSave VMRS R2, FPSCR STMDB R3!, {R2,R12} ; Push FPSCR, maintain 8-byte alignment - IF {TARGET_FEATURE_EXTENSION_REGISTER_COUNT} == 16 - VSTMDB R3!, {D0-D15} - LDRB R2, [R0, #TCB_SP_FRAME] ; Record in TCB that VFP/D16 state is stacked - ORR R2, R2, #2 - STRB R2, [R0, #TCB_SP_FRAME] - ENDIF - IF {TARGET_FEATURE_EXTENSION_REGISTER_COUNT} == 32 - VSTMDB R3!, {D0-D15} - VSTMDB R3!, {D16-D31} - LDRB R2, [R0, #TCB_SP_FRAME] ; Record in TCB that NEON/D32 state is stacked - ORR R2, R2, #4 - STRB R2, [R0, #TCB_SP_FRAME] - ENDIF + + VSTMDB R3!, {D0-D15} ; Save D0-D15 + #ifdef __ARM_ADVANCED_SIMD__ + VSTMDB R3!, {D16-D31} ; Save D16-D31 + #endif + LDRB R2, [R0, #TCB_SP_FRAME] + #ifdef __ARM_ADVANCED_SIMD__ + ORR R2, R2, #4 ; NEON state + #else + ORR R2, R2, #2 ; VFP state + #endif + STRB R2, [R0, #TCB_SP_FRAME] ; Record VFP/NEON state osRtxContextSave1 STR R3, [R0, #TCB_SP_OFS] ; Store user sp to osRtxInfo.thread.run.curr @@ -399,12 +397,12 @@ osRtxContextRestore MCR p15, 0, R2, c1, c0, 2 ; Write CPACR BEQ osRtxContextRestore1 ; No VFP ISB ; Only sync if we enabled VFP, otherwise we will context switch before next VFP instruction anyway - IF {TARGET_FEATURE_EXTENSION_REGISTER_COUNT} == 32 - VLDMIA R3!, {D16-D31} - ENDIF - VLDMIA R3!, {D0-D15} + #ifdef __ARM_ADVANCED_SIMD__ + VLDMIA R3!, {D16-D31} ; Restore D16-D31 + #endif + VLDMIA R3!, {D0-D15} ; Restore D0-D15 LDR R2, [R3] - VMSR FPSCR, R2 + VMSR FPSCR, R2 ; Restore FPSCR ADD R3, R3, #8 osRtxContextRestore1