arch/arm/vfp/vfpmodule.c

   1 // SPDX-License-Identifier: GPL-2.0-only
   2 /*
   3  *  linux/arch/arm/vfp/vfpmodule.c
   4  *
   5  *  Copyright (C) 2004 ARM Limited.
   6  *  Written by Deep Blue Solutions Limited.
   7  */
   8 #include <linux/types.h>
   9 #include <linux/cpu.h>
  10 #include <linux/cpu_pm.h>
  11 #include <linux/hardirq.h>
  12 #include <linux/kernel.h>
  13 #include <linux/notifier.h>
  14 #include <linux/signal.h>
  15 #include <linux/sched/signal.h>
  16 #include <linux/smp.h>
  17 #include <linux/init.h>
  18 #include <linux/uaccess.h>
  19 #include <linux/user.h>
  20 #include <linux/export.h>
  21
  22 #include <asm/cp15.h>
  23 #include <asm/cputype.h>
  24 #include <asm/system_info.h>
  25 #include <asm/thread_notify.h>
  26 #include <asm/traps.h>
  27 #include <asm/vfp.h>
  28
  29 #include "vfpinstr.h"
  30 #include "vfp.h"
  31
  32 /*
  33  * Our undef handlers (in entry.S)
  34  */
  35 asmlinkage void vfp_support_entry(u32, void *, u32, u32);
  36
  37 static bool have_vfp __ro_after_init;
  38
  39 /*
  40  * Dual-use variable.
  41  * Used in startup: set to non-zero if VFP checks fail
  42  * After startup, holds VFP architecture
  43  */
  44 static unsigned int __initdata VFP_arch;
  45
  46 /*
  47  * The pointer to the vfpstate structure of the thread which currently
  48  * owns the context held in the VFP hardware, or NULL if the hardware
  49  * context is invalid.
  50  *
  51  * For UP, this is sufficient to tell which thread owns the VFP context.
  52  * However, for SMP, we also need to check the CPU number stored in the
  53  * saved state too to catch migrations.
  54  */
  55 union vfp_state *vfp_current_hw_state[NR_CPUS];
  56
  57 /*
  58  * Is 'thread's most up to date state stored in this CPUs hardware?
  59  * Must be called from non-preemptible context.
  60  */
  61 static bool vfp_state_in_hw(unsigned int cpu, struct thread_info *thread)
  62 {
  63 #ifdef CONFIG_SMP
  64         if (thread->vfpstate.hard.cpu != cpu)
  65                 return false;
  66 #endif
  67         return vfp_current_hw_state[cpu] == &thread->vfpstate;
  68 }
  69
  70 /*
  71  * Force a reload of the VFP context from the thread structure.  We do
  72  * this by ensuring that access to the VFP hardware is disabled, and
  73  * clear vfp_current_hw_state.  Must be called from non-preemptible context.
  74  */
  75 static void vfp_force_reload(unsigned int cpu, struct thread_info *thread)
  76 {
  77         if (vfp_state_in_hw(cpu, thread)) {
  78                 fmxr(FPEXC, fmrx(FPEXC) & ~FPEXC_EN);
  79                 vfp_current_hw_state[cpu] = NULL;
  80         }
  81 #ifdef CONFIG_SMP
  82         thread->vfpstate.hard.cpu = NR_CPUS;
  83 #endif
  84 }
  85
  86 /*
  87  * Per-thread VFP initialization.
  88  */
  89 static void vfp_thread_flush(struct thread_info *thread)
  90 {
  91         union vfp_state *vfp = &thread->vfpstate;
  92         unsigned int cpu;
  93
  94         /*
  95          * Disable VFP to ensure we initialize it first.  We must ensure
  96          * that the modification of vfp_current_hw_state[] and hardware
  97          * disable are done for the same CPU and without preemption.
  98          *
  99          * Do this first to ensure that preemption won't overwrite our
 100          * state saving should access to the VFP be enabled at this point.
 101          */
 102         cpu = get_cpu();
 103         if (vfp_current_hw_state[cpu] == vfp)
 104                 vfp_current_hw_state[cpu] = NULL;
 105         fmxr(FPEXC, fmrx(FPEXC) & ~FPEXC_EN);
 106         put_cpu();
 107
 108         memset(vfp, 0, sizeof(union vfp_state));
 109
 110         vfp->hard.fpexc = FPEXC_EN;
 111         vfp->hard.fpscr = FPSCR_ROUND_NEAREST;
 112 #ifdef CONFIG_SMP
 113         vfp->hard.cpu = NR_CPUS;
 114 #endif
 115 }
 116
 117 static void vfp_thread_exit(struct thread_info *thread)
 118 {
 119         /* release case: Per-thread VFP cleanup. */
 120         union vfp_state *vfp = &thread->vfpstate;
 121         unsigned int cpu = get_cpu();
 122
 123         if (vfp_current_hw_state[cpu] == vfp)
 124                 vfp_current_hw_state[cpu] = NULL;
 125         put_cpu();
 126 }
 127
 128 static void vfp_thread_copy(struct thread_info *thread)
 129 {
 130         struct thread_info *parent = current_thread_info();
 131
 132         vfp_sync_hwstate(parent);
 133         thread->vfpstate = parent->vfpstate;
 134 #ifdef CONFIG_SMP
 135         thread->vfpstate.hard.cpu = NR_CPUS;
 136 #endif
 137 }
 138
 139 /*
 140  * When this function is called with the following 'cmd's, the following
 141  * is true while this function is being run:
 142  *  THREAD_NOFTIFY_SWTICH:
 143  *   - the previously running thread will not be scheduled onto another CPU.
 144  *   - the next thread to be run (v) will not be running on another CPU.
 145  *   - thread->cpu is the local CPU number
 146  *   - not preemptible as we're called in the middle of a thread switch
 147  *  THREAD_NOTIFY_FLUSH:
 148  *   - the thread (v) will be running on the local CPU, so
 149  *      v === current_thread_info()
 150  *   - thread->cpu is the local CPU number at the time it is accessed,
 151  *      but may change at any time.
 152  *   - we could be preempted if tree preempt rcu is enabled, so
 153  *      it is unsafe to use thread->cpu.
 154  *  THREAD_NOTIFY_EXIT
 155  *   - we could be preempted if tree preempt rcu is enabled, so
 156  *      it is unsafe to use thread->cpu.
 157  */
 158 static int vfp_notifier(struct notifier_block *self, unsigned long cmd, void *v)
 159 {
 160         struct thread_info *thread = v;
 161         u32 fpexc;
 162 #ifdef CONFIG_SMP
 163         unsigned int cpu;
 164 #endif
 165
 166         switch (cmd) {
 167         case THREAD_NOTIFY_SWITCH:
 168                 fpexc = fmrx(FPEXC);
 169
 170 #ifdef CONFIG_SMP
 171                 cpu = thread->cpu;
 172
 173                 /*
 174                  * On SMP, if VFP is enabled, save the old state in
 175                  * case the thread migrates to a different CPU. The
 176                  * restoring is done lazily.
 177                  */
 178                 if ((fpexc & FPEXC_EN) && vfp_current_hw_state[cpu])
 179                         vfp_save_state(vfp_current_hw_state[cpu], fpexc);
 180 #endif
 181
 182                 /*
 183                  * Always disable VFP so we can lazily save/restore the
 184                  * old state.
 185                  */
 186                 fmxr(FPEXC, fpexc & ~FPEXC_EN);
 187                 break;
 188
 189         case THREAD_NOTIFY_FLUSH:
 190                 vfp_thread_flush(thread);
 191                 break;
 192
 193         case THREAD_NOTIFY_EXIT:
 194                 vfp_thread_exit(thread);
 195                 break;
 196
 197         case THREAD_NOTIFY_COPY:
 198                 vfp_thread_copy(thread);
 199                 break;
 200         }
 201
 202         return NOTIFY_DONE;
 203 }
 204
 205 static struct notifier_block vfp_notifier_block = {
 206         .notifier_call  = vfp_notifier,
 207 };
 208
 209 /*
 210  * Raise a SIGFPE for the current process.
 211  * sicode describes the signal being raised.
 212  */
 213 static void vfp_raise_sigfpe(unsigned int sicode, struct pt_regs *regs)
 214 {
 215         /*
 216          * This is the same as NWFPE, because it's not clear what
 217          * this is used for
 218          */
 219         current->thread.error_code = 0;
 220         current->thread.trap_no = 6;
 221
 222         send_sig_fault(SIGFPE, sicode,
 223                        (void __user *)(instruction_pointer(regs) - 4),
 224                        current);
 225 }
 226
 227 static void vfp_panic(char *reason, u32 inst)
 228 {
 229         int i;
 230
 231         pr_err("VFP: Error: %s\n", reason);
 232         pr_err("VFP: EXC 0x%08x SCR 0x%08x INST 0x%08x\n",
 233                 fmrx(FPEXC), fmrx(FPSCR), inst);
 234         for (i = 0; i < 32; i += 2)
 235                 pr_err("VFP: s%2u: 0x%08x s%2u: 0x%08x\n",
 236                        i, vfp_get_float(i), i+1, vfp_get_float(i+1));
 237 }
 238
 239 /*
 240  * Process bitmask of exception conditions.
 241  */
 242 static void vfp_raise_exceptions(u32 exceptions, u32 inst, u32 fpscr, struct pt_regs *regs)
 243 {
 244         int si_code = 0;
 245
 246         pr_debug("VFP: raising exceptions %08x\n", exceptions);
 247
 248         if (exceptions == VFP_EXCEPTION_ERROR) {
 249                 vfp_panic("unhandled bounce", inst);
 250                 vfp_raise_sigfpe(FPE_FLTINV, regs);
 251                 return;
 252         }
 253
 254         /*
 255          * If any of the status flags are set, update the FPSCR.
 256          * Comparison instructions always return at least one of
 257          * these flags set.
 258          */
 259         if (exceptions & (FPSCR_N|FPSCR_Z|FPSCR_C|FPSCR_V))
 260                 fpscr &= ~(FPSCR_N|FPSCR_Z|FPSCR_C|FPSCR_V);
 261
 262         fpscr |= exceptions;
 263
 264         fmxr(FPSCR, fpscr);
 265
 266 #define RAISE(stat,en,sig)                              \
 267         if (exceptions & stat && fpscr & en)            \
 268                 si_code = sig;
 269
 270         /*
 271          * These are arranged in priority order, least to highest.
 272          */
 273         RAISE(FPSCR_DZC, FPSCR_DZE, FPE_FLTDIV);
 274         RAISE(FPSCR_IXC, FPSCR_IXE, FPE_FLTRES);
 275         RAISE(FPSCR_UFC, FPSCR_UFE, FPE_FLTUND);
 276         RAISE(FPSCR_OFC, FPSCR_OFE, FPE_FLTOVF);
 277         RAISE(FPSCR_IOC, FPSCR_IOE, FPE_FLTINV);
 278
 279         if (si_code)
 280                 vfp_raise_sigfpe(si_code, regs);
 281 }
 282
 283 /*
 284  * Emulate a VFP instruction.
 285  */
 286 static u32 vfp_emulate_instruction(u32 inst, u32 fpscr, struct pt_regs *regs)
 287 {
 288         u32 exceptions = VFP_EXCEPTION_ERROR;
 289
 290         pr_debug("VFP: emulate: INST=0x%08x SCR=0x%08x\n", inst, fpscr);
 291
 292         if (INST_CPRTDO(inst)) {
 293                 if (!INST_CPRT(inst)) {
 294                         /*
 295                          * CPDO
 296                          */
 297                         if (vfp_single(inst)) {
 298                                 exceptions = vfp_single_cpdo(inst, fpscr);
 299                         } else {
 300                                 exceptions = vfp_double_cpdo(inst, fpscr);
 301                         }
 302                 } else {
 303                         /*
 304                          * A CPRT instruction can not appear in FPINST2, nor
 305                          * can it cause an exception.  Therefore, we do not
 306                          * have to emulate it.
 307                          */
 308                 }
 309         } else {
 310                 /*
 311                  * A CPDT instruction can not appear in FPINST2, nor can
 312                  * it cause an exception.  Therefore, we do not have to
 313                  * emulate it.
 314                  */
 315         }
 316         return exceptions & ~VFP_NAN_FLAG;
 317 }
 318
 319 /*
 320  * Package up a bounce condition.
 321  */
 322 void VFP_bounce(u32 trigger, u32 fpexc, struct pt_regs *regs)
 323 {
 324         u32 fpscr, orig_fpscr, fpsid, exceptions;
 325
 326         pr_debug("VFP: bounce: trigger %08x fpexc %08x\n", trigger, fpexc);
 327
 328         /*
 329          * At this point, FPEXC can have the following configuration:
 330          *
 331          *  EX DEX IXE
 332          *  0   1   x   - synchronous exception
 333          *  1   x   0   - asynchronous exception
 334          *  1   x   1   - sychronous on VFP subarch 1 and asynchronous on later
 335          *  0   0   1   - synchronous on VFP9 (non-standard subarch 1
 336          *                implementation), undefined otherwise
 337          *
 338          * Clear various bits and enable access to the VFP so we can
 339          * handle the bounce.
 340          */
 341         fmxr(FPEXC, fpexc & ~(FPEXC_EX|FPEXC_DEX|FPEXC_FP2V|FPEXC_VV|FPEXC_TRAP_MASK));
 342
 343         fpsid = fmrx(FPSID);
 344         orig_fpscr = fpscr = fmrx(FPSCR);
 345
 346         /*
 347          * Check for the special VFP subarch 1 and FPSCR.IXE bit case
 348          */
 349         if ((fpsid & FPSID_ARCH_MASK) == (1 << FPSID_ARCH_BIT)
 350             && (fpscr & FPSCR_IXE)) {
 351                 /*
 352                  * Synchronous exception, emulate the trigger instruction
 353                  */
 354                 goto emulate;
 355         }
 356
 357         if (fpexc & FPEXC_EX) {
 358 #ifndef CONFIG_CPU_FEROCEON
 359                 /*
 360                  * Asynchronous exception. The instruction is read from FPINST
 361                  * and the interrupted instruction has to be restarted.
 362                  */
 363                 trigger = fmrx(FPINST);
 364                 regs->ARM_pc -= 4;
 365 #endif
 366         } else if (!(fpexc & FPEXC_DEX)) {
 367                 /*
 368                  * Illegal combination of bits. It can be caused by an
 369                  * unallocated VFP instruction but with FPSCR.IXE set and not
 370                  * on VFP subarch 1.
 371                  */
 372                  vfp_raise_exceptions(VFP_EXCEPTION_ERROR, trigger, fpscr, regs);
 373                 goto exit;
 374         }
 375
 376         /*
 377          * Modify fpscr to indicate the number of iterations remaining.
 378          * If FPEXC.EX is 0, FPEXC.DEX is 1 and the FPEXC.VV bit indicates
 379          * whether FPEXC.VECITR or FPSCR.LEN is used.
 380          */
 381         if (fpexc & (FPEXC_EX | FPEXC_VV)) {
 382                 u32 len;
 383
 384                 len = fpexc + (1 << FPEXC_LENGTH_BIT);
 385
 386                 fpscr &= ~FPSCR_LENGTH_MASK;
 387                 fpscr |= (len & FPEXC_LENGTH_MASK) << (FPSCR_LENGTH_BIT - FPEXC_LENGTH_BIT);
 388         }
 389
 390         /*
 391          * Handle the first FP instruction.  We used to take note of the
 392          * FPEXC bounce reason, but this appears to be unreliable.
 393          * Emulate the bounced instruction instead.
 394          */
 395         exceptions = vfp_emulate_instruction(trigger, fpscr, regs);
 396         if (exceptions)
 397                 vfp_raise_exceptions(exceptions, trigger, orig_fpscr, regs);
 398
 399         /*
 400          * If there isn't a second FP instruction, exit now. Note that
 401          * the FPEXC.FP2V bit is valid only if FPEXC.EX is 1.
 402          */
 403         if ((fpexc & (FPEXC_EX | FPEXC_FP2V)) != (FPEXC_EX | FPEXC_FP2V))
 404                 goto exit;
 405
 406         /*
 407          * The barrier() here prevents fpinst2 being read
 408          * before the condition above.
 409          */
 410         barrier();
 411         trigger = fmrx(FPINST2);
 412
 413  emulate:
 414         exceptions = vfp_emulate_instruction(trigger, orig_fpscr, regs);
 415         if (exceptions)
 416                 vfp_raise_exceptions(exceptions, trigger, orig_fpscr, regs);
 417  exit:
 418         local_bh_enable();
 419 }
 420
 421 static void vfp_enable(void *unused)
 422 {
 423         u32 access;
 424
 425         BUG_ON(preemptible());
 426         access = get_copro_access();
 427
 428         /*
 429          * Enable full access to VFP (cp10 and cp11)
 430          */
 431         set_copro_access(access | CPACC_FULL(10) | CPACC_FULL(11));
 432 }
 433
 434 /* Called by platforms on which we want to disable VFP because it may not be
 435  * present on all CPUs within a SMP complex. Needs to be called prior to
 436  * vfp_init().
 437  */
 438 void __init vfp_disable(void)
 439 {
 440         if (VFP_arch) {
 441                 pr_debug("%s: should be called prior to vfp_init\n", __func__);
 442                 return;
 443         }
 444         VFP_arch = 1;
 445 }
 446
 447 #ifdef CONFIG_CPU_PM
 448 static int vfp_pm_suspend(void)
 449 {
 450         struct thread_info *ti = current_thread_info();
 451         u32 fpexc = fmrx(FPEXC);
 452
 453         /* if vfp is on, then save state for resumption */
 454         if (fpexc & FPEXC_EN) {
 455                 pr_debug("%s: saving vfp state\n", __func__);
 456                 vfp_save_state(&ti->vfpstate, fpexc);
 457
 458                 /* disable, just in case */
 459                 fmxr(FPEXC, fmrx(FPEXC) & ~FPEXC_EN);
 460         } else if (vfp_current_hw_state[ti->cpu]) {
 461 #ifndef CONFIG_SMP
 462                 fmxr(FPEXC, fpexc | FPEXC_EN);
 463                 vfp_save_state(vfp_current_hw_state[ti->cpu], fpexc);
 464                 fmxr(FPEXC, fpexc);
 465 #endif
 466         }
 467
 468         /* clear any information we had about last context state */
 469         vfp_current_hw_state[ti->cpu] = NULL;
 470
 471         return 0;
 472 }
 473
 474 static void vfp_pm_resume(void)
 475 {
 476         /* ensure we have access to the vfp */
 477         vfp_enable(NULL);
 478
 479         /* and disable it to ensure the next usage restores the state */
 480         fmxr(FPEXC, fmrx(FPEXC) & ~FPEXC_EN);
 481 }
 482
 483 static int vfp_cpu_pm_notifier(struct notifier_block *self, unsigned long cmd,
 484         void *v)
 485 {
 486         switch (cmd) {
 487         case CPU_PM_ENTER:
 488                 vfp_pm_suspend();
 489                 break;
 490         case CPU_PM_ENTER_FAILED:
 491         case CPU_PM_EXIT:
 492                 vfp_pm_resume();
 493                 break;
 494         }
 495         return NOTIFY_OK;
 496 }
 497
 498 static struct notifier_block vfp_cpu_pm_notifier_block = {
 499         .notifier_call = vfp_cpu_pm_notifier,
 500 };
 501
 502 static void vfp_pm_init(void)
 503 {
 504         cpu_pm_register_notifier(&vfp_cpu_pm_notifier_block);
 505 }
 506
 507 #else
 508 static inline void vfp_pm_init(void) { }
 509 #endif /* CONFIG_CPU_PM */
 510
 511 /*
 512  * Ensure that the VFP state stored in 'thread->vfpstate' is up to date
 513  * with the hardware state.
 514  */
 515 void vfp_sync_hwstate(struct thread_info *thread)
 516 {
 517         unsigned int cpu = get_cpu();
 518
 519         local_bh_disable();
 520
 521         if (vfp_state_in_hw(cpu, thread)) {
 522                 u32 fpexc = fmrx(FPEXC);
 523
 524                 /*
 525                  * Save the last VFP state on this CPU.
 526                  */
 527                 fmxr(FPEXC, fpexc | FPEXC_EN);
 528                 vfp_save_state(&thread->vfpstate, fpexc | FPEXC_EN);
 529                 fmxr(FPEXC, fpexc);
 530         }
 531
 532         local_bh_enable();
 533         put_cpu();
 534 }
 535
 536 /* Ensure that the thread reloads the hardware VFP state on the next use. */
 537 void vfp_flush_hwstate(struct thread_info *thread)
 538 {
 539         unsigned int cpu = get_cpu();
 540
 541         vfp_force_reload(cpu, thread);
 542
 543         put_cpu();
 544 }
 545
 546 /*
 547  * Save the current VFP state into the provided structures and prepare
 548  * for entry into a new function (signal handler).
 549  */
 550 int vfp_preserve_user_clear_hwstate(struct user_vfp *ufp,
 551                                     struct user_vfp_exc *ufp_exc)
 552 {
 553         struct thread_info *thread = current_thread_info();
 554         struct vfp_hard_struct *hwstate = &thread->vfpstate.hard;
 555
 556         /* Ensure that the saved hwstate is up-to-date. */
 557         vfp_sync_hwstate(thread);
 558
 559         /*
 560          * Copy the floating point registers. There can be unused
 561          * registers see asm/hwcap.h for details.
 562          */
 563         memcpy(&ufp->fpregs, &hwstate->fpregs, sizeof(hwstate->fpregs));
 564
 565         /*
 566          * Copy the status and control register.
 567          */
 568         ufp->fpscr = hwstate->fpscr;
 569
 570         /*
 571          * Copy the exception registers.
 572          */
 573         ufp_exc->fpexc = hwstate->fpexc;
 574         ufp_exc->fpinst = hwstate->fpinst;
 575         ufp_exc->fpinst2 = hwstate->fpinst2;
 576
 577         /* Ensure that VFP is disabled. */
 578         vfp_flush_hwstate(thread);
 579
 580         /*
 581          * As per the PCS, clear the length and stride bits for function
 582          * entry.
 583          */
 584         hwstate->fpscr &= ~(FPSCR_LENGTH_MASK | FPSCR_STRIDE_MASK);
 585         return 0;
 586 }
 587
 588 /* Sanitise and restore the current VFP state from the provided structures. */
 589 int vfp_restore_user_hwstate(struct user_vfp *ufp, struct user_vfp_exc *ufp_exc)
 590 {
 591         struct thread_info *thread = current_thread_info();
 592         struct vfp_hard_struct *hwstate = &thread->vfpstate.hard;
 593         unsigned long fpexc;
 594
 595         /* Disable VFP to avoid corrupting the new thread state. */
 596         vfp_flush_hwstate(thread);
 597
 598         /*
 599          * Copy the floating point registers. There can be unused
 600          * registers see asm/hwcap.h for details.
 601          */
 602         memcpy(&hwstate->fpregs, &ufp->fpregs, sizeof(hwstate->fpregs));
 603         /*
 604          * Copy the status and control register.
 605          */
 606         hwstate->fpscr = ufp->fpscr;
 607
 608         /*
 609          * Sanitise and restore the exception registers.
 610          */
 611         fpexc = ufp_exc->fpexc;
 612
 613         /* Ensure the VFP is enabled. */
 614         fpexc |= FPEXC_EN;
 615
 616         /* Ensure FPINST2 is invalid and the exception flag is cleared. */
 617         fpexc &= ~(FPEXC_EX | FPEXC_FP2V);
 618         hwstate->fpexc = fpexc;
 619
 620         hwstate->fpinst = ufp_exc->fpinst;
 621         hwstate->fpinst2 = ufp_exc->fpinst2;
 622
 623         return 0;
 624 }
 625
 626 /*
 627  * VFP hardware can lose all context when a CPU goes offline.
 628  * As we will be running in SMP mode with CPU hotplug, we will save the
 629  * hardware state at every thread switch.  We clear our held state when
 630  * a CPU has been killed, indicating that the VFP hardware doesn't contain
 631  * a threads VFP state.  When a CPU starts up, we re-enable access to the
 632  * VFP hardware. The callbacks below are called on the CPU which
 633  * is being offlined/onlined.
 634  */
 635 static int vfp_dying_cpu(unsigned int cpu)
 636 {
 637         vfp_current_hw_state[cpu] = NULL;
 638         return 0;
 639 }
 640
 641 static int vfp_starting_cpu(unsigned int unused)
 642 {
 643         vfp_enable(NULL);
 644         return 0;
 645 }
 646
 647 /*
 648  * Entered with:
 649  *
 650  *  r0  = instruction opcode (32-bit ARM or two 16-bit Thumb)
 651  *  r1  = thread_info pointer
 652  *  r2  = PC value to resume execution after successful emulation
 653  *  r3  = normal "successful" return address
 654  *  lr  = unrecognised instruction return address
 655  */
 656 asmlinkage void vfp_entry(u32 trigger, struct thread_info *ti, u32 resume_pc,
 657                           u32 resume_return_address)
 658 {
 659         if (unlikely(!have_vfp))
 660                 return;
 661
 662         local_bh_disable();
 663         vfp_support_entry(trigger, ti, resume_pc, resume_return_address);
 664 }
 665
 666 #ifdef CONFIG_KERNEL_MODE_NEON
 667
 668 static int vfp_kmode_exception(struct pt_regs *regs, unsigned int instr)
 669 {
 670         /*
 671          * If we reach this point, a floating point exception has been raised
 672          * while running in kernel mode. If the NEON/VFP unit was enabled at the
 673          * time, it means a VFP instruction has been issued that requires
 674          * software assistance to complete, something which is not currently
 675          * supported in kernel mode.
 676          * If the NEON/VFP unit was disabled, and the location pointed to below
 677          * is properly preceded by a call to kernel_neon_begin(), something has
 678          * caused the task to be scheduled out and back in again. In this case,
 679          * rebuilding and running with CONFIG_DEBUG_ATOMIC_SLEEP enabled should
 680          * be helpful in localizing the problem.
 681          */
 682         if (fmrx(FPEXC) & FPEXC_EN)
 683                 pr_crit("BUG: unsupported FP instruction in kernel mode\n");
 684         else
 685                 pr_crit("BUG: FP instruction issued in kernel mode with FP unit disabled\n");
 686         pr_crit("FPEXC == 0x%08x\n", fmrx(FPEXC));
 687         return 1;
 688 }
 689
 690 static struct undef_hook vfp_kmode_exception_hook[] = {{
 691         .instr_mask     = 0xfe000000,
 692         .instr_val      = 0xf2000000,
 693         .cpsr_mask      = MODE_MASK | PSR_T_BIT,
 694         .cpsr_val       = SVC_MODE,
 695         .fn             = vfp_kmode_exception,
 696 }, {
 697         .instr_mask     = 0xff100000,
 698         .instr_val      = 0xf4000000,
 699         .cpsr_mask      = MODE_MASK | PSR_T_BIT,
 700         .cpsr_val       = SVC_MODE,
 701         .fn             = vfp_kmode_exception,
 702 }, {
 703         .instr_mask     = 0xef000000,
 704         .instr_val      = 0xef000000,
 705         .cpsr_mask      = MODE_MASK | PSR_T_BIT,
 706         .cpsr_val       = SVC_MODE | PSR_T_BIT,
 707         .fn             = vfp_kmode_exception,
 708 }, {
 709         .instr_mask     = 0xff100000,
 710         .instr_val      = 0xf9000000,
 711         .cpsr_mask      = MODE_MASK | PSR_T_BIT,
 712         .cpsr_val       = SVC_MODE | PSR_T_BIT,
 713         .fn             = vfp_kmode_exception,
 714 }, {
 715         .instr_mask     = 0x0c000e00,
 716         .instr_val      = 0x0c000a00,
 717         .cpsr_mask      = MODE_MASK,
 718         .cpsr_val       = SVC_MODE,
 719         .fn             = vfp_kmode_exception,
 720 }};
 721
 722 static int __init vfp_kmode_exception_hook_init(void)
 723 {
 724         int i;
 725
 726         for (i = 0; i < ARRAY_SIZE(vfp_kmode_exception_hook); i++)
 727                 register_undef_hook(&vfp_kmode_exception_hook[i]);
 728         return 0;
 729 }
 730 subsys_initcall(vfp_kmode_exception_hook_init);
 731
 732 /*
 733  * Kernel-side NEON support functions
 734  */
 735 void kernel_neon_begin(void)
 736 {
 737         struct thread_info *thread = current_thread_info();
 738         unsigned int cpu;
 739         u32 fpexc;
 740
 741         local_bh_disable();
 742
 743         /*
 744          * Kernel mode NEON is only allowed outside of hardirq context with
 745          * preemption and softirq processing disabled. This will make sure that
 746          * the kernel mode NEON register contents never need to be preserved.
 747          */
 748         BUG_ON(in_hardirq());
 749         cpu = __smp_processor_id();
 750
 751         fpexc = fmrx(FPEXC) | FPEXC_EN;
 752         fmxr(FPEXC, fpexc);
 753
 754         /*
 755          * Save the userland NEON/VFP state. Under UP,
 756          * the owner could be a task other than 'current'
 757          */
 758         if (vfp_state_in_hw(cpu, thread))
 759                 vfp_save_state(&thread->vfpstate, fpexc);
 760 #ifndef CONFIG_SMP
 761         else if (vfp_current_hw_state[cpu] != NULL)
 762                 vfp_save_state(vfp_current_hw_state[cpu], fpexc);
 763 #endif
 764         vfp_current_hw_state[cpu] = NULL;
 765 }
 766 EXPORT_SYMBOL(kernel_neon_begin);
 767
 768 void kernel_neon_end(void)
 769 {
 770         /* Disable the NEON/VFP unit. */
 771         fmxr(FPEXC, fmrx(FPEXC) & ~FPEXC_EN);
 772         local_bh_enable();
 773 }
 774 EXPORT_SYMBOL(kernel_neon_end);
 775
 776 #endif /* CONFIG_KERNEL_MODE_NEON */
 777
 778 static int __init vfp_detect(struct pt_regs *regs, unsigned int instr)
 779 {
 780         VFP_arch = UINT_MAX;    /* mark as not present */
 781         regs->ARM_pc += 4;
 782         return 0;
 783 }
 784
 785 static struct undef_hook vfp_detect_hook __initdata = {
 786         .instr_mask     = 0x0c000e00,
 787         .instr_val      = 0x0c000a00,
 788         .cpsr_mask      = MODE_MASK,
 789         .cpsr_val       = SVC_MODE,
 790         .fn             = vfp_detect,
 791 };
 792
 793 /*
 794  * VFP support code initialisation.
 795  */
 796 static int __init vfp_init(void)
 797 {
 798         unsigned int vfpsid;
 799         unsigned int cpu_arch = cpu_architecture();
 800         unsigned int isar6;
 801
 802         /*
 803          * Enable the access to the VFP on all online CPUs so the
 804          * following test on FPSID will succeed.
 805          */
 806         if (cpu_arch >= CPU_ARCH_ARMv6)
 807                 on_each_cpu(vfp_enable, NULL, 1);
 808
 809         /*
 810          * First check that there is a VFP that we can use.
 811          * The handler is already setup to just log calls, so
 812          * we just need to read the VFPSID register.
 813          */
 814         register_undef_hook(&vfp_detect_hook);
 815         barrier();
 816         vfpsid = fmrx(FPSID);
 817         barrier();
 818         unregister_undef_hook(&vfp_detect_hook);
 819
 820         pr_info("VFP support v0.3: ");
 821         if (VFP_arch) {
 822                 pr_cont("not present\n");
 823                 return 0;
 824         /* Extract the architecture on CPUID scheme */
 825         } else if ((read_cpuid_id() & 0x000f0000) == 0x000f0000) {
 826                 VFP_arch = vfpsid & FPSID_CPUID_ARCH_MASK;
 827                 VFP_arch >>= FPSID_ARCH_BIT;
 828                 /*
 829                  * Check for the presence of the Advanced SIMD
 830                  * load/store instructions, integer and single
 831                  * precision floating point operations. Only check
 832                  * for NEON if the hardware has the MVFR registers.
 833                  */
 834                 if (IS_ENABLED(CONFIG_NEON) &&
 835                    (fmrx(MVFR1) & 0x000fff00) == 0x00011100)
 836                         elf_hwcap |= HWCAP_NEON;
 837
 838                 if (IS_ENABLED(CONFIG_VFPv3)) {
 839                         u32 mvfr0 = fmrx(MVFR0);
 840                         if (((mvfr0 & MVFR0_DP_MASK) >> MVFR0_DP_BIT) == 0x2 ||
 841                             ((mvfr0 & MVFR0_SP_MASK) >> MVFR0_SP_BIT) == 0x2) {
 842                                 elf_hwcap |= HWCAP_VFPv3;
 843                                 /*
 844                                  * Check for VFPv3 D16 and VFPv4 D16.  CPUs in
 845                                  * this configuration only have 16 x 64bit
 846                                  * registers.
 847                                  */
 848                                 if ((mvfr0 & MVFR0_A_SIMD_MASK) == 1)
 849                                         /* also v4-D16 */
 850                                         elf_hwcap |= HWCAP_VFPv3D16;
 851                                 else
 852                                         elf_hwcap |= HWCAP_VFPD32;
 853                         }
 854
 855                         if ((fmrx(MVFR1) & 0xf0000000) == 0x10000000)
 856                                 elf_hwcap |= HWCAP_VFPv4;
 857                         if (((fmrx(MVFR1) & MVFR1_ASIMDHP_MASK) >> MVFR1_ASIMDHP_BIT) == 0x2)
 858                                 elf_hwcap |= HWCAP_ASIMDHP;
 859                         if (((fmrx(MVFR1) & MVFR1_FPHP_MASK) >> MVFR1_FPHP_BIT) == 0x3)
 860                                 elf_hwcap |= HWCAP_FPHP;
 861                 }
 862
 863                 /*
 864                  * Check for the presence of Advanced SIMD Dot Product
 865                  * instructions.
 866                  */
 867                 isar6 = read_cpuid_ext(CPUID_EXT_ISAR6);
 868                 if (cpuid_feature_extract_field(isar6, 4) == 0x1)
 869                         elf_hwcap |= HWCAP_ASIMDDP;
 870                 /*
 871                  * Check for the presence of Advanced SIMD Floating point
 872                  * half-precision multiplication instructions.
 873                  */
 874                 if (cpuid_feature_extract_field(isar6, 8) == 0x1)
 875                         elf_hwcap |= HWCAP_ASIMDFHM;
 876                 /*
 877                  * Check for the presence of Advanced SIMD Bfloat16
 878                  * floating point instructions.
 879                  */
 880                 if (cpuid_feature_extract_field(isar6, 20) == 0x1)
 881                         elf_hwcap |= HWCAP_ASIMDBF16;
 882                 /*
 883                  * Check for the presence of Advanced SIMD and floating point
 884                  * Int8 matrix multiplication instructions instructions.
 885                  */
 886                 if (cpuid_feature_extract_field(isar6, 24) == 0x1)
 887                         elf_hwcap |= HWCAP_I8MM;
 888
 889         /* Extract the architecture version on pre-cpuid scheme */
 890         } else {
 891                 if (vfpsid & FPSID_NODOUBLE) {
 892                         pr_cont("no double precision support\n");
 893                         return 0;
 894                 }
 895
 896                 VFP_arch = (vfpsid & FPSID_ARCH_MASK) >> FPSID_ARCH_BIT;
 897         }
 898
 899         cpuhp_setup_state_nocalls(CPUHP_AP_ARM_VFP_STARTING,
 900                                   "arm/vfp:starting", vfp_starting_cpu,
 901                                   vfp_dying_cpu);
 902
 903         have_vfp = true;
 904
 905         thread_register_notifier(&vfp_notifier_block);
 906         vfp_pm_init();
 907
 908         /*
 909          * We detected VFP, and the support code is
 910          * in place; report VFP support to userspace.
 911          */
 912         elf_hwcap |= HWCAP_VFP;
 913
 914         pr_cont("implementor %02x architecture %d part %02x variant %x rev %x\n",
 915                 (vfpsid & FPSID_IMPLEMENTER_MASK) >> FPSID_IMPLEMENTER_BIT,
 916                 VFP_arch,
 917                 (vfpsid & FPSID_PART_MASK) >> FPSID_PART_BIT,
 918                 (vfpsid & FPSID_VARIANT_MASK) >> FPSID_VARIANT_BIT,
 919                 (vfpsid & FPSID_REV_MASK) >> FPSID_REV_BIT);
 920
 921         return 0;
 922 }
 923
 924 core_initcall(vfp_init);