1 /*- 2 * Copyright (c) 2015-2016 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/malloc.h> 38 #include <sys/pcpu.h> 39 #include <sys/proc.h> 40 41 #include <machine/armreg.h> 42 #include <machine/pcb.h> 43 #include <machine/vfp.h> 44 45 /* Sanity check we can store all the VFP registers */ 46 CTASSERT(sizeof(((struct pcb *)0)->pcb_fpustate.vfp_regs) == 16 * 32); 47 48 static MALLOC_DEFINE(M_FPUKERN_CTX, "fpukern_ctx", 49 "Kernel contexts for VFP state"); 50 51 struct fpu_kern_ctx { 52 struct vfpstate *prev; 53 #define FPU_KERN_CTX_DUMMY 0x01 /* avoided save for the kern thread */ 54 #define FPU_KERN_CTX_INUSE 0x02 55 uint32_t flags; 56 struct vfpstate state; 57 }; 58 59 static void 60 vfp_enable(void) 61 { 62 uint32_t cpacr; 63 64 cpacr = READ_SPECIALREG(cpacr_el1); 65 cpacr = (cpacr & ~CPACR_FPEN_MASK) | CPACR_FPEN_TRAP_NONE; 66 WRITE_SPECIALREG(cpacr_el1, cpacr); 67 isb(); 68 } 69 70 static void 71 vfp_disable(void) 72 { 73 uint32_t cpacr; 74 75 cpacr = READ_SPECIALREG(cpacr_el1); 76 cpacr = (cpacr & ~CPACR_FPEN_MASK) | CPACR_FPEN_TRAP_ALL1; 77 WRITE_SPECIALREG(cpacr_el1, cpacr); 78 isb(); 79 } 80 81 /* 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. 86 */ 87 void 88 vfp_discard(struct thread *td) 89 { 90 91 #ifdef INVARIANTS 92 if (td != NULL) 93 CRITICAL_ASSERT(td); 94 #endif 95 if (PCPU_GET(fpcurthread) == td) 96 PCPU_SET(fpcurthread, NULL); 97 98 vfp_disable(); 99 } 100 101 static void 102 vfp_store(struct vfpstate *state) 103 { 104 __int128_t *vfp_state; 105 uint64_t fpcr, fpsr; 106 107 vfp_state = state->vfp_regs; 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 state->vfp_fpcr = fpcr; 130 state->vfp_fpsr = fpsr; 131 } 132 133 static void 134 vfp_restore(struct vfpstate *state) 135 { 136 __int128_t *vfp_state; 137 uint64_t fpcr, fpsr; 138 139 vfp_state = state->vfp_regs; 140 fpcr = state->vfp_fpcr; 141 fpsr = state->vfp_fpsr; 142 143 __asm __volatile( 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" 160 "msr fpcr, %0 \n" 161 "msr fpsr, %1 \n" 162 : : "r"(fpcr), "r"(fpsr), "r"(vfp_state)); 163 } 164 165 void 166 vfp_save_state(struct thread *td, struct pcb *pcb) 167 { 168 uint32_t cpacr; 169 170 KASSERT(pcb != NULL, ("NULL vfp pcb")); 171 KASSERT(td == NULL || td->td_pcb == pcb, ("Invalid vfp pcb")); 172 173 if (td == NULL) 174 td = curthread; 175 176 critical_enter(); 177 /* 178 * Only store the registers if the VFP is enabled, 179 * i.e. return if we are trapping on FP access. 180 */ 181 cpacr = READ_SPECIALREG(cpacr_el1); 182 if ((cpacr & CPACR_FPEN_MASK) == CPACR_FPEN_TRAP_NONE) { 183 KASSERT(PCPU_GET(fpcurthread) == td, 184 ("Storing an invalid VFP state")); 185 186 vfp_store(pcb->pcb_fpusaved); 187 dsb(ish); 188 vfp_disable(); 189 } 190 critical_exit(); 191 } 192 193 void 194 vfp_restore_state(void) 195 { 196 struct pcb *curpcb; 197 u_int cpu; 198 199 critical_enter(); 200 201 cpu = PCPU_GET(cpuid); 202 curpcb = curthread->td_pcb; 203 curpcb->pcb_fpflags |= PCB_FP_STARTED; 204 205 vfp_enable(); 206 207 /* 208 * If the previous thread on this cpu to use the VFP was not the 209 * current threas, or the current thread last used it on a different 210 * cpu we need to restore the old state. 211 */ 212 if (PCPU_GET(fpcurthread) != curthread || cpu != curpcb->pcb_vfpcpu) { 213 214 vfp_restore(curthread->td_pcb->pcb_fpusaved); 215 PCPU_SET(fpcurthread, curthread); 216 curpcb->pcb_vfpcpu = cpu; 217 } 218 219 critical_exit(); 220 } 221 222 void 223 vfp_init(void) 224 { 225 uint64_t pfr; 226 227 /* Check if there is a vfp unit present */ 228 pfr = READ_SPECIALREG(id_aa64pfr0_el1); 229 if ((pfr & ID_AA64PFR0_FP_MASK) == ID_AA64PFR0_FP_NONE) 230 return; 231 232 /* Disable to be enabled when it's used */ 233 vfp_disable(); 234 } 235 236 SYSINIT(vfp, SI_SUB_CPU, SI_ORDER_ANY, vfp_init, NULL); 237 238 struct fpu_kern_ctx * 239 fpu_kern_alloc_ctx(u_int flags) 240 { 241 struct fpu_kern_ctx *res; 242 size_t sz; 243 244 sz = sizeof(struct fpu_kern_ctx); 245 res = malloc(sz, M_FPUKERN_CTX, ((flags & FPU_KERN_NOWAIT) ? 246 M_NOWAIT : M_WAITOK) | M_ZERO); 247 return (res); 248 } 249 250 void 251 fpu_kern_free_ctx(struct fpu_kern_ctx *ctx) 252 { 253 254 KASSERT((ctx->flags & FPU_KERN_CTX_INUSE) == 0, ("free'ing inuse ctx")); 255 /* XXXAndrew clear the memory ? */ 256 free(ctx, M_FPUKERN_CTX); 257 } 258 259 int 260 fpu_kern_enter(struct thread *td, struct fpu_kern_ctx *ctx, u_int flags) 261 { 262 struct pcb *pcb; 263 264 pcb = td->td_pcb; 265 KASSERT((flags & FPU_KERN_NOCTX) != 0 || ctx != NULL, 266 ("ctx is required when !FPU_KERN_NOCTX")); 267 KASSERT(ctx == NULL || (ctx->flags & FPU_KERN_CTX_INUSE) == 0, 268 ("using inuse ctx")); 269 KASSERT((pcb->pcb_fpflags & PCB_FP_NOSAVE) == 0, 270 ("recursive fpu_kern_enter while in PCB_FP_NOSAVE state")); 271 272 if ((flags & FPU_KERN_NOCTX) != 0) { 273 critical_enter(); 274 if (curthread == PCPU_GET(fpcurthread)) { 275 vfp_save_state(curthread, pcb); 276 PCPU_SET(fpcurthread, NULL); 277 } else { 278 KASSERT(PCPU_GET(fpcurthread) == NULL, 279 ("invalid fpcurthread")); 280 } 281 282 vfp_enable(); 283 pcb->pcb_fpflags |= PCB_FP_KERN | PCB_FP_NOSAVE | 284 PCB_FP_STARTED; 285 return (0); 286 } 287 288 if ((flags & FPU_KERN_KTHR) != 0 && is_fpu_kern_thread(0)) { 289 ctx->flags = FPU_KERN_CTX_DUMMY | FPU_KERN_CTX_INUSE; 290 return (0); 291 } 292 /* 293 * Check either we are already using the VFP in the kernel, or 294 * the the saved state points to the default user space. 295 */ 296 KASSERT((pcb->pcb_fpflags & PCB_FP_KERN) != 0 || 297 pcb->pcb_fpusaved == &pcb->pcb_fpustate, 298 ("Mangled pcb_fpusaved %x %p %p", pcb->pcb_fpflags, pcb->pcb_fpusaved, &pcb->pcb_fpustate)); 299 ctx->flags = FPU_KERN_CTX_INUSE; 300 vfp_save_state(curthread, pcb); 301 ctx->prev = pcb->pcb_fpusaved; 302 pcb->pcb_fpusaved = &ctx->state; 303 pcb->pcb_fpflags |= PCB_FP_KERN; 304 pcb->pcb_fpflags &= ~PCB_FP_STARTED; 305 306 return (0); 307 } 308 309 int 310 fpu_kern_leave(struct thread *td, struct fpu_kern_ctx *ctx) 311 { 312 struct pcb *pcb; 313 314 pcb = td->td_pcb; 315 316 if ((pcb->pcb_fpflags & PCB_FP_NOSAVE) != 0) { 317 KASSERT(ctx == NULL, ("non-null ctx after FPU_KERN_NOCTX")); 318 KASSERT(PCPU_GET(fpcurthread) == NULL, 319 ("non-NULL fpcurthread for PCB_FP_NOSAVE")); 320 CRITICAL_ASSERT(td); 321 322 vfp_disable(); 323 pcb->pcb_fpflags &= ~(PCB_FP_NOSAVE | PCB_FP_STARTED); 324 critical_exit(); 325 } else { 326 KASSERT((ctx->flags & FPU_KERN_CTX_INUSE) != 0, 327 ("FPU context not inuse")); 328 ctx->flags &= ~FPU_KERN_CTX_INUSE; 329 330 if (is_fpu_kern_thread(0) && 331 (ctx->flags & FPU_KERN_CTX_DUMMY) != 0) 332 return (0); 333 KASSERT((ctx->flags & FPU_KERN_CTX_DUMMY) == 0, ("dummy ctx")); 334 critical_enter(); 335 vfp_discard(td); 336 critical_exit(); 337 pcb->pcb_fpflags &= ~PCB_FP_STARTED; 338 pcb->pcb_fpusaved = ctx->prev; 339 } 340 341 if (pcb->pcb_fpusaved == &pcb->pcb_fpustate) { 342 pcb->pcb_fpflags &= ~PCB_FP_KERN; 343 } else { 344 KASSERT((pcb->pcb_fpflags & PCB_FP_KERN) != 0, 345 ("unpaired fpu_kern_leave")); 346 } 347 348 return (0); 349 } 350 351 int 352 fpu_kern_thread(u_int flags) 353 { 354 struct pcb *pcb = curthread->td_pcb; 355 356 KASSERT((curthread->td_pflags & TDP_KTHREAD) != 0, 357 ("Only kthread may use fpu_kern_thread")); 358 KASSERT(pcb->pcb_fpusaved == &pcb->pcb_fpustate, 359 ("Mangled pcb_fpusaved")); 360 KASSERT((pcb->pcb_fpflags & PCB_FP_KERN) == 0, 361 ("Thread already setup for the VFP")); 362 pcb->pcb_fpflags |= PCB_FP_KERN; 363 return (0); 364 } 365 366 int 367 is_fpu_kern_thread(u_int flags) 368 { 369 struct pcb *curpcb; 370 371 if ((curthread->td_pflags & TDP_KTHREAD) == 0) 372 return (0); 373 curpcb = curthread->td_pcb; 374 return ((curpcb->pcb_fpflags & PCB_FP_KERN) != 0); 375 } 376 #endif 377