Merge tag 'tegra-for-5.2-firmware' of git://git.kernel.org/pub/scm/linux/kernel/git/tegra/linux into arm/soc

firmware: tegra: Changes for v5.2-rc1

This set of changes includes improvements for Trusted Foundations and
also moves the source files for this support into the standard location
under drivers/firmware.

* tag 'tegra-for-5.2-firmware' of git://git.kernel.org/pub/scm/linux/kernel/git/tegra/linux:
  firmware: Move Trusted Foundations support
  ARM: tegra: Sort dependencies alphabetically
  ARM: tegra: Add firmware calls required for suspend-resume on Tegra30
  ARM: tegra: Always boot CPU in ARM-mode
  ARM: tegra: Don't apply CPU erratas in insecure mode
  ARM: tegra: Set up L2 cache using Trusted Foundations firmware
  ARM: trusted_foundations: Provide information about whether firmware is registered
  ARM: trusted_foundations: Make prepare_idle call to take mode argument
  ARM: trusted_foundations: Support L2 cache maintenance

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

16 files changed, 122 insertions, 235 deletions

diff --git a/arch/arm/Kconfig b/arch/arm/Kconfig
index 850b4805e2d1..61d949e9233f 100644
--- a/arch/arm/Kconfig
+++ b/arch/arm/Kconfig
@@ -900,8 +900,6 @@ config PLAT_PXA
 config PLAT_VERSATILE
 	bool
 
-source "arch/arm/firmware/Kconfig"
-
 source "arch/arm/mm/Kconfig"
 
 config IWMMXT
diff --git a/arch/arm/Makefile b/arch/arm/Makefile
index 807a7d06c2a0..05ecc004de86 100644
--- a/arch/arm/Makefile
+++ b/arch/arm/Makefile
@@ -290,7 +290,6 @@ core-y				+= arch/arm/kernel/ arch/arm/mm/ arch/arm/common/
 core-y				+= arch/arm/probes/
 core-y				+= arch/arm/net/
 core-y				+= arch/arm/crypto/
-core-y				+= arch/arm/firmware/
 core-y				+= $(machdirs) $(platdirs)
 
 drivers-$(CONFIG_OPROFILE)      += arch/arm/oprofile/
