2 files changed, 41 insertions, 2 deletions

diff --git a/arch/arm/mach-omap2/sleep34xx.S b/arch/arm/mach-omap2/sleep34xx.S
index 12a8ba0e0a3c..d522cd70bf53 100644
--- a/arch/arm/mach-omap2/sleep34xx.S
+++ b/arch/arm/mach-omap2/sleep34xx.S
@@ -284,6 +284,21 @@ restore:
         mcr     p15, 0, r0, c7, c10, 5  @ data memory barrier
         .word   0xE1600071              @ call SMI monitor (smi #1)
 
+#ifdef CONFIG_OMAP3_L2_AUX_SECURE_SAVE_RESTORE
+        /* Restore L2 aux control register */
+        @ set service ID for PPA
+        mov     r0, #CONFIG_OMAP3_L2_AUX_SECURE_SERVICE_SET_ID
+        mov     r12, r0         @ copy 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
+        ldr     r4, scratchpad_base
+        ldr     r3, [r4, #0xBC]
+        adds    r3, r3, #8      @ 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)
+#endif
         b       logic_l1_restore
 l2_inv_api_params:
         .word   0x1, 0x00
@@ -297,6 +312,11 @@ smi:    .word 0xE1600070		@ Call SMI monitor (smieq)
         ldr     r0, [r3,#4]
         mov     r12, #0x3
         .word 0xE1600070        @ Call SMI monitor (smieq)
+        ldr     r4, scratchpad_base
+        ldr     r3, [r4,#0xBC]
+        ldr     r0, [r3,#12]
+        mov     r12, #0x2
+        .word 0xE1600070        @ Call SMI monitor (smieq)
 logic_l1_restore:
         mov     r1, #0
         /* Invalidate all instruction caches to PoU
@@ -305,7 +325,7 @@ logic_l1_restore:
 
         ldr     r4, scratchpad_base
         ldr     r3, [r4,#0xBC]
-        adds    r3, r3, #8
+        adds    r3, r3, #16
         ldmia   r3!, {r4-r6}
         mov     sp, r4
         msr     spsr_cxsf, r5
@@ -424,7 +444,9 @@ 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}
+        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:stored in r1*/
         /* 1 - Only L1 and logic lost */
         /* 2 - Only L2 lost */
diff --git a/arch/arm/plat-omap/Kconfig b/arch/arm/plat-omap/Kconfig
index 97d0c79ffd2b..be9484a28b12 100644
--- a/arch/arm/plat-omap/Kconfig
+++ b/arch/arm/plat-omap/Kconfig
@@ -135,6 +135,23 @@ config OMAP_32K_TIMER
 
 endchoice
 
+config OMAP3_L2_AUX_SECURE_SAVE_RESTORE
+        bool "OMAP3 HS/EMU save and restore for L2 AUX control register"
+        depends on ARCH_OMAP3 && PM
+        default n
+        help
+          Without this option, L2 Auxiliary control register contents are
+          lost during off-mode entry on HS/EMU devices. This feature
+          requires support from PPA / boot-loader in HS/EMU devices, which
+          currently does not exist by default.
+
+config OMAP3_L2_AUX_SECURE_SERVICE_SET_ID
+        int "Service ID for the support routine to set L2 AUX control"
+        depends on OMAP3_L2_AUX_SECURE_SAVE_RESTORE
+        default 43
+        help
+          PPA routine service ID for setting L2 auxiliary control register.
+
 config OMAP_32K_TIMER_HZ
         int "Kernel internal timer frequency for 32KHz timer"
         range 32 1024