3 files changed, 67 insertions, 0 deletions

diff --git a/arch/arm/mach-tegra/headsmp.S b/arch/arm/mach-tegra/headsmp.S
index 93f0370cc95b..82dc84b6b868 100644
--- a/arch/arm/mach-tegra/headsmp.S
+++ b/arch/arm/mach-tegra/headsmp.S
@@ -68,6 +68,55 @@ ENTRY(tegra_secondary_startup)
         b       secondary_startup
 ENDPROC(tegra_secondary_startup)
 
+#ifdef CONFIG_PM_SLEEP
+/*
+ *      tegra_resume
+ *
+ *        CPU boot vector when restarting the a CPU following
+ *        an LP2 transition. Also branched to by LP0 and LP1 resume after
+ *        re-enabling sdram.
+ */
+ENTRY(tegra_resume)
+        bl      v7_invalidate_l1
+        /* Enable coresight */
+        mov32   r0, 0xC5ACCE55
+        mcr     p14, 0, r0, c7, c12, 6
+
+        cpu_id  r0
+        cmp     r0, #0                          @ CPU0?
+        bne     cpu_resume                      @ no
+
+#ifdef CONFIG_ARCH_TEGRA_3x_SOC
+        /* Are we on Tegra20? */
+        mov32   r6, TEGRA_APB_MISC_BASE
+        ldr     r0, [r6, #APB_MISC_GP_HIDREV]
+        and     r0, r0, #0xff00
+        cmp     r0, #(0x20 << 8)
+        beq     1f                              @ Yes
+        /* Clear the flow controller flags for this CPU. */
+        mov32   r2, TEGRA_FLOW_CTRL_BASE + FLOW_CTRL_CPU0_CSR   @ CPU0 CSR
+        ldr     r1, [r2]
+        /* Clear event & intr flag */
+        orr     r1, r1, \
+                #FLOW_CTRL_CSR_INTR_FLAG | FLOW_CTRL_CSR_EVENT_FLAG
+        movw    r0, #0x0FFD     @ enable, cluster_switch, immed, & bitmaps
+        bic     r1, r1, r0
+        str     r1, [r2]
+1:
+#endif
+
+#ifdef CONFIG_HAVE_ARM_SCU
+        /* enable SCU */
+        mov32   r0, TEGRA_ARM_PERIF_BASE
+        ldr     r1, [r0]
+        orr     r1, r1, #1
+        str     r1, [r0]
+#endif
+
+        b       cpu_resume
+ENDPROC(tegra_resume)
+#endif
+
         .align L1_CACHE_SHIFT
 ENTRY(__tegra_cpu_reset_handler_start)
 
@@ -121,6 +170,17 @@ ENTRY(__tegra_cpu_reset_handler)
 1:
 #endif
 
+        /* Waking up from LP2? */
+        ldr     r9, [r12, #RESET_DATA(MASK_LP2)]
+        tst     r9, r11                         @ if in_lp2
+        beq     __is_not_lp2
+        ldr     lr, [r12, #RESET_DATA(STARTUP_LP2)]
+        cmp     lr, #0
+        bleq    __die                           @ no LP2 startup handler
+        bx      lr
+
+__is_not_lp2:
+
 #ifdef CONFIG_SMP
         /*
          * Can only be secondary boot (initial or hotplug) but CPU 0
diff --git a/arch/arm/mach-tegra/reset.c b/arch/arm/mach-tegra/reset.c
index e05da7d10c3b..3fd89ecd158e 100644
--- a/arch/arm/mach-tegra/reset.c
+++ b/arch/arm/mach-tegra/reset.c
@@ -25,6 +25,7 @@
 #include "iomap.h"
 #include "irammap.h"
 #include "reset.h"
+#include "sleep.h"
 #include "fuse.h"
 
 #define TEGRA_IRAM_RESET_BASE (TEGRA_IRAM_BASE + \
@@ -79,5 +80,10 @@ void __init tegra_cpu_reset_handler_init(void)
                 virt_to_phys((void *)tegra_secondary_startup);
 #endif
 
+#ifdef CONFIG_PM_SLEEP
+        __tegra_cpu_reset_handler_data[TEGRA_RESET_STARTUP_LP2] =
+                virt_to_phys((void *)tegra_resume);
+#endif
+
         tegra_cpu_reset_handler_enable();
 }
diff --git a/arch/arm/mach-tegra/sleep.h b/arch/arm/mach-tegra/sleep.h
index 4889b281c5f9..addb83f5bc73 100644
--- a/arch/arm/mach-tegra/sleep.h
+++ b/arch/arm/mach-tegra/sleep.h
@@ -72,6 +72,7 @@
         dsb
 .endm
 #else
+void tegra_resume(void);
 
 #ifdef CONFIG_HOTPLUG_CPU
 void tegra20_hotplug_init(void);