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