vfp.c revision 266341
1238384Sjkim/*- 2238384Sjkim * Copyright (c) 2014 Ian Lepore <ian@freebsd.org> 3238384Sjkim * Copyright (c) 2012 Mark Tinguely 4238384Sjkim * 5238384Sjkim * All rights reserved. 6238384Sjkim * 7238384Sjkim * Redistribution and use in source and binary forms, with or without 8238384Sjkim * modification, are permitted provided that the following conditions 9238384Sjkim * are met: 10238384Sjkim * 1. Redistributions of source code must retain the above copyright 11238384Sjkim * notice, this list of conditions and the following disclaimer. 12238384Sjkim * 2. Redistributions in binary form must reproduce the above copyright 13238384Sjkim * notice, this list of conditions and the following disclaimer in the 14238384Sjkim * documentation and/or other materials provided with the distribution. 15238384Sjkim * 16238384Sjkim * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND 17238384Sjkim * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 18238384Sjkim * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 19238384Sjkim * ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE 20238384Sjkim * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 21238384Sjkim * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS 22238384Sjkim * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 23238384Sjkim * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 24238384Sjkim * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY 25238384Sjkim * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF 26238384Sjkim * SUCH DAMAGE. 27238384Sjkim */ 28238384Sjkim 29238384Sjkim#include <sys/cdefs.h> 30238384Sjkim__FBSDID("$FreeBSD: stable/10/sys/arm/arm/vfp.c 266341 2014-05-17 19:37:04Z ian $"); 31238384Sjkim 32238384Sjkim#ifdef VFP 33238384Sjkim#include <sys/param.h> 34238384Sjkim#include <sys/systm.h> 35238384Sjkim#include <sys/proc.h> 36238384Sjkim#include <sys/kernel.h> 37238384Sjkim 38238384Sjkim#include <machine/armreg.h> 39238384Sjkim#include <machine/frame.h> 40238384Sjkim#include <machine/fp.h> 41238384Sjkim#include <machine/pcb.h> 42238384Sjkim#include <machine/undefined.h> 43238384Sjkim#include <machine/vfp.h> 44238384Sjkim 45238384Sjkim/* function prototypes */ 46238384Sjkimstatic int vfp_bounce(u_int, u_int, struct trapframe *, int); 47238384Sjkimstatic void vfp_restore(struct vfp_state *); 48238384Sjkim 49238384Sjkimextern int vfp_exists; 50238384Sjkimstatic struct undefined_handler vfp10_uh, vfp11_uh; 51238384Sjkim/* If true the VFP unit has 32 double registers, otherwise it has 16 */ 52238384Sjkimstatic int is_d32; 53238384Sjkim 54238384Sjkim/* The VFMXR command using coprocessor commands */ 55238384Sjkim#define fmxr(reg, val) \ 56238384Sjkim __asm __volatile("mcr p10, 7, %0, " __STRING(reg) " , c0, 0" :: "r"(val)); 57238384Sjkim 58238384Sjkim/* The VFMRX command using coprocessor commands */ 59238384Sjkim#define fmrx(reg) \ 60238384Sjkim({ u_int val = 0;\ 61238384Sjkim __asm __volatile("mrc p10, 7, %0, " __STRING(reg) " , c0, 0" : "=r"(val));\ 62238384Sjkim val; \ 63238384Sjkim}) 64238384Sjkim 65238384Sjkim/* 66238384Sjkim * Work around an issue with GCC where the asm it generates is not unified 67238384Sjkim * syntax and fails to assemble because it expects the ldcleq instruction in the 68238384Sjkim * form ldc<c>l, not in the UAL form ldcl<c>, and similar for stcleq. 69238384Sjkim */ 70238384Sjkim#ifdef __clang__ 71238384Sjkim#define LDCLNE "ldclne " 72238384Sjkim#define STCLNE "stclne " 73238384Sjkim#else 74238384Sjkim#define LDCLNE "ldcnel " 75238384Sjkim#define STCLNE "stcnel " 76238384Sjkim#endif 77238384Sjkim 78238384Sjkimstatic u_int 79238384Sjkimget_coprocessorACR(void) 80238384Sjkim{ 81238384Sjkim u_int val; 82238384Sjkim __asm __volatile("mrc p15, 0, %0, c1, c0, 2" : "=r" (val) : : "cc"); 83238384Sjkim return val; 84238384Sjkim} 85238384Sjkim 86238384Sjkimstatic void 87238384Sjkimset_coprocessorACR(u_int val) 88238384Sjkim{ 89238384Sjkim __asm __volatile("mcr p15, 0, %0, c1, c0, 2\n\t" 90238384Sjkim : : "r" (val) : "cc"); 91238384Sjkim isb(); 92238384Sjkim} 93238384Sjkim 94238384Sjkim 95238384Sjkim /* called for each cpu */ 96238384Sjkimvoid 97238384Sjkimvfp_init(void) 98238384Sjkim{ 99238384Sjkim u_int fpsid, fpexc, tmp; 100238384Sjkim u_int coproc, vfp_arch; 101238384Sjkim 102238384Sjkim coproc = get_coprocessorACR(); 103238384Sjkim coproc |= COPROC10 | COPROC11; 104238384Sjkim set_coprocessorACR(coproc); 105238384Sjkim 106238384Sjkim fpsid = fmrx(VFPSID); /* read the vfp system id */ 107238384Sjkim fpexc = fmrx(VFPEXC); /* read the vfp exception reg */ 108238384Sjkim 109238384Sjkim if (!(fpsid & VFPSID_HARDSOFT_IMP)) { 110238384Sjkim vfp_exists = 1; 111238384Sjkim is_d32 = 0; 112238384Sjkim PCPU_SET(vfpsid, fpsid); /* save the VFPSID */ 113238384Sjkim 114238384Sjkim vfp_arch = 115238384Sjkim (fpsid & VFPSID_SUBVERSION2_MASK) >> VFPSID_SUBVERSION_OFF; 116238384Sjkim 117238384Sjkim if (vfp_arch >= VFP_ARCH3) { 118238384Sjkim tmp = fmrx(VMVFR0); 119238384Sjkim PCPU_SET(vfpmvfr0, tmp); 120238384Sjkim 121238384Sjkim if ((tmp & VMVFR0_RB_MASK) == 2) 122238384Sjkim is_d32 = 1; 123238384Sjkim 124238384Sjkim tmp = fmrx(VMVFR1); 125238384Sjkim PCPU_SET(vfpmvfr1, tmp); 126238384Sjkim } 127238384Sjkim 128238384Sjkim /* initialize the coprocess 10 and 11 calls 129238384Sjkim * These are called to restore the registers and enable 130238384Sjkim * the VFP hardware. 131238384Sjkim */ 132238384Sjkim if (vfp10_uh.uh_handler == NULL) { 133238384Sjkim vfp10_uh.uh_handler = vfp_bounce; 134238384Sjkim vfp11_uh.uh_handler = vfp_bounce; 135238384Sjkim install_coproc_handler_static(10, &vfp10_uh); 136238384Sjkim install_coproc_handler_static(11, &vfp11_uh); 137238384Sjkim } 138238384Sjkim } 139238384Sjkim} 140238384Sjkim 141238384SjkimSYSINIT(vfp, SI_SUB_CPU, SI_ORDER_ANY, vfp_init, NULL); 142238384Sjkim 143238384Sjkim 144238384Sjkim/* start VFP unit, restore the vfp registers from the PCB and retry 145238384Sjkim * the instruction 146238384Sjkim */ 147238384Sjkimstatic int 148238384Sjkimvfp_bounce(u_int addr, u_int insn, struct trapframe *frame, int code) 149238384Sjkim{ 150238384Sjkim u_int cpu, fpexc; 151238384Sjkim struct pcb *curpcb; 152238384Sjkim ksiginfo_t ksi; 153238384Sjkim 154238384Sjkim if ((code & FAULT_USER) == 0) 155238384Sjkim panic("undefined floating point instruction in supervisor mode"); 156238384Sjkim 157238384Sjkim critical_enter(); 158238384Sjkim 159238384Sjkim /* 160238384Sjkim * If the VFP is already on and we got an undefined instruction, then 161238384Sjkim * something tried to executate a truly invalid instruction that maps to 162238384Sjkim * the VFP. 163238384Sjkim */ 164238384Sjkim fpexc = fmrx(VFPEXC); 165238384Sjkim if (fpexc & VFPEXC_EN) { 166238384Sjkim /* Clear any exceptions */ 167238384Sjkim fmxr(VFPEXC, fpexc & ~(VFPEXC_EX | VFPEXC_FP2V)); 168238384Sjkim 169238384Sjkim /* kill the process - we do not handle emulation */ 170238384Sjkim critical_exit(); 171238384Sjkim 172238384Sjkim if (fpexc & VFPEXC_EX) { 173 /* We have an exception, signal a SIGFPE */ 174 ksiginfo_init_trap(&ksi); 175 ksi.ksi_signo = SIGFPE; 176 if (fpexc & VFPEXC_UFC) 177 ksi.ksi_code = FPE_FLTUND; 178 else if (fpexc & VFPEXC_OFC) 179 ksi.ksi_code = FPE_FLTOVF; 180 else if (fpexc & VFPEXC_IOC) 181 ksi.ksi_code = FPE_FLTINV; 182 ksi.ksi_addr = (void *)addr; 183 trapsignal(curthread, &ksi); 184 return 0; 185 } 186 187 return 1; 188 } 189 190 /* 191 * If the last time this thread used the VFP it was on this core, and 192 * the last thread to use the VFP on this core was this thread, then the 193 * VFP state is valid, otherwise restore this thread's state to the VFP. 194 */ 195 fmxr(VFPEXC, fpexc | VFPEXC_EN); 196 curpcb = curthread->td_pcb; 197 cpu = PCPU_GET(cpu); 198 if (curpcb->pcb_vfpcpu != cpu || curthread != PCPU_GET(fpcurthread)) { 199 vfp_restore(&curpcb->pcb_vfpstate); 200 curpcb->pcb_vfpcpu = cpu; 201 PCPU_SET(fpcurthread, curthread); 202 } 203 204 critical_exit(); 205 return (0); 206} 207 208/* 209 * Restore the given state to the VFP hardware. 210 */ 211static void 212vfp_restore(struct vfp_state *vfpsave) 213{ 214 uint32_t fpexc; 215 216 /* On VFPv2 we may need to restore FPINST and FPINST2 */ 217 fpexc = vfpsave->fpexec; 218 if (fpexc & VFPEXC_EX) { 219 fmxr(VFPINST, vfpsave->fpinst); 220 if (fpexc & VFPEXC_FP2V) 221 fmxr(VFPINST2, vfpsave->fpinst2); 222 } 223 fmxr(VFPSCR, vfpsave->fpscr); 224 225 __asm __volatile("ldc p10, c0, [%0], #128\n" /* d0-d15 */ 226 "cmp %1, #0\n" /* -D16 or -D32? */ 227 LDCLNE "p11, c0, [%0], #128\n" /* d16-d31 */ 228 "addeq %0, %0, #128\n" /* skip missing regs */ 229 : : "r" (vfpsave), "r" (is_d32) : "cc"); 230 231 fmxr(VFPEXC, fpexc); 232} 233 234/* 235 * If the VFP is on, save its current state and turn it off if requested to do 236 * so. If the VFP is not on, does not change the values at *vfpsave. Caller is 237 * responsible for preventing a context switch while this is running. 238 */ 239void 240vfp_store(struct vfp_state *vfpsave, boolean_t disable_vfp) 241{ 242 uint32_t fpexc; 243 244 fpexc = fmrx(VFPEXC); /* Is the vfp enabled? */ 245 if (fpexc & VFPEXC_EN) { 246 vfpsave->fpexec = fpexc; 247 vfpsave->fpscr = fmrx(VFPSCR); 248 249 /* On VFPv2 we may need to save FPINST and FPINST2 */ 250 if (fpexc & VFPEXC_EX) { 251 vfpsave->fpinst = fmrx(VFPINST); 252 if (fpexc & VFPEXC_FP2V) 253 vfpsave->fpinst2 = fmrx(VFPINST2); 254 fpexc &= ~VFPEXC_EX; 255 } 256 257 __asm __volatile( 258 "stc p11, c0, [%0], #128\n" /* d0-d15 */ 259 "cmp %1, #0\n" /* -D16 or -D32? */ 260 STCLNE "p11, c0, [%0], #128\n" /* d16-d31 */ 261 "addeq %0, %0, #128\n" /* skip missing regs */ 262 : : "r" (vfpsave), "r" (is_d32) : "cc"); 263 264 if (disable_vfp) 265 fmxr(VFPEXC , fpexc & ~VFPEXC_EN); 266 } 267} 268 269/* 270 * The current thread is dying. If the state currently in the hardware belongs 271 * to the current thread, set fpcurthread to NULL to indicate that the VFP 272 * hardware state does not belong to any thread. If the VFP is on, turn it off. 273 * Called only from cpu_throw(), so we don't have to worry about a context 274 * switch here. 275 */ 276void 277vfp_discard(struct thread *td) 278{ 279 u_int tmp; 280 281 if (PCPU_GET(fpcurthread) == td) 282 PCPU_SET(fpcurthread, NULL); 283 284 tmp = fmrx(VFPEXC); 285 if (tmp & VFPEXC_EN) 286 fmxr(VFPEXC, tmp & ~VFPEXC_EN); 287} 288 289#endif 290 291