ARMmbed
/
mbed-os
mirror of https://github.com/ARMmbed/mbed-os.git
1
0
Fork 0
Code Issues Projects Releases Wiki Activity

RTX5: Cortex-A exception handlers updated (VFP register count detection)

pull/5628/head
Robert Rostohar 2017-10-27 10:44:59 +02:00 committed by TomoYamanaka
parent 4f4b4ddf36
commit 461c215636
3 changed files with 89 additions and 93 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

27
rtos/TARGET_CORTEX/rtx5/RTX/Source/TOOLCHAIN_ARM/TARGET_CORTEX_A/irq_ca.S
View File

@ -384,19 +384,18 @@ osRtxContextSave
VMRS R2, FPSCR VMRS R2, FPSCR
STMDB R3!, {R2,R12} ; Push FPSCR, maintain 8-byte alignment STMDB R3!, {R2,R12} ; Push FPSCR, maintain 8-byte alignment
IF {TARGET_FEATURE_EXTENSION_REGISTER_COUNT} == 16
VSTMDB R3!, {D0-D15} VSTMDB R3!, {D0-D15} ; Save 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 IF {TARGET_FEATURE_EXTENSION_REGISTER_COUNT} == 32
VSTMDB R3!, {D0-D15} VSTMDB R3!, {D16-D31} ; Save D16-D31
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 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 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
@ -414,11 +413,11 @@ osRtxContextRestore
BEQ osRtxContextRestore1 ; No VFP BEQ osRtxContextRestore1 ; No VFP
ISB ; Only sync if we enabled VFP, otherwise we will context switch before next VFP instruction anyway ISB ; Only sync if we enabled VFP, otherwise we will context switch before next VFP instruction anyway
IF {TARGET_FEATURE_EXTENSION_REGISTER_COUNT} == 32 IF {TARGET_FEATURE_EXTENSION_REGISTER_COUNT} == 32
VLDMIA R3!, {D16-D31} VLDMIA R3!, {D16-D31} ; Restore D16-D31
ENDIF ENDIF
VLDMIA R3!, {D0-D15} VLDMIA R3!, {D0-D15} ; Restore D0-D15
LDR R2, [R3] LDR R2, [R3]
VMSR FPSCR, R2 VMSR FPSCR, R2 ; Restore FPSCR
ADD R3, R3, #8 ADD R3, R3, #8
osRtxContextRestore1 osRtxContextRestore1

119
rtos/TARGET_CORTEX/rtx5/RTX/Source/TOOLCHAIN_GCC/TARGET_CORTEX_A/irq_ca.S
View File

