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) { @@ -291,12 +296,37 @@ /* Disable to be enabled when it's used */ vfp_disable(); +} + +static void +vfp_sysinit(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); + + /* 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(); - if (PCPU_GET(cpuid) == 0) - thread0.td_pcb->pcb_fpusaved->vfp_fpcr = VFPCR_INIT; + /* 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); +SYSINIT(vfp, SI_SUB_CPU, SI_ORDER_ANY, vfp_sysinit, NULL); struct fpu_kern_ctx * fpu_kern_alloc_ctx(u_int flags) @@ -433,4 +463,27 @@ 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 @@ -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)