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         /*
 174          * savectx() will be called on panic with dumppcb as an argument,
 175          * dumppcb doesn't have pcb_fpusaved set, so set it to save
 176          * the VFP registers.
 177          */
 178         if (pcb->pcb_fpusaved == NULL)
 179                 pcb->pcb_fpusaved = &pcb->pcb_fpustate;
 180
 181         if (td == NULL)
 182                 td = curthread;
 183
 184         critical_enter();
 185         /*
 186          * Only store the registers if the VFP is enabled,
 187          * i.e. return if we are trapping on FP access.
 188          */
 189         cpacr = READ_SPECIALREG(cpacr_el1);
 190         if ((cpacr & CPACR_FPEN_MASK) == CPACR_FPEN_TRAP_NONE) {
 191                 KASSERT(PCPU_GET(fpcurthread) == td,
 192                     ("Storing an invalid VFP state"));
 193
 194                 vfp_store(pcb->pcb_fpusaved);
 195                 dsb(ish);
 196                 vfp_disable();
 197         }
 198         critical_exit();
 199 }
 200
 201 void
 202 vfp_restore_state(void)
 203 {
 204         struct pcb *curpcb;
 205         u_int cpu;
 206
 207         critical_enter();
 208
 209         cpu = PCPU_GET(cpuid);
 210         curpcb = curthread->td_pcb;
 211         curpcb->pcb_fpflags |= PCB_FP_STARTED;
 212
 213         vfp_enable();
 214
 215         /*
 216          * If the previous thread on this cpu to use the VFP was not the
 217          * current thread, or the current thread last used it on a different
 218          * cpu we need to restore the old state.
 219          */
 220         if (PCPU_GET(fpcurthread) != curthread || cpu != curpcb->pcb_vfpcpu) {
 221                 vfp_restore(curthread->td_pcb->pcb_fpusaved);
 222                 PCPU_SET(fpcurthread, curthread);
 223                 curpcb->pcb_vfpcpu = cpu;
 224         }
 225
 226         critical_exit();
 227 }
 228
 229 void
 230 vfp_init(void)
 231 {
 232         uint64_t pfr;
 233
 234         /* Check if there is a vfp unit present */
 235         pfr = READ_SPECIALREG(id_aa64pfr0_el1);
 236         if ((pfr & ID_AA64PFR0_FP_MASK) == ID_AA64PFR0_FP_NONE)
 237                 return;
 238
 239         /* Disable to be enabled when it's used */
 240         vfp_disable();
 241 }
 242
 243 SYSINIT(vfp, SI_SUB_CPU, SI_ORDER_ANY, vfp_init, NULL);
 244
 245 struct fpu_kern_ctx *
 246 fpu_kern_alloc_ctx(u_int flags)
 247 {
 248         struct fpu_kern_ctx *res;
 249         size_t sz;
 250
 251         sz = sizeof(struct fpu_kern_ctx);
 252         res = malloc(sz, M_FPUKERN_CTX, ((flags & FPU_KERN_NOWAIT) ?
 253             M_NOWAIT : M_WAITOK) | M_ZERO);
 254         return (res);
 255 }
 256
 257 void
 258 fpu_kern_free_ctx(struct fpu_kern_ctx *ctx)
 259 {
 260
 261         KASSERT((ctx->flags & FPU_KERN_CTX_INUSE) == 0, ("free'ing inuse ctx"));
 262         /* XXXAndrew clear the memory ? */
 263         free(ctx, M_FPUKERN_CTX);
 264 }
 265
 266 void
 267 fpu_kern_enter(struct thread *td, struct fpu_kern_ctx *ctx, u_int flags)
 268 {
 269         struct pcb *pcb;
 270
 271         pcb = td->td_pcb;
 272         KASSERT((flags & FPU_KERN_NOCTX) != 0 || ctx != NULL,
 273             ("ctx is required when !FPU_KERN_NOCTX"));
 274         KASSERT(ctx == NULL || (ctx->flags & FPU_KERN_CTX_INUSE) == 0,
 275             ("using inuse ctx"));
 276         KASSERT((pcb->pcb_fpflags & PCB_FP_NOSAVE) == 0,
 277             ("recursive fpu_kern_enter while in PCB_FP_NOSAVE state"));
 278
 279         if ((flags & FPU_KERN_NOCTX) != 0) {
 280                 critical_enter();
 281                 if (curthread == PCPU_GET(fpcurthread)) {
 282                         vfp_save_state(curthread, pcb);
 283                 }
 284                 PCPU_SET(fpcurthread, NULL);
 285
 286                 vfp_enable();
 287                 pcb->pcb_fpflags |= PCB_FP_KERN | PCB_FP_NOSAVE |
 288                     PCB_FP_STARTED;
 289                 return;
 290         }
 291
 292         if ((flags & FPU_KERN_KTHR) != 0 && is_fpu_kern_thread(0)) {
 293                 ctx->flags = FPU_KERN_CTX_DUMMY | FPU_KERN_CTX_INUSE;
 294                 return;
 295         }
 296         /*
 297          * Check either we are already using the VFP in the kernel, or
 298          * the the saved state points to the default user space.
 299          */
 300         KASSERT((pcb->pcb_fpflags & PCB_FP_KERN) != 0 ||
 301             pcb->pcb_fpusaved == &pcb->pcb_fpustate,
 302             ("Mangled pcb_fpusaved %x %p %p", pcb->pcb_fpflags, pcb->pcb_fpusaved, &pcb->pcb_fpustate));
 303         ctx->flags = FPU_KERN_CTX_INUSE;
 304         vfp_save_state(curthread, pcb);
 305         ctx->prev = pcb->pcb_fpusaved;
 306         pcb->pcb_fpusaved = &ctx->state;
 307         pcb->pcb_fpflags |= PCB_FP_KERN;
 308         pcb->pcb_fpflags &= ~PCB_FP_STARTED;
 309
 310         return;
 311 }
 312
 313 int
 314 fpu_kern_leave(struct thread *td, struct fpu_kern_ctx *ctx)
 315 {
 316         struct pcb *pcb;
 317
 318         pcb = td->td_pcb;
 319
 320         if ((pcb->pcb_fpflags & PCB_FP_NOSAVE) != 0) {
 321                 KASSERT(ctx == NULL, ("non-null ctx after FPU_KERN_NOCTX"));
 322                 KASSERT(PCPU_GET(fpcurthread) == NULL,
 323                     ("non-NULL fpcurthread for PCB_FP_NOSAVE"));
 324                 CRITICAL_ASSERT(td);
 325
 326                 vfp_disable();
 327                 pcb->pcb_fpflags &= ~(PCB_FP_NOSAVE | PCB_FP_STARTED);
 328                 critical_exit();
 329         } else {
 330                 KASSERT((ctx->flags & FPU_KERN_CTX_INUSE) != 0,
 331                     ("FPU context not inuse"));
 332                 ctx->flags &= ~FPU_KERN_CTX_INUSE;
 333
 334                 if (is_fpu_kern_thread(0) &&
 335                     (ctx->flags & FPU_KERN_CTX_DUMMY) != 0)
 336                         return (0);
 337                 KASSERT((ctx->flags & FPU_KERN_CTX_DUMMY) == 0, ("dummy ctx"));
 338                 critical_enter();
 339                 vfp_discard(td);
 340                 critical_exit();
 341                 pcb->pcb_fpflags &= ~PCB_FP_STARTED;
 342                 pcb->pcb_fpusaved = ctx->prev;
 343         }
 344
 345         if (pcb->pcb_fpusaved == &pcb->pcb_fpustate) {
 346                 pcb->pcb_fpflags &= ~PCB_FP_KERN;
 347         } else {
 348                 KASSERT((pcb->pcb_fpflags & PCB_FP_KERN) != 0,
 349                     ("unpaired fpu_kern_leave"));
 350         }
 351
 352         return (0);
 353 }
 354
 355 int
 356 fpu_kern_thread(u_int flags)
 357 {
 358         struct pcb *pcb = curthread->td_pcb;
 359
 360         KASSERT((curthread->td_pflags & TDP_KTHREAD) != 0,
 361             ("Only kthread may use fpu_kern_thread"));
 362         KASSERT(pcb->pcb_fpusaved == &pcb->pcb_fpustate,
 363             ("Mangled pcb_fpusaved"));
 364         KASSERT((pcb->pcb_fpflags & PCB_FP_KERN) == 0,
 365             ("Thread already setup for the VFP"));
 366         pcb->pcb_fpflags |= PCB_FP_KERN;
 367         return (0);
 368 }
 369
 370 int
 371 is_fpu_kern_thread(u_int flags)
 372 {
 373         struct pcb *curpcb;
 374
 375         if ((curthread->td_pflags & TDP_KTHREAD) == 0)
 376                 return (0);
 377         curpcb = curthread->td_pcb;
 378         return ((curpcb->pcb_fpflags & PCB_FP_KERN) != 0);
 379 }
 380 #endif