2 * Copyright (c) 2015 The FreeBSD Foundation
5 * This software was developed by Andrew Turner under
6 * sponsorship from the FreeBSD Foundation.
8 * Redistribution and use in source and binary forms, with or without
9 * modification, are permitted provided that the following conditions
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.
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
30 #include <sys/cdefs.h>
31 __FBSDID("$FreeBSD$");
34 #include <sys/param.h>
35 #include <sys/systm.h>
36 #include <sys/kernel.h>
40 #include <machine/armreg.h>
41 #include <machine/pcb.h>
42 #include <machine/vfp.h>
44 /* Sanity check we can store all the VFP registers */
45 CTASSERT(sizeof(((struct pcb *)0)->pcb_vfp) == 16 * 32);
52 cpacr = READ_SPECIALREG(cpacr_el1);
53 cpacr = (cpacr & ~CPACR_FPEN_MASK) | CPACR_FPEN_TRAP_NONE;
54 WRITE_SPECIALREG(cpacr_el1, cpacr);
63 cpacr = READ_SPECIALREG(cpacr_el1);
64 cpacr = (cpacr & ~CPACR_FPEN_MASK) | CPACR_FPEN_TRAP_ALL1;
65 WRITE_SPECIALREG(cpacr_el1, cpacr);
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.
75 vfp_discard(struct thread *td)
78 if (PCPU_GET(fpcurthread) == td)
79 PCPU_SET(fpcurthread, NULL);
85 vfp_save_state(struct thread *td, struct pcb *pcb)
87 __int128_t *vfp_state;
91 KASSERT(pcb != NULL, ("NULL vfp pcb"));
92 KASSERT(td == NULL || td->td_pcb == pcb, ("Invalid vfp pcb"));
99 * Only store the registers if the VFP is enabled,
100 * i.e. return if we are trapping on FP access.
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"));
107 vfp_state = pcb->pcb_vfp;
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));
129 pcb->pcb_fpcr = fpcr;
130 pcb->pcb_fpsr = fpsr;
139 vfp_restore_state(void)
141 __int128_t *vfp_state;
148 cpu = PCPU_GET(cpuid);
149 curpcb = curthread->td_pcb;
150 curpcb->pcb_fpflags |= PCB_FP_STARTED;
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.
159 if (PCPU_GET(fpcurthread) != curthread || cpu != curpcb->pcb_vfpcpu) {
161 vfp_state = curthread->td_pcb->pcb_vfp;
162 fpcr = curthread->td_pcb->pcb_fpcr;
163 fpsr = curthread->td_pcb->pcb_fpsr;
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"
184 : : "r"(fpcr), "r"(fpsr), "r"(vfp_state));
186 PCPU_SET(fpcurthread, curthread);
187 curpcb->pcb_vfpcpu = cpu;
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)
203 /* Disable to be enabled when it's used */
207 SYSINIT(vfp, SI_SUB_CPU, SI_ORDER_ANY, vfp_init, NULL);