arm: tegra: optimize L2 enable/disable paths for secureos

For the CONFIG_TRUSTED_FOUNDATION code paths, differentiate L2
enable vs. reenable, which are different SMCs (won't trigger an
invalidate in the case of a reenable).

On an L2 disable SMC, optionally pass a 0 for the L2 ways arg,
which skips the full clean/invalidate (and simply just disabled
the L2).

In order to safely skip flushing the L2 on the disable, we have
to be careful what we dirty from the type we flush the L1 and
disable the L2.

Bug 939415
Signed-off-by: Chris Johnson<cwj@nvidia.com>

Change-Id: I756d2ceda83d5d8d6bc5670218e9d874d5e5f62a
Reviewed-on: http://git-master/r/119786
Reviewed-by: Simone Willett <swillett@nvidia.com>
Tested-by: Simone Willett <swillett@nvidia.com>

5 files changed, 144 insertions, 91 deletions

diff --git a/arch/arm/mach-tegra/common.c b/arch/arm/mach-tegra/common.c
index b914d5d71462..85eb11f64118 100644
--- a/arch/arm/mach-tegra/common.c
+++ b/arch/arm/mach-tegra/common.c
@@ -2,7 +2,7 @@
  * arch/arm/mach-tegra/common.c
  *
  * Copyright (C) 2010 Google, Inc.
- * Copyright (C) 2010-2012 NVIDIA Corporation
+ * Copyright (C) 2010-2012, NVIDIA Corporation. All rights reserved.
  *
  * Author:
  *	Colin Cross <ccross@android.com>
@@ -226,61 +226,13 @@ static __initdata struct tegra_clk_init_table common_clk_init_table[] = {
 	{ NULL,		NULL,		0,		0},
 };
 
-#ifdef CONFIG_CACHE_L2X0
 #ifdef CONFIG_TRUSTED_FOUNDATIONS
-static void tegra_cache_smc(bool enable, u32 arg)
-{
-	void __iomem *p = IO_ADDRESS(TEGRA_ARM_PERIF_BASE) + 0x3000;
-	bool need_affinity_switch;
-	bool can_switch_affinity;
-	bool l2x0_enabled;
-	cpumask_t local_cpu_mask;
-	cpumask_t saved_cpu_mask;
-	unsigned long flags;
-	long ret;
-
-	/*
-	 * ISSUE : Some registers of PL310 controler must be written
-	 *              from Secure context (and from CPU0)!
-	 *
-	 * When called form Normal we obtain an abort or do nothing.
-	 * Instructions that must be called in Secure:
-	 *      - Write to Control register (L2X0_CTRL==0x100)
-	 *      - Write in Auxiliary controler (L2X0_AUX_CTRL==0x104)
-	 *      - Invalidate all entries (L2X0_INV_WAY==0x77C),
-	 *              mandatory at boot time.
-	 *      - Tag and Data RAM Latency Control Registers
-	 *              (0x108 & 0x10C) must be written in Secure.
-	 */
-	need_affinity_switch = (smp_processor_id() != 0);
-	can_switch_affinity = !irqs_disabled();
-
-	WARN_ON(need_affinity_switch && !can_switch_affinity);
-	if (need_affinity_switch && can_switch_affinity) {
-		cpu_set(0, local_cpu_mask);
-		sched_getaffinity(0, &saved_cpu_mask);
-		ret = sched_setaffinity(0, &local_cpu_mask);
-		WARN_ON(ret != 0);
-	}
-
-	local_irq_save(flags);
-	l2x0_enabled = readl_relaxed(p + L2X0_CTRL) & 1;
-	if (enable && !l2x0_enabled)
-		tegra_generic_smc(0xFFFFF100, 0x00000001, arg);
-	else if (!enable && l2x0_enabled)
-		tegra_generic_smc(0xFFFFF100, 0x00000002, arg);
-	local_irq_restore(flags);
-
-	if (need_affinity_switch && can_switch_affinity) {
-		ret = sched_setaffinity(0, &saved_cpu_mask);
-		WARN_ON(ret != 0);
-	}
-}
+#define CACHE_LINE_SIZE		32
 
