diff options
Diffstat (limited to 'cpukit/score/cpu/arm/armv4-exception-default.S')
-rw-r--r-- | cpukit/score/cpu/arm/armv4-exception-default.S | 24 |
1 files changed, 19 insertions, 5 deletions
diff --git a/cpukit/score/cpu/arm/armv4-exception-default.S b/cpukit/score/cpu/arm/armv4-exception-default.S index 950ad671bf..a0ee46c25b 100644 --- a/cpukit/score/cpu/arm/armv4-exception-default.S +++ b/cpukit/score/cpu/arm/armv4-exception-default.S @@ -118,13 +118,18 @@ save_more_context: /* Argument for high level handler */ mov r0, sp -#ifdef ARM_MULTILIB_VFP_D32 + /* Clear VFP context pointer */ + add r3, sp, #ARM_EXCEPTION_FRAME_VFP_CONTEXT_OFFSET + mov r1, #0 + str r1, [r3] + +#ifdef ARM_MULTILIB_VFP /* Ensure that the FPU is enabled */ vmrs r1, FPEXC tst r1, #(1 << 30) - beq fpu_save_done + beq 1f - add r3, sp, #ARM_EXCEPTION_FRAME_VFP_CONTEXT_OFFSET + /* Save VFP context */ sub sp, #(ARM_VFP_CONTEXT_SIZE + 4) add r4, sp, #4 bic r4, r4, #7 @@ -132,10 +137,19 @@ save_more_context: vmrs r2, FPSCR stmia r4!, {r1-r2} vstmia r4!, {d0-d15} +#ifdef ARM_MULTILIB_VFP_D32 vstmia r4!, {d16-d31} - -fpu_save_done: +#else + mov r1, #0 + mov r2, #0 + adds r3, r4, #128 +2: + stmia r4!, {r1-r2} + cmp r4, r3 + bne 2b #endif +1: +#endif /* ARM_MULTILIB_VFP */ /* Call high level handler */ SWITCH_FROM_ARM_TO_THUMB r1 |