18 files changed, 182 insertions, 93 deletions

diff --git a/arch/arm/mach-tegra/Makefile b/arch/arm/mach-tegra/Makefile
index d011f0ad49c4..98b184efc110 100644
--- a/arch/arm/mach-tegra/Makefile
+++ b/arch/arm/mach-tegra/Makefile
@@ -30,6 +30,7 @@ obj-$(CONFIG_HOTPLUG_CPU)               += hotplug.o
 obj-$(CONFIG_TEGRA_PCI)                 += pcie.o
 
 obj-$(CONFIG_ARCH_TEGRA_114_SOC)        += tegra114_speedo.o
+obj-$(CONFIG_ARCH_TEGRA_114_SOC)        += sleep-tegra30.o
 ifeq ($(CONFIG_CPU_IDLE),y)
 obj-$(CONFIG_ARCH_TEGRA_114_SOC)        += cpuidle-tegra114.o
 endif
diff --git a/arch/arm/mach-tegra/common.c b/arch/arm/mach-tegra/common.c
index 9f852c6fe5b9..ec5836b1e713 100644
--- a/arch/arm/mach-tegra/common.c
+++ b/arch/arm/mach-tegra/common.c
@@ -29,6 +29,7 @@
 
 #include "board.h"
 #include "common.h"
+#include "cpuidle.h"
 #include "fuse.h"
 #include "iomap.h"
 #include "irq.h"
@@ -108,5 +109,6 @@ void __init tegra_init_early(void)
 void __init tegra_init_late(void)
 {
         tegra_init_suspend();
+        tegra_cpuidle_init();
         tegra_powergate_debugfs_init();
 }
diff --git a/arch/arm/mach-tegra/common.h b/arch/arm/mach-tegra/common.h
index 5900cc44f780..32f8eb3fe344 100644
--- a/arch/arm/mach-tegra/common.h
+++ b/arch/arm/mach-tegra/common.h
@@ -2,3 +2,4 @@ extern struct smp_operations tegra_smp_ops;
 
 extern int tegra_cpu_kill(unsigned int cpu);
 extern void tegra_cpu_die(unsigned int cpu);
