tegra: Save CP14 registers as part of CPU suspend context

The CP14 (debug interface) registers were not being saved as part of
the CPU suspend context. This can cause attached JTAG debuggers to
lose their mind after CPU hotplug or suspend.

Change-Id: Ia9cfd8a711160fd1f0852c33e3fb72f15298de85
Reviewed-on: http://git-master/r/4281
Reviewed-by: Scott Williams <scwilliams@nvidia.com>
Tested-by: Scott Williams <scwilliams@nvidia.com>
Reviewed-by: Aleksandr Frid <afrid@nvidia.com>
Reviewed-by: Gary King <gking@nvidia.com>

4 files changed, 117 insertions, 13 deletions

diff --git a/arch/arm/mach-tegra/cortex-a9.S b/arch/arm/mach-tegra/cortex-a9.S
index b5cbc12c1f4d..9a3893f94d68 100644
--- a/arch/arm/mach-tegra/cortex-a9.S
+++ b/arch/arm/mach-tegra/cortex-a9.S
@@ -35,11 +35,8 @@
 
 /*	.section ".cpuinit.text", "ax"*/
 
-#define CONTEXT_SIZE_WORDS_SHIFT 7
-#define CONTEXT_SIZE_WORDS (1<<CONTEXT_SIZE_WORDS_SHIFT)
-
 /*
- * spooled CPU context is 512B / CPU
+ * spooled CPU context is 1KB / CPU
  */
 #define CTX_SP		0
 #define CTX_CPSR	4
@@ -106,13 +103,34 @@
 #define CTX_VFP_REGS	256
 #define CTX_VFP_SIZE	(32 * 8)
 
+#define CTX_CP14_REGS	512
+#define CTS_CP14_DSCR	512
+#define CTX_CP14_WFAR	516
+#define CTX_CP14_VCR	520
+#define CTX_CP14_CLAIM	524
+
+/* Each of the folowing is 2 32-bit registers */
+#define CTS_CP14_BKPT_0	528
+#define CTS_CP14_BKPT_1	536
+#define CTS_CP14_BKPT_2	544
+#define CTS_CP14_BKPT_3	552
+#define CTS_CP14_BKPT_4	560
+#define CTS_CP14_BKPT_5	568
+
+/* Each of the folowing is 2 32-bit registers */
+#define CTS_CP14_WPT_0	576
+#define CTS_CP14_WPT_1	584
+#define CTS_CP14_WPT_2	592
+#define CTS_CP14_WPT_3	600
+
+#include "power.h"
 #include "power-macros.S"
 
 .macro	ctx_ptr, rd, tmp
 	cpu_id	\tmp
 	mov32	\rd, tegra_context_area
 	ldr	\rd, [\rd]
-	add	\rd, \rd, \tmp, lsl #(CONTEXT_SIZE_WORDS_SHIFT+2)
+	add	\rd, \rd, \tmp, lsl #(CONTEXT_SIZE_BYTES_SHIFT)
 .endm
 
 .macro	translate, pa, va, tmp
@@ -161,7 +179,7 @@ ENTRY(__cortex_a9_save)
 
 	/* zero-out context area */
 	mov	r9, r8
-	add	r10, r8, #(CONTEXT_SIZE_WORDS*4)
+	add	r10, r8, #(CONTEXT_SIZE_BYTES)
 	mov	r0, #0
 	mov	r1, #0
 	mov	r2, #0
@@ -249,6 +267,48 @@ ENTRY(__cortex_a9_save)
 	cps	0x13			@ back to SVC
 	mov	r8, r0
 
