sys/arm64/arm64/vfp.c

   1 /*-
   2  * Copyright (c) 2015-2016 The FreeBSD Foundation
   3  *
   4  * This software was developed by Andrew Turner under
   5  * sponsorship from the FreeBSD Foundation.
   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 #ifdef VFP
  31 #include <sys/param.h>
  32 #include <sys/systm.h>
  33 #include <sys/limits.h>
  34 #include <sys/kernel.h>
  35 #include <sys/malloc.h>
  36 #include <sys/pcpu.h>
  37 #include <sys/proc.h>
  38
  39 #include <vm/uma.h>
  40
  41 #include <machine/armreg.h>
  42 #include <machine/md_var.h>
  43 #include <machine/pcb.h>
  44 #include <machine/vfp.h>
  45
  46 /* Sanity check we can store all the VFP registers */
  47 CTASSERT(sizeof(((struct pcb *)0)->pcb_fpustate.vfp_regs) == 16 * 32);
  48
  49 static MALLOC_DEFINE(M_FPUKERN_CTX, "fpukern_ctx",
  50     "Kernel contexts for VFP state");
  51
  52 struct fpu_kern_ctx {
  53         struct vfpstate *prev;
  54 #define FPU_KERN_CTX_DUMMY      0x01    /* avoided save for the kern thread */
  55 #define FPU_KERN_CTX_INUSE      0x02
  56         uint32_t         flags;
  57         struct vfpstate  state;
  58 };
  59
  60 static uma_zone_t fpu_save_area_zone;
  61 static struct vfpstate *fpu_initialstate;
  62
  63 void
  64 vfp_enable(void)
  65 {
  66         uint32_t cpacr;
  67
  68         cpacr = READ_SPECIALREG(cpacr_el1);
  69         cpacr = (cpacr & ~CPACR_FPEN_MASK) | CPACR_FPEN_TRAP_NONE;
  70         WRITE_SPECIALREG(cpacr_el1, cpacr);
  71         isb();
  72 }
  73
  74 void
  75 vfp_disable(void)
  76 {
  77         uint32_t cpacr;
  78
  79         cpacr = READ_SPECIALREG(cpacr_el1);
  80         cpacr = (cpacr & ~CPACR_FPEN_MASK) | CPACR_FPEN_TRAP_ALL1;
  81         WRITE_SPECIALREG(cpacr_el1, cpacr);
  82         isb();
  83 }
  84
  85 /*
  86  * Called when the thread is dying or when discarding the kernel VFP state.
  87  * If the thread was the last to use the VFP unit mark it as unused to tell
  88  * the kernel the fp state is unowned. Ensure the VFP unit is off so we get
  89  * an exception on the next access.
  90  */
  91 void
  92 vfp_discard(struct thread *td)
  93 {
  94
  95 #ifdef INVARIANTS
  96         if (td != NULL)
  97                 CRITICAL_ASSERT(td);
  98 #endif
  99         if (PCPU_GET(fpcurthread) == td)
 100                 PCPU_SET(fpcurthread, NULL);
 101
 102         vfp_disable();
 103 }
 104
 105 void
 106 vfp_store(struct vfpstate *state)
 107 {
 108         __uint128_t *vfp_state;
 109         uint64_t fpcr, fpsr;
 110
 111         vfp_state = state->vfp_regs;
 112         __asm __volatile(
 113             "mrs        %0, fpcr                \n"
 114             "mrs        %1, fpsr                \n"
 115             "stp        q0,  q1,  [%2, #16 *  0]\n"
 116             "stp        q2,  q3,  [%2, #16 *  2]\n"
 117             "stp        q4,  q5,  [%2, #16 *  4]\n"
 118             "stp        q6,  q7,  [%2, #16 *  6]\n"
 119             "stp        q8,  q9,  [%2, #16 *  8]\n"
 120             "stp        q10, q11, [%2, #16 * 10]\n"
 121             "stp        q12, q13, [%2, #16 * 12]\n"
 122             "stp        q14, q15, [%2, #16 * 14]\n"
 123             "stp        q16, q17, [%2, #16 * 16]\n"
 124             "stp        q18, q19, [%2, #16 * 18]\n"
 125             "stp        q20, q21, [%2, #16 * 20]\n"
 126             "stp        q22, q23, [%2, #16 * 22]\n"
 127             "stp        q24, q25, [%2, #16 * 24]\n"
 128             "stp        q26, q27, [%2, #16 * 26]\n"
 129             "stp        q28, q29, [%2, #16 * 28]\n"
 130             "stp        q30, q31, [%2, #16 * 30]\n"
 131             : "=&r"(fpcr), "=&r"(fpsr) : "r"(vfp_state));
 132
 133         state->vfp_fpcr = fpcr;
 134         state->vfp_fpsr = fpsr;
 135 }
 136
 137 void
 138 vfp_restore(struct vfpstate *state)
 139 {
 140         __uint128_t *vfp_state;
 141         uint64_t fpcr, fpsr;
 142
 143         vfp_state = state->vfp_regs;
 144         fpcr = state->vfp_fpcr;
 145         fpsr = state->vfp_fpsr;
 146
 147         __asm __volatile(
 148             "ldp        q0,  q1,  [%2, #16 *  0]\n"
 149             "ldp        q2,  q3,  [%2, #16 *  2]\n"
 150             "ldp        q4,  q5,  [%2, #16 *  4]\n"
 151             "ldp        q6,  q7,  [%2, #16 *  6]\n"
 152             "ldp        q8,  q9,  [%2, #16 *  8]\n"
 153             "ldp        q10, q11, [%2, #16 * 10]\n"
 154             "ldp        q12, q13, [%2, #16 * 12]\n"
 155             "ldp        q14, q15, [%2, #16 * 14]\n"
 156             "ldp        q16, q17, [%2, #16 * 16]\n"
 157             "ldp        q18, q19, [%2, #16 * 18]\n"
 158             "ldp        q20, q21, [%2, #16 * 20]\n"
 159             "ldp        q22, q23, [%2, #16 * 22]\n"
 160             "ldp        q24, q25, [%2, #16 * 24]\n"
 161             "ldp        q26, q27, [%2, #16 * 26]\n"
 162             "ldp        q28, q29, [%2, #16 * 28]\n"
 163             "ldp        q30, q31, [%2, #16 * 30]\n"
 164             "msr        fpcr, %0                \n"
 165             "msr        fpsr, %1                \n"
 166             : : "r"(fpcr), "r"(fpsr), "r"(vfp_state));
 167 }
 168
 169 void
 170 vfp_save_state(struct thread *td, struct pcb *pcb)
 171 {
 172         uint32_t cpacr;
 173
 174         KASSERT(pcb != NULL, ("NULL vfp pcb"));
 175         KASSERT(td == NULL || td->td_pcb == pcb, ("Invalid vfp pcb"));
 176
 177         /*
 178          * savectx() will be called on panic with dumppcb as an argument,
 179          * dumppcb doesn't have pcb_fpusaved set, so set it to save
 180          * the VFP registers.
 181          */
 182         if (pcb->pcb_fpusaved == NULL)
 183                 pcb->pcb_fpusaved = &pcb->pcb_fpustate;
 184
 185         if (td == NULL)
 186                 td = curthread;
 187
 188         critical_enter();
 189         /*
 190          * Only store the registers if the VFP is enabled,
 191          * i.e. return if we are trapping on FP access.
 192          */
 193         cpacr = READ_SPECIALREG(cpacr_el1);
 194         if ((cpacr & CPACR_FPEN_MASK) == CPACR_FPEN_TRAP_NONE) {
 195                 KASSERT(PCPU_GET(fpcurthread) == td,
 196                     ("Storing an invalid VFP state"));
 197
 198                 vfp_store(pcb->pcb_fpusaved);
 199                 dsb(ish);
 200                 vfp_disable();
 201         }
 202         critical_exit();
 203 }
 204
 205 /*
 206  * Update the VFP state for a forked process or new thread. The PCB will
 207  * have been copied from the old thread.
 208  */
 209 void
 210 vfp_new_thread(struct thread *newtd, struct thread *oldtd, bool fork)
 211 {
 212         struct pcb *newpcb;
 213
 214         newpcb = newtd->td_pcb;
 215
 216         /* Kernel threads start with clean VFP */
 217         if ((oldtd->td_pflags & TDP_KTHREAD) != 0) {
 218                 newpcb->pcb_fpflags &=
 219                     ~(PCB_FP_STARTED | PCB_FP_KERN | PCB_FP_NOSAVE);
 220         } else {
 221                 MPASS((newpcb->pcb_fpflags & (PCB_FP_KERN|PCB_FP_NOSAVE)) == 0);
 222                 if (!fork) {
 223                         newpcb->pcb_fpflags &= ~PCB_FP_STARTED;
 224                 }
 225         }
 226
 227         newpcb->pcb_fpusaved = &newpcb->pcb_fpustate;
 228         newpcb->pcb_vfpcpu = UINT_MAX;
 229 }
 230
 231 /*
 232  * Reset the FP state to avoid leaking state from the parent process across
 233  * execve() (and to ensure that we get a consistent floating point environment
 234  * in every new process).
 235  */
 236 void
 237 vfp_reset_state(struct thread *td, struct pcb *pcb)
 238 {
 239         /* Discard the threads VFP state before resetting it */
 240         critical_enter();
 241         vfp_discard(td);
 242         critical_exit();
 243
 244         /*
 245          * Clear the thread state. The VFP is disabled and is not the current
 246          * VFP thread so we won't change any of these on context switch.
 247          */
 248         bzero(&pcb->pcb_fpustate.vfp_regs, sizeof(pcb->pcb_fpustate.vfp_regs));
 249         KASSERT(pcb->pcb_fpusaved == &pcb->pcb_fpustate,
 250             ("pcb_fpusaved should point to pcb_fpustate."));
 251         pcb->pcb_fpustate.vfp_fpcr = VFPCR_INIT;
 252         pcb->pcb_fpustate.vfp_fpsr = 0;
 253         pcb->pcb_vfpcpu = UINT_MAX;
 254         pcb->pcb_fpflags = 0;
 255 }
 256
 257 void
 258 vfp_restore_state(void)
 259 {
 260         struct pcb *curpcb;
 261         u_int cpu;
 262
 263         critical_enter();
 264
 265         cpu = PCPU_GET(cpuid);
 266         curpcb = curthread->td_pcb;
 267         curpcb->pcb_fpflags |= PCB_FP_STARTED;
 268
 269         vfp_enable();
 270
 271         /*
 272          * If the previous thread on this cpu to use the VFP was not the
 273          * current thread, or the current thread last used it on a different
 274          * cpu we need to restore the old state.
 275          */
 276         if (PCPU_GET(fpcurthread) != curthread || cpu != curpcb->pcb_vfpcpu) {
 277                 vfp_restore(curthread->td_pcb->pcb_fpusaved);
 278                 PCPU_SET(fpcurthread, curthread);
 279                 curpcb->pcb_vfpcpu = cpu;
 280         }
 281
 282         critical_exit();
 283 }
 284
 285 void
 286 vfp_init_secondary(void)
 287 {
 288         uint64_t pfr;
 289
 290         /* Check if there is a vfp unit present */
 291         pfr = READ_SPECIALREG(id_aa64pfr0_el1);
 292         if ((pfr & ID_AA64PFR0_FP_MASK) == ID_AA64PFR0_FP_NONE)
 293                 return;
 294
 295         /* Disable to be enabled when it's used */
 296         vfp_disable();
 297 }
 298
 299 static void
 300 vfp_init(const void *dummy __unused)
 301 {
 302         uint64_t pfr;
 303
 304         /* Check if there is a vfp unit present */
 305         pfr = READ_SPECIALREG(id_aa64pfr0_el1);
 306         if ((pfr & ID_AA64PFR0_FP_MASK) == ID_AA64PFR0_FP_NONE)
 307                 return;
 308
 309         fpu_save_area_zone = uma_zcreate("VFP_save_area",
 310             sizeof(struct vfpstate), NULL, NULL, NULL, NULL,
 311             _Alignof(struct vfpstate) - 1, 0);
 312         fpu_initialstate = uma_zalloc(fpu_save_area_zone, M_WAITOK | M_ZERO);
 313
 314         /* Ensure the VFP is enabled before accessing it in vfp_store */
 315         vfp_enable();
 316         vfp_store(fpu_initialstate);
 317
 318         /* Disable to be enabled when it's used */
 319         vfp_disable();
 320
 321         /* Zero the VFP registers but keep fpcr and fpsr */
 322         bzero(fpu_initialstate->vfp_regs, sizeof(fpu_initialstate->vfp_regs));
 323
 324         thread0.td_pcb->pcb_fpusaved->vfp_fpcr = VFPCR_INIT;
 325 }
 326
 327 SYSINIT(vfp, SI_SUB_CPU, SI_ORDER_ANY, vfp_init, NULL);
 328
 329 struct fpu_kern_ctx *
 330 fpu_kern_alloc_ctx(u_int flags)
 331 {
 332         struct fpu_kern_ctx *res;
 333         size_t sz;
 334
 335         sz = sizeof(struct fpu_kern_ctx);
 336         res = malloc(sz, M_FPUKERN_CTX, ((flags & FPU_KERN_NOWAIT) ?
 337             M_NOWAIT : M_WAITOK) | M_ZERO);
 338         return (res);
 339 }
 340
 341 void
 342 fpu_kern_free_ctx(struct fpu_kern_ctx *ctx)
 343 {
 344
 345         KASSERT((ctx->flags & FPU_KERN_CTX_INUSE) == 0, ("free'ing inuse ctx"));
 346         /* XXXAndrew clear the memory ? */
 347         free(ctx, M_FPUKERN_CTX);
 348 }
 349
 350 void
 351 fpu_kern_enter(struct thread *td, struct fpu_kern_ctx *ctx, u_int flags)
 352 {
 353         struct pcb *pcb;
 354
 355         pcb = td->td_pcb;
 356         KASSERT((flags & FPU_KERN_NOCTX) != 0 || ctx != NULL,
 357             ("ctx is required when !FPU_KERN_NOCTX"));
 358         KASSERT(ctx == NULL || (ctx->flags & FPU_KERN_CTX_INUSE) == 0,
 359             ("using inuse ctx"));
 360         KASSERT((pcb->pcb_fpflags & PCB_FP_NOSAVE) == 0,
 361             ("recursive fpu_kern_enter while in PCB_FP_NOSAVE state"));
 362
 363         if ((flags & FPU_KERN_NOCTX) != 0) {
 364                 critical_enter();
 365                 if (curthread == PCPU_GET(fpcurthread)) {
 366                         vfp_save_state(curthread, pcb);
 367                 }
 368                 PCPU_SET(fpcurthread, NULL);
 369
 370                 vfp_enable();
 371                 pcb->pcb_fpflags |= PCB_FP_KERN | PCB_FP_NOSAVE |
 372                     PCB_FP_STARTED;
 373                 return;
 374         }
 375
 376         if ((flags & FPU_KERN_KTHR) != 0 && is_fpu_kern_thread(0)) {
 377                 ctx->flags = FPU_KERN_CTX_DUMMY | FPU_KERN_CTX_INUSE;
 378                 return;
 379         }
 380         /*
 381          * Check either we are already using the VFP in the kernel, or
 382          * the saved state points to the default user space.
 383          */
 384         KASSERT((pcb->pcb_fpflags & PCB_FP_KERN) != 0 ||
 385             pcb->pcb_fpusaved == &pcb->pcb_fpustate,
 386             ("Mangled pcb_fpusaved %x %p %p", pcb->pcb_fpflags, pcb->pcb_fpusaved, &pcb->pcb_fpustate));
 387         ctx->flags = FPU_KERN_CTX_INUSE;
 388         vfp_save_state(curthread, pcb);
 389         ctx->prev = pcb->pcb_fpusaved;
 390         pcb->pcb_fpusaved = &ctx->state;
 391         pcb->pcb_fpflags |= PCB_FP_KERN;
 392         pcb->pcb_fpflags &= ~PCB_FP_STARTED;
 393
 394         return;
 395 }
 396
 397 int
 398 fpu_kern_leave(struct thread *td, struct fpu_kern_ctx *ctx)
 399 {
 400         struct pcb *pcb;
 401
 402         pcb = td->td_pcb;
 403
 404         if ((pcb->pcb_fpflags & PCB_FP_NOSAVE) != 0) {
 405                 KASSERT(ctx == NULL, ("non-null ctx after FPU_KERN_NOCTX"));
 406                 KASSERT(PCPU_GET(fpcurthread) == NULL,
 407                     ("non-NULL fpcurthread for PCB_FP_NOSAVE"));
 408                 CRITICAL_ASSERT(td);
 409
 410                 vfp_disable();
 411                 pcb->pcb_fpflags &= ~(PCB_FP_NOSAVE | PCB_FP_STARTED);
 412                 critical_exit();
 413         } else {
 414                 KASSERT((ctx->flags & FPU_KERN_CTX_INUSE) != 0,
 415                     ("FPU context not inuse"));
 416                 ctx->flags &= ~FPU_KERN_CTX_INUSE;
 417
 418                 if (is_fpu_kern_thread(0) &&
 419                     (ctx->flags & FPU_KERN_CTX_DUMMY) != 0)
 420                         return (0);
 421                 KASSERT((ctx->flags & FPU_KERN_CTX_DUMMY) == 0, ("dummy ctx"));
 422                 critical_enter();
 423                 vfp_discard(td);
 424                 critical_exit();
 425                 pcb->pcb_fpflags &= ~PCB_FP_STARTED;
 426                 pcb->pcb_fpusaved = ctx->prev;
 427         }
 428
 429         if (pcb->pcb_fpusaved == &pcb->pcb_fpustate) {
 430                 pcb->pcb_fpflags &= ~PCB_FP_KERN;
 431         } else {
 432                 KASSERT((pcb->pcb_fpflags & PCB_FP_KERN) != 0,
 433                     ("unpaired fpu_kern_leave"));
 434         }
 435
 436         return (0);
 437 }
 438
 439 int
 440 fpu_kern_thread(u_int flags __unused)
 441 {
 442         struct pcb *pcb = curthread->td_pcb;
 443
 444         KASSERT((curthread->td_pflags & TDP_KTHREAD) != 0,
 445             ("Only kthread may use fpu_kern_thread"));
 446         KASSERT(pcb->pcb_fpusaved == &pcb->pcb_fpustate,
 447             ("Mangled pcb_fpusaved"));
 448         KASSERT((pcb->pcb_fpflags & PCB_FP_KERN) == 0,
 449             ("Thread already setup for the VFP"));
 450         pcb->pcb_fpflags |= PCB_FP_KERN;
 451         return (0);
 452 }
 453
 454 int
 455 is_fpu_kern_thread(u_int flags __unused)
 456 {
 457         struct pcb *curpcb;
 458
 459         if ((curthread->td_pflags & TDP_KTHREAD) == 0)
 460                 return (0);
 461         curpcb = curthread->td_pcb;
 462         return ((curpcb->pcb_fpflags & PCB_FP_KERN) != 0);
 463 }
 464
 465 /*
 466  * FPU save area alloc/free/init utility routines
 467  */
 468 struct vfpstate *
 469 fpu_save_area_alloc(void)
 470 {
 471         return (uma_zalloc(fpu_save_area_zone, M_WAITOK));
 472 }
 473
 474 void
 475 fpu_save_area_free(struct vfpstate *fsa)
 476 {
 477         uma_zfree(fpu_save_area_zone, fsa);
 478 }
 479
 480 void
 481 fpu_save_area_reset(struct vfpstate *fsa)
 482 {
 483         memcpy(fsa, fpu_initialstate, sizeof(*fsa));
 484 }
 485 #endif