1266277Sian/*- 2266277Sian * 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$"); 31239268Sgonzo 32254461Sandrew#ifdef VFP 33239268Sgonzo#include <sys/param.h> 34239268Sgonzo#include <sys/systm.h> 35239268Sgonzo#include <sys/proc.h> 36239268Sgonzo#include <sys/kernel.h> 37239268Sgonzo 38266277Sian#include <machine/armreg.h> 39259329Sian#include <machine/frame.h> 40239268Sgonzo#include <machine/fp.h> 41239268Sgonzo#include <machine/pcb.h> 42239268Sgonzo#include <machine/undefined.h> 43239268Sgonzo#include <machine/vfp.h> 44239268Sgonzo 45239268Sgonzo/* function prototypes */ 46266160Sianstatic int vfp_bounce(u_int, u_int, struct trapframe *, int); 47266160Sianstatic void vfp_restore(struct vfp_state *); 48239268Sgonzo 49249176Sandrewextern int vfp_exists; 50239268Sgonzostatic struct undefined_handler vfp10_uh, vfp11_uh; 51251712Sandrew/* If true the VFP unit has 32 double registers, otherwise it has 16 */ 52251712Sandrewstatic int is_d32; 53239268Sgonzo 54239268Sgonzo/* The VFMXR command using coprocessor commands */ 55239268Sgonzo#define fmxr(reg, val) \ 56251712Sandrew __asm __volatile("mcr p10, 7, %0, " __STRING(reg) " , c0, 0" :: "r"(val)); 57239268Sgonzo 58239268Sgonzo/* The VFMRX command using coprocessor commands */ 59239268Sgonzo#define fmrx(reg) \ 60239268Sgonzo({ u_int val = 0;\ 61251712Sandrew __asm __volatile("mrc p10, 7, %0, " __STRING(reg) " , c0, 0" : "=r"(val));\ 62251712Sandrew val; \ 63239268Sgonzo}) 64239268Sgonzo 65266277Sian/* 66266277Sian * Work around an issue with GCC where the asm it generates is not unified 67266277Sian * syntax and fails to assemble because it expects the ldcleq instruction in the 68266277Sian * form ldc<c>l, not in the UAL form ldcl<c>, and similar for stcleq. 69266277Sian */ 70266277Sian#ifdef __clang__ 71266277Sian#define LDCLNE "ldclne " 72266277Sian#define STCLNE "stclne " 73266277Sian#else 74266277Sian#define LDCLNE "ldcnel " 75266277Sian#define STCLNE "stcnel " 76266277Sian#endif 77266277Sian 78266277Sianstatic u_int 79239268Sgonzoget_coprocessorACR(void) 80239268Sgonzo{ 81239268Sgonzo u_int val; 82239268Sgonzo __asm __volatile("mrc p15, 0, %0, c1, c0, 2" : "=r" (val) : : "cc"); 83239268Sgonzo return val; 84239268Sgonzo} 85239268Sgonzo 86266277Sianstatic void 87239268Sgonzoset_coprocessorACR(u_int val) 88239268Sgonzo{ 89239268Sgonzo __asm __volatile("mcr p15, 0, %0, c1, c0, 2\n\t" 90239268Sgonzo : : "r" (val) : "cc"); 91247340Scognet isb(); 92239268Sgonzo} 93239268Sgonzo 94239268Sgonzo 95239268Sgonzo /* called for each cpu */ 96239268Sgonzovoid 97239268Sgonzovfp_init(void) 98239268Sgonzo{ 99239268Sgonzo u_int fpsid, fpexc, tmp; 100251712Sandrew u_int coproc, vfp_arch; 101239268Sgonzo 102239268Sgonzo coproc = get_coprocessorACR(); 103239268Sgonzo coproc |= COPROC10 | COPROC11; 104239268Sgonzo set_coprocessorACR(coproc); 105239268Sgonzo 106251712Sandrew fpsid = fmrx(VFPSID); /* read the vfp system id */ 107251712Sandrew fpexc = fmrx(VFPEXC); /* read the vfp exception reg */ 108239268Sgonzo 109239268Sgonzo if (!(fpsid & VFPSID_HARDSOFT_IMP)) { 110239268Sgonzo vfp_exists = 1; 111251712Sandrew is_d32 = 0; 112239268Sgonzo PCPU_SET(vfpsid, fpsid); /* save the VFPSID */ 113251712Sandrew 114251712Sandrew vfp_arch = 115251712Sandrew (fpsid & VFPSID_SUBVERSION2_MASK) >> VFPSID_SUBVERSION_OFF; 116251712Sandrew 117251712Sandrew if (vfp_arch >= VFP_ARCH3) { 118251712Sandrew tmp = fmrx(VMVFR0); 119239268Sgonzo PCPU_SET(vfpmvfr0, tmp); 120251712Sandrew 121251712Sandrew if ((tmp & VMVFR0_RB_MASK) == 2) 122251712Sandrew is_d32 = 1; 123251712Sandrew 124251712Sandrew tmp = fmrx(VMVFR1); 125239268Sgonzo PCPU_SET(vfpmvfr1, tmp); 126239268Sgonzo } 127251712Sandrew 128239268Sgonzo /* initialize the coprocess 10 and 11 calls 129239268Sgonzo * These are called to restore the registers and enable 130239268Sgonzo * the VFP hardware. 131239268Sgonzo */ 132239268Sgonzo if (vfp10_uh.uh_handler == NULL) { 133239268Sgonzo vfp10_uh.uh_handler = vfp_bounce; 134239268Sgonzo vfp11_uh.uh_handler = vfp_bounce; 135239268Sgonzo install_coproc_handler_static(10, &vfp10_uh); 136239268Sgonzo install_coproc_handler_static(11, &vfp11_uh); 137239268Sgonzo } 138239268Sgonzo } 139239268Sgonzo} 140239268Sgonzo 141239268SgonzoSYSINIT(vfp, SI_SUB_CPU, SI_ORDER_ANY, vfp_init, NULL); 142239268Sgonzo 143239268Sgonzo 144239268Sgonzo/* start VFP unit, restore the vfp registers from the PCB and retry 145239268Sgonzo * the instruction 146239268Sgonzo */ 147266160Sianstatic int 148239268Sgonzovfp_bounce(u_int addr, u_int insn, struct trapframe *frame, int code) 149239268Sgonzo{ 150266277Sian u_int cpu, fpexc; 151239268Sgonzo struct pcb *curpcb; 152266341Sian ksiginfo_t ksi; 153239268Sgonzo 154266277Sian if ((code & FAULT_USER) == 0) 155266277Sian panic("undefined floating point instruction in supervisor mode"); 156266277Sian 157266277Sian critical_enter(); 158266277Sian 159266277Sian /* 160266277Sian * If the VFP is already on and we got an undefined instruction, then 161266277Sian * something tried to executate a truly invalid instruction that maps to 162266277Sian * the VFP. 163266277Sian */ 164266277Sian fpexc = fmrx(VFPEXC); 165239268Sgonzo if (fpexc & VFPEXC_EN) { 166266341Sian /* Clear any exceptions */ 167266341Sian fmxr(VFPEXC, fpexc & ~(VFPEXC_EX | VFPEXC_FP2V)); 168266341Sian 169266277Sian /* kill the process - we do not handle emulation */ 170266277Sian critical_exit(); 171266341Sian 172266341Sian if (fpexc & VFPEXC_EX) { 173266341Sian /* We have an exception, signal a SIGFPE */ 174266341Sian ksiginfo_init_trap(&ksi); 175266341Sian ksi.ksi_signo = SIGFPE; 176266341Sian if (fpexc & VFPEXC_UFC) 177266341Sian ksi.ksi_code = FPE_FLTUND; 178266341Sian else if (fpexc & VFPEXC_OFC) 179266341Sian ksi.ksi_code = FPE_FLTOVF; 180266341Sian else if (fpexc & VFPEXC_IOC) 181266341Sian ksi.ksi_code = FPE_FLTINV; 182266341Sian ksi.ksi_addr = (void *)addr; 183266341Sian trapsignal(curthread, &ksi); 184266341Sian return 0; 185266341Sian } 186266341Sian 187266277Sian return 1; 188266277Sian } 189239268Sgonzo 190266277Sian /* 191266277Sian * If the last time this thread used the VFP it was on this core, and 192266277Sian * the last thread to use the VFP on this core was this thread, then the 193266277Sian * VFP state is valid, otherwise restore this thread's state to the VFP. 194266277Sian */ 195266277Sian fmxr(VFPEXC, fpexc | VFPEXC_EN); 196266277Sian curpcb = curthread->td_pcb; 197266277Sian cpu = PCPU_GET(cpu); 198266277Sian if (curpcb->pcb_vfpcpu != cpu || curthread != PCPU_GET(fpcurthread)) { 199266277Sian vfp_restore(&curpcb->pcb_vfpstate); 200266277Sian curpcb->pcb_vfpcpu = cpu; 201266277Sian PCPU_SET(fpcurthread, curthread); 202239268Sgonzo } 203266277Sian 204266277Sian critical_exit(); 205266277Sian return (0); 206239268Sgonzo} 207239268Sgonzo 208266277Sian/* 209266277Sian * Restore the given state to the VFP hardware. 210239268Sgonzo */ 211266160Sianstatic void 212239268Sgonzovfp_restore(struct vfp_state *vfpsave) 213239268Sgonzo{ 214266341Sian uint32_t fpexc; 215239268Sgonzo 216266341Sian /* On VFPv2 we may need to restore FPINST and FPINST2 */ 217266341Sian fpexc = vfpsave->fpexec; 218266341Sian if (fpexc & VFPEXC_EX) { 219266341Sian fmxr(VFPINST, vfpsave->fpinst); 220266341Sian if (fpexc & VFPEXC_FP2V) 221266341Sian fmxr(VFPINST2, vfpsave->fpinst2); 222266341Sian } 223266341Sian fmxr(VFPSCR, vfpsave->fpscr); 224266341Sian 225266341Sian __asm __volatile("ldc p10, c0, [%0], #128\n" /* d0-d15 */ 226266341Sian "cmp %1, #0\n" /* -D16 or -D32? */ 227266341Sian LDCLNE "p11, c0, [%0], #128\n" /* d16-d31 */ 228266341Sian "addeq %0, %0, #128\n" /* skip missing regs */ 229266341Sian : : "r" (vfpsave), "r" (is_d32) : "cc"); 230266341Sian 231266341Sian fmxr(VFPEXC, fpexc); 232239268Sgonzo} 233239268Sgonzo 234266277Sian/* 235266277Sian * If the VFP is on, save its current state and turn it off if requested to do 236266277Sian * so. If the VFP is not on, does not change the values at *vfpsave. Caller is 237266277Sian * responsible for preventing a context switch while this is running. 238239268Sgonzo */ 239239268Sgonzovoid 240266277Sianvfp_store(struct vfp_state *vfpsave, boolean_t disable_vfp) 241239268Sgonzo{ 242266341Sian uint32_t fpexc; 243239268Sgonzo 244266341Sian fpexc = fmrx(VFPEXC); /* Is the vfp enabled? */ 245266341Sian if (fpexc & VFPEXC_EN) { 246266341Sian vfpsave->fpexec = fpexc; 247266341Sian vfpsave->fpscr = fmrx(VFPSCR); 248266341Sian 249266341Sian /* On VFPv2 we may need to save FPINST and FPINST2 */ 250266341Sian if (fpexc & VFPEXC_EX) { 251266341Sian vfpsave->fpinst = fmrx(VFPINST); 252266341Sian if (fpexc & VFPEXC_FP2V) 253266341Sian vfpsave->fpinst2 = fmrx(VFPINST2); 254266341Sian fpexc &= ~VFPEXC_EX; 255266341Sian } 256266341Sian 257266277Sian __asm __volatile( 258266341Sian "stc p11, c0, [%0], #128\n" /* d0-d15 */ 259266341Sian "cmp %1, #0\n" /* -D16 or -D32? */ 260266341Sian STCLNE "p11, c0, [%0], #128\n" /* d16-d31 */ 261266341Sian "addeq %0, %0, #128\n" /* skip missing regs */ 262266341Sian : : "r" (vfpsave), "r" (is_d32) : "cc"); 263266341Sian 264266277Sian if (disable_vfp) 265266341Sian fmxr(VFPEXC , fpexc & ~VFPEXC_EN); 266239268Sgonzo } 267239268Sgonzo} 268239268Sgonzo 269266277Sian/* 270266277Sian * The current thread is dying. If the state currently in the hardware belongs 271266277Sian * to the current thread, set fpcurthread to NULL to indicate that the VFP 272266277Sian * hardware state does not belong to any thread. If the VFP is on, turn it off. 273266277Sian * Called only from cpu_throw(), so we don't have to worry about a context 274266277Sian * switch here. 275239268Sgonzo */ 276239268Sgonzovoid 277266277Sianvfp_discard(struct thread *td) 278239268Sgonzo{ 279266277Sian u_int tmp; 280239268Sgonzo 281266277Sian if (PCPU_GET(fpcurthread) == td) 282266277Sian PCPU_SET(fpcurthread, NULL); 283266277Sian 284251712Sandrew tmp = fmrx(VFPEXC); 285266277Sian if (tmp & VFPEXC_EN) 286266277Sian fmxr(VFPEXC, tmp & ~VFPEXC_EN); 287239268Sgonzo} 288239268Sgonzo 289254461Sandrew#endif 290254461Sandrew 291