ARM: v7: Save CP14 registers across suspend

Change-Id: I48bd9ddf9f0a65b1754560bae261d0b3faa69e06
Signed-off-by: Scott Williams <scwilliams@nvidia.com>
Signed-off-by: Dan Willemsen <dwillemsen@nvidia.com>

Rebase-Id: Rbbd2387b98c81919b3a8bf9782828c8b4ef33f45

1 files changed, 167 insertions, 5 deletions

diff --git a/arch/arm/mm/proc-v7.S b/arch/arm/mm/proc-v7.S
index e219e06558e6..26ee164fc803 100644
--- a/arch/arm/mm/proc-v7.S
+++ b/arch/arm/mm/proc-v7.S
@@ -217,10 +217,40 @@ ENDPROC(cpu_v7_set_pte_ext)
 
 /* Suspend/resume support: derived from arch/arm/mach-s5pv210/sleep.S */
 .globl	cpu_v7_suspend_size
-.equ	cpu_v7_suspend_size, 4 * 10
+.equ	cpu_v7_suspend_size, (4 * (10 + 4 + (16 * 2) + (16 * 2)))
+/* 10 CP15 registers
+ * 4 CP14 registers
+ * 16x2 CP14 breakpoint registers (maximum)
+ * 16x2 CP14 watchpoint registers (maximum)
+ */
+
+.macro  save_brkpt cm
+	mrc	p14, 0, r4, c0, \cm, 4
+	mrc	p14, 0, r5, c0, \cm, 5
+	stmia	r0!, {r4 - r5}
+.endm
+
+.macro  restore_brkpt cm
+	ldmia	r0!, {r4 - r5}
+	mcr	p14, 0, r4, c0, \cm, 4
+	mcr	p14, 0, r5, c0, \cm, 5
+.endm
+
+.macro  save_wpt cm
+	mrc	p14, 0, r4, c0, \cm, 6
+	mrc	p14, 0, r5, c0, \cm, 7
+	stmia	r0!, {r4 - r5}
+.endm
+
+.macro  restore_wpt cm
+	ldmia	r0!, {r4 - r5}
+	mcr	p14, 0, r4, c0, \cm, 6
+	mcr	p14, 0, r5, c0, \cm, 7
+.endm
+
 #ifdef CONFIG_PM_SLEEP
 ENTRY(cpu_v7_do_suspend)
-	stmfd	sp!, {r3 - r11, lr}
+	stmfd	sp!, {r0, r3 - r11, lr}
 	mrc	p15, 0, r3, c15, c0, 1	@ diag
 	mrc	p15, 0, r4, c13, c0, 0	@ FCSE/PID
 	mrc	p15, 0, r5, c13, c0, 1	@ Context ID
@@ -232,8 +262,72 @@ ENTRY(cpu_v7_do_suspend)
 	mrc	p15, 0, r9, c1, c0, 0	@ Control register
 	mrc	p15, 0, r10, c1, c0, 1	@ Auxiliary control register
 	mrc	p15, 0, r11, c1, c0, 2	@ Co-processor access control
-	stmia	r0, {r6 - r11}
-	ldmfd	sp!, {r4 - r11, pc}
+	stmia	r0!, {r6 - r11}
+
+	/* Save CP14 debug controller context */
+	mrc	p14, 0, r4, c0, c1, 0	@ DSCR
+	mrc	p14, 0, r5, c0, c6, 0	@ WFAR
+	mrc	p14, 0, r6, c0, c7, 0	@ VCR
+	mrc	p14, 0, r7, c7, c9, 6	@ CLAIM
+	stmia	r0!, {r4-r7}
+
+	mrc	p14, 0, r8, c0, c0, 0   @ read IDR
+	mov	r3, r8, lsr #24
+	and	r3, r3, #0xf            @ r3 has the number of brkpt
+	rsb	r3, r3, #0xf
+
+	/* r3 = (15 - #of brkpt) ;
+	   switch offset = r3*12 - 4 = (r3*3 - 1)<<2
+	*/
+	add	r3, r3, r3, lsl #1
+	sub	r3, r3, #1
+	add	pc, pc, r3, lsl #2
+
+	save_brkpt	c15
+	save_brkpt	c14
+	save_brkpt	c13
+	save_brkpt	c12
+	save_brkpt	c11
+	save_brkpt	c10
+	save_brkpt	c9
+	save_brkpt	c8
+	save_brkpt	c7
+	save_brkpt	c6
+	save_brkpt	c5
+	save_brkpt	c4
+	save_brkpt	c3
+	save_brkpt	c2
+	save_brkpt	c1
+	save_brkpt	c0
+
+	mov	r3, r8, lsr #28         @ r3 has the number of wpt
+	rsb	r3, r3, #0xf
+
+	/* r3 = (15 - #of wpt) ;
+	   switch offset = r3*12 - 4 = (r3*3 - 1)<<2
+	*/
+	add	r3, r3, r3, lsl #1
+	sub	r3, r3, #1
+	add	pc, pc, r3, lsl #2
+
+	save_wpt	c15
+	save_wpt	c14
+	save_wpt	c13
+	save_wpt	c12
+	save_wpt	c11
+	save_wpt	c10
+	save_wpt	c9
+	save_wpt	c8
+	save_wpt	c7
+	save_wpt	c6
+	save_wpt	c5
+	save_wpt	c4
+	save_wpt	c3
+	save_wpt	c2
+	save_wpt	c1
+	save_wpt	c0
+
+	ldmfd	sp!, {r0, r3 - r11, pc}
 ENDPROC(cpu_v7_do_suspend)
 
 ENTRY(cpu_v7_do_resume)
