1 files changed, 198 insertions, 22 deletions

diff --git a/arch/arm/mach-omap2/sleep34xx.S b/arch/arm/mach-omap2/sleep34xx.S
index e5e2553e79a6..15268f8b61de 100644
--- a/arch/arm/mach-omap2/sleep34xx.S
+++ b/arch/arm/mach-omap2/sleep34xx.S
@@ -27,22 +27,35 @@
 #include <linux/linkage.h>
 #include <asm/assembler.h>
 #include <mach/io.h>
-#include <mach/control.h>
+#include <plat/control.h>
 
+#include "cm.h"
 #include "prm.h"
 #include "sdrc.h"
 
 #define PM_PREPWSTST_CORE_V     OMAP34XX_PRM_REGADDR(CORE_MOD, \
                                 OMAP3430_PM_PREPWSTST)
+#define PM_PREPWSTST_CORE_P     0x48306AE8
 #define PM_PREPWSTST_MPU_V      OMAP34XX_PRM_REGADDR(MPU_MOD, \
                                 OMAP3430_PM_PREPWSTST)
-#define PM_PWSTCTRL_MPU_P       OMAP34XX_PRM_REGADDR(MPU_MOD, PM_PWSTCTRL)
+#define PM_PWSTCTRL_MPU_P       OMAP3430_PRM_BASE + MPU_MOD + PM_PWSTCTRL
+#define CM_IDLEST1_CORE_V       OMAP34XX_CM_REGADDR(CORE_MOD, CM_IDLEST1)
+#define SRAM_BASE_P             0x40200000
+#define CONTROL_STAT            0x480022F0
 #define SCRATCHPAD_MEM_OFFS     0x310 /* Move this as correct place is
                                        * available */
-#define SCRATCHPAD_BASE_P       OMAP343X_CTRL_REGADDR(\
-                                OMAP343X_CONTROL_MEM_WKUP +\
-                                SCRATCHPAD_MEM_OFFS)
+#define SCRATCHPAD_BASE_P       (OMAP343X_CTRL_BASE + OMAP343X_CONTROL_MEM_WKUP\
+                                                + SCRATCHPAD_MEM_OFFS)
 #define SDRC_POWER_V            OMAP34XX_SDRC_REGADDR(SDRC_POWER)
+#define SDRC_SYSCONFIG_P        (OMAP343X_SDRC_BASE + SDRC_SYSCONFIG)
+#define SDRC_MR_0_P             (OMAP343X_SDRC_BASE + SDRC_MR_0)
+#define SDRC_EMR2_0_P           (OMAP343X_SDRC_BASE + SDRC_EMR2_0)
+#define SDRC_MANUAL_0_P         (OMAP343X_SDRC_BASE + SDRC_MANUAL_0)
+#define SDRC_MR_1_P             (OMAP343X_SDRC_BASE + SDRC_MR_1)
+#define SDRC_EMR2_1_P           (OMAP343X_SDRC_BASE + SDRC_EMR2_1)
+#define SDRC_MANUAL_1_P         (OMAP343X_SDRC_BASE + SDRC_MANUAL_1)
+#define SDRC_DLLA_STATUS_V      OMAP34XX_SDRC_REGADDR(SDRC_DLLA_STATUS)
+#define SDRC_DLLA_CTRL_V        OMAP34XX_SDRC_REGADDR(SDRC_DLLA_CTRL)
 
         .text
 /* Function call to get the restore pointer for resume from OFF */
@@ -51,7 +64,93 @@ ENTRY(get_restore_pointer)
         adr     r0, restore
         ldmfd   sp!, {pc}     @ restore regs and return
 ENTRY(get_restore_pointer_sz)
