1262941Sian/*- 2262941Sian * Copyright (c) 2014 Ian Lepore <ian@freebsd.org> 3239268Sgonzo * Copyright (c) 2012 Mark Tinguely 4239268Sgonzo * 5239268Sgonzo * All rights reserved. 6239268Sgonzo * 7239268Sgonzo * Redistribution and use in source and binary forms, with or without 8239268Sgonzo * modification, are permitted provided that the following conditions 9239268Sgonzo * are met: 10239268Sgonzo * 1. Redistributions of source code must retain the above copyright 11239268Sgonzo * notice, this list of conditions and the following disclaimer. 12239268Sgonzo * 2. Redistributions in binary form must reproduce the above copyright 13239268Sgonzo * notice, this list of conditions and the following disclaimer in the 14239268Sgonzo * documentation and/or other materials provided with the distribution. 15239268Sgonzo * 16239268Sgonzo * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND 17239268Sgonzo * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 18239268Sgonzo * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 19239268Sgonzo * ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE 20239268Sgonzo * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 21239268Sgonzo * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS 22239268Sgonzo * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 23239268Sgonzo * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 24239268Sgonzo * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY 25239268Sgonzo * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF 26239268Sgonzo * SUCH DAMAGE. 27239268Sgonzo */ 28254461Sandrew 29239268Sgonzo#include <sys/cdefs.h> 30239268Sgonzo__FBSDID("$FreeBSD: stable/11/sys/arm/arm/vfp.c 325810 2017-11-14 16:03:07Z jhb $"); 31239268Sgonzo 32254461Sandrew#ifdef VFP 33239268Sgonzo#include <sys/param.h> 34239268Sgonzo#include <sys/systm.h> 35239268Sgonzo#include <sys/proc.h> 36325810Sjhb#include <sys/imgact_elf.h> 37239268Sgonzo#include <sys/kernel.h> 38239268Sgonzo 39262941Sian#include <machine/armreg.h> 40325810Sjhb#include <machine/elf.h> 41257200Sian#include <machine/frame.h> 42288983Skib#include <machine/md_var.h> 43239268Sgonzo#include <machine/pcb.h> 44239268Sgonzo#include <machine/undefined.h> 45239268Sgonzo#include <machine/vfp.h> 46239268Sgonzo 47239268Sgonzo/* function prototypes */ 48261563Sandrewstatic int vfp_bounce(u_int, u_int, struct trapframe *, int); 49261563Sandrewstatic void vfp_restore(struct vfp_state *); 50239268Sgonzo 51249176Sandrewextern int vfp_exists; 52239268Sgonzostatic struct undefined_handler vfp10_uh, vfp11_uh; 53251712Sandrew/* If true the VFP unit has 32 double registers, otherwise it has 16 */ 54251712Sandrewstatic int is_d32; 55239268Sgonzo 56276518Sian/* 57276518Sian * About .fpu directives in this file... 58276518Sian * 59276518Sian * We should need simply .fpu vfpv3, but clang 3.5 has a quirk where setting 60276518Sian * vfpv3 doesn't imply that vfp2 features are also available -- both have to be 61276518Sian * explicitly set to get all the features of both. This is probably a bug in 62276518Sian * clang, so it may get fixed and require changes here some day. Other changes 63276518Sian * are probably coming in clang too, because there is email and open PRs 64276518Sian * indicating they want to completely disable the ability to use .fpu and 65276518Sian * similar directives in inline asm. That would be catastrophic for us, 66276518Sian * hopefully they come to their senses. There was also some discusion of a new 67276518Sian * syntax such as .push fpu=vfpv3; ...; .pop fpu; and that would be ideal for 68276518Sian * us, better than what we have now really. 69276518Sian * 70276518Sian * For gcc, each .fpu directive completely overrides the prior directive, unlike 71276518Sian * with clang, but luckily on gcc saying v3 implies all the v2 features as well. 72276518Sian */ 73276518Sian 74239268Sgonzo#define fmxr(reg, val) \ 75276518Sian __asm __volatile(" .fpu vfpv2\n .fpu vfpv3\n" \ 76276518Sian " vmsr " __STRING(reg) ", %0" :: "r"(val)); 77239268Sgonzo 78239268Sgonzo#define fmrx(reg) \ 79239268Sgonzo({ u_int val = 0;\ 80276518Sian __asm __volatile(" .fpu vfpv2\n .fpu vfpv3\n" \ 81276518Sian " vmrs %0, " __STRING(reg) : "=r"(val)); \ 82251712Sandrew val; \ 83239268Sgonzo}) 84239268Sgonzo 85262941Sianstatic u_int 86239268Sgonzoget_coprocessorACR(void) 87239268Sgonzo{ 88239268Sgonzo u_int val; 89239268Sgonzo __asm __volatile("mrc p15, 0, %0, c1, c0, 2" : "=r" (val) : : "cc"); 90239268Sgonzo return val; 91239268Sgonzo} 92239268Sgonzo 93262941Sianstatic void 94239268Sgonzoset_coprocessorACR(u_int val) 95239268Sgonzo{ 96239268Sgonzo __asm __volatile("mcr p15, 0, %0, c1, c0, 2\n\t" 97239268Sgonzo : : "r" (val) : "cc"); 98247340Scognet isb(); 99239268Sgonzo} 100239268Sgonzo 101239268Sgonzo 102239268Sgonzo /* called for each cpu */ 103239268Sgonzovoid 104239268Sgonzovfp_init(void) 105239268Sgonzo{ 106239268Sgonzo u_int fpsid, fpexc, tmp; 107251712Sandrew u_int coproc, vfp_arch; 108239268Sgonzo 109239268Sgonzo coproc = get_coprocessorACR(); 110239268Sgonzo coproc |= COPROC10 | COPROC11; 111239268Sgonzo set_coprocessorACR(coproc); 112283366Sandrew 113276518Sian fpsid = fmrx(fpsid); /* read the vfp system id */ 114276518Sian fpexc = fmrx(fpexc); /* read the vfp exception reg */ 115239268Sgonzo 116239268Sgonzo if (!(fpsid & VFPSID_HARDSOFT_IMP)) { 117239268Sgonzo vfp_exists = 1; 118251712Sandrew is_d32 = 0; 119276518Sian PCPU_SET(vfpsid, fpsid); /* save the fpsid */ 120325810Sjhb elf_hwcap |= HWCAP_VFP; 121251712Sandrew 122251712Sandrew vfp_arch = 123251712Sandrew (fpsid & VFPSID_SUBVERSION2_MASK) >> VFPSID_SUBVERSION_OFF; 124251712Sandrew 125251712Sandrew if (vfp_arch >= VFP_ARCH3) { 126276518Sian tmp = fmrx(mvfr0); 127239268Sgonzo PCPU_SET(vfpmvfr0, tmp); 128325810Sjhb elf_hwcap |= HWCAP_VFPv3; 129251712Sandrew 130325810Sjhb if ((tmp & VMVFR0_RB_MASK) == 2) { 131325810Sjhb elf_hwcap |= HWCAP_VFPD32; 132251712Sandrew is_d32 = 1; 133325810Sjhb } else 134325810Sjhb elf_hwcap |= HWCAP_VFPv3D16; 135251712Sandrew 136276518Sian tmp = fmrx(mvfr1); 137239268Sgonzo PCPU_SET(vfpmvfr1, tmp); 138288983Skib 139288983Skib if (PCPU_GET(cpuid) == 0) { 140288983Skib if ((tmp & VMVFR1_FZ_MASK) == 0x1) { 141288983Skib /* Denormals arithmetic support */ 142288983Skib initial_fpscr &= ~VFPSCR_FZ; 143288983Skib thread0.td_pcb->pcb_vfpstate.fpscr = 144288983Skib initial_fpscr; 145288983Skib } 146288983Skib } 147325810Sjhb 148325810Sjhb if ((tmp & VMVFR1_LS_MASK) >> VMVFR1_LS_OFF == 1 && 149325810Sjhb (tmp & VMVFR1_I_MASK) >> VMVFR1_I_OFF == 1 && 150325810Sjhb (tmp & VMVFR1_SP_MASK) >> VMVFR1_SP_OFF == 1) 151325810Sjhb elf_hwcap |= HWCAP_NEON; 152325810Sjhb if ((tmp & VMVFR1_FMAC_MASK) >> VMVFR1_FMAC_OFF == 1) 153325810Sjhb elf_hwcap |= HWCAP_VFPv4; 154239268Sgonzo } 155251712Sandrew 156239268Sgonzo /* initialize the coprocess 10 and 11 calls 157239268Sgonzo * These are called to restore the registers and enable 158239268Sgonzo * the VFP hardware. 159239268Sgonzo */ 160239268Sgonzo if (vfp10_uh.uh_handler == NULL) { 161239268Sgonzo vfp10_uh.uh_handler = vfp_bounce; 162239268Sgonzo vfp11_uh.uh_handler = vfp_bounce; 163239268Sgonzo install_coproc_handler_static(10, &vfp10_uh); 164239268Sgonzo install_coproc_handler_static(11, &vfp11_uh); 165239268Sgonzo } 166239268Sgonzo } 167239268Sgonzo} 168239268Sgonzo 169239268SgonzoSYSINIT(vfp, SI_SUB_CPU, SI_ORDER_ANY, vfp_init, NULL); 170239268Sgonzo 171239268Sgonzo 172239268Sgonzo/* start VFP unit, restore the vfp registers from the PCB and retry 173239268Sgonzo * the instruction 174239268Sgonzo */ 175261563Sandrewstatic int 176239268Sgonzovfp_bounce(u_int addr, u_int insn, struct trapframe *frame, int code) 177239268Sgonzo{ 178262941Sian u_int cpu, fpexc; 179239268Sgonzo struct pcb *curpcb; 180263914Sandrew ksiginfo_t ksi; 181239268Sgonzo 182262941Sian if ((code & FAULT_USER) == 0) 183262941Sian panic("undefined floating point instruction in supervisor mode"); 184262941Sian 185262941Sian critical_enter(); 186262941Sian 187262941Sian /* 188262941Sian * If the VFP is already on and we got an undefined instruction, then 189262941Sian * something tried to executate a truly invalid instruction that maps to 190262941Sian * the VFP. 191262941Sian */ 192276518Sian fpexc = fmrx(fpexc); 193239268Sgonzo if (fpexc & VFPEXC_EN) { 194263914Sandrew /* Clear any exceptions */ 195290349Sgonzo fmxr(fpexc, fpexc & ~(VFPEXC_EX | VFPEXC_FP2V)); 196263914Sandrew 197262941Sian /* kill the process - we do not handle emulation */ 198262941Sian critical_exit(); 199263914Sandrew 200290349Sgonzo if (fpexc & VFPEXC_EX) { 201263914Sandrew /* We have an exception, signal a SIGFPE */ 202263914Sandrew ksiginfo_init_trap(&ksi); 203263914Sandrew ksi.ksi_signo = SIGFPE; 204263914Sandrew if (fpexc & VFPEXC_UFC) 205263914Sandrew ksi.ksi_code = FPE_FLTUND; 206263914Sandrew else if (fpexc & VFPEXC_OFC) 207263914Sandrew ksi.ksi_code = FPE_FLTOVF; 208263914Sandrew else if (fpexc & VFPEXC_IOC) 209263914Sandrew ksi.ksi_code = FPE_FLTINV; 210263914Sandrew ksi.ksi_addr = (void *)addr; 211263914Sandrew trapsignal(curthread, &ksi); 212263914Sandrew return 0; 213263914Sandrew } 214263914Sandrew 215262941Sian return 1; 216262941Sian } 217239268Sgonzo 218262941Sian /* 219262941Sian * If the last time this thread used the VFP it was on this core, and 220262941Sian * the last thread to use the VFP on this core was this thread, then the 221262941Sian * VFP state is valid, otherwise restore this thread's state to the VFP. 222262941Sian */ 223276518Sian fmxr(fpexc, fpexc | VFPEXC_EN); 224262941Sian curpcb = curthread->td_pcb; 225284109Sandrew cpu = PCPU_GET(cpuid); 226262941Sian if (curpcb->pcb_vfpcpu != cpu || curthread != PCPU_GET(fpcurthread)) { 227262941Sian vfp_restore(&curpcb->pcb_vfpstate); 228262941Sian curpcb->pcb_vfpcpu = cpu; 229262941Sian PCPU_SET(fpcurthread, curthread); 230239268Sgonzo } 231262941Sian 232262941Sian critical_exit(); 233262941Sian return (0); 234239268Sgonzo} 235239268Sgonzo 236262941Sian/* 237262941Sian * Restore the given state to the VFP hardware. 238239268Sgonzo */ 239261563Sandrewstatic void 240239268Sgonzovfp_restore(struct vfp_state *vfpsave) 241239268Sgonzo{ 242263914Sandrew uint32_t fpexc; 243239268Sgonzo 244276518Sian /* On vfpv3 we may need to restore FPINST and FPINST2 */ 245263914Sandrew fpexc = vfpsave->fpexec; 246263914Sandrew if (fpexc & VFPEXC_EX) { 247276518Sian fmxr(fpinst, vfpsave->fpinst); 248263914Sandrew if (fpexc & VFPEXC_FP2V) 249276518Sian fmxr(fpinst2, vfpsave->fpinst2); 250263914Sandrew } 251276518Sian fmxr(fpscr, vfpsave->fpscr); 252263914Sandrew 253276518Sian __asm __volatile( 254276518Sian " .fpu vfpv2\n" 255276518Sian " .fpu vfpv3\n" 256276518Sian " vldmia %0!, {d0-d15}\n" /* d0-d15 */ 257276518Sian " cmp %1, #0\n" /* -D16 or -D32? */ 258276518Sian " vldmiane %0!, {d16-d31}\n" /* d16-d31 */ 259276518Sian " addeq %0, %0, #128\n" /* skip missing regs */ 260276518Sian : "+&r" (vfpsave) : "r" (is_d32) : "cc" 261276518Sian ); 262263914Sandrew 263276518Sian fmxr(fpexc, fpexc); 264239268Sgonzo} 265239268Sgonzo 266262941Sian/* 267262941Sian * If the VFP is on, save its current state and turn it off if requested to do 268262941Sian * so. If the VFP is not on, does not change the values at *vfpsave. Caller is 269262941Sian * responsible for preventing a context switch while this is running. 270239268Sgonzo */ 271239268Sgonzovoid 272262941Sianvfp_store(struct vfp_state *vfpsave, boolean_t disable_vfp) 273239268Sgonzo{ 274263914Sandrew uint32_t fpexc; 275239268Sgonzo 276276518Sian fpexc = fmrx(fpexc); /* Is the vfp enabled? */ 277263914Sandrew if (fpexc & VFPEXC_EN) { 278263914Sandrew vfpsave->fpexec = fpexc; 279276518Sian vfpsave->fpscr = fmrx(fpscr); 280263914Sandrew 281276518Sian /* On vfpv3 we may need to save FPINST and FPINST2 */ 282263914Sandrew if (fpexc & VFPEXC_EX) { 283276518Sian vfpsave->fpinst = fmrx(fpinst); 284263914Sandrew if (fpexc & VFPEXC_FP2V) 285276518Sian vfpsave->fpinst2 = fmrx(fpinst2); 286263914Sandrew fpexc &= ~VFPEXC_EX; 287263914Sandrew } 288263914Sandrew 289262941Sian __asm __volatile( 290276518Sian " .fpu vfpv2\n" 291276518Sian " .fpu vfpv3\n" 292276518Sian " vstmia %0!, {d0-d15}\n" /* d0-d15 */ 293276518Sian " cmp %1, #0\n" /* -D16 or -D32? */ 294276518Sian " vstmiane r0!, {d16-d31}\n" /* d16-d31 */ 295276518Sian " addeq %0, %0, #128\n" /* skip missing regs */ 296276518Sian : "+&r" (vfpsave) : "r" (is_d32) : "cc" 297276518Sian ); 298263914Sandrew 299262941Sian if (disable_vfp) 300276518Sian fmxr(fpexc , fpexc & ~VFPEXC_EN); 301239268Sgonzo } 302239268Sgonzo} 303239268Sgonzo 304262941Sian/* 305262948Sian * The current thread is dying. If the state currently in the hardware belongs 306262948Sian * to the current thread, set fpcurthread to NULL to indicate that the VFP 307262948Sian * hardware state does not belong to any thread. If the VFP is on, turn it off. 308262948Sian * Called only from cpu_throw(), so we don't have to worry about a context 309262948Sian * switch here. 310239268Sgonzo */ 311239268Sgonzovoid 312262948Sianvfp_discard(struct thread *td) 313239268Sgonzo{ 314262941Sian u_int tmp; 315239268Sgonzo 316262948Sian if (PCPU_GET(fpcurthread) == td) 317262948Sian PCPU_SET(fpcurthread, NULL); 318262948Sian 319276518Sian tmp = fmrx(fpexc); 320262948Sian if (tmp & VFPEXC_EN) 321276518Sian fmxr(fpexc, tmp & ~VFPEXC_EN); 322239268Sgonzo} 323239268Sgonzo 324254461Sandrew#endif 325254461Sandrew 326