1ba1c2daaSIan Lepore /*- 24d846d26SWarner Losh * SPDX-License-Identifier: BSD-2-Clause 3af3dc4a7SPedro F. Giffuni * 4ba1c2daaSIan Lepore * Copyright (c) 2014 Ian Lepore <ian@freebsd.org> 5cf1a573fSOleksandr Tymoshenko * Copyright (c) 2012 Mark Tinguely 6cf1a573fSOleksandr Tymoshenko * 7cf1a573fSOleksandr Tymoshenko * All rights reserved. 8cf1a573fSOleksandr Tymoshenko * 9cf1a573fSOleksandr Tymoshenko * Redistribution and use in source and binary forms, with or without 10cf1a573fSOleksandr Tymoshenko * modification, are permitted provided that the following conditions 11cf1a573fSOleksandr Tymoshenko * are met: 12cf1a573fSOleksandr Tymoshenko * 1. Redistributions of source code must retain the above copyright 13cf1a573fSOleksandr Tymoshenko * notice, this list of conditions and the following disclaimer. 14cf1a573fSOleksandr Tymoshenko * 2. Redistributions in binary form must reproduce the above copyright 15cf1a573fSOleksandr Tymoshenko * notice, this list of conditions and the following disclaimer in the 16cf1a573fSOleksandr Tymoshenko * documentation and/or other materials provided with the distribution. 17cf1a573fSOleksandr Tymoshenko * 18cf1a573fSOleksandr Tymoshenko * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND 19cf1a573fSOleksandr Tymoshenko * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20cf1a573fSOleksandr Tymoshenko * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 21cf1a573fSOleksandr Tymoshenko * ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE 22cf1a573fSOleksandr Tymoshenko * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 23cf1a573fSOleksandr Tymoshenko * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS 24cf1a573fSOleksandr Tymoshenko * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 25cf1a573fSOleksandr Tymoshenko * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 26cf1a573fSOleksandr Tymoshenko * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY 27cf1a573fSOleksandr Tymoshenko * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF 28cf1a573fSOleksandr Tymoshenko * SUCH DAMAGE. 29cf1a573fSOleksandr Tymoshenko */ 30c5de7237SAndrew Turner 31cf1a573fSOleksandr Tymoshenko #include <sys/cdefs.h> 32cf1a573fSOleksandr Tymoshenko __FBSDID("$FreeBSD$"); 33cf1a573fSOleksandr Tymoshenko 34c5de7237SAndrew Turner #ifdef VFP 35cf1a573fSOleksandr Tymoshenko #include <sys/param.h> 36cf1a573fSOleksandr Tymoshenko #include <sys/systm.h> 3798c666cfSKornel Dulęba #include <sys/limits.h> 38cf1a573fSOleksandr Tymoshenko #include <sys/proc.h> 3919e1bd01SJohn Baldwin #include <sys/imgact_elf.h> 40cf1a573fSOleksandr Tymoshenko #include <sys/kernel.h> 41cf1a573fSOleksandr Tymoshenko 42ba1c2daaSIan Lepore #include <machine/armreg.h> 4319e1bd01SJohn Baldwin #include <machine/elf.h> 4464894120SIan Lepore #include <machine/frame.h> 45c890751dSKonstantin Belousov #include <machine/md_var.h> 46cf1a573fSOleksandr Tymoshenko #include <machine/pcb.h> 47cf1a573fSOleksandr Tymoshenko #include <machine/undefined.h> 48cf1a573fSOleksandr Tymoshenko #include <machine/vfp.h> 49cf1a573fSOleksandr Tymoshenko 50cf1a573fSOleksandr Tymoshenko /* function prototypes */ 5115a922dfSAndrew Turner static int vfp_bounce(u_int, u_int, struct trapframe *, int); 5215a922dfSAndrew Turner static void vfp_restore(struct vfp_state *); 53cf1a573fSOleksandr Tymoshenko 54c46c2523SAndrew Turner extern int vfp_exists; 55cf1a573fSOleksandr Tymoshenko static struct undefined_handler vfp10_uh, vfp11_uh; 5693ef7ecbSAndrew Turner /* If true the VFP unit has 32 double registers, otherwise it has 16 */ 5793ef7ecbSAndrew Turner static int is_d32; 58cf1a573fSOleksandr Tymoshenko 596926e269SKornel Dulęba struct fpu_kern_ctx { 606926e269SKornel Dulęba struct vfp_state *prev; 616926e269SKornel Dulęba #define FPU_KERN_CTX_DUMMY 0x01 /* avoided save for the kern thread */ 626926e269SKornel Dulęba #define FPU_KERN_CTX_INUSE 0x02 636926e269SKornel Dulęba uint32_t flags; 646926e269SKornel Dulęba struct vfp_state state; 656926e269SKornel Dulęba }; 666926e269SKornel Dulęba 67bd96d089SIan Lepore /* 68bd96d089SIan Lepore * About .fpu directives in this file... 69bd96d089SIan Lepore * 70bd96d089SIan Lepore * We should need simply .fpu vfpv3, but clang 3.5 has a quirk where setting 71bd96d089SIan Lepore * vfpv3 doesn't imply that vfp2 features are also available -- both have to be 72bd96d089SIan Lepore * explicitly set to get all the features of both. This is probably a bug in 73bd96d089SIan Lepore * clang, so it may get fixed and require changes here some day. Other changes 74bd96d089SIan Lepore * are probably coming in clang too, because there is email and open PRs 75bd96d089SIan Lepore * indicating they want to completely disable the ability to use .fpu and 76bd96d089SIan Lepore * similar directives in inline asm. That would be catastrophic for us, 77bd96d089SIan Lepore * hopefully they come to their senses. There was also some discusion of a new 78bd96d089SIan Lepore * syntax such as .push fpu=vfpv3; ...; .pop fpu; and that would be ideal for 79bd96d089SIan Lepore * us, better than what we have now really. 80bd96d089SIan Lepore * 81bd96d089SIan Lepore * For gcc, each .fpu directive completely overrides the prior directive, unlike 82bd96d089SIan Lepore * with clang, but luckily on gcc saying v3 implies all the v2 features as well. 83bd96d089SIan Lepore */ 84cf1a573fSOleksandr Tymoshenko 85bd96d089SIan Lepore #define fmxr(reg, val) \ 86bd96d089SIan Lepore __asm __volatile(" .fpu vfpv2\n .fpu vfpv3\n" \ 87bd96d089SIan Lepore " vmsr " __STRING(reg) ", %0" :: "r"(val)); 88bd96d089SIan Lepore 89cf1a573fSOleksandr Tymoshenko #define fmrx(reg) \ 90cf1a573fSOleksandr Tymoshenko ({ u_int val = 0;\ 91bd96d089SIan Lepore __asm __volatile(" .fpu vfpv2\n .fpu vfpv3\n" \ 92bd96d089SIan Lepore " vmrs %0, " __STRING(reg) : "=r"(val)); \ 93cf1a573fSOleksandr Tymoshenko val; \ 94cf1a573fSOleksandr Tymoshenko }) 95cf1a573fSOleksandr Tymoshenko 96ba1c2daaSIan Lepore static u_int 97cf1a573fSOleksandr Tymoshenko get_coprocessorACR(void) 98cf1a573fSOleksandr Tymoshenko { 99cf1a573fSOleksandr Tymoshenko u_int val; 100cf1a573fSOleksandr Tymoshenko __asm __volatile("mrc p15, 0, %0, c1, c0, 2" : "=r" (val) : : "cc"); 101cf1a573fSOleksandr Tymoshenko return val; 102cf1a573fSOleksandr Tymoshenko } 103cf1a573fSOleksandr Tymoshenko 104ba1c2daaSIan Lepore static void 105cf1a573fSOleksandr Tymoshenko set_coprocessorACR(u_int val) 106cf1a573fSOleksandr Tymoshenko { 107cf1a573fSOleksandr Tymoshenko __asm __volatile("mcr p15, 0, %0, c1, c0, 2\n\t" 108cf1a573fSOleksandr Tymoshenko : : "r" (val) : "cc"); 10948617973SOlivier Houchard isb(); 110cf1a573fSOleksandr Tymoshenko } 111cf1a573fSOleksandr Tymoshenko 1126926e269SKornel Dulęba static void 1136926e269SKornel Dulęba vfp_enable(void) 1146926e269SKornel Dulęba { 1156926e269SKornel Dulęba uint32_t fpexc; 1166926e269SKornel Dulęba 1176926e269SKornel Dulęba fpexc = fmrx(fpexc); 1186926e269SKornel Dulęba fmxr(fpexc, fpexc | VFPEXC_EN); 1196926e269SKornel Dulęba isb(); 1206926e269SKornel Dulęba } 1216926e269SKornel Dulęba 1226926e269SKornel Dulęba static void 1236926e269SKornel Dulęba vfp_disable(void) 1246926e269SKornel Dulęba { 1256926e269SKornel Dulęba uint32_t fpexc; 1266926e269SKornel Dulęba 1276926e269SKornel Dulęba fpexc = fmrx(fpexc); 1286926e269SKornel Dulęba fmxr(fpexc, fpexc & ~VFPEXC_EN); 1296926e269SKornel Dulęba isb(); 1306926e269SKornel Dulęba } 1316926e269SKornel Dulęba 132cf1a573fSOleksandr Tymoshenko /* called for each cpu */ 133cf1a573fSOleksandr Tymoshenko void 134cf1a573fSOleksandr Tymoshenko vfp_init(void) 135cf1a573fSOleksandr Tymoshenko { 136feb08ab0SWarner Losh u_int fpsid, tmp; 13793ef7ecbSAndrew Turner u_int coproc, vfp_arch; 138cf1a573fSOleksandr Tymoshenko 139cf1a573fSOleksandr Tymoshenko coproc = get_coprocessorACR(); 140cf1a573fSOleksandr Tymoshenko coproc |= COPROC10 | COPROC11; 141cf1a573fSOleksandr Tymoshenko set_coprocessorACR(coproc); 142cf1a573fSOleksandr Tymoshenko 143bd96d089SIan Lepore fpsid = fmrx(fpsid); /* read the vfp system id */ 144cf1a573fSOleksandr Tymoshenko 145cf1a573fSOleksandr Tymoshenko if (!(fpsid & VFPSID_HARDSOFT_IMP)) { 146cf1a573fSOleksandr Tymoshenko vfp_exists = 1; 14793ef7ecbSAndrew Turner is_d32 = 0; 148bd96d089SIan Lepore PCPU_SET(vfpsid, fpsid); /* save the fpsid */ 14919e1bd01SJohn Baldwin elf_hwcap |= HWCAP_VFP; 15093ef7ecbSAndrew Turner 15193ef7ecbSAndrew Turner vfp_arch = 15293ef7ecbSAndrew Turner (fpsid & VFPSID_SUBVERSION2_MASK) >> VFPSID_SUBVERSION_OFF; 15393ef7ecbSAndrew Turner 15493ef7ecbSAndrew Turner if (vfp_arch >= VFP_ARCH3) { 155bd96d089SIan Lepore tmp = fmrx(mvfr0); 156cf1a573fSOleksandr Tymoshenko PCPU_SET(vfpmvfr0, tmp); 15719e1bd01SJohn Baldwin elf_hwcap |= HWCAP_VFPv3; 15893ef7ecbSAndrew Turner 15919e1bd01SJohn Baldwin if ((tmp & VMVFR0_RB_MASK) == 2) { 16019e1bd01SJohn Baldwin elf_hwcap |= HWCAP_VFPD32; 16193ef7ecbSAndrew Turner is_d32 = 1; 16219e1bd01SJohn Baldwin } else 16319e1bd01SJohn Baldwin elf_hwcap |= HWCAP_VFPv3D16; 16493ef7ecbSAndrew Turner 165bd96d089SIan Lepore tmp = fmrx(mvfr1); 166cf1a573fSOleksandr Tymoshenko PCPU_SET(vfpmvfr1, tmp); 167c890751dSKonstantin Belousov 168c890751dSKonstantin Belousov if (PCPU_GET(cpuid) == 0) { 169c890751dSKonstantin Belousov if ((tmp & VMVFR1_FZ_MASK) == 0x1) { 170c890751dSKonstantin Belousov /* Denormals arithmetic support */ 171c890751dSKonstantin Belousov initial_fpscr &= ~VFPSCR_FZ; 172c890751dSKonstantin Belousov thread0.td_pcb->pcb_vfpstate.fpscr = 173c890751dSKonstantin Belousov initial_fpscr; 174c890751dSKonstantin Belousov } 175c890751dSKonstantin Belousov } 17664f73a4cSJohn Baldwin 17764f73a4cSJohn Baldwin if ((tmp & VMVFR1_LS_MASK) >> VMVFR1_LS_OFF == 1 && 17864f73a4cSJohn Baldwin (tmp & VMVFR1_I_MASK) >> VMVFR1_I_OFF == 1 && 17964f73a4cSJohn Baldwin (tmp & VMVFR1_SP_MASK) >> VMVFR1_SP_OFF == 1) 18064f73a4cSJohn Baldwin elf_hwcap |= HWCAP_NEON; 1810cbf724eSMichal Meloun if ((tmp & VMVFR1_FMAC_MASK) >> VMVFR1_FMAC_OFF == 1) 1820cbf724eSMichal Meloun elf_hwcap |= HWCAP_VFPv4; 183cf1a573fSOleksandr Tymoshenko } 18493ef7ecbSAndrew Turner 185cf1a573fSOleksandr Tymoshenko /* initialize the coprocess 10 and 11 calls 186cf1a573fSOleksandr Tymoshenko * These are called to restore the registers and enable 187cf1a573fSOleksandr Tymoshenko * the VFP hardware. 188cf1a573fSOleksandr Tymoshenko */ 189cf1a573fSOleksandr Tymoshenko if (vfp10_uh.uh_handler == NULL) { 190cf1a573fSOleksandr Tymoshenko vfp10_uh.uh_handler = vfp_bounce; 191cf1a573fSOleksandr Tymoshenko vfp11_uh.uh_handler = vfp_bounce; 192cf1a573fSOleksandr Tymoshenko install_coproc_handler_static(10, &vfp10_uh); 193cf1a573fSOleksandr Tymoshenko install_coproc_handler_static(11, &vfp11_uh); 194cf1a573fSOleksandr Tymoshenko } 195cf1a573fSOleksandr Tymoshenko } 196cf1a573fSOleksandr Tymoshenko } 197cf1a573fSOleksandr Tymoshenko 198cf1a573fSOleksandr Tymoshenko SYSINIT(vfp, SI_SUB_CPU, SI_ORDER_ANY, vfp_init, NULL); 199cf1a573fSOleksandr Tymoshenko 200cf1a573fSOleksandr Tymoshenko /* start VFP unit, restore the vfp registers from the PCB and retry 201cf1a573fSOleksandr Tymoshenko * the instruction 202cf1a573fSOleksandr Tymoshenko */ 20315a922dfSAndrew Turner static int 204cf1a573fSOleksandr Tymoshenko vfp_bounce(u_int addr, u_int insn, struct trapframe *frame, int code) 205cf1a573fSOleksandr Tymoshenko { 206ba1c2daaSIan Lepore u_int cpu, fpexc; 207cf1a573fSOleksandr Tymoshenko struct pcb *curpcb; 208d4f9011cSAndrew Turner ksiginfo_t ksi; 209cf1a573fSOleksandr Tymoshenko 210ba1c2daaSIan Lepore if ((code & FAULT_USER) == 0) 211ba1c2daaSIan Lepore panic("undefined floating point instruction in supervisor mode"); 212ba1c2daaSIan Lepore 213ba1c2daaSIan Lepore critical_enter(); 214ba1c2daaSIan Lepore 215ba1c2daaSIan Lepore /* 216ba1c2daaSIan Lepore * If the VFP is already on and we got an undefined instruction, then 217ba1c2daaSIan Lepore * something tried to executate a truly invalid instruction that maps to 218ba1c2daaSIan Lepore * the VFP. 219cf1a573fSOleksandr Tymoshenko */ 220bd96d089SIan Lepore fpexc = fmrx(fpexc); 221ba1c2daaSIan Lepore if (fpexc & VFPEXC_EN) { 222d4f9011cSAndrew Turner /* Clear any exceptions */ 22323e00b90SOleksandr Tymoshenko fmxr(fpexc, fpexc & ~(VFPEXC_EX | VFPEXC_FP2V)); 224d4f9011cSAndrew Turner 225cf1a573fSOleksandr Tymoshenko /* kill the process - we do not handle emulation */ 226ba1c2daaSIan Lepore critical_exit(); 227d4f9011cSAndrew Turner 22823e00b90SOleksandr Tymoshenko if (fpexc & VFPEXC_EX) { 229d4f9011cSAndrew Turner /* We have an exception, signal a SIGFPE */ 230d4f9011cSAndrew Turner ksiginfo_init_trap(&ksi); 231d4f9011cSAndrew Turner ksi.ksi_signo = SIGFPE; 232d4f9011cSAndrew Turner if (fpexc & VFPEXC_UFC) 233d4f9011cSAndrew Turner ksi.ksi_code = FPE_FLTUND; 234d4f9011cSAndrew Turner else if (fpexc & VFPEXC_OFC) 235d4f9011cSAndrew Turner ksi.ksi_code = FPE_FLTOVF; 236d4f9011cSAndrew Turner else if (fpexc & VFPEXC_IOC) 237d4f9011cSAndrew Turner ksi.ksi_code = FPE_FLTINV; 238d4f9011cSAndrew Turner ksi.ksi_addr = (void *)addr; 239d4f9011cSAndrew Turner trapsignal(curthread, &ksi); 240d4f9011cSAndrew Turner return 0; 241d4f9011cSAndrew Turner } 242d4f9011cSAndrew Turner 243cf1a573fSOleksandr Tymoshenko return 1; 244cf1a573fSOleksandr Tymoshenko } 245ba1c2daaSIan Lepore 246ba1c2daaSIan Lepore /* 247ba1c2daaSIan Lepore * If the last time this thread used the VFP it was on this core, and 248ba1c2daaSIan Lepore * the last thread to use the VFP on this core was this thread, then the 249ba1c2daaSIan Lepore * VFP state is valid, otherwise restore this thread's state to the VFP. 250cf1a573fSOleksandr Tymoshenko */ 251bd96d089SIan Lepore fmxr(fpexc, fpexc | VFPEXC_EN); 252ba1c2daaSIan Lepore curpcb = curthread->td_pcb; 253e1f2f9bdSAndrew Turner cpu = PCPU_GET(cpuid); 254ba1c2daaSIan Lepore if (curpcb->pcb_vfpcpu != cpu || curthread != PCPU_GET(fpcurthread)) { 2556926e269SKornel Dulęba vfp_restore(curpcb->pcb_vfpsaved); 256ba1c2daaSIan Lepore curpcb->pcb_vfpcpu = cpu; 257ba1c2daaSIan Lepore PCPU_SET(fpcurthread, curthread); 258cf1a573fSOleksandr Tymoshenko } 259cf1a573fSOleksandr Tymoshenko 260ba1c2daaSIan Lepore critical_exit(); 26198c666cfSKornel Dulęba 26298c666cfSKornel Dulęba KASSERT(curpcb->pcb_vfpsaved == &curpcb->pcb_vfpstate, 26398c666cfSKornel Dulęba ("Kernel VFP state in use when entering userspace")); 26498c666cfSKornel Dulęba 265ba1c2daaSIan Lepore return (0); 266ba1c2daaSIan Lepore } 267ba1c2daaSIan Lepore 268ba1c2daaSIan Lepore /* 26998c666cfSKornel Dulęba * Update the VFP state for a forked process or new thread. The PCB will 27098c666cfSKornel Dulęba * have been copied from the old thread. 27198c666cfSKornel Dulęba * The code is heavily based on arm64 logic. 27298c666cfSKornel Dulęba */ 27398c666cfSKornel Dulęba void 27498c666cfSKornel Dulęba vfp_new_thread(struct thread *newtd, struct thread *oldtd, bool fork) 27598c666cfSKornel Dulęba { 27698c666cfSKornel Dulęba struct pcb *newpcb; 27798c666cfSKornel Dulęba 27898c666cfSKornel Dulęba newpcb = newtd->td_pcb; 27998c666cfSKornel Dulęba 28098c666cfSKornel Dulęba /* Kernel threads start with clean VFP */ 28198c666cfSKornel Dulęba if ((oldtd->td_pflags & TDP_KTHREAD) != 0) { 28298c666cfSKornel Dulęba newpcb->pcb_fpflags &= 28398c666cfSKornel Dulęba ~(PCB_FP_STARTED | PCB_FP_KERN | PCB_FP_NOSAVE); 28498c666cfSKornel Dulęba } else { 28598c666cfSKornel Dulęba MPASS((newpcb->pcb_fpflags & (PCB_FP_KERN|PCB_FP_NOSAVE)) == 0); 28698c666cfSKornel Dulęba if (!fork) { 28798c666cfSKornel Dulęba newpcb->pcb_fpflags &= ~PCB_FP_STARTED; 28898c666cfSKornel Dulęba } 28998c666cfSKornel Dulęba } 29098c666cfSKornel Dulęba 29198c666cfSKornel Dulęba newpcb->pcb_vfpsaved = &newpcb->pcb_vfpstate; 29298c666cfSKornel Dulęba newpcb->pcb_vfpcpu = UINT_MAX; 29398c666cfSKornel Dulęba } 29498c666cfSKornel Dulęba /* 295ba1c2daaSIan Lepore * Restore the given state to the VFP hardware. 296cf1a573fSOleksandr Tymoshenko */ 29715a922dfSAndrew Turner static void 298cf1a573fSOleksandr Tymoshenko vfp_restore(struct vfp_state *vfpsave) 299cf1a573fSOleksandr Tymoshenko { 300d4f9011cSAndrew Turner uint32_t fpexc; 301cf1a573fSOleksandr Tymoshenko 302bd96d089SIan Lepore /* On vfpv3 we may need to restore FPINST and FPINST2 */ 303d4f9011cSAndrew Turner fpexc = vfpsave->fpexec; 304d4f9011cSAndrew Turner if (fpexc & VFPEXC_EX) { 305bd96d089SIan Lepore fmxr(fpinst, vfpsave->fpinst); 306d4f9011cSAndrew Turner if (fpexc & VFPEXC_FP2V) 307bd96d089SIan Lepore fmxr(fpinst2, vfpsave->fpinst2); 308d4f9011cSAndrew Turner } 309bd96d089SIan Lepore fmxr(fpscr, vfpsave->fpscr); 310d4f9011cSAndrew Turner 311bd96d089SIan Lepore __asm __volatile( 312bd96d089SIan Lepore " .fpu vfpv2\n" 313bd96d089SIan Lepore " .fpu vfpv3\n" 314bd96d089SIan Lepore " vldmia %0!, {d0-d15}\n" /* d0-d15 */ 315d4f9011cSAndrew Turner " cmp %1, #0\n" /* -D16 or -D32? */ 316bd96d089SIan Lepore " vldmiane %0!, {d16-d31}\n" /* d16-d31 */ 317d4f9011cSAndrew Turner " addeq %0, %0, #128\n" /* skip missing regs */ 318bd96d089SIan Lepore : "+&r" (vfpsave) : "r" (is_d32) : "cc" 319bd96d089SIan Lepore ); 320d4f9011cSAndrew Turner 321bd96d089SIan Lepore fmxr(fpexc, fpexc); 322cf1a573fSOleksandr Tymoshenko } 323cf1a573fSOleksandr Tymoshenko 324ba1c2daaSIan Lepore /* 325ba1c2daaSIan Lepore * If the VFP is on, save its current state and turn it off if requested to do 326ba1c2daaSIan Lepore * so. If the VFP is not on, does not change the values at *vfpsave. Caller is 327ba1c2daaSIan Lepore * responsible for preventing a context switch while this is running. 328cf1a573fSOleksandr Tymoshenko */ 329cf1a573fSOleksandr Tymoshenko void 330ba1c2daaSIan Lepore vfp_store(struct vfp_state *vfpsave, boolean_t disable_vfp) 331cf1a573fSOleksandr Tymoshenko { 332d4f9011cSAndrew Turner uint32_t fpexc; 333cf1a573fSOleksandr Tymoshenko 334bd96d089SIan Lepore fpexc = fmrx(fpexc); /* Is the vfp enabled? */ 335d4f9011cSAndrew Turner if (fpexc & VFPEXC_EN) { 336d4f9011cSAndrew Turner vfpsave->fpexec = fpexc; 337bd96d089SIan Lepore vfpsave->fpscr = fmrx(fpscr); 338d4f9011cSAndrew Turner 339bd96d089SIan Lepore /* On vfpv3 we may need to save FPINST and FPINST2 */ 340d4f9011cSAndrew Turner if (fpexc & VFPEXC_EX) { 341bd96d089SIan Lepore vfpsave->fpinst = fmrx(fpinst); 342d4f9011cSAndrew Turner if (fpexc & VFPEXC_FP2V) 343bd96d089SIan Lepore vfpsave->fpinst2 = fmrx(fpinst2); 344d4f9011cSAndrew Turner fpexc &= ~VFPEXC_EX; 345d4f9011cSAndrew Turner } 346d4f9011cSAndrew Turner 347ba1c2daaSIan Lepore __asm __volatile( 348bd96d089SIan Lepore " .fpu vfpv2\n" 349bd96d089SIan Lepore " .fpu vfpv3\n" 350bd96d089SIan Lepore " vstmia %0!, {d0-d15}\n" /* d0-d15 */ 351d4f9011cSAndrew Turner " cmp %1, #0\n" /* -D16 or -D32? */ 3529ea04586SAndrew Turner " vstmiane %0!, {d16-d31}\n" /* d16-d31 */ 353d4f9011cSAndrew Turner " addeq %0, %0, #128\n" /* skip missing regs */ 354bd96d089SIan Lepore : "+&r" (vfpsave) : "r" (is_d32) : "cc" 355bd96d089SIan Lepore ); 356d4f9011cSAndrew Turner 357ba1c2daaSIan Lepore if (disable_vfp) 358bd96d089SIan Lepore fmxr(fpexc , fpexc & ~VFPEXC_EN); 359cf1a573fSOleksandr Tymoshenko } 360cf1a573fSOleksandr Tymoshenko } 361cf1a573fSOleksandr Tymoshenko 362ba1c2daaSIan Lepore /* 3637a797a24SIan Lepore * The current thread is dying. If the state currently in the hardware belongs 3647a797a24SIan Lepore * to the current thread, set fpcurthread to NULL to indicate that the VFP 3657a797a24SIan Lepore * hardware state does not belong to any thread. If the VFP is on, turn it off. 366cf1a573fSOleksandr Tymoshenko */ 367cf1a573fSOleksandr Tymoshenko void 3687a797a24SIan Lepore vfp_discard(struct thread *td) 369cf1a573fSOleksandr Tymoshenko { 370ba1c2daaSIan Lepore u_int tmp; 371cf1a573fSOleksandr Tymoshenko 3727a797a24SIan Lepore if (PCPU_GET(fpcurthread) == td) 3737a797a24SIan Lepore PCPU_SET(fpcurthread, NULL); 3747a797a24SIan Lepore 375bd96d089SIan Lepore tmp = fmrx(fpexc); 3767a797a24SIan Lepore if (tmp & VFPEXC_EN) 377bd96d089SIan Lepore fmxr(fpexc, tmp & ~VFPEXC_EN); 378cf1a573fSOleksandr Tymoshenko } 379cf1a573fSOleksandr Tymoshenko 3806926e269SKornel Dulęba void 3816926e269SKornel Dulęba vfp_save_state(struct thread *td, struct pcb *pcb) 3826926e269SKornel Dulęba { 3836926e269SKornel Dulęba int32_t fpexc; 3846926e269SKornel Dulęba 3856926e269SKornel Dulęba KASSERT(pcb != NULL, ("NULL vfp pcb")); 3866926e269SKornel Dulęba KASSERT(td == NULL || td->td_pcb == pcb, ("Invalid vfp pcb")); 3876926e269SKornel Dulęba 3886926e269SKornel Dulęba /* 3896926e269SKornel Dulęba * savectx() will be called on panic with dumppcb as an argument, 3906926e269SKornel Dulęba * dumppcb doesn't have pcb_vfpsaved set, so set it to save 3916926e269SKornel Dulęba * the VFP registers. 3926926e269SKornel Dulęba */ 3936926e269SKornel Dulęba if (pcb->pcb_vfpsaved == NULL) 3946926e269SKornel Dulęba pcb->pcb_vfpsaved = &pcb->pcb_vfpstate; 3956926e269SKornel Dulęba 3966926e269SKornel Dulęba if (td == NULL) 3976926e269SKornel Dulęba td = curthread; 3986926e269SKornel Dulęba 3996926e269SKornel Dulęba critical_enter(); 4006926e269SKornel Dulęba /* 4016926e269SKornel Dulęba * Only store the registers if the VFP is enabled, 4026926e269SKornel Dulęba * i.e. return if we are trapping on FP access. 4036926e269SKornel Dulęba */ 4046926e269SKornel Dulęba fpexc = fmrx(fpexc); 4056926e269SKornel Dulęba if (fpexc & VFPEXC_EN) { 4066926e269SKornel Dulęba KASSERT(PCPU_GET(fpcurthread) == td, 4076926e269SKornel Dulęba ("Storing an invalid VFP state")); 4086926e269SKornel Dulęba 4096926e269SKornel Dulęba vfp_store(pcb->pcb_vfpsaved, true); 4106926e269SKornel Dulęba } 4116926e269SKornel Dulęba critical_exit(); 4126926e269SKornel Dulęba } 4136926e269SKornel Dulęba 4146926e269SKornel Dulęba void 4156926e269SKornel Dulęba fpu_kern_enter(struct thread *td, struct fpu_kern_ctx *ctx, u_int flags) 4166926e269SKornel Dulęba { 4176926e269SKornel Dulęba struct pcb *pcb; 4186926e269SKornel Dulęba 4196926e269SKornel Dulęba pcb = td->td_pcb; 4206926e269SKornel Dulęba KASSERT((flags & FPU_KERN_NOCTX) != 0 || ctx != NULL, 4216926e269SKornel Dulęba ("ctx is required when !FPU_KERN_NOCTX")); 4226926e269SKornel Dulęba KASSERT(ctx == NULL || (ctx->flags & FPU_KERN_CTX_INUSE) == 0, 4236926e269SKornel Dulęba ("using inuse ctx")); 4246926e269SKornel Dulęba KASSERT((pcb->pcb_fpflags & PCB_FP_NOSAVE) == 0, 4256926e269SKornel Dulęba ("recursive fpu_kern_enter while in PCB_FP_NOSAVE state")); 4266926e269SKornel Dulęba 4276926e269SKornel Dulęba if ((flags & FPU_KERN_NOCTX) != 0) { 4286926e269SKornel Dulęba critical_enter(); 4296926e269SKornel Dulęba if (curthread == PCPU_GET(fpcurthread)) { 4306926e269SKornel Dulęba vfp_save_state(curthread, pcb); 4316926e269SKornel Dulęba } 4326926e269SKornel Dulęba PCPU_SET(fpcurthread, NULL); 4336926e269SKornel Dulęba 4346926e269SKornel Dulęba vfp_enable(); 4356926e269SKornel Dulęba pcb->pcb_fpflags |= PCB_FP_KERN | PCB_FP_NOSAVE | 4366926e269SKornel Dulęba PCB_FP_STARTED; 4376926e269SKornel Dulęba return; 4386926e269SKornel Dulęba } 4396926e269SKornel Dulęba 4406926e269SKornel Dulęba if ((flags & FPU_KERN_KTHR) != 0 && is_fpu_kern_thread(0)) { 4416926e269SKornel Dulęba ctx->flags = FPU_KERN_CTX_DUMMY | FPU_KERN_CTX_INUSE; 4426926e269SKornel Dulęba return; 4436926e269SKornel Dulęba } 4446926e269SKornel Dulęba /* 4456926e269SKornel Dulęba * Check either we are already using the VFP in the kernel, or 4466926e269SKornel Dulęba * the the saved state points to the default user space. 4476926e269SKornel Dulęba */ 4486926e269SKornel Dulęba KASSERT((pcb->pcb_fpflags & PCB_FP_KERN) != 0 || 4496926e269SKornel Dulęba pcb->pcb_vfpsaved == &pcb->pcb_vfpstate, 4506926e269SKornel Dulęba ("Mangled pcb_vfpsaved %x %p %p", pcb->pcb_fpflags, pcb->pcb_vfpsaved, 4516926e269SKornel Dulęba &pcb->pcb_vfpstate)); 4526926e269SKornel Dulęba ctx->flags = FPU_KERN_CTX_INUSE; 4536926e269SKornel Dulęba vfp_save_state(curthread, pcb); 4546926e269SKornel Dulęba ctx->prev = pcb->pcb_vfpsaved; 4556926e269SKornel Dulęba pcb->pcb_vfpsaved = &ctx->state; 4566926e269SKornel Dulęba pcb->pcb_fpflags |= PCB_FP_KERN; 4576926e269SKornel Dulęba pcb->pcb_fpflags &= ~PCB_FP_STARTED; 4586926e269SKornel Dulęba 4596926e269SKornel Dulęba return; 4606926e269SKornel Dulęba } 4616926e269SKornel Dulęba 4626926e269SKornel Dulęba int 4636926e269SKornel Dulęba fpu_kern_leave(struct thread *td, struct fpu_kern_ctx *ctx) 4646926e269SKornel Dulęba { 4656926e269SKornel Dulęba struct pcb *pcb; 4666926e269SKornel Dulęba 4676926e269SKornel Dulęba pcb = td->td_pcb; 4686926e269SKornel Dulęba 4696926e269SKornel Dulęba if ((pcb->pcb_fpflags & PCB_FP_NOSAVE) != 0) { 4706926e269SKornel Dulęba KASSERT(ctx == NULL, ("non-null ctx after FPU_KERN_NOCTX")); 4716926e269SKornel Dulęba KASSERT(PCPU_GET(fpcurthread) == NULL, 4726926e269SKornel Dulęba ("non-NULL fpcurthread for PCB_FP_NOSAVE")); 4736926e269SKornel Dulęba CRITICAL_ASSERT(td); 4746926e269SKornel Dulęba 4756926e269SKornel Dulęba vfp_disable(); 4766926e269SKornel Dulęba pcb->pcb_fpflags &= ~(PCB_FP_NOSAVE | PCB_FP_STARTED); 4776926e269SKornel Dulęba critical_exit(); 4786926e269SKornel Dulęba } else { 4796926e269SKornel Dulęba KASSERT((ctx->flags & FPU_KERN_CTX_INUSE) != 0, 4806926e269SKornel Dulęba ("FPU context not inuse")); 4816926e269SKornel Dulęba ctx->flags &= ~FPU_KERN_CTX_INUSE; 4826926e269SKornel Dulęba 4836926e269SKornel Dulęba if (is_fpu_kern_thread(0) && 4846926e269SKornel Dulęba (ctx->flags & FPU_KERN_CTX_DUMMY) != 0) 4856926e269SKornel Dulęba return (0); 4866926e269SKornel Dulęba KASSERT((ctx->flags & FPU_KERN_CTX_DUMMY) == 0, ("dummy ctx")); 4876926e269SKornel Dulęba critical_enter(); 4886926e269SKornel Dulęba vfp_discard(td); 4896926e269SKornel Dulęba critical_exit(); 4906926e269SKornel Dulęba pcb->pcb_fpflags &= ~PCB_FP_STARTED; 4916926e269SKornel Dulęba pcb->pcb_vfpsaved = ctx->prev; 4926926e269SKornel Dulęba } 4936926e269SKornel Dulęba 4946926e269SKornel Dulęba if (pcb->pcb_vfpsaved == &pcb->pcb_vfpstate) { 4956926e269SKornel Dulęba pcb->pcb_fpflags &= ~PCB_FP_KERN; 4966926e269SKornel Dulęba } else { 4976926e269SKornel Dulęba KASSERT((pcb->pcb_fpflags & PCB_FP_KERN) != 0, 4986926e269SKornel Dulęba ("unpaired fpu_kern_leave")); 4996926e269SKornel Dulęba } 5006926e269SKornel Dulęba 5016926e269SKornel Dulęba return (0); 5026926e269SKornel Dulęba } 5036926e269SKornel Dulęba 5046926e269SKornel Dulęba int 5056926e269SKornel Dulęba fpu_kern_thread(u_int flags __unused) 5066926e269SKornel Dulęba { 5076926e269SKornel Dulęba struct pcb *pcb = curthread->td_pcb; 5086926e269SKornel Dulęba 5096926e269SKornel Dulęba KASSERT((curthread->td_pflags & TDP_KTHREAD) != 0, 5106926e269SKornel Dulęba ("Only kthread may use fpu_kern_thread")); 5116926e269SKornel Dulęba KASSERT(pcb->pcb_vfpsaved == &pcb->pcb_vfpstate, 5126926e269SKornel Dulęba ("Mangled pcb_vfpsaved")); 5136926e269SKornel Dulęba KASSERT((pcb->pcb_fpflags & PCB_FP_KERN) == 0, 5146926e269SKornel Dulęba ("Thread already setup for the VFP")); 5156926e269SKornel Dulęba pcb->pcb_fpflags |= PCB_FP_KERN; 5166926e269SKornel Dulęba return (0); 5176926e269SKornel Dulęba } 5186926e269SKornel Dulęba 5196926e269SKornel Dulęba int 5206926e269SKornel Dulęba is_fpu_kern_thread(u_int flags __unused) 5216926e269SKornel Dulęba { 5226926e269SKornel Dulęba struct pcb *curpcb; 5236926e269SKornel Dulęba 5246926e269SKornel Dulęba if ((curthread->td_pflags & TDP_KTHREAD) == 0) 5256926e269SKornel Dulęba return (0); 5266926e269SKornel Dulęba curpcb = curthread->td_pcb; 5276926e269SKornel Dulęba return ((curpcb->pcb_fpflags & PCB_FP_KERN) != 0); 5286926e269SKornel Dulęba } 5296926e269SKornel Dulęba 530c5de7237SAndrew Turner #endif 531