1 /* $NetBSD: vfp_init.c,v 1.22 2013/08/03 20:16:44 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 #include <arm/mcontext.h> 43 44 #include <uvm/uvm_extern.h> /* for pmap.h */ 45 46 extern int cpu_media_and_vfp_features[]; 47 extern int cpu_neon_present; 48 49 #ifdef FPU_VFP 50 51 /* FLDMD <X>, {d0-d15} */ 52 static inline void 53 load_vfpregs_lo(const uint64_t *p) 54 { 55 /* vldmia rN, {d0-d15} */ 56 __asm __volatile("ldc\tp11, c0, [%0], {32}" :: "r" (p) : "memory"); 57 } 58 59 /* FSTMD <X>, {d0-d15} */ 60 static inline void 61 save_vfpregs_lo(uint64_t *p) 62 { 63 __asm __volatile("stc\tp11, c0, [%0], {32}" :: "r" (p) : "memory"); 64 } 65 66 #ifdef CPU_CORTEX 67 /* FLDMD <X>, {d16-d31} */ 68 static inline void 69 load_vfpregs_hi(const uint64_t *p) 70 { 71 __asm __volatile("ldcl\tp11, c0, [%0], {32}" :: "r" (&p[16]) : "memory"); 72 } 73 74 /* FLDMD <X>, {d16-d31} */ 75 static inline void 76 save_vfpregs_hi(uint64_t *p) 77 { 78 __asm __volatile("stcl\tp11, c0, [%0], {32}" :: "r" (&p[16]) : "memory"); 79 } 80 #endif 81 82 static inline void 83 load_vfpregs(const struct vfpreg *fregs) 84 { 85 load_vfpregs_lo(fregs->vfp_regs); 86 #ifdef CPU_CORTEX 87 #ifdef CPU_ARM11 88 switch (curcpu()->ci_vfp_id) { 89 case FPU_VFP_CORTEXA5: 90 case FPU_VFP_CORTEXA7: 91 case FPU_VFP_CORTEXA8: 92 case FPU_VFP_CORTEXA9: 93 case FPU_VFP_CORTEXA15: 94 #endif 95 load_vfpregs_hi(fregs->vfp_regs); 96 #ifdef CPU_ARM11 97 break; 98 } 99 #endif 100 #endif 101 } 102 103 static inline void 104 save_vfpregs(struct vfpreg *fregs) 105 { 106 save_vfpregs_lo(fregs->vfp_regs); 107 #ifdef CPU_CORTEX 108 #ifdef CPU_ARM11 109 switch (curcpu()->ci_vfp_id) { 110 case FPU_VFP_CORTEXA5: 111 case FPU_VFP_CORTEXA7: 112 case FPU_VFP_CORTEXA8: 113 case FPU_VFP_CORTEXA9: 114 case FPU_VFP_CORTEXA15: 115 #endif 116 save_vfpregs_hi(fregs->vfp_regs); 117 #ifdef CPU_ARM11 118 break; 119 } 120 #endif 121 #endif 122 } 123 124 /* The real handler for VFP bounces. */ 125 static int vfp_handler(u_int, u_int, trapframe_t *, int); 126 #ifdef CPU_CORTEX 127 static int neon_handler(u_int, u_int, trapframe_t *, int); 128 #endif 129 130 static void vfp_state_load(lwp_t *, u_int); 131 static void vfp_state_save(lwp_t *, u_int); 132 static void vfp_state_release(lwp_t *, u_int); 133 134 const pcu_ops_t arm_vfp_ops = { 135 .pcu_id = PCU_FPU, 136 .pcu_state_save = vfp_state_save, 137 .pcu_state_load = vfp_state_load, 138 .pcu_state_release = vfp_state_release, 139 }; 140 141 struct evcnt vfpevent_use; 142 struct evcnt vfpevent_reuse; 143 struct evcnt vfpevent_fpe; 144 145 /* 146 * Used to test for a VFP. The following function is installed as a coproc10 147 * handler on the undefined instruction vector and then we issue a VFP 148 * instruction. If undefined_test is non zero then the VFP did not handle 149 * the instruction so must be absent, or disabled. 150 */ 151 152 static int undefined_test; 153 154 static int 155 vfp_test(u_int address, u_int insn, trapframe_t *frame, int fault_code) 156 { 157 158 frame->tf_pc += INSN_SIZE; 159 ++undefined_test; 160 return 0; 161 } 162 163 #endif /* FPU_VFP */ 164 165 struct evcnt vfp_fpscr_ev = 166 EVCNT_INITIALIZER(EVCNT_TYPE_TRAP, NULL, "VFP", "FPSCR traps"); 167 EVCNT_ATTACH_STATIC(vfp_fpscr_ev); 168 169 static int 170 vfp_fpscr_handler(u_int address, u_int insn, trapframe_t *frame, int fault_code) 171 { 172 struct lwp * const l = curlwp; 173 const u_int regno = (insn >> 12) & 0xf; 174 /* 175 * Only match move to/from the FPSCR register and we 176 * can't be using the SP,LR,PC as a source. 177 */ 178 if ((insn & 0xffef0fff) != 0xeee10a10 || regno > 12) 179 return 1; 180 181 struct pcb * const pcb = lwp_getpcb(l); 182 183 #ifdef FPU_VFP 184 /* 185 * If FPU is valid somewhere, let's just reenable VFP and 186 * retry the instruction (only safe thing to do since the 187 * pcb has a stale copy). 188 */ 189 if (pcb->pcb_vfp.vfp_fpexc & VFP_FPEXC_EN) 190 return 1; 191 #endif 192 193 if (__predict_false((l->l_md.md_flags & MDLWP_VFPUSED) == 0)) { 194 l->l_md.md_flags |= MDLWP_VFPUSED; 195 pcb->pcb_vfp.vfp_fpscr = 196 (VFP_FPSCR_DN | VFP_FPSCR_FZ); /* Runfast */ 197 } 198 199 /* 200 * We know know the pcb has the saved copy. 201 */ 202 register_t * const regp = &frame->tf_r0 + regno; 203 if (insn & 0x00100000) { 204 *regp = pcb->pcb_vfp.vfp_fpscr; 205 } else { 206 register_t tmp = *regp; 207 if (!(cpu_media_and_vfp_features[0] & ARM_MVFR0_EXCEPT_MASK)) 208 tmp &= ~VFP_FPSCR_ESUM; 209 pcb->pcb_vfp.vfp_fpscr = tmp; 210 } 211 212 vfp_fpscr_ev.ev_count++; 213 214 frame->tf_pc += INSN_SIZE; 215 return 0; 216 } 217 218 #ifndef FPU_VFP 219 /* 220 * If we don't want VFP support, we still need to handle emulating VFP FPSCR 221 * instructions. 222 */ 223 void 224 vfp_attach(void) 225 { 226 install_coproc_handler(VFP_COPROC, vfp_fpscr_handler); 227 } 228 229 #else 230 #if 0 231 static bool 232 vfp_patch_branch(uintptr_t code, uintptr_t func, uintptr_t newfunc) 233 { 234 for (;; code += sizeof(uint32_t)) { 235 uint32_t insn = *(uint32_t *)code; 236 if ((insn & 0xffd08000) == 0xe8908000) /* ldm ... { pc } */ 237 return false; 238 if ((insn & 0xfffffff0) == 0xe12fff10) /* bx rN */ 239 return false; 240 if ((insn & 0xf1a0f000) == 0xe1a0f000) /* mov pc, ... */ 241 return false; 242 if ((insn >> 25) != 0x75) /* not b/bl insn */ 243 continue; 244 intptr_t imm26 = ((int32_t)insn << 8) >> 6; 245 if (code + imm26 + 8 == func) { 246 int32_t imm24 = (newfunc - (code + 8)) >> 2; 247 uint32_t new_insn = (insn & 0xff000000) 248 | (imm24 & 0xffffff); 249 KASSERTMSG((uint32_t)((imm24 >> 24) + 1) <= 1, "%x", 250 ((imm24 >> 24) + 1)); 251 *(uint32_t *)code = new_insn; 252 cpu_idcache_wbinv_range(code, sizeof(uint32_t)); 253 return true; 254 } 255 } 256 } 257 #endif 258 259 void 260 vfp_attach(void) 261 { 262 struct cpu_info * const ci = curcpu(); 263 const char *model = NULL; 264 bool vfp_p = false; 265 266 if (CPU_ID_ARM11_P(curcpu()->ci_arm_cpuid) 267 || CPU_ID_CORTEX_P(curcpu()->ci_arm_cpuid)) { 268 const uint32_t cpacr_vfp = CPACR_CPn(VFP_COPROC); 269 const uint32_t cpacr_vfp2 = CPACR_CPn(VFP_COPROC2); 270 271 /* 272 * We first need to enable access to the coprocessors. 273 */ 274 uint32_t cpacr = armreg_cpacr_read(); 275 cpacr |= __SHIFTIN(CPACR_ALL, cpacr_vfp); 276 cpacr |= __SHIFTIN(CPACR_ALL, cpacr_vfp2); 277 #if 0 278 if (CPU_ID_CORTEX_P(curcpu()->ci_arm_cpuid)) { 279 /* 280 * Disable access to the upper 16 FP registers and NEON. 281 */ 282 cpacr |= CPACR_V7_D32DIS; 283 cpacr |= CPACR_V7_ASEDIS; 284 } 285 #endif 286 armreg_cpacr_write(cpacr); 287 288 /* 289 * If we could enable them, then they exist. 290 */ 291 cpacr = armreg_cpacr_read(); 292 vfp_p = __SHIFTOUT(cpacr, cpacr_vfp2) != CPACR_NOACCESS 293 || __SHIFTOUT(cpacr, cpacr_vfp) != CPACR_NOACCESS; 294 } 295 296 void *uh = install_coproc_handler(VFP_COPROC, vfp_test); 297 298 undefined_test = 0; 299 300 const uint32_t fpsid = armreg_fpsid_read(); 301 302 remove_coproc_handler(uh); 303 304 if (undefined_test != 0) { 305 aprint_normal_dev(ci->ci_dev, "No VFP detected\n"); 306 install_coproc_handler(VFP_COPROC, vfp_fpscr_handler); 307 ci->ci_vfp_id = 0; 308 return; 309 } 310 311 ci->ci_vfp_id = fpsid; 312 switch (fpsid & ~ VFP_FPSID_REV_MSK) { 313 case FPU_VFP10_ARM10E: 314 model = "VFP10 R1"; 315 break; 316 case FPU_VFP11_ARM11: 317 model = "VFP11"; 318 break; 319 case FPU_VFP_CORTEXA5: 320 case FPU_VFP_CORTEXA7: 321 case FPU_VFP_CORTEXA8: 322 case FPU_VFP_CORTEXA9: 323 case FPU_VFP_CORTEXA15: 324 model = "NEON MPE (VFP 3.0+)"; 325 cpu_neon_present = 1; 326 break; 327 default: 328 aprint_normal_dev(ci->ci_dev, "unrecognized VFP version %x\n", 329 fpsid); 330 install_coproc_handler(VFP_COPROC, vfp_fpscr_handler); 331 return; 332 } 333 334 cpu_fpu_present = 1; 335 cpu_media_and_vfp_features[0] = armreg_mvfr0_read(); 336 cpu_media_and_vfp_features[1] = armreg_mvfr1_read(); 337 if (fpsid != 0) { 338 aprint_normal("vfp%d at %s: %s\n", 339 device_unit(curcpu()->ci_dev), 340 device_xname(curcpu()->ci_dev), 341 model); 342 aprint_verbose("vfp%d: mvfr: [0]=%#x [1]=%#x\n", 343 device_unit(curcpu()->ci_dev), 344 cpu_media_and_vfp_features[0], 345 cpu_media_and_vfp_features[1]); 346 } 347 evcnt_attach_dynamic(&vfpevent_use, EVCNT_TYPE_MISC, NULL, 348 "VFP", "coproc use"); 349 evcnt_attach_dynamic(&vfpevent_reuse, EVCNT_TYPE_MISC, NULL, 350 "VFP", "coproc re-use"); 351 evcnt_attach_dynamic(&vfpevent_fpe, EVCNT_TYPE_TRAP, NULL, 352 "VFP", "coproc fault"); 353 install_coproc_handler(VFP_COPROC, vfp_handler); 354 install_coproc_handler(VFP_COPROC2, vfp_handler); 355 #ifdef CPU_CORTEX 356 install_coproc_handler(CORE_UNKNOWN_HANDLER, neon_handler); 357 #endif 358 359 #if 0 360 vfp_patch_branch((uintptr_t)pmap_copy_page_generic, 361 (uintptr_t)bcopy_page, (uintptr_t)bcopy_page_vfp); 362 vfp_patch_branch((uintptr_t)pmap_zero_page_generic, 363 (uintptr_t)bzero_page, (uintptr_t)bzero_page_vfp); 364 #endif 365 } 366 367 /* The real handler for VFP bounces. */ 368 static int 369 vfp_handler(u_int address, u_int insn, trapframe_t *frame, int fault_code) 370 { 371 struct cpu_info * const ci = curcpu(); 372 373 /* This shouldn't ever happen. */ 374 if (fault_code != FAULT_USER) 375 panic("VFP fault at %#x in non-user mode", frame->tf_pc); 376 377 if (ci->ci_vfp_id == 0) 378 /* No VFP detected, just fault. */ 379 return 1; 380 381 uint32_t fpexc = armreg_fpexc_read(); 382 if (fpexc & VFP_FPEXC_EX) { 383 ksiginfo_t ksi; 384 KASSERT(fpexc & VFP_FPEXC_EN); 385 386 vfpevent_fpe.ev_count++; 387 388 pcu_save(&arm_vfp_ops); 389 390 /* 391 * Need the clear the exception condition so any signal 392 * can run. 393 */ 394 armreg_fpexc_write(fpexc & ~(VFP_FPEXC_EX|VFP_FPEXE_FSUM)); 395 396 KSI_INIT_TRAP(&ksi); 397 ksi.ksi_signo = SIGFPE; 398 if (fpexc & VFP_FPEXC_IXF) 399 ksi.ksi_code = FPE_FLTRES; 400 else if (fpexc & VFP_FPEXC_UFF) 401 ksi.ksi_code = FPE_FLTUND; 402 else if (fpexc & VFP_FPEXC_OFF) 403 ksi.ksi_code = FPE_FLTOVF; 404 else if (fpexc & VFP_FPEXC_DZF) 405 ksi.ksi_code = FPE_FLTDIV; 406 else if (fpexc & VFP_FPEXC_IOF) 407 ksi.ksi_code = FPE_FLTINV; 408 ksi.ksi_addr = (uint32_t *)address; 409 ksi.ksi_trap = 0; 410 trapsignal(curlwp, &ksi); 411 return 0; 412 } 413 414 /* 415 * If we are just changing/fetching FPSCR, don't bother loading it. 416 */ 417 if (!vfp_fpscr_handler(address, insn, frame, fault_code)) 418 return 0; 419 420 pcu_load(&arm_vfp_ops); 421 422 /* Need to restart the faulted instruction. */ 423 // frame->tf_pc -= INSN_SIZE; 424 return 0; 425 } 426 427 #ifdef CPU_CORTEX 428 /* The real handler for NEON bounces. */ 429 static int 430 neon_handler(u_int address, u_int insn, trapframe_t *frame, int fault_code) 431 { 432 struct cpu_info * const ci = curcpu(); 433 434 if (ci->ci_vfp_id == 0) 435 /* No VFP detected, just fault. */ 436 return 1; 437 438 if ((insn & 0xfe000000) != 0xf2000000 439 && (insn & 0xfe000000) != 0xf4000000) 440 /* Not NEON instruction, just fault. */ 441 return 1; 442 443 /* This shouldn't ever happen. */ 444 if (fault_code != FAULT_USER) 445 panic("NEON fault in non-user mode"); 446 447 pcu_load(&arm_vfp_ops); 448 449 /* Need to restart the faulted instruction. */ 450 // frame->tf_pc -= INSN_SIZE; 451 return 0; 452 } 453 #endif 454 455 static void 456 vfp_state_load(lwp_t *l, u_int flags) 457 { 458 struct pcb * const pcb = lwp_getpcb(l); 459 460 KASSERT(flags & PCU_ENABLE); 461 462 if (flags & PCU_KERNEL) { 463 if ((flags & PCU_LOADED) == 0) { 464 pcb->pcb_kernel_vfp.vfp_fpexc = pcb->pcb_vfp.vfp_fpexc; 465 } 466 pcb->pcb_vfp.vfp_fpexc = VFP_FPEXC_EN; 467 armreg_fpexc_write(pcb->pcb_vfp.vfp_fpexc); 468 /* 469 * Load the kernel registers (just the first 16) if they've 470 * been used.. 471 */ 472 if (flags & PCU_LOADED) { 473 load_vfpregs_lo(pcb->pcb_kernel_vfp.vfp_regs); 474 } 475 return; 476 } 477 struct vfpreg * const fregs = &pcb->pcb_vfp; 478 479 /* 480 * Instrument VFP usage -- if a process has not previously 481 * used the VFP, mark it as having used VFP for the first time, 482 * and count this event. 483 * 484 * If a process has used the VFP, count a "used VFP, and took 485 * a trap to use it again" event. 486 */ 487 if (__predict_false((l->l_md.md_flags & MDLWP_VFPUSED) == 0)) { 488 vfpevent_use.ev_count++; 489 l->l_md.md_flags |= MDLWP_VFPUSED; 490 pcb->pcb_vfp.vfp_fpscr = /* Runfast */ 491 (VFP_FPSCR_DN | VFP_FPSCR_FZ | VFP_FPSCR_RN); 492 } else { 493 vfpevent_reuse.ev_count++; 494 } 495 496 if (fregs->vfp_fpexc & VFP_FPEXC_EN) { 497 /* 498 * If we think the VFP is enabled, it must have be disabled by 499 * vfp_state_release for another LWP so we can just restore 500 * FPEXC and return since our VFP state is still loaded. 501 */ 502 armreg_fpexc_write(fregs->vfp_fpexc); 503 return; 504 } 505 506 /* Load and Enable the VFP (so that we can write the registers). */ 507 if (flags & PCU_RELOAD) { 508 uint32_t fpexc = armreg_fpexc_read(); 509 KDASSERT((fpexc & VFP_FPEXC_EX) == 0); 510 armreg_fpexc_write(fpexc | VFP_FPEXC_EN); 511 512 load_vfpregs(fregs); 513 armreg_fpscr_write(fregs->vfp_fpscr); 514 515 if (fregs->vfp_fpexc & VFP_FPEXC_EX) { 516 /* Need to restore the exception handling state. */ 517 armreg_fpinst2_write(fregs->vfp_fpinst2); 518 if (fregs->vfp_fpexc & VFP_FPEXC_FP2V) 519 armreg_fpinst_write(fregs->vfp_fpinst); 520 } 521 } 522 523 /* Finally, restore the FPEXC but don't enable the VFP. */ 524 fregs->vfp_fpexc |= VFP_FPEXC_EN; 525 armreg_fpexc_write(fregs->vfp_fpexc); 526 } 527 528 void 529 vfp_state_save(lwp_t *l, u_int flags) 530 { 531 struct pcb * const pcb = lwp_getpcb(l); 532 uint32_t fpexc = armreg_fpexc_read(); 533 armreg_fpexc_write((fpexc | VFP_FPEXC_EN) & ~VFP_FPEXC_EX); 534 535 if (flags & PCU_KERNEL) { 536 /* 537 * Save the kernel set of VFP registers. 538 * (just the first 16). 539 */ 540 save_vfpregs_lo(pcb->pcb_kernel_vfp.vfp_regs); 541 return; 542 } 543 544 struct vfpreg * const fregs = &pcb->pcb_vfp; 545 546 /* 547 * Enable the VFP (so we can read the registers). 548 * Make sure the exception bit is cleared so that we can 549 * safely dump the registers. 550 */ 551 fregs->vfp_fpexc = fpexc; 552 if (fpexc & VFP_FPEXC_EX) { 553 /* Need to save the exception handling state */ 554 fregs->vfp_fpinst = armreg_fpinst_read(); 555 if (fpexc & VFP_FPEXC_FP2V) 556 fregs->vfp_fpinst2 = armreg_fpinst2_read(); 557 } 558 fregs->vfp_fpscr = armreg_fpscr_read(); 559 save_vfpregs(fregs); 560 561 /* Disable the VFP. */ 562 armreg_fpexc_write(fpexc); 563 } 564 565 void 566 vfp_state_release(lwp_t *l, u_int flags) 567 { 568 struct pcb * const pcb = lwp_getpcb(l); 569 570 if (flags & PCU_KERNEL) { 571 /* 572 * Restore the FPEXC since we borrowed that field. 573 */ 574 pcb->pcb_vfp.vfp_fpexc = pcb->pcb_kernel_vfp.vfp_fpexc; 575 } else { 576 /* 577 * Now mark the VFP as disabled (and our state 578 * has been already saved or is being discarded). 579 */ 580 pcb->pcb_vfp.vfp_fpexc &= ~VFP_FPEXC_EN; 581 } 582 583 /* 584 * Turn off the FPU so the next time a VFP instruction is issued 585 * an exception happens. We don't know if this LWP's state was 586 * loaded but if we turned off the FPU for some other LWP, when 587 * pcu_load invokes vfp_state_load it will see that VFP_FPEXC_EN 588 * is still set so it just restore fpexc and return since its 589 * contents are still sitting in the VFP. 590 */ 591 armreg_fpexc_write(armreg_fpexc_read() & ~VFP_FPEXC_EN); 592 } 593 594 void 595 vfp_savecontext(void) 596 { 597 pcu_save(&arm_vfp_ops); 598 } 599 600 void 601 vfp_discardcontext(void) 602 { 603 pcu_discard(&arm_vfp_ops); 604 } 605 606 void 607 vfp_kernel_acquire(void) 608 { 609 if (__predict_false(cpu_intr_p())) { 610 armreg_fpexc_write(VFP_FPEXC_EN); 611 if (curcpu()->ci_data.cpu_pcu_curlwp[PCU_FPU] != NULL) { 612 lwp_t * const l = curlwp; 613 struct pcb * const pcb = lwp_getpcb(l); 614 KASSERT((l->l_md.md_flags & MDLWP_VFPINTR) == 0); 615 l->l_md.md_flags |= MDLWP_VFPINTR; 616 save_vfpregs_lo(&pcb->pcb_kernel_vfp.vfp_regs[16]); 617 } 618 } else { 619 pcu_kernel_acquire(&arm_vfp_ops); 620 } 621 } 622 623 void 624 vfp_kernel_release(void) 625 { 626 if (__predict_false(cpu_intr_p())) { 627 uint32_t fpexc = 0; 628 if (curcpu()->ci_data.cpu_pcu_curlwp[PCU_FPU] != NULL) { 629 lwp_t * const l = curlwp; 630 struct pcb * const pcb = lwp_getpcb(l); 631 KASSERT(l->l_md.md_flags & MDLWP_VFPINTR); 632 load_vfpregs_lo(&pcb->pcb_kernel_vfp.vfp_regs[16]); 633 l->l_md.md_flags &= ~MDLWP_VFPINTR; 634 fpexc = pcb->pcb_vfp.vfp_fpexc; 635 } 636 armreg_fpexc_write(fpexc); 637 } else { 638 pcu_kernel_release(&arm_vfp_ops); 639 } 640 } 641 642 void 643 vfp_getcontext(struct lwp *l, mcontext_t *mcp, int *flagsp) 644 { 645 if (l->l_md.md_flags & MDLWP_VFPUSED) { 646 const struct pcb * const pcb = lwp_getpcb(l); 647 pcu_save(&arm_vfp_ops); 648 mcp->__fpu.__vfpregs.__vfp_fpscr = pcb->pcb_vfp.vfp_fpscr; 649 memcpy(mcp->__fpu.__vfpregs.__vfp_fstmx, pcb->pcb_vfp.vfp_regs, 650 sizeof(mcp->__fpu.__vfpregs.__vfp_fstmx)); 651 *flagsp |= _UC_FPU|_UC_ARM_VFP; 652 } 653 } 654 655 void 656 vfp_setcontext(struct lwp *l, const mcontext_t *mcp) 657 { 658 pcu_discard(&arm_vfp_ops); 659 struct pcb * const pcb = lwp_getpcb(l); 660 l->l_md.md_flags |= MDLWP_VFPUSED; 661 pcb->pcb_vfp.vfp_fpscr = mcp->__fpu.__vfpregs.__vfp_fpscr; 662 memcpy(pcb->pcb_vfp.vfp_regs, mcp->__fpu.__vfpregs.__vfp_fstmx, 663 sizeof(mcp->__fpu.__vfpregs.__vfp_fstmx)); 664 } 665 666 #endif /* FPU_VFP */ 667