1 files changed, 14 insertions, 1 deletions

diff --git a/arch/arm/mach-shmobile/smp-r8a7791.c b/arch/arm/mach-shmobile/smp-r8a7791.c
index 2df5bd190fe4..ec979529f30f 100644
--- 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,