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/kernel.h>
  37 #include <sys/malloc.h>
  38 #include <sys/pcpu.h>
  39 #include <sys/proc.h>
  40
  41 #include <machine/armreg.h>
  42 #include <machine/pcb.h>
  43 #include <machine/vfp.h>
  44
  45 /* Sanity check we can store all the VFP registers */
  46 CTASSERT(sizeof(((struct pcb *)0)->pcb_fpustate.vfp_regs) == 16 * 32);
  47
  48 static MALLOC_DEFINE(M_FPUKERN_CTX, "fpukern_ctx",
  49     "Kernel contexts for VFP state");
  50
  51 struct fpu_kern_ctx {
  52         struct vfpstate *prev;
  53 #define FPU_KERN_CTX_DUMMY      0x01    /* avoided save for the kern thread */
  54 #define FPU_KERN_CTX_INUSE      0x02
  55         uint32_t         flags;
  56         struct vfpstate  state;
  57 };
  58
  59 static void
  60 vfp_enable(void)
  61 {
  62         uint32_t cpacr;
  63
  64         cpacr = READ_SPECIALREG(cpacr_el1);
  65         cpacr = (cpacr & ~CPACR_FPEN_MASK) | CPACR_FPEN_TRAP_NONE;
  66         WRITE_SPECIALREG(cpacr_el1, cpacr);
  67         isb();
  68 }
  69
  70 static void
  71 vfp_disable(void)
  72 {
  73         uint32_t cpacr;
  74
  75         cpacr = READ_SPECIALREG(cpacr_el1);
  76         cpacr = (cpacr & ~CPACR_FPEN_MASK) | CPACR_FPEN_TRAP_ALL1;
  77         WRITE_SPECIALREG(cpacr_el1, cpacr);
  78         isb();
  79 }
  80
  81 /*
  82  * Called when the thread is dying or when discarding the kernel VFP state.
  83  * If the thread was the last to use the VFP unit mark it as unused to tell
  84  * the kernel the fp state is unowned. Ensure the VFP unit is off so we get
  85  * an exception on the next access.
  86  */
  87 void
  88 vfp_discard(struct thread *td)
  89 {
  90
  91 #ifdef INVARIANTS
  92         if (td != NULL)
  93                 CRITICAL_ASSERT(td);
  94 #endif
  95         if (PCPU_GET(fpcurthread) == td)
  96                 PCPU_SET(fpcurthread, NULL);
  97
  98         vfp_disable();
  99 }
 100
 101 static void
 102 vfp_store(struct vfpstate *state)
 103 {
 104         __int128_t *vfp_state;
 105         uint64_t fpcr, fpsr;
 106
 107         vfp_state = state->vfp_regs;
 108         __asm __volatile(
 109             "mrs        %0, fpcr                \n"
 110             "mrs        %1, fpsr                \n"
 111             "stp        q0,  q1,  [%2, #16 *  0]\n"
 112             "stp        q2,  q3,  [%2, #16 *  2]\n"
 113             "stp        q4,  q5,  [%2, #16 *  4]\n"
 114             "stp        q6,  q7,  [%2, #16 *  6]\n"
 115             "stp        q8,  q9,  [%2, #16 *  8]\n"
 116             "stp        q10, q11, [%2, #16 * 10]\n"
 117             "stp        q12, q13, [%2, #16 * 12]\n"
 118             "stp        q14, q15, [%2, #16 * 14]\n"
 119             "stp        q16, q17, [%2, #16 * 16]\n"
 120             "stp        q18, q19, [%2, #16 * 18]\n"
 121             "stp        q20, q21, [%2, #16 * 20]\n"
 122             "stp        q22, q23, [%2, #16 * 22]\n"
 123             "stp        q24, q25, [%2, #16 * 24]\n"
 124             "stp        q26, q27, [%2, #16 * 26]\n"
 125             "stp        q28, q29, [%2, #16 * 28]\n"
 126             "stp        q30, q31, [%2, #16 * 30]\n"
 127             : "=&r"(fpcr), "=&r"(fpsr) : "r"(vfp_state));
 128
 129         state->vfp_fpcr = fpcr;
 130         state->vfp_fpsr = fpsr;
 131 }
 132
 133 static void
 134 vfp_restore(struct vfpstate *state)
 135 {
 136         __int128_t *vfp_state;
 137         uint64_t fpcr, fpsr;
 138
 139         vfp_state = state->vfp_regs;
 140         fpcr = state->vfp_fpcr;
 141         fpsr = state->vfp_fpsr;
 142
 143         __asm __volatile(
 144             "ldp        q0,  q1,  [%2, #16 *  0]\n"
 145             "ldp        q2,  q3,  [%2, #16 *  2]\n"
 146             "ldp        q4,  q5,  [%2, #16 *  4]\n"
 147             "ldp        q6,  q7,  [%2, #16 *  6]\n"
 148             "ldp        q8,  q9,  [%2, #16 *  8]\n"
 149             "ldp        q10, q11, [%2, #16 * 10]\n"
 150             "ldp        q12, q13, [%2, #16 * 12]\n"
 151             "ldp        q14, q15, [%2, #16 * 14]\n"
 152             "ldp        q16, q17, [%2, #16 * 16]\n"
 153             "ldp        q18, q19, [%2, #16 * 18]\n"
 154             "ldp        q20, q21, [%2, #16 * 20]\n"
 155             "ldp        q22, q23, [%2, #16 * 22]\n"
 156             "ldp        q24, q25, [%2, #16 * 24]\n"
 157             "ldp        q26, q27, [%2, #16 * 26]\n"
 158             "ldp        q28, q29, [%2, #16 * 28]\n"
 159             "ldp        q30, q31, [%2, #16 * 30]\n"
 160             "msr        fpcr, %0                \n"
 161             "msr        fpsr, %1                \n"
 162             : : "r"(fpcr), "r"(fpsr), "r"(vfp_state));
 163 }
 164
 165 void
 166 vfp_save_state(struct thread *td, struct pcb *pcb)
 167 {
 168         uint32_t cpacr;
 169
 170         KASSERT(pcb != NULL, ("NULL vfp pcb"));
 171         KASSERT(td == NULL || td->td_pcb == pcb, ("Invalid vfp pcb"));
 172
 173         if (td == NULL)
 174                 td = curthread;
 175
 176         critical_enter();
 177         /*
 178          * Only store the registers if the VFP is enabled,
 179          * i.e. return if we are trapping on FP access.
 180          */
 181         cpacr = READ_SPECIALREG(cpacr_el1);
 182         if ((cpacr & CPACR_FPEN_MASK) == CPACR_FPEN_TRAP_NONE) {
 183                 KASSERT(PCPU_GET(fpcurthread) == td,
 184                     ("Storing an invalid VFP state"));
 185
 186                 vfp_store(pcb->pcb_fpusaved);
 187                 dsb(ish);
 188                 vfp_disable();
 189         }
 190         critical_exit();
 191 }
 192
 193 void
 194 vfp_restore_state(void)
 195 {
 196         struct pcb *curpcb;
 197         u_int cpu;
 198
 199         critical_enter();
 200
 201         cpu = PCPU_GET(cpuid);
 202         curpcb = curthread->td_pcb;
 203         curpcb->pcb_fpflags |= PCB_FP_STARTED;
 204
 205         vfp_enable();
 206
 207         /*
 208          * If the previous thread on this cpu to use the VFP was not the
 209          * current thread, or the current thread last used it on a different
 210          * cpu we need to restore the old state.
 211          */
 212         if (PCPU_GET(fpcurthread) != curthread || cpu != curpcb->pcb_vfpcpu) {
 213
 214                 vfp_restore(curthread->td_pcb->pcb_fpusaved);
 215                 PCPU_SET(fpcurthread, curthread);
 216                 curpcb->pcb_vfpcpu = cpu;
 217         }
 218
 219         critical_exit();
 220 }
 221
 222 void
 223 vfp_init(void)
 224 {
 225         uint64_t pfr;
 226
 227         /* Check if there is a vfp unit present */
 228         pfr = READ_SPECIALREG(id_aa64pfr0_el1);
 229         if ((pfr & ID_AA64PFR0_FP_MASK) == ID_AA64PFR0_FP_NONE)
 230                 return;
 231
 232         /* Disable to be enabled when it's used */
 233         vfp_disable();
 234 }
 235
 236 SYSINIT(vfp, SI_SUB_CPU, SI_ORDER_ANY, vfp_init, NULL);
 237
 238 struct fpu_kern_ctx *
 239 fpu_kern_alloc_ctx(u_int flags)
 240 {
 241         struct fpu_kern_ctx *res;
 242         size_t sz;
 243
 244         sz = sizeof(struct fpu_kern_ctx);
 245         res = malloc(sz, M_FPUKERN_CTX, ((flags & FPU_KERN_NOWAIT) ?
 246             M_NOWAIT : M_WAITOK) | M_ZERO);
 247         return (res);
 248 }
 249
 250 void
 251 fpu_kern_free_ctx(struct fpu_kern_ctx *ctx)
 252 {
 253
 254         KASSERT((ctx->flags & FPU_KERN_CTX_INUSE) == 0, ("free'ing inuse ctx"));
 255         /* XXXAndrew clear the memory ? */
 256         free(ctx, M_FPUKERN_CTX);
 257 }
 258
 259 int
 260 fpu_kern_enter(struct thread *td, struct fpu_kern_ctx *ctx, u_int flags)
 261 {
 262         struct pcb *pcb;
 263
 264         pcb = td->td_pcb;
 265         KASSERT((flags & FPU_KERN_NOCTX) != 0 || ctx != NULL,
 266             ("ctx is required when !FPU_KERN_NOCTX"));
 267         KASSERT(ctx == NULL || (ctx->flags & FPU_KERN_CTX_INUSE) == 0,
 268             ("using inuse ctx"));
 269         KASSERT((pcb->pcb_fpflags & PCB_FP_NOSAVE) == 0,
 270             ("recursive fpu_kern_enter while in PCB_FP_NOSAVE state"));
 271
 272         if ((flags & FPU_KERN_NOCTX) != 0) {
 273                 critical_enter();
 274                 if (curthread == PCPU_GET(fpcurthread)) {
 275                         vfp_save_state(curthread, pcb);
 276                         PCPU_SET(fpcurthread, NULL);
 277                 } else {
 278                         KASSERT(PCPU_GET(fpcurthread) == NULL,
 279                             ("invalid fpcurthread"));
 280                 }
 281
 282                 vfp_enable();
 283                 pcb->pcb_fpflags |= PCB_FP_KERN | PCB_FP_NOSAVE |
 284                     PCB_FP_STARTED;
 285                 return (0);
 286         }
 287
 288         if ((flags & FPU_KERN_KTHR) != 0 && is_fpu_kern_thread(0)) {
 289                 ctx->flags = FPU_KERN_CTX_DUMMY | FPU_KERN_CTX_INUSE;
 290                 return (0);
 291         }
 292         /*
 293          * Check either we are already using the VFP in the kernel, or
 294          * the the saved state points to the default user space.
 295          */
 296         KASSERT((pcb->pcb_fpflags & PCB_FP_KERN) != 0 ||
 297             pcb->pcb_fpusaved == &pcb->pcb_fpustate,
 298             ("Mangled pcb_fpusaved %x %p %p", pcb->pcb_fpflags, pcb->pcb_fpusaved, &pcb->pcb_fpustate));
 299         ctx->flags = FPU_KERN_CTX_INUSE;
 300         vfp_save_state(curthread, pcb);
 301         ctx->prev = pcb->pcb_fpusaved;
 302         pcb->pcb_fpusaved = &ctx->state;
 303         pcb->pcb_fpflags |= PCB_FP_KERN;
 304         pcb->pcb_fpflags &= ~PCB_FP_STARTED;
 305
 306         return (0);
 307 }
 308
 309 int
 310 fpu_kern_leave(struct thread *td, struct fpu_kern_ctx *ctx)
 311 {
 312         struct pcb *pcb;
 313
 314         pcb = td->td_pcb;
 315
 316         if ((pcb->pcb_fpflags & PCB_FP_NOSAVE) != 0) {
 317                 KASSERT(ctx == NULL, ("non-null ctx after FPU_KERN_NOCTX"));
 318                 KASSERT(PCPU_GET(fpcurthread) == NULL,
 319                     ("non-NULL fpcurthread for PCB_FP_NOSAVE"));
 320                 CRITICAL_ASSERT(td);
 321
 322                 vfp_disable();
 323                 pcb->pcb_fpflags &= ~(PCB_FP_NOSAVE | PCB_FP_STARTED);
 324                 critical_exit();
 325         } else {
 326                 KASSERT((ctx->flags & FPU_KERN_CTX_INUSE) != 0,
 327                     ("FPU context not inuse"));
 328                 ctx->flags &= ~FPU_KERN_CTX_INUSE;
 329
 330                 if (is_fpu_kern_thread(0) &&
 331                     (ctx->flags & FPU_KERN_CTX_DUMMY) != 0)
 332                         return (0);
 333                 KASSERT((ctx->flags & FPU_KERN_CTX_DUMMY) == 0, ("dummy ctx"));
 334                 critical_enter();
 335                 vfp_discard(td);
 336                 critical_exit();
 337                 pcb->pcb_fpflags &= ~PCB_FP_STARTED;
 338                 pcb->pcb_fpusaved = ctx->prev;
 339         }
 340
 341         if (pcb->pcb_fpusaved == &pcb->pcb_fpustate) {
 342                 pcb->pcb_fpflags &= ~PCB_FP_KERN;
 343         } else {
 344                 KASSERT((pcb->pcb_fpflags & PCB_FP_KERN) != 0,
 345                     ("unpaired fpu_kern_leave"));
 346         }
 347
 348         return (0);
 349 }
 350
 351 int
 352 fpu_kern_thread(u_int flags)
 353 {
 354         struct pcb *pcb = curthread->td_pcb;
 355
 356         KASSERT((curthread->td_pflags & TDP_KTHREAD) != 0,
 357             ("Only kthread may use fpu_kern_thread"));
 358         KASSERT(pcb->pcb_fpusaved == &pcb->pcb_fpustate,
 359             ("Mangled pcb_fpusaved"));
 360         KASSERT((pcb->pcb_fpflags & PCB_FP_KERN) == 0,
 361             ("Thread already setup for the VFP"));
 362         pcb->pcb_fpflags |= PCB_FP_KERN;
 363         return (0);
 364 }
 365
 366 int
 367 is_fpu_kern_thread(u_int flags)
 368 {
 369         struct pcb *curpcb;
 370
 371         if ((curthread->td_pflags & TDP_KTHREAD) == 0)
 372                 return (0);
 373         curpcb = curthread->td_pcb;
 374         return ((curpcb->pcb_fpflags & PCB_FP_KERN) != 0);
 375 }
 376 #endif