1 files changed, 25 insertions, 3 deletions

diff --git a/arch/arm/mm/proc-v7.S b/arch/arm/mm/proc-v7.S
index 195731d3813b..b74ea60891d5 100644
--- a/arch/arm/mm/proc-v7.S
+++ b/arch/arm/mm/proc-v7.S
@@ -169,9 +169,31 @@ ENDPROC(cpu_pj4b_do_idle)
         globl_equ       cpu_pj4b_do_idle,       cpu_v7_do_idle
 #endif
         globl_equ       cpu_pj4b_dcache_clean_area,     cpu_v7_dcache_clean_area
-        globl_equ       cpu_pj4b_do_suspend,    cpu_v7_do_suspend
-        globl_equ       cpu_pj4b_do_resume,     cpu_v7_do_resume
-        globl_equ       cpu_pj4b_suspend_size,  cpu_v7_suspend_size
+#ifdef CONFIG_ARM_CPU_SUSPEND
+ENTRY(cpu_pj4b_do_suspend)
+        stmfd   sp!, {r6 - r10}
+        mrc     p15, 1, r6, c15, c1, 0  @ save CP15 - extra features
+        mrc     p15, 1, r7, c15, c2, 0  @ save CP15 - Aux Func Modes Ctrl 0
+        mrc     p15, 1, r8, c15, c1, 2  @ save CP15 - Aux Debug Modes Ctrl 2
+        mrc     p15, 1, r9, c15, c1, 1  @ save CP15 - Aux Debug Modes Ctrl 1
+        mrc     p15, 0, r10, c9, c14, 0  @ save CP15 - PMC
+        stmia   r0!, {r6 - r10}
+        ldmfd   sp!, {r6 - r10}
+        b cpu_v7_do_suspend
+ENDPROC(cpu_pj4b_do_suspend)
+
+ENTRY(cpu_pj4b_do_resume)
+        ldmia   r0!, {r6 - r10}
+        mcr     p15, 1, r6, c15, c1, 0  @ save CP15 - extra features
+        mcr     p15, 1, r7, c15, c2, 0  @ save CP15 - Aux Func Modes Ctrl 0
+        mcr     p15, 1, r8, c15, c1, 2  @ save CP15 - Aux Debug Modes Ctrl 2
+        mcr     p15, 1, r9, c15, c1, 1  @ save CP15 - Aux Debug Modes Ctrl 1
+        mcr     p15, 0, r10, c9, c14, 0  @ save CP15 - PMC
+        b cpu_v7_do_resume
+ENDPROC(cpu_pj4b_do_resume)
+#endif
+.globl  cpu_pj4b_suspend_size
+.equ    cpu_pj4b_suspend_size, 4 * 14
 
 #endif