diff --git a/arch/arm/firmware/Kconfig b/arch/arm/firmware/Kconfig
deleted file mode 100644
index ad396af68e47..000000000000
--- a/arch/arm/firmware/Kconfig
+++ /dev/null
@@ -1,29 +0,0 @@
-config ARCH_SUPPORTS_FIRMWARE
-	bool
-
-config ARCH_SUPPORTS_TRUSTED_FOUNDATIONS
-	bool
-	select ARCH_SUPPORTS_FIRMWARE
-
-menu "Firmware options"
-	depends on ARCH_SUPPORTS_FIRMWARE
-
-config TRUSTED_FOUNDATIONS
-	bool "Trusted Foundations secure monitor support"
-	depends on ARCH_SUPPORTS_TRUSTED_FOUNDATIONS
-	default y
-	help
-	  Some devices (including most Tegra-based consumer devices on the
-	  market) are booted with the Trusted Foundations secure monitor
-	  active, requiring some core operations to be performed by the secure
-	  monitor instead of the kernel.
-
-	  This option allows the kernel to invoke the secure monitor whenever
-	  required on devices using Trusted Foundations. See
-	  arch/arm/include/asm/trusted_foundations.h or the
-	  tlm,trusted-foundations device tree binding documentation for details
-	  on how to use it.
-
-	  Say n if you don't know what this is about.
-
-endmenu
diff --git a/arch/arm/firmware/Makefile b/arch/arm/firmware/Makefile
deleted file mode 100644
index 6e41336b0bc4..000000000000
--- a/arch/arm/firmware/Makefile
+++ /dev/null
@@ -1,4 +0,0 @@
-obj-$(CONFIG_TRUSTED_FOUNDATIONS)	+= trusted_foundations.o
-
-# tf_generic_smc() fails to build with -fsanitize-coverage=trace-pc
-KCOV_INSTRUMENT                := n
diff --git a/arch/arm/firmware/trusted_foundations.c b/arch/arm/firmware/trusted_foundations.c
deleted file mode 100644
index 689e6565abfc..000000000000
--- a/arch/arm/firmware/trusted_foundations.c
+++ /dev/null
@@ -1,103 +0,0 @@
-/*
- * Trusted Foundations support for ARM CPUs
- *
- * Copyright (c) 2013, NVIDIA Corporation.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful, but WITHOUT
- * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
- * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
- * more details.
- */
-
-#include <linux/kernel.h>
-#include <linux/init.h>
-#include <linux/of.h>
-#include <asm/firmware.h>
-#include <asm/trusted_foundations.h>
-
-#define TF_SET_CPU_BOOT_ADDR_SMC 0xfffff200
-
-#define TF_CPU_PM		0xfffffffc
-#define TF_CPU_PM_S3		0xffffffe3
-#define TF_CPU_PM_S2		0xffffffe6
-#define TF_CPU_PM_S2_NO_MC_CLK	0xffffffe5
-#define TF_CPU_PM_S1		0xffffffe4
-#define TF_CPU_PM_S1_NOFLUSH_L2	0xffffffe7
-
-static unsigned long cpu_boot_addr;
-
-static void tf_generic_smc(u32 type, u32 arg1, u32 arg2)
-{
-	register u32 r0 asm("r0") = type;
-	register u32 r1 asm("r1") = arg1;
-	register u32 r2 asm("r2") = arg2;
-
-	asm volatile(
-		".arch_extension	sec\n\t"
-		"stmfd	sp!, {r4 - r11}\n\t"
-		__asmeq("%0", "r0")
-		__asmeq("%1", "r1")
-		__asmeq("%2", "r2")
-		"mov	r3, #0\n\t"
-		"mov	r4, #0\n\t"
-		"smc	#0\n\t"
-		"ldmfd	sp!, {r4 - r11}\n\t"
-		:
-		: "r" (r0), "r" (r1), "r" (r2)
-		: "memory", "r3", "r12", "lr");
-}
-
-static int tf_set_cpu_boot_addr(int cpu, unsigned long boot_addr)
-{
-	cpu_boot_addr = boot_addr;
-	tf_generic_smc(TF_SET_CPU_BOOT_ADDR_SMC, cpu_boot_addr, 0);
-
-	return 0;
-}
-
-static int tf_prepare_idle(void)
-{
-	tf_generic_smc(TF_CPU_PM, TF_CPU_PM_S1_NOFLUSH_L2, cpu_boot_addr);
-
-	return 0;
-}
-
-static const struct firmware_ops trusted_foundations_ops = {
-	.set_cpu_boot_addr = tf_set_cpu_boot_addr,
-	.prepare_idle = tf_prepare_idle,
-};
-
-void register_trusted_foundations(struct trusted_foundations_platform_data *pd)
-{
-	/*
-	 * we are not using version information for now since currently
-	 * supported SMCs are compatible with all TF releases
-	 */
-	register_firmware_ops(&trusted_foundations_ops);
-}
-
-void of_register_trusted_foundations(void)
-{
-	struct device_node *node;
-	struct trusted_foundations_platform_data pdata;
-	int err;
-
-	node = of_find_compatible_node(NULL, NULL, "tlm,trusted-foundations");
-	if (!node)
-		return;
-
-	err = of_property_read_u32(node, "tlm,version-major",
-				   &pdata.version_major);
-	if (err != 0)
-		panic("Trusted Foundation: missing version-major property\n");
-	err = of_property_read_u32(node, "tlm,version-minor",
-				   &pdata.version_minor);
-	if (err != 0)
-		panic("Trusted Foundation: missing version-minor property\n");
-	register_trusted_foundations(&pdata);
-}
diff --git a/arch/arm/include/asm/firmware.h b/arch/arm/include/asm/firmware.h
index 34c1d96ef46d..6698272bbcbf 100644
--- a/arch/arm/include/asm/firmware.h
+++ b/arch/arm/include/asm/firmware.h
@@ -24,7 +24,7 @@ struct firmware_ops {
 	/*
 	 * Inform the firmware we intend to enter CPU idle mode
 	 */
-	int (*prepare_idle)(void);
+	int (*prepare_idle)(unsigned long mode);
 	/*
 	 * Enters CPU idle mode
 	 */
diff --git a/arch/arm/include/asm/trusted_foundations.h b/arch/arm/include/asm/trusted_foundations.h
deleted file mode 100644
index 00748350cf72..000000000000
--- a/arch/arm/include/asm/trusted_foundations.h
+++ /dev/null
@@ -1,73 +0,0 @@
-/*
- * Copyright (c) 2013, NVIDIA Corporation.
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful, but WITHOUT
- * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
- * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
- * more details.
- */
-
-/*
- * Support for the Trusted Foundations secure monitor.
- *
- * Trusted Foundation comes active on some ARM consumer devices (most
- * Tegra-based devices sold on the market are concerned). Such devices can only
- * perform some basic operations, like setting the CPU reset vector, through
- * SMC calls to the secure monitor. The calls are completely specific to
- * Trusted Foundations, and do *not* follow the SMC calling convention or the
- * PSCI standard.
- */
-
-#ifndef __ASM_ARM_TRUSTED_FOUNDATIONS_H
-#define __ASM_ARM_TRUSTED_FOUNDATIONS_H
-
-#include <linux/printk.h>
-#include <linux/bug.h>
-#include <linux/of.h>
-#include <linux/cpu.h>
-#include <linux/smp.h>
-
-struct trusted_foundations_platform_data {
-	unsigned int version_major;
-	unsigned int version_minor;
-};
-
-#if IS_ENABLED(CONFIG_TRUSTED_FOUNDATIONS)
-
-void register_trusted_foundations(struct trusted_foundations_platform_data *pd);
-void of_register_trusted_foundations(void);
-
-#else /* CONFIG_TRUSTED_FOUNDATIONS */
-
-static inline void register_trusted_foundations(
-				   struct trusted_foundations_platform_data *pd)
-{
-	/*
-	 * If the system requires TF and we cannot provide it, continue booting
-	 * but disable features that cannot be provided.
-	 */
-	pr_err("No support for Trusted Foundations, continuing in degraded mode.\n");
-	pr_err("Secondary processors as well as CPU PM will be disabled.\n");
-#if IS_ENABLED(CONFIG_SMP)
-	setup_max_cpus = 0;
-#endif
-	cpu_idle_poll_ctrl(true);
-}
-
-static inline void of_register_trusted_foundations(void)
-{
-	/*
-	 * If we find the target should enable TF but does not support it,
-	 * fail as the system won't be able to do much anyway
-	 */
-	if (of_find_compatible_node(NULL, NULL, "tlm,trusted-foundations"))
-		register_trusted_foundations(NULL);
-}
-#endif /* CONFIG_TRUSTED_FOUNDATIONS */
-
-#endif
diff --git a/arch/arm/mach-tegra/Kconfig b/arch/arm/mach-tegra/Kconfig
index 7f3b83e0d324..63e89e75639b 100644
--- a/arch/arm/mach-tegra/Kconfig
+++ b/arch/arm/mach-tegra/Kconfig
@@ -2,7 +2,7 @@
 menuconfig ARCH_TEGRA
 	bool "NVIDIA Tegra"
 	depends on ARCH_MULTI_V7
-	select ARCH_SUPPORTS_TRUSTED_FOUNDATIONS
+	select ARCH_HAS_RESET_CONTROLLER
 	select ARM_AMBA
 	select ARM_GIC
 	select CLKSRC_MMIO
@@ -11,7 +11,6 @@ menuconfig ARCH_TEGRA
 	select HAVE_ARM_TWD if SMP
 	select PINCTRL
 	select PM_OPP
-	select ARCH_HAS_RESET_CONTROLLER
 	select RESET_CONTROLLER
 	select SOC_BUS
 	select ZONE_DMA if ARM_LPAE
diff --git a/arch/arm/mach-tegra/cpuidle-tegra114.c b/arch/arm/mach-tegra/cpuidle-tegra114.c
index e3fbcfedf845..43c695d83f03 100644
--- a/arch/arm/mach-tegra/cpuidle-tegra114.c
+++ b/arch/arm/mach-tegra/cpuidle-tegra114.c
@@ -21,6 +21,8 @@
 #include <linux/kernel.h>
 #include <linux/module.h>
 
+#include <linux/firmware/trusted_foundations.h>
+
 #include <asm/cpuidle.h>
 #include <asm/smp_plat.h>
 #include <asm/suspend.h>
@@ -46,7 +48,7 @@ static int tegra114_idle_power_down(struct cpuidle_device *dev,
 	tegra_set_cpu_in_lp2();
 	cpu_pm_enter();
 
-	call_firmware_op(prepare_idle);
+	call_firmware_op(prepare_idle, TF_PM_MODE_LP2_NOFLUSH_L2);
 
 	/* Do suspend by ourselves if the firmware does not implement it */
 	if (call_firmware_op(do_idle, 0) == -ENOSYS)
diff --git a/arch/arm/mach-tegra/pm.c b/arch/arm/mach-tegra/pm.c
index 1ad5719779b0..1b0ade06f204 100644
--- a/arch/arm/mach-tegra/pm.c
+++ b/arch/arm/mach-tegra/pm.c
@@ -27,12 +27,15 @@
 #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>
@@ -159,6 +162,28 @@ int tegra_cpu_do_idle(void)
 
 static int tegra_sleep_cpu(unsigned long v2p)
 {
+	/*
+	 * 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.
+	 */
+	if (trusted_foundations_registered())
+		outer_disable();
+
+	/*
+	 * 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);
 
@@ -197,6 +222,14 @@ void tegra_idle_lp2_last(void)
 
 	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();
 }
@@ -215,6 +248,15 @@ enum tegra_suspend_mode tegra_pm_validate_suspend_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);
 
@@ -342,6 +384,14 @@ static int tegra_suspend_enter(suspend_state_t state)
 
 	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();
diff --git a/arch/arm/mach-tegra/reset-handler.S b/arch/arm/mach-tegra/reset-handler.S
index e22ccf87eded..cd94d7c41fc0 100644
--- a/arch/arm/mach-tegra/reset-handler.S
+++ b/arch/arm/mach-tegra/reset-handler.S
@@ -20,6 +20,7 @@
 #include <soc/tegra/flowctrl.h>
 #include <soc/tegra/fuse.h>
 
+#include <asm/assembler.h>
 #include <asm/asm-offsets.h>
 #include <asm/cache.h>
 
@@ -29,8 +30,6 @@
 
 #define PMC_SCRATCH41	0x140
 
-#define RESET_DATA(x)	((TEGRA_RESET_##x)*4)
-
 #ifdef CONFIG_PM_SLEEP
 /*
  *	tegra_resume
@@ -78,6 +77,7 @@ ENTRY(tegra_resume)
 	orr	r1, r1, #1
 	str	r1, [r0]
 #endif
+	bl	tegra_resume_trusted_foundations
 
 #ifdef CONFIG_CACHE_L2X0
 	/* L2 cache resume & re-enable */
@@ -90,6 +90,30 @@ end_ca9_scu_l2_resume:
 
 	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. */
+	smc	#0
+
+	b	cpu_resume
+ENDPROC(tegra_resume_trusted_foundations)
 #endif
 
 	.align L1_CACHE_SHIFT
@@ -115,12 +139,19 @@ ENTRY(__tegra_cpu_reset_handler_start)
  *       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
@@ -155,7 +186,6 @@ after_errata:
 	and	r10, r10, #0x3			@ R10 = CPU number
 	mov	r11, #1
 	mov	r11, r11, lsl r10  		@ R11 = CPU mask
-	adr	r12, __tegra_cpu_reset_handler_data
 
 #ifdef CONFIG_SMP
 	/* Does the OS know about this CPU? */
@@ -169,10 +199,9 @@ after_errata:
 	cmp	r6, #TEGRA20
 	bne	1f
 	/* If not CPU0, don't let CPU0 reset CPU1 now that CPU1 is coming up. */
-	mov32	r5, TEGRA_IRAM_BASE + TEGRA_IRAM_RESET_HANDLER_OFFSET
 	mov	r0, #CPU_NOT_RESETTABLE
 	cmp	r10, #0
-	strbne	r0, [r5, #__tegra20_cpu1_resettable_status_offset]
+	strbne	r0, [r12, #RESET_DATA(RESETTABLE_STATUS)]
 1:
 #endif
 
@@ -277,14 +306,13 @@ 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
+	.rept   TEGRA_RESET_DATA_SIZE
+	.long   0
 	.endr
-	.globl	__tegra20_cpu1_resettable_status_offset
-	.equ	__tegra20_cpu1_resettable_status_offset, \
-					. - __tegra_cpu_reset_handler_start
-	.byte	0
 	.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
index dc558892753c..35dc5d419b6f 100644
--- a/arch/arm/mach-tegra/reset.c
+++ b/arch/arm/mach-tegra/reset.c
@@ -19,6 +19,8 @@
 #include <linux/init.h>
 #include <linux/io.h>
 
+#include <linux/firmware/trusted_foundations.h>
+
 #include <soc/tegra/fuse.h>
 
 #include <asm/cacheflush.h>
@@ -89,6 +91,8 @@ static void __init tegra_cpu_reset_handler_enable(void)
 
 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] =
diff --git a/arch/arm/mach-tegra/reset.h b/arch/arm/mach-tegra/reset.h
index 9c479c7925b8..db0e6b3097ab 100644
--- a/arch/arm/mach-tegra/reset.h
+++ b/arch/arm/mach-tegra/reset.h
@@ -25,7 +25,11 @@
 #define TEGRA_RESET_STARTUP_SECONDARY	3
 #define TEGRA_RESET_STARTUP_LP2		4
 #define TEGRA_RESET_STARTUP_LP1		5
-#define TEGRA_RESET_DATA_SIZE		6
+#define TEGRA_RESET_RESETTABLE_STATUS	6
+#define TEGRA_RESET_TF_PRESENT		7
+#define TEGRA_RESET_DATA_SIZE		8
+
+#define RESET_DATA(x)	((TEGRA_RESET_##x)*4)
 
 #ifndef __ASSEMBLY__
 
@@ -49,7 +53,8 @@ void __tegra_cpu_reset_handler_end(void);
 	 (u32)__tegra_cpu_reset_handler_start)))
 #define tegra20_cpu1_resettable_status \
 	(IO_ADDRESS(TEGRA_IRAM_BASE + TEGRA_IRAM_RESET_HANDLER_OFFSET + \
-	 (u32)__tegra20_cpu1_resettable_status_offset))
+	((u32)&__tegra_cpu_reset_handler_data[TEGRA_RESET_RESETTABLE_STATUS] - \
+	 (u32)__tegra_cpu_reset_handler_start)))
 #endif
 
 #define tegra_cpu_reset_handler_offset \
