Merge tag 'tegra-for-3.9-soc-cpuidle' of git://git.kernel.org/pub/scm/linux/kernel/git/swarren/linux-tegra into next/soc

From Stephen Warren:
ARM: tegra: cpuidle enhancements

This pull request implements a new "LP2" cpuidle state for Tegra20,
which makes use of the couple cpuidle feature.

It is based on (most of) the previous pull request, with tag
tegra-for-3.9-soc-usb.

* tag 'tegra-for-3.9-soc-cpuidle' of git://git.kernel.org/pub/scm/linux/kernel/git/swarren/linux-tegra:
  ARM: tegra20: cpuidle: apply coupled cpuidle for powered-down mode
  ARM: tegra20: flowctrl: add support for cpu_suspend_enter/exit
  clk: tegra20: Implementing CPU low-power function for tegra_cpu_car_ops
  ARM: tegra20: cpuidle: add powered-down state for secondary CPU
  ARM: tegra: add pending SGI checking API

Signed-off-by: Olof Johansson <olof@lixom.net>

10 files changed, 516 insertions, 9 deletions

diff --git a/arch/arm/mach-tegra/Kconfig b/arch/arm/mach-tegra/Kconfig
index 1ec7f80e2af5..abc688fd4807 100644
--- a/arch/arm/mach-tegra/Kconfig
+++ b/arch/arm/mach-tegra/Kconfig
@@ -4,6 +4,7 @@ comment "NVIDIA Tegra options"
 
 config ARCH_TEGRA_2x_SOC
         bool "Enable support for Tegra20 family"
+        select ARCH_NEEDS_CPU_IDLE_COUPLED if SMP
         select ARCH_REQUIRE_GPIOLIB
         select ARM_ERRATA_720789
         select ARM_ERRATA_742230 if SMP
diff --git a/arch/arm/mach-tegra/cpuidle-tegra20.c b/arch/arm/mach-tegra/cpuidle-tegra20.c
index d32e8b0dbd4f..825ced4f7a40 100644
--- a/arch/arm/mach-tegra/cpuidle-tegra20.c
+++ b/arch/arm/mach-tegra/cpuidle-tegra20.c
@@ -22,21 +22,199 @@
 #include <linux/kernel.h>
 #include <linux/module.h>
 #include <linux/cpuidle.h>
+#include <linux/cpu_pm.h>
+#include <linux/clockchips.h>
+#include <linux/clk/tegra.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"
+#include "iomap.h"
+#include "irq.h"
+#include "flowctrl.h"
+
+#ifdef CONFIG_PM_SLEEP
+static bool abort_flag;
+static atomic_t abort_barrier;
+static int tegra20_idle_lp2_coupled(struct cpuidle_device *dev,
+                                    struct cpuidle_driver *drv,
+                                    int index);
+#endif
+
+static struct cpuidle_state tegra_idle_states[] = {
+        [0] = ARM_CPUIDLE_WFI_STATE_PWR(600),
+#ifdef CONFIG_PM_SLEEP
+        [1] = {
+                .enter                  = tegra20_idle_lp2_coupled,
+                .exit_latency           = 5000,
+                .target_residency       = 10000,
+                .power_usage            = 0,
+                .flags                  = CPUIDLE_FLAG_TIME_VALID |
+                                          CPUIDLE_FLAG_COUPLED,
+                .name                   = "powered-down",
+                .desc                   = "CPU power gated",
+        },
+#endif
+};
 
 static struct cpuidle_driver tegra_idle_driver = {
         .name = "tegra_idle",
         .owner = THIS_MODULE,
         .en_core_tk_irqen = 1,
-        .state_count = 1,
-        .states = {
-                [0] = ARM_CPUIDLE_WFI_STATE_PWR(600),
-        },
 };
 
 static DEFINE_PER_CPU(struct cpuidle_device, tegra_idle_device);
 
