sys/arm/arm/vfp.c

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