+extern int tegra_cpu_disable(unsigned int cpu);
diff --git a/arch/arm/mach-tegra/cpuidle-tegra20.c b/arch/arm/mach-tegra/cpuidle-tegra20.c
index 0cdba8de8c77..706aa4215c36 100644
--- a/arch/arm/mach-tegra/cpuidle-tegra20.c
+++ b/arch/arm/mach-tegra/cpuidle-tegra20.c
@@ -177,7 +177,6 @@ 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())
@@ -193,16 +192,16 @@ static int tegra20_idle_lp2_coupled(struct cpuidle_device *dev,
 
         local_fiq_disable();
 
-        tegra_set_cpu_in_lp2(cpu);
+        tegra_set_cpu_in_lp2();
         cpu_pm_enter();
 
-        if (cpu == 0)
+        if (dev->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);
+        tegra_clear_cpu_in_lp2();
 
         local_fiq_enable();
 
@@ -214,8 +213,5 @@ static int tegra20_idle_lp2_coupled(struct cpuidle_device *dev,
 
 int __init tegra20_cpuidle_init(void)
 {
-#ifdef CONFIG_PM_SLEEP
-        tegra_tear_down_cpu = tegra20_tear_down_cpu;
-#endif
         return cpuidle_register(&tegra_idle_driver, cpu_possible_mask);
 }
diff --git a/arch/arm/mach-tegra/cpuidle-tegra30.c b/arch/arm/mach-tegra/cpuidle-tegra30.c
index 3cf9aca5f3ea..ed2a2a7bae4d 100644
--- a/arch/arm/mach-tegra/cpuidle-tegra30.c
+++ b/arch/arm/mach-tegra/cpuidle-tegra30.c
@@ -114,16 +114,15 @@ static int 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;
         bool last_cpu;
 
         local_fiq_disable();
 
-        last_cpu = tegra_set_cpu_in_lp2(cpu);
+        last_cpu = tegra_set_cpu_in_lp2();
         cpu_pm_enter();
 
-        if (cpu == 0) {
+        if (dev->cpu == 0) {
                 if (last_cpu)
                         entered_lp2 = tegra30_cpu_cluster_power_down(dev, drv,
                                                                      index);
@@ -134,7 +133,7 @@ static int tegra30_idle_lp2(struct cpuidle_device *dev,
         }
 
         cpu_pm_exit();
-        tegra_clear_cpu_in_lp2(cpu);
+        tegra_clear_cpu_in_lp2();
 
         local_fiq_enable();
 
@@ -146,8 +145,5 @@ static int tegra30_idle_lp2(struct cpuidle_device *dev,
 
 int __init tegra30_cpuidle_init(void)
 {
-#ifdef CONFIG_PM_SLEEP
-        tegra_tear_down_cpu = tegra30_tear_down_cpu;
-#endif
         return cpuidle_register(&tegra_idle_driver, NULL);
 }
diff --git a/arch/arm/mach-tegra/cpuidle.c b/arch/arm/mach-tegra/cpuidle.c
index 4b744c4661e2..e85973cef037 100644
--- a/arch/arm/mach-tegra/cpuidle.c
+++ b/arch/arm/mach-tegra/cpuidle.c
@@ -27,25 +27,20 @@
 #include "fuse.h"
 #include "cpuidle.h"
 
-static int __init tegra_cpuidle_init(void)
+void __init tegra_cpuidle_init(void)
 {
-        int ret;
-
         switch (tegra_chip_id) {
         case TEGRA20:
-                ret = tegra20_cpuidle_init();
+                if (IS_ENABLED(CONFIG_ARCH_TEGRA_2x_SOC))
+                        tegra20_cpuidle_init();
                 break;
         case TEGRA30:
-                ret = tegra30_cpuidle_init();
+                if (IS_ENABLED(CONFIG_ARCH_TEGRA_3x_SOC))
+                        tegra30_cpuidle_init();
                 break;
         case TEGRA114:
-                ret = tegra114_cpuidle_init();
-                break;
-        default:
-                ret = -ENODEV;
+                if (IS_ENABLED(CONFIG_ARCH_TEGRA_114_SOC))
+                        tegra114_cpuidle_init();
                 break;
         }
-
-        return ret;
 }
-device_initcall(tegra_cpuidle_init);
diff --git a/arch/arm/mach-tegra/cpuidle.h b/arch/arm/mach-tegra/cpuidle.h
index d733f75d0208..9ec2c1ab0fa4 100644
--- a/arch/arm/mach-tegra/cpuidle.h
+++ b/arch/arm/mach-tegra/cpuidle.h
@@ -17,22 +17,13 @@
 #ifndef __MACH_TEGRA_CPUIDLE_H
 #define __MACH_TEGRA_CPUIDLE_H
 
-#ifdef CONFIG_ARCH_TEGRA_2x_SOC
+#ifdef CONFIG_CPU_IDLE
 int tegra20_cpuidle_init(void);
-#else
-static inline int tegra20_cpuidle_init(void) { return -ENODEV; }
-#endif
-
-#ifdef CONFIG_ARCH_TEGRA_3x_SOC
 int tegra30_cpuidle_init(void);
-#else
-static inline int tegra30_cpuidle_init(void) { return -ENODEV; }
-#endif
-
-#ifdef CONFIG_ARCH_TEGRA_114_SOC
 int tegra114_cpuidle_init(void);
+void tegra_cpuidle_init(void);
 #else
-static inline int tegra114_cpuidle_init(void) { return -ENODEV; }
+static inline void tegra_cpuidle_init(void) {}
 #endif
 
 #endif
diff --git a/arch/arm/mach-tegra/flowctrl.h b/arch/arm/mach-tegra/flowctrl.h
index 67eab56699bd..7a29bae799a7 100644
--- a/arch/arm/mach-tegra/flowctrl.h
+++ b/arch/arm/mach-tegra/flowctrl.h
@@ -25,6 +25,7 @@
 #define FLOW_CTRL_WAITEVENT             (2 << 29)
 #define FLOW_CTRL_WAIT_FOR_INTERRUPT    (4 << 29)
 #define FLOW_CTRL_JTAG_RESUME           (1 << 28)
+#define FLOW_CTRL_SCLK_RESUME           (1 << 27)
 #define FLOW_CTRL_HALT_CPU_IRQ          (1 << 10)
 #define FLOW_CTRL_HALT_CPU_FIQ          (1 << 8)
 #define FLOW_CTRL_CPU0_CSR              0x8
diff --git a/arch/arm/mach-tegra/fuse.h b/arch/arm/mach-tegra/fuse.h
index aacc00d05980..def79683bef6 100644
--- a/arch/arm/mach-tegra/fuse.h
+++ b/arch/arm/mach-tegra/fuse.h
@@ -19,16 +19,6 @@
 #ifndef __MACH_TEGRA_FUSE_H
 #define __MACH_TEGRA_FUSE_H
 
-enum tegra_revision {
-        TEGRA_REVISION_UNKNOWN = 0,
-        TEGRA_REVISION_A01,
-        TEGRA_REVISION_A02,
-        TEGRA_REVISION_A03,
-        TEGRA_REVISION_A03p,
-        TEGRA_REVISION_A04,
-        TEGRA_REVISION_MAX,
-};
-
 #define SKU_ID_T20      8
 #define SKU_ID_T25SE    20
 #define SKU_ID_AP25     23
@@ -40,6 +30,17 @@ enum tegra_revision {
 #define TEGRA30         0x30
 #define TEGRA114        0x35
 
+#ifndef __ASSEMBLY__
+enum tegra_revision {
+        TEGRA_REVISION_UNKNOWN = 0,
+        TEGRA_REVISION_A01,
+        TEGRA_REVISION_A02,
+        TEGRA_REVISION_A03,
+        TEGRA_REVISION_A03p,
+        TEGRA_REVISION_A04,
+        TEGRA_REVISION_MAX,
+};
+
 extern int tegra_sku_id;
 extern int tegra_cpu_process_id;
 extern int tegra_core_process_id;
@@ -72,5 +73,6 @@ void tegra114_init_speedo_data(void);
 #else
 static inline void tegra114_init_speedo_data(void) {}
 #endif
+#endif /* __ASSEMBLY__ */
 
 #endif
diff --git a/arch/arm/mach-tegra/hotplug.c b/arch/arm/mach-tegra/hotplug.c
index 184914a68d73..a52c10e0a857 100644
--- a/arch/arm/mach-tegra/hotplug.c
+++ b/arch/arm/mach-tegra/hotplug.c
@@ -46,6 +46,17 @@ void __ref tegra_cpu_die(unsigned int cpu)
         BUG();
 }
 
+int tegra_cpu_disable(unsigned int cpu)
+{
+        switch (tegra_chip_id) {
+        case TEGRA20:
+        case TEGRA30:
+                return cpu == 0 ? -EPERM : 0;
+        default:
+                return 0;
+        }
+}
+
 void __init tegra_hotplug_init(void)
 {
         if (!IS_ENABLED(CONFIG_HOTPLUG_CPU))
@@ -55,4 +66,6 @@ void __init tegra_hotplug_init(void)
                 tegra_hotplug_shutdown = tegra20_hotplug_shutdown;
         if (IS_ENABLED(CONFIG_ARCH_TEGRA_3x_SOC) && tegra_chip_id == TEGRA30)
                 tegra_hotplug_shutdown = tegra30_hotplug_shutdown;
+        if (IS_ENABLED(CONFIG_ARCH_TEGRA_114_SOC) && tegra_chip_id == TEGRA114)
+                tegra_hotplug_shutdown = tegra30_hotplug_shutdown;
 }
diff --git a/arch/arm/mach-tegra/platsmp.c b/arch/arm/mach-tegra/platsmp.c
index fad4226ef710..24db4ac428ae 100644
--- a/arch/arm/mach-tegra/platsmp.c
+++ b/arch/arm/mach-tegra/platsmp.c
@@ -140,8 +140,31 @@ remove_clamps:
 
 static int tegra114_boot_secondary(unsigned int cpu, struct task_struct *idle)
 {
+        int ret = 0;
+
         cpu = cpu_logical_map(cpu);
-        return tegra_pmc_cpu_power_on(cpu);
+
+        if (cpumask_test_cpu(cpu, &tegra_cpu_init_mask)) {
+                /*
+                 * Warm boot flow
+                 * The flow controller in charge of the power state and
+                 * control for each CPU.
+                 */
+                /* set SCLK as event trigger for flow controller */
+                flowctrl_write_cpu_csr(cpu, 1);
+                flowctrl_write_cpu_halt(cpu,
+                                FLOW_CTRL_WAITEVENT | FLOW_CTRL_SCLK_RESUME);
+        } else {
+                /*
+                 * Cold boot flow
+                 * The CPU is powered up by toggling PMC directly. It will
+                 * also initial power state in flow controller. After that,
+                 * the CPU's power state is maintained by flow controller.
+                 */
+                ret = tegra_pmc_cpu_power_on(cpu);
+        }
+
+        return ret;
 }
 
 static int __cpuinit tegra_boot_secondary(unsigned int cpu,
@@ -173,5 +196,6 @@ struct smp_operations tegra_smp_ops __initdata = {
 #ifdef CONFIG_HOTPLUG_CPU
         .cpu_kill               = tegra_cpu_kill,
         .cpu_die                = tegra_cpu_die,
+        .cpu_disable            = tegra_cpu_disable,
 #endif
 };
diff --git a/arch/arm/mach-tegra/pm.c b/arch/arm/mach-tegra/pm.c
index 45cf52c7e528..94e69bee3da5 100644
--- a/arch/arm/mach-tegra/pm.c
+++ b/arch/arm/mach-tegra/pm.c
@@ -44,6 +44,20 @@
 static DEFINE_SPINLOCK(tegra_lp2_lock);
 void (*tegra_tear_down_cpu)(void);
 
+static void tegra_tear_down_cpu_init(void)
+{
+        switch (tegra_chip_id) {
+        case TEGRA20:
+                if (IS_ENABLED(CONFIG_ARCH_TEGRA_2x_SOC))
+                        tegra_tear_down_cpu = tegra20_tear_down_cpu;
+                break;
+        case TEGRA30:
+                if (IS_ENABLED(CONFIG_ARCH_TEGRA_3x_SOC))
+                        tegra_tear_down_cpu = tegra30_tear_down_cpu;
+                break;
+        }
+}
+
 /*
  * restore_cpu_complex
  *
@@ -91,8 +105,9 @@ static void suspend_cpu_complex(void)
         flowctrl_cpu_suspend_enter(cpu);
 }
 
-void tegra_clear_cpu_in_lp2(int phy_cpu_id)
+void tegra_clear_cpu_in_lp2(void)
 {
+        int phy_cpu_id = cpu_logical_map(smp_processor_id());
         u32 *cpu_in_lp2 = tegra_cpu_lp2_mask;
 
         spin_lock(&tegra_lp2_lock);
@@ -103,8 +118,9 @@ void tegra_clear_cpu_in_lp2(int phy_cpu_id)
         spin_unlock(&tegra_lp2_lock);
 }
 
-bool tegra_set_cpu_in_lp2(int phy_cpu_id)
+bool tegra_set_cpu_in_lp2(void)
 {
+        int phy_cpu_id = cpu_logical_map(smp_processor_id());
         bool last_cpu = false;
         cpumask_t *cpu_lp2_mask = tegra_cpu_lp2_mask;
         u32 *cpu_in_lp2 = tegra_cpu_lp2_mask;
@@ -192,7 +208,7 @@ static int __cpuinit tegra_suspend_enter(suspend_state_t state)
         suspend_cpu_complex();
         switch (mode) {
         case TEGRA_SUSPEND_LP2:
-                tegra_set_cpu_in_lp2(0);
+                tegra_set_cpu_in_lp2();
                 break;
         default:
                 break;
@@ -202,7 +218,7 @@ static int __cpuinit tegra_suspend_enter(suspend_state_t state)
 
         switch (mode) {
         case TEGRA_SUSPEND_LP2:
-                tegra_clear_cpu_in_lp2(0);
+                tegra_clear_cpu_in_lp2();
                 break;
         default:
                 break;
@@ -224,6 +240,7 @@ void __init tegra_init_suspend(void)
         if (tegra_pmc_get_suspend_mode() == TEGRA_SUSPEND_NONE)
                 return;
 
+        tegra_tear_down_cpu_init();
         tegra_pmc_suspend_init();
 
         suspend_set_ops(&tegra_suspend_ops);
diff --git a/arch/arm/mach-tegra/pm.h b/arch/arm/mach-tegra/pm.h
index 778a4aa7c3fa..94c4b9d9077c 100644
--- a/arch/arm/mach-tegra/pm.h
+++ b/arch/arm/mach-tegra/pm.h
@@ -28,8 +28,8 @@ extern unsigned long l2x0_saved_regs_addr;
 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);
+void tegra_clear_cpu_in_lp2(void);
+bool tegra_set_cpu_in_lp2(void);
 
 void tegra_idle_lp2_last(void);
 extern void (*tegra_tear_down_cpu)(void);
diff --git a/arch/arm/mach-tegra/reset-handler.S b/arch/arm/mach-tegra/reset-handler.S
index e6de88a2ea06..39dc9e7834f3 100644
--- a/arch/arm/mach-tegra/reset-handler.S
+++ b/arch/arm/mach-tegra/reset-handler.S
@@ -22,11 +22,11 @@
 #include <asm/hardware/cache-l2x0.h>
 
 #include "flowctrl.h"
+#include "fuse.h"
 #include "iomap.h"
 #include "reset.h"
 #include "sleep.h"
 
-#define APB_MISC_GP_HIDREV      0x804
 #define PMC_SCRATCH41   0x140
 
 #define RESET_DATA(x)   ((TEGRA_RESET_##x)*4)
@@ -38,34 +38,40 @@
  *        CPU boot vector when restarting the a CPU following
  *        an LP2 transition. Also branched to by LP0 and LP1 resume after
  *        re-enabling sdram.
+ *
+ *      r6: SoC ID
  */
 ENTRY(tegra_resume)
         bl      v7_invalidate_l1
 
         cpu_id  r0
+        tegra_get_soc_id TEGRA_APB_MISC_BASE, r6
+        cmp     r6, #TEGRA114
+        beq     no_cpu0_chk
+
         cmp     r0, #0                          @ CPU0?
  THUMB( it      ne )
         bne     cpu_resume                      @ no
+no_cpu0_chk:
 
-#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)
+        cmp     r6, #TEGRA20
         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]
+        cpu_to_csr_reg r1, r0
+        mov32   r2, TEGRA_FLOW_CTRL_BASE
+        ldr     r1, [r2, r1]
         /* 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
+        movw    r0, #0x3FFD     @ enable, cluster_switch, immed, bitmaps
+                                @ & ext flags for CPU power mgnt
         bic     r1, r1, r0
         str     r1, [r2]
 1:
-#endif
 
+        check_cpu_part_num 0xc09, r8, r9
+        bne     not_ca9
 #ifdef CONFIG_HAVE_ARM_SCU
         /* enable SCU */
         mov32   r0, TEGRA_ARM_PERIF_BASE
@@ -76,6 +82,7 @@ ENTRY(tegra_resume)
 
         /* L2 cache resume & re-enable */
         l2_cache_resume r0, r1, r2, l2x0_saved_regs_addr
+not_ca9:
 
         b       cpu_resume
 ENDPROC(tegra_resume)
@@ -98,7 +105,7 @@ ENTRY(__tegra_cpu_reset_handler_start)
  * Register usage within the reset handler:
  *
  *      Others: scratch
- *      R6  = SoC ID << 8
+ *      R6  = SoC ID
  *      R7  = CPU present (to the OS) mask
  *      R8  = CPU in LP1 state mask
  *      R9  = CPU in LP2 state mask
@@ -115,12 +122,10 @@ ENTRY(__tegra_cpu_reset_handler)
 
         cpsid   aif, 0x13                       @ SVC mode, interrupts disabled
 
-        mov32   r6, TEGRA_APB_MISC_BASE
-        ldr     r6, [r6, #APB_MISC_GP_HIDREV]
-        and     r6, r6, #0xff00
+        tegra_get_soc_id TEGRA_APB_MISC_BASE, r6
 #ifdef CONFIG_ARCH_TEGRA_2x_SOC
 t20_check:
-        cmp     r6, #(0x20 << 8)
+        cmp     r6, #TEGRA20
         bne     after_t20_check
 t20_errata:
         # Tegra20 is a Cortex-A9 r1p1
@@ -136,7 +141,7 @@ after_t20_check:
 #endif
 #ifdef CONFIG_ARCH_TEGRA_3x_SOC
 t30_check:
-        cmp     r6, #(0x30 << 8)
+        cmp     r6, #TEGRA30
         bne     after_t30_check
 t30_errata:
         # Tegra30 is a Cortex-A9 r2p9
@@ -163,7 +168,7 @@ after_errata:
 
 #ifdef CONFIG_ARCH_TEGRA_2x_SOC
         /* Are we on Tegra20? */
-        cmp     r6, #(0x20 << 8)
+        cmp     r6, #TEGRA20
         bne     1f
         /* If not CPU0, don't let CPU0 reset CPU1 now that CPU1 is coming up. */
         mov32   r5, TEGRA_PMC_BASE
@@ -186,11 +191,14 @@ __is_not_lp2:
 
 #ifdef CONFIG_SMP
         /*
-         * Can only be secondary boot (initial or hotplug) but CPU 0
-         * cannot be here.
+         * Can only be secondary boot (initial or hotplug)
+         * CPU0 can't be here for Tegra20/30
          */
+        cmp     r6, #TEGRA114
+        beq     __no_cpu0_chk
         cmp     r10, #0
         bleq    __die                           @ CPU0 cannot be here
+__no_cpu0_chk:
         ldr     lr, [r12, #RESET_DATA(STARTUP_SECONDARY)]
         cmp     lr, #0
         bleq    __die                           @ no secondary startup handler
@@ -210,10 +218,7 @@ __die:
         mov32   r7, TEGRA_CLK_RESET_BASE
 
         /* 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)
+        cmp     r6, #TEGRA20
         bne     1f
 
 #ifdef CONFIG_ARCH_TEGRA_2x_SOC
diff --git a/arch/arm/mach-tegra/sleep-tegra30.S b/arch/arm/mach-tegra/sleep-tegra30.S
index d29dfcce948d..ada8821b48be 100644
--- a/arch/arm/mach-tegra/sleep-tegra30.S
+++ b/arch/arm/mach-tegra/sleep-tegra30.S
@@ -19,6 +19,7 @@
 #include <asm/assembler.h>
 #include <asm/asm-offsets.h>
 
+#include "fuse.h"
 #include "sleep.h"
 #include "flowctrl.h"
 
@@ -43,14 +44,19 @@ ENDPROC(tegra30_hotplug_shutdown)
  *
  * Puts the current CPU in wait-for-event mode on the flow controller
  * and powergates it -- flags (in R0) indicate the request type.
- * Must never be called for CPU 0.
  *
- * corrupts r0-r4, r12
+ * r10 = SoC ID
+ * corrupts r0-r4, r10-r12
  */
 ENTRY(tegra30_cpu_shutdown)
         cpu_id  r3
+        tegra_get_soc_id TEGRA_APB_MISC_VIRT, r10
+        cmp     r10, #TEGRA30
+        bne     _no_cpu0_chk    @ It's not Tegra30
+
         cmp     r3, #0
         moveq   pc, lr          @ Must never be called for CPU 0
+_no_cpu0_chk:
 
         ldr     r12, =TEGRA_FLOW_CTRL_VIRT
         cpu_to_csr_reg r1, r3
@@ -65,7 +71,9 @@ ENTRY(tegra30_cpu_shutdown)
         movw    r12, \
                 FLOW_CTRL_CSR_INTR_FLAG | FLOW_CTRL_CSR_EVENT_FLAG | \
                 FLOW_CTRL_CSR_ENABLE
-        mov     r4, #(1 << 4)
+        cmp     r10, #TEGRA30
+        moveq   r4, #(1 << 4)                   @ wfe bitmap
+        movne   r4, #(1 << 8)                   @ wfi bitmap
  ARM(   orr     r12, r12, r4, lsl r3    )
  THUMB( lsl     r4, r4, r3              )
  THUMB( orr     r12, r12, r4            )
@@ -79,9 +87,20 @@ delay_1:
         cpsid   a                               @ disable imprecise aborts.
         ldr     r3, [r1]                        @ read CSR
         str     r3, [r1]                        @ clear CSR
+
         tst     r0, #TEGRA30_POWER_HOTPLUG_SHUTDOWN
+        beq     flow_ctrl_setting_for_lp2
+
+        /* flow controller set up for hotplug */
+        mov     r3, #FLOW_CTRL_WAITEVENT                @ For hotplug
+        b       flow_ctrl_done
+flow_ctrl_setting_for_lp2:
+        /* flow controller set up for LP2 */
+        cmp     r10, #TEGRA30
         moveq   r3, #FLOW_CTRL_WAIT_FOR_INTERRUPT       @ For LP2
-        movne   r3, #FLOW_CTRL_WAITEVENT                @ For hotplug
+        movne   r3, #FLOW_CTRL_WAITEVENT
+flow_ctrl_done:
+        cmp     r10, #TEGRA30
         str     r3, [r2]
         ldr     r0, [r2]
         b       wfe_war
@@ -89,7 +108,8 @@ delay_1:
 __cpu_reset_again:
         dsb
         .align 5
-        wfe                                     @ CPU should be power gated here
+        wfeeq                                   @ CPU should be power gated here
+        wfine
 wfe_war:
         b       __cpu_reset_again
 
diff --git a/arch/arm/mach-tegra/sleep.S b/arch/arm/mach-tegra/sleep.S
index 364d84523fba..9daaef26b0f6 100644
--- a/arch/arm/mach-tegra/sleep.S
+++ b/arch/arm/mach-tegra/sleep.S
@@ -106,9 +106,11 @@ ENTRY(tegra_shut_off_mmu)
         isb
 #ifdef CONFIG_CACHE_L2X0
         /* Disable L2 cache */
-        mov32   r4, TEGRA_ARM_PERIF_BASE + 0x3000
-        mov     r5, #0
-        str     r5, [r4, #L2X0_CTRL]
+        check_cpu_part_num 0xc09, r9, r10
+        movweq  r4, #:lower16:(TEGRA_ARM_PERIF_BASE + 0x3000)
+        movteq  r4, #:upper16:(TEGRA_ARM_PERIF_BASE + 0x3000)
+        moveq   r5, #0
+        streq   r5, [r4, #L2X0_CTRL]
 #endif
         mov     pc, r0
 ENDPROC(tegra_shut_off_mmu)
diff --git a/arch/arm/mach-tegra/sleep.h b/arch/arm/mach-tegra/sleep.h
index 2080fb12ce26..98b7da698f2b 100644
--- a/arch/arm/mach-tegra/sleep.h
+++ b/arch/arm/mach-tegra/sleep.h
@@ -25,6 +25,8 @@
                                         + IO_PPSB_VIRT)
 #define TEGRA_CLK_RESET_VIRT (TEGRA_CLK_RESET_BASE - IO_PPSB_PHYS \
                                         + IO_PPSB_VIRT)
+#define TEGRA_APB_MISC_VIRT (TEGRA_APB_MISC_BASE - IO_APB_PHYS \
+                                        + IO_APB_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 */
@@ -70,19 +72,40 @@
         movt    \reg, #:upper16:\val
 .endm
 
+/* Marco to check CPU part num */
+.macro check_cpu_part_num part_num, tmp1, tmp2
+        mrc     p15, 0, \tmp1, c0, c0, 0
+        ubfx    \tmp1, \tmp1, #4, #12
+        mov32   \tmp2, \part_num
+        cmp     \tmp1, \tmp2
+.endm
+
 /* Macro to exit SMP coherency. */
 .macro exit_smp, tmp1, tmp2
         mrc     p15, 0, \tmp1, c1, c0, 1        @ ACTLR
         bic     \tmp1, \tmp1, #(1<<6) | (1<<0)  @ clear ACTLR.SMP | ACTLR.FW
         mcr     p15, 0, \tmp1, c1, c0, 1        @ ACTLR
         isb
-        cpu_id  \tmp1
-        mov     \tmp1, \tmp1, lsl #2
-        mov     \tmp2, #0xf
-        mov     \tmp2, \tmp2, lsl \tmp1
-        mov32   \tmp1, TEGRA_ARM_PERIF_VIRT + 0xC
-        str     \tmp2, [\tmp1]                  @ invalidate SCU tags for CPU
+#ifdef CONFIG_HAVE_ARM_SCU
+        check_cpu_part_num 0xc09, \tmp1, \tmp2
+        mrceq   p15, 0, \tmp1, c0, c0, 5
+        andeq   \tmp1, \tmp1, #0xF
+        moveq   \tmp1, \tmp1, lsl #2
+        moveq   \tmp2, #0xf
+        moveq   \tmp2, \tmp2, lsl \tmp1
+        ldreq   \tmp1, =(TEGRA_ARM_PERIF_VIRT + 0xC)
+        streq   \tmp2, [\tmp1]                  @ invalidate SCU tags for CPU
         dsb
+#endif
+.endm
+
+/* Macro to check Tegra revision */
+#define APB_MISC_GP_HIDREV      0x804
+.macro tegra_get_soc_id base, tmp1
+        mov32   \tmp1, \base
+        ldr     \tmp1, [\tmp1, #APB_MISC_GP_HIDREV]
+        and     \tmp1, \tmp1, #0xff00
+        mov     \tmp1, \tmp1, lsr #8
 .endm
 
 /* Macro to resume & re-enable L2 cache */
diff --git a/arch/arm/mach-tegra/tegra2_emc.c b/arch/arm/mach-tegra/tegra2_emc.c
index 31e69a019bdd..3ae4a7f1a2fb 100644
--- a/arch/arm/mach-tegra/tegra2_emc.c
+++ b/arch/arm/mach-tegra/tegra2_emc.c
@@ -183,7 +183,7 @@ static struct device_node *tegra_emc_ramcode_devnode(struct device_node *np)
         u32 reg;
 
         for_each_child_of_node(np, iter) {
-                if (of_property_read_u32(np, "nvidia,ram-code", &reg))
+                if (of_property_read_u32(iter, "nvidia,ram-code", &reg))
                         continue;
                 if (reg == tegra_bct_strapping)
                         return of_node_get(iter);