+	/* Save CP14 debug controller context */
+	add	r9, r8, #CTX_CP14_REGS
+	mrc     p14, 0, r0, c0, c1, 0	@ DSCR
+	mrc	p14, 0, r1, c0, c6, 0	@ WFAR
+	mrc	p14, 0, r2, c0, c7, 0	@ VCR
+	mrc	p14, 0, r3, c7, c9, 6	@ CLAIM
+	stmia	r9, {r0-r3}
+
+	add	r9, r8, #CTS_CP14_BKPT_0
+	mrc	p14, 0, r2, c0, c0, 4
+	mrc	p14, 0, r3, c0, c0, 5
+	stmia	r9!, {r2-r3}     	@ BRKPT_0
+	mrc	p14, 0, r2, c0, c1, 4
+	mrc	p14, 0, r3, c0, c1, 5
+	stmia	r9!, {r2-r3}     	@ BRKPT_0
+	mrc	p14, 0, r2, c0, c2, 4
+	mrc	p14, 0, r3, c0, c2, 5
+	stmia	r9!, {r2-r3}     	@ BRKPT_0
+	mrc	p14, 0, r2, c0, c3, 4
+	mrc	p14, 0, r3, c0, c3, 5
+	stmia	r9!, {r2-r3}     	@ BRKPT_0
+	mrc	p14, 0, r2, c0, c4, 4
+	mrc	p14, 0, r3, c0, c4, 5
+	stmia	r9!, {r2-r3}     	@ BRKPT_0
+	mrc	p14, 0, r2, c0, c5, 4
+	mrc	p14, 0, r3, c0, c5, 5
+	stmia	r9!, {r2-r3}     	@ BRKPT_0
+
+	add	r9, r8, #CTS_CP14_WPT_0
+	mrc	p14, 0, r2, c0, c0, 6
+	mrc	p14, 0, r3, c0, c0, 7
+	stmia	r9!, {r2-r3}     	@ WPT_0
+	mrc	p14, 0, r2, c0, c1, 6
+	mrc	p14, 0, r3, c0, c1, 7
+	stmia	r9!, {r2-r3}     	@ WPT_0
+	mrc	p14, 0, r2, c0, c2, 6
+	mrc	p14, 0, r3, c0, c2, 7
+	stmia	r9!, {r2-r3}     	@ WPT_0
+	mrc	p14, 0, r2, c0, c3, 6
+	mrc	p14, 0, r3, c0, c3, 7
+	stmia	r9!, {r2-r3}     	@ WPT_0
+
 #ifdef CONFIG_CACHE_L2X0
 	cpu_id	r4
 	cmp	r4, #0
@@ -266,7 +326,7 @@ ENTRY(__cortex_a9_save)
 
 __cortex_a9_save_clean_cache:
 	mov	r10, r8
-	add	r9, r10, #(CONTEXT_SIZE_WORDS*4)
+	add	r9, r10, #(CONTEXT_SIZE_BYTES)
 	add	r9, r9, #(L1_CACHE_BYTES-1)
 	bic	r10, r10, #(L1_CACHE_BYTES-1)
 	bic	r9, r9, #(L1_CACHE_BYTES-1)
@@ -387,6 +447,50 @@ ENTRY(__cortex_a9_restore)
 	msr	cpsr_cxsf, r6
 	msr	spsr_cxsf, r7
 
+	/* Restore CP14 debug controller context */
+	add	r9, r8, #CTX_CP14_REGS
+	ldmia	r9, {r0-r3}
+	mcr	p14, 0, r1, c0, c6, 0	@ WFAR
+	mcr	p14, 0, r2, c0, c7, 0	@ VCR
+	mcr	p14, 0, r3, c7, c8, 6	@ CLAIM
+
+	add	r9, r8, #CTS_CP14_BKPT_0
+	ldmia	r9!, {r2-r3}		@ BRKPT_0
+	mcr	p14, 0, r2, c0, c0, 4
+	mcr	p14, 0, r3, c0, c0, 5
+	ldmia	r9!, {r2-r3}		@ BRKPT_0
+	mcr	p14, 0, r2, c0, c1, 4
+	mcr	p14, 0, r3, c0, c1, 5
+	ldmia	r9!, {r2-r3}		@ BRKPT_0
+	mcr	p14, 0, r2, c0, c2, 4
+	mcr	p14, 0, r3, c0, c2, 5
+	ldmia	r9!, {r2-r3}		@ BRKPT_0
+	mcr	p14, 0, r2, c0, c3, 4
+	mcr	p14, 0, r3, c0, c3, 5
+	ldmia	r9!, {r2-r3}		@ BRKPT_0
+	mcr	p14, 0, r2, c0, c4, 4
+	mcr	p14, 0, r3, c0, c4, 5
+	ldmia	r9!, {r2-r3}		@ BRKPT_0
+	mcr	p14, 0, r2, c0, c5, 4
+	mcr	p14, 0, r3, c0, c5, 5
+
+	add	r9, r8, #CTS_CP14_WPT_0
+	ldmia	r9!, {r2-r3}		@ WPT_0
+	mcr	p14, 0, r2, c0, c0, 6
+	mcr	p14, 0, r3, c0, c0, 7
+	ldmia	r9!, {r2-r3}		@ WPT_0
+	mcr	p14, 0, r2, c0, c1, 6
+	mcr	p14, 0, r3, c0, c1, 7
+	ldmia	r9!, {r2-r3}		@ WPT_0
+	mcr	p14, 0, r2, c0, c2, 6
+	mcr	p14, 0, r3, c0, c2, 7
+	ldmia	r9!, {r2-r3}		@ WPT_0
+	mcr	p14, 0, r2, c0, c3, 6
+	mcr	p14, 0, r3, c0, c3, 7
+	isb
+	mcr	p14, 0, r0, c0, c2, 2	@ DSCR
+	isb
+
 #ifdef CONFIG_VFPv3
 	orr	r4, lr, #0xF00000
 	mcr	p15, 0, r4, c1, c0, 2	@ enable coproc access
