1 /* $NetBSD: vfp_init.c,v 1.5 2012/08/16 18:16:25 matt Exp $ */ 2 3 /* 4 * Copyright (c) 2008 ARM Ltd 5 * All rights reserved. 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 * 3. The name of the company may not be used to endorse or promote 16 * products derived from this software without specific prior written 17 * permission. 18 * 19 * THIS SOFTWARE IS PROVIDED BY ARM LTD ``AS IS'' AND ANY EXPRESS OR 20 * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 21 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 22 * ARE DISCLAIMED. IN NO EVENT SHALL ARM LTD BE LIABLE FOR ANY 23 * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 24 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE 25 * GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 26 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 27 * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 28 * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 29 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 30 */ 31 32 #include <sys/param.h> 33 #include <sys/types.h> 34 #include <sys/systm.h> 35 #include <sys/device.h> 36 #include <sys/proc.h> 37 #include <sys/cpu.h> 38 39 #include <arm/pcb.h> 40 #include <arm/undefined.h> 41 #include <arm/vfpreg.h> 42 43 /* 44 * Use generic co-processor instructions to avoid assembly problems. 45 */ 46 47 /* FMRX <X>, fpsid */ 48 static inline uint32_t 49 read_fpsid(void) 50 { 51 uint32_t rv; 52 __asm __volatile("mrc p10, 7, %0, c0, c0, 0" : "=r" (rv)); 53 return rv; 54 } 55 56 /* FMRX <X>, fpexc */ 57 static inline uint32_t 58 read_fpscr(void) 59 { 60 uint32_t rv; 61 __asm __volatile("mrc p10, 7, %0, c1, c0, 0" : "=r" (rv)); 62 return rv; 63 } 64 65 /* FMRX <X>, fpexc */ 66 static inline uint32_t 67 read_fpexc(void) 68 { 69 uint32_t rv; 70 __asm __volatile("mrc p10, 7, %0, c8, c0, 0" : "=r" (rv)); 71 return rv; 72 } 73 74 /* FMRX <X>, fpinst */ 75 static inline uint32_t 76 read_fpinst(void) 77 { 78 uint32_t rv; 79 __asm __volatile("mrc p10, 7, %0, c9, c0, 0" : "=r" (rv)); 80 return rv; 81 } 82 83 /* FMRX <X>, fpinst2 */ 84 static inline uint32_t 85 read_fpinst2(void) 86 { 87 uint32_t rv; 88 __asm __volatile("mrc p10, 7, %0, c10, c0, 0" : "=r" (rv)); 89 return rv; 90 } 91 92 /* FSTMD <X>, {d0-d15} */ 93 #define save_vfpregs(X) __asm __volatile("stc p11, c0, [%0], {32}" : \ 94 : "r" (X) : "memory") 95 96 /* FMXR <X>, fpscr */ 97 #define write_fpscr(X) __asm __volatile("mcr p10, 7, %0, c1, c0, 0" : \ 98 : "r" (X)) 99 /* FMXR <X>, fpexc */ 100 #define write_fpexc(X) __asm __volatile("mcr p10, 7, %0, c8, c0, 0" : \ 101 : "r" (X)) 102 /* FMXR <X>, fpinst */ 103 #define write_fpinst(X) __asm __volatile("mcr p10, 7, %0, c9, c0, 0" : \ 104 : "r" (X)) 105 /* FMXR <X>, fpinst2 */ 106 #define write_fpinst2(X) __asm __volatile("mcr p10, 7, %0, c10, c0, 0" : \ 107 : "r" (X)) 108 /* FLDMD <X>, {d0-d15} */ 109 #define load_vfpregs(X) __asm __volatile("ldc p11, c0, [%0], {32}" : \ 110 : "r" (X) : "memory"); 111 112 #ifdef FPU_VFP 113 114 /* The real handler for VFP bounces. */ 115 static int vfp_handler(u_int, u_int, trapframe_t *, int); 116 static int vfp_handler(u_int, u_int, trapframe_t *, int); 117 118 static void vfp_state_load(lwp_t *, bool); 119 static void vfp_state_save(lwp_t *); 120 static void vfp_state_release(lwp_t *); 121 122 const pcu_ops_t arm_vfp_ops = { 123 .pcu_id = PCU_FPU, 124 .pcu_state_load = vfp_state_load, 125 .pcu_state_save = vfp_state_save, 126 .pcu_state_release = vfp_state_release, 127 }; 128 129 struct evcnt vfpevent_use; 130 struct evcnt vfpevent_reuse; 131 132 /* 133 * Used to test for a VFP. The following function is installed as a coproc10 134 * handler on the undefined instruction vector and then we issue a VFP 135 * instruction. If undefined_test is non zero then the VFP did not handle 136 * the instruction so must be absent, or disabled. 137 */ 138 139 static int undefined_test; 140 141 static int 142 vfp_test(u_int address, u_int insn, trapframe_t *frame, int fault_code) 143 { 144 145 frame->tf_pc += INSN_SIZE; 146 ++undefined_test; 147 return 0; 148 } 149 150 #endif /* FPU_VFP */ 151 152 struct evcnt vfp_fpscr_ev = 153 EVCNT_INITIALIZER(EVCNT_TYPE_TRAP, NULL, "VFP", "FPSCR traps"); 154 EVCNT_ATTACH_STATIC(vfp_fpscr_ev); 155 156 static int 157 vfp_fpscr_handler(u_int address, u_int insn, trapframe_t *frame, int fault_code) 158 { 159 struct lwp * const l = curlwp; 160 const u_int regno = (insn >> 12) & 0xf; 161 /* 162 * Only match move to/from the FPSCR register and we 163 * can't be using the SP,LR,PC as a source. 164 */ 165 if ((insn & 0xffef0fff) != 0xeee10a10 || regno > 12) 166 return 1; 167 168 struct pcb * const pcb = lwp_getpcb(l); 169 170 #ifdef FPU_VFP 171 /* 172 * If FPU is valid somewhere, let's just reenable VFP and 173 * retry the instruction (only safe thing to do since the 174 * pcb has a stale copy). 175 */ 176 if (pcb->pcb_vfp.vfp_fpexc & VFP_FPEXC_EN) 177 return 1; 178 #endif 179 180 if (__predict_false((l->l_md.md_flags & MDLWP_VFPUSED) == 0)) { 181 l->l_md.md_flags |= MDLWP_VFPUSED; 182 pcb->pcb_vfp.vfp_fpscr = 183 (VFP_FPSCR_DN | VFP_FPSCR_FZ); /* Runfast */ 184 } 185 186 /* 187 * We know know the pcb has the saved copy. 188 */ 189 register_t * const regp = &frame->tf_r0 + regno; 190 if (insn & 0x00100000) { 191 *regp = pcb->pcb_vfp.vfp_fpscr; 192 } else { 193 pcb->pcb_vfp.vfp_fpscr = *regp; 194 } 195 196 vfp_fpscr_ev.ev_count++; 197 198 frame->tf_pc += INSN_SIZE; 199 return 0; 200 } 201 202 #ifndef FPU_VFP 203 /* 204 * If we don't want VFP support, we still need to handle emulating VFP FPSCR 205 * instructions. 206 */ 207 void 208 vfp_attach(void) 209 { 210 install_coproc_handler(VFP_COPROC, vfp_fpscr_handler); 211 } 212 213 #else 214 void 215 vfp_attach(void) 216 { 217 struct cpu_info * const ci = curcpu(); 218 const char *model = NULL; 219 void *uh; 220 221 uh = install_coproc_handler(VFP_COPROC, vfp_test); 222 223 undefined_test = 0; 224 225 const uint32_t fpsid = read_fpsid(); 226 227 remove_coproc_handler(uh); 228 229 if (undefined_test != 0) { 230 aprint_normal_dev(ci->ci_dev, "No VFP detected\n"); 231 install_coproc_handler(VFP_COPROC, vfp_fpscr_handler); 232 ci->ci_vfp_id = 0; 233 return; 234 } 235 236 ci->ci_vfp_id = fpsid; 237 switch (fpsid & ~ VFP_FPSID_REV_MSK) { 238 case FPU_VFP10_ARM10E: 239 model = "VFP10 R1"; 240 break; 241 case FPU_VFP11_ARM11: 242 model = "VFP11"; 243 break; 244 default: 245 aprint_normal_dev(ci->ci_dev, "unrecognized VFP version %x\n", 246 fpsid); 247 install_coproc_handler(VFP_COPROC, vfp_fpscr_handler); 248 return; 249 } 250 251 if (fpsid != 0) { 252 aprint_normal("vfp%d at %s: %s\n", 253 curcpu()->ci_dev->dv_unit, curcpu()->ci_dev->dv_xname, 254 model); 255 } 256 evcnt_attach_dynamic(&vfpevent_use, EVCNT_TYPE_MISC, NULL, 257 "VFP", "proc use"); 258 evcnt_attach_dynamic(&vfpevent_reuse, EVCNT_TYPE_MISC, NULL, 259 "VFP", "proc re-use"); 260 install_coproc_handler(VFP_COPROC, vfp_handler); 261 install_coproc_handler(VFP_COPROC2, vfp_handler); 262 } 263 264 /* The real handler for VFP bounces. */ 265 static int 266 vfp_handler(u_int address, u_int insn, trapframe_t *frame, 267 int fault_code) 268 { 269 struct cpu_info * const ci = curcpu(); 270 271 /* This shouldn't ever happen. */ 272 if (fault_code != FAULT_USER) 273 panic("VFP fault in non-user mode"); 274 275 if (ci->ci_vfp_id == 0) 276 /* No VFP detected, just fault. */ 277 return 1; 278 279 /* 280 * If we are just changing/fetching FPSCR, don't bother loading it. 281 */ 282 if (!vfp_fpscr_handler(address, insn, frame, fault_code)) 283 return 0; 284 285 pcu_load(&arm_vfp_ops); 286 287 /* Need to restart the faulted instruction. */ 288 // frame->tf_pc -= INSN_SIZE; 289 return 0; 290 } 291 292 static void 293 vfp_state_load(lwp_t *l, bool used) 294 { 295 struct pcb * const pcb = lwp_getpcb(l); 296 struct vfpreg * const fregs = &pcb->pcb_vfp; 297 298 /* 299 * Instrument VFP usage -- if a process has not previously 300 * used the VFP, mark it as having used VFP for the first time, 301 * and count this event. 302 * 303 * If a process has used the VFP, count a "used VFP, and took 304 * a trap to use it again" event. 305 */ 306 if (__predict_false((l->l_md.md_flags & MDLWP_VFPUSED) == 0)) { 307 vfpevent_use.ev_count++; 308 l->l_md.md_flags |= MDLWP_VFPUSED; 309 pcb->pcb_vfp.vfp_fpscr = 310 (VFP_FPSCR_DN | VFP_FPSCR_FZ); /* Runfast */ 311 } else { 312 vfpevent_reuse.ev_count++; 313 } 314 315 if (fregs->vfp_fpexc & VFP_FPEXC_EN) { 316 /* 317 * If we think the VFP is enabled, it must have be disabled by 318 * vfp_state_release for another LWP so we can just restore 319 * FPEXC and return since our VFP state is still loaded. 320 */ 321 write_fpexc(fregs->vfp_fpexc); 322 return; 323 } 324 325 /* Enable the VFP (so that we can write the registers). */ 326 uint32_t fpexc = read_fpexc(); 327 KDASSERT((fpexc & VFP_FPEXC_EX) == 0); 328 write_fpexc(fpexc | VFP_FPEXC_EN); 329 330 load_vfpregs(fregs->vfp_regs); 331 write_fpscr(fregs->vfp_fpscr); 332 333 if (fregs->vfp_fpexc & VFP_FPEXC_EX) { 334 struct cpu_info * const ci = curcpu(); 335 /* Need to restore the exception handling state. */ 336 switch (ci->ci_vfp_id) { 337 case FPU_VFP10_ARM10E: 338 case FPU_VFP11_ARM11: 339 write_fpinst2(fregs->vfp_fpinst2); 340 write_fpinst(fregs->vfp_fpinst); 341 break; 342 default: 343 panic("%s: Unsupported VFP %#x", 344 __func__, ci->ci_vfp_id); 345 } 346 } 347 348 /* Finally, restore the FPEXC but don't enable the VFP. */ 349 fregs->vfp_fpexc |= VFP_FPEXC_EN; 350 write_fpexc(fregs->vfp_fpexc); 351 } 352 353 void 354 vfp_state_save(lwp_t *l) 355 { 356 struct pcb * const pcb = lwp_getpcb(l); 357 struct vfpreg * const fregs = &pcb->pcb_vfp; 358 359 /* 360 * If it's already disabled, then the state has been saved 361 * (or discarded). 362 */ 363 if ((fregs->vfp_fpexc & VFP_FPEXC_EN) == 0) 364 return; 365 366 /* 367 * Enable the VFP (so we can read the registers). 368 * Make sure the exception bit is cleared so that we can 369 * safely dump the registers. 370 */ 371 uint32_t fpexc = read_fpexc(); 372 write_fpexc((fpexc | VFP_FPEXC_EN) & ~VFP_FPEXC_EX); 373 374 fregs->vfp_fpexc = fpexc; 375 if (fpexc & VFP_FPEXC_EX) { 376 struct cpu_info * const ci = curcpu(); 377 /* Need to save the exception handling state */ 378 switch (ci->ci_vfp_id) { 379 case FPU_VFP10_ARM10E: 380 case FPU_VFP11_ARM11: 381 fregs->vfp_fpinst = read_fpinst(); 382 fregs->vfp_fpinst2 = read_fpinst2(); 383 break; 384 default: 385 panic("%s: Unsupported VFP %#x", 386 __func__, ci->ci_vfp_id); 387 } 388 } 389 fregs->vfp_fpscr = read_fpscr(); 390 save_vfpregs(fregs->vfp_regs); 391 392 /* Disable the VFP. */ 393 write_fpexc(fpexc); 394 } 395 396 void 397 vfp_state_release(lwp_t *l) 398 { 399 struct pcb * const pcb = lwp_getpcb(l); 400 401 /* 402 * Now mark the VFP as disabled (and our state has been already 403 * saved or is being discarded). 404 */ 405 pcb->pcb_vfp.vfp_fpexc &= ~VFP_FPEXC_EN; 406 407 /* 408 * Turn off the FPU so the next time a VFP instruction is issued 409 * an exception happens. We don't know if this LWP's state was 410 * loaded but if we turned off the FPU for some other LWP, when 411 * pcu_load invokes vfp_state_load it will see that VFP_FPEXC_EN 412 * is still set so it just restroe fpexc and return since its 413 * contents are still sitting in the VFP. 414 */ 415 write_fpexc(read_fpexc() & ~VFP_FPEXC_EN); 416 } 417 418 void 419 vfp_savecontext(void) 420 { 421 pcu_save(&arm_vfp_ops); 422 } 423 424 void 425 vfp_discardcontext(void) 426 { 427 pcu_discard(&arm_vfp_ops); 428 } 429 430 #endif /* FPU_VFP */ 431