1/* 2 * linux/arch/arm/vfp/vfphw.S 3 * 4 * Copyright (C) 2004 ARM Limited. 5 * Written by Deep Blue Solutions Limited. 6 * 7 * This program is free software; you can redistribute it and/or modify 8 * it under the terms of the GNU General Public License version 2 as 9 * published by the Free Software Foundation. 10 * 11 * This code is called from the kernel's undefined instruction trap. 12 * r9 holds the return address for successful handling. 13 * lr holds the return address for unrecognised instructions. 14 * r10 points at the start of the private FP workspace in the thread structure 15 * sp points to a struct pt_regs (as defined in include/asm/proc/ptrace.h) 16 */ 17#include <asm/thread_info.h> 18#include <asm/vfpmacros.h> 19#include "../kernel/entry-header.S" 20 21 .macro DBGSTR, str 22#ifdef DEBUG 23 stmfd sp!, {r0-r3, ip, lr} 24 add r0, pc, #4 25 bl printk 26 b 1f 27 .asciz "<7>VFP: \str\n" 28 .balign 4 291: ldmfd sp!, {r0-r3, ip, lr} 30#endif 31 .endm 32 33 .macro DBGSTR1, str, arg 34#ifdef DEBUG 35 stmfd sp!, {r0-r3, ip, lr} 36 mov r1, \arg 37 add r0, pc, #4 38 bl printk 39 b 1f 40 .asciz "<7>VFP: \str\n" 41 .balign 4 421: ldmfd sp!, {r0-r3, ip, lr} 43#endif 44 .endm 45 46 .macro DBGSTR3, str, arg1, arg2, arg3 47#ifdef DEBUG 48 stmfd sp!, {r0-r3, ip, lr} 49 mov r3, \arg3 50 mov r2, \arg2 51 mov r1, \arg1 52 add r0, pc, #4 53 bl printk 54 b 1f 55 .asciz "<7>VFP: \str\n" 56 .balign 4 571: ldmfd sp!, {r0-r3, ip, lr} 58#endif 59 .endm 60 61 62@ VFP hardware support entry point. 63@ 64@ r0 = faulted instruction 65@ r2 = faulted PC+4 66@ r9 = successful return 67@ r10 = vfp_state union 68@ lr = failure return 69 70 .globl vfp_support_entry 71vfp_support_entry: 72 DBGSTR3 "instr %08x pc %08x state %p", r0, r2, r10 73 74 VFPFMRX r1, FPEXC @ Is the VFP enabled? 75 DBGSTR1 "fpexc %08x", r1 76 tst r1, #FPEXC_ENABLE 77 bne look_for_VFP_exceptions @ VFP is already enabled 78 79 DBGSTR1 "enable %x", r10 80 ldr r3, last_VFP_context_address 81 orr r1, r1, #FPEXC_ENABLE @ user FPEXC has the enable bit set 82 ldr r4, [r3] @ last_VFP_context pointer 83 bic r5, r1, #FPEXC_EXCEPTION @ make sure exceptions are disabled 84 cmp r4, r10 85 beq check_for_exception @ we are returning to the same 86 @ process, so the registers are 87 @ still there. In this case, we do 88 @ not want to drop a pending exception. 89 90 VFPFMXR FPEXC, r5 @ enable VFP, disable any pending 91 @ exceptions, so we can get at the 92 @ rest of it 93 94 @ Save out the current registers to the old thread state 95 96 DBGSTR1 "save old state %p", r4 97 cmp r4, #0 98 beq no_old_VFP_process 99 VFPFMRX r5, FPSCR @ current status 100 VFPFMRX r6, FPINST @ FPINST (always there, rev0 onwards) 101 tst r1, #FPEXC_FPV2 @ is there an FPINST2 to read? 102 VFPFMRX r8, FPINST2, NE @ FPINST2 if needed - avoids reading 103 @ nonexistant reg on rev0 104 VFPFSTMIA r4 @ save the working registers 105 stmia r4, {r1, r5, r6, r8} @ save FPEXC, FPSCR, FPINST, FPINST2 106 @ and point r4 at the word at the 107 @ start of the register dump 108 109no_old_VFP_process: 110 DBGSTR1 "load state %p", r10 111 str r10, [r3] @ update the last_VFP_context pointer 112 @ Load the saved state back into the VFP 113 VFPFLDMIA r10 @ reload the working registers while 114 @ FPEXC is in a safe state 115 ldmia r10, {r1, r5, r6, r8} @ load FPEXC, FPSCR, FPINST, FPINST2 116 tst r1, #FPEXC_FPV2 @ is there an FPINST2 to write? 117 VFPFMXR FPINST2, r8, NE @ FPINST2 if needed - avoids writing 118 @ nonexistant reg on rev0 119 VFPFMXR FPINST, r6 120 VFPFMXR FPSCR, r5 @ restore status 121 122check_for_exception: 123 tst r1, #FPEXC_EXCEPTION 124 bne process_exception @ might as well handle the pending 125 @ exception before retrying branch 126 @ out before setting an FPEXC that 127 @ stops us reading stuff 128 VFPFMXR FPEXC, r1 @ restore FPEXC last 129 sub r2, r2, #4 130 str r2, [sp, #S_PC] @ retry the instruction 131 mov pc, r9 @ we think we have handled things 132 133 134look_for_VFP_exceptions: 135 tst r1, #FPEXC_EXCEPTION 136 bne process_exception 137 VFPFMRX r5, FPSCR 138 tst r5, #FPSCR_IXE @ IXE doesn't set FPEXC_EXCEPTION ! 139 bne process_exception 140 141 @ Fall into hand on to next handler - appropriate coproc instr 142 @ not recognised by VFP 143 144 DBGSTR "not VFP" 145 mov pc, lr 146 147process_exception: 148 DBGSTR "bounce" 149 sub r2, r2, #4 150 str r2, [sp, #S_PC] @ retry the instruction on exit from 151 @ the imprecise exception handling in 152 @ the support code 153 mov r2, sp @ nothing stacked - regdump is at TOS 154 mov lr, r9 @ setup for a return to the user code. 155 156 @ Now call the C code to package up the bounce to the support code 157 @ r0 holds the trigger instruction 158 @ r1 holds the FPEXC value 159 @ r2 pointer to register dump 160 b VFP9_bounce @ we have handled this - the support 161 @ code will raise an exception if 162 @ required. If not, the user code will 163 @ retry the faulted instruction 164 165last_VFP_context_address: 166 .word last_VFP_context 167 168 .globl vfp_get_float 169vfp_get_float: 170 add pc, pc, r0, lsl #3 171 mov r0, r0 172 .irp dr,0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15 173 mrc p10, 0, r0, c\dr, c0, 0 @ fmrs r0, s0 174 mov pc, lr 175 mrc p10, 0, r0, c\dr, c0, 4 @ fmrs r0, s1 176 mov pc, lr 177 .endr 178 179 .globl vfp_put_float 180vfp_put_float: 181 add pc, pc, r0, lsl #3 182 mov r0, r0 183 .irp dr,0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15 184 mcr p10, 0, r1, c\dr, c0, 0 @ fmsr r0, s0 185 mov pc, lr 186 mcr p10, 0, r1, c\dr, c0, 4 @ fmsr r0, s1 187 mov pc, lr 188 .endr 189 190 .globl vfp_get_double 191vfp_get_double: 192 add pc, pc, r0, lsl #3 193 mov r0, r0 194 .irp dr,0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15 195 mrrc p11, 1, r0, r1, c\dr @ fmrrd r0, r1, d\dr 196 mov pc, lr 197 .endr 198 199 @ virtual register 16 for compare with zero 200 mov r0, #0 201 mov r1, #0 202 mov pc, lr 203 204 .globl vfp_put_double 205vfp_put_double: 206 add pc, pc, r0, lsl #3 207 mov r0, r0 208 .irp dr,0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15 209 mcrr p11, 1, r1, r2, c\dr @ fmdrr r1, r2, d\dr 210 mov pc, lr 211 .endr 212