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