2 * Copyright (c) 2015-2016 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>
37 #include <sys/malloc.h>
41 #include <machine/armreg.h>
42 #include <machine/pcb.h>
43 #include <machine/vfp.h>
45 /* Sanity check we can store all the VFP registers */
46 CTASSERT(sizeof(((struct pcb *)0)->pcb_fpustate.vfp_regs) == 16 * 32);
48 static MALLOC_DEFINE(M_FPUKERN_CTX, "fpukern_ctx",
49 "Kernel contexts for VFP state");
52 struct vfpstate *prev;
53 #define FPU_KERN_CTX_DUMMY 0x01 /* avoided save for the kern thread */
54 #define FPU_KERN_CTX_INUSE 0x02
56 struct vfpstate state;
64 cpacr = READ_SPECIALREG(cpacr_el1);
65 cpacr = (cpacr & ~CPACR_FPEN_MASK) | CPACR_FPEN_TRAP_NONE;
66 WRITE_SPECIALREG(cpacr_el1, cpacr);
75 cpacr = READ_SPECIALREG(cpacr_el1);
76 cpacr = (cpacr & ~CPACR_FPEN_MASK) | CPACR_FPEN_TRAP_ALL1;
77 WRITE_SPECIALREG(cpacr_el1, cpacr);
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.
88 vfp_discard(struct thread *td)
95 if (PCPU_GET(fpcurthread) == td)
96 PCPU_SET(fpcurthread, NULL);
102 vfp_store(struct vfpstate *state)
104 __int128_t *vfp_state;
107 vfp_state = state->vfp_regs;
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 state->vfp_fpcr = fpcr;
130 state->vfp_fpsr = fpsr;
134 vfp_restore(struct vfpstate *state)
136 __int128_t *vfp_state;
139 vfp_state = state->vfp_regs;
140 fpcr = state->vfp_fpcr;
141 fpsr = state->vfp_fpsr;
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"
162 : : "r"(fpcr), "r"(fpsr), "r"(vfp_state));
166 vfp_save_state(struct thread *td, struct pcb *pcb)
170 KASSERT(pcb != NULL, ("NULL vfp pcb"));
171 KASSERT(td == NULL || td->td_pcb == pcb, ("Invalid vfp pcb"));
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
178 if (pcb->pcb_fpusaved == NULL)
179 pcb->pcb_fpusaved = &pcb->pcb_fpustate;
186 * Only store the registers if the VFP is enabled,
187 * i.e. return if we are trapping on FP access.
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"));
194 vfp_store(pcb->pcb_fpusaved);
202 vfp_restore_state(void)
209 cpu = PCPU_GET(cpuid);
210 curpcb = curthread->td_pcb;
211 curpcb->pcb_fpflags |= PCB_FP_STARTED;
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.
220 if (PCPU_GET(fpcurthread) != curthread || cpu != curpcb->pcb_vfpcpu) {
222 vfp_restore(curthread->td_pcb->pcb_fpusaved);
223 PCPU_SET(fpcurthread, curthread);
224 curpcb->pcb_vfpcpu = cpu;
235 /* Check if there is a vfp unit present */
236 pfr = READ_SPECIALREG(id_aa64pfr0_el1);
237 if ((pfr & ID_AA64PFR0_FP_MASK) == ID_AA64PFR0_FP_NONE)
240 /* Disable to be enabled when it's used */
244 SYSINIT(vfp, SI_SUB_CPU, SI_ORDER_ANY, vfp_init, NULL);
246 struct fpu_kern_ctx *
247 fpu_kern_alloc_ctx(u_int flags)
249 struct fpu_kern_ctx *res;
252 sz = sizeof(struct fpu_kern_ctx);
253 res = malloc(sz, M_FPUKERN_CTX, ((flags & FPU_KERN_NOWAIT) ?
254 M_NOWAIT : M_WAITOK) | M_ZERO);
259 fpu_kern_free_ctx(struct fpu_kern_ctx *ctx)
262 KASSERT((ctx->flags & FPU_KERN_CTX_INUSE) == 0, ("free'ing inuse ctx"));
263 /* XXXAndrew clear the memory ? */
264 free(ctx, M_FPUKERN_CTX);
268 fpu_kern_enter(struct thread *td, struct fpu_kern_ctx *ctx, u_int flags)
273 KASSERT((flags & FPU_KERN_NOCTX) != 0 || ctx != NULL,
274 ("ctx is required when !FPU_KERN_NOCTX"));
275 KASSERT(ctx == NULL || (ctx->flags & FPU_KERN_CTX_INUSE) == 0,
276 ("using inuse ctx"));
277 KASSERT((pcb->pcb_fpflags & PCB_FP_NOSAVE) == 0,
278 ("recursive fpu_kern_enter while in PCB_FP_NOSAVE state"));
280 if ((flags & FPU_KERN_NOCTX) != 0) {
282 if (curthread == PCPU_GET(fpcurthread)) {
283 vfp_save_state(curthread, pcb);
285 PCPU_SET(fpcurthread, NULL);
288 pcb->pcb_fpflags |= PCB_FP_KERN | PCB_FP_NOSAVE |
293 if ((flags & FPU_KERN_KTHR) != 0 && is_fpu_kern_thread(0)) {
294 ctx->flags = FPU_KERN_CTX_DUMMY | FPU_KERN_CTX_INUSE;
298 * Check either we are already using the VFP in the kernel, or
299 * the the saved state points to the default user space.
301 KASSERT((pcb->pcb_fpflags & PCB_FP_KERN) != 0 ||
302 pcb->pcb_fpusaved == &pcb->pcb_fpustate,
303 ("Mangled pcb_fpusaved %x %p %p", pcb->pcb_fpflags, pcb->pcb_fpusaved, &pcb->pcb_fpustate));
304 ctx->flags = FPU_KERN_CTX_INUSE;
305 vfp_save_state(curthread, pcb);
306 ctx->prev = pcb->pcb_fpusaved;
307 pcb->pcb_fpusaved = &ctx->state;
308 pcb->pcb_fpflags |= PCB_FP_KERN;
309 pcb->pcb_fpflags &= ~PCB_FP_STARTED;
315 fpu_kern_leave(struct thread *td, struct fpu_kern_ctx *ctx)
321 if ((pcb->pcb_fpflags & PCB_FP_NOSAVE) != 0) {
322 KASSERT(ctx == NULL, ("non-null ctx after FPU_KERN_NOCTX"));
323 KASSERT(PCPU_GET(fpcurthread) == NULL,
324 ("non-NULL fpcurthread for PCB_FP_NOSAVE"));
328 pcb->pcb_fpflags &= ~(PCB_FP_NOSAVE | PCB_FP_STARTED);
331 KASSERT((ctx->flags & FPU_KERN_CTX_INUSE) != 0,
332 ("FPU context not inuse"));
333 ctx->flags &= ~FPU_KERN_CTX_INUSE;
335 if (is_fpu_kern_thread(0) &&
336 (ctx->flags & FPU_KERN_CTX_DUMMY) != 0)
338 KASSERT((ctx->flags & FPU_KERN_CTX_DUMMY) == 0, ("dummy ctx"));
342 pcb->pcb_fpflags &= ~PCB_FP_STARTED;
343 pcb->pcb_fpusaved = ctx->prev;
346 if (pcb->pcb_fpusaved == &pcb->pcb_fpustate) {
347 pcb->pcb_fpflags &= ~PCB_FP_KERN;
349 KASSERT((pcb->pcb_fpflags & PCB_FP_KERN) != 0,
350 ("unpaired fpu_kern_leave"));
357 fpu_kern_thread(u_int flags)
359 struct pcb *pcb = curthread->td_pcb;
361 KASSERT((curthread->td_pflags & TDP_KTHREAD) != 0,
362 ("Only kthread may use fpu_kern_thread"));
363 KASSERT(pcb->pcb_fpusaved == &pcb->pcb_fpustate,
364 ("Mangled pcb_fpusaved"));
365 KASSERT((pcb->pcb_fpflags & PCB_FP_KERN) == 0,
366 ("Thread already setup for the VFP"));
367 pcb->pcb_fpflags |= PCB_FP_KERN;
372 is_fpu_kern_thread(u_int flags)
376 if ((curthread->td_pflags & TDP_KTHREAD) == 0)
378 curpcb = curthread->td_pcb;
379 return ((curpcb->pcb_fpflags & PCB_FP_KERN) != 0);