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 @@ static int tegra30_boot_secondary(unsigned int cpu, struct task_struct *idle)
 
 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);
diff --git a/drivers/clk/tegra/clk-tegra114.c b/drivers/clk/tegra/clk-tegra114.c
index d78e16ee161c..40d939d091bf 100644
--- a/drivers/clk/tegra/clk-tegra114.c
+++ b/drivers/clk/tegra/clk-tegra114.c
@@ -250,6 +250,9 @@
 #define CLK_SOURCE_XUSB_DEV_SRC 0x60c
 #define CLK_SOURCE_EMC 0x19c
 
+/* Tegra CPU clock and reset control regs */
+#define CLK_RST_CONTROLLER_CPU_CMPLX_STATUS	0x470
+
 static int periph_clk_enb_refcnt[CLK_OUT_ENB_NUM * 32];
 
 static void __iomem *clk_base;
@@ -2000,7 +2003,25 @@ static __init void tegra114_periph_clk_init(void __iomem *clk_base)
 	}
 }
 
-static struct tegra_cpu_car_ops tegra114_cpu_car_ops;
+/* Tegra114 CPU clock and reset control functions */
+static void tegra114_wait_cpu_in_reset(u32 cpu)
+{
+	unsigned int reg;
+
+	do {
+		reg = readl(clk_base + CLK_RST_CONTROLLER_CPU_CMPLX_STATUS);
+		cpu_relax();
+	} while (!(reg & (1 << cpu)));  /* check CPU been reset or not */
+}
+static void tegra114_disable_cpu_clock(u32 cpu)
+{
+	/* flow controller would take care in the power sequence. */
+}
+
+static struct tegra_cpu_car_ops tegra114_cpu_car_ops = {
+	.wait_for_reset	= tegra114_wait_cpu_in_reset,
+	.disable_clock	= tegra114_disable_cpu_clock,
+};
 
 static const struct of_device_id pmc_match[] __initconst = {
 	{ .compatible = "nvidia,tegra114-pmc" },