-static void tegra_l2x0_disable(void)
+static inline void tegra_l2x0_disable_tz(void)
 {
-	unsigned long flags;
 	static u32 l2x0_way_mask;
+	BUG_ON(smp_processor_id() != 0);
 
 	if (!l2x0_way_mask) {
 		void __iomem *p = IO_ADDRESS(TEGRA_ARM_PERIF_BASE) + 0x3000;
@@ -291,30 +243,62 @@ static void tegra_l2x0_disable(void)
 		ways = (aux_ctrl & (1 << 16)) ? 16 : 8;
 		l2x0_way_mask = (1 << ways) - 1;
 	}
+#ifdef CONFIG_ARCH_TEGRA_2x_SOC
+	/* flush all ways on disable */
+	tegra_generic_smc_uncached(0xFFFFF100, 0x00000002, l2x0_way_mask);
+#elif defined(CONFIG_ARCH_TEGRA_3x_SOC)
+	if (tegra_is_cpu_in_lp2(0)) {
+		register unsigned long sp asm ("sp");
 
-	local_irq_save(flags);
-	tegra_cache_smc(false, l2x0_way_mask);
-	local_irq_restore(flags);
+		/* flush only the stack, if entering LP2 */
+		__cpuc_flush_dcache_area((void *)sp, (CACHE_LINE_SIZE * 2));
+		outer_flush_range(__pa(sp), __pa(sp) + (CACHE_LINE_SIZE * 2));
+
+		/* pass zero arg, so secureos flushes only its workspace */
+		tegra_generic_smc_uncached(0xFFFFF100, 0x00000002, 0x0);
+	} else {
+		/* flush all ways on disable, if entering LP0/LP1 */
+		tegra_generic_smc_uncached(0xFFFFF100,
+						0x00000002, l2x0_way_mask);
+	}
+#endif
 }
-#endif	/* CONFIG_TRUSTED_FOUNDATIONS  */
 
-void tegra_init_cache(bool init)
+static inline void tegra_init_cache_tz(bool init)
 {
 	void __iomem *p = IO_ADDRESS(TEGRA_ARM_PERIF_BASE) + 0x3000;
 	u32 aux_ctrl;
 
-#ifdef CONFIG_TRUSTED_FOUNDATIONS
-	/* issue the SMC to enable the L2 */
-	aux_ctrl = readl_relaxed(p + L2X0_AUX_CTRL);
-	tegra_cache_smc(true, aux_ctrl);
+	BUG_ON(smp_processor_id() != 0);
+
+	if (init) {
+		/* init L2 from secureos */
+		tegra_generic_smc(0xFFFFF100, 0x00000001, 0x0);
+
+		/* common init called for outer call hookup */
+		aux_ctrl = readl_relaxed(p + L2X0_AUX_CTRL);
+		l2x0_init(p, aux_ctrl, 0xFFFFFFFF);
 
-	/* after init, reread aux_ctrl and register handlers */
-	aux_ctrl = readl_relaxed(p + L2X0_AUX_CTRL);
-	l2x0_init(p, aux_ctrl, 0xFFFFFFFF);
+		/* use our outer_disable() routine */
+		outer_cache.disable = tegra_l2x0_disable_tz;
+	} else {
+		/* reenable L2 in secureos */
+		aux_ctrl = readl_relaxed(p + L2X0_AUX_CTRL);
+		tegra_generic_smc_uncached(0xFFFFF100, 0x00000004, aux_ctrl);
+	}
+}
+#endif	/* CONFIG_TRUSTED_FOUNDATIONS  */
 
-	/* override outer_disable() with our disable */
-	outer_cache.disable = tegra_l2x0_disable;
+#ifdef CONFIG_CACHE_L2X0
+void tegra_init_cache(bool init)
+{
+#ifdef CONFIG_TRUSTED_FOUNDATIONS
+	/* enable/re-enable of L2 handled by secureos */
+	return tegra_init_cache_tz(init);
 #else
+	void __iomem *p = IO_ADDRESS(TEGRA_ARM_PERIF_BASE) + 0x3000;
+	u32 aux_ctrl;
+
 #if defined(CONFIG_ARCH_TEGRA_2x_SOC)
 	writel_relaxed(0x331, p + L2X0_TAG_LATENCY_CTRL);
 	writel_relaxed(0x441, p + L2X0_DATA_LATENCY_CTRL);
@@ -364,7 +348,7 @@ void tegra_init_cache(bool init)
 	l2x0_enable();
 #endif
 }
