1cf1a573fSOleksandr Tymoshenko /* 2cf1a573fSOleksandr Tymoshenko * Copyright (c) 2012 Mark Tinguely 3cf1a573fSOleksandr Tymoshenko * 4cf1a573fSOleksandr Tymoshenko * All rights reserved. 5cf1a573fSOleksandr Tymoshenko * 6cf1a573fSOleksandr Tymoshenko * Redistribution and use in source and binary forms, with or without 7cf1a573fSOleksandr Tymoshenko * modification, are permitted provided that the following conditions 8cf1a573fSOleksandr Tymoshenko * are met: 9cf1a573fSOleksandr Tymoshenko * 1. Redistributions of source code must retain the above copyright 10cf1a573fSOleksandr Tymoshenko * notice, this list of conditions and the following disclaimer. 11cf1a573fSOleksandr Tymoshenko * 2. Redistributions in binary form must reproduce the above copyright 12cf1a573fSOleksandr Tymoshenko * notice, this list of conditions and the following disclaimer in the 13cf1a573fSOleksandr Tymoshenko * documentation and/or other materials provided with the distribution. 14cf1a573fSOleksandr Tymoshenko * 15cf1a573fSOleksandr Tymoshenko * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND 16cf1a573fSOleksandr Tymoshenko * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 17cf1a573fSOleksandr Tymoshenko * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 18cf1a573fSOleksandr Tymoshenko * ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE 19cf1a573fSOleksandr Tymoshenko * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 20cf1a573fSOleksandr Tymoshenko * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS 21cf1a573fSOleksandr Tymoshenko * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 22cf1a573fSOleksandr Tymoshenko * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 23cf1a573fSOleksandr Tymoshenko * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY 24cf1a573fSOleksandr Tymoshenko * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF 25cf1a573fSOleksandr Tymoshenko * SUCH DAMAGE. 26cf1a573fSOleksandr Tymoshenko */ 27cf1a573fSOleksandr Tymoshenko #include <sys/cdefs.h> 28cf1a573fSOleksandr Tymoshenko __FBSDID("$FreeBSD$"); 29cf1a573fSOleksandr Tymoshenko 30cf1a573fSOleksandr Tymoshenko 31cf1a573fSOleksandr Tymoshenko #include <sys/param.h> 32cf1a573fSOleksandr Tymoshenko #include <sys/systm.h> 33cf1a573fSOleksandr Tymoshenko #include <sys/proc.h> 34cf1a573fSOleksandr Tymoshenko #include <sys/kernel.h> 35cf1a573fSOleksandr Tymoshenko 36cf1a573fSOleksandr Tymoshenko #include <machine/fp.h> 37cf1a573fSOleksandr Tymoshenko #include <machine/pcb.h> 38cf1a573fSOleksandr Tymoshenko #include <machine/undefined.h> 39cf1a573fSOleksandr Tymoshenko #include <machine/vfp.h> 40cf1a573fSOleksandr Tymoshenko 41cf1a573fSOleksandr Tymoshenko /* function prototypes */ 42cf1a573fSOleksandr Tymoshenko unsigned int get_coprocessorACR(void); 43cf1a573fSOleksandr Tymoshenko int vfp_bounce(u_int, u_int, struct trapframe *, int); 44cf1a573fSOleksandr Tymoshenko void vfp_discard(void); 45cf1a573fSOleksandr Tymoshenko void vfp_enable(void); 46cf1a573fSOleksandr Tymoshenko void vfp_restore(struct vfp_state *); 47cf1a573fSOleksandr Tymoshenko void vfp_store(struct vfp_state *); 48cf1a573fSOleksandr Tymoshenko void set_coprocessorACR(u_int); 49cf1a573fSOleksandr Tymoshenko 50c46c2523SAndrew Turner extern int vfp_exists; 51cf1a573fSOleksandr Tymoshenko static struct undefined_handler vfp10_uh, vfp11_uh; 5293ef7ecbSAndrew Turner /* If true the VFP unit has 32 double registers, otherwise it has 16 */ 5393ef7ecbSAndrew Turner static int is_d32; 54cf1a573fSOleksandr Tymoshenko 55cf1a573fSOleksandr Tymoshenko /* The VFMXR command using coprocessor commands */ 56cf1a573fSOleksandr Tymoshenko #define fmxr(reg, val) \ 5793ef7ecbSAndrew Turner __asm __volatile("mcr p10, 7, %0, " __STRING(reg) " , c0, 0" :: "r"(val)); 58cf1a573fSOleksandr Tymoshenko 59cf1a573fSOleksandr Tymoshenko /* The VFMRX command using coprocessor commands */ 60cf1a573fSOleksandr Tymoshenko #define fmrx(reg) \ 61cf1a573fSOleksandr Tymoshenko ({ u_int val = 0;\ 6293ef7ecbSAndrew Turner __asm __volatile("mrc p10, 7, %0, " __STRING(reg) " , c0, 0" : "=r"(val));\ 63cf1a573fSOleksandr Tymoshenko val; \ 64cf1a573fSOleksandr Tymoshenko }) 65cf1a573fSOleksandr Tymoshenko 66cf1a573fSOleksandr Tymoshenko u_int 67cf1a573fSOleksandr Tymoshenko get_coprocessorACR(void) 68cf1a573fSOleksandr Tymoshenko { 69cf1a573fSOleksandr Tymoshenko u_int val; 70cf1a573fSOleksandr Tymoshenko __asm __volatile("mrc p15, 0, %0, c1, c0, 2" : "=r" (val) : : "cc"); 71cf1a573fSOleksandr Tymoshenko return val; 72cf1a573fSOleksandr Tymoshenko } 73cf1a573fSOleksandr Tymoshenko 74cf1a573fSOleksandr Tymoshenko void 75cf1a573fSOleksandr Tymoshenko set_coprocessorACR(u_int val) 76cf1a573fSOleksandr Tymoshenko { 77cf1a573fSOleksandr Tymoshenko __asm __volatile("mcr p15, 0, %0, c1, c0, 2\n\t" 78cf1a573fSOleksandr Tymoshenko : : "r" (val) : "cc"); 7948617973SOlivier Houchard isb(); 80cf1a573fSOleksandr Tymoshenko } 81cf1a573fSOleksandr Tymoshenko 82cf1a573fSOleksandr Tymoshenko 83cf1a573fSOleksandr Tymoshenko /* called for each cpu */ 84cf1a573fSOleksandr Tymoshenko void 85cf1a573fSOleksandr Tymoshenko vfp_init(void) 86cf1a573fSOleksandr Tymoshenko { 87cf1a573fSOleksandr Tymoshenko u_int fpsid, fpexc, tmp; 8893ef7ecbSAndrew Turner u_int coproc, vfp_arch; 89cf1a573fSOleksandr Tymoshenko 90cf1a573fSOleksandr Tymoshenko coproc = get_coprocessorACR(); 91cf1a573fSOleksandr Tymoshenko coproc |= COPROC10 | COPROC11; 92cf1a573fSOleksandr Tymoshenko set_coprocessorACR(coproc); 93cf1a573fSOleksandr Tymoshenko 9493ef7ecbSAndrew Turner fpsid = fmrx(VFPSID); /* read the vfp system id */ 9593ef7ecbSAndrew Turner fpexc = fmrx(VFPEXC); /* read the vfp exception reg */ 96cf1a573fSOleksandr Tymoshenko 97cf1a573fSOleksandr Tymoshenko if (!(fpsid & VFPSID_HARDSOFT_IMP)) { 98cf1a573fSOleksandr Tymoshenko vfp_exists = 1; 9993ef7ecbSAndrew Turner is_d32 = 0; 100cf1a573fSOleksandr Tymoshenko PCPU_SET(vfpsid, fpsid); /* save the VFPSID */ 10193ef7ecbSAndrew Turner 10293ef7ecbSAndrew Turner vfp_arch = 10393ef7ecbSAndrew Turner (fpsid & VFPSID_SUBVERSION2_MASK) >> VFPSID_SUBVERSION_OFF; 10493ef7ecbSAndrew Turner 10593ef7ecbSAndrew Turner if (vfp_arch >= VFP_ARCH3) { 10693ef7ecbSAndrew Turner tmp = fmrx(VMVFR0); 107cf1a573fSOleksandr Tymoshenko PCPU_SET(vfpmvfr0, tmp); 10893ef7ecbSAndrew Turner 10993ef7ecbSAndrew Turner if ((tmp & VMVFR0_RB_MASK) == 2) 11093ef7ecbSAndrew Turner is_d32 = 1; 11193ef7ecbSAndrew Turner 11293ef7ecbSAndrew Turner tmp = fmrx(VMVFR1); 113cf1a573fSOleksandr Tymoshenko PCPU_SET(vfpmvfr1, tmp); 114cf1a573fSOleksandr Tymoshenko } 11593ef7ecbSAndrew Turner 116cf1a573fSOleksandr Tymoshenko /* initialize the coprocess 10 and 11 calls 117cf1a573fSOleksandr Tymoshenko * These are called to restore the registers and enable 118cf1a573fSOleksandr Tymoshenko * the VFP hardware. 119cf1a573fSOleksandr Tymoshenko */ 120cf1a573fSOleksandr Tymoshenko if (vfp10_uh.uh_handler == NULL) { 121cf1a573fSOleksandr Tymoshenko vfp10_uh.uh_handler = vfp_bounce; 122cf1a573fSOleksandr Tymoshenko vfp11_uh.uh_handler = vfp_bounce; 123cf1a573fSOleksandr Tymoshenko install_coproc_handler_static(10, &vfp10_uh); 124cf1a573fSOleksandr Tymoshenko install_coproc_handler_static(11, &vfp11_uh); 125cf1a573fSOleksandr Tymoshenko } 126cf1a573fSOleksandr Tymoshenko } 127cf1a573fSOleksandr Tymoshenko } 128cf1a573fSOleksandr Tymoshenko 129cf1a573fSOleksandr Tymoshenko SYSINIT(vfp, SI_SUB_CPU, SI_ORDER_ANY, vfp_init, NULL); 130cf1a573fSOleksandr Tymoshenko 131cf1a573fSOleksandr Tymoshenko 132cf1a573fSOleksandr Tymoshenko /* start VFP unit, restore the vfp registers from the PCB and retry 133cf1a573fSOleksandr Tymoshenko * the instruction 134cf1a573fSOleksandr Tymoshenko */ 135cf1a573fSOleksandr Tymoshenko int 136cf1a573fSOleksandr Tymoshenko vfp_bounce(u_int addr, u_int insn, struct trapframe *frame, int code) 137cf1a573fSOleksandr Tymoshenko { 138cf1a573fSOleksandr Tymoshenko u_int fpexc; 139cf1a573fSOleksandr Tymoshenko struct pcb *curpcb; 140cf1a573fSOleksandr Tymoshenko struct thread *vfptd; 141cf1a573fSOleksandr Tymoshenko 142cf1a573fSOleksandr Tymoshenko if (!vfp_exists) 143cf1a573fSOleksandr Tymoshenko return 1; /* vfp does not exist */ 14493ef7ecbSAndrew Turner fpexc = fmrx(VFPEXC); /* read the vfp exception reg */ 145cf1a573fSOleksandr Tymoshenko if (fpexc & VFPEXC_EN) { 146cf1a573fSOleksandr Tymoshenko vfptd = PCPU_GET(vfpcthread); 147cf1a573fSOleksandr Tymoshenko /* did the kernel call the vfp or exception that expect us 148cf1a573fSOleksandr Tymoshenko * to emulate the command. Newer hardware does not require 149cf1a573fSOleksandr Tymoshenko * emulation, so we don't emulate yet. 150cf1a573fSOleksandr Tymoshenko */ 151cf1a573fSOleksandr Tymoshenko #ifdef SMP 152cf1a573fSOleksandr Tymoshenko /* don't save if newer registers are on another processor */ 153cf1a573fSOleksandr Tymoshenko if (vfptd /* && (vfptd == curthread) */ && 15448617973SOlivier Houchard (vfptd->td_pcb->pcb_vfpcpu == PCPU_GET(cpu))) 155cf1a573fSOleksandr Tymoshenko #else 156cf1a573fSOleksandr Tymoshenko /* someone did not save their registers, */ 157cf1a573fSOleksandr Tymoshenko if (vfptd /* && (vfptd == curthread) */) 158cf1a573fSOleksandr Tymoshenko #endif 159cf1a573fSOleksandr Tymoshenko vfp_store(&vfptd->td_pcb->pcb_vfpstate); 160cf1a573fSOleksandr Tymoshenko 161cf1a573fSOleksandr Tymoshenko fpexc &= ~VFPEXC_EN; 16293ef7ecbSAndrew Turner fmxr(VFPEXC, fpexc); /* turn vfp hardware off */ 163cf1a573fSOleksandr Tymoshenko if (vfptd == curthread) { 164cf1a573fSOleksandr Tymoshenko /* kill the process - we do not handle emulation */ 165cf1a573fSOleksandr Tymoshenko killproc(curthread->td_proc, "vfp emulation"); 166cf1a573fSOleksandr Tymoshenko return 1; 167cf1a573fSOleksandr Tymoshenko } 168cf1a573fSOleksandr Tymoshenko /* should not happen. someone did not save their context */ 169cf1a573fSOleksandr Tymoshenko printf("vfp_bounce: vfpcthread: %p curthread: %p\n", 170cf1a573fSOleksandr Tymoshenko vfptd, curthread); 171cf1a573fSOleksandr Tymoshenko } 172cf1a573fSOleksandr Tymoshenko fpexc |= VFPEXC_EN; 17393ef7ecbSAndrew Turner fmxr(VFPEXC, fpexc); /* enable the vfp and repeat command */ 174cf1a573fSOleksandr Tymoshenko curpcb = PCPU_GET(curpcb); 175cf1a573fSOleksandr Tymoshenko /* If we were the last process to use the VFP, the process did not 176cf1a573fSOleksandr Tymoshenko * use a VFP on another processor, then the registers in the VFP 177cf1a573fSOleksandr Tymoshenko * will still be ours and are current. Eventually, we will make the 178cf1a573fSOleksandr Tymoshenko * restore smarter. 179cf1a573fSOleksandr Tymoshenko */ 180cf1a573fSOleksandr Tymoshenko vfp_restore(&curpcb->pcb_vfpstate); 181cf1a573fSOleksandr Tymoshenko #ifdef SMP 18248617973SOlivier Houchard curpcb->pcb_vfpcpu = PCPU_GET(cpu); 183cf1a573fSOleksandr Tymoshenko #endif 184cf1a573fSOleksandr Tymoshenko PCPU_SET(vfpcthread, PCPU_GET(curthread)); 185cf1a573fSOleksandr Tymoshenko return 0; 186cf1a573fSOleksandr Tymoshenko } 187cf1a573fSOleksandr Tymoshenko 188cf1a573fSOleksandr Tymoshenko /* vfs_store is called from from a VFP command to restore the registers and 189cf1a573fSOleksandr Tymoshenko * turn on the VFP hardware. 190cf1a573fSOleksandr Tymoshenko * Eventually we will use the information that this process was the last 191cf1a573fSOleksandr Tymoshenko * to use the VFP hardware and bypass the restore, just turn on the hardware. 192cf1a573fSOleksandr Tymoshenko */ 193cf1a573fSOleksandr Tymoshenko void 194cf1a573fSOleksandr Tymoshenko vfp_restore(struct vfp_state *vfpsave) 195cf1a573fSOleksandr Tymoshenko { 196cf1a573fSOleksandr Tymoshenko u_int vfpscr = 0; 197cf1a573fSOleksandr Tymoshenko 198cf1a573fSOleksandr Tymoshenko if (vfpsave) { 19993ef7ecbSAndrew Turner __asm __volatile("ldc p10, c0, [%0], #128\n" /* d0-d15 */ 20093ef7ecbSAndrew Turner "cmp %0, 0\n" /* -D16 or -D32? */ 20193ef7ecbSAndrew Turner "ldcleq p11, c0, [%0], #128\n" /* d16-d31 */ 20293ef7ecbSAndrew Turner "addne %0, %0, #128\n" /* skip missing regs */ 203cf1a573fSOleksandr Tymoshenko "ldr %1, [%0]\n" /* set old vfpscr */ 204cf1a573fSOleksandr Tymoshenko "mcr p10, 7, %1, cr1, c0, 0\n" 20593ef7ecbSAndrew Turner :: "r" (vfpsave), "r" (vfpscr), "r" (is_d32) 20693ef7ecbSAndrew Turner : "cc"); 207cf1a573fSOleksandr Tymoshenko PCPU_SET(vfpcthread, PCPU_GET(curthread)); 208cf1a573fSOleksandr Tymoshenko } 209cf1a573fSOleksandr Tymoshenko } 210cf1a573fSOleksandr Tymoshenko 211cf1a573fSOleksandr Tymoshenko /* vfs_store is called from switch to save the vfp hardware registers 212cf1a573fSOleksandr Tymoshenko * into the pcb before switching to another process. 213cf1a573fSOleksandr Tymoshenko * we already know that the new process is different from this old 214cf1a573fSOleksandr Tymoshenko * process and that this process last used the VFP registers. 215cf1a573fSOleksandr Tymoshenko * Below we check to see if the VFP has been enabled since the last 216cf1a573fSOleksandr Tymoshenko * register save. 217cf1a573fSOleksandr Tymoshenko * This routine will exit with the VFP turned off. The next VFP user 218cf1a573fSOleksandr Tymoshenko * will trap to restore its registers and turn on the VFP hardware. 219cf1a573fSOleksandr Tymoshenko */ 220cf1a573fSOleksandr Tymoshenko void 221cf1a573fSOleksandr Tymoshenko vfp_store(struct vfp_state *vfpsave) 222cf1a573fSOleksandr Tymoshenko { 223cf1a573fSOleksandr Tymoshenko u_int tmp, vfpscr = 0; 224cf1a573fSOleksandr Tymoshenko 22593ef7ecbSAndrew Turner tmp = fmrx(VFPEXC); /* Is the vfp enabled? */ 226cf1a573fSOleksandr Tymoshenko if (vfpsave && tmp & VFPEXC_EN) { 22793ef7ecbSAndrew Turner __asm __volatile("stc p11, c0, [%1], #128\n" /* d0-d15 */ 22893ef7ecbSAndrew Turner "cmp %0, 0\n" /* -D16 or -D32? */ 22993ef7ecbSAndrew Turner "stcleq p11, c0, [%1], #128\n" /* d16-d31 */ 23093ef7ecbSAndrew Turner "addne %1, %1, #128\n" /* skip missing regs */ 23193ef7ecbSAndrew Turner "mrc p10, 7, %0, cr1, c0, 0\n" /* fmxr(VFPSCR) */ 23293ef7ecbSAndrew Turner "str %0, [%1]\n" /* save vfpscr */ 23393ef7ecbSAndrew Turner : "=&r" (vfpscr) : "r" (vfpsave), "r" (is_d32) : "cc"); 234cf1a573fSOleksandr Tymoshenko } 235cf1a573fSOleksandr Tymoshenko #ifndef SMP 236cf1a573fSOleksandr Tymoshenko /* eventually we will use this information for UP also */ 237cf1a573fSOleksandr Tymoshenko PCPU_SET(vfpcthread, 0); 238cf1a573fSOleksandr Tymoshenko #endif 239cf1a573fSOleksandr Tymoshenko tmp &= ~VFPEXC_EN; /* disable the vfp hardware */ 24093ef7ecbSAndrew Turner fmxr(VFPEXC , tmp); 241cf1a573fSOleksandr Tymoshenko } 242cf1a573fSOleksandr Tymoshenko 243cf1a573fSOleksandr Tymoshenko /* discard the registers at cpu_thread_free() when fpcurthread == td. 244cf1a573fSOleksandr Tymoshenko * Turn off the VFP hardware. 245cf1a573fSOleksandr Tymoshenko */ 246cf1a573fSOleksandr Tymoshenko void 247cf1a573fSOleksandr Tymoshenko vfp_discard() 248cf1a573fSOleksandr Tymoshenko { 249cf1a573fSOleksandr Tymoshenko u_int tmp = 0; 250cf1a573fSOleksandr Tymoshenko 251cf1a573fSOleksandr Tymoshenko PCPU_SET(vfpcthread, 0); /* permanent forget about reg */ 25293ef7ecbSAndrew Turner tmp = fmrx(VFPEXC); 253cf1a573fSOleksandr Tymoshenko tmp &= ~VFPEXC_EN; /* turn off VFP hardware */ 25493ef7ecbSAndrew Turner fmxr(VFPEXC, tmp); 255cf1a573fSOleksandr Tymoshenko } 256cf1a573fSOleksandr Tymoshenko 257cf1a573fSOleksandr Tymoshenko /* Enable the VFP hardware without restoring registers. 258cf1a573fSOleksandr Tymoshenko * Called when the registers are still in the VFP unit 259cf1a573fSOleksandr Tymoshenko */ 260cf1a573fSOleksandr Tymoshenko void 261cf1a573fSOleksandr Tymoshenko vfp_enable() 262cf1a573fSOleksandr Tymoshenko { 263cf1a573fSOleksandr Tymoshenko u_int tmp = 0; 264cf1a573fSOleksandr Tymoshenko 26593ef7ecbSAndrew Turner tmp = fmrx(VFPEXC); 266cf1a573fSOleksandr Tymoshenko tmp |= VFPEXC_EN; 26793ef7ecbSAndrew Turner fmxr(VFPEXC, tmp); 268cf1a573fSOleksandr Tymoshenko } 269