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