1 /*-
2 * Copyright (c) 2015-2016 The FreeBSD Foundation
3 *
4 * This software was developed by Andrew Turner under
5 * sponsorship from the FreeBSD Foundation.
6 *
7 * Redistribution and use in source and binary forms, with or without
8 * modification, are permitted provided that the following conditions
9 * are met:
10 * 1. Redistributions of source code must retain the above copyright
11 * notice, this list of conditions and the following disclaimer.
12 * 2. Redistributions in binary form must reproduce the above copyright
13 * notice, this list of conditions and the following disclaimer in the
14 * documentation and/or other materials provided with the distribution.
15 *
16 * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
17 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
18 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
19 * ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
20 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
21 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
22 * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
23 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
24 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
25 * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
26 * SUCH DAMAGE.
27 */
28
29 #include <sys/cdefs.h>
30 __FBSDID("$FreeBSD$");
31
32 #ifdef VFP
33 #include <sys/param.h>
34 #include <sys/systm.h>
35 #include <sys/limits.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/md_var.h>
43 #include <machine/pcb.h>
44 #include <machine/vfp.h>
45
46 /* Sanity check we can store all the VFP registers */
47 CTASSERT(sizeof(((struct pcb *)0)->pcb_fpustate.vfp_regs) == 16 * 32);
48
49 static MALLOC_DEFINE(M_FPUKERN_CTX, "fpukern_ctx",
50 "Kernel contexts for VFP state");
51
52 struct fpu_kern_ctx {
53 struct vfpstate *prev;
54 #define FPU_KERN_CTX_DUMMY 0x01 /* avoided save for the kern thread */
55 #define FPU_KERN_CTX_INUSE 0x02
56 uint32_t flags;
57 struct vfpstate state;
58 };
59
60 static void
vfp_enable(void)61 vfp_enable(void)
62 {
63 uint32_t cpacr;
64
65 cpacr = READ_SPECIALREG(cpacr_el1);
66 cpacr = (cpacr & ~CPACR_FPEN_MASK) | CPACR_FPEN_TRAP_NONE;
67 WRITE_SPECIALREG(cpacr_el1, cpacr);
68 isb();
69 }
70
71 static void
vfp_disable(void)72 vfp_disable(void)
73 {
74 uint32_t cpacr;
75
76 cpacr = READ_SPECIALREG(cpacr_el1);
77 cpacr = (cpacr & ~CPACR_FPEN_MASK) | CPACR_FPEN_TRAP_ALL1;
78 WRITE_SPECIALREG(cpacr_el1, cpacr);
79 isb();
80 }
81
82 /*
83 * Called when the thread is dying or when discarding the kernel VFP state.
84 * If the thread was the last to use the VFP unit mark it as unused to tell
85 * the kernel the fp state is unowned. Ensure the VFP unit is off so we get
86 * an exception on the next access.
87 */
88 void
vfp_discard(struct thread * td)89 vfp_discard(struct thread *td)
90 {
91
92 #ifdef INVARIANTS
93 if (td != NULL)
94 CRITICAL_ASSERT(td);
95 #endif
96 if (PCPU_GET(fpcurthread) == td)
97 PCPU_SET(fpcurthread, NULL);
98
99 vfp_disable();
100 }
101
102 static void
vfp_store(struct vfpstate * state)103 vfp_store(struct vfpstate *state)
104 {
105 __uint128_t *vfp_state;
106 uint64_t fpcr, fpsr;
107
108 vfp_state = state->vfp_regs;
109 __asm __volatile(
110 "mrs %0, fpcr \n"
111 "mrs %1, fpsr \n"
112 "stp q0, q1, [%2, #16 * 0]\n"
113 "stp q2, q3, [%2, #16 * 2]\n"
114 "stp q4, q5, [%2, #16 * 4]\n"
115 "stp q6, q7, [%2, #16 * 6]\n"
116 "stp q8, q9, [%2, #16 * 8]\n"
117 "stp q10, q11, [%2, #16 * 10]\n"
118 "stp q12, q13, [%2, #16 * 12]\n"
119 "stp q14, q15, [%2, #16 * 14]\n"
120 "stp q16, q17, [%2, #16 * 16]\n"
121 "stp q18, q19, [%2, #16 * 18]\n"
122 "stp q20, q21, [%2, #16 * 20]\n"
123 "stp q22, q23, [%2, #16 * 22]\n"
124 "stp q24, q25, [%2, #16 * 24]\n"
125 "stp q26, q27, [%2, #16 * 26]\n"
126 "stp q28, q29, [%2, #16 * 28]\n"
127 "stp q30, q31, [%2, #16 * 30]\n"
128 : "=&r"(fpcr), "=&r"(fpsr) : "r"(vfp_state));
129
130 state->vfp_fpcr = fpcr;
131 state->vfp_fpsr = fpsr;
132 }
133
134 static void
vfp_restore(struct vfpstate * state)135 vfp_restore(struct vfpstate *state)
136 {
137 __uint128_t *vfp_state;
138 uint64_t fpcr, fpsr;
139
140 vfp_state = state->vfp_regs;
141 fpcr = state->vfp_fpcr;
142 fpsr = state->vfp_fpsr;
143
144 __asm __volatile(
145 "ldp q0, q1, [%2, #16 * 0]\n"
146 "ldp q2, q3, [%2, #16 * 2]\n"
147 "ldp q4, q5, [%2, #16 * 4]\n"
148 "ldp q6, q7, [%2, #16 * 6]\n"
149 "ldp q8, q9, [%2, #16 * 8]\n"
150 "ldp q10, q11, [%2, #16 * 10]\n"
151 "ldp q12, q13, [%2, #16 * 12]\n"
152 "ldp q14, q15, [%2, #16 * 14]\n"
153 "ldp q16, q17, [%2, #16 * 16]\n"
154 "ldp q18, q19, [%2, #16 * 18]\n"
155 "ldp q20, q21, [%2, #16 * 20]\n"
156 "ldp q22, q23, [%2, #16 * 22]\n"
157 "ldp q24, q25, [%2, #16 * 24]\n"
158 "ldp q26, q27, [%2, #16 * 26]\n"
159 "ldp q28, q29, [%2, #16 * 28]\n"
160 "ldp q30, q31, [%2, #16 * 30]\n"
161 "msr fpcr, %0 \n"
162 "msr fpsr, %1 \n"
163 : : "r"(fpcr), "r"(fpsr), "r"(vfp_state));
164 }
165
166 void
vfp_save_state(struct thread * td,struct pcb * pcb)167 vfp_save_state(struct thread *td, struct pcb *pcb)
168 {
169 uint32_t cpacr;
170
171 KASSERT(pcb != NULL, ("NULL vfp pcb"));
172 KASSERT(td == NULL || td->td_pcb == pcb, ("Invalid vfp pcb"));
173
174 /*
175 * savectx() will be called on panic with dumppcb as an argument,
176 * dumppcb doesn't have pcb_fpusaved set, so set it to save
177 * the VFP registers.
178 */
179 if (pcb->pcb_fpusaved == NULL)
180 pcb->pcb_fpusaved = &pcb->pcb_fpustate;
181
182 if (td == NULL)
183 td = curthread;
184
185 critical_enter();
186 /*
187 * Only store the registers if the VFP is enabled,
188 * i.e. return if we are trapping on FP access.
189 */
190 cpacr = READ_SPECIALREG(cpacr_el1);
191 if ((cpacr & CPACR_FPEN_MASK) == CPACR_FPEN_TRAP_NONE) {
192 KASSERT(PCPU_GET(fpcurthread) == td,
193 ("Storing an invalid VFP state"));
194
195 vfp_store(pcb->pcb_fpusaved);
196 dsb(ish);
197 vfp_disable();
198 }
199 critical_exit();
200 }
201
202 /*
203 * Reset the FP state to avoid leaking state from the parent process across
204 * execve() (and to ensure that we get a consistent floating point environment
205 * in every new process).
206 */
207 void
vfp_reset_state(struct thread * td,struct pcb * pcb)208 vfp_reset_state(struct thread *td, struct pcb *pcb)
209 {
210 critical_enter();
211 bzero(&pcb->pcb_fpustate.vfp_regs, sizeof(pcb->pcb_fpustate.vfp_regs));
212 KASSERT(pcb->pcb_fpusaved == &pcb->pcb_fpustate,
213 ("pcb_fpusaved should point to pcb_fpustate."));
214 pcb->pcb_fpustate.vfp_fpcr = initial_fpcr;
215 pcb->pcb_fpustate.vfp_fpsr = 0;
216 pcb->pcb_vfpcpu = UINT_MAX;
217 pcb->pcb_fpflags = 0;
218 vfp_discard(td);
219 critical_exit();
220 }
221
222 void
vfp_restore_state(void)223 vfp_restore_state(void)
224 {
225 struct pcb *curpcb;
226 u_int cpu;
227
228 critical_enter();
229
230 cpu = PCPU_GET(cpuid);
231 curpcb = curthread->td_pcb;
232 curpcb->pcb_fpflags |= PCB_FP_STARTED;
233
234 vfp_enable();
235
236 /*
237 * If the previous thread on this cpu to use the VFP was not the
238 * current thread, or the current thread last used it on a different
239 * cpu we need to restore the old state.
240 */
241 if (PCPU_GET(fpcurthread) != curthread || cpu != curpcb->pcb_vfpcpu) {
242 vfp_restore(curthread->td_pcb->pcb_fpusaved);
243 PCPU_SET(fpcurthread, curthread);
244 curpcb->pcb_vfpcpu = cpu;
245 }
246
247 critical_exit();
248 }
249
250 void
vfp_init(void)251 vfp_init(void)
252 {
253 uint64_t pfr;
254
255 /* Check if there is a vfp unit present */
256 pfr = READ_SPECIALREG(id_aa64pfr0_el1);
257 if ((pfr & ID_AA64PFR0_FP_MASK) == ID_AA64PFR0_FP_NONE)
258 return;
259
260 /* Disable to be enabled when it's used */
261 vfp_disable();
262
263 if (PCPU_GET(cpuid) == 0)
264 thread0.td_pcb->pcb_fpusaved->vfp_fpcr = initial_fpcr;
265 }
266
267 SYSINIT(vfp, SI_SUB_CPU, SI_ORDER_ANY, vfp_init, NULL);
268
269 struct fpu_kern_ctx *
fpu_kern_alloc_ctx(u_int flags)270 fpu_kern_alloc_ctx(u_int flags)
271 {
272 struct fpu_kern_ctx *res;
273 size_t sz;
274
275 sz = sizeof(struct fpu_kern_ctx);
276 res = malloc(sz, M_FPUKERN_CTX, ((flags & FPU_KERN_NOWAIT) ?
277 M_NOWAIT : M_WAITOK) | M_ZERO);
278 return (res);
279 }
280
281 void
fpu_kern_free_ctx(struct fpu_kern_ctx * ctx)282 fpu_kern_free_ctx(struct fpu_kern_ctx *ctx)
283 {
284
285 KASSERT((ctx->flags & FPU_KERN_CTX_INUSE) == 0, ("free'ing inuse ctx"));
286 /* XXXAndrew clear the memory ? */
287 free(ctx, M_FPUKERN_CTX);
288 }
289
290 void
fpu_kern_enter(struct thread * td,struct fpu_kern_ctx * ctx,u_int flags)291 fpu_kern_enter(struct thread *td, struct fpu_kern_ctx *ctx, u_int flags)
292 {
293 struct pcb *pcb;
294
295 pcb = td->td_pcb;
296 KASSERT((flags & FPU_KERN_NOCTX) != 0 || ctx != NULL,
297 ("ctx is required when !FPU_KERN_NOCTX"));
298 KASSERT(ctx == NULL || (ctx->flags & FPU_KERN_CTX_INUSE) == 0,
299 ("using inuse ctx"));
300 KASSERT((pcb->pcb_fpflags & PCB_FP_NOSAVE) == 0,
301 ("recursive fpu_kern_enter while in PCB_FP_NOSAVE state"));
302
303 if ((flags & FPU_KERN_NOCTX) != 0) {
304 critical_enter();
305 if (curthread == PCPU_GET(fpcurthread)) {
306 vfp_save_state(curthread, pcb);
307 }
308 PCPU_SET(fpcurthread, NULL);
309
310 vfp_enable();
311 pcb->pcb_fpflags |= PCB_FP_KERN | PCB_FP_NOSAVE |
312 PCB_FP_STARTED;
313 return;
314 }
315
316 if ((flags & FPU_KERN_KTHR) != 0 && is_fpu_kern_thread(0)) {
317 ctx->flags = FPU_KERN_CTX_DUMMY | FPU_KERN_CTX_INUSE;
318 return;
319 }
320 /*
321 * Check either we are already using the VFP in the kernel, or
322 * the the saved state points to the default user space.
323 */
324 KASSERT((pcb->pcb_fpflags & PCB_FP_KERN) != 0 ||
325 pcb->pcb_fpusaved == &pcb->pcb_fpustate,
326 ("Mangled pcb_fpusaved %x %p %p", pcb->pcb_fpflags, pcb->pcb_fpusaved, &pcb->pcb_fpustate));
327 ctx->flags = FPU_KERN_CTX_INUSE;
328 vfp_save_state(curthread, pcb);
329 ctx->prev = pcb->pcb_fpusaved;
330 pcb->pcb_fpusaved = &ctx->state;
331 pcb->pcb_fpflags |= PCB_FP_KERN;
332 pcb->pcb_fpflags &= ~PCB_FP_STARTED;
333
334 return;
335 }
336
337 int
fpu_kern_leave(struct thread * td,struct fpu_kern_ctx * ctx)338 fpu_kern_leave(struct thread *td, struct fpu_kern_ctx *ctx)
339 {
340 struct pcb *pcb;
341
342 pcb = td->td_pcb;
343
344 if ((pcb->pcb_fpflags & PCB_FP_NOSAVE) != 0) {
345 KASSERT(ctx == NULL, ("non-null ctx after FPU_KERN_NOCTX"));
346 KASSERT(PCPU_GET(fpcurthread) == NULL,
347 ("non-NULL fpcurthread for PCB_FP_NOSAVE"));
348 CRITICAL_ASSERT(td);
349
350 vfp_disable();
351 pcb->pcb_fpflags &= ~(PCB_FP_NOSAVE | PCB_FP_STARTED);
352 critical_exit();
353 } else {
354 KASSERT((ctx->flags & FPU_KERN_CTX_INUSE) != 0,
355 ("FPU context not inuse"));
356 ctx->flags &= ~FPU_KERN_CTX_INUSE;
357
358 if (is_fpu_kern_thread(0) &&
359 (ctx->flags & FPU_KERN_CTX_DUMMY) != 0)
360 return (0);
361 KASSERT((ctx->flags & FPU_KERN_CTX_DUMMY) == 0, ("dummy ctx"));
362 critical_enter();
363 vfp_discard(td);
364 critical_exit();
365 pcb->pcb_fpflags &= ~PCB_FP_STARTED;
366 pcb->pcb_fpusaved = ctx->prev;
367 }
368
369 if (pcb->pcb_fpusaved == &pcb->pcb_fpustate) {
370 pcb->pcb_fpflags &= ~PCB_FP_KERN;
371 } else {
372 KASSERT((pcb->pcb_fpflags & PCB_FP_KERN) != 0,
373 ("unpaired fpu_kern_leave"));
374 }
375
376 return (0);
377 }
378
379 int
fpu_kern_thread(u_int flags __unused)380 fpu_kern_thread(u_int flags __unused)
381 {
382 struct pcb *pcb = curthread->td_pcb;
383
384 KASSERT((curthread->td_pflags & TDP_KTHREAD) != 0,
385 ("Only kthread may use fpu_kern_thread"));
386 KASSERT(pcb->pcb_fpusaved == &pcb->pcb_fpustate,
387 ("Mangled pcb_fpusaved"));
388 KASSERT((pcb->pcb_fpflags & PCB_FP_KERN) == 0,
389 ("Thread already setup for the VFP"));
390 pcb->pcb_fpflags |= PCB_FP_KERN;
391 return (0);
392 }
393
394 int
is_fpu_kern_thread(u_int flags __unused)395 is_fpu_kern_thread(u_int flags __unused)
396 {
397 struct pcb *curpcb;
398
399 if ((curthread->td_pflags & TDP_KTHREAD) == 0)
400 return (0);
401 curpcb = curthread->td_pcb;
402 return ((curpcb->pcb_fpflags & PCB_FP_KERN) != 0);
403 }
404 #endif
405