arch/arm/vfp/vfpmodule.c

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