1 /* 2 * linux/arch/arm/vfp/vfpmodule.c 3 * 4 * Copyright (C) 2004 ARM Limited. 5 * Written by Deep Blue Solutions Limited. 6 * 7 * This program is free software; you can redistribute it and/or modify 8 * it under the terms of the GNU General Public License version 2 as 9 * published by the Free Software Foundation. 10 */ 11 #include <linux/module.h> 12 #include <linux/types.h> 13 #include <linux/cpu.h> 14 #include <linux/kernel.h> 15 #include <linux/notifier.h> 16 #include <linux/signal.h> 17 #include <linux/sched.h> 18 #include <linux/smp.h> 19 #include <linux/init.h> 20 21 #include <asm/cputype.h> 22 #include <asm/thread_notify.h> 23 #include <asm/vfp.h> 24 25 #include "vfpinstr.h" 26 #include "vfp.h" 27 28 /* 29 * Our undef handlers (in entry.S) 30 */ 31 void vfp_testing_entry(void); 32 void vfp_support_entry(void); 33 void vfp_null_entry(void); 34 35 void (*vfp_vector)(void) = vfp_null_entry; 36 union vfp_state *last_VFP_context[NR_CPUS]; 37 38 /* 39 * Dual-use variable. 40 * Used in startup: set to non-zero if VFP checks fail 41 * After startup, holds VFP architecture 42 */ 43 unsigned int VFP_arch; 44 45 /* 46 * Per-thread VFP initialization. 47 */ 48 static void vfp_thread_flush(struct thread_info *thread) 49 { 50 union vfp_state *vfp = &thread->vfpstate; 51 unsigned int cpu; 52 53 memset(vfp, 0, sizeof(union vfp_state)); 54 55 vfp->hard.fpexc = FPEXC_EN; 56 vfp->hard.fpscr = FPSCR_ROUND_NEAREST; 57 58 /* 59 * Disable VFP to ensure we initialize it first. We must ensure 60 * that the modification of last_VFP_context[] and hardware disable 61 * are done for the same CPU and without preemption. 62 */ 63 cpu = get_cpu(); 64 if (last_VFP_context[cpu] == vfp) 65 last_VFP_context[cpu] = NULL; 66 fmxr(FPEXC, fmrx(FPEXC) & ~FPEXC_EN); 67 put_cpu(); 68 } 69 70 static void vfp_thread_exit(struct thread_info *thread) 71 { 72 /* release case: Per-thread VFP cleanup. */ 73 union vfp_state *vfp = &thread->vfpstate; 74 unsigned int cpu = get_cpu(); 75 76 if (last_VFP_context[cpu] == vfp) 77 last_VFP_context[cpu] = NULL; 78 put_cpu(); 79 } 80 81 /* 82 * When this function is called with the following 'cmd's, the following 83 * is true while this function is being run: 84 * THREAD_NOFTIFY_SWTICH: 85 * - the previously running thread will not be scheduled onto another CPU. 86 * - the next thread to be run (v) will not be running on another CPU. 87 * - thread->cpu is the local CPU number 88 * - not preemptible as we're called in the middle of a thread switch 89 * THREAD_NOTIFY_FLUSH: 90 * - the thread (v) will be running on the local CPU, so 91 * v === current_thread_info() 92 * - thread->cpu is the local CPU number at the time it is accessed, 93 * but may change at any time. 94 * - we could be preempted if tree preempt rcu is enabled, so 95 * it is unsafe to use thread->cpu. 96 * THREAD_NOTIFY_EXIT 97 * - the thread (v) will be running on the local CPU, so 98 * v === current_thread_info() 99 * - thread->cpu is the local CPU number at the time it is accessed, 100 * but may change at any time. 101 * - we could be preempted if tree preempt rcu is enabled, so 102 * it is unsafe to use thread->cpu. 103 */ 104 static int vfp_notifier(struct notifier_block *self, unsigned long cmd, void *v) 105 { 106 struct thread_info *thread = v; 107 108 if (likely(cmd == THREAD_NOTIFY_SWITCH)) { 109 u32 fpexc = fmrx(FPEXC); 110 111 #ifdef CONFIG_SMP 112 unsigned int cpu = thread->cpu; 113 114 /* 115 * On SMP, if VFP is enabled, save the old state in 116 * case the thread migrates to a different CPU. The 117 * restoring is done lazily. 118 */ 119 if ((fpexc & FPEXC_EN) && last_VFP_context[cpu]) { 120 vfp_save_state(last_VFP_context[cpu], fpexc); 121 last_VFP_context[cpu]->hard.cpu = cpu; 122 } 123 /* 124 * Thread migration, just force the reloading of the 125 * state on the new CPU in case the VFP registers 126 * contain stale data. 127 */ 128 if (thread->vfpstate.hard.cpu != cpu) 129 last_VFP_context[cpu] = NULL; 130 #endif 131 132 /* 133 * Always disable VFP so we can lazily save/restore the 134 * old state. 135 */ 136 fmxr(FPEXC, fpexc & ~FPEXC_EN); 137 return NOTIFY_DONE; 138 } 139 140 if (cmd == THREAD_NOTIFY_FLUSH) 141 vfp_thread_flush(thread); 142 else 143 vfp_thread_exit(thread); 144 145 return NOTIFY_DONE; 146 } 147 148 static struct notifier_block vfp_notifier_block = { 149 .notifier_call = vfp_notifier, 150 }; 151 152 /* 153 * Raise a SIGFPE for the current process. 154 * sicode describes the signal being raised. 155 */ 156 void vfp_raise_sigfpe(unsigned int sicode, struct pt_regs *regs) 157 { 158 siginfo_t info; 159 160 memset(&info, 0, sizeof(info)); 161 162 info.si_signo = SIGFPE; 163 info.si_code = sicode; 164 info.si_addr = (void __user *)(instruction_pointer(regs) - 4); 165 166 /* 167 * This is the same as NWFPE, because it's not clear what 168 * this is used for 169 */ 170 current->thread.error_code = 0; 171 current->thread.trap_no = 6; 172 173 send_sig_info(SIGFPE, &info, current); 174 } 175 176 static void vfp_panic(char *reason, u32 inst) 177 { 178 int i; 179 180 printk(KERN_ERR "VFP: Error: %s\n", reason); 181 printk(KERN_ERR "VFP: EXC 0x%08x SCR 0x%08x INST 0x%08x\n", 182 fmrx(FPEXC), fmrx(FPSCR), inst); 183 for (i = 0; i < 32; i += 2) 184 printk(KERN_ERR "VFP: s%2u: 0x%08x s%2u: 0x%08x\n", 185 i, vfp_get_float(i), i+1, vfp_get_float(i+1)); 186 } 187 188 /* 189 * Process bitmask of exception conditions. 190 */ 191 static void vfp_raise_exceptions(u32 exceptions, u32 inst, u32 fpscr, struct pt_regs *regs) 192 { 193 int si_code = 0; 194 195 pr_debug("VFP: raising exceptions %08x\n", exceptions); 196 197 if (exceptions == VFP_EXCEPTION_ERROR) { 198 vfp_panic("unhandled bounce", inst); 199 vfp_raise_sigfpe(0, regs); 200 return; 201 } 202 203 /* 204 * If any of the status flags are set, update the FPSCR. 205 * Comparison instructions always return at least one of 206 * these flags set. 207 */ 208 if (exceptions & (FPSCR_N|FPSCR_Z|FPSCR_C|FPSCR_V)) 209 fpscr &= ~(FPSCR_N|FPSCR_Z|FPSCR_C|FPSCR_V); 210 211 fpscr |= exceptions; 212 213 fmxr(FPSCR, fpscr); 214 215 #define RAISE(stat,en,sig) \ 216 if (exceptions & stat && fpscr & en) \ 217 si_code = sig; 218 219 /* 220 * These are arranged in priority order, least to highest. 221 */ 222 RAISE(FPSCR_DZC, FPSCR_DZE, FPE_FLTDIV); 223 RAISE(FPSCR_IXC, FPSCR_IXE, FPE_FLTRES); 224 RAISE(FPSCR_UFC, FPSCR_UFE, FPE_FLTUND); 225 RAISE(FPSCR_OFC, FPSCR_OFE, FPE_FLTOVF); 226 RAISE(FPSCR_IOC, FPSCR_IOE, FPE_FLTINV); 227 228 if (si_code) 229 vfp_raise_sigfpe(si_code, regs); 230 } 231 232 /* 233 * Emulate a VFP instruction. 234 */ 235 static u32 vfp_emulate_instruction(u32 inst, u32 fpscr, struct pt_regs *regs) 236 { 237 u32 exceptions = VFP_EXCEPTION_ERROR; 238 239 pr_debug("VFP: emulate: INST=0x%08x SCR=0x%08x\n", inst, fpscr); 240 241 if (INST_CPRTDO(inst)) { 242 if (!INST_CPRT(inst)) { 243 /* 244 * CPDO 245 */ 246 if (vfp_single(inst)) { 247 exceptions = vfp_single_cpdo(inst, fpscr); 248 } else { 249 exceptions = vfp_double_cpdo(inst, fpscr); 250 } 251 } else { 252 /* 253 * A CPRT instruction can not appear in FPINST2, nor 254 * can it cause an exception. Therefore, we do not 255 * have to emulate it. 256 */ 257 } 258 } else { 259 /* 260 * A CPDT instruction can not appear in FPINST2, nor can 261 * it cause an exception. Therefore, we do not have to 262 * emulate it. 263 */ 264 } 265 return exceptions & ~VFP_NAN_FLAG; 266 } 267 268 /* 269 * Package up a bounce condition. 270 */ 271 void VFP_bounce(u32 trigger, u32 fpexc, struct pt_regs *regs) 272 { 273 u32 fpscr, orig_fpscr, fpsid, exceptions; 274 275 pr_debug("VFP: bounce: trigger %08x fpexc %08x\n", trigger, fpexc); 276 277 /* 278 * At this point, FPEXC can have the following configuration: 279 * 280 * EX DEX IXE 281 * 0 1 x - synchronous exception 282 * 1 x 0 - asynchronous exception 283 * 1 x 1 - sychronous on VFP subarch 1 and asynchronous on later 284 * 0 0 1 - synchronous on VFP9 (non-standard subarch 1 285 * implementation), undefined otherwise 286 * 287 * Clear various bits and enable access to the VFP so we can 288 * handle the bounce. 289 */ 290 fmxr(FPEXC, fpexc & ~(FPEXC_EX|FPEXC_DEX|FPEXC_FP2V|FPEXC_VV|FPEXC_TRAP_MASK)); 291 292 fpsid = fmrx(FPSID); 293 orig_fpscr = fpscr = fmrx(FPSCR); 294 295 /* 296 * Check for the special VFP subarch 1 and FPSCR.IXE bit case 297 */ 298 if ((fpsid & FPSID_ARCH_MASK) == (1 << FPSID_ARCH_BIT) 299 && (fpscr & FPSCR_IXE)) { 300 /* 301 * Synchronous exception, emulate the trigger instruction 302 */ 303 goto emulate; 304 } 305 306 if (fpexc & FPEXC_EX) { 307 #ifndef CONFIG_CPU_FEROCEON 308 /* 309 * Asynchronous exception. The instruction is read from FPINST 310 * and the interrupted instruction has to be restarted. 311 */ 312 trigger = fmrx(FPINST); 313 regs->ARM_pc -= 4; 314 #endif 315 } else if (!(fpexc & FPEXC_DEX)) { 316 /* 317 * Illegal combination of bits. It can be caused by an 318 * unallocated VFP instruction but with FPSCR.IXE set and not 319 * on VFP subarch 1. 320 */ 321 vfp_raise_exceptions(VFP_EXCEPTION_ERROR, trigger, fpscr, regs); 322 goto exit; 323 } 324 325 /* 326 * Modify fpscr to indicate the number of iterations remaining. 327 * If FPEXC.EX is 0, FPEXC.DEX is 1 and the FPEXC.VV bit indicates 328 * whether FPEXC.VECITR or FPSCR.LEN is used. 329 */ 330 if (fpexc & (FPEXC_EX | FPEXC_VV)) { 331 u32 len; 332 333 len = fpexc + (1 << FPEXC_LENGTH_BIT); 334 335 fpscr &= ~FPSCR_LENGTH_MASK; 336 fpscr |= (len & FPEXC_LENGTH_MASK) << (FPSCR_LENGTH_BIT - FPEXC_LENGTH_BIT); 337 } 338 339 /* 340 * Handle the first FP instruction. We used to take note of the 341 * FPEXC bounce reason, but this appears to be unreliable. 342 * Emulate the bounced instruction instead. 343 */ 344 exceptions = vfp_emulate_instruction(trigger, fpscr, regs); 345 if (exceptions) 346 vfp_raise_exceptions(exceptions, trigger, orig_fpscr, regs); 347 348 /* 349 * If there isn't a second FP instruction, exit now. Note that 350 * the FPEXC.FP2V bit is valid only if FPEXC.EX is 1. 351 */ 352 if (fpexc ^ (FPEXC_EX | FPEXC_FP2V)) 353 goto exit; 354 355 /* 356 * The barrier() here prevents fpinst2 being read 357 * before the condition above. 358 */ 359 barrier(); 360 trigger = fmrx(FPINST2); 361 362 emulate: 363 exceptions = vfp_emulate_instruction(trigger, orig_fpscr, regs); 364 if (exceptions) 365 vfp_raise_exceptions(exceptions, trigger, orig_fpscr, regs); 366 exit: 367 preempt_enable(); 368 } 369 370 static void vfp_enable(void *unused) 371 { 372 u32 access = get_copro_access(); 373 374 /* 375 * Enable full access to VFP (cp10 and cp11) 376 */ 377 set_copro_access(access | CPACC_FULL(10) | CPACC_FULL(11)); 378 } 379 380 #ifdef CONFIG_PM 381 #include <linux/sysdev.h> 382 383 static int vfp_pm_suspend(struct sys_device *dev, pm_message_t state) 384 { 385 struct thread_info *ti = current_thread_info(); 386 u32 fpexc = fmrx(FPEXC); 387 388 /* if vfp is on, then save state for resumption */ 389 if (fpexc & FPEXC_EN) { 390 printk(KERN_DEBUG "%s: saving vfp state\n", __func__); 391 vfp_save_state(&ti->vfpstate, fpexc); 392 393 /* disable, just in case */ 394 fmxr(FPEXC, fmrx(FPEXC) & ~FPEXC_EN); 395 } 396 397 /* clear any information we had about last context state */ 398 memset(last_VFP_context, 0, sizeof(last_VFP_context)); 399 400 return 0; 401 } 402 403 static int vfp_pm_resume(struct sys_device *dev) 404 { 405 /* ensure we have access to the vfp */ 406 vfp_enable(NULL); 407 408 /* and disable it to ensure the next usage restores the state */ 409 fmxr(FPEXC, fmrx(FPEXC) & ~FPEXC_EN); 410 411 return 0; 412 } 413 414 static struct sysdev_class vfp_pm_sysclass = { 415 .name = "vfp", 416 .suspend = vfp_pm_suspend, 417 .resume = vfp_pm_resume, 418 }; 419 420 static struct sys_device vfp_pm_sysdev = { 421 .cls = &vfp_pm_sysclass, 422 }; 423 424 static void vfp_pm_init(void) 425 { 426 sysdev_class_register(&vfp_pm_sysclass); 427 sysdev_register(&vfp_pm_sysdev); 428 } 429 430 431 #else 432 static inline void vfp_pm_init(void) { } 433 #endif /* CONFIG_PM */ 434 435 void vfp_sync_hwstate(struct thread_info *thread) 436 { 437 unsigned int cpu = get_cpu(); 438 439 /* 440 * If the thread we're interested in is the current owner of the 441 * hardware VFP state, then we need to save its state. 442 */ 443 if (last_VFP_context[cpu] == &thread->vfpstate) { 444 u32 fpexc = fmrx(FPEXC); 445 446 /* 447 * Save the last VFP state on this CPU. 448 */ 449 fmxr(FPEXC, fpexc | FPEXC_EN); 450 vfp_save_state(&thread->vfpstate, fpexc | FPEXC_EN); 451 fmxr(FPEXC, fpexc); 452 } 453 454 put_cpu(); 455 } 456 457 void vfp_flush_hwstate(struct thread_info *thread) 458 { 459 unsigned int cpu = get_cpu(); 460 461 /* 462 * If the thread we're interested in is the current owner of the 463 * hardware VFP state, then we need to save its state. 464 */ 465 if (last_VFP_context[cpu] == &thread->vfpstate) { 466 u32 fpexc = fmrx(FPEXC); 467 468 fmxr(FPEXC, fpexc & ~FPEXC_EN); 469 470 /* 471 * Set the context to NULL to force a reload the next time 472 * the thread uses the VFP. 473 */ 474 last_VFP_context[cpu] = NULL; 475 } 476 477 #ifdef CONFIG_SMP 478 /* 479 * For SMP we still have to take care of the case where the thread 480 * migrates to another CPU and then back to the original CPU on which 481 * the last VFP user is still the same thread. Mark the thread VFP 482 * state as belonging to a non-existent CPU so that the saved one will 483 * be reloaded in the above case. 484 */ 485 thread->vfpstate.hard.cpu = NR_CPUS; 486 #endif 487 put_cpu(); 488 } 489 490 /* 491 * VFP hardware can lose all context when a CPU goes offline. 492 * Safely clear our held state when a CPU has been killed, and 493 * re-enable access to VFP when the CPU comes back online. 494 * 495 * Both CPU_DYING and CPU_STARTING are called on the CPU which 496 * is being offlined/onlined. 497 */ 498 static int vfp_hotplug(struct notifier_block *b, unsigned long action, 499 void *hcpu) 500 { 501 if (action == CPU_DYING || action == CPU_DYING_FROZEN) { 502 unsigned int cpu = (long)hcpu; 503 last_VFP_context[cpu] = NULL; 504 } else if (action == CPU_STARTING || action == CPU_STARTING_FROZEN) 505 vfp_enable(NULL); 506 return NOTIFY_OK; 507 } 508 509 /* 510 * VFP support code initialisation. 511 */ 512 static int __init vfp_init(void) 513 { 514 unsigned int vfpsid; 515 unsigned int cpu_arch = cpu_architecture(); 516 517 if (cpu_arch >= CPU_ARCH_ARMv6) 518 vfp_enable(NULL); 519 520 /* 521 * First check that there is a VFP that we can use. 522 * The handler is already setup to just log calls, so 523 * we just need to read the VFPSID register. 524 */ 525 vfp_vector = vfp_testing_entry; 526 barrier(); 527 vfpsid = fmrx(FPSID); 528 barrier(); 529 vfp_vector = vfp_null_entry; 530 531 printk(KERN_INFO "VFP support v0.3: "); 532 if (VFP_arch) 533 printk("not present\n"); 534 else if (vfpsid & FPSID_NODOUBLE) { 535 printk("no double precision support\n"); 536 } else { 537 hotcpu_notifier(vfp_hotplug, 0); 538 539 smp_call_function(vfp_enable, NULL, 1); 540 541 VFP_arch = (vfpsid & FPSID_ARCH_MASK) >> FPSID_ARCH_BIT; /* Extract the architecture version */ 542 printk("implementor %02x architecture %d part %02x variant %x rev %x\n", 543 (vfpsid & FPSID_IMPLEMENTER_MASK) >> FPSID_IMPLEMENTER_BIT, 544 (vfpsid & FPSID_ARCH_MASK) >> FPSID_ARCH_BIT, 545 (vfpsid & FPSID_PART_MASK) >> FPSID_PART_BIT, 546 (vfpsid & FPSID_VARIANT_MASK) >> FPSID_VARIANT_BIT, 547 (vfpsid & FPSID_REV_MASK) >> FPSID_REV_BIT); 548 549 vfp_vector = vfp_support_entry; 550 551 thread_register_notifier(&vfp_notifier_block); 552 vfp_pm_init(); 553 554 /* 555 * We detected VFP, and the support code is 556 * in place; report VFP support to userspace. 557 */ 558 elf_hwcap |= HWCAP_VFP; 559 #ifdef CONFIG_VFPv3 560 if (VFP_arch >= 2) { 561 elf_hwcap |= HWCAP_VFPv3; 562 563 /* 564 * Check for VFPv3 D16. CPUs in this configuration 565 * only have 16 x 64bit registers. 566 */ 567 if (((fmrx(MVFR0) & MVFR0_A_SIMD_MASK)) == 1) 568 elf_hwcap |= HWCAP_VFPv3D16; 569 } 570 #endif 571 #ifdef CONFIG_NEON 572 /* 573 * Check for the presence of the Advanced SIMD 574 * load/store instructions, integer and single 575 * precision floating point operations. Only check 576 * for NEON if the hardware has the MVFR registers. 577 */ 578 if ((read_cpuid_id() & 0x000f0000) == 0x000f0000) { 579 if ((fmrx(MVFR1) & 0x000fff00) == 0x00011100) 580 elf_hwcap |= HWCAP_NEON; 581 } 582 #endif 583 } 584 return 0; 585 } 586 587 late_initcall(vfp_init); 588