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