8 files changed, 253 insertions, 0 deletions

diff --git a/arch/arm/mach-tegra/Makefile b/arch/arm/mach-tegra/Makefile
index 488159eaa42..0979e8bba78 100644
--- a/arch/arm/mach-tegra/Makefile
+++ b/arch/arm/mach-tegra/Makefile
@@ -8,6 +8,7 @@ obj-y					+= pmc.o
 obj-y                                   += flowctrl.o
 obj-y                                   += powergate.o
 obj-y                                   += apbio.o
+obj-y                                   += pm.o
 obj-$(CONFIG_CPU_IDLE)                  += cpuidle.o
 obj-$(CONFIG_CPU_IDLE)                  += sleep.o
 obj-$(CONFIG_ARCH_TEGRA_2x_SOC)         += tegra20_clocks.o
diff --git a/arch/arm/mach-tegra/cpuidle-tegra30.c b/arch/arm/mach-tegra/cpuidle-tegra30.c
index 37e75512f69..cc48d7fa335 100644
--- a/arch/arm/mach-tegra/cpuidle-tegra30.c
+++ b/arch/arm/mach-tegra/cpuidle-tegra30.c
@@ -22,21 +22,107 @@
 #include <linux/kernel.h>
 #include <linux/module.h>
 #include <linux/cpuidle.h>
+#include <linux/cpu_pm.h>
+#include <linux/clockchips.h>
 
 #include <asm/cpuidle.h>
+#include <asm/proc-fns.h>
+#include <asm/suspend.h>
+#include <asm/smp_plat.h>
+
+#include "pm.h"
+#include "sleep.h"
+
+#ifdef CONFIG_PM_SLEEP
+static int tegra30_idle_lp2(struct cpuidle_device *dev,
+                            struct cpuidle_driver *drv,
+                            int index);
+#endif
 
 static struct cpuidle_driver tegra_idle_driver = {
         .name = "tegra_idle",
         .owner = THIS_MODULE,
         .en_core_tk_irqen = 1,
+#ifdef CONFIG_PM_SLEEP
+        .state_count = 2,
+#else
         .state_count = 1,
+#endif
         .states = {
                 [0] = ARM_CPUIDLE_WFI_STATE_PWR(600),
+#ifdef CONFIG_PM_SLEEP
+                [1] = {
+                        .enter                  = tegra30_idle_lp2,
+                        .exit_latency           = 2000,
+                        .target_residency       = 2200,
+                        .power_usage            = 0,
+                        .flags                  = CPUIDLE_FLAG_TIME_VALID,
+                        .name                   = "powered-down",
+                        .desc                   = "CPU power gated",
+                },
+#endif
         },
 };
 
 static DEFINE_PER_CPU(struct cpuidle_device, tegra_idle_device);
 
