From bb84ec16fc9ac705937100df83b61f33e2fbb679 Mon Sep 17 00:00:00 2001
From: Dietmar Eggemann <dietmar.eggemann@arm.com>
Date: Fri, 12 Oct 2012 09:30:07 +0100
Subject: ARM: hw_breakpoint: v7.1 self-hosted debug powerdown support

This patch introduces os save and restore mechanism for v7.1 debug and
self-hosted debuggers.
It enables the os to save DBGDSCR before powerdown and restore it when power
is restored.

The clearing of the os lock in the restore function kick-starts the debug
logic again.

The os save and restore routines are hooked into the CPU PM event notifier
chain. CPU PM events are used to save and restore per-cpu context when a
single CPU is preparing to enter or has exited a low power state.

Signed-off-by: Dietmar Eggemann <dietmar.eggemann@arm.com>
---
 arch/arm/kernel/hw_breakpoint.c | 57 +++++++++++++++++++++++++++++++++++++++++
 1 file changed, 57 insertions(+)

diff --git a/arch/arm/kernel/hw_breakpoint.c b/arch/arm/kernel/hw_breakpoint.c
index 281bf330124..eed4d0cdd74 100644
--- a/arch/arm/kernel/hw_breakpoint.c
+++ b/arch/arm/kernel/hw_breakpoint.c
@@ -28,6 +28,7 @@
 #include <linux/perf_event.h>
 #include <linux/hw_breakpoint.h>
 #include <linux/smp.h>
+#include <linux/cpu_pm.h>
 
 #include <asm/cacheflush.h>
 #include <asm/cputype.h>
@@ -42,6 +43,11 @@ static DEFINE_PER_CPU(struct perf_event *, bp_on_reg[ARM_MAX_BRP]);
 /* Watchpoint currently in use for each WRP. */
 static DEFINE_PER_CPU(struct perf_event *, wp_on_reg[ARM_MAX_WRP]);
 
+#ifdef CONFIG_CPU_PM
+/* Storage for OS Save and Restore. */
+static DEFINE_PER_CPU(u32, cpu_dscr);
+#endif
+
 /* Number of BRP/WRP registers on this CPU. */
 static int core_num_brps;
 static int core_num_wrps;
@@ -990,6 +996,55 @@ static struct notifier_block __cpuinitdata dbg_reset_nb = {
 	.notifier_call = dbg_reset_notify,
 };
 
+#ifdef CONFIG_CPU_PM
+static void os_save(int cpu)
+{
+	/* Set OS Lock. */
+	asm volatile("mcr p14, 0, %0, c1, c0, 4" : : "r" (0xC5ACCE55));
+	isb();
+
+	/* Save DSCRext. */
+	ARM_DBG_READ(c2, 2, per_cpu(cpu_dscr, cpu));
+}
+
+static void os_restore(int cpu)
+{
+	/* Restore DSCRext. */
+	ARM_DBG_WRITE(c2, 2, per_cpu(cpu_dscr, cpu));
+
+	/* Clear OS Lock. */
+	asm volatile("mcr p14, 0, %0, c1, c0, 4" : : "r" (0));
+	isb();
+}
+
+static int dbg_cpu_pm_notify(struct notifier_block *self, unsigned long action,
+				void *v)
+{
+	int cpu = smp_processor_id();
+
+	if (action == CPU_PM_ENTER)
+		os_save(cpu);
+	else if (action == CPU_PM_EXIT)
+		os_restore(cpu);
+
+	return NOTIFY_OK;
+}
+
+static struct notifier_block __cpuinitdata dbg_cpu_pm_nb = {
+	.notifier_call = dbg_cpu_pm_notify,
+};
+
+static void __init pm_init(void)
+{
+	if (get_debug_arch() == ARM_DEBUG_ARCH_V7_1)
+		cpu_pm_register_notifier(&dbg_cpu_pm_nb);
+}
+#else
+static inline void pm_init(void)
+{
+}
+#endif
+
 static int __init arch_hw_breakpoint_init(void)
 {
 	u32 dscr;
@@ -1048,6 +1103,8 @@ static int __init arch_hw_breakpoint_init(void)
 
 	/* Register hotplug notifier. */
 	register_cpu_notifier(&dbg_reset_nb);
+
+	pm_init();
 	return 0;
 }
 arch_initcall(arch_hw_breakpoint_init);
-- 
cgit v1.2.3