sys/arm/arm/vfp.c

   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$");
  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 */
  46 static int vfp_bounce(u_int, u_int, struct trapframe *, int);
  47 static void vfp_restore(struct vfp_state *);
  48
  49 extern int vfp_exists;
  50 static struct undefined_handler vfp10_uh, vfp11_uh;
  51 /* If true the VFP unit has 32 double registers, otherwise it has 16 */
  52 static 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
  78 static u_int
  79 get_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
  86 static void
  87 set_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 */
  96 void
  97 vfp_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
 141 SYSINIT(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  */
 147 static int
 148 vfp_bounce(u_int addr, u_int insn, struct trapframe *frame, int code)
 149 {
 150         u_int cpu, fpexc;
 151         struct pcb *curpcb;
 152         ksiginfo_t ksi;
 153
 154         if ((code & FAULT_USER) == 0)
 155                 panic("undefined floating point instruction in supervisor mode");
 156
 157         critical_enter();
 158
 159         /*
 160          * If the VFP is already on and we got an undefined instruction, then
 161          * something tried to executate a truly invalid instruction that maps to
 162          * the VFP.
 163          */
 164         fpexc = fmrx(VFPEXC);
 165         if (fpexc & VFPEXC_EN) {
 166                 /* Clear any exceptions */
 167                 fmxr(VFPEXC, fpexc & ~(VFPEXC_EX | VFPEXC_FP2V));
 168
 169                 /* kill the process - we do not handle emulation */
 170                 critical_exit();
 171
 172                 if (fpexc & VFPEXC_EX) {
 173                         /* We have an exception, signal a SIGFPE */
 174                         ksiginfo_init_trap(&ksi);
 175                         ksi.ksi_signo = SIGFPE;
 176                         if (fpexc & VFPEXC_UFC)
 177                                 ksi.ksi_code = FPE_FLTUND;
 178                         else if (fpexc & VFPEXC_OFC)
 179                                 ksi.ksi_code = FPE_FLTOVF;
 180                         else if (fpexc & VFPEXC_IOC)
 181                                 ksi.ksi_code = FPE_FLTINV;
 182                         ksi.ksi_addr = (void *)addr;
 183                         trapsignal(curthread, &ksi);
 184                         return 0;
 185                 }
 186
 187                 return 1;
 188         }
 189
 190         /*
 191          * If the last time this thread used the VFP it was on this core, and
 192          * the last thread to use the VFP on this core was this thread, then the
 193          * VFP state is valid, otherwise restore this thread's state to the VFP.
 194          */
 195         fmxr(VFPEXC, fpexc | VFPEXC_EN);
 196         curpcb = curthread->td_pcb;
 197         cpu = PCPU_GET(cpu);
 198         if (curpcb->pcb_vfpcpu != cpu || curthread != PCPU_GET(fpcurthread)) {
 199                 vfp_restore(&curpcb->pcb_vfpstate);
 200                 curpcb->pcb_vfpcpu = cpu;
 201                 PCPU_SET(fpcurthread, curthread);
 202         }
 203
 204         critical_exit();
 205         return (0);
 206 }
 207
 208 /*
 209  * Restore the given state to the VFP hardware.
 210  */
 211 static void
 212 vfp_restore(struct vfp_state *vfpsave)
 213 {
 214         uint32_t fpexc;
 215
 216         /* On VFPv2 we may need to restore FPINST and FPINST2 */
 217         fpexc = vfpsave->fpexec;
 218         if (fpexc & VFPEXC_EX) {
 219                 fmxr(VFPINST, vfpsave->fpinst);
 220                 if (fpexc & VFPEXC_FP2V)
 221                         fmxr(VFPINST2, vfpsave->fpinst2);
 222         }
 223         fmxr(VFPSCR, vfpsave->fpscr);
 224
 225         __asm __volatile("ldc   p10, c0, [%0], #128\n" /* d0-d15 */
 226                         "cmp    %1, #0\n"               /* -D16 or -D32? */
 227                         LDCLNE "p11, c0, [%0], #128\n"  /* d16-d31 */
 228                         "addeq  %0, %0, #128\n"         /* skip missing regs */
 229                         : : "r" (vfpsave), "r" (is_d32) : "cc");
 230
 231         fmxr(VFPEXC, fpexc);
 232 }
 233
 234 /*
 235  * If the VFP is on, save its current state and turn it off if requested to do
 236  * so.  If the VFP is not on, does not change the values at *vfpsave.  Caller is
 237  * responsible for preventing a context switch while this is running.
 238  */
 239 void
 240 vfp_store(struct vfp_state *vfpsave, boolean_t disable_vfp)
 241 {
 242         uint32_t fpexc;
 243
 244         fpexc = fmrx(VFPEXC);           /* Is the vfp enabled? */
 245         if (fpexc & VFPEXC_EN) {
 246                 vfpsave->fpexec = fpexc;
 247                 vfpsave->fpscr = fmrx(VFPSCR);
 248
 249                 /* On VFPv2 we may need to save FPINST and FPINST2 */
 250                 if (fpexc & VFPEXC_EX) {
 251                         vfpsave->fpinst = fmrx(VFPINST);
 252                         if (fpexc & VFPEXC_FP2V)
 253                                 vfpsave->fpinst2 = fmrx(VFPINST2);
 254                         fpexc &= ~VFPEXC_EX;
 255                 }
 256
 257                 __asm __volatile(
 258                         "stc    p11, c0, [%0], #128\n"  /* d0-d15 */
 259                         "cmp    %1, #0\n"               /* -D16 or -D32? */
 260                         STCLNE "p11, c0, [%0], #128\n"  /* d16-d31 */
 261                         "addeq  %0, %0, #128\n"         /* skip missing regs */
 262                         : : "r" (vfpsave), "r" (is_d32) : "cc");
 263
 264                 if (disable_vfp)
 265                         fmxr(VFPEXC , fpexc & ~VFPEXC_EN);
 266         }
 267 }
 268
 269 /*
 270  * The current thread is dying.  If the state currently in the hardware belongs
 271  * to the current thread, set fpcurthread to NULL to indicate that the VFP
 272  * hardware state does not belong to any thread.  If the VFP is on, turn it off.
 273  * Called only from cpu_throw(), so we don't have to worry about a context
 274  * switch here.
 275  */
 276 void
 277 vfp_discard(struct thread *td)
 278 {
 279         u_int tmp;
 280
 281         if (PCPU_GET(fpcurthread) == td)
 282                 PCPU_SET(fpcurthread, NULL);
 283
 284         tmp = fmrx(VFPEXC);
 285         if (tmp & VFPEXC_EN)
 286                 fmxr(VFPEXC, tmp & ~VFPEXC_EN);
 287 }
 288
 289 #endif
 290