1 /* $NetBSD: vfp_init.c,v 1.3 2009/11/21 20:32:28 rmind 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 pcb *pcb; 166 struct lwp *l; 167 168 /* This shouldn't ever happen. */ 169 if (fault_code != FAULT_USER) 170 panic("VFP fault in non-user mode"); 171 172 if (ci->ci_vfp.vfp_id == 0) 173 /* No VFP detected, just fault. */ 174 return 1; 175 176 l = curlwp; 177 pcb = lwp_getpcb(l); 178 179 if ((l->l_md.md_flags & MDP_VFPUSED) && ci->ci_vfp.vfp_fpcurlwp == l) { 180 uint32_t fpexc; 181 182 printf("VFP bounce @%x (insn=%x) lwp=%p\n", address, 183 instruction, l); 184 read_fpexc(&fpexc); 185 if ((fpexc & VFP_FPEXC_EN) == 0) 186 printf("vfp not enabled\n"); 187 vfp_saveregs_lwp(l, 1); 188 printf(" fpexc = 0x%08x fpscr = 0x%08x\n", fpexc, 189 pcb->pcb_vfp.vfp_fpscr); 190 printf(" fpinst = 0x%08x fpinst2 = 0x%08x\n", 191 pcb->pcb_vfp.vfp_fpinst, 192 pcb->pcb_vfp.vfp_fpinst2); 193 return 1; 194 } 195 196 if (ci->ci_vfp.vfp_fpcurlwp != NULL) 197 vfp_saveregs_cpu(ci, 1); 198 199 KDASSERT(ci->ci_vfp.vfp_fpcurlwp == NULL); 200 201 KDASSERT(pcb->pcb_vfpcpu == NULL); 202 203 // VFPCPU_LOCK(pcb, s); 204 205 pcb->pcb_vfpcpu = ci; 206 ci->ci_vfp.vfp_fpcurlwp = l; 207 208 // VFPCPU_UNLOCK(pcb, s); 209 210 /* 211 * Instrument VFP usage -- if a process has not previously 212 * used the VFP, mark it as having used VFP for the first time, 213 * and count this event. 214 * 215 * If a process has used the VFP, count a "used VFP, and took 216 * a trap to use it again" event. 217 */ 218 if ((l->l_md.md_flags & MDP_VFPUSED) == 0) { 219 vfpevent_use.ev_count++; 220 l->l_md.md_flags |= MDP_VFPUSED; 221 pcb->pcb_vfp.vfp_fpscr = 222 (VFP_FPSCR_DN | VFP_FPSCR_FZ); /* Runfast */ 223 } else 224 vfpevent_reuse.ev_count++; 225 226 vfp_load_regs(&pcb->pcb_vfp); 227 228 /* Need to restart the faulted instruction. */ 229 // frame->tf_pc -= INSN_SIZE; 230 return 0; 231 } 232 233 static void 234 vfp_load_regs(struct vfpreg *fregs) 235 { 236 uint32_t fpexc; 237 238 /* Enable the VFP (so that we can write the registers). */ 239 read_fpexc(&fpexc); 240 KDASSERT((fpexc & VFP_FPEXC_EX) == 0); 241 write_fpexc(fpexc | VFP_FPEXC_EN); 242 243 load_vfpregs(fregs->vfp_regs); 244 write_fpscr(fregs->vfp_fpscr); 245 if (fregs->vfp_fpexc & VFP_FPEXC_EX) { 246 /* Need to restore the exception handling state. */ 247 switch (curcpu()->ci_vfp.vfp_id) { 248 case FPU_VFP10_ARM10E: 249 case FPU_VFP11_ARM11: 250 write_fpinst2(fregs->vfp_fpinst2); 251 write_fpinst(fregs->vfp_fpinst); 252 break; 253 default: 254 panic("vfp_load_regs: Unsupported VFP"); 255 } 256 } 257 /* Finally, restore the FPEXC and enable the VFP. */ 258 write_fpexc(fregs->vfp_fpexc | VFP_FPEXC_EN); 259 } 260 261 void 262 vfp_saveregs_cpu(struct cpu_info *ci, int save) 263 { 264 struct lwp *l; 265 struct pcb *pcb; 266 uint32_t fpexc; 267 268 KDASSERT(ci == curcpu()); 269 270 l = ci->ci_vfp.vfp_fpcurlwp; 271 if (l == NULL) 272 return; 273 274 pcb = lwp_getpcb(l); 275 read_fpexc(&fpexc); 276 277 if (save) { 278 struct vfpreg *fregs = &pcb->pcb_vfp; 279 280 /* 281 * Enable the VFP (so we can read the registers). 282 * Make sure the exception bit is cleared so that we can 283 * safely dump the registers. 284 */ 285 write_fpexc((fpexc | VFP_FPEXC_EN) & ~VFP_FPEXC_EX); 286 287 fregs->vfp_fpexc = fpexc; 288 if (fpexc & VFP_FPEXC_EX) { 289 /* Need to save the exception handling state */ 290 switch (ci->ci_vfp.vfp_id) { 291 case FPU_VFP10_ARM10E: 292 case FPU_VFP11_ARM11: 293 read_fpinst(&fregs->vfp_fpinst); 294 read_fpinst2(&fregs->vfp_fpinst2); 295 break; 296 default: 297 panic("vfp_saveregs_cpu: Unsupported VFP"); 298 } 299 } 300 read_fpscr(&fregs->vfp_fpscr); 301 save_vfpregs(fregs->vfp_regs); 302 } 303 /* Disable the VFP. */ 304 write_fpexc(fpexc & ~VFP_FPEXC_EN); 305 // VFPCPU_LOCK(pcb, s); 306 307 pcb->pcb_vfpcpu = NULL; 308 ci->ci_vfp.vfp_fpcurlwp = NULL; 309 // VFPCPU_UNLOCK(pcb, s); 310 } 311 312 void 313 vfp_saveregs_lwp(struct lwp *l, int save) 314 { 315 struct cpu_info *ci = curcpu(); 316 struct cpu_info *oci; 317 struct pcb *pcb; 318 319 pcb = lwp_getpcb(l); 320 KDASSERT(pcb != NULL); 321 322 // VFPCPU_LOCK(pcb, s); 323 324 oci = pcb->pcb_vfpcpu; 325 if (oci == NULL) { 326 // VFPCPU_UNLOCK(pcb, s); 327 return; 328 } 329 330 #if defined(MULTIPROCESSOR) 331 /* 332 * On a multiprocessor system this is where we would send an IPI 333 * to the processor holding the VFP state for this process. 334 */ 335 #error MULTIPROCESSOR 336 #else 337 KASSERT(ci->ci_vfp.vfp_fpcurlwp == l); 338 // VFPCPU_UNLOCK(pcb, s); 339 vfp_saveregs_cpu(ci, save); 340 #endif 341 } 342 343 void 344 vfp_savecontext(void) 345 { 346 struct cpu_info *ci = curcpu(); 347 uint32_t fpexc; 348 349 if (ci->ci_vfp.vfp_fpcurlwp != NULL) { 350 read_fpexc(&fpexc); 351 write_fpexc(fpexc & ~VFP_FPEXC_EN); 352 } 353 } 354 355 void 356 vfp_loadcontext(struct lwp *l) 357 { 358 uint32_t fpexc; 359 360 if (curcpu()->ci_vfp.vfp_fpcurlwp == l) { 361 read_fpexc(&fpexc); 362 write_fpexc(fpexc | VFP_FPEXC_EN); 363 } 364 } 365