-#endif
+#endif /* CONFIG_CACHE_L2X0 */
 
 static void __init tegra_init_power(void)
 {
diff --git a/arch/arm/mach-tegra/headsmp.S b/arch/arm/mach-tegra/headsmp.S
index 4e28a558cc38..63852b99b3d3 100644
--- a/arch/arm/mach-tegra/headsmp.S
+++ b/arch/arm/mach-tegra/headsmp.S
@@ -3,7 +3,7 @@
  *
  * CPU initialization routines for Tegra SoCs
  *
- * Copyright (c) 2009-2011, NVIDIA Corporation.
+ * Copyright (c) 2009-2012, NVIDIA Corporation. All rights reserved.
  * Copyright (c) 2011 Google, Inc.
  * Author: Colin Cross <ccross@android.com>
  *         Gary King <gking@nvidia.com>
@@ -90,8 +90,9 @@ ENTRY(tegra_resume)
 	str	r1, [r0]
 
 #ifdef CONFIG_TRUSTED_FOUNDATIONS
-	/* wake up (should have specified args?) */
-	bl	tegra_generic_smc
+	/* wake up */
+	mov	r0, #0x00000003
+	bl	tegra_generic_smc_local
 #endif
 
 	b	tegra_cpu_resume_phys
diff --git a/arch/arm/mach-tegra/pm.c b/arch/arm/mach-tegra/pm.c
index 9049e37ca05a..d251e57626aa 100644
--- a/arch/arm/mach-tegra/pm.c
+++ b/arch/arm/mach-tegra/pm.c
@@ -3,7 +3,7 @@
  *
  * CPU complex suspend & resume functions for Tegra SoCs
  *
- * Copyright (c) 2009-2012, NVIDIA Corporation.
+ * Copyright (c) 2009-2012, NVIDIA Corporation. All rights reserved.
  *
  * 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
@@ -546,17 +546,27 @@ bool tegra_set_cpu_in_lp2(int cpu)
 	return last_cpu;
 }
 
