vfp.c revision 262941
1/*- 2 * Copyright (c) 2014 Ian Lepore <ian@freebsd.org> 3 * Copyright (c) 2012 Mark Tinguely 4 * 5 * All rights reserved. 6 * 7 * Redistribution and use in source and binary forms, with or without 8 * modification, are permitted provided that the following conditions 9 * are met: 10 * 1. Redistributions of source code must retain the above copyright 11 * notice, this list of conditions and the following disclaimer. 12 * 2. Redistributions in binary form must reproduce the above copyright 13 * notice, this list of conditions and the following disclaimer in the 14 * documentation and/or other materials provided with the distribution. 15 * 16 * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND 17 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 18 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 19 * ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE 20 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 21 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS 22 * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 23 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 24 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY 25 * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF 26 * SUCH DAMAGE. 27 */ 28 29#include <sys/cdefs.h> 30__FBSDID("$FreeBSD: head/sys/arm/arm/vfp.c 262941 2014-03-09 03:00:03Z ian $"); 31 32#ifdef VFP 33#include <sys/param.h> 34#include <sys/systm.h> 35#include <sys/proc.h> 36#include <sys/kernel.h> 37 38#include <machine/armreg.h> 39#include <machine/frame.h> 40#include <machine/fp.h> 41#include <machine/pcb.h> 42#include <machine/undefined.h> 43#include <machine/vfp.h> 44 45/* function prototypes */ 46static int vfp_bounce(u_int, u_int, struct trapframe *, int); 47static void vfp_restore(struct vfp_state *); 48 49extern int vfp_exists; 50static struct undefined_handler vfp10_uh, vfp11_uh; 51/* If true the VFP unit has 32 double registers, otherwise it has 16 */ 52static int is_d32; 53 54/* The VFMXR command using coprocessor commands */ 55#define fmxr(reg, val) \ 56 __asm __volatile("mcr p10, 7, %0, " __STRING(reg) " , c0, 0" :: "r"(val)); 57 58/* The VFMRX command using coprocessor commands */ 59#define fmrx(reg) \ 60({ u_int val = 0;\ 61 __asm __volatile("mrc p10, 7, %0, " __STRING(reg) " , c0, 0" : "=r"(val));\ 62 val; \ 63}) 64 65/* 66 * Work around an issue with GCC where the asm it generates is not unified 67 * syntax and fails to assemble because it expects the ldcleq instruction in the 68 * form ldc<c>l, not in the UAL form ldcl<c>, and similar for stcleq. 69 */ 70#ifdef __clang__ 71#define LDCLNE "ldclne " 72#define STCLNE "stclne " 73#else 74#define LDCLNE "ldcnel " 75#define STCLNE "stcnel " 76#endif 77 78static u_int 79get_coprocessorACR(void) 80{ 81 u_int val; 82 __asm __volatile("mrc p15, 0, %0, c1, c0, 2" : "=r" (val) : : "cc"); 83 return val; 84} 85 86static void 87set_coprocessorACR(u_int val) 88{ 89 __asm __volatile("mcr p15, 0, %0, c1, c0, 2\n\t" 90 : : "r" (val) : "cc"); 91 isb(); 92} 93 94 95 /* called for each cpu */ 96void 97vfp_init(void) 98{ 99 u_int fpsid, fpexc, tmp; 100 u_int coproc, vfp_arch; 101 102 coproc = get_coprocessorACR(); 103 coproc |= COPROC10 | COPROC11; 104 set_coprocessorACR(coproc); 105 106 fpsid = fmrx(VFPSID); /* read the vfp system id */ 107 fpexc = fmrx(VFPEXC); /* read the vfp exception reg */ 108 109 if (!(fpsid & VFPSID_HARDSOFT_IMP)) { 110 vfp_exists = 1; 111 is_d32 = 0; 112 PCPU_SET(vfpsid, fpsid); /* save the VFPSID */ 113 114 vfp_arch = 115 (fpsid & VFPSID_SUBVERSION2_MASK) >> VFPSID_SUBVERSION_OFF; 116 117 if (vfp_arch >= VFP_ARCH3) { 118 tmp = fmrx(VMVFR0); 119 PCPU_SET(vfpmvfr0, tmp); 120 121 if ((tmp & VMVFR0_RB_MASK) == 2) 122 is_d32 = 1; 123 124 tmp = fmrx(VMVFR1); 125 PCPU_SET(vfpmvfr1, tmp); 126 } 127 128 /* initialize the coprocess 10 and 11 calls 129 * These are called to restore the registers and enable 130 * the VFP hardware. 131 */ 132 if (vfp10_uh.uh_handler == NULL) { 133 vfp10_uh.uh_handler = vfp_bounce; 134 vfp11_uh.uh_handler = vfp_bounce; 135 install_coproc_handler_static(10, &vfp10_uh); 136 install_coproc_handler_static(11, &vfp11_uh); 137 } 138 } 139} 140 141SYSINIT(vfp, SI_SUB_CPU, SI_ORDER_ANY, vfp_init, NULL); 142 143 144/* start VFP unit, restore the vfp registers from the PCB and retry 145 * the instruction 146 */ 147static int 148vfp_bounce(u_int addr, u_int insn, struct trapframe *frame, int code) 149{ 150 u_int cpu, fpexc; 151 struct pcb *curpcb; 152 153 if ((code & FAULT_USER) == 0) 154 panic("undefined floating point instruction in supervisor mode"); 155 156 critical_enter(); 157 158 /* 159 * If the VFP is already on and we got an undefined instruction, then 160 * something tried to executate a truly invalid instruction that maps to 161 * the VFP. 162 */ 163 fpexc = fmrx(VFPEXC); 164 if (fpexc & VFPEXC_EN) { 165 /* kill the process - we do not handle emulation */ 166 critical_exit(); 167 killproc(curthread->td_proc, "vfp emulation"); 168 return 1; 169 } 170 171 /* 172 * If the last time this thread used the VFP it was on this core, and 173 * the last thread to use the VFP on this core was this thread, then the 174 * VFP state is valid, otherwise restore this thread's state to the VFP. 175 */ 176 fmxr(VFPEXC, fpexc | VFPEXC_EN); 177 curpcb = curthread->td_pcb; 178 cpu = PCPU_GET(cpu); 179 if (curpcb->pcb_vfpcpu != cpu || curthread != PCPU_GET(fpcurthread)) { 180 vfp_restore(&curpcb->pcb_vfpstate); 181 curpcb->pcb_vfpcpu = cpu; 182 PCPU_SET(fpcurthread, curthread); 183 } 184 185 critical_exit(); 186 return (0); 187} 188 189/* 190 * Restore the given state to the VFP hardware. 191 */ 192static void 193vfp_restore(struct vfp_state *vfpsave) 194{ 195 u_int vfpscr = 0; 196 197 __asm __volatile("ldc p10, c0, [%1], #128\n" /* d0-d15 */ 198 "cmp %2, #0\n" /* -D16 or -D32? */ 199 LDCLNE "p11, c0, [%1], #128\n" /* d16-d31 */ 200 "addeq %1, %1, #128\n" /* skip missing regs */ 201 "ldr %0, [%1]\n" /* set old vfpscr */ 202 "mcr p10, 7, %0, cr1, c0, 0\n" 203 : "=&r" (vfpscr) : "r" (vfpsave), "r" (is_d32) : "cc"); 204} 205 206/* 207 * If the VFP is on, save its current state and turn it off if requested to do 208 * so. If the VFP is not on, does not change the values at *vfpsave. Caller is 209 * responsible for preventing a context switch while this is running. 210 */ 211void 212vfp_store(struct vfp_state *vfpsave, boolean_t disable_vfp) 213{ 214 u_int tmp, vfpscr; 215 216 tmp = fmrx(VFPEXC); /* Is the vfp enabled? */ 217 if (tmp & VFPEXC_EN) { 218 __asm __volatile( 219 "stc p11, c0, [%1], #128\n" /* d0-d15 */ 220 "cmp %2, #0\n" /* -D16 or -D32? */ 221 STCLNE "p11, c0, [%1], #128\n" /* d16-d31 */ 222 "addeq %1, %1, #128\n" /* skip missing regs */ 223 "mrc p10, 7, %0, cr1, c0, 0\n" /* fmxr(VFPSCR) */ 224 "str %0, [%1]\n" /* save vfpscr */ 225 : "=&r" (vfpscr) : "r" (vfpsave), "r" (is_d32) : "cc"); 226 if (disable_vfp) 227 fmxr(VFPEXC , tmp & ~VFPEXC_EN); 228 } 229} 230 231/* 232 * If the VFP hardware is on, the current thread was using it but now that 233 * thread is dying. Turn off the VFP and set pcpu fpcurthread to 0, to indicate 234 * that the VFP hardware state does not belong to any thread. Called only from 235 * cpu_throw(), so we don't have to worry about a context switch here. 236 */ 237void 238vfp_discard() 239{ 240 u_int tmp; 241 242 tmp = fmrx(VFPEXC); 243 if (tmp & VFPEXC_EN) { 244 fmxr(VFPEXC, tmp & ~VFPEXC_EN); 245 PCPU_SET(fpcurthread, 0); 246 } 247} 248 249#endif 250 251