+#ifdef CONFIG_PM_SLEEP
+#ifdef CONFIG_SMP
+static bool tegra30_cpu_core_power_down(struct cpuidle_device *dev,
+                                        struct cpuidle_driver *drv,
+                                        int index)
+{
+        clockevents_notify(CLOCK_EVT_NOTIFY_BROADCAST_ENTER, &dev->cpu);
+
+        smp_wmb();
+
+        save_cpu_arch_register();
+
+        cpu_suspend(0, tegra30_sleep_cpu_secondary_finish);
+
+        restore_cpu_arch_register();
+
+        clockevents_notify(CLOCK_EVT_NOTIFY_BROADCAST_EXIT, &dev->cpu);
+
+        return true;
+}
+#else
+static inline bool tegra30_cpu_core_power_down(struct cpuidle_device *dev,
+                                               struct cpuidle_driver *drv,
+                                               int index)
+{
+        return true;
+}
+#endif
+
+static int __cpuinit tegra30_idle_lp2(struct cpuidle_device *dev,
+                                      struct cpuidle_driver *drv,
+                                      int index)
+{
+        u32 cpu = is_smp() ? cpu_logical_map(dev->cpu) : dev->cpu;
+        bool entered_lp2 = false;
+
+        local_fiq_disable();
+
+        tegra_set_cpu_in_lp2(cpu);
+        cpu_pm_enter();
+
+        if (cpu == 0)
+                cpu_do_idle();
+        else
+                entered_lp2 = tegra30_cpu_core_power_down(dev, drv, index);
+
+        cpu_pm_exit();
+        tegra_clear_cpu_in_lp2(cpu);
+
+        local_fiq_enable();
+
+        smp_rmb();
+
+        return (entered_lp2) ? index : 0;
+}
+#endif
+
 int __init tegra30_cpuidle_init(void)
 {
         int ret;
diff --git a/arch/arm/mach-tegra/pm.c b/arch/arm/mach-tegra/pm.c
new file mode 100644
index 00000000000..f88595abc47
--- /dev/null
+++ b/arch/arm/mach-tegra/pm.c
@@ -0,0 +1,74 @@
+/*
+ * CPU complex suspend & resume functions for Tegra SoCs
+ *
+ * Copyright (c) 2009-2012, NVIDIA Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <linux/kernel.h>
+#include <linux/spinlock.h>
+#include <linux/io.h>
+#include <linux/cpumask.h>
+
+#include "iomap.h"
+#include "reset.h"
+
+#ifdef CONFIG_PM_SLEEP
+static unsigned int g_diag_reg;
+static DEFINE_SPINLOCK(tegra_lp2_lock);
+
+void save_cpu_arch_register(void)
+{
+        /* read diagnostic register */
+        asm("mrc p15, 0, %0, c15, c0, 1" : "=r"(g_diag_reg) : : "cc");
+        return;
+}
+
+void restore_cpu_arch_register(void)
+{
+        /* write diagnostic register */
+        asm("mcr p15, 0, %0, c15, c0, 1" : : "r"(g_diag_reg) : "cc");
+        return;
+}
+
+void __cpuinit tegra_clear_cpu_in_lp2(int phy_cpu_id)
+{
+        u32 *cpu_in_lp2 = tegra_cpu_lp2_mask;
+
+        spin_lock(&tegra_lp2_lock);
+
+        BUG_ON(!(*cpu_in_lp2 & BIT(phy_cpu_id)));
+        *cpu_in_lp2 &= ~BIT(phy_cpu_id);
+
+        spin_unlock(&tegra_lp2_lock);
+}
+
+bool __cpuinit tegra_set_cpu_in_lp2(int phy_cpu_id)
+{
+        bool last_cpu = false;
+        cpumask_t *cpu_lp2_mask = tegra_cpu_lp2_mask;
+        u32 *cpu_in_lp2 = tegra_cpu_lp2_mask;
+
+        spin_lock(&tegra_lp2_lock);
+
+        BUG_ON((*cpu_in_lp2 & BIT(phy_cpu_id)));
+        *cpu_in_lp2 |= BIT(phy_cpu_id);
+
+        if ((phy_cpu_id == 0) && cpumask_equal(cpu_lp2_mask, cpu_online_mask))
+                last_cpu = true;
+
+        spin_unlock(&tegra_lp2_lock);
+        return last_cpu;
+}
+#endif
diff --git a/arch/arm/mach-tegra/pm.h b/arch/arm/mach-tegra/pm.h
new file mode 100644
index 00000000000..bcfc45faad7
--- /dev/null
+++ b/arch/arm/mach-tegra/pm.h
@@ -0,0 +1,30 @@
+/*
+ * Copyright (C) 2010 Google, Inc.
+ * Copyright (c) 2010-2012 NVIDIA Corporation. All rights reserved.
+ *
+ * Author:
+ *      Colin Cross <ccross@google.com>
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef _MACH_TEGRA_PM_H_
+#define _MACH_TEGRA_PM_H_
+
+void save_cpu_arch_register(void);
+void restore_cpu_arch_register(void);
+
+void tegra_clear_cpu_in_lp2(int phy_cpu_id);
+bool tegra_set_cpu_in_lp2(int phy_cpu_id);
+
+#endif /* _MACH_TEGRA_PM_H_ */
diff --git a/arch/arm/mach-tegra/reset.h b/arch/arm/mach-tegra/reset.h
index de88bf851dd..c90d8e9c4ad 100644
--- a/arch/arm/mach-tegra/reset.h
+++ b/arch/arm/mach-tegra/reset.h
@@ -29,6 +29,8 @@
 
 #ifndef __ASSEMBLY__
 
+#include "irammap.h"
+
 extern unsigned long __tegra_cpu_reset_handler_data[TEGRA_RESET_DATA_SIZE];
 
 void __tegra_cpu_reset_handler_start(void);
@@ -36,6 +38,13 @@ void __tegra_cpu_reset_handler(void);
 void __tegra_cpu_reset_handler_end(void);
 void tegra_secondary_startup(void);
 