+#ifdef CONFIG_PM_SLEEP
+#ifdef CONFIG_SMP
+static void __iomem *pmc = IO_ADDRESS(TEGRA_PMC_BASE);
+
+static int tegra20_reset_sleeping_cpu_1(void)
+{
+        int ret = 0;
+
+        tegra_pen_lock();
+
+        if (readl(pmc + PMC_SCRATCH41) == CPU_RESETTABLE)
+                tegra20_cpu_shutdown(1);
+        else
+                ret = -EINVAL;
+
+        tegra_pen_unlock();
+
+        return ret;
+}
+
+static void tegra20_wake_cpu1_from_reset(void)
+{
+        tegra_pen_lock();
+
+        tegra20_cpu_clear_resettable();
+
+        /* enable cpu clock on cpu */
+        tegra_enable_cpu_clock(1);
+
+        /* take the CPU out of reset */
+        tegra_cpu_out_of_reset(1);
+
+        /* unhalt the cpu */
+        flowctrl_write_cpu_halt(1, 0);
+
+        tegra_pen_unlock();
+}
+
+static int tegra20_reset_cpu_1(void)
+{
+        if (!cpu_online(1) || !tegra20_reset_sleeping_cpu_1())
+                return 0;
+
+        tegra20_wake_cpu1_from_reset();
+        return -EBUSY;
+}
+#else
+static inline void tegra20_wake_cpu1_from_reset(void)
+{
+}
+
+static inline int tegra20_reset_cpu_1(void)
+{
+        return 0;
+}
+#endif
+
+static bool tegra20_cpu_cluster_power_down(struct cpuidle_device *dev,
+                                           struct cpuidle_driver *drv,
+                                           int index)
+{
+        struct cpuidle_state *state = &drv->states[index];
+        u32 cpu_on_time = state->exit_latency;
+        u32 cpu_off_time = state->target_residency - state->exit_latency;
+
+        while (tegra20_cpu_is_resettable_soon())
+                cpu_relax();
+
+        if (tegra20_reset_cpu_1() || !tegra_cpu_rail_off_ready())
+                return false;
+
+        clockevents_notify(CLOCK_EVT_NOTIFY_BROADCAST_ENTER, &dev->cpu);
+
+        tegra_idle_lp2_last(cpu_on_time, cpu_off_time);
+
+        clockevents_notify(CLOCK_EVT_NOTIFY_BROADCAST_EXIT, &dev->cpu);
+
+        if (cpu_online(1))
+                tegra20_wake_cpu1_from_reset();
+
+        return true;
+}
+
+#ifdef CONFIG_SMP
+static bool tegra20_idle_enter_lp2_cpu_1(struct cpuidle_device *dev,
+                                         struct cpuidle_driver *drv,
+                                         int index)
+{
+        clockevents_notify(CLOCK_EVT_NOTIFY_BROADCAST_ENTER, &dev->cpu);
+
+        cpu_suspend(0, tegra20_sleep_cpu_secondary_finish);
+
+        tegra20_cpu_clear_resettable();
+
+        clockevents_notify(CLOCK_EVT_NOTIFY_BROADCAST_EXIT, &dev->cpu);
+
+        return true;
+}
+#else
+static inline bool tegra20_idle_enter_lp2_cpu_1(struct cpuidle_device *dev,
+                                                struct cpuidle_driver *drv,
+                                                int index)
+{
+        return true;
+}
+#endif
+
+static int tegra20_idle_lp2_coupled(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;
+
+        if (tegra_pending_sgi())
+                ACCESS_ONCE(abort_flag) = true;
+
+        cpuidle_coupled_parallel_barrier(dev, &abort_barrier);
+
+        if (abort_flag) {
+                cpuidle_coupled_parallel_barrier(dev, &abort_barrier);
+                abort_flag = false;     /* clean flag for next coming */
+                return -EINTR;
+        }
+
+        local_fiq_disable();
+
+        tegra_set_cpu_in_lp2(cpu);
+        cpu_pm_enter();
+
+        if (cpu == 0)
+                entered_lp2 = tegra20_cpu_cluster_power_down(dev, drv, index);
+        else
+                entered_lp2 = tegra20_idle_enter_lp2_cpu_1(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 tegra20_cpuidle_init(void)
 {
         int ret;
@@ -44,6 +222,14 @@ int __init tegra20_cpuidle_init(void)
         struct cpuidle_device *dev;
         struct cpuidle_driver *drv = &tegra_idle_driver;
 
+#ifdef CONFIG_PM_SLEEP
+        tegra_tear_down_cpu = tegra20_tear_down_cpu;
+#endif
+
+        drv->state_count = ARRAY_SIZE(tegra_idle_states);
+        memcpy(drv->states, tegra_idle_states,
+                        drv->state_count * sizeof(drv->states[0]));
+
         ret = cpuidle_register_driver(&tegra_idle_driver);
         if (ret) {
                 pr_err("CPUidle driver registration failed\n");
@@ -53,6 +239,9 @@ int __init tegra20_cpuidle_init(void)
         for_each_possible_cpu(cpu) {
                 dev = &per_cpu(tegra_idle_device, cpu);
                 dev->cpu = cpu;
+#ifdef CONFIG_ARCH_NEEDS_CPU_IDLE_COUPLED
+                dev->coupled_cpus = *cpu_possible_mask;
+#endif
 
                 dev->state_count = drv->state_count;
                 ret = cpuidle_register_device(dev);
diff --git a/arch/arm/mach-tegra/flowctrl.c b/arch/arm/mach-tegra/flowctrl.c
index 5393eb2cae21..b477ef310dcd 100644
--- a/arch/arm/mach-tegra/flowctrl.c
+++ b/arch/arm/mach-tegra/flowctrl.c
@@ -25,6 +25,7 @@
 
 #include "flowctrl.h"
 #include "iomap.h"
+#include "fuse.h"
 
 static u8 flowctrl_offset_halt_cpu[] = {
         FLOW_CTRL_HALT_CPU0_EVENTS,
@@ -75,11 +76,26 @@ void flowctrl_cpu_suspend_enter(unsigned int cpuid)
         int i;
 
         reg = flowctrl_read_cpu_csr(cpuid);
-        reg &= ~TEGRA30_FLOW_CTRL_CSR_WFE_BITMAP;       /* clear wfe bitmap */
-        reg &= ~TEGRA30_FLOW_CTRL_CSR_WFI_BITMAP;       /* clear wfi bitmap */
+        switch (tegra_chip_id) {
+        case TEGRA20:
+                /* clear wfe bitmap */
+                reg &= ~TEGRA20_FLOW_CTRL_CSR_WFE_BITMAP;
+                /* clear wfi bitmap */
+                reg &= ~TEGRA20_FLOW_CTRL_CSR_WFI_BITMAP;
+                /* pwr gating on wfe */
+                reg |= TEGRA20_FLOW_CTRL_CSR_WFE_CPU0 << cpuid;
+                break;
+        case TEGRA30:
+                /* clear wfe bitmap */
+                reg &= ~TEGRA30_FLOW_CTRL_CSR_WFE_BITMAP;
+                /* clear wfi bitmap */
+                reg &= ~TEGRA30_FLOW_CTRL_CSR_WFI_BITMAP;
+                /* pwr gating on wfi */
+                reg |= TEGRA30_FLOW_CTRL_CSR_WFI_CPU0 << cpuid;
+                break;
+        }
         reg |= FLOW_CTRL_CSR_INTR_FLAG;                 /* clear intr flag */
         reg |= FLOW_CTRL_CSR_EVENT_FLAG;                /* clear event flag */
-        reg |= TEGRA30_FLOW_CTRL_CSR_WFI_CPU0 << cpuid; /* pwr gating on wfi */
         reg |= FLOW_CTRL_CSR_ENABLE;                    /* pwr gating */
         flowctrl_write_cpu_csr(cpuid, reg);
 
@@ -99,8 +115,20 @@ void flowctrl_cpu_suspend_exit(unsigned int cpuid)
 
         /* Disable powergating via flow controller for CPU0 */
         reg = flowctrl_read_cpu_csr(cpuid);
-        reg &= ~TEGRA30_FLOW_CTRL_CSR_WFE_BITMAP;       /* clear wfe bitmap */
-        reg &= ~TEGRA30_FLOW_CTRL_CSR_WFI_BITMAP;       /* clear wfi bitmap */
+        switch (tegra_chip_id) {
+        case TEGRA20:
+                /* clear wfe bitmap */
+                reg &= ~TEGRA20_FLOW_CTRL_CSR_WFE_BITMAP;
+                /* clear wfi bitmap */
+                reg &= ~TEGRA20_FLOW_CTRL_CSR_WFI_BITMAP;
+                break;
+        case TEGRA30:
+                /* clear wfe bitmap */
+                reg &= ~TEGRA30_FLOW_CTRL_CSR_WFE_BITMAP;
+                /* clear wfi bitmap */
+                reg &= ~TEGRA30_FLOW_CTRL_CSR_WFI_BITMAP;
+                break;
+        }
         reg &= ~FLOW_CTRL_CSR_ENABLE;                   /* clear enable */
         reg |= FLOW_CTRL_CSR_INTR_FLAG;                 /* clear intr */
         reg |= FLOW_CTRL_CSR_EVENT_FLAG;                /* clear event */
diff --git a/arch/arm/mach-tegra/flowctrl.h b/arch/arm/mach-tegra/flowctrl.h
index 0798dec1832d..67eab56699bd 100644
--- a/arch/arm/mach-tegra/flowctrl.h
+++ b/arch/arm/mach-tegra/flowctrl.h
@@ -34,6 +34,10 @@
 #define FLOW_CTRL_HALT_CPU1_EVENTS      0x14
 #define FLOW_CTRL_CPU1_CSR              0x18
 
+#define TEGRA20_FLOW_CTRL_CSR_WFE_CPU0          (1 << 4)
+#define TEGRA20_FLOW_CTRL_CSR_WFE_BITMAP        (3 << 4)
+#define TEGRA20_FLOW_CTRL_CSR_WFI_BITMAP        0
+
 #define TEGRA30_FLOW_CTRL_CSR_WFI_CPU0          (1 << 8)
 #define TEGRA30_FLOW_CTRL_CSR_WFE_BITMAP        (0xF << 4)
 #define TEGRA30_FLOW_CTRL_CSR_WFI_BITMAP        (0xF << 8)
diff --git a/arch/arm/mach-tegra/irq.c b/arch/arm/mach-tegra/irq.c
index 2ff2128cb9d8..1952e82797cc 100644
--- a/arch/arm/mach-tegra/irq.c
+++ b/arch/arm/mach-tegra/irq.c
@@ -44,6 +44,8 @@
 
 #define FIRST_LEGACY_IRQ 32
 
+#define SGI_MASK 0xFFFF
+
 static int num_ictlrs;
 
 static void __iomem *ictlr_reg_base[] = {
@@ -54,6 +56,19 @@ static void __iomem *ictlr_reg_base[] = {
         IO_ADDRESS(TEGRA_QUINARY_ICTLR_BASE),
 };
 
+bool tegra_pending_sgi(void)
+{
+        u32 pending_set;
+        void __iomem *distbase = IO_ADDRESS(TEGRA_ARM_INT_DIST_BASE);
+
+        pending_set = readl_relaxed(distbase + GIC_DIST_PENDING_SET);
+
+        if (pending_set & SGI_MASK)
+                return true;
+
+        return false;
+}
+
 static inline void tegra_irq_write_mask(unsigned int irq, unsigned long reg)
 {
         void __iomem *base;
diff --git a/arch/arm/mach-tegra/irq.h b/arch/arm/mach-tegra/irq.h
new file mode 100644
index 000000000000..5142649bba05
--- /dev/null
+++ b/arch/arm/mach-tegra/irq.h
@@ -0,0 +1,22 @@
+/*
+ * Copyright (c) 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/>.
+ */
+
+#ifndef __TEGRA_IRQ_H
+#define __TEGRA_IRQ_H
+
+bool tegra_pending_sgi(void);
+
+#endif
diff --git a/arch/arm/mach-tegra/pm.c b/arch/arm/mach-tegra/pm.c
index abfe9b93cc0c..523604de666f 100644
--- a/arch/arm/mach-tegra/pm.c
+++ b/arch/arm/mach-tegra/pm.c
@@ -36,6 +36,7 @@
 #include "iomap.h"
 #include "reset.h"
 #include "flowctrl.h"
+#include "fuse.h"
 #include "sleep.h"
 
 #define TEGRA_POWER_CPU_PWRREQ_OE       (1 << 16)  /* CPU pwr req enable */
@@ -173,6 +174,8 @@ bool tegra_set_cpu_in_lp2(int phy_cpu_id)
 
         if ((phy_cpu_id == 0) && cpumask_equal(cpu_lp2_mask, cpu_online_mask))
                 last_cpu = true;
+        else if (tegra_chip_id == TEGRA20 && phy_cpu_id == 1)
+                tegra20_cpu_set_resettable_soon();
 
         spin_unlock(&tegra_lp2_lock);
         return last_cpu;
diff --git a/arch/arm/mach-tegra/sleep-tegra20.S b/arch/arm/mach-tegra/sleep-tegra20.S
index ad2ca07d0578..9f6bfafdd512 100644
--- a/arch/arm/mach-tegra/sleep-tegra20.S
+++ b/arch/arm/mach-tegra/sleep-tegra20.S
@@ -21,6 +21,8 @@
 #include <linux/linkage.h>
 
 #include <asm/assembler.h>
+#include <asm/proc-fns.h>
+#include <asm/cp15.h>
 
 #include "sleep.h"
 #include "flowctrl.h"
@@ -55,6 +57,9 @@ ENDPROC(tegra20_hotplug_shutdown)
 ENTRY(tegra20_cpu_shutdown)
         cmp     r0, #0
         moveq   pc, lr                  @ must not be called for CPU 0
+        mov32   r1, TEGRA_PMC_VIRT + PMC_SCRATCH41
+        mov     r12, #CPU_RESETTABLE
+        str     r12, [r1]
 
         cpu_to_halt_reg r1, r0
         ldr     r3, =TEGRA_FLOW_CTRL_VIRT
@@ -75,3 +80,198 @@ ENTRY(tegra20_cpu_shutdown)
         mov     pc, lr
 ENDPROC(tegra20_cpu_shutdown)
 #endif
+
+#ifdef CONFIG_PM_SLEEP
+/*
+ * tegra_pen_lock
+ *
+ * spinlock implementation with no atomic test-and-set and no coherence
+ * using Peterson's algorithm on strongly-ordered registers
+ * used to synchronize a cpu waking up from wfi with entering lp2 on idle
+ *
+ * The reference link of Peterson's algorithm:
+ * http://en.wikipedia.org/wiki/Peterson's_algorithm
+ *
+ * SCRATCH37 = r1 = !turn (inverted from Peterson's algorithm)
+ * on cpu 0:
+ * r2 = flag[0] (in SCRATCH38)
+ * r3 = flag[1] (in SCRATCH39)
+ * on cpu1:
+ * r2 = flag[1] (in SCRATCH39)
+ * r3 = flag[0] (in SCRATCH38)
+ *
+ * must be called with MMU on
+ * corrupts r0-r3, r12
+ */
+ENTRY(tegra_pen_lock)
+        mov32   r3, TEGRA_PMC_VIRT
+        cpu_id  r0
+        add     r1, r3, #PMC_SCRATCH37
+        cmp     r0, #0
+        addeq   r2, r3, #PMC_SCRATCH38
+        addeq   r3, r3, #PMC_SCRATCH39
+        addne   r2, r3, #PMC_SCRATCH39
+        addne   r3, r3, #PMC_SCRATCH38
+
+        mov     r12, #1
+        str     r12, [r2]               @ flag[cpu] = 1
+        dsb
+        str     r12, [r1]               @ !turn = cpu
+1:      dsb
+        ldr     r12, [r3]
+        cmp     r12, #1                 @ flag[!cpu] == 1?
+        ldreq   r12, [r1]
+        cmpeq   r12, r0                 @ !turn == cpu?
+        beq     1b                      @ while !turn == cpu && flag[!cpu] == 1
+
+        mov     pc, lr                  @ locked
+ENDPROC(tegra_pen_lock)
+
+ENTRY(tegra_pen_unlock)
+        dsb
+        mov32   r3, TEGRA_PMC_VIRT
+        cpu_id  r0
+        cmp     r0, #0
+        addeq   r2, r3, #PMC_SCRATCH38
+        addne   r2, r3, #PMC_SCRATCH39
+        mov     r12, #0
+        str     r12, [r2]
+        mov     pc, lr
+ENDPROC(tegra_pen_unlock)
+
+/*
+ * tegra20_cpu_clear_resettable(void)
+ *
+ * Called to clear the "resettable soon" flag in PMC_SCRATCH41 when
+ * it is expected that the secondary CPU will be idle soon.
+ */
+ENTRY(tegra20_cpu_clear_resettable)
+        mov32   r1, TEGRA_PMC_VIRT + PMC_SCRATCH41
+        mov     r12, #CPU_NOT_RESETTABLE
+        str     r12, [r1]
+        mov     pc, lr
+ENDPROC(tegra20_cpu_clear_resettable)
+
+/*
+ * tegra20_cpu_set_resettable_soon(void)
+ *
+ * Called to set the "resettable soon" flag in PMC_SCRATCH41 when
+ * it is expected that the secondary CPU will be idle soon.
+ */
+ENTRY(tegra20_cpu_set_resettable_soon)
+        mov32   r1, TEGRA_PMC_VIRT + PMC_SCRATCH41
+        mov     r12, #CPU_RESETTABLE_SOON
+        str     r12, [r1]
+        mov     pc, lr
+ENDPROC(tegra20_cpu_set_resettable_soon)
+
+/*
+ * tegra20_cpu_is_resettable_soon(void)
+ *
+ * Returns true if the "resettable soon" flag in PMC_SCRATCH41 has been
+ * set because it is expected that the secondary CPU will be idle soon.
+ */
+ENTRY(tegra20_cpu_is_resettable_soon)
+        mov32   r1, TEGRA_PMC_VIRT + PMC_SCRATCH41
+        ldr     r12, [r1]
+        cmp     r12, #CPU_RESETTABLE_SOON
+        moveq   r0, #1
+        movne   r0, #0
+        mov     pc, lr
+ENDPROC(tegra20_cpu_is_resettable_soon)
+
+/*
+ * tegra20_sleep_cpu_secondary_finish(unsigned long v2p)
+ *
+ * Enters WFI on secondary CPU by exiting coherency.
+ */
+ENTRY(tegra20_sleep_cpu_secondary_finish)
+        stmfd   sp!, {r4-r11, lr}
+
+        mrc     p15, 0, r11, c1, c0, 1  @ save actlr before exiting coherency
+
+        /* Flush and disable the L1 data cache */
+        bl      tegra_disable_clean_inv_dcache
+
+        mov32   r0, TEGRA_PMC_VIRT + PMC_SCRATCH41
+        mov     r3, #CPU_RESETTABLE
+        str     r3, [r0]
+
+        bl      cpu_do_idle
+
+        /*
+         * cpu may be reset while in wfi, which will return through
+         * tegra_resume to cpu_resume
+         * or interrupt may wake wfi, which will return here
+         * cpu state is unchanged - MMU is on, cache is on, coherency
+         * is off, and the data cache is off
+         *
+         * r11 contains the original actlr
+         */
+
+        bl      tegra_pen_lock
+
+        mov32   r3, TEGRA_PMC_VIRT
+        add     r0, r3, #PMC_SCRATCH41
+        mov     r3, #CPU_NOT_RESETTABLE
+        str     r3, [r0]
+
+        bl      tegra_pen_unlock
+
+        /* Re-enable the data cache */
+        mrc     p15, 0, r10, c1, c0, 0
+        orr     r10, r10, #CR_C
+        mcr     p15, 0, r10, c1, c0, 0
+        isb
+
+        mcr     p15, 0, r11, c1, c0, 1  @ reenable coherency
+
+        /* Invalidate the TLBs & BTAC */
+        mov     r1, #0
+        mcr     p15, 0, r1, c8, c3, 0   @ invalidate shared TLBs
+        mcr     p15, 0, r1, c7, c1, 6   @ invalidate shared BTAC
+        dsb
+        isb
+
+        /* the cpu was running with coherency disabled,
+         * caches may be out of date */
+        bl      v7_flush_kern_cache_louis
+
+        ldmfd   sp!, {r4 - r11, pc}
+ENDPROC(tegra20_sleep_cpu_secondary_finish)
+
+/*
+ * tegra20_tear_down_cpu
+ *
+ * Switches the CPU cluster to PLL-P and enters sleep.
+ */
+ENTRY(tegra20_tear_down_cpu)
+        bl      tegra_switch_cpu_to_pllp
+        b       tegra20_enter_sleep
+ENDPROC(tegra20_tear_down_cpu)
+
+/*
+ * tegra20_enter_sleep
+ *
+ * uses flow controller to enter sleep state
+ * executes from IRAM with SDRAM in selfrefresh when target state is LP0 or LP1
+ * executes from SDRAM with target state is LP2
+ */
+tegra20_enter_sleep:
+        mov32   r6, TEGRA_FLOW_CTRL_BASE
+
+        mov     r0, #FLOW_CTRL_WAIT_FOR_INTERRUPT
+        orr     r0, r0, #FLOW_CTRL_HALT_CPU_IRQ | FLOW_CTRL_HALT_CPU_FIQ
+        cpu_id  r1
+        cpu_to_halt_reg r1, r1
+        str     r0, [r6, r1]
+        dsb
+        ldr     r0, [r6, r1] /* memory barrier */
+
+halted:
+        dsb
+        wfe     /* CPU should be power gated here */
+        isb
+        b       halted
+
+#endif
diff --git a/arch/arm/mach-tegra/sleep.S b/arch/arm/mach-tegra/sleep.S
index addae357da3f..364d84523fba 100644
--- a/arch/arm/mach-tegra/sleep.S
+++ b/arch/arm/mach-tegra/sleep.S
@@ -34,6 +34,9 @@
 #include "flowctrl.h"
 #include "sleep.h"
 
+#define CLK_RESET_CCLK_BURST    0x20
+#define CLK_RESET_CCLK_DIVIDER  0x24
+
 #if defined(CONFIG_HOTPLUG_CPU) || defined(CONFIG_PM_SLEEP)
 /*
  * tegra_disable_clean_inv_dcache
@@ -110,4 +113,20 @@ ENTRY(tegra_shut_off_mmu)
         mov     pc, r0
 ENDPROC(tegra_shut_off_mmu)
         .popsection
+
+/*
+ * tegra_switch_cpu_to_pllp
+ *
+ * In LP2 the normal cpu clock pllx will be turned off. Switch the CPU to pllp
+ */
+ENTRY(tegra_switch_cpu_to_pllp)
+        /* in LP2 idle (SDRAM active), set the CPU burst policy to PLLP */
+        mov32   r5, TEGRA_CLK_RESET_BASE
+        mov     r0, #(2 << 28)                  @ burst policy = run mode
+        orr     r0, r0, #(4 << 4)               @ use PLLP in run mode burst
+        str     r0, [r5, #CLK_RESET_CCLK_BURST]
+        mov     r0, #0
+        str     r0, [r5, #CLK_RESET_CCLK_DIVIDER]
+        mov     pc, lr
+ENDPROC(tegra_switch_cpu_to_pllp)
 #endif
diff --git a/arch/arm/mach-tegra/sleep.h b/arch/arm/mach-tegra/sleep.h
index 56505c381ea8..4ffae541726e 100644
--- a/arch/arm/mach-tegra/sleep.h
+++ b/arch/arm/mach-tegra/sleep.h
@@ -25,6 +25,19 @@
                                         + IO_PPSB_VIRT)
 #define TEGRA_CLK_RESET_VIRT (TEGRA_CLK_RESET_BASE - IO_PPSB_PHYS \
                                         + IO_PPSB_VIRT)
+#define TEGRA_PMC_VIRT  (TEGRA_PMC_BASE - IO_APB_PHYS + IO_APB_VIRT)
+
+/* PMC_SCRATCH37-39 and 41 are used for tegra_pen_lock and idle */
+#define PMC_SCRATCH37   0x130
+#define PMC_SCRATCH38   0x134
+#define PMC_SCRATCH39   0x138
+#define PMC_SCRATCH41   0x140
+
+#ifdef CONFIG_ARCH_TEGRA_2x_SOC
+#define CPU_RESETTABLE          2
+#define CPU_RESETTABLE_SOON     1
+#define CPU_NOT_RESETTABLE      0
+#endif
 
 #ifdef __ASSEMBLY__
 /* returns the offset of the flow controller halt register for a cpu */
@@ -104,6 +117,8 @@ exit_l2_resume:
 .endm
 #endif /* CONFIG_CACHE_L2X0 */
 #else
+void tegra_pen_lock(void);
+void tegra_pen_unlock(void);
 void tegra_resume(void);
 int tegra_sleep_cpu_finish(unsigned long);
 void tegra_disable_clean_inv_dcache(void);
@@ -116,6 +131,17 @@ static inline void tegra20_hotplug_init(void) {}
 static inline void tegra30_hotplug_init(void) {}
 #endif
 
+void tegra20_cpu_shutdown(int cpu);
+int tegra20_cpu_is_resettable_soon(void);
+void tegra20_cpu_clear_resettable(void);
+#ifdef CONFIG_ARCH_TEGRA_2x_SOC
+void tegra20_cpu_set_resettable_soon(void);
+#else
+static inline void tegra20_cpu_set_resettable_soon(void) {}
+#endif
+
+int tegra20_sleep_cpu_secondary_finish(unsigned long);
+void tegra20_tear_down_cpu(void);
 int tegra30_sleep_cpu_secondary_finish(unsigned long);
 void tegra30_tear_down_cpu(void);