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@ r11 = CPU number 69@ lr = failure return 70 71 .globl vfp_support_entry 72vfp_support_entry: 73 DBGSTR3 "instr %08x pc %08x state %p", r0, r2, r10 74 75 VFPFMRX r1, FPEXC @ Is the VFP enabled? 76 DBGSTR1 "fpexc %08x", r1 77 tst r1, #FPEXC_ENABLE 78 bne look_for_VFP_exceptions @ VFP is already enabled 79 80 DBGSTR1 "enable %x", r10 81 ldr r3, last_VFP_context_address 82 orr r1, r1, #FPEXC_ENABLE @ user FPEXC has the enable bit set 83 ldr r4, [r3, r11, lsl #2] @ last_VFP_context pointer 84 bic r5, r1, #FPEXC_EXCEPTION @ make sure exceptions are disabled 85 cmp r4, r10 86 beq check_for_exception @ we are returning to the same 87 @ process, so the registers are 88 @ still there. In this case, we do 89 @ not want to drop a pending exception. 90 91 VFPFMXR FPEXC, r5 @ enable VFP, disable any pending 92 @ exceptions, so we can get at the 93 @ rest of it 94 95#ifndef CONFIG_SMP 96 @ Save out the current registers to the old thread state 97 @ No need for SMP since this is not done lazily 98 99 DBGSTR1 "save old state %p", r4 100 cmp r4, #0 101 beq no_old_VFP_process 102 VFPFMRX r5, FPSCR @ current status 103 VFPFMRX r6, FPINST @ FPINST (always there, rev0 onwards) 104 tst r1, #FPEXC_FPV2 @ is there an FPINST2 to read? 105 VFPFMRX r8, FPINST2, NE @ FPINST2 if needed - avoids reading 106 @ nonexistant reg on rev0 107 VFPFSTMIA r4 @ save the working registers 108 stmia r4, {r1, r5, r6, r8} @ save FPEXC, FPSCR, FPINST, FPINST2 109 @ and point r4 at the word at the 110 @ start of the register dump 111#endif 112 113no_old_VFP_process: 114 DBGSTR1 "load state %p", r10 115 str r10, [r3, r11, lsl #2] @ update the last_VFP_context pointer 116 @ Load the saved state back into the VFP 117 VFPFLDMIA r10 @ reload the working registers while 118 @ FPEXC is in a safe state 119 ldmia r10, {r1, r5, r6, r8} @ load FPEXC, FPSCR, FPINST, FPINST2 120 tst r1, #FPEXC_FPV2 @ is there an FPINST2 to write? 121 VFPFMXR FPINST2, r8, NE @ FPINST2 if needed - avoids writing 122 @ nonexistant reg on rev0 123 VFPFMXR FPINST, r6 124 VFPFMXR FPSCR, r5 @ restore status 125 126check_for_exception: 127 tst r1, #FPEXC_EXCEPTION 128 bne process_exception @ might as well handle the pending 129 @ exception before retrying branch 130 @ out before setting an FPEXC that 131 @ stops us reading stuff 132 VFPFMXR FPEXC, r1 @ restore FPEXC last 133 sub r2, r2, #4 134 str r2, [sp, #S_PC] @ retry the instruction 135 mov pc, r9 @ we think we have handled things 136 137 138look_for_VFP_exceptions: 139 tst r1, #FPEXC_EXCEPTION 140 bne process_exception 141 VFPFMRX r5, FPSCR 142 tst r5, #FPSCR_IXE @ IXE doesn't set FPEXC_EXCEPTION ! 143 bne process_exception 144 145 @ Fall into hand on to next handler - appropriate coproc instr 146 @ not recognised by VFP 147 148 DBGSTR "not VFP" 149 mov pc, lr 150 151process_exception: 152 DBGSTR "bounce" 153 sub r2, r2, #4 154 str r2, [sp, #S_PC] @ retry the instruction on exit from 155 @ the imprecise exception handling in 156 @ the support code 157 mov r2, sp @ nothing stacked - regdump is at TOS 158 mov lr, r9 @ setup for a return to the user code. 159 160 @ Now call the C code to package up the bounce to the support code 161 @ r0 holds the trigger instruction 162 @ r1 holds the FPEXC value 163 @ r2 pointer to register dump 164 b VFP9_bounce @ we have handled this - the support 165 @ code will raise an exception if 166 @ required. If not, the user code will 167 @ retry the faulted instruction 168 169#ifdef CONFIG_SMP 170 .globl vfp_save_state 171 .type vfp_save_state, %function 172vfp_save_state: 173 @ Save the current VFP state 174 @ r0 - save location 175 @ r1 - FPEXC 176 DBGSTR1 "save VFP state %p", r0 177 VFPFMRX r2, FPSCR @ current status 178 VFPFMRX r3, FPINST @ FPINST (always there, rev0 onwards) 179 tst r1, #FPEXC_FPV2 @ is there an FPINST2 to read? 180 VFPFMRX r12, FPINST2, NE @ FPINST2 if needed - avoids reading 181 @ nonexistant reg on rev0 182 VFPFSTMIA r0 @ save the working registers 183 stmia r0, {r1, r2, r3, r12} @ save FPEXC, FPSCR, FPINST, FPINST2 184 mov pc, lr 185#endif 186 187last_VFP_context_address: 188 .word last_VFP_context 189 190 .globl vfp_get_float 191vfp_get_float: 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 mrc p10, 0, r0, c\dr, c0, 0 @ fmrs r0, s0 196 mov pc, lr 197 mrc p10, 0, r0, c\dr, c0, 4 @ fmrs r0, s1 198 mov pc, lr 199 .endr 200 201 .globl vfp_put_float 202vfp_put_float: 203 add pc, pc, r1, lsl #3 204 mov r0, r0 205 .irp dr,0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15 206 mcr p10, 0, r0, c\dr, c0, 0 @ fmsr r0, s0 207 mov pc, lr 208 mcr p10, 0, r0, c\dr, c0, 4 @ fmsr r0, s1 209 mov pc, lr 210 .endr 211 212 .globl vfp_get_double 213vfp_get_double: 214 add pc, pc, r0, lsl #3 215 mov r0, r0 216 .irp dr,0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15 217 fmrrd r0, r1, d\dr 218 mov pc, lr 219 .endr 220 221 @ virtual register 16 for compare with zero 222 mov r0, #0 223 mov r1, #0 224 mov pc, lr 225 226 .globl vfp_put_double 227vfp_put_double: 228 add pc, pc, r2, lsl #3 229 mov r0, r0 230 .irp dr,0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15 231 fmdrr d\dr, r0, r1 232 mov pc, lr 233 .endr 234