sys/arch/arm/vfp/vfp_init.c

   1 /*      $NetBSD: vfp_init.c,v 1.2 2009/03/18 10:22:24 cegger Exp $ */
   2
   3 /*
   4  * Copyright (c) 2008 ARM Ltd
   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  * 3. The name of the company may not be used to endorse or promote
  16  *    products derived from this software without specific prior written
  17  *    permission.
  18  *
  19  * THIS SOFTWARE IS PROVIDED BY ARM LTD ``AS IS'' AND ANY EXPRESS OR
  20  * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
  21  * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
  22  * ARE DISCLAIMED.  IN NO EVENT SHALL ARM LTD BE LIABLE FOR ANY
  23  * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
  24  * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE
  25  * GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
  26  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
  27  * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
  28  * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
  29  * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  30  */
  31
  32 #include <sys/param.h>
  33 #include <sys/types.h>
  34 #include <sys/systm.h>
  35 #include <sys/device.h>
  36 #include <sys/proc.h>
  37
  38 #include <arm/undefined.h>
  39 #include <machine/cpu.h>
  40
  41 #include <arm/vfpvar.h>
  42 #include <arm/vfpreg.h>
  43
  44 /*
  45  * Use generic co-processor instructions to avoid assembly problems.
  46  */
  47
  48 /* FMRX <X>, fpsid */
  49 #define read_fpsid(X)   __asm __volatile("mrc p10, 7, %0, c0, c0, 0" \
  50                             : "=r" (*(X)) : : "memory")
  51 /* FMRX <X>, fpscr */
  52 #define read_fpscr(X)   __asm __volatile("mrc p10, 7, %0, c1, c0, 0" \
  53                             : "=r" (*(X)))
  54 /* FMRX <X>, fpexc */
  55 #define read_fpexc(X)   __asm __volatile("mrc p10, 7, %0, c8, c0, 0" \
  56                             : "=r" (*(X)))
  57 /* FMRX <X>, fpinst */
  58 #define read_fpinst(X)  __asm __volatile("mrc p10, 7, %0, c9, c0, 0" \
  59                             : "=r" (*(X)))
  60 /* FMRX <X>, fpinst2 */
  61 #define read_fpinst2(X) __asm __volatile("mrc p10, 7, %0, c10, c0, 0" \
  62                             : "=r" (*(X)))
  63 /* FSTMD <X>, {d0-d15} */
  64 #define save_vfpregs(X) __asm __volatile("stc p11, c0, [%0], {32}" : \
  65                             : "r" (X) : "memory")
  66
  67 /* FMXR <X>, fpscr */
  68 #define write_fpscr(X)  __asm __volatile("mcr p10, 7, %0, c1, c0, 0" : \
  69                             : "r" (X))
  70 /* FMXR <X>, fpexc */
  71 #define write_fpexc(X)  __asm __volatile("mcr p10, 7, %0, c8, c0, 0" : \
  72                             : "r" (X))
  73 /* FMXR <X>, fpinst */
  74 #define write_fpinst(X) __asm __volatile("mcr p10, 7, %0, c9, c0, 0" : \
  75                             : "r" (X))
  76 /* FMXR <X>, fpinst2 */
  77 #define write_fpinst2(X) __asm __volatile("mcr p10, 7, %0, c10, c0, 0" : \
  78                             : "r" (X))
  79 /* FLDMD <X>, {d0-d15} */
  80 #define load_vfpregs(X) __asm __volatile("ldc p11, c0, [%0], {32}" : \
  81                             : "r" (X) : "memory");
  82
  83 /* The real handler for VFP bounces.  */
  84 static int vfp_handler(u_int, u_int, trapframe_t *, int);
  85
  86 static void vfp_load_regs(struct vfpreg *);
  87
  88 struct evcnt vfpevent_use;
  89 struct evcnt vfpevent_reuse;
  90
  91 /*
  92  * Used to test for a VFP. The following function is installed as a coproc10
  93  * handler on the undefined instruction vector and then we issue a VFP
  94  * instruction. If undefined_test is non zero then the VFP did not handle
  95  * the instruction so must be absent, or disabled.
  96  */
  97
  98 static int undefined_test;
  99
 100 static int
 101 vfp_test(u_int address, u_int instruction, trapframe_t *frame, int fault_code)
 102 {
 103
 104         frame->tf_pc += INSN_SIZE;
 105         ++undefined_test;
 106         return(0);
 107 }
 108
 109 void
 110 vfp_attach(void)
 111 {
 112         void *uh;
 113         uint32_t fpsid;
 114         const char *model = NULL;
 115
 116         uh = install_coproc_handler(VFP_COPROC, vfp_test);
 117
 118         undefined_test = 0;
 119
 120         read_fpsid(&fpsid);
 121
 122         remove_coproc_handler(uh);
 123
 124         if (undefined_test != 0) {
 125                 aprint_normal("%s: No VFP detected\n",
 126                     curcpu()->ci_dev->dv_xname);
 127                 curcpu()->ci_vfp.vfp_id = 0;
 128                 return;
 129         }
 130
 131         curcpu()->ci_vfp.vfp_id = fpsid;
 132         switch (fpsid & ~ VFP_FPSID_REV_MSK)
 133                 {
 134                 case FPU_VFP10_ARM10E:
 135                         model = "VFP10 R1";
 136                         break;
 137                 case FPU_VFP11_ARM11:
 138                         model = "VFP11";
 139                         break;
 140                 default:
 141                         aprint_normal("%s: unrecognized VFP version %x\n",
 142                             curcpu()->ci_dev->dv_xname, fpsid);
 143                         fpsid = 0;      /* Not recognised. */
 144                         return;
 145                 }
 146
 147         if (fpsid != 0) {
 148                 aprint_normal("vfp%d at %s: %s\n",
 149                     curcpu()->ci_dev->dv_unit, curcpu()->ci_dev->dv_xname,
 150                     model);
 151         }
 152         evcnt_attach_dynamic(&vfpevent_use, EVCNT_TYPE_MISC, NULL,
 153             "VFP", "proc use");
 154         evcnt_attach_dynamic(&vfpevent_reuse, EVCNT_TYPE_MISC, NULL,
 155             "VFP", "proc re-use");
 156         install_coproc_handler(VFP_COPROC, vfp_handler);
 157         install_coproc_handler(VFP_COPROC2, vfp_handler);
 158 }
 159
 160 /* The real handler for VFP bounces.  */
 161 static int vfp_handler(u_int address, u_int instruction, trapframe_t *frame,
 162     int fault_code)
 163 {
 164         struct cpu_info *ci = curcpu();
 165         struct pcb *pcb;
 166         struct lwp *l;
 167
 168         /* This shouldn't ever happen.  */
 169         if (fault_code != FAULT_USER)
 170                 panic("VFP fault in non-user mode");
 171
 172         if (ci->ci_vfp.vfp_id == 0)
 173                 /* No VFP detected, just fault.  */
 174                 return 1;
 175
 176         l = curlwp;
 177         pcb = lwp_getpcb(l);
 178
 179         if ((l->l_md.md_flags & MDP_VFPUSED) && ci->ci_vfp.vfp_fpcurlwp == l) {
 180                 uint32_t fpexc;
 181
 182                 printf("VFP bounce @%x (insn=%x) lwp=%p\n", address,
 183                     instruction, l);
 184                 read_fpexc(&fpexc);
 185                 if ((fpexc & VFP_FPEXC_EN) == 0)
 186                         printf("vfp not enabled\n");
 187                 vfp_saveregs_lwp(l, 1);
 188                 printf(" fpexc = 0x%08x  fpscr = 0x%08x\n", fpexc,
 189                     pcb->pcb_vfp.vfp_fpscr);
 190                 printf(" fpinst = 0x%08x fpinst2 = 0x%08x\n",
 191                     pcb->pcb_vfp.vfp_fpinst,
 192                     pcb->pcb_vfp.vfp_fpinst2);
 193                 return 1;
 194         }
 195
 196         if (ci->ci_vfp.vfp_fpcurlwp != NULL)
 197                 vfp_saveregs_cpu(ci, 1);
 198
 199         KDASSERT(ci->ci_vfp.vfp_fpcurlwp == NULL);
 200
 201         KDASSERT(pcb->pcb_vfpcpu == NULL);
 202
 203 //      VFPCPU_LOCK(pcb, s);
 204
 205         pcb->pcb_vfpcpu = ci;
 206         ci->ci_vfp.vfp_fpcurlwp = l;
 207
 208 //      VFPCPU_UNLOCK(pcb, s);
 209
 210         /*
 211          * Instrument VFP usage -- if a process has not previously
 212          * used the VFP, mark it as having used VFP for the first time,
 213          * and count this event.
 214          *
 215          * If a process has used the VFP, count a "used VFP, and took
 216          * a trap to use it again" event.
 217          */
 218         if ((l->l_md.md_flags & MDP_VFPUSED) == 0) {
 219                 vfpevent_use.ev_count++;
 220                 l->l_md.md_flags |= MDP_VFPUSED;
 221                 pcb->pcb_vfp.vfp_fpscr =
 222                     (VFP_FPSCR_DN | VFP_FPSCR_FZ);      /* Runfast */
 223         } else
 224                 vfpevent_reuse.ev_count++;
 225
 226         vfp_load_regs(&pcb->pcb_vfp);
 227
 228         /* Need to restart the faulted instruction.  */
 229 //      frame->tf_pc -= INSN_SIZE;
 230         return 0;
 231 }
 232
 233 static void
 234 vfp_load_regs(struct vfpreg *fregs)
 235 {
 236         uint32_t fpexc;
 237
 238         /* Enable the VFP (so that we can write the registers).  */
 239         read_fpexc(&fpexc);
 240         KDASSERT((fpexc & VFP_FPEXC_EX) == 0);
 241         write_fpexc(fpexc | VFP_FPEXC_EN);
 242
 243         load_vfpregs(fregs->vfp_regs);
 244         write_fpscr(fregs->vfp_fpscr);
 245         if (fregs->vfp_fpexc & VFP_FPEXC_EX) {
 246                 /* Need to restore the exception handling state.  */
 247                 switch (curcpu()->ci_vfp.vfp_id) {
 248                 case FPU_VFP10_ARM10E:
 249                 case FPU_VFP11_ARM11:
 250                         write_fpinst2(fregs->vfp_fpinst2);
 251                         write_fpinst(fregs->vfp_fpinst);
 252                         break;
 253                 default:
 254                         panic("vfp_load_regs: Unsupported VFP");
 255                 }
 256         }
 257         /* Finally, restore the FPEXC and enable the VFP. */
 258         write_fpexc(fregs->vfp_fpexc | VFP_FPEXC_EN);
 259 }
 260
 261 void
 262 vfp_saveregs_cpu(struct cpu_info *ci, int save)
 263 {
 264         struct lwp *l;
 265         struct pcb *pcb;
 266         uint32_t fpexc;
 267
 268         KDASSERT(ci == curcpu());
 269
 270         l = ci->ci_vfp.vfp_fpcurlwp;
 271         if (l == NULL)
 272                 return;
 273
 274         pcb = lwp_getpcb(l);
 275         read_fpexc(&fpexc);
 276
 277         if (save) {
 278                 struct vfpreg *fregs = &pcb->pcb_vfp;
 279
 280                 /*
 281                  * Enable the VFP (so we can read the registers).
 282                  * Make sure the exception bit is cleared so that we can
 283                  * safely dump the registers.
 284                  */
 285                 write_fpexc((fpexc | VFP_FPEXC_EN) & ~VFP_FPEXC_EX);
 286
 287                 fregs->vfp_fpexc = fpexc;
 288                 if (fpexc & VFP_FPEXC_EX) {
 289                         /* Need to save the exception handling state */
 290                         switch (ci->ci_vfp.vfp_id) {
 291                         case FPU_VFP10_ARM10E:
 292                         case FPU_VFP11_ARM11:
 293                                 read_fpinst(&fregs->vfp_fpinst);
 294                                 read_fpinst2(&fregs->vfp_fpinst2);
 295                                 break;
 296                         default:
 297                                 panic("vfp_saveregs_cpu: Unsupported VFP");
 298                         }
 299                 }
 300                 read_fpscr(&fregs->vfp_fpscr);
 301                 save_vfpregs(fregs->vfp_regs);
 302         }
 303         /* Disable the VFP.  */
 304         write_fpexc(fpexc & ~VFP_FPEXC_EN);
 305 //      VFPCPU_LOCK(pcb, s);
 306
 307         pcb->pcb_vfpcpu = NULL;
 308         ci->ci_vfp.vfp_fpcurlwp = NULL;
 309 //      VFPCPU_UNLOCK(pcb, s);
 310 }
 311
 312 void
 313 vfp_saveregs_lwp(struct lwp *l, int save)
 314 {
 315         struct cpu_info *ci = curcpu();
 316         struct cpu_info *oci;
 317         struct pcb *pcb;
 318
 319         pcb = lwp_getpcb(l);
 320         KDASSERT(pcb != NULL);
 321
 322 //      VFPCPU_LOCK(pcb, s);
 323
 324         oci = pcb->pcb_vfpcpu;
 325         if (oci == NULL) {
 326                 // VFPCPU_UNLOCK(pcb, s);
 327                 return;
 328         }
 329
 330 #if defined(MULTIPROCESSOR)
 331         /*
 332          * On a multiprocessor system this is where we would send an IPI
 333          * to the processor holding the VFP state for this process.
 334          */
 335 #error MULTIPROCESSOR
 336 #else
 337         KASSERT(ci->ci_vfp.vfp_fpcurlwp == l);
 338 //      VFPCPU_UNLOCK(pcb, s);
 339         vfp_saveregs_cpu(ci, save);
 340 #endif
 341 }
 342
 343 void
 344 vfp_savecontext(void)
 345 {
 346         struct cpu_info *ci = curcpu();
 347         uint32_t fpexc;
 348
 349         if (ci->ci_vfp.vfp_fpcurlwp != NULL) {
 350                 read_fpexc(&fpexc);
 351                 write_fpexc(fpexc & ~VFP_FPEXC_EN);
 352         }
 353 }
 354
 355 void
 356 vfp_loadcontext(struct lwp *l)
 357 {
 358         uint32_t fpexc;
 359
 360         if (curcpu()->ci_vfp.vfp_fpcurlwp == l) {
 361                 read_fpexc(&fpexc);
 362                 write_fpexc(fpexc | VFP_FPEXC_EN);
 363         }
 364 }