diff --git a/arch/arm/mach-tegra/sleep-tegra20.S b/arch/arm/mach-tegra/sleep-tegra20.S
index dedeebfccc55..50d51d3465f6 100644
--- a/arch/arm/mach-tegra/sleep-tegra20.S
+++ b/arch/arm/mach-tegra/sleep-tegra20.S
@@ -28,6 +28,7 @@
 #include <asm/cache.h>
 
 #include "irammap.h"
+#include "reset.h"
 #include "sleep.h"
 
 #define EMC_CFG				0xc
@@ -53,6 +54,9 @@
 #define APB_MISC_XM2CFGCPADCTRL2	0x8e4
 #define APB_MISC_XM2CFGDPADCTRL2	0x8e8
 
+#define __tegra20_cpu1_resettable_status_offset \
+	(__tegra_cpu_reset_handler_data_offset + RESET_DATA(RESETTABLE_STATUS))
+
 .macro pll_enable, rd, r_car_base, pll_base
 	ldr	\rd, [\r_car_base, #\pll_base]
 	tst	\rd, #(1 << 30)
diff --git a/arch/arm/mach-tegra/sleep.S b/arch/arm/mach-tegra/sleep.S
index 5e3496753df1..1735ded5a812 100644
--- a/arch/arm/mach-tegra/sleep.S
+++ b/arch/arm/mach-tegra/sleep.S
@@ -49,8 +49,9 @@ ENTRY(tegra_disable_clean_inv_dcache)
 
 	/* Disable the D-cache */
 	mrc	p15, 0, r2, c1, c0, 0
+	tst	r2, #CR_C			@ see tegra_sleep_cpu()
 	bic	r2, r2, #CR_C
-	mcr	p15, 0, r2, c1, c0, 0
+	mcrne	p15, 0, r2, c1, c0, 0
 	isb
 
 	/* Flush the D-cache */
@@ -132,10 +133,13 @@ ENTRY(tegra_shut_off_mmu)
 #ifdef CONFIG_CACHE_L2X0
 	/* Disable L2 cache */
 	check_cpu_part_num 0xc09, r9, r10
-	movweq	r2, #:lower16:(TEGRA_ARM_PERIF_BASE + 0x3000)
-	movteq	r2, #:upper16:(TEGRA_ARM_PERIF_BASE + 0x3000)
-	moveq	r3, #0
-	streq	r3, [r2, #L2X0_CTRL]
+	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)
diff --git a/arch/arm/mach-tegra/tegra.c b/arch/arm/mach-tegra/tegra.c
index f9587be48235..3e88f67dd521 100644
--- a/arch/arm/mach-tegra/tegra.c
+++ b/arch/arm/mach-tegra/tegra.c
@@ -35,15 +35,17 @@
 #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/setup.h>
-#include <asm/trusted_foundations.h>
 
 #include "board.h"
 #include "common.h"
@@ -74,6 +76,7 @@ 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)