vfp.c revision 266159
1/* 2 * Copyright (c) 2012 Mark Tinguely 3 * 4 * All rights reserved. 5 * 6 * Redistribution and use in source and binary forms, with or without 7 * modification, are permitted provided that the following conditions 8 * are met: 9 * 1. Redistributions of source code must retain the above copyright 10 * notice, this list of conditions and the following disclaimer. 11 * 2. Redistributions in binary form must reproduce the above copyright 12 * notice, this list of conditions and the following disclaimer in the 13 * documentation and/or other materials provided with the distribution. 14 * 15 * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND 16 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 17 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 18 * ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE 19 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 20 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS 21 * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 22 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 23 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY 24 * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF 25 * SUCH DAMAGE. 26 */ 27 28#include <sys/cdefs.h> 29__FBSDID("$FreeBSD: stable/10/sys/arm/arm/vfp.c 266159 2014-05-15 16:59:47Z ian $"); 30 31#ifdef VFP 32#include <sys/param.h> 33#include <sys/systm.h> 34#include <sys/proc.h> 35#include <sys/kernel.h> 36 37#include <machine/frame.h> 38#include <machine/fp.h> 39#include <machine/pcb.h> 40#include <machine/undefined.h> 41#include <machine/vfp.h> 42 43/* function prototypes */ 44unsigned int get_coprocessorACR(void); 45int vfp_bounce(u_int, u_int, struct trapframe *, int); 46void vfp_discard(void); 47void vfp_enable(void); 48void vfp_restore(struct vfp_state *); 49void vfp_store(struct vfp_state *); 50void set_coprocessorACR(u_int); 51 52extern int vfp_exists; 53static struct undefined_handler vfp10_uh, vfp11_uh; 54/* If true the VFP unit has 32 double registers, otherwise it has 16 */ 55static int is_d32; 56 57/* The VFMXR command using coprocessor commands */ 58#define fmxr(reg, val) \ 59 __asm __volatile("mcr p10, 7, %0, " __STRING(reg) " , c0, 0" :: "r"(val)); 60 61/* The VFMRX command using coprocessor commands */ 62#define fmrx(reg) \ 63({ u_int val = 0;\ 64 __asm __volatile("mrc p10, 7, %0, " __STRING(reg) " , c0, 0" : "=r"(val));\ 65 val; \ 66}) 67 68u_int 69get_coprocessorACR(void) 70{ 71 u_int val; 72 __asm __volatile("mrc p15, 0, %0, c1, c0, 2" : "=r" (val) : : "cc"); 73 return val; 74} 75 76void 77set_coprocessorACR(u_int val) 78{ 79 __asm __volatile("mcr p15, 0, %0, c1, c0, 2\n\t" 80 : : "r" (val) : "cc"); 81 isb(); 82} 83 84 85 /* called for each cpu */ 86void 87vfp_init(void) 88{ 89 u_int fpsid, fpexc, tmp; 90 u_int coproc, vfp_arch; 91 92 coproc = get_coprocessorACR(); 93 coproc |= COPROC10 | COPROC11; 94 set_coprocessorACR(coproc); 95 96 fpsid = fmrx(VFPSID); /* read the vfp system id */ 97 fpexc = fmrx(VFPEXC); /* read the vfp exception reg */ 98 99 if (!(fpsid & VFPSID_HARDSOFT_IMP)) { 100 vfp_exists = 1; 101 is_d32 = 0; 102 PCPU_SET(vfpsid, fpsid); /* save the VFPSID */ 103 104 vfp_arch = 105 (fpsid & VFPSID_SUBVERSION2_MASK) >> VFPSID_SUBVERSION_OFF; 106 107 if (vfp_arch >= VFP_ARCH3) { 108 tmp = fmrx(VMVFR0); 109 PCPU_SET(vfpmvfr0, tmp); 110 111 if ((tmp & VMVFR0_RB_MASK) == 2) 112 is_d32 = 1; 113 114 tmp = fmrx(VMVFR1); 115 PCPU_SET(vfpmvfr1, tmp); 116 } 117 118 /* initialize the coprocess 10 and 11 calls 119 * These are called to restore the registers and enable 120 * the VFP hardware. 121 */ 122 if (vfp10_uh.uh_handler == NULL) { 123 vfp10_uh.uh_handler = vfp_bounce; 124 vfp11_uh.uh_handler = vfp_bounce; 125 install_coproc_handler_static(10, &vfp10_uh); 126 install_coproc_handler_static(11, &vfp11_uh); 127 } 128 } 129} 130 131SYSINIT(vfp, SI_SUB_CPU, SI_ORDER_ANY, vfp_init, NULL); 132 133 134/* start VFP unit, restore the vfp registers from the PCB and retry 135 * the instruction 136 */ 137int 138vfp_bounce(u_int addr, u_int insn, struct trapframe *frame, int code) 139{ 140 u_int fpexc; 141 struct pcb *curpcb; 142 struct thread *vfptd; 143 int i; 144 145 if (!vfp_exists) 146 return 1; /* vfp does not exist */ 147 i = disable_interrupts(I32_bit|F32_bit); 148 fpexc = fmrx(VFPEXC); /* read the vfp exception reg */ 149 if (fpexc & VFPEXC_EN) { 150 vfptd = PCPU_GET(vfpcthread); 151 /* did the kernel call the vfp or exception that expect us 152 * to emulate the command. Newer hardware does not require 153 * emulation, so we don't emulate yet. 154 */ 155#ifdef SMP 156 /* don't save if newer registers are on another processor */ 157 if (vfptd /* && (vfptd == curthread) */ && 158 (vfptd->td_pcb->pcb_vfpcpu == PCPU_GET(cpu))) 159#else 160 /* someone did not save their registers, */ 161 if (vfptd /* && (vfptd == curthread) */) 162#endif 163 vfp_store(&vfptd->td_pcb->pcb_vfpstate); 164 165 fpexc &= ~VFPEXC_EN; 166 fmxr(VFPEXC, fpexc); /* turn vfp hardware off */ 167 if (vfptd == curthread) { 168 /* kill the process - we do not handle emulation */ 169 restore_interrupts(i); 170 killproc(curthread->td_proc, "vfp emulation"); 171 return 1; 172 } 173 /* should not happen. someone did not save their context */ 174 printf("vfp_bounce: vfpcthread: %p curthread: %p\n", 175 vfptd, curthread); 176 } 177 fpexc |= VFPEXC_EN; 178 fmxr(VFPEXC, fpexc); /* enable the vfp and repeat command */ 179 curpcb = curthread->td_pcb; 180 /* If we were the last process to use the VFP, the process did not 181 * use a VFP on another processor, then the registers in the VFP 182 * will still be ours and are current. Eventually, we will make the 183 * restore smarter. 184 */ 185 vfp_restore(&curpcb->pcb_vfpstate); 186#ifdef SMP 187 curpcb->pcb_vfpcpu = PCPU_GET(cpu); 188#endif 189 PCPU_SET(vfpcthread, curthread); 190 restore_interrupts(i); 191 return 0; 192} 193 194/* vfs_store is called from from a VFP command to restore the registers and 195 * turn on the VFP hardware. 196 * Eventually we will use the information that this process was the last 197 * to use the VFP hardware and bypass the restore, just turn on the hardware. 198 */ 199void 200vfp_restore(struct vfp_state *vfpsave) 201{ 202 u_int vfpscr = 0; 203 204 /* 205 * Work around an issue with GCC where the asm it generates is 206 * not unified syntax and fails to assemble because it expects 207 * the ldcleq instruction in the form ldc<c>l, not in the UAL 208 * form ldcl<c>, and similar for stcleq. 209 */ 210#ifdef __clang__ 211#define ldclne "ldclne" 212#define stclne "stclne" 213#else 214#define ldclne "ldcnel" 215#define stclne "stcnel" 216#endif 217 if (vfpsave) { 218 __asm __volatile("ldc p10, c0, [%1], #128\n" /* d0-d15 */ 219 "cmp %2, #0\n" /* -D16 or -D32? */ 220 ldclne" p11, c0, [%1], #128\n" /* d16-d31 */ 221 "addeq %1, %1, #128\n" /* skip missing regs */ 222 "ldr %0, [%1]\n" /* set old vfpscr */ 223 "mcr p10, 7, %0, cr1, c0, 0\n" 224 : "=&r" (vfpscr) : "r" (vfpsave), "r" (is_d32) : "cc"); 225 } 226} 227 228/* vfs_store is called from switch to save the vfp hardware registers 229 * into the pcb before switching to another process. 230 * we already know that the new process is different from this old 231 * process and that this process last used the VFP registers. 232 * Below we check to see if the VFP has been enabled since the last 233 * register save. 234 * This routine will exit with the VFP turned off. The next VFP user 235 * will trap to restore its registers and turn on the VFP hardware. 236 */ 237void 238vfp_store(struct vfp_state *vfpsave) 239{ 240 u_int tmp, vfpscr = 0; 241 242 tmp = fmrx(VFPEXC); /* Is the vfp enabled? */ 243 if (vfpsave && (tmp & VFPEXC_EN)) { 244 __asm __volatile("stc p11, c0, [%1], #128\n" /* d0-d15 */ 245 "cmp %2, #0\n" /* -D16 or -D32? */ 246 stclne" p11, c0, [%1], #128\n" /* d16-d31 */ 247 "addeq %1, %1, #128\n" /* skip missing regs */ 248 "mrc p10, 7, %0, cr1, c0, 0\n" /* fmxr(VFPSCR) */ 249 "str %0, [%1]\n" /* save vfpscr */ 250 : "=&r" (vfpscr) : "r" (vfpsave), "r" (is_d32) : "cc"); 251 } 252#undef ldcleq 253#undef stcleq 254 255#ifndef SMP 256 /* eventually we will use this information for UP also */ 257 PCPU_SET(vfpcthread, 0); 258#endif 259 tmp &= ~VFPEXC_EN; /* disable the vfp hardware */ 260 fmxr(VFPEXC , tmp); 261} 262 263/* discard the registers at cpu_thread_free() when fpcurthread == td. 264 * Turn off the VFP hardware. 265 */ 266void 267vfp_discard() 268{ 269 u_int tmp = 0; 270 271 /* 272 * No need to protect the access to vfpcthread by disabling 273 * interrupts, since it's called from cpu_throw(), who is called 274 * with interrupts disabled. 275 */ 276 277 PCPU_SET(vfpcthread, 0); /* permanent forget about reg */ 278 tmp = fmrx(VFPEXC); 279 tmp &= ~VFPEXC_EN; /* turn off VFP hardware */ 280 fmxr(VFPEXC, tmp); 281} 282 283/* Enable the VFP hardware without restoring registers. 284 * Called when the registers are still in the VFP unit 285 */ 286void 287vfp_enable() 288{ 289 u_int tmp = 0; 290 291 tmp = fmrx(VFPEXC); 292 tmp |= VFPEXC_EN; 293 fmxr(VFPEXC, tmp); 294} 295#endif 296 297