ARM: tegra: cpuidle: add CPU resume function

The CPU suspending on Tegra means CPU power gating. We add a resume
function for taking care the CPUs that resume from power gating status.
This function was been hooked to the reset handler. We take care
everything here before go into kernel.

Be aware of that, you may see the legacy power status "LP2" in the code
which is exactly the same meaning of "CPU power down".

Based on the work by:
Scott Williams <scwilliams@nvidia.com>
Colin Cross <ccross@android.com>
Gary King <gking@nvidia.com>

Signed-off-by: Joseph Lo <josephl@nvidia.com>
Signed-off-by: Stephen Warren <swarren@nvidia.com>

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);