1 files changed, 136 insertions, 67 deletions

diff --git a/arch/arm64/kernel/hw_breakpoint.c b/arch/arm64/kernel/hw_breakpoint.c
index ff516f6691e4..f17f581116fc 100644
--- a/arch/arm64/kernel/hw_breakpoint.c
+++ b/arch/arm64/kernel/hw_breakpoint.c
@@ -20,6 +20,7 @@
 
 #define pr_fmt(fmt) "hw-breakpoint: " fmt
 
+#include <linux/cpu_pm.h>
 #include <linux/errno.h>
 #include <linux/hw_breakpoint.h>
 #include <linux/perf_event.h>
@@ -169,15 +170,68 @@ static enum debug_el debug_exception_level(int privilege)
         }
 }
 
-/*
- * Install a perf counter breakpoint.
+enum hw_breakpoint_ops {
+        HW_BREAKPOINT_INSTALL,
+        HW_BREAKPOINT_UNINSTALL,
+        HW_BREAKPOINT_RESTORE
+};
+
+/**
+ * hw_breakpoint_slot_setup - Find and setup a perf slot according to
+ *                            operations
+ *
+ * @slots: pointer to array of slots
+ * @max_slots: max number of slots
+ * @bp: perf_event to setup
+ * @ops: operation to be carried out on the slot
+ *
+ * Return:
+ *      slot index on success
+ *      -ENOSPC if no slot is available/matches
+ *      -EINVAL on wrong operations parameter
  */
-int arch_install_hw_breakpoint(struct perf_event *bp)
+static int hw_breakpoint_slot_setup(struct perf_event **slots, int max_slots,
+                                    struct perf_event *bp,
+                                    enum hw_breakpoint_ops ops)
+{
+        int i;
+        struct perf_event **slot;
+
+        for (i = 0; i < max_slots; ++i) {
+                slot = &slots[i];
+                switch (ops) {
+                case HW_BREAKPOINT_INSTALL:
+                        if (!*slot) {
+                                *slot = bp;
+                                return i;
+                        }
+                        break;
+                case HW_BREAKPOINT_UNINSTALL:
+                        if (*slot == bp) {
+                                *slot = NULL;
+                                return i;
+                        }
+                        break;
+                case HW_BREAKPOINT_RESTORE:
+                        if (*slot == bp)
+                                return i;
+                        break;
+                default:
+                        pr_warn_once("Unhandled hw breakpoint ops %d\n", ops);
+                        return -EINVAL;
+                }
+        }
+        return -ENOSPC;
+}
+
+static int hw_breakpoint_control(struct perf_event *bp,
+                                 enum hw_breakpoint_ops ops)
 {
         struct arch_hw_breakpoint *info = counter_arch_bp(bp);
-        struct perf_event **slot, **slots;
+        struct perf_event **slots;
         struct debug_info *debug_info = &current->thread.debug;
         int i, max_slots, ctrl_reg, val_reg, reg_enable;
+        enum debug_el dbg_el = debug_exception_level(info->ctrl.privilege);
         u32 ctrl;
 
         if (info->ctrl.type == ARM_BREAKPOINT_EXECUTE) {
@@ -196,67 +250,54 @@ int arch_install_hw_breakpoint(struct perf_event *bp)
                 reg_enable = !debug_info->wps_disabled;
         }
 
-        for (i = 0; i < max_slots; ++i) {
-                slot = &slots[i];
-
-                if (!*slot) {
-                        *slot = bp;
-                        break;
-                }
-        }
-
-        if (WARN_ONCE(i == max_slots, "Can't find any breakpoint slot"))
-                return -ENOSPC;
+        i = hw_breakpoint_slot_setup(slots, max_slots, bp, ops);
 
-        /* Ensure debug monitors are enabled at the correct exception level.  */
-        enable_debug_monitors(debug_exception_level(info->ctrl.privilege));
+        if (WARN_ONCE(i < 0, "Can't find any breakpoint slot"))
+                return i;
 
-        /* Setup the address register. */
-        write_wb_reg(val_reg, i, info->address);
+        switch (ops) {
+        case HW_BREAKPOINT_INSTALL:
+                /*
+                 * Ensure debug monitors are enabled at the correct exception
+                 * level.
+                 */
+                enable_debug_monitors(dbg_el);
+                /* Fall through */
+        case HW_BREAKPOINT_RESTORE:
+                /* Setup the address register. */
+                write_wb_reg(val_reg, i, info->address);
+
+                /* Setup the control register. */
+                ctrl = encode_ctrl_reg(info->ctrl);
+                write_wb_reg(ctrl_reg, i,
+                             reg_enable ? ctrl | 0x1 : ctrl & ~0x1);
+                break;
+        case HW_BREAKPOINT_UNINSTALL:
+                /* Reset the control register. */
+                write_wb_reg(ctrl_reg, i, 0);
 
-        /* Setup the control register. */
-        ctrl = encode_ctrl_reg(info->ctrl);
-        write_wb_reg(ctrl_reg, i, reg_enable ? ctrl | 0x1 : ctrl & ~0x1);
+                /*
+                 * Release the debug monitors for the correct exception
+                 * level.
+                 */
+                disable_debug_monitors(dbg_el);
+                break;
+        }
 
         return 0;
 }
 
