1 files changed, 48 insertions, 20 deletions

diff --git a/arch/arm/kernel/hw_breakpoint.c b/arch/arm/kernel/hw_breakpoint.c
index c9f3f0467570..44b84fe6e1b0 100644
--- a/arch/arm/kernel/hw_breakpoint.c
+++ b/arch/arm/kernel/hw_breakpoint.c
@@ -137,11 +137,10 @@ static u8 get_debug_arch(void)
         u32 didr;
 
         /* Do we implement the extended CPUID interface? */
-        if (((read_cpuid_id() >> 16) & 0xf) != 0xf) {
-                pr_warning("CPUID feature registers not supported. "
-                                "Assuming v6 debug is present.\n");
+        if (WARN_ONCE((((read_cpuid_id() >> 16) & 0xf) != 0xf),
+            "CPUID feature registers not supported. "
+            "Assuming v6 debug is present.\n"))
                 return ARM_DEBUG_ARCH_V6;
-        }
 
         ARM_DBG_READ(c0, 0, didr);
         return (didr >> 16) & 0xf;
@@ -152,6 +151,12 @@ u8 arch_get_debug_arch(void)
         return debug_arch;
 }
 
+static int debug_arch_supported(void)
+{
+        u8 arch = get_debug_arch();
+        return arch >= ARM_DEBUG_ARCH_V6 && arch <= ARM_DEBUG_ARCH_V7_ECP14;
+}
+
 /* Determine number of BRP register available. */
 static int get_num_brp_resources(void)
 {
@@ -268,6 +273,9 @@ out:
 
 int hw_breakpoint_slots(int type)
 {
+        if (!debug_arch_supported())
+                return 0;
+
         /*
          * We can be called early, so don't rely on
          * our static variables being initialised.
@@ -828,20 +836,33 @@ static int hw_breakpoint_pending(unsigned long addr, unsigned int fsr,
 /*
  * One-time initialisation.
  */
-static void reset_ctrl_regs(void *unused)
+static void reset_ctrl_regs(void *info)
 {
-        int i;
+        int i, cpu = smp_processor_id();
+        u32 dbg_power;
+        cpumask_t *cpumask = info;
 
         /*
          * v7 debug contains save and restore registers so that debug state
-         * can be maintained across low-power modes without leaving
-         * the debug logic powered up. It is IMPLEMENTATION DEFINED whether
-         * we can write to the debug registers out of reset, so we must
-         * unlock the OS Lock Access Register to avoid taking undefined
-         * instruction exceptions later on.
+         * can be maintained across low-power modes without leaving the debug
+         * logic powered up. It is IMPLEMENTATION DEFINED whether we can access
+         * the debug registers out of reset, so we must unlock the OS Lock
+         * Access Register to avoid taking undefined instruction exceptions
+         * later on.
          */
         if (debug_arch >= ARM_DEBUG_ARCH_V7_ECP14) {
                 /*
+                 * Ensure sticky power-down is clear (i.e. debug logic is
+                 * powered up).
+                 */
+                asm volatile("mrc p14, 0, %0, c1, c5, 4" : "=r" (dbg_power));
+                if ((dbg_power & 0x1) == 0) {
+                        pr_warning("CPU %d debug is powered down!\n", cpu);
+                        cpumask_or(cpumask, cpumask, cpumask_of(cpu));
+                        return;
+                }
+
+                /*
                  * Unconditionally clear the lock by writing a value
                  * other than 0xC5ACCE55 to the access register.
                  */
@@ -879,10 +900,11 @@ static struct notifier_block __cpuinitdata dbg_reset_nb = {
 static int __init arch_hw_breakpoint_init(void)
 {
         u32 dscr;
+        cpumask_t cpumask = { CPU_BITS_NONE };
 
         debug_arch = get_debug_arch();
 
-        if (debug_arch > ARM_DEBUG_ARCH_V7_ECP14) {
+        if (!debug_arch_supported()) {
                 pr_info("debug architecture 0x%x unsupported.\n", debug_arch);
                 return 0;
         }
@@ -899,18 +921,24 @@ static int __init arch_hw_breakpoint_init(void)
                 pr_info("%d breakpoint(s) reserved for watchpoint "
                                 "single-step.\n", core_num_reserved_brps);
 
+        /*
+         * Reset the breakpoint resources. We assume that a halting
+         * debugger will leave the world in a nice state for us.
+         */
+        on_each_cpu(reset_ctrl_regs, &cpumask, 1);
+        if (!cpumask_empty(&cpumask)) {
+                core_num_brps = 0;
+                core_num_reserved_brps = 0;
+                core_num_wrps = 0;
+                return 0;
+        }
+
         ARM_DBG_READ(c1, 0, dscr);
         if (dscr & ARM_DSCR_HDBGEN) {
+                max_watchpoint_len = 4;
                 pr_warning("halting debug mode enabled. Assuming maximum "
-                                "watchpoint size of 4 bytes.");
+                           "watchpoint size of %u bytes.", max_watchpoint_len);
         } else {
-                /*
-                 * Reset the breakpoint resources. We assume that a halting
-                 * debugger will leave the world in a nice state for us.
-                 */
-                smp_call_function(reset_ctrl_regs, NULL, 1);
-                reset_ctrl_regs(NULL);
-
                 /* Work out the maximum supported watchpoint length. */
                 max_watchpoint_len = get_max_wp_len();
                 pr_info("maximum watchpoint size is %u bytes.\n",