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