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