1 files changed, 189 insertions, 329 deletions

diff --git a/arch/arm/mach-omap2/sleep34xx.S b/arch/arm/mach-omap2/sleep34xx.S
index 63f10669571a..f2ea1bd1c691 100644
--- a/arch/arm/mach-omap2/sleep34xx.S
+++ b/arch/arm/mach-omap2/sleep34xx.S
@@ -74,46 +74,6 @@
  * API functions
  */
 
-/*
- * The "get_*restore_pointer" functions are used to provide a
- * physical restore address where the ROM code jumps while waking
- * up from MPU OFF/OSWR state.
- * The restore pointer is stored into the scratchpad.
- */
-
-        .text
-/* Function call to get the restore pointer for resume from OFF */
-ENTRY(get_restore_pointer)
-        stmfd   sp!, {lr}       @ save registers on stack
-        adr     r0, restore
-        ldmfd   sp!, {pc}       @ restore regs and return
-ENDPROC(get_restore_pointer)
-        .align
-ENTRY(get_restore_pointer_sz)
-        .word   . - get_restore_pointer
-
-        .text
-/* Function call to get the restore pointer for 3630 resume from OFF */
-ENTRY(get_omap3630_restore_pointer)
-        stmfd   sp!, {lr}       @ save registers on stack
-        adr     r0, restore_3630
-        ldmfd   sp!, {pc}       @ restore regs and return
-ENDPROC(get_omap3630_restore_pointer)
-        .align
-ENTRY(get_omap3630_restore_pointer_sz)
-        .word   . - get_omap3630_restore_pointer
-
-        .text
-/* Function call to get the restore pointer for ES3 to resume from OFF */
-ENTRY(get_es3_restore_pointer)
-        stmfd   sp!, {lr}       @ save registers on stack
-        adr     r0, restore_es3
-        ldmfd   sp!, {pc}       @ restore regs and return
-ENDPROC(get_es3_restore_pointer)
-        .align
-ENTRY(get_es3_restore_pointer_sz)
-        .word   . - get_es3_restore_pointer
-
         .text
 /*
  * L2 cache needs to be toggled for stable OFF mode functionality on 3630.
@@ -133,7 +93,7 @@ ENDPROC(enable_omap3630_toggle_l2_on_restore)
 /* Function to call rom code to save secure ram context */
         .align  3
 ENTRY(save_secure_ram_context)
