sys/arm64/arm64/vfp.c

   1 /*-
   2  * Copyright (c) 2015 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/pcpu.h>
  38 #include <sys/proc.h>
  39
  40 #include <machine/armreg.h>
  41 #include <machine/pcb.h>
  42 #include <machine/vfp.h>
  43
  44 /* Sanity check we can store all the VFP registers */
  45 CTASSERT(sizeof(((struct pcb *)0)->pcb_vfp) == 16 * 32);
  46
  47 static void
  48 vfp_enable(void)
  49 {
  50         uint32_t cpacr;
  51
  52         cpacr = READ_SPECIALREG(cpacr_el1);
  53         cpacr = (cpacr & ~CPACR_FPEN_MASK) | CPACR_FPEN_TRAP_NONE;
  54         WRITE_SPECIALREG(cpacr_el1, cpacr);
  55         isb();
  56 }
  57
  58 static void
  59 vfp_disable(void)
  60 {
  61         uint32_t cpacr;
  62
  63         cpacr = READ_SPECIALREG(cpacr_el1);
  64         cpacr = (cpacr & ~CPACR_FPEN_MASK) | CPACR_FPEN_TRAP_ALL1;
  65         WRITE_SPECIALREG(cpacr_el1, cpacr);
  66         isb();
  67 }
  68
  69 /*
  70  * Called when the thread is dying. If the thread was the last to use the
  71  * VFP unit mark it as unused to tell the kernel the fp state is unowned.
  72  * Ensure the VFP unit is off so we get an exception on the next access.
  73  */
  74 void
  75 vfp_discard(struct thread *td)
  76 {
  77
  78         if (PCPU_GET(fpcurthread) == td)
  79                 PCPU_SET(fpcurthread, NULL);
  80
  81         vfp_disable();
  82 }
  83
  84 void
  85 vfp_save_state(struct thread *td, struct pcb *pcb)
  86 {
  87         __int128_t *vfp_state;
  88         uint64_t fpcr, fpsr;
  89         uint32_t cpacr;
  90
  91         KASSERT(pcb != NULL, ("NULL vfp pcb"));
  92         KASSERT(td == NULL || td->td_pcb == pcb, ("Invalid vfp pcb"));
  93
  94         if (td == NULL)
  95                 td = curthread;
  96
  97         critical_enter();
  98         /*
  99          * Only store the registers if the VFP is enabled,
 100          * i.e. return if we are trapping on FP access.
 101          */
 102         cpacr = READ_SPECIALREG(cpacr_el1);
 103         if ((cpacr & CPACR_FPEN_MASK) == CPACR_FPEN_TRAP_NONE) {
 104                 KASSERT(PCPU_GET(fpcurthread) == td,
 105                     ("Storing an invalid VFP state"));
 106
 107                 vfp_state = pcb->pcb_vfp;
 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                 pcb->pcb_fpcr = fpcr;
 130                 pcb->pcb_fpsr = fpsr;
 131
 132                 dsb(ish);
 133                 vfp_disable();
 134         }
 135         critical_exit();
 136 }
 137
 138 void
 139 vfp_restore_state(void)
 140 {
 141         __int128_t *vfp_state;
 142         uint64_t fpcr, fpsr;
 143         struct pcb *curpcb;
 144         u_int cpu;
 145
 146         critical_enter();
 147
 148         cpu = PCPU_GET(cpuid);
 149         curpcb = curthread->td_pcb;
 150         curpcb->pcb_fpflags |= PCB_FP_STARTED;
 151
 152         vfp_enable();
 153
 154         /*
 155          * If the previous thread on this cpu to use the VFP was not the
 156          * current threas, or the current thread last used it on a different
 157          * cpu we need to restore the old state.
 158          */
 159         if (PCPU_GET(fpcurthread) != curthread || cpu != curpcb->pcb_vfpcpu) {
 160
 161                 vfp_state = curthread->td_pcb->pcb_vfp;
 162                 fpcr = curthread->td_pcb->pcb_fpcr;
 163                 fpsr = curthread->td_pcb->pcb_fpsr;
 164
 165                 __asm __volatile(
 166                     "ldp        q0,  q1,  [%2, #16 *  0]\n"
 167                     "ldp        q2,  q3,  [%2, #16 *  2]\n"
 168                     "ldp        q4,  q5,  [%2, #16 *  4]\n"
 169                     "ldp        q6,  q7,  [%2, #16 *  6]\n"
 170                     "ldp        q8,  q9,  [%2, #16 *  8]\n"
 171                     "ldp        q10, q11, [%2, #16 * 10]\n"
 172                     "ldp        q12, q13, [%2, #16 * 12]\n"
 173                     "ldp        q14, q15, [%2, #16 * 14]\n"
 174                     "ldp        q16, q17, [%2, #16 * 16]\n"
 175                     "ldp        q18, q19, [%2, #16 * 18]\n"
 176                     "ldp        q20, q21, [%2, #16 * 20]\n"
 177                     "ldp        q22, q23, [%2, #16 * 22]\n"
 178                     "ldp        q24, q25, [%2, #16 * 24]\n"
 179                     "ldp        q26, q27, [%2, #16 * 26]\n"
 180                     "ldp        q28, q29, [%2, #16 * 28]\n"
 181                     "ldp        q30, q31, [%2, #16 * 30]\n"
 182                     "msr        fpcr, %0                \n"
 183                     "msr        fpsr, %1                \n"
 184                     : : "r"(fpcr), "r"(fpsr), "r"(vfp_state));
 185
 186                 PCPU_SET(fpcurthread, curthread);
 187                 curpcb->pcb_vfpcpu = cpu;
 188         }
 189
 190         critical_exit();
 191 }
 192
 193 void
 194 vfp_init(void)
 195 {
 196         uint64_t pfr;
 197
 198         /* Check if there is a vfp unit present */
 199         pfr = READ_SPECIALREG(id_aa64pfr0_el1);
 200         if ((pfr & ID_AA64PFR0_FP_MASK) == ID_AA64PFR0_FP_NONE)
 201                 return;
 202
 203         /* Disable to be enabled when it's used */
 204         vfp_disable();
 205 }
 206
 207 SYSINIT(vfp, SI_SUB_CPU, SI_ORDER_ANY, vfp_init, NULL);
 208
 209 #endif