1 /* $NetBSD: vfp_init.c,v 1.2 2009/03/18 10:22:24 cegger 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 38 #include <arm/undefined.h> 39 #include <machine/cpu.h> 40 41 #include <arm/vfpvar.h> 42 #include <arm/vfpreg.h> 43 44 /* 45 * Use generic co-processor instructions to avoid assembly problems. 46 */ 47 48 /* FMRX <X>, fpsid */ 49 #define read_fpsid(X) __asm __volatile("mrc p10, 7, %0, c0, c0, 0" \ 50 : "=r" (*(X)) : : "memory") 51 /* FMRX <X>, fpscr */ 52 #define read_fpscr(X) __asm __volatile("mrc p10, 7, %0, c1, c0, 0" \ 53 : "=r" (*(X))) 54 /* FMRX <X>, fpexc */ 55 #define read_fpexc(X) __asm __volatile("mrc p10, 7, %0, c8, c0, 0" \ 56 : "=r" (*(X))) 57 /* FMRX <X>, fpinst */ 58 #define read_fpinst(X) __asm __volatile("mrc p10, 7, %0, c9, c0, 0" \ 59 : "=r" (*(X))) 60 /* FMRX <X>, fpinst2 */ 61 #define read_fpinst2(X) __asm __volatile("mrc p10, 7, %0, c10, c0, 0" \ 62 : "=r" (*(X))) 63 /* FSTMD <X>, {d0-d15} */ 64 #define save_vfpregs(X) __asm __volatile("stc p11, c0, [%0], {32}" : \ 65 : "r" (X) : "memory") 66 67 /* FMXR <X>, fpscr */ 68 #define write_fpscr(X) __asm __volatile("mcr p10, 7, %0, c1, c0, 0" : \ 69 : "r" (X)) 70 /* FMXR <X>, fpexc */ 71 #define write_fpexc(X) __asm __volatile("mcr p10, 7, %0, c8, c0, 0" : \ 72 : "r" (X)) 73 /* FMXR <X>, fpinst */ 74 #define write_fpinst(X) __asm __volatile("mcr p10, 7, %0, c9, c0, 0" : \ 75 : "r" (X)) 76 /* FMXR <X>, fpinst2 */ 77 #define write_fpinst2(X) __asm __volatile("mcr p10, 7, %0, c10, c0, 0" : \ 78 : "r" (X)) 79 /* FLDMD <X>, {d0-d15} */ 80 #define load_vfpregs(X) __asm __volatile("ldc p11, c0, [%0], {32}" : \ 81 : "r" (X) : "memory"); 82 83 /* The real handler for VFP bounces. */ 84 static int vfp_handler(u_int, u_int, trapframe_t *, int); 85 86 static void vfp_load_regs(struct vfpreg *); 87 88 struct evcnt vfpevent_use; 89 struct evcnt vfpevent_reuse; 90 91 /* 92 * Used to test for a VFP. The following function is installed as a coproc10 93 * handler on the undefined instruction vector and then we issue a VFP 94 * instruction. If undefined_test is non zero then the VFP did not handle 95 * the instruction so must be absent, or disabled. 96 */ 97 98 static int undefined_test; 99 100 static int 101 vfp_test(u_int address, u_int instruction, trapframe_t *frame, int fault_code) 102 { 103 104 frame->tf_pc += INSN_SIZE; 105 ++undefined_test; 106 return(0); 107 } 108 109 void 110 vfp_attach(void) 111 { 112 void *uh; 113 uint32_t fpsid; 114 const char *model = NULL; 115 116 uh = install_coproc_handler(VFP_COPROC, vfp_test); 117 118 undefined_test = 0; 119 120 read_fpsid(&fpsid); 121 122 remove_coproc_handler(uh); 123 124 if (undefined_test != 0) { 125 aprint_normal("%s: No VFP detected\n", 126 curcpu()->ci_dev->dv_xname); 127 curcpu()->ci_vfp.vfp_id = 0; 128 return; 129 } 130 131 curcpu()->ci_vfp.vfp_id = fpsid; 132 switch (fpsid & ~ VFP_FPSID_REV_MSK) 133 { 134 case FPU_VFP10_ARM10E: 135 model = "VFP10 R1"; 136 break; 137 case FPU_VFP11_ARM11: 138 model = "VFP11"; 139 break; 140 default: 141 aprint_normal("%s: unrecognized VFP version %x\n", 142 curcpu()->ci_dev->dv_xname, fpsid); 143 fpsid = 0; /* Not recognised. */ 144 return; 145 } 146 147 if (fpsid != 0) { 148 aprint_normal("vfp%d at %s: %s\n", 149 curcpu()->ci_dev->dv_unit, curcpu()->ci_dev->dv_xname, 150 model); 151 } 152 evcnt_attach_dynamic(&vfpevent_use, EVCNT_TYPE_MISC, NULL, 153 "VFP", "proc use"); 154 evcnt_attach_dynamic(&vfpevent_reuse, EVCNT_TYPE_MISC, NULL, 155 "VFP", "proc re-use"); 156 install_coproc_handler(VFP_COPROC, vfp_handler); 157 install_coproc_handler(VFP_COPROC2, vfp_handler); 158 } 159 160 /* The real handler for VFP bounces. */ 161 static int vfp_handler(u_int address, u_int instruction, trapframe_t *frame, 162 int fault_code) 163 { 164 struct cpu_info *ci = curcpu(); 165 struct lwp *l; 166 167 /* This shouldn't ever happen. */ 168 if (fault_code != FAULT_USER) 169 panic("VFP fault in non-user mode"); 170 171 if (ci->ci_vfp.vfp_id == 0) 172 /* No VFP detected, just fault. */ 173 return 1; 174 175 l = curlwp; 176 177 if ((l->l_md.md_flags & MDP_VFPUSED) && ci->ci_vfp.vfp_fpcurlwp == l) { 178 uint32_t fpexc; 179 180 printf("VFP bounce @%x (insn=%x) lwp=%p\n", address, 181 instruction, l); 182 read_fpexc(&fpexc); 183 if ((fpexc & VFP_FPEXC_EN) == 0) 184 printf("vfp not enabled\n"); 185 vfp_saveregs_lwp(l, 1); 186 printf(" fpexc = 0x%08x fpscr = 0x%08x\n", fpexc, 187 l->l_addr->u_pcb.pcb_vfp.vfp_fpscr); 188 printf(" fpinst = 0x%08x fpinst2 = 0x%08x\n", 189 l->l_addr->u_pcb.pcb_vfp.vfp_fpinst, 190 l->l_addr->u_pcb.pcb_vfp.vfp_fpinst2); 191 return 1; 192 } 193 194 if (ci->ci_vfp.vfp_fpcurlwp != NULL) 195 vfp_saveregs_cpu(ci, 1); 196 197 KDASSERT(ci->ci_vfp.vfp_fpcurlwp == NULL); 198 199 KDASSERT(l->l_addr->u_pcb.pcb_vfpcpu == NULL); 200 201 // VFPCPU_LOCK(&l->l_addr->u_pcb, s); 202 203 l->l_addr->u_pcb.pcb_vfpcpu = ci; 204 ci->ci_vfp.vfp_fpcurlwp = l; 205 206 // VFPCPU_UNLOCK(&l->l_addr->u_pcb, s); 207 208 /* 209 * Instrument VFP usage -- if a process has not previously 210 * used the VFP, mark it as having used VFP for the first time, 211 * and count this event. 212 * 213 * If a process has used the VFP, count a "used VFP, and took 214 * a trap to use it again" event. 215 */ 216 if ((l->l_md.md_flags & MDP_VFPUSED) == 0) { 217 vfpevent_use.ev_count++; 218 l->l_md.md_flags |= MDP_VFPUSED; 219 l->l_addr->u_pcb.pcb_vfp.vfp_fpscr = 220 (VFP_FPSCR_DN | VFP_FPSCR_FZ); /* Runfast */ 221 } else 222 vfpevent_reuse.ev_count++; 223 224 vfp_load_regs(&l->l_addr->u_pcb.pcb_vfp); 225 226 /* Need to restart the faulted instruction. */ 227 // frame->tf_pc -= INSN_SIZE; 228 return 0; 229 } 230 231 static void 232 vfp_load_regs(struct vfpreg *fregs) 233 { 234 uint32_t fpexc; 235 236 /* Enable the VFP (so that we can write the registers). */ 237 read_fpexc(&fpexc); 238 KDASSERT((fpexc & VFP_FPEXC_EX) == 0); 239 write_fpexc(fpexc | VFP_FPEXC_EN); 240 241 load_vfpregs(fregs->vfp_regs); 242 write_fpscr(fregs->vfp_fpscr); 243 if (fregs->vfp_fpexc & VFP_FPEXC_EX) { 244 /* Need to restore the exception handling state. */ 245 switch (curcpu()->ci_vfp.vfp_id) { 246 case FPU_VFP10_ARM10E: 247 case FPU_VFP11_ARM11: 248 write_fpinst2(fregs->vfp_fpinst2); 249 write_fpinst(fregs->vfp_fpinst); 250 break; 251 default: 252 panic("vfp_load_regs: Unsupported VFP"); 253 } 254 } 255 /* Finally, restore the FPEXC and enable the VFP. */ 256 write_fpexc(fregs->vfp_fpexc | VFP_FPEXC_EN); 257 } 258 259 void 260 vfp_saveregs_cpu(struct cpu_info *ci, int save) 261 { 262 struct lwp *l; 263 uint32_t fpexc; 264 265 KDASSERT(ci == curcpu()); 266 267 l = ci->ci_vfp.vfp_fpcurlwp; 268 if (l == NULL) 269 return; 270 271 read_fpexc(&fpexc); 272 273 if (save) { 274 struct vfpreg *fregs = &l->l_addr->u_pcb.pcb_vfp; 275 276 /* 277 * Enable the VFP (so we can read the registers). 278 * Make sure the exception bit is cleared so that we can 279 * safely dump the registers. 280 */ 281 write_fpexc((fpexc | VFP_FPEXC_EN) & ~VFP_FPEXC_EX); 282 283 fregs->vfp_fpexc = fpexc; 284 if (fpexc & VFP_FPEXC_EX) { 285 /* Need to save the exception handling state */ 286 switch (ci->ci_vfp.vfp_id) { 287 case FPU_VFP10_ARM10E: 288 case FPU_VFP11_ARM11: 289 read_fpinst(&fregs->vfp_fpinst); 290 read_fpinst2(&fregs->vfp_fpinst2); 291 break; 292 default: 293 panic("vfp_saveregs_cpu: Unsupported VFP"); 294 } 295 } 296 read_fpscr(&fregs->vfp_fpscr); 297 save_vfpregs(fregs->vfp_regs); 298 } 299 /* Disable the VFP. */ 300 write_fpexc(fpexc & ~VFP_FPEXC_EN); 301 // VFPCPU_LOCK(&l->l_addr->u_pcb, s); 302 303 l->l_addr->u_pcb.pcb_vfpcpu = NULL; 304 ci->ci_vfp.vfp_fpcurlwp = NULL; 305 // VFPCPU_UNLOCK(&l->l_addr->u_pcb, s); 306 } 307 308 void 309 vfp_saveregs_lwp(struct lwp *l, int save) 310 { 311 struct cpu_info *ci = curcpu(); 312 struct cpu_info *oci; 313 314 KDASSERT(l->l_addr != NULL); 315 316 // VFPCPU_LOCK(&l->l_addr->u_pcb, s); 317 318 oci = l->l_addr->u_pcb.pcb_vfpcpu; 319 if (oci == NULL) { 320 // VFPCPU_UNLOCK(&l->l_addr->u_pcb, s); 321 return; 322 } 323 324 #if defined(MULTIPROCESSOR) 325 /* 326 * On a multiprocessor system this is where we would send an IPI 327 * to the processor holding the VFP state for this process. 328 */ 329 #error MULTIPROCESSOR 330 #else 331 KASSERT(ci->ci_vfp.vfp_fpcurlwp == l); 332 // VFPCPU_UNLOCK(&l->l_addr->u_pcb, s); 333 vfp_saveregs_cpu(ci, save); 334 #endif 335 } 336 337 void 338 vfp_savecontext(void) 339 { 340 struct cpu_info *ci = curcpu(); 341 uint32_t fpexc; 342 343 if (ci->ci_vfp.vfp_fpcurlwp != NULL) { 344 read_fpexc(&fpexc); 345 write_fpexc(fpexc & ~VFP_FPEXC_EN); 346 } 347 } 348 349 void 350 vfp_loadcontext(struct lwp *l) 351 { 352 uint32_t fpexc; 353 354 if (curcpu()->ci_vfp.vfp_fpcurlwp == l) { 355 read_fpexc(&fpexc); 356 write_fpexc(fpexc | VFP_FPEXC_EN); 357 } 358 } 359