vfp.c revision 247340
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#include <sys/cdefs.h> 28__FBSDID("$FreeBSD: head/sys/arm/arm/vfp.c 247340 2013-02-26 19:59:52Z cognet $"); 29 30 31#include <sys/param.h> 32#include <sys/systm.h> 33#include <sys/proc.h> 34#include <sys/kernel.h> 35 36#include <machine/fp.h> 37#include <machine/pcb.h> 38#include <machine/undefined.h> 39#include <machine/vfp.h> 40 41/* function prototypes */ 42unsigned int get_coprocessorACR(void); 43int vfp_bounce(u_int, u_int, struct trapframe *, int); 44void vfp_discard(void); 45void vfp_enable(void); 46void vfp_restore(struct vfp_state *); 47void vfp_store(struct vfp_state *); 48void set_coprocessorACR(u_int); 49 50boolean_t vfp_exists; 51static struct undefined_handler vfp10_uh, vfp11_uh; 52 53/* The VFMXR command using coprocessor commands */ 54#define fmxr(reg, val) \ 55 __asm __volatile("mcr p10, 7, %0, " #reg " , c0, 0" :: "r" (val)); 56 57/* The VFMRX command using coprocessor commands */ 58#define fmrx(reg) \ 59({ u_int val = 0;\ 60 __asm __volatile("mrc p10, 7, %0, " #reg " , c0, 0" : "=r" (val));\ 61 val; \ 62}) 63 64u_int 65get_coprocessorACR(void) 66{ 67 u_int val; 68 __asm __volatile("mrc p15, 0, %0, c1, c0, 2" : "=r" (val) : : "cc"); 69 return val; 70} 71 72void 73set_coprocessorACR(u_int val) 74{ 75 __asm __volatile("mcr p15, 0, %0, c1, c0, 2\n\t" 76 : : "r" (val) : "cc"); 77 isb(); 78} 79 80 81 /* called for each cpu */ 82void 83vfp_init(void) 84{ 85 u_int fpsid, fpexc, tmp; 86 u_int coproc; 87 88 coproc = get_coprocessorACR(); 89 coproc |= COPROC10 | COPROC11; 90 set_coprocessorACR(coproc); 91 92 fpsid = fmrx(cr0); /* read the vfp system id */ 93 fpexc = fmrx(cr8); /* read the vfp exception reg */ 94 95 if (!(fpsid & VFPSID_HARDSOFT_IMP)) { 96 vfp_exists = 1; 97 PCPU_SET(vfpsid, fpsid); /* save the VFPSID */ 98 if ((fpsid & VFPSID_SUBVERSION2_MASK) == VFP_ARCH3) { 99 tmp = fmrx(cr7); /* extended registers */ 100 PCPU_SET(vfpmvfr0, tmp); 101 tmp = fmrx(cr6); /* extended registers */ 102 PCPU_SET(vfpmvfr1, tmp); 103 } 104 /* initialize the coprocess 10 and 11 calls 105 * These are called to restore the registers and enable 106 * the VFP hardware. 107 */ 108 if (vfp10_uh.uh_handler == NULL) { 109 vfp10_uh.uh_handler = vfp_bounce; 110 vfp11_uh.uh_handler = vfp_bounce; 111 install_coproc_handler_static(10, &vfp10_uh); 112 install_coproc_handler_static(11, &vfp11_uh); 113 } 114 } 115} 116 117SYSINIT(vfp, SI_SUB_CPU, SI_ORDER_ANY, vfp_init, NULL); 118 119 120/* start VFP unit, restore the vfp registers from the PCB and retry 121 * the instruction 122 */ 123int 124vfp_bounce(u_int addr, u_int insn, struct trapframe *frame, int code) 125{ 126 u_int fpexc; 127 struct pcb *curpcb; 128 struct thread *vfptd; 129 130 if (!vfp_exists) 131 return 1; /* vfp does not exist */ 132 fpexc = fmrx(cr8); /* read the vfp exception reg */ 133 if (fpexc & VFPEXC_EN) { 134 vfptd = PCPU_GET(vfpcthread); 135 /* did the kernel call the vfp or exception that expect us 136 * to emulate the command. Newer hardware does not require 137 * emulation, so we don't emulate yet. 138 */ 139#ifdef SMP 140 /* don't save if newer registers are on another processor */ 141 if (vfptd /* && (vfptd == curthread) */ && 142 (vfptd->td_pcb->pcb_vfpcpu == PCPU_GET(cpu))) 143#else 144 /* someone did not save their registers, */ 145 if (vfptd /* && (vfptd == curthread) */) 146#endif 147 vfp_store(&vfptd->td_pcb->pcb_vfpstate); 148 149 fpexc &= ~VFPEXC_EN; 150 fmxr(cr8, fpexc); /* turn vfp hardware off */ 151 if (vfptd == curthread) { 152 /* kill the process - we do not handle emulation */ 153 killproc(curthread->td_proc, "vfp emulation"); 154 return 1; 155 } 156 /* should not happen. someone did not save their context */ 157 printf("vfp_bounce: vfpcthread: %p curthread: %p\n", 158 vfptd, curthread); 159 } 160 fpexc |= VFPEXC_EN; 161 fmxr(cr8, fpexc); /* enable the vfp and repeat command */ 162 curpcb = PCPU_GET(curpcb); 163 /* If we were the last process to use the VFP, the process did not 164 * use a VFP on another processor, then the registers in the VFP 165 * will still be ours and are current. Eventually, we will make the 166 * restore smarter. 167 */ 168 vfp_restore(&curpcb->pcb_vfpstate); 169#ifdef SMP 170 curpcb->pcb_vfpcpu = PCPU_GET(cpu); 171#endif 172 PCPU_SET(vfpcthread, PCPU_GET(curthread)); 173 return 0; 174} 175 176/* vfs_store is called from from a VFP command to restore the registers and 177 * turn on the VFP hardware. 178 * Eventually we will use the information that this process was the last 179 * to use the VFP hardware and bypass the restore, just turn on the hardware. 180 */ 181void 182vfp_restore(struct vfp_state *vfpsave) 183{ 184 u_int vfpscr = 0; 185 186 if (vfpsave) { 187 __asm __volatile("ldc p10, c0, [%0], #128\n" /* d0-d31 */ 188#ifndef VFPv2 189 "ldcl p11, c0, [%0], #128\n" /* d16-d31 */ 190#else 191 "add %0, %0, #128\n" /* slip missing regs */ 192#endif 193 "ldr %1, [%0]\n" /* set old vfpscr */ 194 "mcr p10, 7, %1, cr1, c0, 0\n" 195 :: "r" (vfpsave), "r" (vfpscr)); 196 PCPU_SET(vfpcthread, PCPU_GET(curthread)); 197 } 198} 199 200/* vfs_store is called from switch to save the vfp hardware registers 201 * into the pcb before switching to another process. 202 * we already know that the new process is different from this old 203 * process and that this process last used the VFP registers. 204 * Below we check to see if the VFP has been enabled since the last 205 * register save. 206 * This routine will exit with the VFP turned off. The next VFP user 207 * will trap to restore its registers and turn on the VFP hardware. 208 */ 209void 210vfp_store(struct vfp_state *vfpsave) 211{ 212 u_int tmp, vfpscr = 0; 213 214 tmp = fmrx(cr8); /* Is the vfp enabled? */ 215 if (vfpsave && tmp & VFPEXC_EN) { 216 __asm __volatile("stc p11, c0, [%1], #128\n" /* d0-d31 */ 217#ifndef VFPv2 218 "stcl p11, c0, [%1], #128\n" 219#else 220 "add %1, %1, #128\n" 221#endif 222 "mrc p10, 7, %0, cr1, c0, 0\n" 223 "str %0, [%1]\n" 224 : "=&r" (vfpscr) : "r" (vfpsave)); 225 } 226#ifndef SMP 227 /* eventually we will use this information for UP also */ 228 PCPU_SET(vfpcthread, 0); 229#endif 230 tmp &= ~VFPEXC_EN; /* disable the vfp hardware */ 231 fmxr(cr8 , tmp); 232} 233 234/* discard the registers at cpu_thread_free() when fpcurthread == td. 235 * Turn off the VFP hardware. 236 */ 237void 238vfp_discard() 239{ 240 u_int tmp = 0; 241 242 PCPU_SET(vfpcthread, 0); /* permanent forget about reg */ 243 tmp = fmrx(cr8); 244 tmp &= ~VFPEXC_EN; /* turn off VFP hardware */ 245 fmxr(cr8, tmp); 246} 247 248/* Enable the VFP hardware without restoring registers. 249 * Called when the registers are still in the VFP unit 250 */ 251void 252vfp_enable() 253{ 254 u_int tmp = 0; 255 256 tmp = fmrx(cr8); 257 tmp |= VFPEXC_EN; 258 fmxr(cr8 , tmp); 259} 260