sys/arm64/arm64/vfp.c

   1 /*-
   2  * Copyright (c) 2015-2016 The FreeBSD Foundation
   3  * All rights reserved.
   4  *
   5  * This software was developed by Andrew Turner under
   6  * sponsorship from the FreeBSD Foundation.
   7  *
   8  * Redistribution and use in source and binary forms, with or without
   9  * modification, are permitted provided that the following conditions
  10  * are met:
  11  * 1. Redistributions of source code must retain the above copyright
  12  *    notice, this list of conditions and the following disclaimer.
  13  * 2. Redistributions in binary form must reproduce the above copyright
  14  *    notice, this list of conditions and the following disclaimer in the
  15  *    documentation and/or other materials provided with the distribution.
  16  *
  17  * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
  18  * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
  19  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
  20  * ARE DISCLAIMED.  IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
  21  * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
  22  * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
  23  * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
  24  * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
  25  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
  26  * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
  27  * SUCH DAMAGE.
  28  */
  29
  30 #include <sys/cdefs.h>
  31 __FBSDID("$FreeBSD$");
  32
  33 #ifdef VFP
  34 #include <sys/param.h>
  35 #include <sys/systm.h>
  36 #include <sys/limits.h>
  37 #include <sys/kernel.h>
  38 #include <sys/malloc.h>
  39 #include <sys/pcpu.h>
  40 #include <sys/proc.h>
  41
  42 #include <machine/armreg.h>
  43 #include <machine/md_var.h>
  44 #include <machine/pcb.h>
  45 #include <machine/vfp.h>
  46
  47 /* Sanity check we can store all the VFP registers */
  48 CTASSERT(sizeof(((struct pcb *)0)->pcb_fpustate.vfp_regs) == 16 * 32);
  49
  50 static MALLOC_DEFINE(M_FPUKERN_CTX, "fpukern_ctx",
  51     "Kernel contexts for VFP state");
  52
  53 struct fpu_kern_ctx {
  54         struct vfpstate *prev;
  55 #define FPU_KERN_CTX_DUMMY      0x01    /* avoided save for the kern thread */
  56 #define FPU_KERN_CTX_INUSE      0x02
  57         uint32_t         flags;
  58         struct vfpstate  state;
  59 };
  60
  61 static void
  62 vfp_enable(void)
  63 {
  64         uint32_t cpacr;
  65
  66         cpacr = READ_SPECIALREG(cpacr_el1);
  67         cpacr = (cpacr & ~CPACR_FPEN_MASK) | CPACR_FPEN_TRAP_NONE;
  68         WRITE_SPECIALREG(cpacr_el1, cpacr);
  69         isb();
  70 }
  71
  72 static void
  73 vfp_disable(void)
  74 {
  75         uint32_t cpacr;
  76
  77         cpacr = READ_SPECIALREG(cpacr_el1);
  78         cpacr = (cpacr & ~CPACR_FPEN_MASK) | CPACR_FPEN_TRAP_ALL1;
  79         WRITE_SPECIALREG(cpacr_el1, cpacr);
  80         isb();
  81 }
  82
  83 /*
  84  * Called when the thread is dying or when discarding the kernel VFP state.
  85  * If the thread was the last to use the VFP unit mark it as unused to tell
  86  * the kernel the fp state is unowned. Ensure the VFP unit is off so we get
  87  * an exception on the next access.
  88  */
  89 void
  90 vfp_discard(struct thread *td)
  91 {
  92
  93 #ifdef INVARIANTS
  94         if (td != NULL)
  95                 CRITICAL_ASSERT(td);
  96 #endif
  97         if (PCPU_GET(fpcurthread) == td)
  98                 PCPU_SET(fpcurthread, NULL);
  99
 100         vfp_disable();
 101 }
 102
 103 static void
 104 vfp_store(struct vfpstate *state)
 105 {
 106         __uint128_t *vfp_state;
 107         uint64_t fpcr, fpsr;
 108
 109         vfp_state = state->vfp_regs;
 110         __asm __volatile(
 111             "mrs        %0, fpcr                \n"
 112             "mrs        %1, fpsr                \n"
 113             "stp        q0,  q1,  [%2, #16 *  0]\n"
 114             "stp        q2,  q3,  [%2, #16 *  2]\n"
 115             "stp        q4,  q5,  [%2, #16 *  4]\n"
 116             "stp        q6,  q7,  [%2, #16 *  6]\n"
 117             "stp        q8,  q9,  [%2, #16 *  8]\n"
 118             "stp        q10, q11, [%2, #16 * 10]\n"
 119             "stp        q12, q13, [%2, #16 * 12]\n"
 120             "stp        q14, q15, [%2, #16 * 14]\n"
 121             "stp        q16, q17, [%2, #16 * 16]\n"
 122             "stp        q18, q19, [%2, #16 * 18]\n"
 123             "stp        q20, q21, [%2, #16 * 20]\n"
 124             "stp        q22, q23, [%2, #16 * 22]\n"
 125             "stp        q24, q25, [%2, #16 * 24]\n"
 126             "stp        q26, q27, [%2, #16 * 26]\n"
 127             "stp        q28, q29, [%2, #16 * 28]\n"
 128             "stp        q30, q31, [%2, #16 * 30]\n"
 129             : "=&r"(fpcr), "=&r"(fpsr) : "r"(vfp_state));
 130
 131         state->vfp_fpcr = fpcr;
 132         state->vfp_fpsr = fpsr;
 133 }
 134
 135 static void
 136 vfp_restore(struct vfpstate *state)
 137 {
 138         __uint128_t *vfp_state;
 139         uint64_t fpcr, fpsr;
 140
 141         vfp_state = state->vfp_regs;
 142         fpcr = state->vfp_fpcr;
 143         fpsr = state->vfp_fpsr;
 144
 145         __asm __volatile(
 146             "ldp        q0,  q1,  [%2, #16 *  0]\n"
 147             "ldp        q2,  q3,  [%2, #16 *  2]\n"
 148             "ldp        q4,  q5,  [%2, #16 *  4]\n"
 149             "ldp        q6,  q7,  [%2, #16 *  6]\n"
 150             "ldp        q8,  q9,  [%2, #16 *  8]\n"
 151             "ldp        q10, q11, [%2, #16 * 10]\n"
 152             "ldp        q12, q13, [%2, #16 * 12]\n"
 153             "ldp        q14, q15, [%2, #16 * 14]\n"
 154             "ldp        q16, q17, [%2, #16 * 16]\n"
 155             "ldp        q18, q19, [%2, #16 * 18]\n"
 156             "ldp        q20, q21, [%2, #16 * 20]\n"
 157             "ldp        q22, q23, [%2, #16 * 22]\n"
 158             "ldp        q24, q25, [%2, #16 * 24]\n"
 159             "ldp        q26, q27, [%2, #16 * 26]\n"
 160             "ldp        q28, q29, [%2, #16 * 28]\n"
 161             "ldp        q30, q31, [%2, #16 * 30]\n"
 162             "msr        fpcr, %0                \n"
 163             "msr        fpsr, %1                \n"
 164             : : "r"(fpcr), "r"(fpsr), "r"(vfp_state));
 165 }
 166
 167 void
 168 vfp_save_state(struct thread *td, struct pcb *pcb)
 169 {
 170         uint32_t cpacr;
 171
 172         KASSERT(pcb != NULL, ("NULL vfp pcb"));
 173         KASSERT(td == NULL || td->td_pcb == pcb, ("Invalid vfp pcb"));
 174
 175         /*
 176          * savectx() will be called on panic with dumppcb as an argument,
 177          * dumppcb doesn't have pcb_fpusaved set, so set it to save
 178          * the VFP registers.
 179          */
 180         if (pcb->pcb_fpusaved == NULL)
 181                 pcb->pcb_fpusaved = &pcb->pcb_fpustate;
 182
 183         if (td == NULL)
 184                 td = curthread;
 185
 186         critical_enter();
 187         /*
 188          * Only store the registers if the VFP is enabled,
 189          * i.e. return if we are trapping on FP access.
 190          */
 191         cpacr = READ_SPECIALREG(cpacr_el1);
 192         if ((cpacr & CPACR_FPEN_MASK) == CPACR_FPEN_TRAP_NONE) {
 193                 KASSERT(PCPU_GET(fpcurthread) == td,
 194                     ("Storing an invalid VFP state"));
 195
 196                 vfp_store(pcb->pcb_fpusaved);
 197                 dsb(ish);
 198                 vfp_disable();
 199         }
 200         critical_exit();
 201 }
 202
 203 /*
 204  * Reset the FP state to avoid leaking state from the parent process across
 205  * execve() (and to ensure that we get a consistent floating point environment
 206  * in every new process).
 207  */
 208 void
 209 vfp_reset_state(struct thread *td, struct pcb *pcb)
 210 {
 211         critical_enter();
 212         bzero(&pcb->pcb_fpustate.vfp_regs, sizeof(pcb->pcb_fpustate.vfp_regs));
 213         KASSERT(pcb->pcb_fpusaved == &pcb->pcb_fpustate,
 214             ("pcb_fpusaved should point to pcb_fpustate."));
 215         pcb->pcb_fpustate.vfp_fpcr = initial_fpcr;
 216         pcb->pcb_fpustate.vfp_fpsr = 0;
 217         pcb->pcb_vfpcpu = UINT_MAX;
 218         pcb->pcb_fpflags = 0;
 219         vfp_discard(td);
 220         critical_exit();
 221 }
 222
 223 void
 224 vfp_restore_state(void)
 225 {
 226         struct pcb *curpcb;
 227         u_int cpu;
 228
 229         critical_enter();
 230
 231         cpu = PCPU_GET(cpuid);
 232         curpcb = curthread->td_pcb;
 233         curpcb->pcb_fpflags |= PCB_FP_STARTED;
 234
 235         vfp_enable();
 236
 237         /*
 238          * If the previous thread on this cpu to use the VFP was not the
 239          * current thread, or the current thread last used it on a different
 240          * cpu we need to restore the old state.
 241          */
 242         if (PCPU_GET(fpcurthread) != curthread || cpu != curpcb->pcb_vfpcpu) {
 243                 vfp_restore(curthread->td_pcb->pcb_fpusaved);
 244                 PCPU_SET(fpcurthread, curthread);
 245                 curpcb->pcb_vfpcpu = cpu;
 246         }
 247
 248         critical_exit();
 249 }
 250
 251 void
 252 vfp_init(void)
 253 {
 254         uint64_t pfr;
 255
 256         /* Check if there is a vfp unit present */
 257         pfr = READ_SPECIALREG(id_aa64pfr0_el1);
 258         if ((pfr & ID_AA64PFR0_FP_MASK) == ID_AA64PFR0_FP_NONE)
 259                 return;
 260
 261         /* Disable to be enabled when it's used */
 262         vfp_disable();
 263
 264         if (PCPU_GET(cpuid) == 0)
 265                 thread0.td_pcb->pcb_fpusaved->vfp_fpcr = initial_fpcr;
 266 }
 267
 268 SYSINIT(vfp, SI_SUB_CPU, SI_ORDER_ANY, vfp_init, NULL);
 269
 270 struct fpu_kern_ctx *
 271 fpu_kern_alloc_ctx(u_int flags)
 272 {
 273         struct fpu_kern_ctx *res;
 274         size_t sz;
 275
 276         sz = sizeof(struct fpu_kern_ctx);
 277         res = malloc(sz, M_FPUKERN_CTX, ((flags & FPU_KERN_NOWAIT) ?
 278             M_NOWAIT : M_WAITOK) | M_ZERO);
 279         return (res);
 280 }
 281
 282 void
 283 fpu_kern_free_ctx(struct fpu_kern_ctx *ctx)
 284 {
 285
 286         KASSERT((ctx->flags & FPU_KERN_CTX_INUSE) == 0, ("free'ing inuse ctx"));
 287         /* XXXAndrew clear the memory ? */
 288         free(ctx, M_FPUKERN_CTX);
 289 }
 290
 291 void
 292 fpu_kern_enter(struct thread *td, struct fpu_kern_ctx *ctx, u_int flags)
 293 {
 294         struct pcb *pcb;
 295
 296         pcb = td->td_pcb;
 297         KASSERT((flags & FPU_KERN_NOCTX) != 0 || ctx != NULL,
 298             ("ctx is required when !FPU_KERN_NOCTX"));
 299         KASSERT(ctx == NULL || (ctx->flags & FPU_KERN_CTX_INUSE) == 0,
 300             ("using inuse ctx"));
 301         KASSERT((pcb->pcb_fpflags & PCB_FP_NOSAVE) == 0,
 302             ("recursive fpu_kern_enter while in PCB_FP_NOSAVE state"));
 303
 304         if ((flags & FPU_KERN_NOCTX) != 0) {
 305                 critical_enter();
 306                 if (curthread == PCPU_GET(fpcurthread)) {
 307                         vfp_save_state(curthread, pcb);
 308                 }
 309                 PCPU_SET(fpcurthread, NULL);
 310
 311                 vfp_enable();
 312                 pcb->pcb_fpflags |= PCB_FP_KERN | PCB_FP_NOSAVE |
 313                     PCB_FP_STARTED;
 314                 return;
 315         }
 316
 317         if ((flags & FPU_KERN_KTHR) != 0 && is_fpu_kern_thread(0)) {
 318                 ctx->flags = FPU_KERN_CTX_DUMMY | FPU_KERN_CTX_INUSE;
 319                 return;
 320         }
 321         /*
 322          * Check either we are already using the VFP in the kernel, or
 323          * the the saved state points to the default user space.
 324          */
 325         KASSERT((pcb->pcb_fpflags & PCB_FP_KERN) != 0 ||
 326             pcb->pcb_fpusaved == &pcb->pcb_fpustate,
 327             ("Mangled pcb_fpusaved %x %p %p", pcb->pcb_fpflags, pcb->pcb_fpusaved, &pcb->pcb_fpustate));
 328         ctx->flags = FPU_KERN_CTX_INUSE;
 329         vfp_save_state(curthread, pcb);
 330         ctx->prev = pcb->pcb_fpusaved;
 331         pcb->pcb_fpusaved = &ctx->state;
 332         pcb->pcb_fpflags |= PCB_FP_KERN;
 333         pcb->pcb_fpflags &= ~PCB_FP_STARTED;
 334
 335         return;
 336 }
 337
 338 int
 339 fpu_kern_leave(struct thread *td, struct fpu_kern_ctx *ctx)
 340 {
 341         struct pcb *pcb;
 342
 343         pcb = td->td_pcb;
 344
 345         if ((pcb->pcb_fpflags & PCB_FP_NOSAVE) != 0) {
 346                 KASSERT(ctx == NULL, ("non-null ctx after FPU_KERN_NOCTX"));
 347                 KASSERT(PCPU_GET(fpcurthread) == NULL,
 348                     ("non-NULL fpcurthread for PCB_FP_NOSAVE"));
 349                 CRITICAL_ASSERT(td);
 350
 351                 vfp_disable();
 352                 pcb->pcb_fpflags &= ~(PCB_FP_NOSAVE | PCB_FP_STARTED);
 353                 critical_exit();
 354         } else {
 355                 KASSERT((ctx->flags & FPU_KERN_CTX_INUSE) != 0,
 356                     ("FPU context not inuse"));
 357                 ctx->flags &= ~FPU_KERN_CTX_INUSE;
 358
 359                 if (is_fpu_kern_thread(0) &&
 360                     (ctx->flags & FPU_KERN_CTX_DUMMY) != 0)
 361                         return (0);
 362                 KASSERT((ctx->flags & FPU_KERN_CTX_DUMMY) == 0, ("dummy ctx"));
 363                 critical_enter();
 364                 vfp_discard(td);
 365                 critical_exit();
 366                 pcb->pcb_fpflags &= ~PCB_FP_STARTED;
 367                 pcb->pcb_fpusaved = ctx->prev;
 368         }
 369
 370         if (pcb->pcb_fpusaved == &pcb->pcb_fpustate) {
 371                 pcb->pcb_fpflags &= ~PCB_FP_KERN;
 372         } else {
 373                 KASSERT((pcb->pcb_fpflags & PCB_FP_KERN) != 0,
 374                     ("unpaired fpu_kern_leave"));
 375         }
 376
 377         return (0);
 378 }
 379
 380 int
 381 fpu_kern_thread(u_int flags __unused)
 382 {
 383         struct pcb *pcb = curthread->td_pcb;
 384
 385         KASSERT((curthread->td_pflags & TDP_KTHREAD) != 0,
 386             ("Only kthread may use fpu_kern_thread"));
 387         KASSERT(pcb->pcb_fpusaved == &pcb->pcb_fpustate,
 388             ("Mangled pcb_fpusaved"));
 389         KASSERT((pcb->pcb_fpflags & PCB_FP_KERN) == 0,
 390             ("Thread already setup for the VFP"));
 391         pcb->pcb_fpflags |= PCB_FP_KERN;
 392         return (0);
 393 }
 394
 395 int
 396 is_fpu_kern_thread(u_int flags __unused)
 397 {
 398         struct pcb *curpcb;
 399
 400         if ((curthread->td_pflags & TDP_KTHREAD) == 0)
 401                 return (0);
 402         curpcb = curthread->td_pcb;
 403         return ((curpcb->pcb_fpflags & PCB_FP_KERN) != 0);
 404 }
 405 #endif