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>

diff --git a/arch/arm/mach-tegra/headsmp.S b/arch/arm/mach-tegra/headsmp.S
index 93f0370cc95b70ff8f1004a4dad1144e3851ce26..82dc84b6b86827c063666046309142d9319fa00b 100644 (file)
--- 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 e05da7d10c3be3651521d1aeeafcbe43afab1628..3fd89ecd158e9432da2a92d4f7ab32e92f1ef3b1 100644 (file)
--- 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 4889b281c5f9a4543e0e4c3480b46757e7028393..addb83f5bc734f23d95a6eab671fe99770bb7e67 100644 (file)
--- 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);