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)
  86 {
  87         __int128_t *vfp_state;
  88         uint64_t fpcr, fpsr;
  89         uint32_t cpacr;
  90
  91         critical_enter();
  92         /*
  93          * Only store the registers if the VFP is enabled,
  94          * i.e. return if we are trapping on FP access.
  95          */
  96         cpacr = READ_SPECIALREG(cpacr_el1);
  97         if ((cpacr & CPACR_FPEN_MASK) == CPACR_FPEN_TRAP_NONE) {
  98                 vfp_state = td->td_pcb->pcb_vfp;
  99                 __asm __volatile(
 100                     "mrs        %0, fpcr                \n"
 101                     "mrs        %1, fpsr                \n"
 102                     "stp        q0,  q1,  [%2, #16 *  0]\n"
 103                     "stp        q2,  q3,  [%2, #16 *  2]\n"
 104                     "stp        q4,  q5,  [%2, #16 *  4]\n"
 105                     "stp        q6,  q7,  [%2, #16 *  6]\n"
 106                     "stp        q8,  q9,  [%2, #16 *  8]\n"
 107                     "stp        q10, q11, [%2, #16 * 10]\n"
 108                     "stp        q12, q13, [%2, #16 * 12]\n"
 109                     "stp        q14, q15, [%2, #16 * 14]\n"
 110                     "stp        q16, q17, [%2, #16 * 16]\n"
 111                     "stp        q18, q19, [%2, #16 * 18]\n"
 112                     "stp        q20, q21, [%2, #16 * 20]\n"
 113                     "stp        q22, q23, [%2, #16 * 22]\n"
 114                     "stp        q24, q25, [%2, #16 * 24]\n"
 115                     "stp        q26, q27, [%2, #16 * 26]\n"
 116                     "stp        q28, q29, [%2, #16 * 28]\n"
 117                     "stp        q30, q31, [%2, #16 * 30]\n"
 118                     : "=&r"(fpcr), "=&r"(fpsr) : "r"(vfp_state));
 119
 120                 td->td_pcb->pcb_fpcr = fpcr;
 121                 td->td_pcb->pcb_fpsr = fpsr;
 122
 123                 dsb();
 124                 vfp_disable();
 125         }
 126         critical_exit();
 127 }
 128
 129 void
 130 vfp_restore_state(void)
 131 {
 132         __int128_t *vfp_state;
 133         uint64_t fpcr, fpsr;
 134         struct pcb *curpcb;
 135         u_int cpu;
 136
 137         critical_enter();
 138
 139         cpu = PCPU_GET(cpuid);
 140         curpcb = curthread->td_pcb;
 141         curpcb->pcb_fpflags |= PCB_FP_STARTED;
 142
 143         vfp_enable();
 144
 145         if (PCPU_GET(fpcurthread) != curthread && cpu != curpcb->pcb_vfpcpu) {
 146
 147                 vfp_state = curthread->td_pcb->pcb_vfp;
 148                 fpcr = curthread->td_pcb->pcb_fpcr;
 149                 fpsr = curthread->td_pcb->pcb_fpsr;
 150
 151                 __asm __volatile(
 152                     "ldp        q0,  q1,  [%2, #16 *  0]\n"
 153                     "ldp        q2,  q3,  [%2, #16 *  2]\n"
 154                     "ldp        q4,  q5,  [%2, #16 *  4]\n"
 155                     "ldp        q6,  q7,  [%2, #16 *  6]\n"
 156                     "ldp        q8,  q9,  [%2, #16 *  8]\n"
 157                     "ldp        q10, q11, [%2, #16 * 10]\n"
 158                     "ldp        q12, q13, [%2, #16 * 12]\n"
 159                     "ldp        q14, q15, [%2, #16 * 14]\n"
 160                     "ldp        q16, q17, [%2, #16 * 16]\n"
 161                     "ldp        q18, q19, [%2, #16 * 18]\n"
 162                     "ldp        q20, q21, [%2, #16 * 20]\n"
 163                     "ldp        q22, q23, [%2, #16 * 22]\n"
 164                     "ldp        q24, q25, [%2, #16 * 24]\n"
 165                     "ldp        q26, q27, [%2, #16 * 26]\n"
 166                     "ldp        q28, q29, [%2, #16 * 28]\n"
 167                     "ldp        q30, q31, [%2, #16 * 30]\n"
 168                     "msr        fpcr, %0                \n"
 169                     "msr        fpsr, %1                \n"
 170                     : : "r"(fpcr), "r"(fpsr), "r"(vfp_state));
 171
 172                 PCPU_SET(fpcurthread, curthread);
 173                 curpcb->pcb_vfpcpu = cpu;
 174         }
 175
 176         critical_exit();
 177 }
 178
 179 void
 180 vfp_init(void)
 181 {
 182         uint64_t pfr;
 183
 184         /* Check if there is a vfp unit present */
 185         pfr = READ_SPECIALREG(id_aa64pfr0_el1);
 186         if ((pfr & ID_AA64PFR0_FP_MASK) == ID_AA64PFR0_FP_NONE)
 187                 return;
 188
 189         /* Disable to be enabled when it's used */
 190         vfp_disable();
 191 }
 192
 193 SYSINIT(vfp, SI_SUB_CPU, SI_ORDER_ANY, vfp_init, NULL);
 194
 195 #endif