From 013628b3e75a0fd474d333a0ef2250b7ad32c39a Mon Sep 17 00:00:00 2001 From: Scott Williams Date: Wed, 20 Jul 2011 19:19:53 -0700 Subject: ARM: v7: Save CP14 registers across suspend Change-Id: I48bd9ddf9f0a65b1754560bae261d0b3faa69e06 Signed-off-by: Scott Williams Signed-off-by: Dan Willemsen Rebase-Id: Rbbd2387b98c81919b3a8bf9782828c8b4ef33f45 --- arch/arm/mm/proc-v7.S | 172 ++++++++++++++++++++++++++++++++++++++++++++++++-- 1 file changed, 167 insertions(+), 5 deletions(-) (limited to 'arch/arm/mm') 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 -- cgit v1.2.3