x86/kprobes: Fix crash when probing CS CALL instructionsgrafted

When using eBPF to probe CS CALL instructions within a function,
a crash can be triggered.

The eBPF tool probes offset 257 of the __hrtimer_run_queues()
function:

<__hrtimer_run_queues+249>:  nopl   0x0(%rax,%rax,1)
<__hrtimer_run_queues+254>:  mov    %r14,%rdi
<__hrtimer_run_queues+257>:  cs call <__x86_indirect_thunk_r12>
<__hrtimer_run_queues+263>:  mov    %eax,%r12d
<__hrtimer_run_queues+266>:  xchg   %ax,%ax
<__hrtimer_run_queues+268>:  mov    %r13,%rdi

Which triggers this crash:

  BUG: unable to handle page fault for address: 00000000000f41c9
  #PF: supervisor write access in kernel mode
  #PF: error_code(0x0002) - not-present page
  PGD 0 P4D 0
  Oops: 0002 [#1] SMP NOPTI
  CPU: 1 PID: 0 Comm: swapper/1 Kdump: loaded Tainted: P
  RIP: 0010:__hrtimer_run_queues+0x106/0x230

Note that __hrtimer_run_queues+0x106 is __hrtimer_run_queues+262, which is
at the 6th byte of the above CS CALL instruction. Since the CS CALL
instruction occupies 6 bytes, the exception occurred in the middle of that
call instruction.

The root cause is that when using eBPF tools to probe in the middle of a
function, a kprobe with INT3 is used as the underlying implementation.

During single-step emulation of the original CALL instruction,
int3_emulate_call() assumes that the probed CALL instruction is 5 bytes
long. However, the actual CS-prefixed CALL instruction occupies 6 bytes,
so it constructs an incorrect exception return address. When the CPU
returns from the kprobe handler, the next instruction to be executed is at
the address of the last byte of that CS CALL instruction. Coincidentally,
starting from that address, the CPU fetches and decodes a completely
different instruction, which ultimately triggers a kernel crash.

Fix the issue by using the actual instruction length obtained from
the instruction decoder when constructing the exception return
address, rather than relying on the hardcoded CALL_INSN_SIZE macro.

[ mingo: Refined the changelog ]

Fixes: 6256e668b7af ("x86/kprobes: Use int3 instead of debug trap for single-step")
Suggested-by: Masami Hiramatsu (Google) <mhiramat@kernel.org>
Signed-off-by: Jinke Han <jinkehan@didiglobal.com>
Signed-off-by: Ingo Molnar <mingo@kernel.org>
Reviewed-by: Masami Hiramatsu (Google) <mhiramat@kernel.org>
Acked-by: Yafang Shao <laoar.shao@gmail.com>
Acked-by: Borislav Petkov <bp@alien8.de>
Cc: Peter Zijlstra <peterz@infradead.org>
Link: https://patch.msgid.link/20260908073742.GA10517@didi-ThinkCentre-M920t-N000

22 files changed, 3374 insertions, 0 deletions

diff --git a/arch/arm/mach-tegra/Kconfig b/arch/arm/mach-tegra/Kconfig
new file mode 100644
index 000000000..3a06ba263
--- /dev/null
+++ b/arch/arm/mach-tegra/Kconfig
@@ -0,0 +1,19 @@
+# SPDX-License-Identifier: GPL-2.0
+menuconfig ARCH_TEGRA
+	bool "NVIDIA Tegra"
+	depends on ARCH_MULTI_V7
+	select ARCH_HAS_RESET_CONTROLLER
+	select ARM_AMBA
+	select ARM_GIC
+	select CLKSRC_MMIO
+	select GPIOLIB
+	select HAVE_ARM_SCU if SMP
+	select HAVE_ARM_TWD if SMP
+	select PINCTRL
+	select PM
+	select PM_OPP
+	select RESET_CONTROLLER
+	select SOC_BUS
+	select ZONE_DMA if ARM_LPAE
+	help
+	  This enables support for NVIDIA Tegra based systems.
diff --git a/arch/arm/mach-tegra/Makefile b/arch/arm/mach-tegra/Makefile
new file mode 100644
index 000000000..9e3abb14f
--- /dev/null
+++ b/arch/arm/mach-tegra/Makefile
@@ -0,0 +1,17 @@
+# SPDX-License-Identifier: GPL-2.0
+obj-y                                   += io.o
+obj-y                                   += irq.o
+obj-y					+= pm.o
+obj-y					+= reset.o
+obj-y					+= reset-handler.o
+obj-y					+= sleep.o
+obj-y					+= tegra.o
+obj-y					+= sleep-tegra20.o
+obj-y					+= sleep-tegra30.o
+obj-$(CONFIG_ARCH_TEGRA_2x_SOC)		+= pm-tegra20.o
+obj-$(CONFIG_ARCH_TEGRA_3x_SOC)		+= pm-tegra30.o
+obj-$(CONFIG_SMP)			+= platsmp.o
+obj-$(CONFIG_HOTPLUG_CPU)               += hotplug.o
+
+obj-$(CONFIG_ARCH_TEGRA_114_SOC)	+= pm-tegra30.o
+obj-$(CONFIG_ARCH_TEGRA_124_SOC)	+= pm-tegra30.o
diff --git a/arch/arm/mach-tegra/board.h b/arch/arm/mach-tegra/board.h
new file mode 100644
index 000000000..86c3ea0d6
--- /dev/null
+++ b/arch/arm/mach-tegra/board.h
@@ -0,0 +1,22 @@
+/* SPDX-License-Identifier: GPL-2.0-only */
+/*
+ * arch/arm/mach-tegra/board.h
+ *
+ * Copyright (c) 2013 NVIDIA Corporation. All rights reserved.
+ * Copyright (C) 2010 Google, Inc.
+ *
+ * Author:
+ *	Colin Cross <ccross@google.com>
+ *	Erik Gilling <konkers@google.com>
+ */
+
+#ifndef __MACH_TEGRA_BOARD_H
+#define __MACH_TEGRA_BOARD_H
+
+#include <linux/types.h>
+#include <linux/reboot.h>
+
+void __init tegra_map_common_io(void);
+void __init tegra_init_irq(void);
+
+#endif
diff --git a/arch/arm/mach-tegra/common.h b/arch/arm/mach-tegra/common.h
new file mode 100644
index 000000000..d59387c4d
--- /dev/null
+++ b/arch/arm/mach-tegra/common.h
@@ -0,0 +1,15 @@
+/* SPDX-License-Identifier: GPL-2.0-only */
+/*
+ * Copyright (c) 2011, ARM Ltd.
+ * Copyright (c) 2013, NVIDIA Corporation. All rights reserved.
+ */
+
+#ifndef __MACH_TEGRA_COMMON_H
+#define __MACH_TEGRA_COMMON_H
+
+extern const struct smp_operations tegra_smp_ops;
+
+extern int tegra_cpu_kill(unsigned int cpu);
+extern void tegra_cpu_die(unsigned int cpu);
+
+#endif
diff --git a/arch/arm/mach-tegra/hotplug.c b/arch/arm/mach-tegra/hotplug.c
new file mode 100644
index 000000000..5c907c2c0
--- /dev/null
+++ b/arch/arm/mach-tegra/hotplug.c
@@ -0,0 +1,74 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ *  Copyright (C) 2002 ARM Ltd.
+ *  All Rights Reserved
+ *  Copyright (c) 2010, 2012-2013, NVIDIA Corporation. All rights reserved.
+ */
+
+#include <linux/clk/tegra.h>
+#include <linux/kernel.h>
+#include <linux/smp.h>
+
+#include <soc/tegra/common.h>
+#include <soc/tegra/fuse.h>
+
+#include <asm/smp_plat.h>
+
+#include "common.h"
+#include "sleep.h"
+
+static void (*tegra_hotplug_shutdown)(void);
+
+int tegra_cpu_kill(unsigned cpu)
+{
+	cpu = cpu_logical_map(cpu);
+
+	/* Clock gate the CPU */
+	tegra_wait_cpu_in_reset(cpu);
+	tegra_disable_cpu_clock(cpu);
+
+	return 1;
+}
+
+/*
+ * platform-specific code to shutdown a CPU
+ *
+ * Called with IRQs disabled
+ */
+void tegra_cpu_die(unsigned int cpu)
+{
+	if (!tegra_hotplug_shutdown) {
+		WARN(1, "hotplug is not yet initialized\n");
+		return;
+	}
+
+	/* Clean L1 data cache */
+	tegra_disable_clean_inv_dcache(TEGRA_FLUSH_CACHE_LOUIS);
+
+	/* Shut down the current CPU. */
+	tegra_hotplug_shutdown();
+
+	/* Should never return here. */
+	BUG();
+}
+
+static int __init tegra_hotplug_init(void)
+{
+	if (!IS_ENABLED(CONFIG_HOTPLUG_CPU))
+		return 0;
+
+	if (!soc_is_tegra())
+		return 0;
+
+	if (IS_ENABLED(CONFIG_ARCH_TEGRA_2x_SOC) && tegra_get_chip_id() == TEGRA20)
+		tegra_hotplug_shutdown = tegra20_hotplug_shutdown;
+	if (IS_ENABLED(CONFIG_ARCH_TEGRA_3x_SOC) && tegra_get_chip_id() == TEGRA30)
+		tegra_hotplug_shutdown = tegra30_hotplug_shutdown;
+	if (IS_ENABLED(CONFIG_ARCH_TEGRA_114_SOC) && tegra_get_chip_id() == TEGRA114)
+		tegra_hotplug_shutdown = tegra30_hotplug_shutdown;
+	if (IS_ENABLED(CONFIG_ARCH_TEGRA_124_SOC) && tegra_get_chip_id() == TEGRA124)
+		tegra_hotplug_shutdown = tegra30_hotplug_shutdown;
+
+	return 0;
+}
+pure_initcall(tegra_hotplug_init);
diff --git a/arch/arm/mach-tegra/io.c b/arch/arm/mach-tegra/io.c
new file mode 100644
index 000000000..afdeb3205
--- /dev/null
+++ b/arch/arm/mach-tegra/io.c
@@ -0,0 +1,55 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * arch/arm/mach-tegra/io.c
+ *
+ * Copyright (C) 2010 Google, Inc.
+ *
+ * Author:
+ *	Colin Cross <ccross@google.com>
+ *	Erik Gilling <konkers@google.com>
+ */
+
+#include <linux/init.h>
+#include <linux/io.h>
+#include <linux/kernel.h>
+#include <linux/mm.h>
+#include <linux/module.h>
+
+#include <asm/mach/map.h>
+#include <asm/page.h>
+
+#include "board.h"
+#include "iomap.h"
+
+static struct map_desc tegra_io_desc[] __initdata = {
+	{
+		.virtual = (unsigned long)IO_PPSB_VIRT,
+		.pfn = __phys_to_pfn(IO_PPSB_PHYS),
+		.length = IO_PPSB_SIZE,
+		.type = MT_DEVICE,
+	},
+	{
+		.virtual = (unsigned long)IO_APB_VIRT,
+		.pfn = __phys_to_pfn(IO_APB_PHYS),
+		.length = IO_APB_SIZE,
+		.type = MT_DEVICE,
+	},
+	{
+		.virtual = (unsigned long)IO_CPU_VIRT,
+		.pfn = __phys_to_pfn(IO_CPU_PHYS),
+		.length = IO_CPU_SIZE,
+		.type = MT_DEVICE,
+	},
+	{
+		.virtual = (unsigned long)IO_IRAM_VIRT,
+		.pfn = __phys_to_pfn(IO_IRAM_PHYS),
+		.length = IO_IRAM_SIZE,
+		.type = MT_DEVICE,
+	},
+};
+
+void __init tegra_map_common_io(void)
+{
+	debug_ll_io_init();
+	iotable_init(tegra_io_desc, ARRAY_SIZE(tegra_io_desc));
+}
diff --git a/arch/arm/mach-tegra/iomap.h b/arch/arm/mach-tegra/iomap.h
new file mode 100644
index 000000000..4cb7e5fee
--- /dev/null
+++ b/arch/arm/mach-tegra/iomap.h
@@ -0,0 +1,129 @@
+/* SPDX-License-Identifier: GPL-2.0-only */
+/*
+ * Copyright (C) 2010 Google, Inc.
+ *
+ * Author:
+ *	Colin Cross <ccross@google.com>
+ *	Erik Gilling <konkers@google.com>
+ */
+
+#ifndef __MACH_TEGRA_IOMAP_H
+#define __MACH_TEGRA_IOMAP_H
+
+#include <linux/pgtable.h>
+#include <linux/sizes.h>
+
+#define TEGRA_IRAM_BASE			0x40000000
+#define TEGRA_IRAM_SIZE			SZ_256K
+
+#define TEGRA_ARM_PERIF_BASE		0x50040000
+#define TEGRA_ARM_PERIF_SIZE		SZ_8K
+
+#define TEGRA_ARM_INT_DIST_BASE		0x50041000
+#define TEGRA_ARM_INT_DIST_SIZE		SZ_4K
+
+#define TEGRA_TMR1_BASE			0x60005000
+#define TEGRA_TMR1_SIZE			SZ_8
+
+#define TEGRA_TMR2_BASE			0x60005008
+#define TEGRA_TMR2_SIZE			SZ_8
+
+#define TEGRA_TMRUS_BASE		0x60005010
+#define TEGRA_TMRUS_SIZE		SZ_64
+
+#define TEGRA_TMR3_BASE			0x60005050
+#define TEGRA_TMR3_SIZE			SZ_8
+
+#define TEGRA_TMR4_BASE			0x60005058
+#define TEGRA_TMR4_SIZE			SZ_8
+
+#define TEGRA_CLK_RESET_BASE		0x60006000
+#define TEGRA_CLK_RESET_SIZE		SZ_4K
+
+#define TEGRA_FLOW_CTRL_BASE		0x60007000
+#define TEGRA_FLOW_CTRL_SIZE		20
+
+#define TEGRA_SB_BASE			0x6000C200
+#define TEGRA_SB_SIZE			256
+
+#define TEGRA_EXCEPTION_VECTORS_BASE    0x6000F000
+#define TEGRA_EXCEPTION_VECTORS_SIZE    SZ_4K
+
+#define TEGRA_APB_MISC_BASE		0x70000000
+#define TEGRA_APB_MISC_SIZE		SZ_4K
+
+#define TEGRA_UARTA_BASE		0x70006000
+#define TEGRA_UARTA_SIZE		SZ_64
+
+#define TEGRA_UARTB_BASE		0x70006040
+#define TEGRA_UARTB_SIZE		SZ_64
+
+#define TEGRA_UARTC_BASE		0x70006200
+#define TEGRA_UARTC_SIZE		SZ_256
+
+#define TEGRA_UARTD_BASE		0x70006300
+#define TEGRA_UARTD_SIZE		SZ_256
+
+#define TEGRA_UARTE_BASE		0x70006400
+#define TEGRA_UARTE_SIZE		SZ_256
+
+#define TEGRA_PMC_BASE			0x7000E400
+#define TEGRA_PMC_SIZE			SZ_256
+
+#define TEGRA_EMC_BASE			0x7000F400
+#define TEGRA_EMC_SIZE			SZ_1K
+
+#define TEGRA_EMC0_BASE			0x7001A000
+#define TEGRA_EMC0_SIZE			SZ_2K
+
+#define TEGRA_EMC1_BASE			0x7001A800
+#define TEGRA_EMC1_SIZE			SZ_2K
+
+#define TEGRA124_EMC_BASE		0x7001B000
+#define TEGRA124_EMC_SIZE		SZ_2K
+
+#define TEGRA_CSITE_BASE		0x70040000
+#define TEGRA_CSITE_SIZE		SZ_256K
+
+/* On TEGRA, many peripherals are very closely packed in
+ * two 256MB io windows (that actually only use about 64KB
+ * at the start of each).
+ *
+ * We will just map the first MMU section of each window (to minimize
+ * pt entries needed) and provide a macro to transform physical
+ * io addresses to an appropriate void __iomem *.
+ */
+
+#define IO_IRAM_PHYS	0x40000000
+#define IO_IRAM_VIRT	IOMEM(0xFE400000)
+#define IO_IRAM_SIZE	SZ_256K
+
+#define IO_CPU_PHYS	0x50040000
+#define IO_CPU_VIRT	IOMEM(0xFE440000)
+#define IO_CPU_SIZE	SZ_16K
+
+#define IO_PPSB_PHYS	0x60000000
+#define IO_PPSB_VIRT	IOMEM(0xFE200000)
+#define IO_PPSB_SIZE	SECTION_SIZE
+
+#define IO_APB_PHYS	0x70000000
+#define IO_APB_VIRT	IOMEM(0xFE000000)
+#define IO_APB_SIZE	SECTION_SIZE
+
+#define IO_TO_VIRT_BETWEEN(p, st, sz)	((p) >= (st) && (p) < ((st) + (sz)))
+#define IO_TO_VIRT_XLATE(p, pst, vst)	(((p) - (pst) + (vst)))
+
+#define IO_TO_VIRT(n) ( \
+	IO_TO_VIRT_BETWEEN((n), IO_PPSB_PHYS, IO_PPSB_SIZE) ?		\
+		IO_TO_VIRT_XLATE((n), IO_PPSB_PHYS, IO_PPSB_VIRT) :	\
+	IO_TO_VIRT_BETWEEN((n), IO_APB_PHYS, IO_APB_SIZE) ?		\
+		IO_TO_VIRT_XLATE((n), IO_APB_PHYS, IO_APB_VIRT) :	\
+	IO_TO_VIRT_BETWEEN((n), IO_CPU_PHYS, IO_CPU_SIZE) ?		\
+		IO_TO_VIRT_XLATE((n), IO_CPU_PHYS, IO_CPU_VIRT) :	\
+	IO_TO_VIRT_BETWEEN((n), IO_IRAM_PHYS, IO_IRAM_SIZE) ?		\
+		IO_TO_VIRT_XLATE((n), IO_IRAM_PHYS, IO_IRAM_VIRT) :	\
+	NULL)
+
+#define IO_ADDRESS(n) (IO_TO_VIRT(n))
+
+#endif
diff --git a/arch/arm/mach-tegra/irammap.h b/arch/arm/mach-tegra/irammap.h
new file mode 100644
index 000000000..a945a1c38
--- /dev/null
+++ b/arch/arm/mach-tegra/irammap.h
@@ -0,0 +1,21 @@
+/* SPDX-License-Identifier: GPL-2.0-only */
+/*
+ * Copyright (c) 2012, NVIDIA CORPORATION.  All rights reserved.
+ */
+
+#ifndef __MACH_TEGRA_IRAMMAP_H
+#define __MACH_TEGRA_IRAMMAP_H
+
+#include <linux/sizes.h>
+
+/* The first 1K of IRAM is permanently reserved for the CPU reset handler */
+#define TEGRA_IRAM_RESET_HANDLER_OFFSET	0
+#define TEGRA_IRAM_RESET_HANDLER_SIZE	SZ_1K
+
+/*
+ * This area is used for LPx resume vector, only while LPx power state is
+ * active. At other times, the AVP may use this area for arbitrary purposes
+ */
+#define TEGRA_IRAM_LPx_RESUME_AREA	(TEGRA_IRAM_BASE + SZ_4K)
+
+#endif
diff --git a/arch/arm/mach-tegra/irq.c b/arch/arm/mach-tegra/irq.c
new file mode 100644
index 000000000..e5a611dce
--- /dev/null
+++ b/arch/arm/mach-tegra/irq.c
@@ -0,0 +1,98 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * Copyright (C) 2011 Google, Inc.
+ *
+ * Author:
+ *	Colin Cross <ccross@android.com>
+ *
+ * Copyright (C) 2010,2013, NVIDIA Corporation
+ */
+
+#include <linux/cpu_pm.h>
+#include <linux/interrupt.h>
+#include <linux/io.h>
+#include <linux/irqchip/arm-gic.h>
+#include <linux/irq.h>
+#include <linux/kernel.h>
+#include <linux/of_address.h>
+#include <linux/of.h>
+#include <linux/syscore_ops.h>
+
+#include <soc/tegra/irq.h>
+
+#include "board.h"
+#include "iomap.h"
+
+#define SGI_MASK 0xFFFF
+
+#ifdef CONFIG_PM_SLEEP
+static void __iomem *tegra_gic_cpu_base;
+#endif
+
+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;
+}
+
+#ifdef CONFIG_PM_SLEEP
+static int tegra_gic_notifier(struct notifier_block *self,
+			      unsigned long cmd, void *v)
+{
+	switch (cmd) {
+	case CPU_PM_ENTER:
+		writel_relaxed(0x1E0, tegra_gic_cpu_base + GIC_CPU_CTRL);
+		break;
+	}
+
+	return NOTIFY_OK;
+}
+
+static struct notifier_block tegra_gic_notifier_block = {
+	.notifier_call = tegra_gic_notifier,
+};
+
+static const struct of_device_id tegra114_dt_gic_match[] __initconst = {
+	{ .compatible = "arm,cortex-a15-gic" },
+	{ }
+};
+
+static void __init tegra114_gic_cpu_pm_registration(void)
+{
+	struct device_node *dn __free(device_node) =
+		of_find_matching_node(NULL, tegra114_dt_gic_match);
+
+	if (!dn)
+		return;
+
+	tegra_gic_cpu_base = of_iomap(dn, 1);
+
+	cpu_pm_register_notifier(&tegra_gic_notifier_block);
+}
+#else
+static void __init tegra114_gic_cpu_pm_registration(void) { }
+#endif
+
+static const struct of_device_id tegra_ictlr_match[] __initconst = {
+	{ .compatible = "nvidia,tegra20-ictlr" },
+	{ .compatible = "nvidia,tegra30-ictlr" },
+	{ }
+};
+
+void __init tegra_init_irq(void)
+{
+	struct device_node *dn __free(device_node) =
+		of_find_matching_node(NULL, tegra_ictlr_match);
+
+	if (WARN_ON(!dn))
+		pr_warn("Outdated DT detected, suspend/resume will NOT work\n");
+
+	tegra114_gic_cpu_pm_registration();
+}
diff --git a/arch/arm/mach-tegra/platsmp.c b/arch/arm/mach-tegra/platsmp.c
new file mode 100644
index 000000000..1f57e7c0f
--- /dev/null
+++ b/arch/arm/mach-tegra/platsmp.c
@@ -0,0 +1,190 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ *  linux/arch/arm/mach-tegra/platsmp.c
+ *
+ *  Copyright (C) 2002 ARM Ltd.
+ *  All Rights Reserved
+ *
+ *  Copyright (C) 2009 Palm
+ *  All Rights Reserved
+ */
+
+#include <linux/clk/tegra.h>
+#include <linux/delay.h>
+#include <linux/device.h>
+#include <linux/errno.h>
+#include <linux/init.h>
+#include <linux/io.h>
+#include <linux/jiffies.h>
+#include <linux/smp.h>
+
+#include <soc/tegra/flowctrl.h>
+#include <soc/tegra/fuse.h>
+#include <soc/tegra/pmc.h>
+
+#include <asm/cacheflush.h>
+#include <asm/mach-types.h>
+#include <asm/smp_plat.h>
+#include <asm/smp_scu.h>
+
+#include "common.h"
+#include "iomap.h"
+#include "reset.h"
+
+static cpumask_t tegra_cpu_init_mask;
+
+static void tegra_secondary_init(unsigned int cpu)
+{
+	cpumask_set_cpu(cpu, &tegra_cpu_init_mask);
+}
+
+
+static int tegra20_boot_secondary(unsigned int cpu, struct task_struct *idle)
+{
+	cpu = cpu_logical_map(cpu);
+
+	/*
+	 * Force the CPU into reset. The CPU must remain in reset when
+	 * the flow controller state is cleared (which will cause the
+	 * flow controller to stop driving reset if the CPU has been
+	 * power-gated via the flow controller). This will have no
+	 * effect on first boot of the CPU since it should already be
+	 * in reset.
+	 */
+	tegra_put_cpu_in_reset(cpu);
+
+	/*
+	 * Unhalt the CPU. If the flow controller was used to
+	 * power-gate the CPU this will cause the flow controller to
+	 * stop driving reset. The CPU will remain in reset because the
+	 * clock and reset block is now driving reset.
+	 */
+	flowctrl_write_cpu_halt(cpu, 0);
+
+	tegra_enable_cpu_clock(cpu);
+	flowctrl_write_cpu_csr(cpu, 0); /* Clear flow controller CSR. */
+	tegra_cpu_out_of_reset(cpu);
+	return 0;
+}
+
+static int tegra30_boot_secondary(unsigned int cpu, struct task_struct *idle)
+{
+	int ret;
+	unsigned long timeout;
+
+	cpu = cpu_logical_map(cpu);
+	tegra_put_cpu_in_reset(cpu);
+	flowctrl_write_cpu_halt(cpu, 0);
+
+	/*
+	 * The power up sequence of cold boot CPU and warm boot CPU
+	 * was different.
+	 *
+	 * For warm boot CPU that was resumed from CPU hotplug, the
+	 * power will be resumed automatically after un-halting the
+	 * flow controller of the warm boot CPU. We need to wait for
+	 * the confirmation that the CPU is powered then removing
+	 * the IO clamps.
+	 * For cold boot CPU, do not wait. After the cold boot CPU be
+	 * booted, it will run to tegra_secondary_init() and set
+	 * tegra_cpu_init_mask which influences what tegra30_boot_secondary()
+	 * next time around.
+	 */
+	if (cpumask_test_cpu(cpu, &tegra_cpu_init_mask)) {
+		timeout = jiffies + msecs_to_jiffies(50);
+		do {
+			if (tegra_pmc_cpu_is_powered(cpu))
+				goto remove_clamps;
+			udelay(10);
+		} while (time_before(jiffies, timeout));
+	}
+
+	/*
+	 * The power status of the cold boot CPU is power gated as
+	 * default. To power up the cold boot CPU, the power should
+	 * be un-gated by un-toggling the power gate register
+	 * manually.
+	 */
+	ret = tegra_pmc_cpu_power_on(cpu);
+	if (ret)
+		return ret;
+
+remove_clamps:
+	/* CPU partition is powered. Enable the CPU clock. */
+	tegra_enable_cpu_clock(cpu);
+	udelay(10);
+
+	/* Remove I/O clamps. */
+	ret = tegra_pmc_cpu_remove_clamping(cpu);
+	if (ret)
+		return ret;
+
+	udelay(10);
+
+	flowctrl_write_cpu_csr(cpu, 0); /* Clear flow controller CSR. */
+	tegra_cpu_out_of_reset(cpu);
+	return 0;
+}
+
+static int tegra114_boot_secondary(unsigned int cpu, struct task_struct *idle)
+{
+	int ret = 0;
+
+	cpu = cpu_logical_map(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 tegra_boot_secondary(unsigned int cpu,
+					  struct task_struct *idle)
+{
+	if (IS_ENABLED(CONFIG_ARCH_TEGRA_2x_SOC) && tegra_get_chip_id() == TEGRA20)
+		return tegra20_boot_secondary(cpu, idle);
+	if (IS_ENABLED(CONFIG_ARCH_TEGRA_3x_SOC) && tegra_get_chip_id() == TEGRA30)
+		return tegra30_boot_secondary(cpu, idle);
+	if (IS_ENABLED(CONFIG_ARCH_TEGRA_114_SOC) && tegra_get_chip_id() == TEGRA114)
+		return tegra114_boot_secondary(cpu, idle);
+	if (IS_ENABLED(CONFIG_ARCH_TEGRA_124_SOC) && tegra_get_chip_id() == TEGRA124)
+		return tegra114_boot_secondary(cpu, idle);
+
+	return -EINVAL;
+}
+
+static void __init tegra_smp_prepare_cpus(unsigned int max_cpus)
+{
+	/* Always mark the boot CPU (CPU0) as initialized. */
+	cpumask_set_cpu(0, &tegra_cpu_init_mask);
+
+	if (scu_a9_has_base())
+		scu_enable(IO_ADDRESS(scu_a9_get_base()));
+}
+
+const struct smp_operations tegra_smp_ops __initconst = {
+	.smp_prepare_cpus	= tegra_smp_prepare_cpus,
+	.smp_secondary_init	= tegra_secondary_init,
+	.smp_boot_secondary	= tegra_boot_secondary,
+#ifdef CONFIG_HOTPLUG_CPU
+	.cpu_kill		= tegra_cpu_kill,
+	.cpu_die		= tegra_cpu_die,
+#endif
+};
diff --git a/arch/arm/mach-tegra/pm-tegra20.c b/arch/arm/mach-tegra/pm-tegra20.c
new file mode 100644
index 000000000..1b4cab81a
--- /dev/null
+++ b/arch/arm/mach-tegra/pm-tegra20.c
@@ -0,0 +1,24 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * Copyright (c) 2013, NVIDIA Corporation. All rights reserved.
+ */
+
+#include <linux/kernel.h>
+
+#include "pm.h"
+
+#ifdef CONFIG_PM_SLEEP
+extern u32 tegra20_iram_start, tegra20_iram_end;
+extern void tegra20_sleep_core_finish(unsigned long);
+
+void tegra20_lp1_iram_hook(void)
+{
+	tegra_lp1_iram.start_addr = &tegra20_iram_start;
+	tegra_lp1_iram.end_addr = &tegra20_iram_end;
+}
+
+void tegra20_sleep_core_init(void)
+{
+	tegra_sleep_core_finish = tegra20_sleep_core_finish;
+}
+#endif
diff --git a/arch/arm/mach-tegra/pm-tegra30.c b/arch/arm/mach-tegra/pm-tegra30.c
new file mode 100644
index 000000000..e1b032953
--- /dev/null
+++ b/arch/arm/mach-tegra/pm-tegra30.c
@@ -0,0 +1,24 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * Copyright (c) 2013, NVIDIA Corporation. All rights reserved.
+ */
+
+#include <linux/kernel.h>
+
+#include "pm.h"
+
+#ifdef CONFIG_PM_SLEEP
+extern u32 tegra30_iram_start, tegra30_iram_end;
+extern void tegra30_sleep_core_finish(unsigned long);
+
+void tegra30_lp1_iram_hook(void)
+{
+	tegra_lp1_iram.start_addr = &tegra30_iram_start;
+	tegra_lp1_iram.end_addr = &tegra30_iram_end;
+}
+
+void tegra30_sleep_core_init(void)
+{
+	tegra_sleep_core_finish = tegra30_sleep_core_finish;
+}
+#endif
diff --git a/arch/arm/mach-tegra/pm.c b/arch/arm/mach-tegra/pm.c
new file mode 100644
index 000000000..b21f51b8e
--- /dev/null
+++ b/arch/arm/mach-tegra/pm.c
@@ -0,0 +1,453 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * CPU complex suspend & resume functions for Tegra SoCs
+ *
+ * Copyright (c) 2009-2012, NVIDIA Corporation. All rights reserved.
+ */
+
+#include <linux/clk/tegra.h>
+#include <linux/cpumask.h>
+#include <linux/cpu_pm.h>
+#include <linux/delay.h>
+#include <linux/err.h>
+#include <linux/io.h>
+#include <linux/kernel.h>
+#include <linux/slab.h>
+#include <linux/spinlock.h>
+#include <linux/suspend.h>
+
+#include <linux/firmware/trusted_foundations.h>
+
+#include <soc/tegra/flowctrl.h>
+#include <soc/tegra/fuse.h>
+#include <soc/tegra/pm.h>
+#include <soc/tegra/pmc.h>
+
+#include <asm/cacheflush.h>
+#include <asm/firmware.h>
+#include <asm/idmap.h>
+#include <asm/proc-fns.h>
+#include <asm/smp_plat.h>
+#include <asm/suspend.h>
+#include <asm/tlbflush.h>
+
+#include "iomap.h"
+#include "pm.h"
+#include "reset.h"
+#include "sleep.h"
+
+#ifdef CONFIG_PM_SLEEP
+static DEFINE_SPINLOCK(tegra_lp2_lock);
+static u32 iram_save_size;
+static void *iram_save_addr;
+struct tegra_lp1_iram tegra_lp1_iram;
+void (*tegra_tear_down_cpu)(void);
+void (*tegra_sleep_core_finish)(unsigned long v2p);
+static int (*tegra_sleep_func)(unsigned long v2p);
+
+static void tegra_tear_down_cpu_init(void)
+{
+	switch (tegra_get_chip_id()) {
+	case TEGRA20:
+		if (IS_ENABLED(CONFIG_ARCH_TEGRA_2x_SOC))
+			tegra_tear_down_cpu = tegra20_tear_down_cpu;
+		break;
+	case TEGRA30:
+	case TEGRA114:
+	case TEGRA124:
+		if (IS_ENABLED(CONFIG_ARCH_TEGRA_3x_SOC) ||
+		    IS_ENABLED(CONFIG_ARCH_TEGRA_114_SOC) ||
+		    IS_ENABLED(CONFIG_ARCH_TEGRA_124_SOC))
+			tegra_tear_down_cpu = tegra30_tear_down_cpu;
+		break;
+	}
+}
+
+/*
+ * restore_cpu_complex
+ *
+ * restores cpu clock setting, clears flow controller
+ *
+ * Always called on CPU 0.
+ */
+static void restore_cpu_complex(void)
+{
+	int cpu = smp_processor_id();
+
+	BUG_ON(cpu != 0);
+
+#ifdef CONFIG_SMP
+	cpu = cpu_logical_map(cpu);
+#endif
+
+	/* Restore the CPU clock settings */
+	tegra_cpu_clock_resume();
+
+	flowctrl_cpu_suspend_exit(cpu);
+}
+
+/*
+ * suspend_cpu_complex
+ *
+ * saves pll state for use by restart_plls, prepares flow controller for
+ * transition to suspend state
+ *
+ * Must always be called on cpu 0.
+ */
+static void suspend_cpu_complex(void)
+{
+	int cpu = smp_processor_id();
+
+	BUG_ON(cpu != 0);
+
+#ifdef CONFIG_SMP
+	cpu = cpu_logical_map(cpu);
+#endif
+
+	/* Save the CPU clock settings */
+	tegra_cpu_clock_suspend();
+
+	flowctrl_cpu_suspend_enter(cpu);
+}
+
+void tegra_pm_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);
+
+	BUG_ON(!(*cpu_in_lp2 & BIT(phy_cpu_id)));
+	*cpu_in_lp2 &= ~BIT(phy_cpu_id);
+
+	spin_unlock(&tegra_lp2_lock);
+}
+
+void tegra_pm_set_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);
+
+	BUG_ON((*cpu_in_lp2 & BIT(phy_cpu_id)));
+	*cpu_in_lp2 |= BIT(phy_cpu_id);
+
+	spin_unlock(&tegra_lp2_lock);
+}
+
+static int tegra_sleep_cpu(unsigned long v2p)
+{
+	if (tegra_cpu_car_ops->rail_off_ready &&
+	    WARN_ON(!tegra_cpu_rail_off_ready()))
+		return -EBUSY;
+
+	/*
+	 * L2 cache disabling using kernel API only allowed when all
+	 * secondary CPU's are offline. Cache have to be disabled with
+	 * MMU-on if cache maintenance is done via Trusted Foundations
+	 * firmware. Note that CPUIDLE won't ever enter powergate on Tegra30
+	 * if any of secondary CPU's is online and this is the LP2-idle
+	 * code-path only for Tegra20/30.
+	 */
+#ifdef CONFIG_OUTER_CACHE
+	if (trusted_foundations_registered() && outer_cache.disable)
+		outer_cache.disable();
+#endif
+	/*
+	 * Note that besides of setting up CPU reset vector this firmware
+	 * call may also do the following, depending on the FW version:
+	 *  1) Disable L2. But this doesn't matter since we already
+	 *     disabled the L2.
+	 *  2) Disable D-cache. This need to be taken into account in
+	 *     particular by the tegra_disable_clean_inv_dcache() which
+	 *     shall avoid the re-disable.
+	 */
+	call_firmware_op(prepare_idle, TF_PM_MODE_LP2);
+
+	setup_mm_for_reboot();
+	tegra_sleep_cpu_finish(v2p);
+
+	/* should never here */
+	BUG();
+
+	return 0;
+}
+
+static void tegra_pm_set(enum tegra_suspend_mode mode)
+{
+	u32 value;
+
+	switch (tegra_get_chip_id()) {
+	case TEGRA20:
+	case TEGRA30:
+		break;
+	default:
+		/* Turn off CRAIL */
+		value = flowctrl_read_cpu_csr(0);
+		value &= ~FLOW_CTRL_CSR_ENABLE_EXT_MASK;
+		value |= FLOW_CTRL_CSR_ENABLE_EXT_CRAIL;
+		flowctrl_write_cpu_csr(0, value);
+		break;
+	}
+
+	tegra_pmc_enter_suspend_mode(mode);
+}
+
+int tegra_pm_enter_lp2(void)
+{
+	int err;
+
+	tegra_pm_set(TEGRA_SUSPEND_LP2);
+
+	cpu_cluster_pm_enter();
+	suspend_cpu_complex();
+
+	err = cpu_suspend(PHYS_OFFSET - PAGE_OFFSET, &tegra_sleep_cpu);
+
+	/*
+	 * Resume L2 cache if it wasn't re-enabled early during resume,
+	 * which is the case for Tegra30 that has to re-enable the cache
+	 * via firmware call. In other cases cache is already enabled and
+	 * hence re-enabling is a no-op. This is always a no-op on Tegra114+.
+	 */
+	outer_resume();
+
+	restore_cpu_complex();
+	cpu_cluster_pm_exit();
+
+	call_firmware_op(prepare_idle, TF_PM_MODE_NONE);
+
+	return err;
+}
+
+enum tegra_suspend_mode tegra_pm_validate_suspend_mode(
+				enum tegra_suspend_mode mode)
+{
+	/*
+	 * The Tegra devices support suspending to LP1 or lower currently.
+	 */
+	if (mode > TEGRA_SUSPEND_LP1)
+		return TEGRA_SUSPEND_LP1;
+
+	return mode;
+}
+
+static int tegra_sleep_core(unsigned long v2p)
+{
+	/*
+	 * Cache have to be disabled with MMU-on if cache maintenance is done
+	 * via Trusted Foundations firmware. This is a no-op on Tegra114+.
+	 */
+	if (trusted_foundations_registered())
+		outer_disable();
+
+	call_firmware_op(prepare_idle, TF_PM_MODE_LP1);
+
+	setup_mm_for_reboot();
+	tegra_sleep_core_finish(v2p);
+
+	/* should never here */
+	BUG();
+
+	return 0;
+}
+
+/*
+ * tegra_lp1_iram_hook
+ *
+ * Hooking the address of LP1 reset vector and SDRAM self-refresh code in
+ * SDRAM. These codes not be copied to IRAM in this fuction. We need to
+ * copy these code to IRAM before LP0/LP1 suspend and restore the content
+ * of IRAM after resume.
+ */
+static bool tegra_lp1_iram_hook(void)
+{
+	switch (tegra_get_chip_id()) {
+	case TEGRA20:
+		if (IS_ENABLED(CONFIG_ARCH_TEGRA_2x_SOC))
+			tegra20_lp1_iram_hook();
+		break;
+	case TEGRA30:
+	case TEGRA114:
+	case TEGRA124:
+		if (IS_ENABLED(CONFIG_ARCH_TEGRA_3x_SOC) ||
+		    IS_ENABLED(CONFIG_ARCH_TEGRA_114_SOC) ||
+		    IS_ENABLED(CONFIG_ARCH_TEGRA_124_SOC))
+			tegra30_lp1_iram_hook();
+		break;
+	default:
+		break;
+	}
+
+	if (!tegra_lp1_iram.start_addr || !tegra_lp1_iram.end_addr)
+		return false;
+
+	iram_save_size = tegra_lp1_iram.end_addr - tegra_lp1_iram.start_addr;
+	iram_save_addr = kmalloc(iram_save_size, GFP_KERNEL);
+	if (!iram_save_addr)
+		return false;
+
+	return true;
+}
+
+static bool tegra_sleep_core_init(void)
+{
+	switch (tegra_get_chip_id()) {
+	case TEGRA20:
+		if (IS_ENABLED(CONFIG_ARCH_TEGRA_2x_SOC))
+			tegra20_sleep_core_init();
+		break;
+	case TEGRA30:
+	case TEGRA114:
+	case TEGRA124:
+		if (IS_ENABLED(CONFIG_ARCH_TEGRA_3x_SOC) ||
+		    IS_ENABLED(CONFIG_ARCH_TEGRA_114_SOC) ||
+		    IS_ENABLED(CONFIG_ARCH_TEGRA_124_SOC))
+			tegra30_sleep_core_init();
+		break;
+	default:
+		break;
+	}
+
+	if (!tegra_sleep_core_finish)
+		return false;
+
+	return true;
+}
+
+static void tegra_suspend_enter_lp1(void)
+{
+	/* copy the reset vector & SDRAM shutdown code into IRAM */
+	memcpy(iram_save_addr, IO_ADDRESS(TEGRA_IRAM_LPx_RESUME_AREA),
+		iram_save_size);
+	memcpy(IO_ADDRESS(TEGRA_IRAM_LPx_RESUME_AREA),
+		tegra_lp1_iram.start_addr, iram_save_size);
+
+	*((u32 *)tegra_cpu_lp1_mask) = 1;
+}
+
+static void tegra_suspend_exit_lp1(void)
+{
+	/* restore IRAM */
+	memcpy(IO_ADDRESS(TEGRA_IRAM_LPx_RESUME_AREA), iram_save_addr,
+		iram_save_size);
+
+	*(u32 *)tegra_cpu_lp1_mask = 0;
+}
+
+static const char *lp_state[TEGRA_MAX_SUSPEND_MODE] = {
+	[TEGRA_SUSPEND_NONE] = "none",
+	[TEGRA_SUSPEND_LP2] = "LP2",
+	[TEGRA_SUSPEND_LP1] = "LP1",
+	[TEGRA_SUSPEND_LP0] = "LP0",
+};
+
+static int tegra_suspend_enter(suspend_state_t state)
+{
+	enum tegra_suspend_mode mode = tegra_pmc_get_suspend_mode();
+
+	if (WARN_ON(mode < TEGRA_SUSPEND_NONE ||
+		    mode >= TEGRA_MAX_SUSPEND_MODE))
+		return -EINVAL;
+
+	pr_info("Entering suspend state %s\n", lp_state[mode]);
+
+	tegra_pm_set(mode);
+
+	local_fiq_disable();
+
+	suspend_cpu_complex();
+	switch (mode) {
+	case TEGRA_SUSPEND_LP1:
+		tegra_suspend_enter_lp1();
+		break;
+	case TEGRA_SUSPEND_LP2:
+		tegra_pm_set_cpu_in_lp2();
+		break;
+	default:
+		break;
+	}
+
+	cpu_suspend(PHYS_OFFSET - PAGE_OFFSET, tegra_sleep_func);
+
+	/*
+	 * Resume L2 cache if it wasn't re-enabled early during resume,
+	 * which is the case for Tegra30 that has to re-enable the cache
+	 * via firmware call. In other cases cache is already enabled and
+	 * hence re-enabling is a no-op.
+	 */
+	outer_resume();
+
+	switch (mode) {
+	case TEGRA_SUSPEND_LP1:
+		tegra_suspend_exit_lp1();
+		break;
+	case TEGRA_SUSPEND_LP2:
+		tegra_pm_clear_cpu_in_lp2();
+		break;
+	default:
+		break;
+	}
+	restore_cpu_complex();
+
+	local_fiq_enable();
+
+	call_firmware_op(prepare_idle, TF_PM_MODE_NONE);
+
+	return 0;
+}
+
+static const struct platform_suspend_ops tegra_suspend_ops = {
+	.valid		= suspend_valid_only_mem,
+	.enter		= tegra_suspend_enter,
+};
+
+void tegra_pm_init_suspend(void)
+{
+	enum tegra_suspend_mode mode = tegra_pmc_get_suspend_mode();
+
+	if (mode == TEGRA_SUSPEND_NONE)
+		return;
+
+	tegra_tear_down_cpu_init();
+
+	if (mode >= TEGRA_SUSPEND_LP1) {
+		if (!tegra_lp1_iram_hook() || !tegra_sleep_core_init()) {
+			pr_err("%s: unable to allocate memory for SDRAM"
+			       "self-refresh -- LP0/LP1 unavailable\n",
+			       __func__);
+			tegra_pmc_set_suspend_mode(TEGRA_SUSPEND_LP2);
+			mode = TEGRA_SUSPEND_LP2;
+		}
+	}
+
+	/* set up sleep function for cpu_suspend */
+	switch (mode) {
+	case TEGRA_SUSPEND_LP1:
+		tegra_sleep_func = tegra_sleep_core;
+		break;
+	case TEGRA_SUSPEND_LP2:
+		tegra_sleep_func = tegra_sleep_cpu;
+		break;
+	default:
+		break;
+	}
+
+	suspend_set_ops(&tegra_suspend_ops);
+}
+
+int tegra_pm_park_secondary_cpu(unsigned long cpu)
+{
+	if (cpu > 0) {
+		tegra_disable_clean_inv_dcache(TEGRA_FLUSH_CACHE_LOUIS);
+
+		if (tegra_get_chip_id() == TEGRA20)
+			tegra20_hotplug_shutdown();
+		else
+			tegra30_hotplug_shutdown();
+	}
+
+	return -EINVAL;
+}
+#endif
diff --git a/arch/arm/mach-tegra/pm.h b/arch/arm/mach-tegra/pm.h
new file mode 100644
index 000000000..e63f96de2
--- /dev/null
+++ b/arch/arm/mach-tegra/pm.h
@@ -0,0 +1,28 @@
+/* SPDX-License-Identifier: GPL-2.0-only */
+/*
+ * Copyright (C) 2010 Google, Inc.
+ * Copyright (c) 2010-2012 NVIDIA Corporation. All rights reserved.
+ *
+ * Author:
+ *	Colin Cross <ccross@google.com>
+ */
+
+#ifndef _MACH_TEGRA_PM_H_
+#define _MACH_TEGRA_PM_H_
+
+struct tegra_lp1_iram {
+	void	*start_addr;
+	void	*end_addr;
+};
+
+extern struct tegra_lp1_iram tegra_lp1_iram;
+extern void (*tegra_sleep_core_finish)(unsigned long v2p);
+
+void tegra20_lp1_iram_hook(void);
+void tegra20_sleep_core_init(void);
+void tegra30_lp1_iram_hook(void);
+void tegra30_sleep_core_init(void);
+
+extern void (*tegra_tear_down_cpu)(void);
+
+#endif /* _MACH_TEGRA_PM_H_ */
diff --git a/arch/arm/mach-tegra/reset-handler.S b/arch/arm/mach-tegra/reset-handler.S
new file mode 100644
index 000000000..0ea456264
--- /dev/null
+++ b/arch/arm/mach-tegra/reset-handler.S
@@ -0,0 +1,303 @@
+/* SPDX-License-Identifier: GPL-2.0-only */
+/*
+ * Copyright (c) 2012, NVIDIA Corporation. All rights reserved.
+ */
+
+#include <linux/init.h>
+#include <linux/linkage.h>
+
+#include <soc/tegra/flowctrl.h>
+#include <soc/tegra/fuse.h>
+
+#include <asm/assembler.h>
+#include <asm/asm-offsets.h>
+#include <asm/cache.h>
+
+#include "iomap.h"
+#include "reset.h"
+#include "sleep.h"
+
+#define PMC_SCRATCH41	0x140
+
+.arch armv7-a
+
+#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.
+ *
+ *	r6: SoC ID
+ *	r8: CPU part number
+ */
+ENTRY(tegra_resume)
+	check_cpu_part_num 0xc09, r8, r9
+	bleq	v7_invalidate_l1
+
+	cpu_id	r0
+	cmp	r0, #0				@ CPU0?
+ THUMB(	it	ne )
+	bne	cpu_resume			@ no
+
+	tegra_get_soc_id TEGRA_APB_MISC_BASE, r6
+	/* Are we on Tegra20? */
+	cmp	r6, #TEGRA20
+	beq	1f				@ Yes
+	/* Clear the flow controller flags for this CPU. */
+	cpu_to_csr_reg r3, r0
+	mov32	r2, TEGRA_FLOW_CTRL_BASE
+	ldr	r1, [r2, r3]
+	/* Clear event & intr flag */
+	orr	r1, r1, \
+		#FLOW_CTRL_CSR_INTR_FLAG | FLOW_CTRL_CSR_EVENT_FLAG
+	movw	r0, #0x3FFD	@ enable, cluster_switch, immed, bitmaps
+				@ & ext flags for CPU power mgnt
+	bic	r1, r1, r0
+	str	r1, [r2, r3]
+1:
+
+	mov32	r9, 0xc09
+	cmp	r8, r9
+	bne	end_ca9_scu_l2_resume
+#ifdef CONFIG_HAVE_ARM_SCU
+	/* enable SCU */
+	mov32	r0, TEGRA_ARM_PERIF_BASE
+	ldr	r1, [r0]
+	orr	r1, r1, #1
+	str	r1, [r0]
+#endif
+	bl	tegra_resume_trusted_foundations
+
+#ifdef CONFIG_CACHE_L2X0
+	/* L2 cache resume & re-enable */
+	bl	l2c310_early_resume
+#endif
+end_ca9_scu_l2_resume:
+	mov32	r9, 0xc0f
+	cmp	r8, r9
+	bleq	tegra_init_l2_for_a15
+
+	b	cpu_resume
+ENDPROC(tegra_resume)
+
+/*
+ *	tegra_resume_trusted_foundations
+ *
+ *	  Trusted Foundations firmware initialization.
+ *
+ *	Doesn't return if firmware presents.
+ *	Corrupted registers: r1, r2
+ */
+ENTRY(tegra_resume_trusted_foundations)
+	/* Check whether Trusted Foundations firmware presents. */
+	mov32	r2, TEGRA_IRAM_BASE + TEGRA_IRAM_RESET_HANDLER_OFFSET
+	ldr	r1, =__tegra_cpu_reset_handler_data_offset + \
+							RESET_DATA(TF_PRESENT)
+	ldr	r1, [r2, r1]
+	cmp	r1, #0
+	reteq	lr
+
+ .arch_extension sec
+	/*
+	 * First call after suspend wakes firmware. No arguments required
+	 * for some firmware versions. Downstream kernel of ASUS TF300T uses
+	 * r0=3 for the wake-up notification.
+	 */
+	mov	r0, #3
+	smc	#0
+
+	b	cpu_resume
+ENDPROC(tegra_resume_trusted_foundations)
+#endif
+
+	.align L1_CACHE_SHIFT
+ENTRY(__tegra_cpu_reset_handler_start)
+
+/*
+ * __tegra_cpu_reset_handler:
+ *
+ * Common handler for all CPU reset events.
+ *
+ * Register usage within the reset handler:
+ *
+ *      Others: scratch
+ *      R6  = SoC ID
+ *      R7  = CPU present (to the OS) mask
+ *      R8  = CPU in LP1 state mask
+ *      R9  = CPU in LP2 state mask
+ *      R10 = CPU number
+ *      R11 = CPU mask
+ *      R12 = pointer to reset handler data
+ *
+ * NOTE: This code is copied to IRAM. All code and data accesses
+ *       must be position-independent.
+ */
+
+	.arm
+	.align L1_CACHE_SHIFT
+ENTRY(__tegra_cpu_reset_handler)
+
+	cpsid	aif, 0x13			@ SVC mode, interrupts disabled
+
+	tegra_get_soc_id TEGRA_APB_MISC_BASE, r6
+
+	adr	r12, __tegra_cpu_reset_handler_data
+	ldr	r5, [r12, #RESET_DATA(TF_PRESENT)]
+	cmp	r5, #0
+	bne	after_errata
+
+#ifdef CONFIG_ARCH_TEGRA_2x_SOC
+t20_check:
+	cmp	r6, #TEGRA20
+	bne	after_t20_check
+t20_errata:
+	# Tegra20 is a Cortex-A9 r1p1
+	mrc	p15, 0, r0, c1, c0, 0   @ read system control register
+	orr	r0, r0, #1 << 14        @ erratum 716044
+	mcr	p15, 0, r0, c1, c0, 0   @ write system control register
+	mrc	p15, 0, r0, c15, c0, 1  @ read diagnostic register
+	orr	r0, r0, #1 << 4         @ erratum 742230
+	orr	r0, r0, #1 << 11        @ erratum 751472
+	mcr	p15, 0, r0, c15, c0, 1  @ write diagnostic register
+	b	after_errata
+after_t20_check:
+#endif
+#ifdef CONFIG_ARCH_TEGRA_3x_SOC
+t30_check:
+	cmp	r6, #TEGRA30
+	bne	after_t30_check
+t30_errata:
+	# Tegra30 is a Cortex-A9 r2p9
+	mrc	p15, 0, r0, c15, c0, 1  @ read diagnostic register
+	orr	r0, r0, #1 << 6         @ erratum 743622
+	orr	r0, r0, #1 << 11        @ erratum 751472
+	mcr	p15, 0, r0, c15, c0, 1  @ write diagnostic register
+	b	after_errata
+after_t30_check:
+#endif
+after_errata:
+	mrc	p15, 0, r10, c0, c0, 5		@ MPIDR
+	and	r10, r10, #0x3			@ R10 = CPU number
+	mov	r11, #1
+	mov	r11, r11, lsl r10  		@ R11 = CPU mask
+
+#ifdef CONFIG_SMP
+	/* Does the OS know about this CPU? */
+	ldr	r7, [r12, #RESET_DATA(MASK_PRESENT)]
+	tst	r7, r11 			@ if !present
+	bleq	__die				@ CPU not present (to OS)
+#endif
+
+	/* Waking up from LP1? */
+	ldr	r8, [r12, #RESET_DATA(MASK_LP1)]
+	tst	r8, r11				@ if in_lp1
+	beq	__is_not_lp1
+	cmp	r10, #0
+	bne	__die				@ only CPU0 can be here
+	ldr	lr, [r12, #RESET_DATA(STARTUP_LP1)]
+	cmp	lr, #0
+	bleq	__die				@ no LP1 startup handler
+ THUMB(	add	lr, lr, #1 )			@ switch to Thumb mode
+	bx	lr
+__is_not_lp1:
+
+	/* 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)
+	 * 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
+	bx	lr
+#endif
+
+/*
+ * We don't know why the CPU reset. Just kill it.
+ * The LR register will contain the address we died at + 4.
+ */
+
+__die:
+	sub	lr, lr, #4
+	mov32	r7, TEGRA_PMC_BASE
+	str	lr, [r7, #PMC_SCRATCH41]
+
+	mov32	r7, TEGRA_CLK_RESET_BASE
+
+	/* Are we on Tegra20? */
+	cmp	r6, #TEGRA20
+	bne	1f
+
+#ifdef CONFIG_ARCH_TEGRA_2x_SOC
+	mov32	r0, 0x1111
+	mov	r1, r0, lsl r10
+	str	r1, [r7, #0x340]		@ CLK_RST_CPU_CMPLX_SET
+#endif
+1:
+#ifdef CONFIG_ARCH_TEGRA_3x_SOC
+	mov32	r6, TEGRA_FLOW_CTRL_BASE
+
+	cmp	r10, #0
+	moveq	r1, #FLOW_CTRL_HALT_CPU0_EVENTS
+	moveq	r2, #FLOW_CTRL_CPU0_CSR
+	movne	r1, r10, lsl #3
+	addne	r2, r1, #(FLOW_CTRL_CPU1_CSR-8)
+	addne	r1, r1, #(FLOW_CTRL_HALT_CPU1_EVENTS-8)
+
+	/* Clear CPU "event" and "interrupt" flags and power gate
+	   it when halting but not before it is in the "WFI" state. */
+	ldr	r0, [r6, +r2]
+	orr	r0, r0, #FLOW_CTRL_CSR_INTR_FLAG | FLOW_CTRL_CSR_EVENT_FLAG
+	orr	r0, r0, #FLOW_CTRL_CSR_ENABLE
+	str	r0, [r6, +r2]
+
+	/* Unconditionally halt this CPU */
+	mov	r0, #FLOW_CTRL_WAITEVENT
+	str	r0, [r6, +r1]
+	ldr	r0, [r6, +r1]			@ memory barrier
+
+	dsb
+	isb
+	wfi					@ CPU should be power gated here
+
+	/* If the CPU didn't power gate above just kill it's clock. */
+
+	mov	r0, r11, lsl #8
+	str	r0, [r7, #348]			@ CLK_CPU_CMPLX_SET
+#endif
+
+	/* If the CPU still isn't dead, just spin here. */
+	b	.
+ENDPROC(__tegra_cpu_reset_handler)
+
+	.align L1_CACHE_SHIFT
+	.type	__tegra_cpu_reset_handler_data, %object
+	.globl	__tegra_cpu_reset_handler_data
+	.globl	__tegra_cpu_reset_handler_data_offset
+	.equ	__tegra_cpu_reset_handler_data_offset, \
+					. - __tegra_cpu_reset_handler_start
+__tegra_cpu_reset_handler_data:
+	.rept   TEGRA_RESET_DATA_SIZE
+	.long   0
+	.endr
+	.align L1_CACHE_SHIFT
+
+ENTRY(__tegra_cpu_reset_handler_end)
diff --git a/arch/arm/mach-tegra/reset.c b/arch/arm/mach-tegra/reset.c
new file mode 100644
index 000000000..ea706fac6
--- /dev/null
+++ b/arch/arm/mach-tegra/reset.c
@@ -0,0 +1,103 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * arch/arm/mach-tegra/reset.c
+ *
+ * Copyright (C) 2011,2012 NVIDIA Corporation.
+ */
+
+#include <linux/bitops.h>
+#include <linux/cpumask.h>
+#include <linux/init.h>
+#include <linux/io.h>
+
+#include <linux/firmware/trusted_foundations.h>
+
+#include <soc/tegra/fuse.h>
+
+#include <asm/cacheflush.h>
+#include <asm/firmware.h>
+#include <asm/hardware/cache-l2x0.h>
+
+#include "iomap.h"
+#include "irammap.h"
+#include "reset.h"
+#include "sleep.h"
+
+#define TEGRA_IRAM_RESET_BASE (TEGRA_IRAM_BASE + \
+				TEGRA_IRAM_RESET_HANDLER_OFFSET)
+
+static bool is_enabled;
+
+static void __init tegra_cpu_reset_handler_set(const u32 reset_address)
+{
+	void __iomem *evp_cpu_reset =
+		IO_ADDRESS(TEGRA_EXCEPTION_VECTORS_BASE + 0x100);
+	void __iomem *sb_ctrl = IO_ADDRESS(TEGRA_SB_BASE);
+	u32 reg;
+
+	/*
+	 * NOTE: This must be the one and only write to the EVP CPU reset
+	 *       vector in the entire system.
+	 */
+	writel(reset_address, evp_cpu_reset);
+	wmb();
+	reg = readl(evp_cpu_reset);
+
+	/*
+	 * Prevent further modifications to the physical reset vector.
+	 *  NOTE: Has no effect on chips prior to Tegra30.
+	 */
+	reg = readl(sb_ctrl);
+	reg |= 2;
+	writel(reg, sb_ctrl);
+	wmb();
+}
+
+static void __init tegra_cpu_reset_handler_enable(void)
+{
+	void __iomem *iram_base = IO_ADDRESS(TEGRA_IRAM_RESET_BASE);
+	const u32 reset_address = TEGRA_IRAM_RESET_BASE +
+						tegra_cpu_reset_handler_offset;
+	int err;
+
+	BUG_ON(is_enabled);
+	BUG_ON(tegra_cpu_reset_handler_size > TEGRA_IRAM_RESET_HANDLER_SIZE);
+
+	memcpy_toio(iram_base, (void *)__tegra_cpu_reset_handler_start,
+			tegra_cpu_reset_handler_size);
+
+	err = call_firmware_op(set_cpu_boot_addr, 0, reset_address);
+	switch (err) {
+	case -ENOSYS:
+		tegra_cpu_reset_handler_set(reset_address);
+		fallthrough;
+	case 0:
+		is_enabled = true;
+		break;
+	default:
+		pr_crit("Cannot set CPU reset handler: %d\n", err);
+		BUG();
+	}
+}
+
+void __init tegra_cpu_reset_handler_init(void)
+{
+	__tegra_cpu_reset_handler_data[TEGRA_RESET_TF_PRESENT] =
+		trusted_foundations_registered();
+
+#ifdef CONFIG_SMP
+	__tegra_cpu_reset_handler_data[TEGRA_RESET_MASK_PRESENT] =
+		*((u32 *)cpu_possible_mask);
+	__tegra_cpu_reset_handler_data[TEGRA_RESET_STARTUP_SECONDARY] =
+		__pa_symbol((void *)secondary_startup);
+#endif
+
+#ifdef CONFIG_PM_SLEEP
+	__tegra_cpu_reset_handler_data[TEGRA_RESET_STARTUP_LP1] =
+		TEGRA_IRAM_LPx_RESUME_AREA;
+	__tegra_cpu_reset_handler_data[TEGRA_RESET_STARTUP_LP2] =
+		__pa_symbol((void *)tegra_resume);
+#endif
+
+	tegra_cpu_reset_handler_enable();
+}
diff --git a/arch/arm/mach-tegra/reset.h b/arch/arm/mach-tegra/reset.h
new file mode 100644
index 000000000..92a89713d
--- /dev/null
+++ b/arch/arm/mach-tegra/reset.h
@@ -0,0 +1,57 @@
+/* SPDX-License-Identifier: GPL-2.0-only */
+/*
+ * arch/arm/mach-tegra/reset.h
+ *
+ * CPU reset dispatcher.
+ *
+ * Copyright (c) 2011, NVIDIA Corporation.
+ */
+
+#ifndef __MACH_TEGRA_RESET_H
+#define __MACH_TEGRA_RESET_H
+
+#define TEGRA_RESET_MASK_PRESENT	0
+#define TEGRA_RESET_MASK_LP1		1
+#define TEGRA_RESET_MASK_LP2		2
+#define TEGRA_RESET_STARTUP_SECONDARY	3
+#define TEGRA_RESET_STARTUP_LP2		4
+#define TEGRA_RESET_STARTUP_LP1		5
+#define TEGRA_RESET_TF_PRESENT		6
+#define TEGRA_RESET_DATA_SIZE		7
+
+#define RESET_DATA(x)	((TEGRA_RESET_##x)*4)
+
+#ifndef __ASSEMBLER__
+
+#include "irammap.h"
+
+extern unsigned long __tegra_cpu_reset_handler_data[TEGRA_RESET_DATA_SIZE];
+
+void __tegra_cpu_reset_handler_start(void);
+void __tegra_cpu_reset_handler(void);
+void __tegra20_cpu1_resettable_status_offset(void);
+void __tegra_cpu_reset_handler_end(void);
+
+#ifdef CONFIG_PM_SLEEP
+#define tegra_cpu_lp1_mask \
+	(IO_ADDRESS(TEGRA_IRAM_BASE + TEGRA_IRAM_RESET_HANDLER_OFFSET + \
+	((u32)&__tegra_cpu_reset_handler_data[TEGRA_RESET_MASK_LP1] - \
+	 (u32)__tegra_cpu_reset_handler_start)))
+#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)
+
+#define tegra_cpu_reset_handler_size \
+		(__tegra_cpu_reset_handler_end - \
+		 __tegra_cpu_reset_handler_start)
+
+void __init tegra_cpu_reset_handler_init(void);
+
+#endif
+#endif
diff --git a/arch/arm/mach-tegra/sleep-tegra20.S b/arch/arm/mach-tegra/sleep-tegra20.S
new file mode 100644
index 000000000..d8cd487a8
--- /dev/null
+++ b/arch/arm/mach-tegra/sleep-tegra20.S
@@ -0,0 +1,437 @@
+/* SPDX-License-Identifier: GPL-2.0-only */
+/*
+ * Copyright (c) 2010-2012, NVIDIA Corporation. All rights reserved.
+ * Copyright (c) 2011, Google, Inc.
+ *
+ * Author: Colin Cross <ccross@android.com>
+ *         Gary King <gking@nvidia.com>
+ */
+
+#include <linux/linkage.h>
+
+#include <soc/tegra/flowctrl.h>
+
+#include <asm/assembler.h>
+#include <asm/proc-fns.h>
+#include <asm/cp15.h>
+#include <asm/cache.h>
+
+#include "irammap.h"
+#include "reset.h"
+#include "sleep.h"
+
+#define EMC_CFG				0xc
+#define EMC_ADR_CFG			0x10
+#define EMC_NOP				0xdc
+#define EMC_SELF_REF			0xe0
+#define EMC_REQ_CTRL			0x2b0
+#define EMC_EMC_STATUS			0x2b4
+
+#define CLK_RESET_CCLK_BURST		0x20
+#define CLK_RESET_CCLK_DIVIDER		0x24
+#define CLK_RESET_SCLK_BURST		0x28
+#define CLK_RESET_SCLK_DIVIDER		0x2c
+#define CLK_RESET_PLLC_BASE		0x80
+#define CLK_RESET_PLLM_BASE		0x90
+#define CLK_RESET_PLLP_BASE		0xa0
+
+#define APB_MISC_XM2CFGCPADCTRL		0x8c8
+#define APB_MISC_XM2CFGDPADCTRL		0x8cc
+#define APB_MISC_XM2CLKCFGPADCTRL	0x8d0
+#define APB_MISC_XM2COMPPADCTRL		0x8d4
+#define APB_MISC_XM2VTTGENPADCTRL	0x8d8
+#define APB_MISC_XM2CFGCPADCTRL2	0x8e4
+#define APB_MISC_XM2CFGDPADCTRL2	0x8e8
+
+#define PLLC_STORE_MASK			(1 << 0)
+#define PLLM_STORE_MASK			(1 << 1)
+#define PLLP_STORE_MASK			(1 << 2)
+
+.arch armv7-a
+
+.macro test_pll_state, rd, test_mask
+	ldr	\rd, tegra_pll_state
+	tst	\rd, #\test_mask
+.endm
+
+.macro store_pll_state, rd, tmp, r_car_base, pll_base, pll_mask
+	ldr	\rd, [\r_car_base, #\pll_base]
+	tst	\rd, #(1 << 30)
+	ldr	\rd, tegra_pll_state
+	biceq	\rd, \rd, #\pll_mask
+	orrne	\rd, \rd, #\pll_mask
+	adr	\tmp, tegra_pll_state
+	str	\rd, [\tmp]
+.endm
+
+.macro pll_enable, rd, r_car_base, pll_base, test_mask
+	test_pll_state \rd, \test_mask
+	beq	1f
+
+	ldr	\rd, [\r_car_base, #\pll_base]
+	tst	\rd, #(1 << 30)
+	orreq	\rd, \rd, #(1 << 30)
+	streq	\rd, [\r_car_base, #\pll_base]
+1:
+.endm
+
+.macro emc_device_mask, rd, base
+	ldr	\rd, [\base, #EMC_ADR_CFG]
+	tst	\rd, #(0x3 << 24)
+	moveq	\rd, #(0x1 << 8)		@ just 1 device
+	movne	\rd, #(0x3 << 8)		@ 2 devices
+.endm
+
+#if defined(CONFIG_HOTPLUG_CPU) || defined(CONFIG_PM_SLEEP)
+/*
+ * tegra20_hotplug_shutdown(void)
+ *
+ * puts the current cpu in reset
+ * should never return
+ */
+ENTRY(tegra20_hotplug_shutdown)
+	/* Put this CPU down */
+	cpu_id	r0
+	bl	tegra20_cpu_shutdown
+	ret	lr			@ should never get here
+ENDPROC(tegra20_hotplug_shutdown)
+
+/*
+ * tegra20_cpu_shutdown(int cpu)
+ *
+ * r0 is cpu to reset
+ *
+ * puts the specified CPU in wait-for-event mode on the flow controller
+ * and puts the CPU in reset
+ * can be called on the current cpu or another cpu
+ * if called on the current cpu, does not return
+ * MUST NOT BE CALLED FOR CPU 0.
+ *
+ * corrupts r0-r3, r12
+ */
+ENTRY(tegra20_cpu_shutdown)
+	cmp	r0, #0
+	reteq	lr			@ must not be called for CPU 0
+
+	cpu_to_halt_reg r1, r0
+	ldr	r3, =TEGRA_FLOW_CTRL_VIRT
+	mov	r2, #FLOW_CTRL_WAITEVENT | FLOW_CTRL_JTAG_RESUME
+	str	r2, [r3, r1]		@ put flow controller in wait event mode
+	ldr	r2, [r3, r1]
+	isb
+	dsb
+	movw	r1, 0x1011
+	mov	r1, r1, lsl r0
+	ldr	r3, =TEGRA_CLK_RESET_VIRT
+	str	r1, [r3, #0x340]	@ put slave CPU in reset
+	isb
+	dsb
+	cpu_id	r3
+	cmp	r3, r0
+	beq	.
+	ret	lr
+ENDPROC(tegra20_cpu_shutdown)
+#endif
+
+#ifdef CONFIG_PM_SLEEP
+/*
+ * tegra20_sleep_core_finish(unsigned long v2p)
+ *
+ * Enters suspend in LP0 or LP1 by turning off the mmu and jumping to
+ * tegra20_tear_down_core in IRAM
+ */
+ENTRY(tegra20_sleep_core_finish)
+	mov     r4, r0
+	/* Flush, disable the L1 data cache and exit SMP */
+	mov     r0, #TEGRA_FLUSH_CACHE_ALL
+	bl	tegra_disable_clean_inv_dcache
+	mov     r0, r4
+
+	mov32	r3, tegra_shut_off_mmu
+	add	r3, r3, r0
+
+	mov32	r0, tegra20_tear_down_core
+	mov32	r1, tegra20_iram_start
+	sub	r0, r0, r1
+	mov32	r1, TEGRA_IRAM_LPx_RESUME_AREA
+	add	r0, r0, r1
+
+	ret	r3
+ENDPROC(tegra20_sleep_core_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)
+
+/* START OF ROUTINES COPIED TO IRAM */
+	.align L1_CACHE_SHIFT
+	.globl tegra20_iram_start
+tegra20_iram_start:
+
+/*
+ * tegra20_lp1_reset
+ *
+ * reset vector for LP1 restore; copied into IRAM during suspend.
+ * Brings the system back up to a safe staring point (SDRAM out of
+ * self-refresh, PLLC, PLLM and PLLP reenabled, CPU running on PLLP,
+ * system clock running on the same PLL that it suspended at), and
+ * jumps to tegra_resume to restore virtual addressing and PLLX.
+ * The physical address of tegra_resume expected to be stored in
+ * PMC_SCRATCH41.
+ *
+ * NOTE: THIS *MUST* BE RELOCATED TO TEGRA_IRAM_LPx_RESUME_AREA.
+ */
+ENTRY(tegra20_lp1_reset)
+	/*
+	 * The CPU and system bus are running at 32KHz and executing from
+	 * IRAM when this code is executed; immediately switch to CLKM and
+	 * enable PLLM, PLLP, PLLC.
+	 */
+	mov32	r0, TEGRA_CLK_RESET_BASE
+
+	mov	r1, #(1 << 28)
+	str	r1, [r0, #CLK_RESET_SCLK_BURST]
+	str	r1, [r0, #CLK_RESET_CCLK_BURST]
+	mov	r1, #0
+	str	r1, [r0, #CLK_RESET_CCLK_DIVIDER]
+	str	r1, [r0, #CLK_RESET_SCLK_DIVIDER]
+
+	pll_enable r1, r0, CLK_RESET_PLLM_BASE, PLLM_STORE_MASK
+	pll_enable r1, r0, CLK_RESET_PLLP_BASE, PLLP_STORE_MASK
+	pll_enable r1, r0, CLK_RESET_PLLC_BASE, PLLC_STORE_MASK
+
+	adr	r2, tegra20_sdram_pad_address
+	adr	r4, tegra20_sdram_pad_save
+	mov	r5, #0
+
+	ldr	r6, tegra20_sdram_pad_size
+padload:
+	ldr	r7, [r2, r5]		@ r7 is the addr in the pad_address
+
+	ldr	r1, [r4, r5]
+	str	r1, [r7]		@ restore the value in pad_save
+
+	add	r5, r5, #4
+	cmp	r6, r5
+	bne	padload
+
+padload_done:
+	/* 255uS delay for PLL stabilization */
+	mov32	r7, TEGRA_TMRUS_BASE
+	ldr	r1, [r7]
+	add	r1, r1, #0xff
+	wait_until r1, r7, r9
+
+	adr	r4, tegra20_sclk_save
+	ldr	r4, [r4]
+	str	r4, [r0, #CLK_RESET_SCLK_BURST]
+	mov32	r4, ((1 << 28) | (4))	@ burst policy is PLLP
+	str	r4, [r0, #CLK_RESET_CCLK_BURST]
+
+	mov32	r0, TEGRA_EMC_BASE
+	ldr	r1, [r0, #EMC_CFG]
+	bic	r1, r1, #(1 << 31)	@ disable DRAM_CLK_STOP
+	str	r1, [r0, #EMC_CFG]
+
+	mov	r1, #0
+	str	r1, [r0, #EMC_SELF_REF]	@ take DRAM out of self refresh
+	mov	r1, #1
+	str	r1, [r0, #EMC_NOP]
+	str	r1, [r0, #EMC_NOP]
+
+	emc_device_mask r1, r0
+
+exit_selfrefresh_loop:
+	ldr	r2, [r0, #EMC_EMC_STATUS]
+	ands	r2, r2, r1
+	bne	exit_selfrefresh_loop
+
+	mov	r1, #0			@ unstall all transactions
+	str	r1, [r0, #EMC_REQ_CTRL]
+
+	mov32	r0, TEGRA_PMC_BASE
+	ldr	r0, [r0, #PMC_SCRATCH41]
+	ret	r0			@ jump to tegra_resume
+ENDPROC(tegra20_lp1_reset)
+
+/*
+ * tegra20_tear_down_core
+ *
+ * copied into and executed from IRAM
+ * puts memory in self-refresh for LP0 and LP1
+ */
+tegra20_tear_down_core:
+	bl	tegra20_sdram_self_refresh
+	bl	tegra20_switch_cpu_to_clk32k
+	b	tegra20_enter_sleep
+
+/*
+ * tegra20_switch_cpu_to_clk32k
+ *
+ * In LP0 and LP1 all PLLs will be turned off. Switch the CPU and system clock
+ * to the 32KHz clock.
+ */
+tegra20_switch_cpu_to_clk32k:
+	/*
+	 * start by switching to CLKM to safely disable PLLs, then switch to
+	 * CLKS.
+	 */
+	mov	r0, #(1 << 28)
+	str	r0, [r5, #CLK_RESET_SCLK_BURST]
+	str	r0, [r5, #CLK_RESET_CCLK_BURST]
+	mov	r0, #0
+	str	r0, [r5, #CLK_RESET_CCLK_DIVIDER]
+	str	r0, [r5, #CLK_RESET_SCLK_DIVIDER]
+
+	/* 2uS delay delay between changing SCLK and disabling PLLs */
+	mov32	r7, TEGRA_TMRUS_BASE
+	ldr	r1, [r7]
+	add	r1, r1, #2
+	wait_until r1, r7, r9
+
+	store_pll_state r0, r1, r5, CLK_RESET_PLLC_BASE, PLLC_STORE_MASK
+	store_pll_state r0, r1, r5, CLK_RESET_PLLM_BASE, PLLM_STORE_MASK
+	store_pll_state r0, r1, r5, CLK_RESET_PLLP_BASE, PLLP_STORE_MASK
+
+	/* disable PLLM, PLLP and PLLC */
+	ldr	r0, [r5, #CLK_RESET_PLLM_BASE]
+	bic	r0, r0, #(1 << 30)
+	str	r0, [r5, #CLK_RESET_PLLM_BASE]
+	ldr	r0, [r5, #CLK_RESET_PLLP_BASE]
+	bic	r0, r0, #(1 << 30)
+	str	r0, [r5, #CLK_RESET_PLLP_BASE]
+	ldr	r0, [r5, #CLK_RESET_PLLC_BASE]
+	bic	r0, r0, #(1 << 30)
+	str	r0, [r5, #CLK_RESET_PLLC_BASE]
+
+	/* switch to CLKS */
+	mov	r0, #0	/* brust policy = 32KHz */
+	str	r0, [r5, #CLK_RESET_SCLK_BURST]
+
+	ret	lr
+
+/*
+ * 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
+
+/*
+ * tegra20_sdram_self_refresh
+ *
+ * called with MMU off and caches disabled
+ * puts sdram in self refresh
+ * must be executed from IRAM
+ */
+tegra20_sdram_self_refresh:
+	mov32	r1, TEGRA_EMC_BASE	@ r1 reserved for emc base addr
+
+	mov	r2, #3
+	str	r2, [r1, #EMC_REQ_CTRL]	@ stall incoming DRAM requests
+
+emcidle:
+	ldr	r2, [r1, #EMC_EMC_STATUS]
+	tst	r2, #4
+	beq	emcidle
+
+	mov	r2, #1
+	str	r2, [r1, #EMC_SELF_REF]
+
+	emc_device_mask r2, r1
+
+emcself:
+	ldr	r3, [r1, #EMC_EMC_STATUS]
+	and	r3, r3, r2
+	cmp	r3, r2
+	bne	emcself			@ loop until DDR in self-refresh
+
+	adr	r2, tegra20_sdram_pad_address
+	adr	r3, tegra20_sdram_pad_safe
+	adr	r4, tegra20_sdram_pad_save
+	mov	r5, #0
+
+	ldr	r6, tegra20_sdram_pad_size
+padsave:
+	ldr	r0, [r2, r5]		@ r0 is the addr in the pad_address
+
+	ldr	r1, [r0]
+	str	r1, [r4, r5]		@ save the content of the addr
+
+	ldr	r1, [r3, r5]
+	str	r1, [r0]		@ set the save val to the addr
+
+	add	r5, r5, #4
+	cmp	r6, r5
+	bne	padsave
+padsave_done:
+
+	mov32	r5, TEGRA_CLK_RESET_BASE
+	ldr	r0, [r5, #CLK_RESET_SCLK_BURST]
+	adr	r2, tegra20_sclk_save
+	str	r0, [r2]
+	dsb
+	ret	lr
+
+tegra20_sdram_pad_address:
+	.word	TEGRA_APB_MISC_BASE + APB_MISC_XM2CFGCPADCTRL
+	.word	TEGRA_APB_MISC_BASE + APB_MISC_XM2CFGDPADCTRL
+	.word	TEGRA_APB_MISC_BASE + APB_MISC_XM2CLKCFGPADCTRL
+	.word	TEGRA_APB_MISC_BASE + APB_MISC_XM2COMPPADCTRL
+	.word	TEGRA_APB_MISC_BASE + APB_MISC_XM2VTTGENPADCTRL
+	.word	TEGRA_APB_MISC_BASE + APB_MISC_XM2CFGCPADCTRL2
+	.word	TEGRA_APB_MISC_BASE + APB_MISC_XM2CFGDPADCTRL2
+
+tegra20_sdram_pad_size:
+	.word	tegra20_sdram_pad_size - tegra20_sdram_pad_address
+
+tegra20_sdram_pad_safe:
+	.word	0x8
+	.word	0x8
+	.word	0x0
+	.word	0x8
+	.word	0x5500
+	.word	0x08080040
+	.word	0x0
+
+tegra20_sclk_save:
+	.word	0x0
+
+tegra20_sdram_pad_save:
+	.rept (tegra20_sdram_pad_size - tegra20_sdram_pad_address) / 4
+	.long	0
+	.endr
+
+tegra_pll_state:
+	.word	0x0
+
+	.ltorg
+/* dummy symbol for end of IRAM */
+	.align L1_CACHE_SHIFT
+	.globl tegra20_iram_end
+tegra20_iram_end:
+	b	.
+#endif
diff --git a/arch/arm/mach-tegra/sleep-tegra30.S b/arch/arm/mach-tegra/sleep-tegra30.S
new file mode 100644
index 000000000..134ea5fe4
--- /dev/null
+++ b/arch/arm/mach-tegra/sleep-tegra30.S
@@ -0,0 +1,909 @@
+/* SPDX-License-Identifier: GPL-2.0-only */
+/*
+ * Copyright (c) 2012, NVIDIA Corporation. All rights reserved.
+ */
+
+#include <linux/linkage.h>
+
+#include <soc/tegra/flowctrl.h>
+#include <soc/tegra/fuse.h>
+
+#include <asm/asm-offsets.h>
+#include <asm/assembler.h>
+#include <asm/cache.h>
+
+#include "irammap.h"
+#include "sleep.h"
+
+#define EMC_CFG				0xc
+#define EMC_ADR_CFG			0x10
+#define EMC_TIMING_CONTROL		0x28
+#define EMC_NOP				0xdc
+#define EMC_SELF_REF			0xe0
+#define EMC_MRW				0xe8
+#define EMC_FBIO_CFG5			0x104
+#define EMC_AUTO_CAL_CONFIG		0x2a4
+#define EMC_AUTO_CAL_INTERVAL		0x2a8
+#define EMC_AUTO_CAL_STATUS		0x2ac
+#define EMC_REQ_CTRL			0x2b0
+#define EMC_CFG_DIG_DLL			0x2bc
+#define EMC_EMC_STATUS			0x2b4
+#define EMC_ZCAL_INTERVAL		0x2e0
+#define EMC_ZQ_CAL			0x2ec
+#define EMC_XM2VTTGENPADCTRL		0x310
+#define EMC_XM2VTTGENPADCTRL2		0x314
+
+#define PMC_CTRL			0x0
+#define PMC_CTRL_SIDE_EFFECT_LP0 (1 << 14) /* enter LP0 when CPU pwr gated */
+
+#define PMC_PLLP_WB0_OVERRIDE		0xf8
+#define PMC_IO_DPD_REQ			0x1b8
+#define PMC_IO_DPD_STATUS		0x1bc
+
+#define CLK_RESET_CCLK_BURST		0x20
+#define CLK_RESET_CCLK_DIVIDER		0x24
+#define CLK_RESET_SCLK_BURST		0x28
+#define CLK_RESET_SCLK_DIVIDER		0x2c
+
+#define CLK_RESET_PLLC_BASE		0x80
+#define CLK_RESET_PLLC_MISC		0x8c
+#define CLK_RESET_PLLM_BASE		0x90
+#define CLK_RESET_PLLM_MISC		0x9c
+#define CLK_RESET_PLLP_BASE		0xa0
+#define CLK_RESET_PLLP_MISC		0xac
+#define CLK_RESET_PLLA_BASE		0xb0
+#define CLK_RESET_PLLA_MISC		0xbc
+#define CLK_RESET_PLLX_BASE		0xe0
+#define CLK_RESET_PLLX_MISC		0xe4
+#define CLK_RESET_PLLX_MISC3		0x518
+#define CLK_RESET_PLLX_MISC3_IDDQ	3
+#define CLK_RESET_PLLM_MISC_IDDQ	5
+#define CLK_RESET_PLLC_MISC_IDDQ	26
+#define CLK_RESET_PLLP_RESHIFT		0x528
+#define CLK_RESET_PLLP_RESHIFT_DEFAULT	0x3b
+#define CLK_RESET_PLLP_RESHIFT_ENABLE	0x3
+
+#define CLK_RESET_CLK_SOURCE_MSELECT	0x3b4
+
+#define MSELECT_CLKM			(0x3 << 30)
+
+#define LOCK_DELAY 50 /* safety delay after lock is detected */
+
+#define TEGRA30_POWER_HOTPLUG_SHUTDOWN	(1 << 27) /* Hotplug shutdown */
+
+#define PLLA_STORE_MASK			(1 << 0)
+#define PLLC_STORE_MASK			(1 << 1)
+#define PLLM_STORE_MASK			(1 << 2)
+#define PLLP_STORE_MASK			(1 << 3)
+#define PLLX_STORE_MASK			(1 << 4)
+#define PLLM_PMC_STORE_MASK		(1 << 5)
+
+.arch armv7-a
+
+.macro emc_device_mask, rd, base
+	ldr	\rd, [\base, #EMC_ADR_CFG]
+	tst	\rd, #0x1
+	moveq	\rd, #(0x1 << 8)		@ just 1 device
+	movne	\rd, #(0x3 << 8)		@ 2 devices
+.endm
+
+.macro emc_timing_update, rd, base
+	mov	\rd, #1
+	str	\rd, [\base, #EMC_TIMING_CONTROL]
+1001:
+	ldr	\rd, [\base, #EMC_EMC_STATUS]
+	tst	\rd, #(0x1<<23)	@ wait EMC_STATUS_TIMING_UPDATE_STALLED is clear
+	bne	1001b
+.endm
+
+.macro test_pll_state, rd, test_mask
+	ldr	\rd, tegra_pll_state
+	tst	\rd, #\test_mask
+.endm
+
+.macro store_pll_state, rd, tmp, r_car_base, pll_base, pll_mask
+	ldr	\rd, [\r_car_base, #\pll_base]
+	tst	\rd, #(1 << 30)
+	ldr	\rd, tegra_pll_state
+	biceq	\rd, \rd, #\pll_mask
+	orrne	\rd, \rd, #\pll_mask
+	adr	\tmp, tegra_pll_state
+	str	\rd, [\tmp]
+.endm
+
+.macro store_pllm_pmc_state, rd, tmp, pmc_base
+	ldr	\rd, [\pmc_base, #PMC_PLLP_WB0_OVERRIDE]
+	tst	\rd, #(1 << 12)
+	ldr	\rd, tegra_pll_state
+	biceq	\rd, \rd, #PLLM_PMC_STORE_MASK
+	orrne	\rd, \rd, #PLLM_PMC_STORE_MASK
+	adr	\tmp, tegra_pll_state
+	str	\rd, [\tmp]
+.endm
+
+.macro pllm_pmc_enable, rd, pmc_base
+	test_pll_state \rd, PLLM_PMC_STORE_MASK
+
+	ldrne	\rd, [\pmc_base, #PMC_PLLP_WB0_OVERRIDE]
+	orrne	\rd, \rd, #(1 << 12)
+	strne	\rd, [\pmc_base, #PMC_PLLP_WB0_OVERRIDE]
+.endm
+
+.macro pll_enable, rd, r_car_base, pll_base, pll_misc, test_mask
+	test_pll_state \rd, \test_mask
+	beq	1f
+
+	ldr	\rd, [\r_car_base, #\pll_base]
+	tst	\rd, #(1 << 30)
+	orreq	\rd, \rd, #(1 << 30)
+	streq	\rd, [\r_car_base, #\pll_base]
+	/* Enable lock detector */
+	.if	\pll_misc
+	ldr	\rd, [\r_car_base, #\pll_misc]
+	bic	\rd, \rd, #(1 << 18)
+	str	\rd, [\r_car_base, #\pll_misc]
+	ldr	\rd, [\r_car_base, #\pll_misc]
+	ldr	\rd, [\r_car_base, #\pll_misc]
+	orr	\rd, \rd, #(1 << 18)
+	str	\rd, [\r_car_base, #\pll_misc]
+	.endif
+1:
+.endm
+
+.macro pll_locked, rd, r_car_base, pll_base, test_mask
+	test_pll_state \rd, \test_mask
+	beq	2f
+1:
+	ldr	\rd, [\r_car_base, #\pll_base]
+	tst	\rd, #(1 << 27)
+	beq	1b
+2:
+.endm
+
+.macro pll_iddq_exit, rd, car, iddq, iddq_bit
+	ldr	\rd, [\car, #\iddq]
+	bic	\rd, \rd, #(1<<\iddq_bit)
+	str	\rd, [\car, #\iddq]
+.endm
+
+.macro pll_iddq_entry, rd, car, iddq, iddq_bit
+	ldr	\rd, [\car, #\iddq]
+	orr	\rd, \rd, #(1<<\iddq_bit)
+	str	\rd, [\car, #\iddq]
+.endm
+
+#if defined(CONFIG_HOTPLUG_CPU) || defined(CONFIG_PM_SLEEP)
+/*
+ * tegra30_hotplug_shutdown(void)
+ *
+ * Powergates the current CPU.
+ * Should never return.
+ */
+ENTRY(tegra30_hotplug_shutdown)
+	/* Powergate this CPU */
+	mov	r0, #TEGRA30_POWER_HOTPLUG_SHUTDOWN
+	bl	tegra30_cpu_shutdown
+	ret	lr			@ should never get here
+ENDPROC(tegra30_hotplug_shutdown)
+
+/*
+ * tegra30_cpu_shutdown(unsigned long flags)
+ *
+ * Puts the current CPU in wait-for-event mode on the flow controller
+ * and powergates it -- flags (in R0) indicate the request type.
+ *
+ * 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
+	reteq	lr		@ Must never be called for CPU 0
+_no_cpu0_chk:
+
+	ldr	r12, =TEGRA_FLOW_CTRL_VIRT
+	cpu_to_csr_reg r1, r3
+	add	r1, r1, r12	@ virtual CSR address for this CPU
+	cpu_to_halt_reg r2, r3
+	add	r2, r2, r12	@ virtual HALT_EVENTS address for this CPU
+
+	/*
+	 * Clear this CPU's "event" and "interrupt" flags and power gate
+	 * it when halting but not before it is in the "WFE" state.
+	 */
+	movw	r12, \
+		FLOW_CTRL_CSR_INTR_FLAG | FLOW_CTRL_CSR_EVENT_FLAG | \
+		FLOW_CTRL_CSR_ENABLE
+	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		)
+	str	r12, [r1]
+
+	/* Halt this CPU. */
+	mov	r3, #0x400
+delay_1:
+	subs	r3, r3, #1			@ delay as a part of wfe war.
+	bge	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
+	orrne	r3, r3, #FLOW_CTRL_HALT_GIC_IRQ
+	orrne	r3, r3, #FLOW_CTRL_HALT_GIC_FIQ
+flow_ctrl_done:
+	cmp	r10, #TEGRA30
+	str	r3, [r2]
+	ldr	r0, [r2]
+	b	wfe_war
+
+__cpu_reset_again:
+	dsb
+	.align 5
+	wfeeq					@ CPU should be power gated here
+	wfine
+wfe_war:
+	b	__cpu_reset_again
+
+	/*
+	 * 38 nop's, which fills rest of wfe cache line and
+	 * 4 more cachelines with nop
+	 */
+	.rept 38
+	nop
+	.endr
+	b	.				@ should never get here
+
+ENDPROC(tegra30_cpu_shutdown)
+#endif
+
+#ifdef CONFIG_PM_SLEEP
+/*
+ * tegra30_sleep_core_finish(unsigned long v2p)
+ *
+ * Enters suspend in LP0 or LP1 by turning off the MMU and jumping to
+ * tegra30_tear_down_core in IRAM
+ */
+ENTRY(tegra30_sleep_core_finish)
+	mov	r4, r0
+	/* Flush, disable the L1 data cache and exit SMP */
+	mov	r0, #TEGRA_FLUSH_CACHE_ALL
+	bl	tegra_disable_clean_inv_dcache
+	mov	r0, r4
+
+	/*
+	 * Preload all the address literals that are needed for the
+	 * CPU power-gating process, to avoid loading from SDRAM which
+	 * are not supported once SDRAM is put into self-refresh.
+	 * LP0 / LP1 use physical address, since the MMU needs to be
+	 * disabled before putting SDRAM into self-refresh to avoid
+	 * memory access due to page table walks.
+	 */
+	mov32	r4, TEGRA_PMC_BASE
+	mov32	r5, TEGRA_CLK_RESET_BASE
+	mov32	r6, TEGRA_FLOW_CTRL_BASE
+	mov32	r7, TEGRA_TMRUS_BASE
+
+	mov32	r3, tegra_shut_off_mmu
+	add	r3, r3, r0
+
+	mov32	r0, tegra30_tear_down_core
+	mov32	r1, tegra30_iram_start
+	sub	r0, r0, r1
+	mov32	r1, TEGRA_IRAM_LPx_RESUME_AREA
+	add	r0, r0, r1
+
+	ret	r3
+ENDPROC(tegra30_sleep_core_finish)
+
+/*
+ * tegra30_pm_secondary_cpu_suspend(unsigned long unused_arg)
+ *
+ * Enters LP2 on secondary CPU by exiting coherency and powergating the CPU.
+ */
+ENTRY(tegra30_pm_secondary_cpu_suspend)
+	mov	r7, lr
+
+	/* Flush and disable the L1 data cache */
+	mov 	r0, #TEGRA_FLUSH_CACHE_LOUIS
+	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
+	ret	r7
+ENDPROC(tegra30_pm_secondary_cpu_suspend)
+
+/*
+ * tegra30_tear_down_cpu
+ *
+ * Switches the CPU to enter sleep.
+ */
+ENTRY(tegra30_tear_down_cpu)
+	mov32	r6, TEGRA_FLOW_CTRL_BASE
+
+	b	tegra30_enter_sleep
+ENDPROC(tegra30_tear_down_cpu)
+
+/* START OF ROUTINES COPIED TO IRAM */
+	.align L1_CACHE_SHIFT
+	.globl tegra30_iram_start
+tegra30_iram_start:
+
+/*
+ * tegra30_lp1_reset
+ *
+ * reset vector for LP1 restore; copied into IRAM during suspend.
+ * Brings the system back up to a safe staring point (SDRAM out of
+ * self-refresh, PLLC, PLLM and PLLP reenabled, CPU running on PLLX,
+ * system clock running on the same PLL that it suspended at), and
+ * jumps to tegra_resume to restore virtual addressing.
+ * The physical address of tegra_resume expected to be stored in
+ * PMC_SCRATCH41.
+ *
+ * NOTE: THIS *MUST* BE RELOCATED TO TEGRA_IRAM_LPx_RESUME_AREA.
+ */
+ENTRY(tegra30_lp1_reset)
+	/*
+	 * The CPU and system bus are running at 32KHz and executing from
+	 * IRAM when this code is executed; immediately switch to CLKM and
+	 * enable PLLP, PLLM, PLLC, PLLA and PLLX.
+	 */
+	mov32	r0, TEGRA_CLK_RESET_BASE
+
+	mov	r1, #(1 << 28)
+	str	r1, [r0, #CLK_RESET_SCLK_BURST]
+	str	r1, [r0, #CLK_RESET_CCLK_BURST]
+	mov	r1, #0
+	str	r1, [r0, #CLK_RESET_CCLK_DIVIDER]
+	str	r1, [r0, #CLK_RESET_SCLK_DIVIDER]
+
+	tegra_get_soc_id TEGRA_APB_MISC_BASE, r10
+	cmp	r10, #TEGRA30
+	beq	_no_pll_iddq_exit
+
+	pll_iddq_exit r1, r0, CLK_RESET_PLLM_MISC, CLK_RESET_PLLM_MISC_IDDQ
+	pll_iddq_exit r1, r0, CLK_RESET_PLLC_MISC, CLK_RESET_PLLC_MISC_IDDQ
+	pll_iddq_exit r1, r0, CLK_RESET_PLLX_MISC3, CLK_RESET_PLLX_MISC3_IDDQ
+
+	mov32	r7, TEGRA_TMRUS_BASE
+	ldr	r1, [r7]
+	add	r1, r1, #2
+	wait_until r1, r7, r3
+
+	/* enable PLLM via PMC */
+	mov32	r2, TEGRA_PMC_BASE
+	pllm_pmc_enable r1, r2
+
+	pll_enable r1, r0, CLK_RESET_PLLM_BASE, 0, PLLM_STORE_MASK
+	pll_enable r1, r0, CLK_RESET_PLLC_BASE, 0, PLLC_STORE_MASK
+	pll_enable r1, r0, CLK_RESET_PLLX_BASE, 0, PLLX_STORE_MASK
+
+	b	_pll_m_c_x_done
+
+_no_pll_iddq_exit:
+	/* enable PLLM via PMC */
+	mov32	r2, TEGRA_PMC_BASE
+	pllm_pmc_enable r1, r2
+
+	pll_enable r1, r0, CLK_RESET_PLLM_BASE, CLK_RESET_PLLM_MISC, PLLM_STORE_MASK
+	pll_enable r1, r0, CLK_RESET_PLLC_BASE, CLK_RESET_PLLC_MISC, PLLC_STORE_MASK
+
+_pll_m_c_x_done:
+	pll_enable r1, r0, CLK_RESET_PLLP_BASE, CLK_RESET_PLLP_MISC, PLLP_STORE_MASK
+	pll_enable r1, r0, CLK_RESET_PLLA_BASE, CLK_RESET_PLLA_MISC, PLLA_STORE_MASK
+
+	pll_locked r1, r0, CLK_RESET_PLLM_BASE, PLLM_STORE_MASK
+	pll_locked r1, r0, CLK_RESET_PLLP_BASE, PLLP_STORE_MASK
+	pll_locked r1, r0, CLK_RESET_PLLA_BASE, PLLA_STORE_MASK
+	pll_locked r1, r0, CLK_RESET_PLLC_BASE, PLLC_STORE_MASK
+
+	/*
+	 * CPUFreq driver could select other PLL for CPU. PLLX will be
+	 * enabled by the Tegra30 CLK driver on an as-needed basis, see
+	 * tegra30_cpu_clock_resume().
+	 */
+	tegra_get_soc_id TEGRA_APB_MISC_BASE, r1
+	cmp	r1, #TEGRA30
+	beq	1f
+
+	pll_locked r1, r0, CLK_RESET_PLLX_BASE, PLLX_STORE_MASK
+
+	ldr	r1, [r0, #CLK_RESET_PLLP_BASE]
+	bic	r1, r1, #(1<<31)	@ disable PllP bypass
+	str	r1, [r0, #CLK_RESET_PLLP_BASE]
+
+	mov	r1, #CLK_RESET_PLLP_RESHIFT_DEFAULT
+	str	r1, [r0, #CLK_RESET_PLLP_RESHIFT]
+1:
+
+	mov32	r7, TEGRA_TMRUS_BASE
+	ldr	r1, [r7]
+	add	r1, r1, #LOCK_DELAY
+	wait_until r1, r7, r3
+
+	adr	r5, tegra_sdram_pad_save
+
+	ldr	r4, [r5, #0x18]		@ restore CLK_SOURCE_MSELECT
+	str	r4, [r0, #CLK_RESET_CLK_SOURCE_MSELECT]
+
+	ldr	r4, [r5, #0x1C]		@ restore SCLK_BURST
+	str	r4, [r0, #CLK_RESET_SCLK_BURST]
+
+	movw	r4, #:lower16:((1 << 28) | (0x4))	@ burst policy is PLLP
+	movt	r4, #:upper16:((1 << 28) | (0x4))
+	str	r4, [r0, #CLK_RESET_CCLK_BURST]
+
+	/* Restore pad power state to normal */
+	ldr	r1, [r5, #0x14]		@ PMC_IO_DPD_STATUS
+	mvn	r1, r1
+	bic	r1, r1, #(1 << 31)
+	orr	r1, r1, #(1 << 30)
+	str	r1, [r2, #PMC_IO_DPD_REQ]	@ DPD_OFF
+
+	cmp	r10, #TEGRA30
+	movweq	r0, #:lower16:TEGRA_EMC_BASE	@ r0 reserved for emc base
+	movteq	r0, #:upper16:TEGRA_EMC_BASE
+	cmp	r10, #TEGRA114
+	movweq	r0, #:lower16:TEGRA_EMC0_BASE
+	movteq	r0, #:upper16:TEGRA_EMC0_BASE
+	cmp	r10, #TEGRA124
+	movweq	r0, #:lower16:TEGRA124_EMC_BASE
+	movteq	r0, #:upper16:TEGRA124_EMC_BASE
+
+exit_self_refresh:
+	ldr	r1, [r5, #0xC]		@ restore EMC_XM2VTTGENPADCTRL
+	str	r1, [r0, #EMC_XM2VTTGENPADCTRL]
+	ldr	r1, [r5, #0x10]		@ restore EMC_XM2VTTGENPADCTRL2
+	str	r1, [r0, #EMC_XM2VTTGENPADCTRL2]
+	ldr	r1, [r5, #0x8]		@ restore EMC_AUTO_CAL_INTERVAL
+	str	r1, [r0, #EMC_AUTO_CAL_INTERVAL]
+
+	/* Relock DLL */
+	ldr	r1, [r0, #EMC_CFG_DIG_DLL]
+	orr	r1, r1, #(1 << 30)	@ set DLL_RESET
+	str	r1, [r0, #EMC_CFG_DIG_DLL]
+
+	emc_timing_update r1, r0
+
+	cmp	r10, #TEGRA114
+	movweq	r1, #:lower16:TEGRA_EMC1_BASE
+	movteq	r1, #:upper16:TEGRA_EMC1_BASE
+	cmpeq	r0, r1
+
+	ldr	r1, [r0, #EMC_AUTO_CAL_CONFIG]
+	orr	r1, r1, #(1 << 31)	@ set AUTO_CAL_ACTIVE
+	orreq	r1, r1, #(1 << 27)	@ set slave mode for channel 1
+	str	r1, [r0, #EMC_AUTO_CAL_CONFIG]
+
+emc_wait_auto_cal_onetime:
+	ldr	r1, [r0, #EMC_AUTO_CAL_STATUS]
+	tst	r1, #(1 << 31)		@ wait until AUTO_CAL_ACTIVE is cleared
+	bne	emc_wait_auto_cal_onetime
+
+	ldr	r1, [r0, #EMC_CFG]
+	bic	r1, r1, #(1 << 31)	@ disable DRAM_CLK_STOP_PD
+	str	r1, [r0, #EMC_CFG]
+
+	mov	r1, #0
+	str	r1, [r0, #EMC_SELF_REF]	@ take DRAM out of self refresh
+	mov	r1, #1
+	cmp	r10, #TEGRA30
+	streq	r1, [r0, #EMC_NOP]
+	streq	r1, [r0, #EMC_NOP]
+
+	emc_device_mask r1, r0
+
+exit_selfrefresh_loop:
+	ldr	r2, [r0, #EMC_EMC_STATUS]
+	ands	r2, r2, r1
+	bne	exit_selfrefresh_loop
+
+	lsr	r1, r1, #8		@ devSel, bit0:dev0, bit1:dev1
+
+	mov32	r7, TEGRA_TMRUS_BASE
+	ldr	r2, [r0, #EMC_FBIO_CFG5]
+
+	and	r2, r2,	#3		@ check DRAM_TYPE
+	cmp	r2, #2
+	beq	emc_lpddr2
+
+	/* Issue a ZQ_CAL for dev0 - DDR3 */
+	mov32	r2, 0x80000011		@ DEV_SELECTION=2, LENGTH=LONG, CMD=1
+	str	r2, [r0, #EMC_ZQ_CAL]
+	ldr	r2, [r7]
+	add	r2, r2, #10
+	wait_until r2, r7, r3
+
+	tst	r1, #2
+	beq	zcal_done
+
+	/* Issue a ZQ_CAL for dev1 - DDR3 */
+	mov32	r2, 0x40000011		@ DEV_SELECTION=1, LENGTH=LONG, CMD=1
+	str	r2, [r0, #EMC_ZQ_CAL]
+	ldr	r2, [r7]
+	add	r2, r2, #10
+	wait_until r2, r7, r3
+	b	zcal_done
+
+emc_lpddr2:
+	/* Issue a ZQ_CAL for dev0 - LPDDR2 */
+	mov32	r2, 0x800A00AB		@ DEV_SELECTION=2, MA=10, OP=0xAB
+	str	r2, [r0, #EMC_MRW]
+	ldr	r2, [r7]
+	add	r2, r2, #1
+	wait_until r2, r7, r3
+
+	tst	r1, #2
+	beq	zcal_done
+
+	/* Issue a ZQ_CAL for dev0 - LPDDR2 */
+	mov32	r2, 0x400A00AB		@ DEV_SELECTION=1, MA=10, OP=0xAB
+	str	r2, [r0, #EMC_MRW]
+	ldr	r2, [r7]
+	add	r2, r2, #1
+	wait_until r2, r7, r3
+
+zcal_done:
+	mov	r1, #0			@ unstall all transactions
+	str	r1, [r0, #EMC_REQ_CTRL]
+	ldr	r1, [r5, #0x4]		@ restore EMC_ZCAL_INTERVAL
+	str	r1, [r0, #EMC_ZCAL_INTERVAL]
+	ldr	r1, [r5, #0x0]		@ restore EMC_CFG
+	str	r1, [r0, #EMC_CFG]
+
+	emc_timing_update r1, r0
+
+	/* Tegra114 had dual EMC channel, now config the other one */
+	cmp	r10, #TEGRA114
+	bne	__no_dual_emc_chanl
+	mov32	r1, TEGRA_EMC1_BASE
+	cmp	r0, r1
+	movne	r0, r1
+	addne	r5, r5, #0x20
+	bne	exit_self_refresh
+__no_dual_emc_chanl:
+
+	mov32	r0, TEGRA_PMC_BASE
+	ldr	r0, [r0, #PMC_SCRATCH41]
+	ret	r0			@ jump to tegra_resume
+ENDPROC(tegra30_lp1_reset)
+
+	.align	L1_CACHE_SHIFT
+tegra30_sdram_pad_address:
+	.word	TEGRA_EMC_BASE + EMC_CFG				@0x0
+	.word	TEGRA_EMC_BASE + EMC_ZCAL_INTERVAL			@0x4
+	.word	TEGRA_EMC_BASE + EMC_AUTO_CAL_INTERVAL			@0x8
+	.word	TEGRA_EMC_BASE + EMC_XM2VTTGENPADCTRL			@0xc
+	.word	TEGRA_EMC_BASE + EMC_XM2VTTGENPADCTRL2			@0x10
+	.word	TEGRA_PMC_BASE + PMC_IO_DPD_STATUS			@0x14
+	.word	TEGRA_CLK_RESET_BASE + CLK_RESET_CLK_SOURCE_MSELECT	@0x18
+	.word	TEGRA_CLK_RESET_BASE + CLK_RESET_SCLK_BURST		@0x1c
+tegra30_sdram_pad_address_end:
+
+tegra114_sdram_pad_address:
+	.word	TEGRA_EMC0_BASE + EMC_CFG				@0x0
+	.word	TEGRA_EMC0_BASE + EMC_ZCAL_INTERVAL			@0x4
+	.word	TEGRA_EMC0_BASE + EMC_AUTO_CAL_INTERVAL			@0x8
+	.word	TEGRA_EMC0_BASE + EMC_XM2VTTGENPADCTRL			@0xc
+	.word	TEGRA_EMC0_BASE + EMC_XM2VTTGENPADCTRL2			@0x10
+	.word	TEGRA_PMC_BASE + PMC_IO_DPD_STATUS			@0x14
+	.word	TEGRA_CLK_RESET_BASE + CLK_RESET_CLK_SOURCE_MSELECT	@0x18
+	.word	TEGRA_CLK_RESET_BASE + CLK_RESET_SCLK_BURST		@0x1c
+	.word	TEGRA_EMC1_BASE + EMC_CFG				@0x20
+	.word	TEGRA_EMC1_BASE + EMC_ZCAL_INTERVAL			@0x24
+	.word	TEGRA_EMC1_BASE + EMC_AUTO_CAL_INTERVAL			@0x28
+	.word	TEGRA_EMC1_BASE + EMC_XM2VTTGENPADCTRL			@0x2c
+	.word	TEGRA_EMC1_BASE + EMC_XM2VTTGENPADCTRL2			@0x30
+tegra114_sdram_pad_adress_end:
+
+tegra124_sdram_pad_address:
+	.word	TEGRA124_EMC_BASE + EMC_CFG				@0x0
+	.word	TEGRA124_EMC_BASE + EMC_ZCAL_INTERVAL			@0x4
+	.word	TEGRA124_EMC_BASE + EMC_AUTO_CAL_INTERVAL		@0x8
+	.word	TEGRA124_EMC_BASE + EMC_XM2VTTGENPADCTRL		@0xc
+	.word	TEGRA124_EMC_BASE + EMC_XM2VTTGENPADCTRL2		@0x10
+	.word	TEGRA_PMC_BASE + PMC_IO_DPD_STATUS			@0x14
+	.word	TEGRA_CLK_RESET_BASE + CLK_RESET_CLK_SOURCE_MSELECT	@0x18
+	.word	TEGRA_CLK_RESET_BASE + CLK_RESET_SCLK_BURST		@0x1c
+tegra124_sdram_pad_address_end:
+
+tegra30_sdram_pad_size:
+	.word	tegra30_sdram_pad_address_end - tegra30_sdram_pad_address
+
+tegra114_sdram_pad_size:
+	.word	tegra114_sdram_pad_adress_end - tegra114_sdram_pad_address
+
+	.type	tegra_sdram_pad_save, %object
+tegra_sdram_pad_save:
+	.rept (tegra114_sdram_pad_adress_end - tegra114_sdram_pad_address) / 4
+	.long	0
+	.endr
+
+tegra_pll_state:
+	.word	0x0
+
+/*
+ * tegra30_tear_down_core
+ *
+ * copied into and executed from IRAM
+ * puts memory in self-refresh for LP0 and LP1
+ */
+tegra30_tear_down_core:
+	bl	tegra30_sdram_self_refresh
+	bl	tegra30_switch_cpu_to_clk32k
+	b	tegra30_enter_sleep
+
+/*
+ * tegra30_switch_cpu_to_clk32k
+ *
+ * In LP0 and LP1 all PLLs will be turned off. Switching the CPU and System CLK
+ * to the 32KHz clock.
+ * r4 = TEGRA_PMC_BASE
+ * r5 = TEGRA_CLK_RESET_BASE
+ * r6 = TEGRA_FLOW_CTRL_BASE
+ * r7 = TEGRA_TMRUS_BASE
+ * r10= SoC ID
+ */
+tegra30_switch_cpu_to_clk32k:
+	/*
+	 * start by jumping to CLKM to safely disable PLLs, then jump to
+	 * CLKS.
+	 */
+	mov	r0, #(1 << 28)
+	str	r0, [r5, #CLK_RESET_SCLK_BURST]
+	/* 2uS delay delay between changing SCLK and CCLK */
+	ldr	r1, [r7]
+	add	r1, r1, #2
+	wait_until r1, r7, r9
+	str	r0, [r5, #CLK_RESET_CCLK_BURST]
+	mov	r0, #0
+	str	r0, [r5, #CLK_RESET_CCLK_DIVIDER]
+	str	r0, [r5, #CLK_RESET_SCLK_DIVIDER]
+
+	/* switch the clock source of mselect to be CLK_M */
+	ldr	r0, [r5, #CLK_RESET_CLK_SOURCE_MSELECT]
+	orr	r0, r0, #MSELECT_CLKM
+	str	r0, [r5, #CLK_RESET_CLK_SOURCE_MSELECT]
+
+	/* 2uS delay delay between changing SCLK and disabling PLLs */
+	ldr	r1, [r7]
+	add	r1, r1, #2
+	wait_until r1, r7, r9
+
+	/* store enable-state of PLLs */
+	store_pll_state r0, r1, r5, CLK_RESET_PLLA_BASE, PLLA_STORE_MASK
+	store_pll_state r0, r1, r5, CLK_RESET_PLLC_BASE, PLLC_STORE_MASK
+	store_pll_state r0, r1, r5, CLK_RESET_PLLM_BASE, PLLM_STORE_MASK
+	store_pll_state r0, r1, r5, CLK_RESET_PLLP_BASE, PLLP_STORE_MASK
+	store_pll_state r0, r1, r5, CLK_RESET_PLLX_BASE, PLLX_STORE_MASK
+	store_pllm_pmc_state r0, r1, r4
+
+	/* disable PLLM via PMC in LP1 */
+	ldr	r0, [r4, #PMC_PLLP_WB0_OVERRIDE]
+	bic	r0, r0, #(1 << 12)
+	str	r0, [r4, #PMC_PLLP_WB0_OVERRIDE]
+
+	/* disable PLLP, PLLA, PLLC and PLLX */
+	tegra_get_soc_id TEGRA_APB_MISC_BASE, r1
+	cmp	r1, #TEGRA30
+	ldr	r0, [r5, #CLK_RESET_PLLP_BASE]
+	orrne	r0, r0, #(1 << 31)	@ enable PllP bypass on fast cluster
+	bic	r0, r0, #(1 << 30)
+	str	r0, [r5, #CLK_RESET_PLLP_BASE]
+	beq	1f
+	mov	r0, #CLK_RESET_PLLP_RESHIFT_ENABLE
+	str	r0, [r5, #CLK_RESET_PLLP_RESHIFT]
+1:
+	ldr	r0, [r5, #CLK_RESET_PLLA_BASE]
+	bic	r0, r0, #(1 << 30)
+	str	r0, [r5, #CLK_RESET_PLLA_BASE]
+	ldr	r0, [r5, #CLK_RESET_PLLC_BASE]
+	bic	r0, r0, #(1 << 30)
+	str	r0, [r5, #CLK_RESET_PLLC_BASE]
+	ldr	r0, [r5, #CLK_RESET_PLLX_BASE]
+	bic	r0, r0, #(1 << 30)
+	str	r0, [r5, #CLK_RESET_PLLX_BASE]
+
+	cmp	r10, #TEGRA30
+	beq	_no_pll_in_iddq
+	pll_iddq_entry r1, r5, CLK_RESET_PLLX_MISC3, CLK_RESET_PLLX_MISC3_IDDQ
+_no_pll_in_iddq:
+
+	/*
+	 * Switch to clk_s (32KHz); bits 28:31=0
+	 * Enable burst on CPU IRQ; bit 24=1
+	 * Set IRQ burst clock source to clk_m; bits 10:8=0
+	 */
+	mov	r0, #(1 << 24)
+	str	r0, [r5, #CLK_RESET_SCLK_BURST]
+
+	ret	lr
+
+/*
+ * tegra30_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
+ * r6 = TEGRA_FLOW_CTRL_BASE
+ */
+tegra30_enter_sleep:
+	cpu_id	r1
+
+	cpu_to_csr_reg	r2, r1
+	ldr	r0, [r6, r2]
+	orr	r0, r0, #FLOW_CTRL_CSR_INTR_FLAG | FLOW_CTRL_CSR_EVENT_FLAG
+	orr	r0, r0, #FLOW_CTRL_CSR_ENABLE
+	str	r0, [r6, r2]
+
+	tegra_get_soc_id TEGRA_APB_MISC_BASE, r10
+	cmp	r10, #TEGRA30
+	mov	r0, #FLOW_CTRL_WAIT_FOR_INTERRUPT
+	orreq	r0, r0, #FLOW_CTRL_HALT_CPU_IRQ | FLOW_CTRL_HALT_CPU_FIQ
+	orrne   r0, r0, #FLOW_CTRL_HALT_LIC_IRQ | FLOW_CTRL_HALT_LIC_FIQ
+
+	cpu_to_halt_reg r2, r1
+	str	r0, [r6, r2]
+	dsb
+	ldr	r0, [r6, r2] /* memory barrier */
+
+	cmp	r10, #TEGRA30
+halted:
+	isb
+	dsb
+	wfine	/* CPU should be power gated here */
+	wfeeq
+
+	/* !!!FIXME!!! Implement halt failure handler */
+	b	halted
+
+/*
+ * tegra30_sdram_self_refresh
+ *
+ * called with MMU off and caches disabled
+ * must be executed from IRAM
+ * r4 = TEGRA_PMC_BASE
+ * r5 = TEGRA_CLK_RESET_BASE
+ * r6 = TEGRA_FLOW_CTRL_BASE
+ * r7 = TEGRA_TMRUS_BASE
+ * r10= SoC ID
+ */
+tegra30_sdram_self_refresh:
+
+	adr	r8, tegra_sdram_pad_save
+	tegra_get_soc_id TEGRA_APB_MISC_BASE, r10
+	cmp	r10, #TEGRA30
+	adreq	r2, tegra30_sdram_pad_address
+	ldreq	r3, tegra30_sdram_pad_size
+	cmp	r10, #TEGRA114
+	adreq	r2, tegra114_sdram_pad_address
+	ldreq	r3, tegra114_sdram_pad_size
+	cmp	r10, #TEGRA124
+	adreq	r2, tegra124_sdram_pad_address
+	ldreq	r3, tegra30_sdram_pad_size
+
+	mov	r9, #0
+
+padsave:
+	ldr	r0, [r2, r9]		@ r0 is the addr in the pad_address
+
+	ldr	r1, [r0]
+	str	r1, [r8, r9]		@ save the content of the addr
+
+	add	r9, r9, #4
+	cmp	r3, r9
+	bne	padsave
+padsave_done:
+
+	dsb
+
+	cmp	r10, #TEGRA30
+	ldreq	r0, =TEGRA_EMC_BASE	@ r0 reserved for emc base addr
+	cmp	r10, #TEGRA114
+	ldreq	r0, =TEGRA_EMC0_BASE
+	cmp	r10, #TEGRA124
+	ldreq	r0, =TEGRA124_EMC_BASE
+
+enter_self_refresh:
+	cmp	r10, #TEGRA30
+	mov	r1, #0
+	str	r1, [r0, #EMC_ZCAL_INTERVAL]
+	str	r1, [r0, #EMC_AUTO_CAL_INTERVAL]
+	ldr	r1, [r0, #EMC_CFG]
+	bic	r1, r1, #(1 << 28)
+	bicne	r1, r1, #(1 << 29)
+	str	r1, [r0, #EMC_CFG]	@ disable DYN_SELF_REF
+
+	emc_timing_update r1, r0
+
+	ldr	r1, [r7]
+	add	r1, r1, #5
+	wait_until r1, r7, r2
+
+emc_wait_auto_cal:
+	ldr	r1, [r0, #EMC_AUTO_CAL_STATUS]
+	tst	r1, #(1 << 31)		@ wait until AUTO_CAL_ACTIVE is cleared
+	bne	emc_wait_auto_cal
+
+	mov	r1, #3
+	str	r1, [r0, #EMC_REQ_CTRL]	@ stall incoming DRAM requests
+
+emcidle:
+	ldr	r1, [r0, #EMC_EMC_STATUS]
+	tst	r1, #4
+	beq	emcidle
+
+	mov	r1, #1
+	str	r1, [r0, #EMC_SELF_REF]
+
+	emc_device_mask r1, r0
+
+emcself:
+	ldr	r2, [r0, #EMC_EMC_STATUS]
+	and	r2, r2, r1
+	cmp	r2, r1
+	bne	emcself			@ loop until DDR in self-refresh
+
+	/* Put VTTGEN in the lowest power mode */
+	ldr	r1, [r0, #EMC_XM2VTTGENPADCTRL]
+	mov32	r2, 0xF8F8FFFF	@ clear XM2VTTGEN_DRVUP and XM2VTTGEN_DRVDN
+	and	r1, r1, r2
+	str	r1, [r0, #EMC_XM2VTTGENPADCTRL]
+	ldr	r1, [r0, #EMC_XM2VTTGENPADCTRL2]
+	cmp	r10, #TEGRA30
+	orreq	r1, r1, #7		@ set E_NO_VTTGEN
+	orrne	r1, r1, #0x3f
+	str	r1, [r0, #EMC_XM2VTTGENPADCTRL2]
+
+	emc_timing_update r1, r0
+
+	/* Tegra114 had dual EMC channel, now config the other one */
+	cmp	r10, #TEGRA114
+	bne	no_dual_emc_chanl
+	mov32	r1, TEGRA_EMC1_BASE
+	cmp	r0, r1
+	movne	r0, r1
+	bne	enter_self_refresh
+no_dual_emc_chanl:
+
+	ldr	r1, [r4, #PMC_CTRL]
+	tst	r1, #PMC_CTRL_SIDE_EFFECT_LP0
+	bne	pmc_io_dpd_skip
+	/*
+	 * Put DDR_DATA, DISC_ADDR_CMD, DDR_ADDR_CMD, POP_ADDR_CMD, POP_CLK
+	 * and COMP in the lowest power mode when LP1.
+	 */
+	mov32	r1, 0x8EC00000
+	str	r1, [r4, #PMC_IO_DPD_REQ]
+pmc_io_dpd_skip:
+
+	dsb
+
+	ret	lr
+
+	.ltorg
+/* dummy symbol for end of IRAM */
+	.align L1_CACHE_SHIFT
+	.global tegra30_iram_end
+tegra30_iram_end:
+	b	.
+#endif
diff --git a/arch/arm/mach-tegra/sleep.S b/arch/arm/mach-tegra/sleep.S
new file mode 100644
index 000000000..945f2c147
--- /dev/null
+++ b/arch/arm/mach-tegra/sleep.S
@@ -0,0 +1,152 @@
+/* SPDX-License-Identifier: GPL-2.0-or-later */
+/*
+ * arch/arm/mach-tegra/sleep.S
+ *
+ * Copyright (c) 2010-2011, NVIDIA Corporation.
+ * Copyright (c) 2011, Google, Inc.
+ *
+ * Author: Colin Cross <ccross@android.com>
+ *         Gary King <gking@nvidia.com>
+ */
+
+#include <linux/linkage.h>
+
+#include <asm/assembler.h>
+#include <asm/cache.h>
+#include <asm/cp15.h>
+#include <asm/hardware/cache-l2x0.h>
+
+#include "iomap.h"
+#include "sleep.h"
+
+#define CLK_RESET_CCLK_BURST	0x20
+#define CLK_RESET_CCLK_DIVIDER  0x24
+
+.arch armv7-a
+
+#if defined(CONFIG_HOTPLUG_CPU) || defined(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
+	tst	r2, #CR_C			@ see tegra_sleep_cpu()
+	bic	r2, r2, #CR_C
+	mcrne	p15, 0, r2, c1, c0, 0
+	isb
+
+	/* Flush the D-cache */
+	cmp	r0, #TEGRA_FLUSH_CACHE_ALL
+	blne	v7_flush_dcache_louis
+	bleq	v7_flush_dcache_all
+
+	/* Trun off coherency */
+	exit_smp r4, r5
+
+	ldmfd	sp!, {r0, r4-r5, r7, r9-r11, pc}
+ENDPROC(tegra_disable_clean_inv_dcache)
+#endif
+
+#ifdef CONFIG_PM_SLEEP
+/*
+ * tegra_init_l2_for_a15
+ *
+ * set up the correct L2 cache data RAM latency
+ */
+ENTRY(tegra_init_l2_for_a15)
+	mrc	p15, 0, r0, c0, c0, 5
+	ubfx	r0, r0, #8, #4
+	tst	r0, #1				@ only need for cluster 0
+	bne	_exit_init_l2_a15
+
+	mrc	p15, 0x1, r0, c9, c0, 2
+	and	r0, r0, #7
+	cmp	r0, #2
+	bicne	r0, r0, #7
+	orrne	r0, r0, #2
+	mcrne	p15, 0x1, r0, c9, c0, 2
+_exit_init_l2_a15:
+
+	ret	lr
+ENDPROC(tegra_init_l2_for_a15)
+
+/*
+ * tegra_sleep_cpu_finish(unsigned long v2p)
+ *
+ * enters suspend in LP2 by turning off the mmu and jumping to
+ * tegra?_tear_down_cpu
+ */
+ENTRY(tegra_sleep_cpu_finish)
+	mov	r4, r0
+	/* Flush and disable the L1 data cache */
+	mov	r0, #TEGRA_FLUSH_CACHE_ALL
+	bl	tegra_disable_clean_inv_dcache
+
+	mov	r0, r4
+	mov32	r6, tegra_tear_down_cpu
+	ldr	r1, [r6]
+	add	r1, r1, r0
+
+	mov32	r3, tegra_shut_off_mmu
+	add	r3, r3, r0
+	mov	r0, r1
+
+	ret	r3
+ENDPROC(tegra_sleep_cpu_finish)
+
+/*
+ * tegra_shut_off_mmu
+ *
+ * r0 = physical address to jump to with mmu off
+ *
+ * called with VA=PA mapping
+ * turns off MMU, icache, dcache and branch prediction
+ */
+	.align	L1_CACHE_SHIFT
+	.pushsection	.idmap.text, "ax"
+ENTRY(tegra_shut_off_mmu)
+	mrc	p15, 0, r3, c1, c0, 0
+	movw	r2, #CR_I | CR_Z | CR_C | CR_M
+	bic	r3, r3, r2
+	dsb
+	mcr	p15, 0, r3, c1, c0, 0
+	isb
+#ifdef CONFIG_CACHE_L2X0
+	/* Disable L2 cache */
+	check_cpu_part_num 0xc09, r9, r10
+	retne	r0
+
+	mov32	r2, TEGRA_ARM_PERIF_BASE + 0x3000
+	ldr	r3, [r2, #L2X0_CTRL]
+	tst	r3, #L2X0_CTRL_EN		@ see tegra_sleep_cpu()
+	mov	r3, #0
+	strne	r3, [r2, #L2X0_CTRL]
+#endif
+	ret	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]
+	ret	lr
+ENDPROC(tegra_switch_cpu_to_pllp)
+#endif
diff --git a/arch/arm/mach-tegra/sleep.h b/arch/arm/mach-tegra/sleep.h
new file mode 100644
index 000000000..e332d261c
--- /dev/null
+++ b/arch/arm/mach-tegra/sleep.h
@@ -0,0 +1,128 @@
+/* SPDX-License-Identifier: GPL-2.0-only */
+/*
+ * Copyright (c) 2010-2013, NVIDIA Corporation. All rights reserved.
+ */
+
+#ifndef __MACH_TEGRA_SLEEP_H
+#define __MACH_TEGRA_SLEEP_H
+
+#include "iomap.h"
+#include "irammap.h"
+
+#define TEGRA_ARM_PERIF_VIRT (TEGRA_ARM_PERIF_BASE - IO_CPU_PHYS \
+					+ IO_CPU_VIRT)
+#define TEGRA_FLOW_CTRL_VIRT (TEGRA_FLOW_CTRL_BASE - IO_PPSB_PHYS \
+					+ 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)
+
+#define TEGRA_IRAM_RESET_BASE_VIRT (IO_IRAM_VIRT + \
+				TEGRA_IRAM_RESET_HANDLER_OFFSET)
+
+/* 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
+
+/* flag of tegra_disable_clean_inv_dcache to do LoUIS or all */
+#define TEGRA_FLUSH_CACHE_LOUIS	0
+#define TEGRA_FLUSH_CACHE_ALL	1
+
+#ifdef __ASSEMBLER__
+/* waits until the microsecond counter (base) is > rn */
+.macro wait_until, rn, base, tmp
+	add	\rn, \rn, #1
+1001:	ldr	\tmp, [\base]
+	cmp	\tmp, \rn
+	bmi	1001b
+.endm
+
+/* returns the offset of the flow controller halt register for a cpu */
+.macro cpu_to_halt_reg rd, rcpu
+	cmp	\rcpu, #0
+	subne	\rd, \rcpu, #1
+	movne	\rd, \rd, lsl #3
+	addne	\rd, \rd, #0x14
+	moveq	\rd, #0
+.endm
+
+/* returns the offset of the flow controller csr register for a cpu */
+.macro cpu_to_csr_reg rd, rcpu
+	cmp	\rcpu, #0
+	subne	\rd, \rcpu, #1
+	movne	\rd, \rd, lsl #3
+	addne	\rd, \rd, #0x18
+	moveq	\rd, #8
+.endm
+
+/* returns the ID of the current processor */
+.macro cpu_id, rd
+	mrc	p15, 0, \rd, c0, c0, 5
+	and	\rd, \rd, #0xF
+.endm
+
+/* loads a 32-bit value into a register without a data access */
+.macro mov32, reg, val
+	movw	\reg, #:lower16:\val
+	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
+#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
+
+#else
+void tegra_resume(void);
+int tegra_sleep_cpu_finish(unsigned long);
+void tegra_disable_clean_inv_dcache(u32 flag);
+
+void tegra20_hotplug_shutdown(void);
+void tegra30_hotplug_shutdown(void);
+
+void tegra20_tear_down_cpu(void);
+void tegra30_tear_down_cpu(void);
+
+#endif
+#endif
diff --git a/arch/arm/mach-tegra/tegra.c b/arch/arm/mach-tegra/tegra.c
new file mode 100644
index 000000000..f324a7e49
--- /dev/null
+++ b/arch/arm/mach-tegra/tegra.c
@@ -0,0 +1,116 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * NVIDIA Tegra SoC device tree board support
+ *
+ * Copyright (C) 2011, 2013, NVIDIA Corporation
+ * Copyright (C) 2010 Secret Lab Technologies, Ltd.
+ * Copyright (C) 2010 Google, Inc.
+ */
+
+#include <linux/clk.h>
+#include <linux/clk/tegra.h>
+#include <linux/dma-mapping.h>
+#include <linux/init.h>
+#include <linux/io.h>
+#include <linux/irqchip.h>
+#include <linux/irqdomain.h>
+#include <linux/kernel.h>
+#include <linux/of_address.h>
+#include <linux/of_fdt.h>
+#include <linux/of.h>
+#include <linux/of_platform.h>
+#include <linux/platform_device.h>
+#include <linux/serial_8250.h>
+#include <linux/slab.h>
+#include <linux/sys_soc.h>
+#include <linux/usb/tegra_usb_phy.h>
+
+#include <linux/firmware/trusted_foundations.h>
+
+#include <soc/tegra/fuse.h>
+#include <soc/tegra/pmc.h>
+
+#include <asm/firmware.h>
+#include <asm/hardware/cache-l2x0.h>
+#include <asm/mach/arch.h>
+#include <asm/mach/time.h>
+#include <asm/mach-types.h>
+#include <asm/psci.h>
+#include <asm/setup.h>
+
+#include "board.h"
+#include "common.h"
+#include "iomap.h"
+#include "pm.h"
+#include "reset.h"
+#include "sleep.h"
+
+/*
+ * Storage for debug-macro.S's state.
+ *
+ * This must be in .data not .bss so that it gets initialized each time the
+ * kernel is loaded. The data is declared here rather than debug-macro.S so
+ * that multiple inclusions of debug-macro.S point at the same data.
+ */
+u32 tegra_uart_config[3] = {
+	/* Debug UART initialization required */
+	1,
+	/* Debug UART physical address */
+	0,
+	/* Debug UART virtual address */
+	0,
+};
+
+static void __init tegra_init_early(void)
+{
+	of_register_trusted_foundations();
+	tegra_cpu_reset_handler_init();
+	call_firmware_op(l2x0_init);
+}
+
+static void __init tegra_dt_init_irq(void)
+{
+	tegra_init_irq();
+	irqchip_init();
+}
+
+static void __init tegra_dt_init(void)
+{
+	struct device *parent = tegra_soc_device_register();
+
+	of_platform_default_populate(NULL, NULL, parent);
+}
+
+static void __init tegra_dt_init_late(void)
+{
+	if (IS_ENABLED(CONFIG_ARCH_TEGRA_2x_SOC) &&
+	    of_machine_is_compatible("nvidia,tegra20"))
+		platform_device_register_simple("tegra20-cpufreq", -1, NULL, 0);
+
+	if (IS_ENABLED(CONFIG_ARM_TEGRA_CPUIDLE) && !psci_smp_available())
+		platform_device_register_simple("tegra-cpuidle", -1, NULL, 0);
+
+	if (IS_ENABLED(CONFIG_ARCH_TEGRA_3x_SOC) &&
+	    of_machine_is_compatible("nvidia,tegra30"))
+		platform_device_register_simple("tegra20-cpufreq", -1, NULL, 0);
+}
+
+static const char * const tegra_dt_board_compat[] = {
+	"nvidia,tegra124",
+	"nvidia,tegra114",
+	"nvidia,tegra30",
+	"nvidia,tegra20",
+	NULL
+};
+
+DT_MACHINE_START(TEGRA_DT, "NVIDIA Tegra SoC (Flattened Device Tree)")
+	.l2c_aux_val	= 0x3c400000,
+	.l2c_aux_mask	= 0xc20fc3ff,
+	.smp		= smp_ops(tegra_smp_ops),
+	.map_io		= tegra_map_common_io,
+	.init_early	= tegra_init_early,
+	.init_irq	= tegra_dt_init_irq,
+	.init_machine	= tegra_dt_init,
+	.init_late	= tegra_dt_init_late,
+	.dt_compat	= tegra_dt_board_compat,
+MACHINE_END