-void arch_uninstall_hw_breakpoint(struct perf_event *bp)
+/*
+ * Install a perf counter breakpoint.
+ */
+int arch_install_hw_breakpoint(struct perf_event *bp)
 {
-        struct arch_hw_breakpoint *info = counter_arch_bp(bp);
-        struct perf_event **slot, **slots;
-        int i, max_slots, base;
-
-        if (info->ctrl.type == ARM_BREAKPOINT_EXECUTE) {
-                /* Breakpoint */
-                base = AARCH64_DBG_REG_BCR;
-                slots = this_cpu_ptr(bp_on_reg);
-                max_slots = core_num_brps;
-        } else {
-                /* Watchpoint */
-                base = AARCH64_DBG_REG_WCR;
-                slots = this_cpu_ptr(wp_on_reg);
-                max_slots = core_num_wrps;
-        }
-
-        /* Remove the breakpoint. */
-        for (i = 0; i < max_slots; ++i) {
-                slot = &slots[i];
-
-                if (*slot == bp) {
-                        *slot = NULL;
-                        break;
-                }
-        }
-
-        if (WARN_ONCE(i == max_slots, "Can't find any breakpoint slot"))
-                return;
-
-        /* Reset the control register. */
-        write_wb_reg(base, i, 0);
+        return hw_breakpoint_control(bp, HW_BREAKPOINT_INSTALL);
+}
 
-        /* Release the debug monitors for the correct exception level.  */
-        disable_debug_monitors(debug_exception_level(info->ctrl.privilege));
+void arch_uninstall_hw_breakpoint(struct perf_event *bp)
+{
+        hw_breakpoint_control(bp, HW_BREAKPOINT_UNINSTALL);
 }
 
 static int get_hbp_len(u8 hbp_len)
@@ -806,18 +847,36 @@ void hw_breakpoint_thread_switch(struct task_struct *next)
 /*
  * CPU initialisation.
  */
-static void reset_ctrl_regs(void *unused)
+static void hw_breakpoint_reset(void *unused)
 {
         int i;
-
-        for (i = 0; i < core_num_brps; ++i) {
-                write_wb_reg(AARCH64_DBG_REG_BCR, i, 0UL);
-                write_wb_reg(AARCH64_DBG_REG_BVR, i, 0UL);
+        struct perf_event **slots;
+        /*
+         * When a CPU goes through cold-boot, it does not have any installed
+         * slot, so it is safe to share the same function for restoring and
+         * resetting breakpoints; when a CPU is hotplugged in, it goes
+         * through the slots, which are all empty, hence it just resets control
+         * and value for debug registers.
+         * When this function is triggered on warm-boot through a CPU PM
+         * notifier some slots might be initialized; if so they are
+         * reprogrammed according to the debug slots content.
+         */
+        for (slots = this_cpu_ptr(bp_on_reg), i = 0; i < core_num_brps; ++i) {
+                if (slots[i]) {
+                        hw_breakpoint_control(slots[i], HW_BREAKPOINT_RESTORE);
+                } else {
+                        write_wb_reg(AARCH64_DBG_REG_BCR, i, 0UL);
+                        write_wb_reg(AARCH64_DBG_REG_BVR, i, 0UL);
+                }
         }
 
-        for (i = 0; i < core_num_wrps; ++i) {
-                write_wb_reg(AARCH64_DBG_REG_WCR, i, 0UL);
-                write_wb_reg(AARCH64_DBG_REG_WVR, i, 0UL);
+        for (slots = this_cpu_ptr(wp_on_reg), i = 0; i < core_num_wrps; ++i) {
+                if (slots[i]) {
+                        hw_breakpoint_control(slots[i], HW_BREAKPOINT_RESTORE);
+                } else {
+                        write_wb_reg(AARCH64_DBG_REG_WCR, i, 0UL);
+                        write_wb_reg(AARCH64_DBG_REG_WVR, i, 0UL);
+                }
         }
 }
 
@@ -827,7 +886,7 @@ static int hw_breakpoint_reset_notify(struct notifier_block *self,
 {
         int cpu = (long)hcpu;
         if (action == CPU_ONLINE)
-                smp_call_function_single(cpu, reset_ctrl_regs, NULL, 1);
+                smp_call_function_single(cpu, hw_breakpoint_reset, NULL, 1);
         return NOTIFY_OK;
 }
 
@@ -835,6 +894,14 @@ static struct notifier_block hw_breakpoint_reset_nb = {
         .notifier_call = hw_breakpoint_reset_notify,
 };
 
+#ifdef CONFIG_ARM64_CPU_SUSPEND
+extern void cpu_suspend_set_dbg_restorer(void (*hw_bp_restore)(void *));
+#else
+static inline void cpu_suspend_set_dbg_restorer(void (*hw_bp_restore)(void *))
+{
+}
+#endif
+
 /*
  * One-time initialisation.
  */
@@ -850,8 +917,8 @@ static int __init arch_hw_breakpoint_init(void)
          * 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);
+        smp_call_function(hw_breakpoint_reset, NULL, 1);
+        hw_breakpoint_reset(NULL);
 
         /* Register debug fault handlers. */
         hook_debug_fault_code(DBG_ESR_EVT_HWBP, breakpoint_handler, SIGTRAP,
@@ -861,6 +928,8 @@ static int __init arch_hw_breakpoint_init(void)
 
         /* Register hotplug notifier. */
         register_cpu_notifier(&hw_breakpoint_reset_nb);
+        /* Register cpu_suspend hw breakpoint restore hook */
+        cpu_suspend_set_dbg_restorer(hw_breakpoint_reset);
 
         return 0;
 }