@@ -245,7 +339,7 @@ ENTRY(cpu_v7_do_resume)
 	mcr	p15, 0, r4, c13, c0, 0	@ FCSE/PID
 	mcr	p15, 0, r5, c13, c0, 1	@ Context ID
 	mcr	p15, 0, r6, c13, c0, 3	@ User r/o thread ID
-	ldmia	r0, {r6 - r11}
+	ldmia	r0!, {r6 - r11}
 	mcr	p15, 0, r6, c3, c0, 0	@ Domain ID
 	mcr	p15, 0, r7, c2, c0, 0	@ TTB 0
 	mcr	p15, 0, r8, c2, c0, 1	@ TTB 1
@@ -259,6 +353,74 @@ ENTRY(cpu_v7_do_resume)
 	mcr	p15, 0, r4, c10, c2, 0	@ write PRRR
 	mcr	p15, 0, r5, c10, c2, 1	@ write NMRR
 	isb
+
+	/* Restore CP14 debug controller context */
+
+	ldmia	r0!, {r2 - r5}
+	mcr	p14, 0, r3, c0, c6, 0	@ WFAR
+	mcr	p14, 0, r4, c0, c7, 0	@ VCR
+	mcr	p14, 0, r5, c7, c8, 6	@ CLAIM
+
+	mrc	p14, 0, r8, c0, c0, 0   @ read IDR
+	mov	r3, r8, lsr #24
+	and	r3, r3, #0xf            @ r3 has the number of brkpt
+	rsb	r3, r3, #0xf
+
+	/* r3 = (15 - #of wpt) ;
+	   switch offset = r3*12 - 4 = (r3*3 - 1)<<2
+	*/
+	add     r3, r3, r3, lsl #1
+	sub	r3, r3, #1
+	add	pc, pc, r3, lsl #2
+
+	restore_brkpt	c15
+	restore_brkpt	c14
+	restore_brkpt	c13
+	restore_brkpt	c12
+	restore_brkpt	c11
+	restore_brkpt	c10
+	restore_brkpt	c9
+	restore_brkpt	c8
+	restore_brkpt	c7
+	restore_brkpt	c6
+	restore_brkpt	c5
+	restore_brkpt	c4
+	restore_brkpt	c3
+	restore_brkpt	c2
+	restore_brkpt	c1
+	restore_brkpt	c0
+
+	mov	r3, r8, lsr #28         @ r3 has the number of wpt
+	rsb	r3, r3, #0xf
+
+	/* r3 = (15 - #of wpt) ;
+	   switch offset = r3*12 - 4 = (r3*3 - 1)<<2
+	*/
+	add     r3, r3, r3, lsl #1
+	sub	r3, r3, #1
+	add	pc, pc, r3, lsl #2
+
+start_restore_wpt:
+	restore_wpt	c15
+	restore_wpt	c14
+	restore_wpt	c13
+	restore_wpt	c12
+	restore_wpt	c11
+	restore_wpt	c10
+	restore_wpt	c9
+	restore_wpt	c8
+	restore_wpt	c7
+	restore_wpt	c6
+	restore_wpt	c5
+	restore_wpt	c4
+	restore_wpt	c3
+	restore_wpt	c2
+	restore_wpt	c1
+	restore_wpt	c0
+	isb
+
+	mcr	p14, 0, r2, c0, c2, 2	@ DSCR
+	isb
 	dsb
 	mov	r0, r9			@ control register
 	mov	r2, r7, lsr #14		@ get TTB0 base