diff --git a/arch/arm/mach-tegra/platsmp.c b/arch/arm/mach-tegra/platsmp.c
index 20843cd1206e..113e62b27de1 100644
--- a/arch/arm/mach-tegra/platsmp.c
+++ b/arch/arm/mach-tegra/platsmp.c
@@ -231,7 +231,7 @@ void __init smp_prepare_cpus(unsigned int max_cpus)
 		max_cpus = ncores;
 
 #if defined(CONFIG_PM) || defined(CONFIG_HOTPLUG_CPU)
-	tegra_context_area = kzalloc(512 * ncores, GFP_KERNEL);
+	tegra_context_area = kzalloc(CONTEXT_SIZE_BYTES * ncores, GFP_KERNEL);
 
 	if (tegra_context_area && create_suspend_pgtable()) {
 		kfree(tegra_context_area);
diff --git a/arch/arm/mach-tegra/power.h b/arch/arm/mach-tegra/power.h
index 78f0c30990fd..57e66cca5569 100644
--- a/arch/arm/mach-tegra/power.h
+++ b/arch/arm/mach-tegra/power.h
@@ -38,6 +38,10 @@
 #define TEGRA_POWER_PMC_SHIFT		8
 #define TEGRA_POWER_PMC_MASK		0x1ff
 
+/* CPU Context area (1KB per CPU) */
+#define CONTEXT_SIZE_BYTES_SHIFT 10
+#define CONTEXT_SIZE_BYTES (1<<CONTEXT_SIZE_BYTES_SHIFT)
+
 /* layout of IRAM used for LP1 save & restore */
 #define TEGRA_IRAM_CODE_AREA		TEGRA_IRAM_BASE + SZ_4K
 #define TEGRA_IRAM_CODE_SIZE		SZ_4K
diff --git a/arch/arm/mach-tegra/tegra2_save.S b/arch/arm/mach-tegra/tegra2_save.S
index 167fe3fc2bcb..1c79be68b4a3 100644
--- a/arch/arm/mach-tegra/tegra2_save.S
+++ b/arch/arm/mach-tegra/tegra2_save.S
@@ -40,10 +40,6 @@
 
 #define TTB_FLAGS 0x6A	@ IRGN_WBWA, OC_RGN_WBWA, S, NOS
 
-#define CONTEXT_SIZE_WORDS_SHIFT 7
-#define CONTEXT_SIZE_WORDS (1<<CONTEXT_SIZE_WORDS_SHIFT)
-
-
 #define EMC_CFG				0xc
 #define EMC_ADR_CFG			0x10
 #define EMC_REFRESH			0x70
@@ -102,7 +98,7 @@ __tear_down_master:
 	tst	sp, #TEGRA_POWER_EFFECT_LP0
 	bne	__l2_clean_done
 	mov32	r0, (TEGRA_ARM_PL310_BASE-IO_CPU_PHYS+IO_CPU_VIRT)
-	add	r3, r8, #(CONTEXT_SIZE_WORDS*4)
+	add	r3, r8, #(CONTEXT_SIZE_BYTES)
 	bic	r8, r8, #0x1f
 	add	r3, r3, #0x1f
 11:	str	r8, [r0, #L2X0_CLEAN_LINE_PA]