1 files changed, 46 insertions, 37 deletions

diff --git a/arch/arm/plat-omap/sleep.S b/arch/arm/plat-omap/sleep.S
index 279490ce772b..9f745836f6aa 100644
--- a/arch/arm/plat-omap/sleep.S
+++ b/arch/arm/plat-omap/sleep.S
@@ -66,7 +66,7 @@ ENTRY(omap1510_idle_loop_suspend)
         @ get ARM_IDLECT2 into r2
         ldrh    r2, [r4, #ARM_IDLECT2_ASM_OFFSET & 0xff]
         mov     r5, #OMAP1510_IDLE_CLOCK_DOMAINS & 0xff
-        orr     r5,r5, #OMAP1510_IDLE_CLOCK_DOMAINS & 0xff00
+        orr     r5, r5, #OMAP1510_IDLE_CLOCK_DOMAINS & 0xff00
         strh    r5, [r4, #ARM_IDLECT2_ASM_OFFSET & 0xff]
 
         @ request ARM idle
@@ -76,7 +76,7 @@ ENTRY(omap1510_idle_loop_suspend)
         strh    r3, [r4, #ARM_IDLECT1_ASM_OFFSET & 0xff]
 
         mov     r5, #IDLE_WAIT_CYCLES & 0xff
-        orr     r5, r5, #IDLE_WAIT_CYCLES & 0xff00
+        orr     r5, r5, #IDLE_WAIT_CYCLES & 0xff00
 l_1510: subs    r5, r5, #1
         bne     l_1510
 /*
@@ -96,7 +96,7 @@ l_1510:	subs	r5, r5, #1
         strh    r2, [r4, #ARM_IDLECT2_ASM_OFFSET & 0xff]
         strh    r1, [r4, #ARM_IDLECT1_ASM_OFFSET & 0xff]
 
-        ldmfd   sp!, {r0 - r12, pc}     @ restore regs and return
+        ldmfd   sp!, {r0 - r12, pc}             @ restore regs and return
 
 ENTRY(omap1510_idle_loop_suspend_sz)
         .word   . - omap1510_idle_loop_suspend
@@ -115,8 +115,8 @@ ENTRY(omap1610_idle_loop_suspend)
         @ turn off clock domains
         @ get ARM_IDLECT2 into r2
         ldrh    r2, [r4, #ARM_IDLECT2_ASM_OFFSET & 0xff]
-        mov     r5, #OMAP1610_IDLE_CLOCK_DOMAINS & 0xff
-        orr     r5,r5, #OMAP1610_IDLE_CLOCK_DOMAINS & 0xff00
+        mov     r5, #OMAP1610_IDLECT2_SLEEP_VAL & 0xff
+        orr     r5, r5, #OMAP1610_IDLECT2_SLEEP_VAL & 0xff00
         strh    r5, [r4, #ARM_IDLECT2_ASM_OFFSET & 0xff]
 
         @ request ARM idle
@@ -126,7 +126,7 @@ ENTRY(omap1610_idle_loop_suspend)
         strh    r3, [r4, #ARM_IDLECT1_ASM_OFFSET & 0xff]
 
         mov     r5, #IDLE_WAIT_CYCLES & 0xff
-        orr     r5, r5, #IDLE_WAIT_CYCLES & 0xff00
+        orr     r5, r5, #IDLE_WAIT_CYCLES & 0xff00
 l_1610: subs    r5, r5, #1
         bne     l_1610
 /*
@@ -146,7 +146,7 @@ l_1610:	subs	r5, r5, #1
         strh    r2, [r4, #ARM_IDLECT2_ASM_OFFSET & 0xff]
         strh    r1, [r4, #ARM_IDLECT1_ASM_OFFSET & 0xff]
 
-        ldmfd   sp!, {r0 - r12, pc}     @ restore regs and return
+        ldmfd   sp!, {r0 - r12, pc}             @ restore regs and return
 
 ENTRY(omap1610_idle_loop_suspend_sz)
         .word   . - omap1610_idle_loop_suspend
@@ -208,7 +208,7 @@ ENTRY(omap1510_cpu_suspend)
 
         @ turn off clock domains
         mov     r5, #OMAP1510_IDLE_CLOCK_DOMAINS & 0xff
-        orr     r5,r5, #OMAP1510_IDLE_CLOCK_DOMAINS & 0xff00
+        orr     r5, r5, #OMAP1510_IDLE_CLOCK_DOMAINS & 0xff00
         strh    r5, [r4, #ARM_IDLECT2_ASM_OFFSET & 0xff]
 
         @ request ARM idle
@@ -217,7 +217,7 @@ ENTRY(omap1510_cpu_suspend)
         strh    r3, [r4, #ARM_IDLECT1_ASM_OFFSET & 0xff]
 
         mov     r5, #IDLE_WAIT_CYCLES & 0xff
-        orr     r5, r5, #IDLE_WAIT_CYCLES & 0xff00
+        orr     r5, r5, #IDLE_WAIT_CYCLES & 0xff00
 l_1510_2:
         subs    r5, r5, #1
         bne     l_1510_2
@@ -237,7 +237,7 @@ l_1510_2:
         strh    r0, [r4, #ARM_IDLECT1_ASM_OFFSET & 0xff]
 
         @ restore regs and return
-        ldmfd   sp!, {r0 - r12, pc}
+        ldmfd   sp!, {r0 - r12, pc}
 
 ENTRY(omap1510_cpu_suspend_sz)
         .word   . - omap1510_cpu_suspend
@@ -249,21 +249,26 @@ ENTRY(omap1610_cpu_suspend)
         @ save registers on stack
         stmfd   sp!, {r0 - r12, lr}
 
+        @ Drain write cache
+        mov     r4, #0
+        mcr     p15, 0, r0, c7, c10, 4
+        nop
+
         @ load base address of Traffic Controller
-        mov     r4, #TCMIF_ASM_BASE & 0xff000000
-        orr     r4, r4, #TCMIF_ASM_BASE & 0x00ff0000
-        orr     r4, r4, #TCMIF_ASM_BASE & 0x0000ff00
+        mov     r6, #TCMIF_ASM_BASE & 0xff000000
+        orr     r6, r6, #TCMIF_ASM_BASE & 0x00ff0000
+        orr     r6, r6, #TCMIF_ASM_BASE & 0x0000ff00
 
         @ prepare to put SDRAM into self-refresh manually
-        ldr     r5, [r4, #EMIFF_SDRAM_CONFIG_ASM_OFFSET & 0xff]
-        orr     r5, r5, #SELF_REFRESH_MODE & 0xff000000
-        orr     r5, r5, #SELF_REFRESH_MODE & 0x000000ff
-        str     r5, [r4, #EMIFF_SDRAM_CONFIG_ASM_OFFSET & 0xff]
+        ldr     r7, [r6, #EMIFF_SDRAM_CONFIG_ASM_OFFSET & 0xff]
+        orr     r9, r7, #SELF_REFRESH_MODE & 0xff000000
+        orr     r9, r9, #SELF_REFRESH_MODE & 0x000000ff
+        str     r9, [r6, #EMIFF_SDRAM_CONFIG_ASM_OFFSET & 0xff]
 
         @ prepare to put EMIFS to Sleep
-        ldr     r5, [r4, #EMIFS_CONFIG_ASM_OFFSET & 0xff]
-        orr     r5, r5, #IDLE_EMIFS_REQUEST & 0xff
-        str     r5, [r4, #EMIFS_CONFIG_ASM_OFFSET & 0xff]
+        ldr     r8, [r6, #EMIFS_CONFIG_ASM_OFFSET & 0xff]
+        orr     r9, r8, #IDLE_EMIFS_REQUEST & 0xff
+        str     r9, [r6, #EMIFS_CONFIG_ASM_OFFSET & 0xff]
 
         @ load base address of ARM_IDLECT1 and ARM_IDLECT2
         mov     r4, #CLKGEN_REG_ASM_BASE & 0xff000000
@@ -271,26 +276,22 @@ ENTRY(omap1610_cpu_suspend)
         orr     r4, r4, #CLKGEN_REG_ASM_BASE & 0x0000ff00
 
         @ turn off clock domains
-        mov     r5, #OMAP1610_IDLE_CLOCK_DOMAINS & 0xff
-        orr     r5,r5, #OMAP1610_IDLE_CLOCK_DOMAINS & 0xff00
-        strh    r5, [r4, #ARM_IDLECT2_ASM_OFFSET & 0xff]
-
-        @ work around errata of OMAP1610/5912. Enable (!) peripheral
-        @ clock to let the chip go into deep sleep
-        ldrh    r5, [r4, #ARM_IDLECT2_ASM_OFFSET & 0xff]
-        orr     r5,r5, #EN_PERCK_BIT & 0xff
+        @ do not disable PERCK (0x04)
+        mov     r5, #OMAP1610_IDLECT2_SLEEP_VAL & 0xff
+        orr     r5, r5, #OMAP1610_IDLECT2_SLEEP_VAL & 0xff00
         strh    r5, [r4, #ARM_IDLECT2_ASM_OFFSET & 0xff]
 
         @ request ARM idle
-        mov     r3, #OMAP1610_DEEP_SLEEP_REQUEST & 0xff
-        orr     r3, r3, #OMAP1610_DEEP_SLEEP_REQUEST & 0xff00
+        mov     r3, #OMAP1610_IDLECT1_SLEEP_VAL & 0xff
+        orr     r3, r3, #OMAP1610_IDLECT1_SLEEP_VAL & 0xff00
         strh    r3, [r4, #ARM_IDLECT1_ASM_OFFSET & 0xff]
 
-        mov     r5, #IDLE_WAIT_CYCLES & 0xff
-        orr     r5, r5, #IDLE_WAIT_CYCLES & 0xff00
-l_1610_2:
-        subs    r5, r5, #1
-        bne     l_1610_2
+        @ disable instruction cache
+        mrc     p15, 0, r9, c1, c0, 0
+        bic     r2, r9, #0x1000
+        mcr     p15, 0, r2, c1, c0, 0
+        nop
+
 /*
  * Let's wait for the next wake up event to wake us up. r0 can't be
  * used here because r0 holds ARM_IDLECT1
@@ -301,13 +302,21 @@ l_1610_2:
  * omap1610_cpu_suspend()'s resume point.
  *
  * It will just start executing here, so we'll restore stuff from the
- * stack, reset the ARM_IDLECT1 and ARM_IDLECT2.
+ * stack.
  */
+        @ re-enable Icache
+        mcr     p15, 0, r9, c1, c0, 0
+
+        @ reset the ARM_IDLECT1 and ARM_IDLECT2.
         strh    r1, [r4, #ARM_IDLECT2_ASM_OFFSET & 0xff]
         strh    r0, [r4, #ARM_IDLECT1_ASM_OFFSET & 0xff]
 
+        @ Restore EMIFF controls
+        str     r7, [r6, #EMIFF_SDRAM_CONFIG_ASM_OFFSET & 0xff]
+        str     r8, [r6, #EMIFS_CONFIG_ASM_OFFSET & 0xff]
+
         @ restore regs and return
-        ldmfd   sp!, {r0 - r12, pc}
+        ldmfd   sp!, {r0 - r12, pc}
 
 ENTRY(omap1610_cpu_suspend_sz)
         .word   . - omap1610_cpu_suspend