Add helpers to allocate an arm64 VFP state struct

This will be used by bhyve and will allow the size to change, e.g. for SVE.

Reviewed by:	markj
Sponsored by:	Arm Ltd
Differential Revision:	https://reviews.freebsd.org/D40131

3 files changed, 60 insertions, 5 deletions

diff --git a/sys/arm64/arm64/mp_machdep.c b/sys/arm64/arm64/mp_machdep.c
index babeee331be0..9aef6a9a080f 100644
--- a/sys/arm64/arm64/mp_machdep.c
+++ b/sys/arm64/arm64/mp_machdep.c
@@ -274,7 +274,7 @@ init_secondary(uint64_t cpu)
 	cpu_initclocks_ap();
 
 #ifdef VFP
-	vfp_init();
+	vfp_init_secondary();
 #endif
 
 	dbg_init();
diff --git a/sys/arm64/arm64/vfp.c b/sys/arm64/arm64/vfp.c
index 22b15ee78596..46ae5d8e8fcd 100644
--- a/sys/arm64/arm64/vfp.c
+++ b/sys/arm64/arm64/vfp.c
@@ -38,6 +38,8 @@ __FBSDID("$FreeBSD$");
 #include <sys/pcpu.h>
 #include <sys/proc.h>
 
+#include <vm/uma.h>
+
 #include <machine/armreg.h>
 #include <machine/md_var.h>
 #include <machine/pcb.h>
@@ -57,6 +59,9 @@ struct fpu_kern_ctx {
 	struct vfpstate	 state;
 };
 
+static uma_zone_t fpu_save_area_zone;
+static struct vfpstate *fpu_initialstate;
+
 void
 vfp_enable(void)
 {
@@ -280,7 +285,7 @@ vfp_restore_state(void)
 }
 
 void
-vfp_init(void)
+vfp_init_secondary(void)
 {
 	uint64_t pfr;
 
@@ -291,9 +296,34 @@ vfp_init(void)
 
 	/* Disable to be enabled when it's used */
 	vfp_disable();
+}
+
+static void
+vfp_init(const void *dummy __unused)
+{
+	uint64_t pfr;
+
+	/* Check if there is a vfp unit present */
+	pfr = READ_SPECIALREG(id_aa64pfr0_el1);
+	if ((pfr & ID_AA64PFR0_FP_MASK) == ID_AA64PFR0_FP_NONE)
+		return;
+
+	fpu_save_area_zone = uma_zcreate("VFP_save_area",
+	    sizeof(struct vfpstate), NULL, NULL, NULL, NULL,
+	    _Alignof(struct vfpstate) - 1, 0);
+	fpu_initialstate = uma_zalloc(fpu_save_area_zone, M_WAITOK | M_ZERO);
 
-	if (PCPU_GET(cpuid) == 0)
-		thread0.td_pcb->pcb_fpusaved->vfp_fpcr = VFPCR_INIT;
+	/* Ensure the VFP is enabled before accessing it in vfp_store */
+	vfp_enable();
+	vfp_store(fpu_initialstate);
+
+	/* Disable to be enabled when it's used */
+	vfp_disable();
+
+	/* Zero the VFP registers but keep fpcr and fpsr */
+	bzero(fpu_initialstate->vfp_regs, sizeof(fpu_initialstate->vfp_regs));
+
+	thread0.td_pcb->pcb_fpusaved->vfp_fpcr = VFPCR_INIT;
 }
 
 SYSINIT(vfp, SI_SUB_CPU, SI_ORDER_ANY, vfp_init, NULL);
@@ -433,4 +463,25 @@ is_fpu_kern_thread(u_int flags __unused)
 	curpcb = curthread->td_pcb;
 	return ((curpcb->pcb_fpflags & PCB_FP_KERN) != 0);
 }
+
+/*
+ * FPU save area alloc/free/init utility routines
+ */
+struct vfpstate *
+fpu_save_area_alloc(void)
+{
+	return (uma_zalloc(fpu_save_area_zone, M_WAITOK));
+}
+
+void
+fpu_save_area_free(struct vfpstate *fsa)
+{
+	uma_zfree(fpu_save_area_zone, fsa);
+}
+
+void
+fpu_save_area_reset(struct vfpstate *fsa)
+{
+	memcpy(fsa, fpu_initialstate, sizeof(*fsa));
+}
 #endif
diff --git a/sys/arm64/include/vfp.h b/sys/arm64/include/vfp.h
index 4f64b0396417..79e710694268 100644
--- a/sys/arm64/include/vfp.h
+++ b/sys/arm64/include/vfp.h
@@ -66,7 +66,7 @@ struct vfpstate {
 struct pcb;
 struct thread;
 
-void	vfp_init(void);
+void	vfp_init_secondary(void);
 void	vfp_enable(void);
 void	vfp_disable(void);
 void	vfp_discard(struct thread *);
@@ -94,6 +94,10 @@ int fpu_kern_leave(struct thread *, struct fpu_kern_ctx *);
 int fpu_kern_thread(u_int);
 int is_fpu_kern_thread(u_int);
 
+struct vfpstate *fpu_save_area_alloc(void);
+void fpu_save_area_free(struct vfpstate *fsa);
+void fpu_save_area_reset(struct vfpstate *fsa);
+
 /* Convert to and from Aarch32 FPSCR to Aarch64 FPCR/FPSR */
 #define VFP_FPSCR_FROM_SRCR(vpsr, vpcr) ((vpsr) | ((vpcr) & 0x7c00000))
 #define VFP_FPSR_FROM_FPSCR(vpscr) ((vpscr) &~ 0x7c00000)