vfp_init.c revision 1.6
1/* $NetBSD: vfp_init.c,v 1.6 2012/09/22 01:44:12 matt Exp $ */ 2 3/* 4 * Copyright (c) 2008 ARM Ltd 5 * All rights reserved. 6 * 7 * Redistribution and use in source and binary forms, with or without 8 * modification, are permitted provided that the following conditions 9 * are met: 10 * 1. Redistributions of source code must retain the above copyright 11 * notice, this list of conditions and the following disclaimer. 12 * 2. Redistributions in binary form must reproduce the above copyright 13 * notice, this list of conditions and the following disclaimer in the 14 * documentation and/or other materials provided with the distribution. 15 * 3. The name of the company may not be used to endorse or promote 16 * products derived from this software without specific prior written 17 * permission. 18 * 19 * THIS SOFTWARE IS PROVIDED BY ARM LTD ``AS IS'' AND ANY EXPRESS OR 20 * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 21 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 22 * ARE DISCLAIMED. IN NO EVENT SHALL ARM LTD BE LIABLE FOR ANY 23 * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 24 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE 25 * GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 26 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 27 * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 28 * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 29 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 30 */ 31 32#include <sys/param.h> 33#include <sys/types.h> 34#include <sys/systm.h> 35#include <sys/device.h> 36#include <sys/proc.h> 37#include <sys/cpu.h> 38 39#include <arm/pcb.h> 40#include <arm/undefined.h> 41#include <arm/vfpreg.h> 42 43/* 44 * Use generic co-processor instructions to avoid assembly problems. 45 */ 46 47/* FMRX <X>, fpsid */ 48static inline uint32_t 49read_fpsid(void) 50{ 51 uint32_t rv; 52 __asm __volatile("mrc p10, 7, %0, c0, c0, 0" : "=r" (rv)); 53 return rv; 54} 55 56/* FMRX <X>, fpexc */ 57static inline uint32_t 58read_fpscr(void) 59{ 60 uint32_t rv; 61 __asm __volatile("mrc p10, 7, %0, c1, c0, 0" : "=r" (rv)); 62 return rv; 63} 64 65/* FMRX <X>, fpexc */ 66static inline uint32_t 67read_fpexc(void) 68{ 69 uint32_t rv; 70 __asm __volatile("mrc p10, 7, %0, c8, c0, 0" : "=r" (rv)); 71 return rv; 72} 73 74/* FMRX <X>, fpinst */ 75static inline uint32_t 76read_fpinst(void) 77{ 78 uint32_t rv; 79 __asm __volatile("mrc p10, 7, %0, c9, c0, 0" : "=r" (rv)); 80 return rv; 81} 82 83/* FMRX <X>, fpinst2 */ 84static inline uint32_t 85read_fpinst2(void) 86{ 87 uint32_t rv; 88 __asm __volatile("mrc p10, 7, %0, c10, c0, 0" : "=r" (rv)); 89 return rv; 90} 91 92/* FSTMD <X>, {d0-d15} */ 93#define save_vfpregs(X) __asm __volatile("stc p11, c0, [%0], {32}" : \ 94 : "r" (X) : "memory") 95 96/* FMXR <X>, fpscr */ 97#define write_fpscr(X) __asm __volatile("mcr p10, 7, %0, c1, c0, 0" : \ 98 : "r" (X)) 99/* FMXR <X>, fpexc */ 100#define write_fpexc(X) __asm __volatile("mcr p10, 7, %0, c8, c0, 0" : \ 101 : "r" (X)) 102/* FMXR <X>, fpinst */ 103#define write_fpinst(X) __asm __volatile("mcr p10, 7, %0, c9, c0, 0" : \ 104 : "r" (X)) 105/* FMXR <X>, fpinst2 */ 106#define write_fpinst2(X) __asm __volatile("mcr p10, 7, %0, c10, c0, 0" : \ 107 : "r" (X)) 108/* FLDMD <X>, {d0-d15} */ 109#define load_vfpregs(X) __asm __volatile("ldc p11, c0, [%0], {32}" : \ 110 : "r" (X) : "memory"); 111 112#ifdef FPU_VFP 113 114/* The real handler for VFP bounces. */ 115static int vfp_handler(u_int, u_int, trapframe_t *, int); 116static int vfp_handler(u_int, u_int, trapframe_t *, int); 117 118static void vfp_state_load(lwp_t *, bool); 119static void vfp_state_save(lwp_t *); 120static void vfp_state_release(lwp_t *); 121 122const pcu_ops_t arm_vfp_ops = { 123 .pcu_id = PCU_FPU, 124 .pcu_state_load = vfp_state_load, 125 .pcu_state_save = vfp_state_save, 126 .pcu_state_release = vfp_state_release, 127}; 128 129struct evcnt vfpevent_use; 130struct evcnt vfpevent_reuse; 131 132/* 133 * Used to test for a VFP. The following function is installed as a coproc10 134 * handler on the undefined instruction vector and then we issue a VFP 135 * instruction. If undefined_test is non zero then the VFP did not handle 136 * the instruction so must be absent, or disabled. 137 */ 138 139static int undefined_test; 140 141static int 142vfp_test(u_int address, u_int insn, trapframe_t *frame, int fault_code) 143{ 144 145 frame->tf_pc += INSN_SIZE; 146 ++undefined_test; 147 return 0; 148} 149 150#endif /* FPU_VFP */ 151 152struct evcnt vfp_fpscr_ev = 153 EVCNT_INITIALIZER(EVCNT_TYPE_TRAP, NULL, "VFP", "FPSCR traps"); 154EVCNT_ATTACH_STATIC(vfp_fpscr_ev); 155 156static int 157vfp_fpscr_handler(u_int address, u_int insn, trapframe_t *frame, int fault_code) 158{ 159 struct lwp * const l = curlwp; 160 const u_int regno = (insn >> 12) & 0xf; 161 /* 162 * Only match move to/from the FPSCR register and we 163 * can't be using the SP,LR,PC as a source. 164 */ 165 if ((insn & 0xffef0fff) != 0xeee10a10 || regno > 12) 166 return 1; 167 168 struct pcb * const pcb = lwp_getpcb(l); 169 170#ifdef FPU_VFP 171 /* 172 * If FPU is valid somewhere, let's just reenable VFP and 173 * retry the instruction (only safe thing to do since the 174 * pcb has a stale copy). 175 */ 176 if (pcb->pcb_vfp.vfp_fpexc & VFP_FPEXC_EN) 177 return 1; 178#endif 179 180 if (__predict_false((l->l_md.md_flags & MDLWP_VFPUSED) == 0)) { 181 l->l_md.md_flags |= MDLWP_VFPUSED; 182 pcb->pcb_vfp.vfp_fpscr = 183 (VFP_FPSCR_DN | VFP_FPSCR_FZ); /* Runfast */ 184 } 185 186 /* 187 * We know know the pcb has the saved copy. 188 */ 189 register_t * const regp = &frame->tf_r0 + regno; 190 if (insn & 0x00100000) { 191 *regp = pcb->pcb_vfp.vfp_fpscr; 192 } else { 193 pcb->pcb_vfp.vfp_fpscr = *regp; 194 } 195 196 vfp_fpscr_ev.ev_count++; 197 198 frame->tf_pc += INSN_SIZE; 199 return 0; 200} 201 202#ifndef FPU_VFP 203/* 204 * If we don't want VFP support, we still need to handle emulating VFP FPSCR 205 * instructions. 206 */ 207void 208vfp_attach(void) 209{ 210 install_coproc_handler(VFP_COPROC, vfp_fpscr_handler); 211} 212 213#else 214void 215vfp_attach(void) 216{ 217 struct cpu_info * const ci = curcpu(); 218 const char *model = NULL; 219 void *uh; 220 221 uh = install_coproc_handler(VFP_COPROC, vfp_test); 222 223 undefined_test = 0; 224 225#ifdef FPU_VFP 226 uint32_t cpacr = armreg_cpacr_read(); 227 cpacr &= ~__BITS(21,20); 228 cpacr &= ~__BITS(23,22); 229 230 cpacr |= __SHIFTIN(1, __BITS(21,20)); 231 cpacr |= __SHIFTIN(1, __BITS(23,22)); 232 armreg_cpacr_write(cpacr); 233 cpacr = armreg_cpacr_read(); 234 if ((cpacr & __BITS(23,22)) == 0) { 235 aprint_normal_dev(ci->ci_dev, "NEON not present\n"); 236 } 237 if ((cpacr & __BITS(21,20)) == 0) { 238 aprint_normal_dev(ci->ci_dev, "VFP not present\n"); 239 } 240#endif 241 242 const uint32_t fpsid = read_fpsid(); 243 244 remove_coproc_handler(uh); 245 246 if (undefined_test != 0) { 247 aprint_normal_dev(ci->ci_dev, "No VFP detected\n"); 248 install_coproc_handler(VFP_COPROC, vfp_fpscr_handler); 249 ci->ci_vfp_id = 0; 250 return; 251 } 252 253 ci->ci_vfp_id = fpsid; 254 switch (fpsid & ~ VFP_FPSID_REV_MSK) { 255 case FPU_VFP10_ARM10E: 256 model = "VFP10 R1"; 257 break; 258 case FPU_VFP11_ARM11: 259 model = "VFP11"; 260 break; 261 case FPU_VFP30_CORTEXA9: 262 model = "NEON MPE w/ VFP 3.0"; 263 break; 264 default: 265 aprint_normal_dev(ci->ci_dev, "unrecognized VFP version %x\n", 266 fpsid); 267 install_coproc_handler(VFP_COPROC, vfp_fpscr_handler); 268 return; 269 } 270 271 if (fpsid != 0) { 272 aprint_normal("vfp%d at %s: %s\n", 273 device_unit(curcpu()->ci_dev), device_xname(curcpu()->ci_dev), 274 model); 275 } 276 evcnt_attach_dynamic(&vfpevent_use, EVCNT_TYPE_MISC, NULL, 277 "VFP", "proc use"); 278 evcnt_attach_dynamic(&vfpevent_reuse, EVCNT_TYPE_MISC, NULL, 279 "VFP", "proc re-use"); 280 install_coproc_handler(VFP_COPROC, vfp_handler); 281 install_coproc_handler(VFP_COPROC2, vfp_handler); 282} 283 284/* The real handler for VFP bounces. */ 285static int 286vfp_handler(u_int address, u_int insn, trapframe_t *frame, 287 int fault_code) 288{ 289 struct cpu_info * const ci = curcpu(); 290 291 /* This shouldn't ever happen. */ 292 if (fault_code != FAULT_USER) 293 panic("VFP fault in non-user mode"); 294 295 if (ci->ci_vfp_id == 0) 296 /* No VFP detected, just fault. */ 297 return 1; 298 299 /* 300 * If we are just changing/fetching FPSCR, don't bother loading it. 301 */ 302 if (!vfp_fpscr_handler(address, insn, frame, fault_code)) 303 return 0; 304 305 pcu_load(&arm_vfp_ops); 306 307 /* Need to restart the faulted instruction. */ 308// frame->tf_pc -= INSN_SIZE; 309 return 0; 310} 311 312static void 313vfp_state_load(lwp_t *l, bool used) 314{ 315 struct pcb * const pcb = lwp_getpcb(l); 316 struct vfpreg * const fregs = &pcb->pcb_vfp; 317 318 /* 319 * Instrument VFP usage -- if a process has not previously 320 * used the VFP, mark it as having used VFP for the first time, 321 * and count this event. 322 * 323 * If a process has used the VFP, count a "used VFP, and took 324 * a trap to use it again" event. 325 */ 326 if (__predict_false((l->l_md.md_flags & MDLWP_VFPUSED) == 0)) { 327 vfpevent_use.ev_count++; 328 l->l_md.md_flags |= MDLWP_VFPUSED; 329 pcb->pcb_vfp.vfp_fpscr = 330 (VFP_FPSCR_DN | VFP_FPSCR_FZ); /* Runfast */ 331 } else { 332 vfpevent_reuse.ev_count++; 333 } 334 335 if (fregs->vfp_fpexc & VFP_FPEXC_EN) { 336 /* 337 * If we think the VFP is enabled, it must have be disabled by 338 * vfp_state_release for another LWP so we can just restore 339 * FPEXC and return since our VFP state is still loaded. 340 */ 341 write_fpexc(fregs->vfp_fpexc); 342 return; 343 } 344 345 /* Enable the VFP (so that we can write the registers). */ 346 uint32_t fpexc = read_fpexc(); 347 KDASSERT((fpexc & VFP_FPEXC_EX) == 0); 348 write_fpexc(fpexc | VFP_FPEXC_EN); 349 350 load_vfpregs(fregs->vfp_regs); 351 write_fpscr(fregs->vfp_fpscr); 352 353 if (fregs->vfp_fpexc & VFP_FPEXC_EX) { 354 struct cpu_info * const ci = curcpu(); 355 /* Need to restore the exception handling state. */ 356 switch (ci->ci_vfp_id) { 357 case FPU_VFP10_ARM10E: 358 case FPU_VFP11_ARM11: 359 write_fpinst2(fregs->vfp_fpinst2); 360 write_fpinst(fregs->vfp_fpinst); 361 break; 362 default: 363 panic("%s: Unsupported VFP %#x", 364 __func__, ci->ci_vfp_id); 365 } 366 } 367 368 /* Finally, restore the FPEXC but don't enable the VFP. */ 369 fregs->vfp_fpexc |= VFP_FPEXC_EN; 370 write_fpexc(fregs->vfp_fpexc); 371} 372 373void 374vfp_state_save(lwp_t *l) 375{ 376 struct pcb * const pcb = lwp_getpcb(l); 377 struct vfpreg * const fregs = &pcb->pcb_vfp; 378 379 /* 380 * If it's already disabled, then the state has been saved 381 * (or discarded). 382 */ 383 if ((fregs->vfp_fpexc & VFP_FPEXC_EN) == 0) 384 return; 385 386 /* 387 * Enable the VFP (so we can read the registers). 388 * Make sure the exception bit is cleared so that we can 389 * safely dump the registers. 390 */ 391 uint32_t fpexc = read_fpexc(); 392 write_fpexc((fpexc | VFP_FPEXC_EN) & ~VFP_FPEXC_EX); 393 394 fregs->vfp_fpexc = fpexc; 395 if (fpexc & VFP_FPEXC_EX) { 396 struct cpu_info * const ci = curcpu(); 397 /* Need to save the exception handling state */ 398 switch (ci->ci_vfp_id) { 399 case FPU_VFP10_ARM10E: 400 case FPU_VFP11_ARM11: 401 fregs->vfp_fpinst = read_fpinst(); 402 fregs->vfp_fpinst2 = read_fpinst2(); 403 break; 404 default: 405 panic("%s: Unsupported VFP %#x", 406 __func__, ci->ci_vfp_id); 407 } 408 } 409 fregs->vfp_fpscr = read_fpscr(); 410 save_vfpregs(fregs->vfp_regs); 411 412 /* Disable the VFP. */ 413 write_fpexc(fpexc); 414} 415 416void 417vfp_state_release(lwp_t *l) 418{ 419 struct pcb * const pcb = lwp_getpcb(l); 420 421 /* 422 * Now mark the VFP as disabled (and our state has been already 423 * saved or is being discarded). 424 */ 425 pcb->pcb_vfp.vfp_fpexc &= ~VFP_FPEXC_EN; 426 427 /* 428 * Turn off the FPU so the next time a VFP instruction is issued 429 * an exception happens. We don't know if this LWP's state was 430 * loaded but if we turned off the FPU for some other LWP, when 431 * pcu_load invokes vfp_state_load it will see that VFP_FPEXC_EN 432 * is still set so it just restroe fpexc and return since its 433 * contents are still sitting in the VFP. 434 */ 435 write_fpexc(read_fpexc() & ~VFP_FPEXC_EN); 436} 437 438void 439vfp_savecontext(void) 440{ 441 pcu_save(&arm_vfp_ops); 442} 443 444void 445vfp_discardcontext(void) 446{ 447 pcu_discard(&arm_vfp_ops); 448} 449 450#endif /* FPU_VFP */ 451