+#ifdef CONFIG_PM_SLEEP
+#define tegra_cpu_lp2_mask \
+        (IO_ADDRESS(TEGRA_IRAM_BASE + TEGRA_IRAM_RESET_HANDLER_OFFSET + \
+        ((u32)&__tegra_cpu_reset_handler_data[TEGRA_RESET_MASK_LP2] - \
+         (u32)__tegra_cpu_reset_handler_start)))
+#endif
+
 #define tegra_cpu_reset_handler_offset \
                 ((u32)__tegra_cpu_reset_handler - \
                  (u32)__tegra_cpu_reset_handler_start)
diff --git a/arch/arm/mach-tegra/sleep-tegra30.S b/arch/arm/mach-tegra/sleep-tegra30.S
index be7614b7c5c..59984d71848 100644
--- a/arch/arm/mach-tegra/sleep-tegra30.S
+++ b/arch/arm/mach-tegra/sleep-tegra30.S
@@ -17,6 +17,7 @@
 #include <linux/linkage.h>
 
 #include <asm/assembler.h>
+#include <asm/asm-offsets.h>
 
 #include "sleep.h"
 #include "flowctrl.h"
@@ -80,6 +81,7 @@ delay_1:
         ldr     r3, [r1]                        @ read CSR
         str     r3, [r1]                        @ clear CSR
         tst     r0, #TEGRA30_POWER_HOTPLUG_SHUTDOWN
+        moveq   r3, #FLOW_CTRL_WAIT_FOR_INTERRUPT       @ For LP2
         movne   r3, #FLOW_CTRL_WAITEVENT                @ For hotplug
         str     r3, [r2]
         ldr     r0, [r2]
@@ -103,3 +105,23 @@ wfe_war:
 
 ENDPROC(tegra30_cpu_shutdown)
 #endif
+
+#ifdef CONFIG_PM_SLEEP
+/*
+ * tegra30_sleep_cpu_secondary_finish(unsigned long v2p)
+ *
+ * Enters LP2 on secondary CPU by exiting coherency and powergating the CPU.
+ */
+ENTRY(tegra30_sleep_cpu_secondary_finish)
+        mov     r7, lr
+
+        /* Flush and disable the L1 data cache */
+        bl      tegra_disable_clean_inv_dcache
+
+        /* Powergate this CPU. */
+        mov     r0, #0                          @ power mode flags (!hotplug)
+        bl      tegra30_cpu_shutdown
+        mov     r0, #1                          @ never return here
+        mov     pc, r7
+ENDPROC(tegra30_sleep_cpu_secondary_finish)
+#endif
diff --git a/arch/arm/mach-tegra/sleep.S b/arch/arm/mach-tegra/sleep.S
index 08e9481c049..91548a77dd9 100644
--- a/arch/arm/mach-tegra/sleep.S
+++ b/arch/arm/mach-tegra/sleep.S
@@ -25,9 +25,38 @@
 #include <linux/linkage.h>
 
 #include <asm/assembler.h>
+#include <asm/cp15.h>
 
 #include "iomap.h"
 
 #include "flowctrl.h"
 #include "sleep.h"
 
+#ifdef CONFIG_PM_SLEEP
+/*
+ * tegra_disable_clean_inv_dcache
+ *
+ * disable, clean & invalidate the D-cache
+ *
+ * Corrupted registers: r1-r3, r6, r8, r9-r11
+ */
+ENTRY(tegra_disable_clean_inv_dcache)
+        stmfd   sp!, {r0, r4-r5, r7, r9-r11, lr}
+        dmb                                     @ ensure ordering
+
+        /* Disable the D-cache */
+        mrc     p15, 0, r2, c1, c0, 0
+        bic     r2, r2, #CR_C
+        mcr     p15, 0, r2, c1, c0, 0
+        isb
+
+        /* Flush the D-cache */
+        bl      v7_flush_dcache_louis
+
+        /* Trun off coherency */
+        exit_smp r4, r5
+
+        ldmfd   sp!, {r0, r4-r5, r7, r9-r11, pc}
+ENDPROC(tegra_disable_clean_inv_dcache)
+
+#endif
diff --git a/arch/arm/mach-tegra/sleep.h b/arch/arm/mach-tegra/sleep.h
index addb83f5bc7..bacf549bd54 100644
--- a/arch/arm/mach-tegra/sleep.h
+++ b/arch/arm/mach-tegra/sleep.h
@@ -82,5 +82,7 @@ static inline void tegra20_hotplug_init(void) {}
 static inline void tegra30_hotplug_init(void) {}
 #endif
 
+int tegra30_sleep_cpu_secondary_finish(unsigned long);
+
 #endif
 #endif