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/md_var.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 /*
  55  * About .fpu directives in this file...
  56  *
  57  * We should need simply .fpu vfpv3, but clang 3.5 has a quirk where setting
  58  * vfpv3 doesn't imply that vfp2 features are also available -- both have to be
  59  * explicitly set to get all the features of both.  This is probably a bug in
  60  * clang, so it may get fixed and require changes here some day.  Other changes
  61  * are probably coming in clang too, because there is email and open PRs
  62  * indicating they want to completely disable the ability to use .fpu and
  63  * similar directives in inline asm.  That would be catastrophic for us,
  64  * hopefully they come to their senses.  There was also some discusion of a new
  65  * syntax such as .push fpu=vfpv3; ...; .pop fpu; and that would be ideal for
  66  * us, better than what we have now really.
  67  *
  68  * For gcc, each .fpu directive completely overrides the prior directive, unlike
  69  * with clang, but luckily on gcc saying v3 implies all the v2 features as well.
  70  */
  71
  72 #define fmxr(reg, val) \
  73     __asm __volatile("  .fpu vfpv2\n .fpu vfpv3\n"                      \
  74                      "  vmsr    " __STRING(reg) ", %0"   :: "r"(val));
  75
  76 #define fmrx(reg) \
  77 ({ u_int val = 0;\
  78     __asm __volatile(" .fpu vfpv2\n .fpu vfpv3\n"                       \
  79                      "  vmrs    %0, " __STRING(reg) : "=r"(val));       \
  80     val; \
  81 })
  82
  83 static u_int
  84 get_coprocessorACR(void)
  85 {
  86         u_int val;
  87         __asm __volatile("mrc p15, 0, %0, c1, c0, 2" : "=r" (val) : : "cc");
  88         return val;
  89 }
  90
  91 static void
  92 set_coprocessorACR(u_int val)
  93 {
  94         __asm __volatile("mcr p15, 0, %0, c1, c0, 2\n\t"
  95          : : "r" (val) : "cc");
  96         isb();
  97 }
  98
  99
 100         /* called for each cpu */
 101 void
 102 vfp_init(void)
 103 {
 104         u_int fpsid, fpexc, tmp;
 105         u_int coproc, vfp_arch;
 106
 107         coproc = get_coprocessorACR();
 108         coproc |= COPROC10 | COPROC11;
 109         set_coprocessorACR(coproc);
 110
 111         fpsid = fmrx(fpsid);            /* read the vfp system id */
 112         fpexc = fmrx(fpexc);            /* read the vfp exception reg */
 113
 114         if (!(fpsid & VFPSID_HARDSOFT_IMP)) {
 115                 vfp_exists = 1;
 116                 is_d32 = 0;
 117                 PCPU_SET(vfpsid, fpsid);        /* save the fpsid */
 118
 119                 vfp_arch =
 120                     (fpsid & VFPSID_SUBVERSION2_MASK) >> VFPSID_SUBVERSION_OFF;
 121
 122                 if (vfp_arch >= VFP_ARCH3) {
 123                         tmp = fmrx(mvfr0);
 124                         PCPU_SET(vfpmvfr0, tmp);
 125
 126                         if ((tmp & VMVFR0_RB_MASK) == 2)
 127                                 is_d32 = 1;
 128
 129                         tmp = fmrx(mvfr1);
 130                         PCPU_SET(vfpmvfr1, tmp);
 131
 132                         if (PCPU_GET(cpuid) == 0) {
 133                                 if ((tmp & VMVFR1_FZ_MASK) == 0x1) {
 134                                         /* Denormals arithmetic support */
 135                                         initial_fpscr &= ~VFPSCR_FZ;
 136                                         thread0.td_pcb->pcb_vfpstate.fpscr =
 137                                             initial_fpscr;
 138                                 }
 139                         }
 140                 }
 141
 142                 /* initialize the coprocess 10 and 11 calls
 143                  * These are called to restore the registers and enable
 144                  * the VFP hardware.
 145                  */
 146                 if (vfp10_uh.uh_handler == NULL) {
 147                         vfp10_uh.uh_handler = vfp_bounce;
 148                         vfp11_uh.uh_handler = vfp_bounce;
 149                         install_coproc_handler_static(10, &vfp10_uh);
 150                         install_coproc_handler_static(11, &vfp11_uh);
 151                 }
 152         }
 153 }
 154
 155 SYSINIT(vfp, SI_SUB_CPU, SI_ORDER_ANY, vfp_init, NULL);
 156
 157
 158 /* start VFP unit, restore the vfp registers from the PCB  and retry
 159  * the instruction
 160  */
 161 static int
 162 vfp_bounce(u_int addr, u_int insn, struct trapframe *frame, int code)
 163 {
 164         u_int cpu, fpexc;
 165         struct pcb *curpcb;
 166         ksiginfo_t ksi;
 167
 168         if ((code & FAULT_USER) == 0)
 169                 panic("undefined floating point instruction in supervisor mode");
 170
 171         critical_enter();
 172
 173         /*
 174          * If the VFP is already on and we got an undefined instruction, then
 175          * something tried to executate a truly invalid instruction that maps to
 176          * the VFP.
 177          */
 178         fpexc = fmrx(fpexc);
 179         if (fpexc & VFPEXC_EN) {
 180                 /* Clear any exceptions */
 181                 fmxr(fpexc, fpexc & ~(VFPEXC_EX | VFPEXC_FP2V));
 182
 183                 /* kill the process - we do not handle emulation */
 184                 critical_exit();
 185
 186                 if (fpexc & VFPEXC_EX) {
 187                         /* We have an exception, signal a SIGFPE */
 188                         ksiginfo_init_trap(&ksi);
 189                         ksi.ksi_signo = SIGFPE;
 190                         if (fpexc & VFPEXC_UFC)
 191                                 ksi.ksi_code = FPE_FLTUND;
 192                         else if (fpexc & VFPEXC_OFC)
 193                                 ksi.ksi_code = FPE_FLTOVF;
 194                         else if (fpexc & VFPEXC_IOC)
 195                                 ksi.ksi_code = FPE_FLTINV;
 196                         ksi.ksi_addr = (void *)addr;
 197                         trapsignal(curthread, &ksi);
 198                         return 0;
 199                 }
 200
 201                 return 1;
 202         }
 203
 204         /*
 205          * If the last time this thread used the VFP it was on this core, and
 206          * the last thread to use the VFP on this core was this thread, then the
 207          * VFP state is valid, otherwise restore this thread's state to the VFP.
 208          */
 209         fmxr(fpexc, fpexc | VFPEXC_EN);
 210         curpcb = curthread->td_pcb;
 211         cpu = PCPU_GET(cpuid);
 212         if (curpcb->pcb_vfpcpu != cpu || curthread != PCPU_GET(fpcurthread)) {
 213                 vfp_restore(&curpcb->pcb_vfpstate);
 214                 curpcb->pcb_vfpcpu = cpu;
 215                 PCPU_SET(fpcurthread, curthread);
 216         }
 217
 218         critical_exit();
 219         return (0);
 220 }
 221
 222 /*
 223  * Restore the given state to the VFP hardware.
 224  */
 225 static void
 226 vfp_restore(struct vfp_state *vfpsave)
 227 {
 228         uint32_t fpexc;
 229
 230         /* On vfpv3 we may need to restore FPINST and FPINST2 */
 231         fpexc = vfpsave->fpexec;
 232         if (fpexc & VFPEXC_EX) {
 233                 fmxr(fpinst, vfpsave->fpinst);
 234                 if (fpexc & VFPEXC_FP2V)
 235                         fmxr(fpinst2, vfpsave->fpinst2);
 236         }
 237         fmxr(fpscr, vfpsave->fpscr);
 238
 239         __asm __volatile(
 240             " .fpu      vfpv2\n"
 241             " .fpu      vfpv3\n"
 242             " vldmia    %0!, {d0-d15}\n"        /* d0-d15 */
 243             " cmp       %1, #0\n"               /* -D16 or -D32? */
 244             " vldmiane  %0!, {d16-d31}\n"       /* d16-d31 */
 245             " addeq     %0, %0, #128\n"         /* skip missing regs */
 246             : "+&r" (vfpsave) : "r" (is_d32) : "cc"
 247             );
 248
 249         fmxr(fpexc, fpexc);
 250 }
 251
 252 /*
 253  * If the VFP is on, save its current state and turn it off if requested to do
 254  * so.  If the VFP is not on, does not change the values at *vfpsave.  Caller is
 255  * responsible for preventing a context switch while this is running.
 256  */
 257 void
 258 vfp_store(struct vfp_state *vfpsave, boolean_t disable_vfp)
 259 {
 260         uint32_t fpexc;
 261
 262         fpexc = fmrx(fpexc);            /* Is the vfp enabled? */
 263         if (fpexc & VFPEXC_EN) {
 264                 vfpsave->fpexec = fpexc;
 265                 vfpsave->fpscr = fmrx(fpscr);
 266
 267                 /* On vfpv3 we may need to save FPINST and FPINST2 */
 268                 if (fpexc & VFPEXC_EX) {
 269                         vfpsave->fpinst = fmrx(fpinst);
 270                         if (fpexc & VFPEXC_FP2V)
 271                                 vfpsave->fpinst2 = fmrx(fpinst2);
 272                         fpexc &= ~VFPEXC_EX;
 273                 }
 274
 275                 __asm __volatile(
 276                     " .fpu      vfpv2\n"
 277                     " .fpu      vfpv3\n"
 278                     " vstmia    %0!, {d0-d15}\n"        /* d0-d15 */
 279                     " cmp       %1, #0\n"               /* -D16 or -D32? */
 280                     " vstmiane  r0!, {d16-d31}\n"       /* d16-d31 */
 281                     " addeq     %0, %0, #128\n"         /* skip missing regs */
 282                     : "+&r" (vfpsave) : "r" (is_d32) : "cc"
 283                     );
 284
 285                 if (disable_vfp)
 286                         fmxr(fpexc , fpexc & ~VFPEXC_EN);
 287         }
 288 }
 289
 290 /*
 291  * The current thread is dying.  If the state currently in the hardware belongs
 292  * to the current thread, set fpcurthread to NULL to indicate that the VFP
 293  * hardware state does not belong to any thread.  If the VFP is on, turn it off.
 294  * Called only from cpu_throw(), so we don't have to worry about a context
 295  * switch here.
 296  */
 297 void
 298 vfp_discard(struct thread *td)
 299 {
 300         u_int tmp;
 301
 302         if (PCPU_GET(fpcurthread) == td)
 303                 PCPU_SET(fpcurthread, NULL);
 304
 305         tmp = fmrx(fpexc);
 306         if (tmp & VFPEXC_EN)
 307                 fmxr(fpexc, tmp & ~VFPEXC_EN);
 308 }
 309
 310 #endif
 311