+bool tegra_is_cpu_in_lp2(int cpu)
+{
+	bool in_lp2;
+
+	spin_lock(&tegra_lp2_lock);
+	in_lp2 = cpumask_test_cpu(cpu, &tegra_in_lp2);
+	spin_unlock(&tegra_lp2_lock);
+	return in_lp2;
+}
+
 static void tegra_sleep_core(enum tegra_suspend_mode mode,
 			     unsigned long v2p)
 {
 #ifdef CONFIG_TRUSTED_FOUNDATIONS
 	if (mode == TEGRA_SUSPEND_LP0) {
-		tegra_generic_smc(0xFFFFFFFC, 0xFFFFFFE3,
-				  virt_to_phys(tegra_resume));
+		tegra_generic_smc_uncached(0xFFFFFFFC, 0xFFFFFFE3,
+					   virt_to_phys(tegra_resume));
 	} else {
-		tegra_generic_smc(0xFFFFFFFC, 0xFFFFFFE6,
-				  (TEGRA_RESET_HANDLER_BASE +
-				   tegra_cpu_reset_handler_offset));
+		tegra_generic_smc_uncached(0xFFFFFFFC, 0xFFFFFFE6,
+					   (TEGRA_RESET_HANDLER_BASE +
+					    tegra_cpu_reset_handler_offset));
 	}
 #endif
 #ifdef CONFIG_ARCH_TEGRA_2x_SOC
@@ -569,9 +579,9 @@ static void tegra_sleep_core(enum tegra_suspend_mode mode,
 static inline void tegra_sleep_cpu(unsigned long v2p)
 {
 #ifdef CONFIG_TRUSTED_FOUNDATIONS
-	tegra_generic_smc(0xFFFFFFFC, 0xFFFFFFE4,
-			  (TEGRA_RESET_HANDLER_BASE +
-			   tegra_cpu_reset_handler_offset));
+	tegra_generic_smc_uncached(0xFFFFFFFC, 0xFFFFFFE4,
+				   (TEGRA_RESET_HANDLER_BASE +
+				    tegra_cpu_reset_handler_offset));
 #endif
 	tegra_sleep_cpu_save(v2p);
 }
diff --git a/arch/arm/mach-tegra/pm.h b/arch/arm/mach-tegra/pm.h
index 498170648819..b78e9b1abc00 100644
--- a/arch/arm/mach-tegra/pm.h
+++ b/arch/arm/mach-tegra/pm.h
@@ -75,6 +75,7 @@ unsigned long tegra_cpu_power_off_time(void);
 unsigned long tegra_cpu_lp2_min_residency(void);
 void tegra_clear_cpu_in_lp2(int cpu);
 bool tegra_set_cpu_in_lp2(int cpu);
+bool tegra_is_cpu_in_lp2(int cpu);
 
 int tegra_suspend_dram(enum tegra_suspend_mode mode, unsigned int flags);
 
@@ -222,6 +223,8 @@ extern bool tegra_all_cpus_booted __read_mostly;
 
 #ifdef CONFIG_TRUSTED_FOUNDATIONS
 void tegra_generic_smc(u32 type, u32 subtype, u32 arg);
+void tegra_generic_smc_local(u32 type, u32 subtype, u32 arg);
+void tegra_generic_smc_uncached(u32 type, u32 subtype, u32 arg);
 #endif
 
 /* The debug channel uart base physical address */
diff --git a/arch/arm/mach-tegra/sleep.S b/arch/arm/mach-tegra/sleep.S
index 973c8677bafe..e86795c5c46a 100644
--- a/arch/arm/mach-tegra/sleep.S
+++ b/arch/arm/mach-tegra/sleep.S
@@ -1,7 +1,7 @@
 /*
  * arch/arm/mach-tegra/sleep.S
  *
- * Copyright (c) 2010-2011, NVIDIA Corporation.
+ * Copyright (c) 2010-2012, NVIDIA Corporation. All rights reserved.
  * Copyright (c) 2011, Google, Inc.
  *
  * Author: Colin Cross <ccross@android.com>
@@ -440,28 +440,83 @@ ENDPROC(tegra_cpu_pllp)
 #endif
 
 #ifdef CONFIG_TRUSTED_FOUNDATIONS
+
 /*
- * tegra_generic_smc
+ * Confirm we're issuing this SMC from CPU0 (only one
+ * currently supported) and issue the instruction.
  *
  * r0 = smc type
  * r1 = smc subtype
  * r2 = argument passed to smc
- *
- * issues SMC (secure monitor call) instruction with
- * the specified parameters.
  */
-ENTRY(tegra_generic_smc)
-	adr	r3, __tegra_smc_stack
-	stmia	r3, {r4-r12, lr}
+.macro smc_issue_smc tmp
+	cpu_id	\tmp
+	cmp	\tmp, #0
+	bne	.
 	mov	r3, #0
 	mov	r4, #0
 	dsb
 	smc	#0
-	adr	r3, __tegra_smc_stack
-	ldmia	r3, {r4-r12, pc}
+.endm
+
+/*
+ * Issue SMC with ctx kept on an uncached stack
+ */
+ENTRY(tegra_generic_smc_uncached)
+#if defined(CONFIG_PM_SLEEP) && defined(CONFIG_CACHE_L2X0)
+	mov32	r3, tegra_cpu_context		@ borrow CPU0's non-cached
+	ldr	r3, [r3]			@ context grows up
+	stmia	r3, {r4-r12, sp, lr}
+
+	smc_issue_smc r5
+
+	mov32	r3, tegra_cpu_context		@ borrow CPU0's non-cached
+	ldr	r3, [r3]			@ context grows up
+	ldmia	r3, {r4-r12, sp, pc}
+#else
+	mov	pc, lr
+#endif
+ENDPROC(tegra_generic_smc_uncached)
+
+/*
+ * Issue SMC with ctx kept on a cacheable stack
+ * 	(args in R0, R1, R2 and R3 holds save/restore ptr)
+ */
+ENTRY(tegra_generic_smc_cached)
+	stmia	r3, {r4-r12, sp, lr}
+	adr	r4, __tegra_smc_current_ctx	@ save current ptr
+	str	r3, [r4]
+
+	smc_issue_smc r5
+
+	adr	r4, __tegra_smc_current_ctx	@ restore from saved ptr
+	ldr	r3, [r4]
+	ldmia	r3, {r4-r12, sp, pc}
+ENDPROC(tegra_generic_smc_cached)
+	.type	__tegra_smc_current_ctx, %object
+__tegra_smc_current_ctx:
+	.long   0
+	.size   __tegra_smc_current_ctx, . - __tegra_smc_current_ctx
+
+#define TEGRA_SMC_SAVED_WORDS	11
+
+/* SMC issued using the current cacheable SP stack */
+ENTRY(tegra_generic_smc)
+	mov	r3, sp					@ use current stack
+	sub	r3, #(TEGRA_SMC_SAVED_WORDS << 2)	@ context grows up
+	b	tegra_generic_smc_cached
 ENDPROC(tegra_generic_smc)
-	.type   __tegra_smc_stack, %object
+
+/* SMC issued using a local cacheable stack */
+ENTRY(tegra_generic_smc_local)
+	adr	r3, __tegra_smc_stack			@ use local stack
+	b	tegra_generic_smc_cached
+ENDPROC(tegra_generic_smc_local)
+	.align	L1_CACHE_SHIFT
+	.type	__tegra_smc_stack, %object
 __tegra_smc_stack:
-	.long   0, 0, 0, 0, 0, 0, 0, 0, 0, 0
+	.rept	TEGRA_SMC_SAVED_WORDS
+	.long   0
+	.endr
 	.size   __tegra_smc_stack, . - __tegra_smc_stack
 #endif