-        stmfd   sp!, {r1-r12, lr}       @ save registers on stack
+        stmfd   sp!, {r4 - r11, lr}     @ save registers on stack
         adr     r3, api_params          @ r3 points to parameters
         str     r0, [r3,#0x4]           @ r0 has sdram address
         ldr     r12, high_mask
@@ -152,7 +112,7 @@ ENTRY(save_secure_ram_context)
         nop
         nop
         nop
-        ldmfd   sp!, {r1-r12, pc}
+        ldmfd   sp!, {r4 - r11, pc}
         .align
 sram_phy_addr_mask:
         .word   SRAM_BASE_P
@@ -179,69 +139,38 @@ ENTRY(save_secure_ram_context_sz)
  *
  *
  * Notes:
- * - this code gets copied to internal SRAM at boot and after wake-up
- *   from OFF mode. The execution pointer in SRAM is _omap_sram_idle.
+ * - only the minimum set of functions gets copied to internal SRAM at boot
+ *   and after wake-up from OFF mode, cf. omap_push_sram_idle. The function
+ *   pointers in SDRAM or SRAM are called depending on the desired low power
+ *   target state.
  * - when the OMAP wakes up it continues at different execution points
  *   depending on the low power mode (non-OFF vs OFF modes),
  *   cf. 'Resume path for xxx mode' comments.
  */
         .align  3
 ENTRY(omap34xx_cpu_suspend)
-        stmfd   sp!, {r0-r12, lr}       @ save registers on stack
+        stmfd   sp!, {r4 - r11, lr}     @ save registers on stack
 
         /*
-         * r0 contains CPU context save/restore pointer in sdram
-         * r1 contains information about saving context:
+         * r0 contains information about saving context:
          *   0 - No context lost
          *   1 - Only L1 and logic lost
          *   2 - Only L2 lost (Even L1 is retained we clean it along with L2)
          *   3 - Both L1 and L2 lost and logic lost
          */
 
-        /* Directly jump to WFI is the context save is not required */
-        cmp     r1, #0x0
-        beq     omap3_do_wfi
+        /*
+         * For OFF mode: save context and jump to WFI in SDRAM (omap3_do_wfi)
+         * For non-OFF modes: jump to the WFI code in SRAM (omap3_do_wfi_sram)
+         */
+        ldr     r4, omap3_do_wfi_sram_addr
+        ldr     r5, [r4]
+        cmp     r0, #0x0                @ If no context save required,
+        bxeq    r5                      @  jump to the WFI code in SRAM
+
 
         /* Otherwise fall through to the save context code */
 save_context_wfi:
-        mov     r8, r0                  @ Store SDRAM address in r8
-        mrc     p15, 0, r5, c1, c0, 1   @ Read Auxiliary Control Register
-        mov     r4, #0x1                @ Number of parameters for restore call
-        stmia   r8!, {r4-r5}            @ Push parameters for restore call
-        mrc     p15, 1, r5, c9, c0, 2   @ Read L2 AUX ctrl register
-        stmia   r8!, {r4-r5}            @ Push parameters for restore call
-
-        /* Check what that target sleep state is from r1 */
-        cmp     r1, #0x2                @ Only L2 lost, no need to save context
-        beq     clean_caches
-
-l1_logic_lost:
-        mov     r4, sp                  @ Store sp
-        mrs     r5, spsr                @ Store spsr
-        mov     r6, lr                  @ Store lr
-        stmia   r8!, {r4-r6}
-
-        mrc     p15, 0, r4, c1, c0, 2   @ Coprocessor access control register
-        mrc     p15, 0, r5, c2, c0, 0   @ TTBR0
-        mrc     p15, 0, r6, c2, c0, 1   @ TTBR1
-        mrc     p15, 0, r7, c2, c0, 2   @ TTBCR
-        stmia   r8!, {r4-r7}
-
-        mrc     p15, 0, r4, c3, c0, 0   @ Domain access Control Register
-        mrc     p15, 0, r5, c10, c2, 0  @ PRRR
-        mrc     p15, 0, r6, c10, c2, 1  @ NMRR
-        stmia   r8!,{r4-r6}
-
-        mrc     p15, 0, r4, c13, c0, 1  @ Context ID
-        mrc     p15, 0, r5, c13, c0, 2  @ User r/w thread and process ID
-        mrc     p15, 0, r6, c12, c0, 0  @ Secure or NS vector base address
-        mrs     r7, cpsr                @ Store current cpsr
-        stmia   r8!, {r4-r7}
-
-        mrc     p15, 0, r4, c1, c0, 0   @ save control register
-        stmia   r8!, {r4}
-
-clean_caches:
         /*
          * jump out to kernel flush routine
          *  - reuse that code is better
@@ -284,7 +213,32 @@ clean_caches:
  THUMB( nop             )
         .arm
 
-omap3_do_wfi:
+        b       omap3_do_wfi
+
+/*
+ * Local variables
+ */
+omap3_do_wfi_sram_addr:
+        .word omap3_do_wfi_sram
+kernel_flush:
+        .word v7_flush_dcache_all
+
+/* ===================================
+ * == WFI instruction => Enter idle ==
+ * ===================================
+ */
+
+/*
+ * Do WFI instruction
+ * Includes the resume path for non-OFF modes
+ *
+ * This code gets copied to internal SRAM and is accessible
+ * from both SDRAM and SRAM:
+ * - executed from SRAM for non-off modes (omap3_do_wfi_sram),
+ * - executed from SDRAM for OFF mode (omap3_do_wfi).
+ */
+        .align  3
+ENTRY(omap3_do_wfi)
         ldr     r4, sdrc_power          @ read the SDRC_POWER register
         ldr     r5, [r4]                @ read the contents of SDRC_POWER
         orr     r5, r5, #0x40           @ enable self refresh on idle req
@@ -316,8 +270,86 @@ omap3_do_wfi:
         nop
         nop
         nop
-        bl wait_sdrc_ok
 
+/*
+ * This function implements the erratum ID i581 WA:
+ *  SDRC state restore before accessing the SDRAM
+ *
+ * Only used at return from non-OFF mode. For OFF
+ * mode the ROM code configures the SDRC and
+ * the DPLL before calling the restore code directly
+ * from DDR.
+ */
+
+/* Make sure SDRC accesses are ok */
+wait_sdrc_ok:
+
+/* DPLL3 must be locked before accessing the SDRC. Maybe the HW ensures this */
+        ldr     r4, cm_idlest_ckgen
+wait_dpll3_lock:
+        ldr     r5, [r4]
+        tst     r5, #1
+        beq     wait_dpll3_lock
+
+        ldr     r4, cm_idlest1_core
+wait_sdrc_ready:
+        ldr     r5, [r4]
+        tst     r5, #0x2
+        bne     wait_sdrc_ready
+        /* allow DLL powerdown upon hw idle req */
+        ldr     r4, sdrc_power
+        ldr     r5, [r4]
+        bic     r5, r5, #0x40
+        str     r5, [r4]
+
+/*
+ * PC-relative stores lead to undefined behaviour in Thumb-2: use a r7 as a
+ * base instead.
+ * Be careful not to clobber r7 when maintaing this code.
+ */
+
+is_dll_in_lock_mode:
+        /* Is dll in lock mode? */
+        ldr     r4, sdrc_dlla_ctrl
+        ldr     r5, [r4]
+        tst     r5, #0x4
+        bne     exit_nonoff_modes       @ Return if locked
+        /* wait till dll locks */
+        adr     r7, kick_counter
+wait_dll_lock_timed:
+        ldr     r4, wait_dll_lock_counter
+        add     r4, r4, #1
+        str     r4, [r7, #wait_dll_lock_counter - kick_counter]
+        ldr     r4, sdrc_dlla_status
+        /* Wait 20uS for lock */
+        mov     r6, #8
+wait_dll_lock:
+        subs    r6, r6, #0x1
+        beq     kick_dll
+        ldr     r5, [r4]
+        and     r5, r5, #0x4
+        cmp     r5, #0x4
+        bne     wait_dll_lock
+        b       exit_nonoff_modes       @ Return when locked
+
+        /* disable/reenable DLL if not locked */
+kick_dll:
+        ldr     r4, sdrc_dlla_ctrl
+        ldr     r5, [r4]
+        mov     r6, r5
+        bic     r6, #(1<<3)             @ disable dll
+        str     r6, [r4]
+        dsb
+        orr     r6, r6, #(1<<3)         @ enable dll
+        str     r6, [r4]
+        dsb
+        ldr     r4, kick_counter
+        add     r4, r4, #1
+        str     r4, [r7]                @ kick_counter
+        b       wait_dll_lock_timed
+
+exit_nonoff_modes:
+        /* Re-enable C-bit if needed */
         mrc     p15, 0, r0, c1, c0, 0
         tst     r0, #(1 << 2)           @ Check C bit enabled?
         orreq   r0, r0, #(1 << 2)       @ Enable the C bit if cleared
@@ -329,7 +361,32 @@ omap3_do_wfi:
  * == Exit point from non-OFF modes ==
  * ===================================
  */
-        ldmfd   sp!, {r0-r12, pc}       @ restore regs and return
+        ldmfd   sp!, {r4 - r11, pc}     @ restore regs and return
+
+/*
+ * Local variables
+ */
+sdrc_power:
+        .word   SDRC_POWER_V
+cm_idlest1_core:
+        .word   CM_IDLEST1_CORE_V
+cm_idlest_ckgen:
+        .word   CM_IDLEST_CKGEN_V
+sdrc_dlla_status:
+        .word   SDRC_DLLA_STATUS_V
+sdrc_dlla_ctrl:
+        .word   SDRC_DLLA_CTRL_V
+        /*
+         * When exporting to userspace while the counters are in SRAM,
+         * these 2 words need to be at the end to facilitate retrival!
+         */
+kick_counter:
+        .word   0
+wait_dll_lock_counter:
+        .word   0
+
+ENTRY(omap3_do_wfi_sz)
+        .word   . - omap3_do_wfi
 
 
 /*
@@ -346,13 +403,17 @@ omap3_do_wfi:
  *  restore_es3: applies to 34xx >= ES3.0
  *  restore_3630: applies to 36xx
  *  restore: common code for 3xxx
+ *
+ * Note: when back from CORE and MPU OFF mode we are running
+ *  from SDRAM, without MMU, without the caches and prediction.
+ *  Also the SRAM content has been cleared.
  */
-restore_es3:
+ENTRY(omap3_restore_es3)
         ldr     r5, pm_prepwstst_core_p
         ldr     r4, [r5]
         and     r4, r4, #0x3
         cmp     r4, #0x0        @ Check if previous power state of CORE is OFF
-        bne     restore
+        bne     omap3_restore   @ Fall through to OMAP3 common code
         adr     r0, es3_sdrc_fix
         ldr     r1, sram_base
         ldr     r2, es3_sdrc_fix_sz
@@ -364,35 +425,32 @@ copy_to_sram:
         bne     copy_to_sram
         ldr     r1, sram_base
         blx     r1
-        b       restore
+        b       omap3_restore   @ Fall through to OMAP3 common code
+ENDPROC(omap3_restore_es3)
 
-restore_3630:
+ENTRY(omap3_restore_3630)
         ldr     r1, pm_prepwstst_core_p
         ldr     r2, [r1]
         and     r2, r2, #0x3
         cmp     r2, #0x0        @ Check if previous power state of CORE is OFF
-        bne     restore
+        bne     omap3_restore   @ Fall through to OMAP3 common code
         /* Disable RTA before giving control */
         ldr     r1, control_mem_rta
         mov     r2, #OMAP36XX_RTA_DISABLE
         str     r2, [r1]
+ENDPROC(omap3_restore_3630)
 
         /* Fall through to common code for the remaining logic */
 
-restore:
+ENTRY(omap3_restore)
         /*
-         * Check what was the reason for mpu reset and store the reason in r9:
-         *  0 - No context lost
-         *  1 - Only L1 and logic lost
-         *  2 - Only L2 lost - In this case, we wont be here
-         *  3 - Both L1 and L2 lost
+         * Read the pwstctrl register to check the reason for mpu reset.
+         * This tells us what was lost.
          */
         ldr     r1, pm_pwstctrl_mpu
         ldr     r2, [r1]
         and     r2, r2, #0x3
         cmp     r2, #0x0        @ Check if target power state was OFF or RET
-        moveq   r9, #0x3        @ MPU OFF => L1 and L2 lost
-        movne   r9, #0x1        @ Only L1 and L2 lost => avoid L2 invalidation
         bne     logic_l1_restore
 
         ldr     r0, l2dis_3630
@@ -471,115 +529,39 @@ logic_l1_restore:
         orr     r1, r1, #2              @ re-enable L2 cache
         mcr     p15, 0, r1, c1, c0, 1
 skipl2reen:
-        mov     r1, #0
-        /*
-         * Invalidate all instruction caches to PoU
-         * and flush branch target cache
-         */
-        mcr     p15, 0, r1, c7, c5, 0
 
-        ldr     r4, scratchpad_base
-        ldr     r3, [r4,#0xBC]
-        adds    r3, r3, #16
-
-        ldmia   r3!, {r4-r6}
-        mov     sp, r4                  @ Restore sp
-        msr     spsr_cxsf, r5           @ Restore spsr
-        mov     lr, r6                  @ Restore lr
-
-        ldmia   r3!, {r4-r7}
-        mcr     p15, 0, r4, c1, c0, 2   @ Coprocessor access Control Register
-        mcr     p15, 0, r5, c2, c0, 0   @ TTBR0
-        mcr     p15, 0, r6, c2, c0, 1   @ TTBR1
-        mcr     p15, 0, r7, c2, c0, 2   @ TTBCR
-
-        ldmia   r3!,{r4-r6}
-        mcr     p15, 0, r4, c3, c0, 0   @ Domain access Control Register
-        mcr     p15, 0, r5, c10, c2, 0  @ PRRR
-        mcr     p15, 0, r6, c10, c2, 1  @ NMRR
-
-
-        ldmia   r3!,{r4-r7}
-        mcr     p15, 0, r4, c13, c0, 1  @ Context ID
-        mcr     p15, 0, r5, c13, c0, 2  @ User r/w thread and process ID
-        mrc     p15, 0, r6, c12, c0, 0  @ Secure or NS vector base address
-        msr     cpsr, r7                @ store cpsr
-
-        /* Enabling MMU here */
-        mrc     p15, 0, r7, c2, c0, 2   @ Read TTBRControl
-        /* Extract N (0:2) bits and decide whether to use TTBR0 or TTBR1 */
-        and     r7, #0x7
-        cmp     r7, #0x0
-        beq     usettbr0
-ttbr_error:
-        /*
-         * More work needs to be done to support N[0:2] value other than 0
-         * So looping here so that the error can be detected
-         */
-        b       ttbr_error
-usettbr0:
-        mrc     p15, 0, r2, c2, c0, 0
-        ldr     r5, ttbrbit_mask
-        and     r2, r5
-        mov     r4, pc
-        ldr     r5, table_index_mask
-        and     r4, r5                  @ r4 = 31 to 20 bits of pc
-        /* Extract the value to be written to table entry */
-        ldr     r1, table_entry
-        /* r1 has the value to be written to table entry*/
-        add     r1, r1, r4
-        /* Getting the address of table entry to modify */
-        lsr     r4, #18
-        /* r2 has the location which needs to be modified */
-        add     r2, r4
-        /* Storing previous entry of location being modified */
-        ldr     r5, scratchpad_base
-        ldr     r4, [r2]
-        str     r4, [r5, #0xC0]
-        /* Modify the table entry */
-        str     r1, [r2]
-        /*
-         * Storing address of entry being modified
-         * - will be restored after enabling MMU
-         */
-        ldr     r5, scratchpad_base
-        str     r2, [r5, #0xC4]
-
-        mov     r0, #0
-        mcr     p15, 0, r0, c7, c5, 4   @ Flush prefetch buffer
-        mcr     p15, 0, r0, c7, c5, 6   @ Invalidate branch predictor array
-        mcr     p15, 0, r0, c8, c5, 0   @ Invalidate instruction TLB
-        mcr     p15, 0, r0, c8, c6, 0   @ Invalidate data TLB
-        /*
-         * Restore control register. This enables the MMU.
-         * The caches and prediction are not enabled here, they
-         * will be enabled after restoring the MMU table entry.
-         */
-        ldmia   r3!, {r4}
-        /* Store previous value of control register in scratchpad */
-        str     r4, [r5, #0xC8]
-        ldr     r2, cache_pred_disable_mask
-        and     r4, r2
-        mcr     p15, 0, r4, c1, c0, 0
-        dsb
-        isb
-        ldr     r0, =restoremmu_on
-        bx      r0
+        /* Now branch to the common CPU resume function */
+        b       cpu_resume
+ENDPROC(omap3_restore)
+
+        .ltorg
 
 /*
- * ==============================
- * == Exit point from OFF mode ==
- * ==============================
+ * Local variables
  */
-restoremmu_on:
-        ldmfd   sp!, {r0-r12, pc}       @ restore regs and return
-
+pm_prepwstst_core_p:
+        .word   PM_PREPWSTST_CORE_P
+pm_pwstctrl_mpu:
+        .word   PM_PWSTCTRL_MPU_P
+scratchpad_base:
+        .word   SCRATCHPAD_BASE_P
+sram_base:
+        .word   SRAM_BASE_P + 0x8000
+control_stat:
+        .word   CONTROL_STAT
+control_mem_rta:
+        .word   CONTROL_MEM_RTA_CTRL
+l2dis_3630:
+        .word   0
 
 /*
  * Internal functions
  */
 
-/* This function implements the erratum ID i443 WA, applies to 34xx >= ES3.0 */
+/*
+ * This function implements the erratum ID i443 WA, applies to 34xx >= ES3.0
+ * Copied to and run from SRAM in order to reconfigure the SDRC parameters.
+ */
         .text
         .align  3
 ENTRY(es3_sdrc_fix)
@@ -609,6 +591,9 @@ ENTRY(es3_sdrc_fix)
         str     r5, [r4]                @ kick off refreshes
         bx      lr
 
+/*
+ * Local variables
+ */
         .align
 sdrc_syscfg:
         .word   SDRC_SYSCONFIG_P
@@ -627,128 +612,3 @@ sdrc_manual_1:
 ENDPROC(es3_sdrc_fix)
 ENTRY(es3_sdrc_fix_sz)
         .word   . - es3_sdrc_fix
-
-/*
- * This function implements the erratum ID i581 WA:
- *  SDRC state restore before accessing the SDRAM
- *
- * Only used at return from non-OFF mode. For OFF
- * mode the ROM code configures the SDRC and
- * the DPLL before calling the restore code directly
- * from DDR.
- */
-
-/* Make sure SDRC accesses are ok */
-wait_sdrc_ok:
-
-/* DPLL3 must be locked before accessing the SDRC. Maybe the HW ensures this */
-        ldr     r4, cm_idlest_ckgen
-wait_dpll3_lock:
-        ldr     r5, [r4]
-        tst     r5, #1
-        beq     wait_dpll3_lock
-
-        ldr     r4, cm_idlest1_core
-wait_sdrc_ready:
-        ldr     r5, [r4]
-        tst     r5, #0x2
-        bne     wait_sdrc_ready
-        /* allow DLL powerdown upon hw idle req */
-        ldr     r4, sdrc_power
-        ldr     r5, [r4]
-        bic     r5, r5, #0x40
-        str     r5, [r4]
-
-/*
- * PC-relative stores lead to undefined behaviour in Thumb-2: use a r7 as a
- * base instead.
- * Be careful not to clobber r7 when maintaing this code.
- */
-
-is_dll_in_lock_mode:
-        /* Is dll in lock mode? */
-        ldr     r4, sdrc_dlla_ctrl
-        ldr     r5, [r4]
-        tst     r5, #0x4
-        bxne    lr                      @ Return if locked
-        /* wait till dll locks */
-        adr     r7, kick_counter
-wait_dll_lock_timed:
-        ldr     r4, wait_dll_lock_counter
-        add     r4, r4, #1
-        str     r4, [r7, #wait_dll_lock_counter - kick_counter]
-        ldr     r4, sdrc_dlla_status
-        /* Wait 20uS for lock */
-        mov     r6, #8
-wait_dll_lock:
-        subs    r6, r6, #0x1
-        beq     kick_dll
-        ldr     r5, [r4]
-        and     r5, r5, #0x4
-        cmp     r5, #0x4
-        bne     wait_dll_lock
-        bx      lr                      @ Return when locked
-
-        /* disable/reenable DLL if not locked */
-kick_dll:
-        ldr     r4, sdrc_dlla_ctrl
-        ldr     r5, [r4]
-        mov     r6, r5
-        bic     r6, #(1<<3)             @ disable dll
-        str     r6, [r4]
-        dsb
-        orr     r6, r6, #(1<<3)         @ enable dll
-        str     r6, [r4]
-        dsb
-        ldr     r4, kick_counter
-        add     r4, r4, #1
-        str     r4, [r7]                @ kick_counter
-        b       wait_dll_lock_timed
-
-        .align
-cm_idlest1_core:
-        .word   CM_IDLEST1_CORE_V
-cm_idlest_ckgen:
-        .word   CM_IDLEST_CKGEN_V
-sdrc_dlla_status:
-        .word   SDRC_DLLA_STATUS_V
-sdrc_dlla_ctrl:
-        .word   SDRC_DLLA_CTRL_V
-pm_prepwstst_core_p:
-        .word   PM_PREPWSTST_CORE_P
-pm_pwstctrl_mpu:
-        .word   PM_PWSTCTRL_MPU_P
-scratchpad_base:
-        .word   SCRATCHPAD_BASE_P
-sram_base:
-        .word   SRAM_BASE_P + 0x8000
-sdrc_power:
-        .word   SDRC_POWER_V
-ttbrbit_mask:
-        .word   0xFFFFC000
-table_index_mask:
-        .word   0xFFF00000
-table_entry:
-        .word   0x00000C02
-cache_pred_disable_mask:
-        .word   0xFFFFE7FB
-control_stat:
-        .word   CONTROL_STAT
-control_mem_rta:
-        .word   CONTROL_MEM_RTA_CTRL
-kernel_flush:
-        .word   v7_flush_dcache_all
-l2dis_3630:
-        .word   0
-        /*
-         * When exporting to userspace while the counters are in SRAM,
-         * these 2 words need to be at the end to facilitate retrival!
-         */
-kick_counter:
-        .word   0
-wait_dll_lock_counter:
-        .word   0
-ENDPROC(omap34xx_cpu_suspend)
-
-ENTRY(omap34xx_cpu_suspend_sz)
-        .word   . - omap34xx_cpu_suspend