Skip to content

Commit 668883a

Browse files
RobertRostoharadbridge
RobertRostohar
authored and
adbridge
committed
RTX5: Cortex-A exception handlers updated (VFP register count detection)
1 parent bbb6372 commit 668883a

File tree

3 files changed

+90
-94
lines changed
  • rtos/TARGET_CORTEX/rtx5/RTX/Source

3 files changed

+90
-94
lines changed

‎rtos/TARGET_CORTEX/rtx5/RTX/Source/TOOLCHAIN_ARM/TARGET_CORTEX_A/irq_ca.S

Lines changed: 12 additions & 13 deletions
Original file line numberDiff line numberDiff line change
@@ -384,19 +384,18 @@ osRtxContextSave
384384

385385
VMRS R2, FPSCR
386386
STMDB R3!, {R2,R12} ; Push FPSCR, maintain 8-byte alignment
387-
IF {TARGET_FEATURE_EXTENSION_REGISTER_COUNT} == 16
388-
VSTMDB R3!, {D0-D15}
389-
LDRB R2, [R0, #TCB_SP_FRAME] ; Record in TCB that VFP/D16 state is stacked
390-
ORR R2, R2, #2
391-
STRB R2, [R0, #TCB_SP_FRAME]
387+
388+
VSTMDB R3!, {D0-D15} ; Save D0-D15
389+
IF {TARGET_FEATURE_EXTENSION_REGISTER_COUNT} == 32
390+
VSTMDB R3!, {D16-D31} ; Save D16-D31
392391
ENDIF
392+
LDRB R2, [R0, #TCB_SP_FRAME]
393393
IF {TARGET_FEATURE_EXTENSION_REGISTER_COUNT} == 32
394-
VSTMDB R3!, {D0-D15}
395-
VSTMDB R3!, {D16-D31}
396-
LDRB R2, [R0, #TCB_SP_FRAME] ; Record in TCB that NEON/D32 state is stacked
397-
ORR R2, R2, #4
398-
STRB R2, [R0, #TCB_SP_FRAME]
394+
ORR R2, R2, #4 ; NEON state
395+
ELSE
396+
ORR R2, R2, #2 ; VFP state
399397
ENDIF
398+
STRB R2, [R0, #TCB_SP_FRAME] ; Record VFP/NEON state
400399

401400
osRtxContextSave1
402401
STR R3, [R0, #TCB_SP_OFS] ; Store user sp to osRtxInfo.thread.run.curr
@@ -414,11 +413,11 @@ osRtxContextRestore
414413
BEQ osRtxContextRestore1 ; No VFP
415414
ISB ; Only sync if we enabled VFP, otherwise we will context switch before next VFP instruction anyway
416415
IF {TARGET_FEATURE_EXTENSION_REGISTER_COUNT} == 32
417-
VLDMIA R3!, {D16-D31}
416+
VLDMIA R3!, {D16-D31} ; Restore D16-D31
418417
ENDIF
419-
VLDMIA R3!, {D0-D15}
418+
VLDMIA R3!, {D0-D15} ; Restore D0-D15
420419
LDR R2, [R3]
421-
VMSR FPSCR, R2
420+
VMSR FPSCR, R2 ; Restore FPSCR
422421
ADD R3, R3, #8
423422

424423
osRtxContextRestore1

‎rtos/TARGET_CORTEX/rtx5/RTX/Source/TOOLCHAIN_GCC/TARGET_CORTEX_A/irq_ca.S

Lines changed: 61 additions & 62 deletions
Original file line numberDiff line numberDiff line change
@@ -361,93 +361,92 @@ osRtxContextSwitch:
361361

362362
// R0 = osRtxInfo.thread.run.curr
363363
// R1 = osRtxInfo.thread.run.next
364-
// R12 = &osRtxInfo.thread.run
364+
// R12 = &osRtxInfo.thread.run
365365

366-
CMP R0, #0 // Is osRtxInfo.thread.run.curr == 0
367-
ADDEQ SP, SP, #32 // Equal, curr deleted, adjust current SP
368-
BEQ osRtxContextRestore // Restore context, run.curr = run.next;
366+
CMP R0, #0 // Is osRtxInfo.thread.run.curr == 0
367+
ADDEQ SP, SP, #32 // Equal, curr deleted, adjust current SP
368+
BEQ osRtxContextRestore // Restore context, run.curr = run.next;
369369

370370
osRtxContextSave:
371371
SUB SP, SP, #4
372-
STM SP, {SP}^ // Save SP_usr to current stack
373-
POP {R3} // Pop SP_usr into R3
372+
STM SP, {SP}^ // Save SP_usr to current stack
373+
POP {R3} // Pop SP_usr into R3
374374

375-
SUB R3, R3, #64 // Adjust user sp to end of basic frame (R4)
376-
STMIA R3!, {R4-R11} // Save R4-R11 to user
377-
POP {R4-R8} // Pop current R0-R12 into R4-R8
378-
STMIA R3!, {R4-R8} // Store them to user stack
379-
STM R3, {LR}^ // Store LR_usr directly
380-
ADD R3, R3, #4 // Adjust user sp to PC
381-
POP {R4-R6} // Pop current LR, PC, CPSR
382-
STMIA R3!, {R5-R6} // Restore user PC and CPSR
375+
SUB R3, R3, #64 // Adjust user sp to end of basic frame (R4)
376+
STMIA R3!, {R4-R11} // Save R4-R11 to user
377+
POP {R4-R8} // Pop current R0-R12 into R4-R8
378+
STMIA R3!, {R4-R8} // Store them to user stack
379+
STM R3, {LR}^ // Store LR_usr directly
380+
ADD R3, R3, #4 // Adjust user sp to PC
381+
POP {R4-R6} // Pop current LR, PC, CPSR
382+
STMIA R3!, {R5-R6} // Restore user PC and CPSR
383383

384-
SUB R3, R3, #64 // Adjust user sp to R4
384+
SUB R3, R3, #64 // Adjust user sp to R4
385385

386386
// Check if VFP state need to be saved
387-
MRC p15, 0, R2, c1, c0, 2 // VFP/NEON access enabled? (CPACR)
387+
MRC p15, 0, R2, c1, c0, 2 // VFP/NEON access enabled? (CPACR)
388388
AND R2, R2, #0x00F00000
389389
CMP R2, #0x00F00000
390-
BNE osRtxContextSave1 // Continue, no VFP
390+
BNE osRtxContextSave1 // Continue, no VFP
391391

392392
VMRS R2, FPSCR
393-
STMDB R3!, {R2,R12} // Push FPSCR, maintain 8-byte alignment
394-
#if TARGET_FEATURE_EXTENSION_REGISTER_COUNT == 16
395-
VSTMDB R3!, {D0-D15}
396-
LDRB R2, [R0, #TCB_SP_FRAME] // Record in TCB that VFP/D16 state is stacked
397-
ORR R2, R2, #2
398-
STRB R2, [R0, #TCB_SP_FRAME]
393+
STMDB R3!, {R2,R12} // Push FPSCR, maintain 8-byte alignment
394+
395+
VSTMDB R3!, {D0-D15} // Save D0-D15
396+
#if __ARM_NEON == 1
397+
VSTMDB R3!, {D16-D31} // Save D16-D31
399398
#endif
400-
#if TARGET_FEATURE_EXTENSION_REGISTER_COUNT == 32
401-
VSTMDB R3!, {D0-D15}
402-
VSTMDB R3!, {D16-D31}
403-
LDRB R2, [R0, #TCB_SP_FRAME] // Record in TCB that NEON/D32 state is stacked
404-
ORR R2, R2, #4
405-
STRB R2, [R0, #TCB_SP_FRAME]
399+
LDRB R2, [R0, #TCB_SP_FRAME]
400+
#if __ARM_NEON == 1
401+
ORR R2, R2, #4 // NEON state
402+
#else
403+
ORR R2, R2, #2 // VFP state
406404
#endif
405+
STRB R2, [R0, #TCB_SP_FRAME] // Record VFP/NEON state
407406

408407
osRtxContextSave1:
409-
STR R3, [R0, #TCB_SP_OFS] // Store user sp to osRtxInfo.thread.run.curr
408+
STR R3, [R0, #TCB_SP_OFS] // Store user sp to osRtxInfo.thread.run.curr
410409

411410
osRtxContextRestore:
412-
STR R1, [R12] // Store run.next to run.curr
413-
LDR R3, [R1, #TCB_SP_OFS] // Load next osRtxThread_t.sp
414-
LDRB R2, [R1, #TCB_SP_FRAME] // Load next osRtxThread_t.stack_frame
415-
416-
ANDS R2, R2, #0x6 // Check stack frame for VFP context
417-
MRC p15, 0, R2, c1, c0, 2 // Read CPACR
418-
ANDEQ R2, R2, #0xFF0FFFFF // Disable VFP/NEON access if incoming task does not have stacked VFP/NEON state
419-
ORRNE R2, R2, #0x00F00000 // Enable VFP/NEON access if incoming task does have stacked VFP/NEON state
420-
MCR p15, 0, R2, c1, c0, 2 // Write CPACR
421-
BEQ osRtxContextRestore1 // No VFP
422-
ISB // Only sync if we enabled VFP, otherwise we will context switch before next VFP instruction anyway
423-
#if TARGET_FEATURE_EXTENSION_REGISTER_COUNT == 32
424-
VLDMIA R3!, {D16-D31}
411+
STR R1, [R12] // Store run.next to run.curr
412+
LDR R3, [R1, #TCB_SP_OFS] // Load next osRtxThread_t.sp
413+
LDRB R2, [R1, #TCB_SP_FRAME] // Load next osRtxThread_t.stack_frame
414+
415+
ANDS R2, R2, #0x6 // Check stack frame for VFP context
416+
MRC p15, 0, R2, c1, c0, 2 // Read CPACR
417+
ANDEQ R2, R2, #0xFF0FFFFF // Disable VFP/NEON access if incoming task does not have stacked VFP/NEON state
418+
ORRNE R2, R2, #0x00F00000 // Enable VFP/NEON access if incoming task does have stacked VFP/NEON state
419+
MCR p15, 0, R2, c1, c0, 2 // Write CPACR
420+
BEQ osRtxContextRestore1 // No VFP
421+
ISB // Only sync if we enabled VFP, otherwise we will context switch before next VFP instruction anyway
422+
#if __ARM_NEON == 1
423+
VLDMIA R3!, {D16-D31} // Restore D16-D31
425424
#endif
426-
VLDMIA R3!, {D0-D15}
425+
VLDMIA R3!, {D0-D15} // Restore D0-D15
427426
LDR R2, [R3]
428-
VMSR FPSCR, R2
427+
VMSR FPSCR, R2 // Restore FPSCR
429428
ADD R3, R3, #8
430429

431430
osRtxContextRestore1:
432-
LDMIA R3!, {R4-R11} // Restore R4-R11
433-
MOV R12, R3 // Move sp pointer to R12
434-
ADD R3, R3, #32 // Adjust sp
435-
PUSH {R3} // Push sp onto stack
436-
LDMIA SP, {SP}^ // Restore SP_usr
437-
ADD SP, SP, #4 // Adjust SP_svc
438-
LDMIA R12!, {R0-R3} // Restore User R0-R3
439-
LDR LR, [R12, #12] // Load SPSR into LR
440-
MSR SPSR_cxsf, LR // Restore SPSR
441-
ADD R12, R12, #4 // Adjust pointer to LR
442-
LDM R12, {LR}^ // Restore LR_usr directly into LR
443-
LDR LR, [R12, #4] // Restore LR
444-
LDR R12, [R12, #-4] // Restore R12
445-
446-
MOVS PC, LR // Return from exception
431+
LDMIA R3!, {R4-R11} // Restore R4-R11
432+
MOV R12, R3 // Move sp pointer to R12
433+
ADD R3, R3, #32 // Adjust sp
434+
PUSH {R3} // Push sp onto stack
435+
LDMIA SP, {SP}^ // Restore SP_usr
436+
ADD SP, SP, #4 // Adjust SP_svc
437+
LDMIA R12!, {R0-R3} // Restore User R0-R3
438+
LDR LR, [R12, #12] // Load SPSR into LR
439+
MSR SPSR_cxsf, LR // Restore SPSR
440+
ADD R12, R12, #4 // Adjust pointer to LR
441+
LDM R12, {LR}^ // Restore LR_usr directly into LR
442+
LDR LR, [R12, #4] // Restore LR
443+
LDR R12, [R12, #-4] // Restore R12
444+
445+
MOVS PC, LR // Return from exception
447446

448447
osRtxContextExit:
449-
POP {R0-R3, R12, LR} // Restore stacked APCS registers
450-
RFEFD SP! // Return from exception
448+
POP {R0-R3, R12, LR} // Restore stacked APCS registers
449+
RFEFD SP! // Return from exception
451450

452451
.fnend
453452
.size osRtxContextSwitch, .-osRtxContextSwitch

‎rtos/TARGET_CORTEX/rtx5/RTX/Source/TOOLCHAIN_IAR/TARGET_CORTEX_A/irq_ca.S

Lines changed: 17 additions & 19 deletions
Original file line numberDiff line numberDiff line change
@@ -42,7 +42,6 @@ TCB_SP_OFS EQU 56 ; osRtxThread_t.sp offset
4242

4343

4444
PRESERVE8
45-
ARM
4645

4746

4847
SECTION .rodata:DATA:NOROOT(2)
@@ -370,19 +369,18 @@ osRtxContextSave
370369

371370
VMRS R2, FPSCR
372371
STMDB R3!, {R2,R12} ; Push FPSCR, maintain 8-byte alignment
373-
IF {TARGET_FEATURE_EXTENSION_REGISTER_COUNT} == 16
374-
VSTMDB R3!, {D0-D15}
375-
LDRB R2, [R0, #TCB_SP_FRAME] ; Record in TCB that VFP/D16 state is stacked
376-
ORR R2, R2, #2
377-
STRB R2, [R0, #TCB_SP_FRAME]
378-
ENDIF
379-
IF {TARGET_FEATURE_EXTENSION_REGISTER_COUNT} == 32
380-
VSTMDB R3!, {D0-D15}
381-
VSTMDB R3!, {D16-D31}
382-
LDRB R2, [R0, #TCB_SP_FRAME] ; Record in TCB that NEON/D32 state is stacked
383-
ORR R2, R2, #4
384-
STRB R2, [R0, #TCB_SP_FRAME]
385-
ENDIF
372+
373+
VSTMDB R3!, {D0-D15} ; Save D0-D15
374+
#ifdef __ARM_ADVANCED_SIMD__
375+
VSTMDB R3!, {D16-D31} ; Save D16-D31
376+
#endif
377+
LDRB R2, [R0, #TCB_SP_FRAME]
378+
#ifdef __ARM_ADVANCED_SIMD__
379+
ORR R2, R2, #4 ; NEON state
380+
#else
381+
ORR R2, R2, #2 ; VFP state
382+
#endif
383+
STRB R2, [R0, #TCB_SP_FRAME] ; Record VFP/NEON state
386384

387385
osRtxContextSave1
388386
STR R3, [R0, #TCB_SP_OFS] ; Store user sp to osRtxInfo.thread.run.curr
@@ -399,12 +397,12 @@ osRtxContextRestore
399397
MCR p15, 0, R2, c1, c0, 2 ; Write CPACR
400398
BEQ osRtxContextRestore1 ; No VFP
401399
ISB ; Only sync if we enabled VFP, otherwise we will context switch before next VFP instruction anyway
402-
IF {TARGET_FEATURE_EXTENSION_REGISTER_COUNT} == 32
403-
VLDMIA R3!, {D16-D31}
404-
ENDIF
405-
VLDMIA R3!, {D0-D15}
400+
#ifdef __ARM_ADVANCED_SIMD__
401+
VLDMIA R3!, {D16-D31} ; Restore D16-D31
402+
#endif
403+
VLDMIA R3!, {D0-D15} ; Restore D0-D15
406404
LDR R2, [R3]
407-
VMSR FPSCR, R2
405+
VMSR FPSCR, R2 ; Restore FPSCR
408406
ADD R3, R3, #8
409407

410408
osRtxContextRestore1

0 commit comments

Comments
 (0)