@ -361,93 +361,92 @@ osRtxContextSwitch:
// R0 = osRtxInfo.thread.run.curr // R0 = osRtxInfo.thread.run.curr
// R1 = osRtxInfo.thread.run.next // R1 = osRtxInfo.thread.run.next
// R12 = &osRtxInfo.thread.run // R12 = &osRtxInfo.thread.run
CMP R0, #0 // Is osRtxInfo.thread.run.curr == 0 CMP R0, #0 // Is osRtxInfo.thread.run.curr == 0
ADDEQ SP, SP, #32 // Equal, curr deleted, adjust current SP ADDEQ SP, SP, #32 // Equal, curr deleted, adjust current SP
BEQ osRtxContextRestore // Restore context, run.curr = run.next; BEQ osRtxContextRestore // Restore context, run.curr = run.next;
osRtxContextSave: osRtxContextSave:
SUB SP, SP, #4 SUB SP, SP, #4
STM SP, {SP}^ // Save SP_usr to current stack STM SP, {SP}^ // Save SP_usr to current stack
POP {R3} // Pop SP_usr into R3 POP {R3} // Pop SP_usr into R3
SUB R3, R3, #64 // Adjust user sp to end of basic frame (R4) SUB R3, R3, #64 // Adjust user sp to end of basic frame (R4)
STMIA R3!, {R4-R11} // Save R4-R11 to user STMIA R3!, {R4-R11} // Save R4-R11 to user
POP {R4-R8} // Pop current R0-R12 into R4-R8 POP {R4-R8} // Pop current R0-R12 into R4-R8
STMIA R3!, {R4-R8} // Store them to user stack STMIA R3!, {R4-R8} // Store them to user stack
STM R3, {LR}^ // Store LR_usr directly STM R3, {LR}^ // Store LR_usr directly
ADD R3, R3, #4 // Adjust user sp to PC ADD R3, R3, #4 // Adjust user sp to PC
POP {R4-R6} // Pop current LR, PC, CPSR POP {R4-R6} // Pop current LR, PC, CPSR
STMIA R3!, {R5-R6} // Restore user PC and 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 // 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 AND R2, R2, #0x00F00000
CMP R2, #0x00F00000 CMP R2, #0x00F00000
BNE osRtxContextSave1 // Continue, no VFP BNE osRtxContextSave1 // Continue, no VFP
VMRS R2, FPSCR VMRS R2, FPSCR
STMDB R3!, {R2,R12} // Push FPSCR, maintain 8-byte alignment STMDB R3!, {R2,R12} // Push FPSCR, maintain 8-byte alignment
#if TARGET_FEATURE_EXTENSION_REGISTER_COUNT == 16
VSTMDB R3!, {D0-D15} VSTMDB R3!, {D0-D15} // Save D0-D15
LDRB R2, [R0, #TCB_SP_FRAME] // Record in TCB that VFP/D16 state is stacked #if __ARM_NEON == 1
ORR R2, R2, #2 VSTMDB R3!, {D16-D31} // Save D16-D31
STRB R2, [R0, #TCB_SP_FRAME]
#endif #endif
#if TARGET_FEATURE_EXTENSION_REGISTER_COUNT == 32 LDRB R2, [R0, #TCB_SP_FRAME]
VSTMDB R3!, {D0-D15} #if __ARM_NEON == 1
VSTMDB R3!, {D16-D31} ORR R2, R2, #4 // NEON state
LDRB R2, [R0, #TCB_SP_FRAME] // Record in TCB that NEON/D32 state is stacked #else
ORR R2, R2, #4 ORR R2, R2, #2 // VFP state
STRB R2, [R0, #TCB_SP_FRAME]
#endif #endif
STRB R2, [R0, #TCB_SP_FRAME] // Record VFP/NEON state
osRtxContextSave1: 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: osRtxContextRestore:
STR R1, [R12] // Store run.next to run.curr STR R1, [R12] // Store run.next to run.curr
LDR R3, [R1, #TCB_SP_OFS] // Load next osRtxThread_t.sp LDR R3, [R1, #TCB_SP_OFS] // Load next osRtxThread_t.sp
LDRB R2, [R1, #TCB_SP_FRAME] // Load next osRtxThread_t.stack_frame LDRB R2, [R1, #TCB_SP_FRAME] // Load next osRtxThread_t.stack_frame
ANDS R2, R2, #0x6 // Check stack frame for VFP context ANDS R2, R2, #0x6 // Check stack frame for VFP context
MRC p15, 0, R2, c1, c0, 2 // Read CPACR 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 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 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 MCR p15, 0, R2, c1, c0, 2 // Write CPACR
BEQ osRtxContextRestore1 // No VFP BEQ osRtxContextRestore1 // No VFP
ISB // Only sync if we enabled VFP, otherwise we will context switch before next VFP instruction anyway ISB // Only sync if we enabled VFP, otherwise we will context switch before next VFP instruction anyway
#if TARGET_FEATURE_EXTENSION_REGISTER_COUNT == 32 #if __ARM_NEON == 1
VLDMIA R3!, {D16-D31} VLDMIA R3!, {D16-D31} // Restore D16-D31
#endif #endif
VLDMIA R3!, {D0-D15} VLDMIA R3!, {D0-D15} // Restore D0-D15
LDR R2, [R3] LDR R2, [R3]
VMSR FPSCR, R2 VMSR FPSCR, R2 // Restore FPSCR
ADD R3, R3, #8 ADD R3, R3, #8
osRtxContextRestore1: osRtxContextRestore1:
LDMIA R3!, {R4-R11} // Restore R4-R11 LDMIA R3!, {R4-R11} // Restore R4-R11
MOV R12, R3 // Move sp pointer to R12 MOV R12, R3 // Move sp pointer to R12
ADD R3, R3, #32 // Adjust sp ADD R3, R3, #32 // Adjust sp
PUSH {R3} // Push sp onto stack PUSH {R3} // Push sp onto stack
LDMIA SP, {SP}^ // Restore SP_usr LDMIA SP, {SP}^ // Restore SP_usr
ADD SP, SP, #4 // Adjust SP_svc ADD SP, SP, #4 // Adjust SP_svc
LDMIA R12!, {R0-R3} // Restore User R0-R3 LDMIA R12!, {R0-R3} // Restore User R0-R3
LDR LR, [R12, #12] // Load SPSR into LR LDR LR, [R12, #12] // Load SPSR into LR
MSR SPSR_cxsf, LR // Restore SPSR MSR SPSR_cxsf, LR // Restore SPSR
ADD R12, R12, #4 // Adjust pointer to LR ADD R12, R12, #4 // Adjust pointer to LR
LDM R12, {LR}^ // Restore LR_usr directly into LR LDM R12, {LR}^ // Restore LR_usr directly into LR
LDR LR, [R12, #4] // Restore LR LDR LR, [R12, #4] // Restore LR
LDR R12, [R12, #-4] // Restore R12 LDR R12, [R12, #-4] // Restore R12
MOVS PC, LR // Return from exception MOVS PC, LR // Return from exception
osRtxContextExit: osRtxContextExit:
POP {R0-R3, R12, LR} // Restore stacked APCS registers POP {R0-R3, R12, LR} // Restore stacked APCS registers
RFEFD SP! // Return from exception RFEFD SP! // Return from exception
.fnend .fnend
.size osRtxContextSwitch, .-osRtxContextSwitch .size osRtxContextSwitch, .-osRtxContextSwitch

36
rtos/TARGET_CORTEX/rtx5/RTX/Source/TOOLCHAIN_IAR/TARGET_CORTEX_A/irq_ca.S
View File

@ -42,7 +42,6 @@ TCB_SP_OFS EQU 56 ; osRtxThread_t.sp offset
PRESERVE8 PRESERVE8
ARM
SECTION .rodata:DATA:NOROOT(2) SECTION .rodata:DATA:NOROOT(2)
@ -370,19 +369,18 @@ osRtxContextSave
VMRS R2, FPSCR VMRS R2, FPSCR
STMDB R3!, {R2,R12} ; Push FPSCR, maintain 8-byte alignment STMDB R3!, {R2,R12} ; Push FPSCR, maintain 8-byte alignment
IF {TARGET_FEATURE_EXTENSION_REGISTER_COUNT} == 16
VSTMDB R3!, {D0-D15} VSTMDB R3!, {D0-D15} ; Save D0-D15
LDRB R2, [R0, #TCB_SP_FRAME] ; Record in TCB that VFP/D16 state is stacked #ifdef __ARM_ADVANCED_SIMD__
ORR R2, R2, #2 VSTMDB R3!, {D16-D31} ; Save D16-D31
STRB R2, [R0, #TCB_SP_FRAME] #endif
ENDIF LDRB R2, [R0, #TCB_SP_FRAME]
IF {TARGET_FEATURE_EXTENSION_REGISTER_COUNT} == 32 #ifdef __ARM_ADVANCED_SIMD__
VSTMDB R3!, {D0-D15} ORR R2, R2, #4 ; NEON state
VSTMDB R3!, {D16-D31} #else
LDRB R2, [R0, #TCB_SP_FRAME] ; Record in TCB that NEON/D32 state is stacked ORR R2, R2, #2 ; VFP state
ORR R2, R2, #4 #endif
STRB R2, [R0, #TCB_SP_FRAME] STRB R2, [R0, #TCB_SP_FRAME] ; Record VFP/NEON state
ENDIF
osRtxContextSave1 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
@ -399,12 +397,12 @@ osRtxContextRestore
MCR p15, 0, R2, c1, c0, 2 ; Write CPACR MCR p15, 0, R2, c1, c0, 2 ; Write CPACR
BEQ osRtxContextRestore1 ; No VFP BEQ osRtxContextRestore1 ; No VFP
ISB ; Only sync if we enabled VFP, otherwise we will context switch before next VFP instruction anyway ISB ; Only sync if we enabled VFP, otherwise we will context switch before next VFP instruction anyway
IF {TARGET_FEATURE_EXTENSION_REGISTER_COUNT} == 32 #ifdef __ARM_ADVANCED_SIMD__
VLDMIA R3!, {D16-D31} VLDMIA R3!, {D16-D31} ; Restore D16-D31
ENDIF #endif
VLDMIA R3!, {D0-D15} VLDMIA R3!, {D0-D15} ; Restore D0-D15
LDR R2, [R3] LDR R2, [R3]
VMSR FPSCR, R2 VMSR FPSCR, R2 ; Restore FPSCR
ADD R3, R3, #8 ADD R3, R3, #8
osRtxContextRestore1 osRtxContextRestore1