Merge tag 'timers-urgent-2020-12-27' of git://git.kernel.org/pub/scm/linux/kernel...

[linux-2.6-microblaze.git] / arch / arm / vfp / vfpmodule.c

diff --git a/arch/arm/vfp/vfpmodule.c b/arch/arm/vfp/vfpmodule.c
index 8c9e7f9..2cb355c 100644 (file)
--- a/arch/arm/vfp/vfpmodule.c
+++ b/arch/arm/vfp/vfpmodule.c
@@ -23,6 +23,7 @@
 #include <asm/cputype.h>
 #include <asm/system_info.h>
 #include <asm/thread_notify.h>
+#include <asm/traps.h>
 #include <asm/vfp.h>
 
 #include "vfpinstr.h"
@@ -31,7 +32,6 @@
 /*
  * Our undef handlers (in entry.S)
  */
-asmlinkage void vfp_testing_entry(void);
 asmlinkage void vfp_support_entry(void);
 asmlinkage void vfp_null_entry(void);
 
@@ -42,7 +42,7 @@ asmlinkage void (*vfp_vector)(void) = vfp_null_entry;
  * Used in startup: set to non-zero if VFP checks fail
  * After startup, holds VFP architecture
  */
-unsigned int VFP_arch;
+static unsigned int __initdata VFP_arch;
 
 /*
  * The pointer to the vfpstate structure of the thread which currently
@@ -436,7 +436,7 @@ static void vfp_enable(void *unused)
  * present on all CPUs within a SMP complex. Needs to be called prior to
  * vfp_init().
  */
-void vfp_disable(void)
+void __init vfp_disable(void)
 {
        if (VFP_arch) {
                pr_debug("%s: should be called prior to vfp_init\n", __func__);
@@ -642,7 +642,9 @@ static int vfp_starting_cpu(unsigned int unused)
        return 0;
 }
 
-void vfp_kmode_exception(void)
+#ifdef CONFIG_KERNEL_MODE_NEON
+
+static int vfp_kmode_exception(struct pt_regs *regs, unsigned int instr)
 {
        /*
         * If we reach this point, a floating point exception has been raised
@@ -660,9 +662,51 @@ void vfp_kmode_exception(void)
                pr_crit("BUG: unsupported FP instruction in kernel mode\n");
        else
                pr_crit("BUG: FP instruction issued in kernel mode with FP unit disabled\n");
+       pr_crit("FPEXC == 0x%08x\n", fmrx(FPEXC));
+       return 1;
 }
 
-#ifdef CONFIG_KERNEL_MODE_NEON
+static struct undef_hook vfp_kmode_exception_hook[] = {{
+       .instr_mask     = 0xfe000000,
+       .instr_val      = 0xf2000000,
+       .cpsr_mask      = MODE_MASK | PSR_T_BIT,
+       .cpsr_val       = SVC_MODE,
+       .fn             = vfp_kmode_exception,
+}, {
+       .instr_mask     = 0xff100000,
+       .instr_val      = 0xf4000000,
+       .cpsr_mask      = MODE_MASK | PSR_T_BIT,
+       .cpsr_val       = SVC_MODE,
+       .fn             = vfp_kmode_exception,
+}, {
+       .instr_mask     = 0xef000000,
+       .instr_val      = 0xef000000,
+       .cpsr_mask      = MODE_MASK | PSR_T_BIT,
+       .cpsr_val       = SVC_MODE | PSR_T_BIT,
+       .fn             = vfp_kmode_exception,
+}, {
+       .instr_mask     = 0xff100000,
+       .instr_val      = 0xf9000000,
+       .cpsr_mask      = MODE_MASK | PSR_T_BIT,
+       .cpsr_val       = SVC_MODE | PSR_T_BIT,
+       .fn             = vfp_kmode_exception,
+}, {
+       .instr_mask     = 0x0c000e00,
+       .instr_val      = 0x0c000a00,
+       .cpsr_mask      = MODE_MASK,
+       .cpsr_val       = SVC_MODE,
+       .fn             = vfp_kmode_exception,
+}};
+
+static int __init vfp_kmode_exception_hook_init(void)
+{
+       int i;
+
+       for (i = 0; i < ARRAY_SIZE(vfp_kmode_exception_hook); i++)
+               register_undef_hook(&vfp_kmode_exception_hook[i]);
+       return 0;
+}
+subsys_initcall(vfp_kmode_exception_hook_init);
 
 /*
  * Kernel-side NEON support functions
@@ -708,6 +752,21 @@ EXPORT_SYMBOL(kernel_neon_end);
 
 #endif /* CONFIG_KERNEL_MODE_NEON */
 
+static int __init vfp_detect(struct pt_regs *regs, unsigned int instr)
+{
+       VFP_arch = UINT_MAX;    /* mark as not present */
+       regs->ARM_pc += 4;
+       return 0;
+}
+
+static struct undef_hook vfp_detect_hook __initdata = {
+       .instr_mask     = 0x0c000e00,
+       .instr_val      = 0x0c000a00,
+       .cpsr_mask      = MODE_MASK,
+       .cpsr_val       = SVC_MODE,
+       .fn             = vfp_detect,
+};
+
 /*
  * VFP support code initialisation.
  */
@@ -728,10 +787,11 @@ static int __init vfp_init(void)
         * The handler is already setup to just log calls, so
         * we just need to read the VFPSID register.
         */
-       vfp_vector = vfp_testing_entry;
+       register_undef_hook(&vfp_detect_hook);
        barrier();
        vfpsid = fmrx(FPSID);
        barrier();
+       unregister_undef_hook(&vfp_detect_hook);
        vfp_vector = vfp_null_entry;
 
        pr_info("VFP support v0.3: ");