diff --git a/arch/arm/plat-s5p/sleep.S b/arch/arm/plat-s5p/sleep.S
index 2cdae4a..dd9516b 100644
--- a/arch/arm/plat-s5p/sleep.S
+++ b/arch/arm/plat-s5p/sleep.S
@@ -44,14 +44,32 @@
 */
 	.text
 
+#ifdef CONFIG_HIBERNATION
+ENTRY(__save_processor_state)
+	mov	r1, #0
+	b	.Ls3c_cpu_save_internal
+ENDPROC(__save_processor_state)
+
+ENTRY(__restore_processor_state)
+	stmfd	sp!, { r3 - r12, lr }
+	ldr	r2, =.Ls3c_cpu_resume_internal
+	mov	r1, #1
+	str	sp, [r0, #40]		@ fixup sp in restore context
+	mov	pc, r2
+ENDPROC(__restore_processor_state)
+#endif
+
 	/* s3c_cpu_save
 	 *
 	 * entry:
 	 *	r0 = save address (virtual addr of s3c_sleep_save_phys)
-	*/
+	 *	r1 (_internal_ only) = CPU sleep trampoline (if any)
+	 */
 
 ENTRY(s3c_cpu_save)
 
+	ldr	r1, =pm_cpu_sleep	@ set trampoline
+.Ls3c_cpu_save_internal:
 	stmfd	sp!, { r3 - r12, lr }
 
 	mrc	p15, 0, r4, c13, c0, 0	@ FCSE/PID
@@ -67,11 +85,15 @@ ENTRY(s3c_cpu_save)
 
 	stmia	r0, { r3 - r13 }
 
+	mov	r4, r1
 	@@ write our state back to RAM
 	bl	s3c_pm_cb_flushcache
 
+	movs	r0, r4
+#ifdef CONFIG_HIBERNATION
+	ldmeqfd	sp!, { r3 - r12, pc }	@ if there was no trampoline, return
+#endif
 	@@ jump to final code to send system to sleep
-	ldr	r0, =pm_cpu_sleep
 	@@ldr	pc, [ r0 ]
 	ldr	r0, [ r0 ]
 	mov	pc, r0
@@ -86,6 +108,7 @@ resume_with_mmu:
 	str	r12, [r4]
 
 	ldmfd	sp!, { r3 - r12, pc }
+ENDPROC(s3c_cpu_save)
 
 	.ltorg
 
@@ -131,6 +154,7 @@ ENTRY(s3c_cpu_resume)
 	mcr	p15, 0, r1, c7, c5, 0		@@ invalidate I Cache
 
 	ldr	r0, s3c_sleep_save_phys	@ address of restore block
+.Ls3c_cpu_resume_internal:
 	ldmia	r0, { r3 - r13 }
 
 	mcr	p15, 0, r4, c13, c0, 0	@ FCSE/PID
@@ -152,6 +176,11 @@ ENTRY(s3c_cpu_resume)
 	mcr	p15, 0, r12, c10, c2, 0	@ write PRRR
 	mcr	p15, 0, r3, c10, c2, 1	@ write NMRR
 
+#ifdef CONFIG_HIBERNATION
+	cmp	r1, #0
+	bne	0f			@ only do MMU phys init
+					@ not called by __restore_processor_state
+#endif
 	/* calculate first section address into r8 */
 	mov	r4, r6
 	ldr	r5, =0x3fff
@@ -175,6 +204,7 @@ ENTRY(s3c_cpu_resume)
 	str	r10, [r4]
 
 	ldr	r2, =resume_with_mmu
+0:
 	mcr	p15, 0, r9, c1, c0, 0		@ turn on MMU, etc
 
         nop
@@ -183,6 +213,9 @@ ENTRY(s3c_cpu_resume)
         nop
         nop					@ second-to-last before mmu
 
+#ifdef CONFIG_HIBERNATION
+	ldmnefd	sp!, { r3 - r12, pc }
+#endif
 	mov	pc, r2				@ go back to virtual address
 
 	.ltorg