diff --git a/sys/arm64/arm64/mp_machdep.c b/sys/arm64/arm64/mp_machdep.c --- a/sys/arm64/arm64/mp_machdep.c +++ b/sys/arm64/arm64/mp_machdep.c @@ -274,7 +274,7 @@ 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 --- a/sys/arm64/arm64/vfp.c +++ b/sys/arm64/arm64/vfp.c @@ -38,6 +38,8 @@ #include #include +#include + #include #include #include @@ -57,6 +59,9 @@ struct vfpstate state; }; +static uma_zone_t fpu_save_area_zone; +static struct vfpstate *fpu_initialstate; + void vfp_enable(void) { @@ -280,7 +285,7 @@ } void -vfp_init(void) +vfp_init_secondary(void) { uint64_t pfr; @@ -291,9 +296,34 @@ /* 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 @@ 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 --- a/sys/arm64/include/vfp.h +++ b/sys/arm64/include/vfp.h @@ -66,7 +66,7 @@ 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_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)