diff options
Diffstat (limited to 'arch/arm/vfp/vfphw.S')
| -rw-r--r-- | arch/arm/vfp/vfphw.S | 64 |
1 files changed, 48 insertions, 16 deletions
diff --git a/arch/arm/vfp/vfphw.S b/arch/arm/vfp/vfphw.S index a5a4e57763c3..66dc2d03b7fc 100644 --- a/arch/arm/vfp/vfphw.S +++ b/arch/arm/vfp/vfphw.S @@ -100,6 +100,7 @@ ENTRY(vfp_support_entry) beq no_old_VFP_process VFPFSTMIA r4, r5 @ save the working registers VFPFMRX r5, FPSCR @ current status +#ifndef CONFIG_CPU_FEROCEON tst r1, #FPEXC_EX @ is there additional state to save? beq 1f VFPFMRX r6, FPINST @ FPINST (only if FPEXC.EX is set) @@ -107,6 +108,7 @@ ENTRY(vfp_support_entry) beq 1f VFPFMRX r8, FPINST2 @ FPINST2 if needed (and present) 1: +#endif stmia r4, {r1, r5, r6, r8} @ save FPEXC, FPSCR, FPINST, FPINST2 @ and point r4 at the word at the @ start of the register dump @@ -119,6 +121,7 @@ no_old_VFP_process: VFPFLDMIA r10, r5 @ reload the working registers while @ FPEXC is in a safe state ldmia r10, {r1, r5, r6, r8} @ load FPEXC, FPSCR, FPINST, FPINST2 +#ifndef CONFIG_CPU_FEROCEON tst r1, #FPEXC_EX @ is there additional state to restore? beq 1f VFPFMXR FPINST, r6 @ restore FPINST (only if FPEXC.EX is set) @@ -126,6 +129,7 @@ no_old_VFP_process: beq 1f VFPFMXR FPINST2, r8 @ FPINST2 if needed (and present) 1: +#endif VFPFMXR FPSCR, r5 @ restore status check_for_exception: @@ -137,6 +141,12 @@ check_for_exception: VFPFMXR FPEXC, r1 @ restore FPEXC last sub r2, r2, #4 str r2, [sp, #S_PC] @ retry the instruction +#ifdef CONFIG_PREEMPT + get_thread_info r10 + ldr r4, [r10, #TI_PREEMPT] @ get preempt count + sub r11, r4, #1 @ decrement it + str r11, [r10, #TI_PREEMPT] +#endif mov pc, r9 @ we think we have handled things @@ -155,6 +165,12 @@ look_for_VFP_exceptions: @ not recognised by VFP DBGSTR "not VFP" +#ifdef CONFIG_PREEMPT + get_thread_info r10 + ldr r4, [r10, #TI_PREEMPT] @ get preempt count + sub r11, r4, #1 @ decrement it + str r11, [r10, #TI_PREEMPT] +#endif mov pc, lr process_exception: @@ -193,40 +209,55 @@ ENDPROC(vfp_save_state) last_VFP_context_address: .word last_VFP_context -ENTRY(vfp_get_float) - add pc, pc, r0, lsl #3 + .macro tbl_branch, base, tmp, shift +#ifdef CONFIG_THUMB2_KERNEL + adr \tmp, 1f + add \tmp, \tmp, \base, lsl \shift + mov pc, \tmp +#else + add pc, pc, \base, lsl \shift mov r0, r0 +#endif +1: + .endm + +ENTRY(vfp_get_float) + tbl_branch r0, r3, #3 .irp dr,0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15 - mrc p10, 0, r0, c\dr, c0, 0 @ fmrs r0, s0 +1: mrc p10, 0, r0, c\dr, c0, 0 @ fmrs r0, s0 mov pc, lr - mrc p10, 0, r0, c\dr, c0, 4 @ fmrs r0, s1 + .org 1b + 8 +1: mrc p10, 0, r0, c\dr, c0, 4 @ fmrs r0, s1 mov pc, lr + .org 1b + 8 .endr ENDPROC(vfp_get_float) ENTRY(vfp_put_float) - add pc, pc, r1, lsl #3 - mov r0, r0 + tbl_branch r1, r3, #3 .irp dr,0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15 - mcr p10, 0, r0, c\dr, c0, 0 @ fmsr r0, s0 +1: mcr p10, 0, r0, c\dr, c0, 0 @ fmsr r0, s0 mov pc, lr - mcr p10, 0, r0, c\dr, c0, 4 @ fmsr r0, s1 + .org 1b + 8 +1: mcr p10, 0, r0, c\dr, c0, 4 @ fmsr r0, s1 mov pc, lr + .org 1b + 8 .endr ENDPROC(vfp_put_float) ENTRY(vfp_get_double) - add pc, pc, r0, lsl #3 - mov r0, r0 + tbl_branch r0, r3, #3 .irp dr,0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15 - fmrrd r0, r1, d\dr +1: fmrrd r0, r1, d\dr mov pc, lr + .org 1b + 8 .endr #ifdef CONFIG_VFPv3 @ d16 - d31 registers .irp dr,0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15 - mrrc p11, 3, r0, r1, c\dr @ fmrrd r0, r1, d\dr +1: mrrc p11, 3, r0, r1, c\dr @ fmrrd r0, r1, d\dr mov pc, lr + .org 1b + 8 .endr #endif @@ -237,17 +268,18 @@ ENTRY(vfp_get_double) ENDPROC(vfp_get_double) ENTRY(vfp_put_double) - add pc, pc, r2, lsl #3 - mov r0, r0 + tbl_branch r2, r3, #3 .irp dr,0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15 - fmdrr d\dr, r0, r1 +1: fmdrr d\dr, r0, r1 mov pc, lr + .org 1b + 8 .endr #ifdef CONFIG_VFPv3 @ d16 - d31 registers .irp dr,0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15 - mcrr p11, 3, r1, r2, c\dr @ fmdrr r1, r2, d\dr +1: mcrr p11, 3, r1, r2, c\dr @ fmdrr r1, r2, d\dr mov pc, lr + .org 1b + 8 .endr #endif ENDPROC(vfp_put_double) |