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