-        .word   . - get_restore_pointer_sz
+        .word   . - get_restore_pointer
+
+        .text
+/* Function call to get the restore pointer for 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
+ENTRY(get_es3_restore_pointer_sz)
+        .word   . - get_es3_restore_pointer
+
+ENTRY(es3_sdrc_fix)
+        ldr     r4, sdrc_syscfg         @ get config addr
+        ldr     r5, [r4]                @ get value
+        tst     r5, #0x100              @ is part access blocked
+        it      eq
+        biceq   r5, r5, #0x100          @ clear bit if set
+        str     r5, [r4]                @ write back change
+        ldr     r4, sdrc_mr_0           @ get config addr
+        ldr     r5, [r4]                @ get value
+        str     r5, [r4]                @ write back change
+        ldr     r4, sdrc_emr2_0         @ get config addr
+        ldr     r5, [r4]                @ get value
+        str     r5, [r4]                @ write back change
+        ldr     r4, sdrc_manual_0       @ get config addr
+        mov     r5, #0x2                @ autorefresh command
+        str     r5, [r4]                @ kick off refreshes
+        ldr     r4, sdrc_mr_1           @ get config addr
+        ldr     r5, [r4]                @ get value
+        str     r5, [r4]                @ write back change
+        ldr     r4, sdrc_emr2_1         @ get config addr
+        ldr     r5, [r4]                @ get value
+        str     r5, [r4]                @ write back change
+        ldr     r4, sdrc_manual_1       @ get config addr
+        mov     r5, #0x2                @ autorefresh command
+        str     r5, [r4]                @ kick off refreshes
+        bx      lr
+sdrc_syscfg:
+        .word   SDRC_SYSCONFIG_P
+sdrc_mr_0:
+        .word   SDRC_MR_0_P
+sdrc_emr2_0:
+        .word   SDRC_EMR2_0_P
+sdrc_manual_0:
+        .word   SDRC_MANUAL_0_P
+sdrc_mr_1:
+        .word   SDRC_MR_1_P
+sdrc_emr2_1:
+        .word   SDRC_EMR2_1_P
+sdrc_manual_1:
+        .word   SDRC_MANUAL_1_P
+ENTRY(es3_sdrc_fix_sz)
+        .word   . - es3_sdrc_fix
+
+/* Function to call rom code to save secure ram context */
+ENTRY(save_secure_ram_context)
+        stmfd   sp!, {r1-r12, lr}       @ save registers on stack
+save_secure_ram_debug:
+        /* b save_secure_ram_debug */   @ enable to debug save code
+        adr     r3, api_params          @ r3 points to parameters
+        str     r0, [r3,#0x4]           @ r0 has sdram address
+        ldr     r12, high_mask
+        and     r3, r3, r12
+        ldr     r12, sram_phy_addr_mask
+        orr     r3, r3, r12
+        mov     r0, #25                 @ set service ID for PPA
+        mov     r12, r0                 @ copy secure service ID in r12
+        mov     r1, #0                  @ set task id for ROM code in r1
+        mov     r2, #4                  @ set some flags in r2, r6
+        mov     r6, #0xff
+        mcr     p15, 0, r0, c7, c10, 4  @ data write barrier
+        mcr     p15, 0, r0, c7, c10, 5  @ data memory barrier
+        .word   0xE1600071              @ call SMI monitor (smi #1)
+        nop
+        nop
+        nop
+        nop
+        ldmfd   sp!, {r1-r12, pc}
+sram_phy_addr_mask:
+        .word   SRAM_BASE_P
+high_mask:
+        .word   0xffff
+api_params:
+        .word   0x4, 0x0, 0x0, 0x1, 0x1
+ENTRY(save_secure_ram_context_sz)
+        .word   . - save_secure_ram_context
+
 /*
  * Forces OMAP into idle state
  *
@@ -92,11 +191,29 @@ loop:
         nop
         nop
         nop
-        bl i_dll_wait
+        bl wait_sdrc_ok
 
         ldmfd   sp!, {r0-r12, pc}               @ restore regs and return
+restore_es3:
+        /*b restore_es3*/               @ Enable to debug restore code
+        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
+        adr     r0, es3_sdrc_fix
+        ldr     r1, sram_base
+        ldr     r2, es3_sdrc_fix_sz
+        mov     r2, r2, ror #2
+copy_to_sram:
+        ldmia   r0!, {r3}       @ val = *src
+        stmia   r1!, {r3}       @ *dst = val
+        subs    r2, r2, #0x1    @ num_words--
+        bne     copy_to_sram
+        ldr     r1, sram_base
+        blx     r1
 restore:
-        /* b restore*/  @ Enable to debug restore code
+        /* b restore*/  @ Enable to debug restore code
         /* Check what was the reason for mpu reset and store the reason in r9*/
         /* 1 - Only L1 and logic lost */
         /* 2 - Only L2 lost - In this case, we wont be here */
@@ -108,9 +225,44 @@ restore:
         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, control_stat
+        ldr     r1, [r0]
+        and     r1, #0x700
+        cmp     r1, #0x300
+        beq     l2_inv_gp
+        mov     r0, #40         @ set service ID for PPA
+        mov     r12, r0         @ copy secure Service ID in r12
+        mov     r1, #0          @ set task id for ROM code in r1
+        mov     r2, #4          @ set some flags in r2, r6
+        mov     r6, #0xff
+        adr     r3, l2_inv_api_params   @ r3 points to dummy parameters
+        mcr     p15, 0, r0, c7, c10, 4  @ data write barrier
+        mcr     p15, 0, r0, c7, c10, 5  @ data memory barrier
+        .word   0xE1600071              @ call SMI monitor (smi #1)
+        /* Write to Aux control register to set some bits */
+        mov     r0, #42         @ set service ID for PPA
+        mov     r12, r0         @ copy secure Service ID in r12
+        mov     r1, #0          @ set task id for ROM code in r1
+        mov     r2, #4          @ set some flags in r2, r6
+        mov     r6, #0xff
+        adr     r3, write_aux_control_params    @ r3 points to parameters
+        mcr     p15, 0, r0, c7, c10, 4  @ data write barrier
+        mcr     p15, 0, r0, c7, c10, 5  @ data memory barrier
+        .word   0xE1600071              @ call SMI monitor (smi #1)
+
+        b       logic_l1_restore
+l2_inv_api_params:
+        .word   0x1, 0x00
+write_aux_control_params:
+        .word   0x1, 0x72
+l2_inv_gp:
         /* Execute smi to invalidate L2 cache */
         mov r12, #0x1                         @ set up to invalide L2
-smi:    .word 0xE1600070                @ Call SMI monitor (smieq)
+smi:    .word 0xE1600070                @ Call SMI monitor (smieq)
+        /* Write to Aux control register to set some bits */
+        mov     r0, #0x72
+        mov     r12, #0x3
+        .word 0xE1600070        @ Call SMI monitor (smieq)
 logic_l1_restore:
         mov     r1, #0
         /* Invalidate all instruction caches to PoU
@@ -391,33 +543,55 @@ skip_l2_inval:
         nop
         nop
         nop
-        bl i_dll_wait
+        bl wait_sdrc_ok
         /* restore regs and return */
         ldmfd   sp!, {r0-r12, pc}
 
-i_dll_wait:
-        ldr     r4, clk_stabilize_delay
+/* Make sure SDRC accesses are ok */
+wait_sdrc_ok:
+        ldr     r4, cm_idlest1_core
+        ldr     r5, [r4]
+        and     r5, r5, #0x2
+        cmp     r5, #0
+        bne     wait_sdrc_ok
+        ldr     r4, sdrc_power
+        ldr     r5, [r4]
+        bic     r5, r5, #0x40
+        str     r5, [r4]
+wait_dll_lock:
+        /* Is dll in lock mode? */
+        ldr     r4, sdrc_dlla_ctrl
+        ldr     r5, [r4]
+        tst     r5, #0x4
+        bxne    lr
+        /* wait till dll locks */
+        ldr     r4, sdrc_dlla_status
+        ldr     r5, [r4]
+        and     r5, r5, #0x4
+        cmp     r5, #0x4
+        bne     wait_dll_lock
+        bx      lr
 
-i_dll_delay:
-        subs    r4, r4, #0x1
-        bne     i_dll_delay
-        ldr     r4, sdrc_power
-        ldr     r5, [r4]
-        bic     r5, r5, #0x40
-        str     r5, [r4]
-        bx      lr
+cm_idlest1_core:
+        .word   CM_IDLEST1_CORE_V
+sdrc_dlla_status:
+        .word   SDRC_DLLA_STATUS_V
+sdrc_dlla_ctrl:
+        .word   SDRC_DLLA_CTRL_V
 pm_prepwstst_core:
         .word   PM_PREPWSTST_CORE_V
+pm_prepwstst_core_p:
+        .word   PM_PREPWSTST_CORE_P
 pm_prepwstst_mpu:
         .word   PM_PREPWSTST_MPU_V
 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
-context_mem:
-        .word   0x803E3E14
 clk_stabilize_delay:
         .word 0x000001FF
 assoc_mask:
@@ -432,5 +606,7 @@ table_entry:
         .word   0x00000C02
 cache_pred_disable_mask:
         .word   0xFFFFE7FB
+control_stat:
+        .word   CONTROL_STAT
 ENTRY(omap34xx_cpu_suspend_sz)
         .word   . - omap34xx_cpu_suspend