Back to home page

LXR

 
 

    


0001 /*
0002  *  linux/arch/arm/vfp/vfphw.S
0003  *
0004  *  Copyright (C) 2004 ARM Limited.
0005  *  Written by Deep Blue Solutions Limited.
0006  *
0007  * This program is free software; you can redistribute it and/or modify
0008  * it under the terms of the GNU General Public License version 2 as
0009  * published by the Free Software Foundation.
0010  *
0011  * This code is called from the kernel's undefined instruction trap.
0012  * r9 holds the return address for successful handling.
0013  * lr holds the return address for unrecognised instructions.
0014  * r10 points at the start of the private FP workspace in the thread structure
0015  * sp points to a struct pt_regs (as defined in include/asm/proc/ptrace.h)
0016  */
0017 #include <linux/init.h>
0018 #include <linux/linkage.h>
0019 #include <asm/thread_info.h>
0020 #include <asm/vfpmacros.h>
0021 #include <linux/kern_levels.h>
0022 #include <asm/assembler.h>
0023 #include <asm/asm-offsets.h>
0024 
0025     .macro  DBGSTR, str
0026 #ifdef DEBUG
0027     stmfd   sp!, {r0-r3, ip, lr}
0028     ldr r0, =1f
0029     bl  printk
0030     ldmfd   sp!, {r0-r3, ip, lr}
0031 
0032     .pushsection .rodata, "a"
0033 1:  .ascii  KERN_DEBUG "VFP: \str\n"
0034     .byte   0
0035     .previous
0036 #endif
0037     .endm
0038 
0039     .macro  DBGSTR1, str, arg
0040 #ifdef DEBUG
0041     stmfd   sp!, {r0-r3, ip, lr}
0042     mov r1, \arg
0043     ldr r0, =1f
0044     bl  printk
0045     ldmfd   sp!, {r0-r3, ip, lr}
0046 
0047     .pushsection .rodata, "a"
0048 1:  .ascii  KERN_DEBUG "VFP: \str\n"
0049     .byte   0
0050     .previous
0051 #endif
0052     .endm
0053 
0054     .macro  DBGSTR3, str, arg1, arg2, arg3
0055 #ifdef DEBUG
0056     stmfd   sp!, {r0-r3, ip, lr}
0057     mov r3, \arg3
0058     mov r2, \arg2
0059     mov r1, \arg1
0060     ldr r0, =1f
0061     bl  printk
0062     ldmfd   sp!, {r0-r3, ip, lr}
0063 
0064     .pushsection .rodata, "a"
0065 1:  .ascii  KERN_DEBUG "VFP: \str\n"
0066     .byte   0
0067     .previous
0068 #endif
0069     .endm
0070 
0071 
0072 @ VFP hardware support entry point.
0073 @
0074 @  r0  = instruction opcode (32-bit ARM or two 16-bit Thumb)
0075 @  r2  = PC value to resume execution after successful emulation
0076 @  r9  = normal "successful" return address
0077 @  r10 = vfp_state union
0078 @  r11 = CPU number
0079 @  lr  = unrecognised instruction return address
0080 @  IRQs enabled.
0081 ENTRY(vfp_support_entry)
0082     DBGSTR3 "instr %08x pc %08x state %p", r0, r2, r10
0083 
0084     ldr r3, [sp, #S_PSR]    @ Neither lazy restore nor FP exceptions
0085     and r3, r3, #MODE_MASK  @ are supported in kernel mode
0086     teq r3, #USR_MODE
0087     bne vfp_kmode_exception @ Returns through lr
0088 
0089     VFPFMRX r1, FPEXC       @ Is the VFP enabled?
0090     DBGSTR1 "fpexc %08x", r1
0091     tst r1, #FPEXC_EN
0092     bne look_for_VFP_exceptions @ VFP is already enabled
0093 
0094     DBGSTR1 "enable %x", r10
0095     ldr r3, vfp_current_hw_state_address
0096     orr r1, r1, #FPEXC_EN   @ user FPEXC has the enable bit set
0097     ldr r4, [r3, r11, lsl #2]   @ vfp_current_hw_state pointer
0098     bic r5, r1, #FPEXC_EX   @ make sure exceptions are disabled
0099     cmp r4, r10         @ this thread owns the hw context?
0100 #ifndef CONFIG_SMP
0101     @ For UP, checking that this thread owns the hw context is
0102     @ sufficient to determine that the hardware state is valid.
0103     beq vfp_hw_state_valid
0104 
0105     @ On UP, we lazily save the VFP context.  As a different
0106     @ thread wants ownership of the VFP hardware, save the old
0107     @ state if there was a previous (valid) owner.
0108 
0109     VFPFMXR FPEXC, r5       @ enable VFP, disable any pending
0110                     @ exceptions, so we can get at the
0111                     @ rest of it
0112 
0113     DBGSTR1 "save old state %p", r4
0114     cmp r4, #0          @ if the vfp_current_hw_state is NULL
0115     beq vfp_reload_hw       @ then the hw state needs reloading
0116     VFPFSTMIA r4, r5        @ save the working registers
0117     VFPFMRX r5, FPSCR       @ current status
0118 #ifndef CONFIG_CPU_FEROCEON
0119     tst r1, #FPEXC_EX       @ is there additional state to save?
0120     beq 1f
0121     VFPFMRX r6, FPINST      @ FPINST (only if FPEXC.EX is set)
0122     tst r1, #FPEXC_FP2V     @ is there an FPINST2 to read?
0123     beq 1f
0124     VFPFMRX r8, FPINST2     @ FPINST2 if needed (and present)
0125 1:
0126 #endif
0127     stmia   r4, {r1, r5, r6, r8}    @ save FPEXC, FPSCR, FPINST, FPINST2
0128 vfp_reload_hw:
0129 
0130 #else
0131     @ For SMP, if this thread does not own the hw context, then we
0132     @ need to reload it.  No need to save the old state as on SMP,
0133     @ we always save the state when we switch away from a thread.
0134     bne vfp_reload_hw
0135 
0136     @ This thread has ownership of the current hardware context.
0137     @ However, it may have been migrated to another CPU, in which
0138     @ case the saved state is newer than the hardware context.
0139     @ Check this by looking at the CPU number which the state was
0140     @ last loaded onto.
0141     ldr ip, [r10, #VFP_CPU]
0142     teq ip, r11
0143     beq vfp_hw_state_valid
0144 
0145 vfp_reload_hw:
0146     @ We're loading this threads state into the VFP hardware. Update
0147     @ the CPU number which contains the most up to date VFP context.
0148     str r11, [r10, #VFP_CPU]
0149 
0150     VFPFMXR FPEXC, r5       @ enable VFP, disable any pending
0151                     @ exceptions, so we can get at the
0152                     @ rest of it
0153 #endif
0154 
0155     DBGSTR1 "load state %p", r10
0156     str r10, [r3, r11, lsl #2]  @ update the vfp_current_hw_state pointer
0157                     @ Load the saved state back into the VFP
0158     VFPFLDMIA r10, r5       @ reload the working registers while
0159                     @ FPEXC is in a safe state
0160     ldmia   r10, {r1, r5, r6, r8}   @ load FPEXC, FPSCR, FPINST, FPINST2
0161 #ifndef CONFIG_CPU_FEROCEON
0162     tst r1, #FPEXC_EX       @ is there additional state to restore?
0163     beq 1f
0164     VFPFMXR FPINST, r6      @ restore FPINST (only if FPEXC.EX is set)
0165     tst r1, #FPEXC_FP2V     @ is there an FPINST2 to write?
0166     beq 1f
0167     VFPFMXR FPINST2, r8     @ FPINST2 if needed (and present)
0168 1:
0169 #endif
0170     VFPFMXR FPSCR, r5       @ restore status
0171 
0172 @ The context stored in the VFP hardware is up to date with this thread
0173 vfp_hw_state_valid:
0174     tst r1, #FPEXC_EX
0175     bne process_exception   @ might as well handle the pending
0176                     @ exception before retrying branch
0177                     @ out before setting an FPEXC that
0178                     @ stops us reading stuff
0179     VFPFMXR FPEXC, r1       @ Restore FPEXC last
0180     sub r2, r2, #4      @ Retry current instruction - if Thumb
0181     str r2, [sp, #S_PC]     @ mode it's two 16-bit instructions,
0182                     @ else it's one 32-bit instruction, so
0183                     @ always subtract 4 from the following
0184                     @ instruction address.
0185     dec_preempt_count_ti r10, r4
0186     ret r9          @ we think we have handled things
0187 
0188 
0189 look_for_VFP_exceptions:
0190     @ Check for synchronous or asynchronous exception
0191     tst r1, #FPEXC_EX | FPEXC_DEX
0192     bne process_exception
0193     @ On some implementations of the VFP subarch 1, setting FPSCR.IXE
0194     @ causes all the CDP instructions to be bounced synchronously without
0195     @ setting the FPEXC.EX bit
0196     VFPFMRX r5, FPSCR
0197     tst r5, #FPSCR_IXE
0198     bne process_exception
0199 
0200     tst r5, #FPSCR_LENGTH_MASK
0201     beq skip
0202     orr r1, r1, #FPEXC_DEX
0203     b   process_exception
0204 skip:
0205 
0206     @ Fall into hand on to next handler - appropriate coproc instr
0207     @ not recognised by VFP
0208 
0209     DBGSTR  "not VFP"
0210     dec_preempt_count_ti r10, r4
0211     ret lr
0212 
0213 process_exception:
0214     DBGSTR  "bounce"
0215     mov r2, sp          @ nothing stacked - regdump is at TOS
0216     mov lr, r9          @ setup for a return to the user code.
0217 
0218     @ Now call the C code to package up the bounce to the support code
0219     @   r0 holds the trigger instruction
0220     @   r1 holds the FPEXC value
0221     @   r2 pointer to register dump
0222     b   VFP_bounce      @ we have handled this - the support
0223                     @ code will raise an exception if
0224                     @ required. If not, the user code will
0225                     @ retry the faulted instruction
0226 ENDPROC(vfp_support_entry)
0227 
0228 ENTRY(vfp_save_state)
0229     @ Save the current VFP state
0230     @ r0 - save location
0231     @ r1 - FPEXC
0232     DBGSTR1 "save VFP state %p", r0
0233     VFPFSTMIA r0, r2        @ save the working registers
0234     VFPFMRX r2, FPSCR       @ current status
0235     tst r1, #FPEXC_EX       @ is there additional state to save?
0236     beq 1f
0237     VFPFMRX r3, FPINST      @ FPINST (only if FPEXC.EX is set)
0238     tst r1, #FPEXC_FP2V     @ is there an FPINST2 to read?
0239     beq 1f
0240     VFPFMRX r12, FPINST2        @ FPINST2 if needed (and present)
0241 1:
0242     stmia   r0, {r1, r2, r3, r12}   @ save FPEXC, FPSCR, FPINST, FPINST2
0243     ret lr
0244 ENDPROC(vfp_save_state)
0245 
0246     .align
0247 vfp_current_hw_state_address:
0248     .word   vfp_current_hw_state
0249 
0250     .macro  tbl_branch, base, tmp, shift
0251 #ifdef CONFIG_THUMB2_KERNEL
0252     adr \tmp, 1f
0253     add \tmp, \tmp, \base, lsl \shift
0254     ret \tmp
0255 #else
0256     add pc, pc, \base, lsl \shift
0257     mov r0, r0
0258 #endif
0259 1:
0260     .endm
0261 
0262 ENTRY(vfp_get_float)
0263     tbl_branch r0, r3, #3
0264     .irp    dr,0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15
0265 1:  mrc p10, 0, r0, c\dr, c0, 0 @ fmrs  r0, s0
0266     ret lr
0267     .org    1b + 8
0268 1:  mrc p10, 0, r0, c\dr, c0, 4 @ fmrs  r0, s1
0269     ret lr
0270     .org    1b + 8
0271     .endr
0272 ENDPROC(vfp_get_float)
0273 
0274 ENTRY(vfp_put_float)
0275     tbl_branch r1, r3, #3
0276     .irp    dr,0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15
0277 1:  mcr p10, 0, r0, c\dr, c0, 0 @ fmsr  r0, s0
0278     ret lr
0279     .org    1b + 8
0280 1:  mcr p10, 0, r0, c\dr, c0, 4 @ fmsr  r0, s1
0281     ret lr
0282     .org    1b + 8
0283     .endr
0284 ENDPROC(vfp_put_float)
0285 
0286 ENTRY(vfp_get_double)
0287     tbl_branch r0, r3, #3
0288     .irp    dr,0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15
0289 1:  fmrrd   r0, r1, d\dr
0290     ret lr
0291     .org    1b + 8
0292     .endr
0293 #ifdef CONFIG_VFPv3
0294     @ d16 - d31 registers
0295     .irp    dr,0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15
0296 1:  mrrc    p11, 3, r0, r1, c\dr    @ fmrrd r0, r1, d\dr
0297     ret lr
0298     .org    1b + 8
0299     .endr
0300 #endif
0301 
0302     @ virtual register 16 (or 32 if VFPv3) for compare with zero
0303     mov r0, #0
0304     mov r1, #0
0305     ret lr
0306 ENDPROC(vfp_get_double)
0307 
0308 ENTRY(vfp_put_double)
0309     tbl_branch r2, r3, #3
0310     .irp    dr,0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15
0311 1:  fmdrr   d\dr, r0, r1
0312     ret lr
0313     .org    1b + 8
0314     .endr
0315 #ifdef CONFIG_VFPv3
0316     @ d16 - d31 registers
0317     .irp    dr,0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15
0318 1:  mcrr    p11, 3, r0, r1, c\dr    @ fmdrr r0, r1, d\dr
0319     ret lr
0320     .org    1b + 8
0321     .endr
0322 #endif
0323 ENDPROC(vfp_put_double)