ARM: shmobile: Check r8a7791 MD21 at SMP boot

On r8a7791 the hardware boot mode bit MD21 indicates if hardware
debug mode is enabled or not. In case hardware debug mode is enabled
print a warning and refrain from booting secondary CPU cores.

Without this patch Koelsch with SW8-4 set to OFF will hang at SMP boot.

Signed-off-by: Magnus Damm <damm@opensource.se>
Tested-by: Geert Uytterhoeven <geert@linux-m68k.org>
Signed-off-by: Simon Horman <horms+renesas@verge.net.au>

diff --git a/arch/arm/mach-shmobile/smp-r8a7791.c b/arch/arm/mach-shmobile/smp-r8a7791.c
index 2df5bd190fe4ce3ca4f40a8bd119d45af4aac1a1..ec979529f30f5b00c3cc94030d67fb5b27efb9c8 100644 (file)
--- a/arch/arm/mach-shmobile/smp-r8a7791.c
+++ b/arch/arm/mach-shmobile/smp-r8a7791.c
@@ -20,6 +20,7 @@
 #include <asm/smp_plat.h>
 #include <mach/common.h>
 #include <mach/r8a7791.h>
+#include <mach/rcar-gen2.h>
 
 #define RST            0xe6160000
 #define CA15BAR                0x0020
@@ -51,9 +52,21 @@ static void __init r8a7791_smp_prepare_cpus(unsigned int max_cpus)
        iounmap(p);
 }
 
+static int r8a7791_smp_boot_secondary(unsigned int cpu,
+                                     struct task_struct *idle)
+{
+       /* Error out when hardware debug mode is enabled */
+       if (rcar_gen2_read_mode_pins() & BIT(21)) {
+               pr_warn("Unable to boot CPU%u when MD21 is set\n", cpu);
+               return -ENOTSUPP;
+       }
+
+       return shmobile_smp_apmu_boot_secondary(cpu, idle);
+}
+
 struct smp_operations r8a7791_smp_ops __initdata = {
        .smp_prepare_cpus       = r8a7791_smp_prepare_cpus,
-       .smp_boot_secondary     = shmobile_smp_apmu_boot_secondary,
+       .smp_boot_secondary     = r8a7791_smp_boot_secondary,
 #ifdef CONFIG_HOTPLUG_CPU
        .cpu_disable            = shmobile_smp_cpu_disable,
        .cpu_die                = shmobile_smp_apmu_cpu_die,