Merge branch 'for-linus' of git://git.kernel.org/pub/scm/linux/kernel/git/mpe/linux

[cascardo/linux.git] / arch / arm / kernel / pj4-cp0.c

diff --git a/arch/arm/kernel/pj4-cp0.c b/arch/arm/kernel/pj4-cp0.c
index fc72086..8153e36 100644 (file)
--- a/arch/arm/kernel/pj4-cp0.c
+++ b/arch/arm/kernel/pj4-cp0.c
@@ -45,7 +45,7 @@ static int iwmmxt_do(struct notifier_block *self, unsigned long cmd, void *t)
        return NOTIFY_DONE;
 }
 
-static struct notifier_block iwmmxt_notifier_block = {
+static struct notifier_block __maybe_unused iwmmxt_notifier_block = {
        .notifier_call  = iwmmxt_do,
 };
 
@@ -72,6 +72,33 @@ static void __init pj4_cp_access_write(u32 value)
                : "=r" (temp) : "r" (value));
 }
 
+static int __init pj4_get_iwmmxt_version(void)
+{
+       u32 cp_access, wcid;
+
+       cp_access = pj4_cp_access_read();
+       pj4_cp_access_write(cp_access | 0xf);
+
+       /* check if coprocessor 0 and 1 are available */
+       if ((pj4_cp_access_read() & 0xf) != 0xf) {
+               pj4_cp_access_write(cp_access);
+               return -ENODEV;
+       }
+
+       /* read iWMMXt coprocessor id register p1, c0 */
+       __asm__ __volatile__ ("mrc    p1, 0, %0, c0, c0, 0\n" : "=r" (wcid));
+
+       pj4_cp_access_write(cp_access);
+
+       /* iWMMXt v1 */
+       if ((wcid & 0xffffff00) == 0x56051000)
+               return 1;
+       /* iWMMXt v2 */
+       if ((wcid & 0xffffff00) == 0x56052000)
+               return 2;
+
+       return -EINVAL;
+}
 
 /*
  * Disable CP0/CP1 on boot, and let call_fpe() and the iWMMXt lazy
@@ -79,17 +106,26 @@ static void __init pj4_cp_access_write(u32 value)
  */
 static int __init pj4_cp0_init(void)
 {
-       u32 cp_access;
+       u32 __maybe_unused cp_access;
+       int vers;
 
        if (!cpu_is_pj4())
                return 0;
 
+       vers = pj4_get_iwmmxt_version();
+       if (vers < 0)
+               return 0;
+
+#ifndef CONFIG_IWMMXT
+       pr_info("PJ4 iWMMXt coprocessor detected, but kernel support is missing.\n");
+#else
        cp_access = pj4_cp_access_read() & ~0xf;
        pj4_cp_access_write(cp_access);
 
-       printk(KERN_INFO "PJ4 iWMMXt coprocessor enabled.\n");
+       pr_info("PJ4 iWMMXt v%d coprocessor enabled.\n", vers);
        elf_hwcap |= HWCAP_IWMMXT;
        thread_register_notifier(&iwmmxt_notifier_block);
+#endif
 
        return 0;
 }