Trusted Foundations kernel changes and driver

Change-Id: I318afbe66efa346b71e82413ac6442672cef4d36
Reviewed-on: http://git-master/r/21196
Reviewed-by: Jonathan B White (Engrg-Mobile) <jwhite@nvidia.com>
Tested-by: Jonathan B White (Engrg-Mobile) <jwhite@nvidia.com>
Reviewed-by: Maria Gutowski <mgutowski@nvidia.com>

7 files changed, 173 insertions, 10 deletions

diff --git a/arch/arm/mach-tegra/common.c b/arch/arm/mach-tegra/common.c
index 71b86b5238e2..4504624cc1fd 100644
--- a/arch/arm/mach-tegra/common.c
+++ b/arch/arm/mach-tegra/common.c
@@ -98,9 +98,21 @@ void __init tegra_init_cache(void)
 #ifdef CONFIG_CACHE_L2X0
 	void __iomem *p = IO_ADDRESS(TEGRA_ARM_PERIF_BASE) + 0x3000;
 
+#ifndef CONFIG_TRUSTED_FOUNDATIONS
+   /*
+   ISSUE : Some registers of PL310 controler must be called from Secure context!
+            When called form Normal we obtain an abort.
+            Instructions that must be called in Secure :
+               - Tag and Data RAM Latency Control Registers (0x108 & 0x10C) must be written in Secure.
+        
+   The following section of code has been regrouped in the implementation of "l2x0_init".
+   The "l2x0_init" will in fact call an SMC intruction to switch from Normal context to Secure context.
+   The configuration and activation will be done in Secure.
+   */
 	writel(0x331, p + L2X0_TAG_LATENCY_CTRL);
 	writel(0x441, p + L2X0_DATA_LATENCY_CTRL);
 	writel(2, p + L2X0_PWR_CTRL);
+#endif
 
 	l2x0_init(p, 0x6C480001, 0x8200c3fe);
 #endif
diff --git a/arch/arm/mach-tegra/cortex-a9.S b/arch/arm/mach-tegra/cortex-a9.S
index 1ca815d0fab8..1b2287033a4b 100644
--- a/arch/arm/mach-tegra/cortex-a9.S
+++ b/arch/arm/mach-tegra/cortex-a9.S
@@ -406,7 +406,10 @@ ENTRY(__cortex_a9_restore)
 	mcr	p15, 2, r0, c0, c0, 0	@ csselr
 	mcr	p15, 0, r1, c1, c0, 0	@ sctlr
 	mcr	p15, 0, r2, c1, c0, 1	@ actlr
+#ifndef CONFIG_TRUSTED_FOUNDATIONS
+	//TL : moved to secure 
 	mcr	p15, 0, r3, c15, c0, 0	@ pctlr
+#endif
 
 	add	r9, r8, #CTX_TTBR0
 	ldmia	r9!, {r0-r7}
@@ -510,8 +513,11 @@ ENTRY(__cortex_a9_restore)
 #endif
 	mcr	p15, 0, lr, c1, c0, 2	@ cpacr (loaded before VFP)
 
+#ifndef CONFIG_TRUSTED_FOUNDATIONS
+	//TL : moved to secure 
 	ldr	r9, [r8, #CTX_DIAGNOSTIC]
 	mcr	p15, 0, r9, c15, c0, 1	@ diag
+#endif
 
 	/* finally, restore the stack and return */
 	ldmfd	sp!, {r3-r12, lr}
@@ -532,6 +538,7 @@ ENTRY(__cortex_a9_l2x0_restart)
 	mov32	r9, (TEGRA_ARM_PL310_BASE-IO_CPU_PHYS+IO_CPU_VIRT)
 	add	r10, r8, #CTX_L2_CTRL
 	ldmia	r10, {r3-r7}
+#ifndef CONFIG_TRUSTED_FOUNDATIONS
 	str	r5, [r9, #L2X0_TAG_LATENCY_CTRL]
 	str	r6, [r9, #L2X0_DATA_LATENCY_CTRL]
 	str	r7, [r9, #L2X0_PREFETCH_OFFSET]
@@ -557,6 +564,21 @@ __reenable_l2x0:
 	dsb
 	isb
 	str	r3, [r9, #L2X0_CTRL]
+#else
+	cmp r3, #0        @ only call SMC if L2 was enable
+	beq l2_done
+	
+	cmp	r0, #0      @ if invalidate, call SMC with R1=1, else R1=4
+	moveq r1, #4
+	movne r1, #1
+// SMC(Enable Cache)
+	ldr r0, =0xFFFFF100
+	ldr r2, =0x00000000
+	ldr r3, =0x00000000
+	ldr r4, =0x00000000
+	smc 0
+l2_done:
+#endif
 #endif
 	b	__cortex_a9_restore
 
diff --git a/arch/arm/mach-tegra/cpuidle.c b/arch/arm/mach-tegra/cpuidle.c
index 23cb9acc588c..e9a2f25da9e8 100644
--- a/arch/arm/mach-tegra/cpuidle.c
+++ b/arch/arm/mach-tegra/cpuidle.c
@@ -205,17 +205,26 @@ static int tegra_tear_down_cpu1(void)
 
 	return 0;
 }
+#ifdef CONFIG_TRUSTED_FOUNDATIONS
+void callGenericSMC(u32 param0, u32 param1, u32 param2);
+#endif
 
 static void tegra_wake_cpu1(void)
 {
 	unsigned long boot_vector;
 	unsigned long old_boot_vector;
 	unsigned long timeout;
+#ifndef CONFIG_TRUSTED_FOUNDATIONS
 	u32 reg;
+   static void __iomem *vector_base = (IO_ADDRESS(TEGRA_EXCEPTION_VECTORS_BASE) + 0x100);
+#endif
 
 	boot_vector = virt_to_phys(tegra_hotplug_startup);
-	old_boot_vector = readl(EVP_CPU_RESET_VECTOR);
-	writel(boot_vector, EVP_CPU_RESET_VECTOR);
+#if CONFIG_TRUSTED_FOUNDATIONS
+	callGenericSMC(0xFFFFFFFC, 0xFFFFFFE5, boot_vector);
+#else
+	old_boot_vector = readl(vector_base);
+	writel(boot_vector, vector_base);
 
 	/* enable cpu clock on cpu */
 	reg = readl(CLK_RST_CONTROLLER_CLK_CPU_CMPLX);
@@ -227,15 +236,17 @@ static void tegra_wake_cpu1(void)
 	/* unhalt the cpu */
 	writel(0, IO_ADDRESS(TEGRA_FLOW_CTRL_BASE) + 0x14);
 
+
 	timeout = jiffies + msecs_to_jiffies(1000);
 	while (time_before(jiffies, timeout)) {
-		if (readl(EVP_CPU_RESET_VECTOR) != boot_vector)
+		if (readl(vector_base) != boot_vector)
 			break;
 		udelay(10);
 	}
 
 	/* put the old boot vector back */
-	writel(old_boot_vector, EVP_CPU_RESET_VECTOR);
+	writel(old_boot_vector, vector_base);
+#endif
 
 	/* CPU1 is now started */
 }
@@ -557,7 +568,7 @@ static int __init tegra_cpuidle_init(void)
 	void __iomem *mask_arm;
 	unsigned int reg;
 	int ret;
-
+   
 	irq_set_affinity(TEGRA_CPUIDLE_BOTH_IDLE, cpumask_of(0));
 	irq_set_affinity(TEGRA_CPUIDLE_TEAR_DOWN, cpumask_of(1));
 
diff --git a/arch/arm/mach-tegra/platsmp.c b/arch/arm/mach-tegra/platsmp.c
index 659c66967fb5..6cacdb910af4 100644
--- a/arch/arm/mach-tegra/platsmp.c
+++ b/arch/arm/mach-tegra/platsmp.c
@@ -75,13 +75,19 @@ void __cpuinit platform_secondary_init(unsigned int cpu)
 #endif
 	spin_unlock(&boot_lock);
 }
+#ifdef CONFIG_TRUSTED_FOUNDATIONS
+void callGenericSMC(u32 param0, u32 param1, u32 param2);
+#endif
 
 int __cpuinit boot_secondary(unsigned int cpu, struct task_struct *idle)
 {
 	unsigned long old_boot_vector;
 	unsigned long boot_vector;
 	unsigned long timeout;
+#ifndef CONFIG_TRUSTED_FOUNDATIONS
 	u32 reg;
+   static void __iomem *vector_base = (IO_ADDRESS(TEGRA_EXCEPTION_VECTORS_BASE) + 0x100);
+#endif
 
 	/*
 	 * set synchronisation state between this boot processor
@@ -99,8 +105,11 @@ int __cpuinit boot_secondary(unsigned int cpu, struct task_struct *idle)
 
 	smp_wmb();
 
-	old_boot_vector = readl(EVP_CPU_RESET_VECTOR);
-	writel(boot_vector, EVP_CPU_RESET_VECTOR);
+#if CONFIG_TRUSTED_FOUNDATIONS
+	callGenericSMC(0xFFFFFFFC, 0xFFFFFFE5, boot_vector);
+#else
+	old_boot_vector = readl(vector_base);
+	writel(boot_vector, vector_base);
 
 	/* enable cpu clock on cpu */
 	reg = readl(CLK_RST_CONTROLLER_CLK_CPU_CMPLX);
@@ -114,13 +123,14 @@ int __cpuinit boot_secondary(unsigned int cpu, struct task_struct *idle)
 
 	timeout = jiffies + HZ;
 	while (time_before(jiffies, timeout)) {
-		if (readl(EVP_CPU_RESET_VECTOR) != boot_vector)
+		if (readl(vector_base) != boot_vector)
 			break;
 		udelay(10);
 	}
 
 	/* put the old boot vector back */
-	writel(old_boot_vector, EVP_CPU_RESET_VECTOR);
+	writel(old_boot_vector, vector_base);
+#endif
 
 	/*
 	 * now the secondary core is starting up let it run its
diff --git a/arch/arm/mach-tegra/suspend.c b/arch/arm/mach-tegra/suspend.c
index 8a3af04a1689..7e581b198216 100644
--- a/arch/arm/mach-tegra/suspend.c
+++ b/arch/arm/mach-tegra/suspend.c
@@ -56,6 +56,28 @@
 #include "board.h"
 #include "power.h"
 
+#ifdef CONFIG_TRUSTED_FOUNDATIONS
+void callGenericSMC(u32 param0, u32 param1, u32 param2)
+{
+	__asm__ volatile(
+		"mov r0, %2\n"
+		"mov r1, %3\n"
+		"mov r2, %4\n"
+		"mov r3, #0\n"
+		"mov r4, #0\n"
+		".word    0xe1600070              @ SMC 0\n"
+		"mov %0, r0\n"
+		"mov %1, r1\n"
+		: "=r" (param0), "=r" (param1)
+		: "r" (param0), "r" (param1),
+		  "r" (param2)
+		: "r0", "r1", "r2", "r3", "r4");
+}
+u32 buffer_rdv[64];
+#endif
+
+/**************** END TL *********************/
+
 struct suspend_context {
 	/*
 	 * The next 7 values are referenced by offset in __restart_plls
@@ -379,6 +401,11 @@ unsigned int tegra_suspend_lp2(unsigned int us)
 	outer_flush_range(__pa(&tegra_sctx),__pa(&tegra_sctx+1));
 	barrier();
 
+#ifdef CONFIG_TRUSTED_FOUNDATIONS
+// TRUSTED LOGIC SMC_STOP/Save State
+	callGenericSMC(0xFFFFFFFC, 0xFFFFFFE4, virt_to_phys(buffer_rdv));
+#endif
+
 	__cortex_a9_save(mode);
 	/* return from __cortex_a9_restore */
 	barrier();
@@ -462,6 +489,10 @@ static void tegra_suspend_dram(bool do_lp0)
 	l2x0_shutdown();
 #endif
 
+#ifdef CONFIG_TRUSTED_FOUNDATIONS
+// TRUSTED LOGIC SMC_STOP/Save State
+	callGenericSMC(0xFFFFFFFC, 0xFFFFFFE3, virt_to_phys(buffer_rdv));
+#endif
 	__cortex_a9_save(mode);
 	restore_cpu_complex();
 
diff --git a/arch/arm/mm/cache-l2x0.c b/arch/arm/mm/cache-l2x0.c
index 9abfa5d2b750..55aff8687bff 100644
--- a/arch/arm/mm/cache-l2x0.c
+++ b/arch/arm/mm/cache-l2x0.c
@@ -23,6 +23,11 @@
 #include <asm/cacheflush.h>
 #include <asm/hardware/cache-l2x0.h>
 
+#ifdef CONFIG_TRUSTED_FOUNDATIONS
+#include <linux/sched.h>
+void callGenericSMC(u32 param0, u32 param1, u32 param2);
+#endif
+
 #define CACHE_LINE_SIZE		32
 
 static void __iomem *l2x0_base;
@@ -249,6 +254,11 @@ static void l2x0_flush_range(unsigned long start, unsigned long end)
 void l2x0_shutdown(void)
 {
 	unsigned long flags;
+#ifdef CONFIG_SMP
+	long ret;
+	cpumask_t saved_cpu_mask;
+	cpumask_t local_cpu_mask = CPU_MASK_NONE;
+#endif
 
 	if (l2x0_disabled)
 		return;
@@ -258,6 +268,7 @@ void l2x0_shutdown(void)
 	local_irq_save(flags);
 
 	if (readl(l2x0_base + L2X0_CTRL) & 1) {
+#ifndef CONFIG_TRUSTED_FOUNDATIONS
 		int m;
 		/* lockdown all ways, all masters to prevent new line
 		 * allocation during maintenance */
@@ -274,6 +285,27 @@ void l2x0_shutdown(void)
 			writel(0, l2x0_base + L2X0_LOCKDOWN_WAY_D + (m*8));
 			writel(0, l2x0_base + L2X0_LOCKDOWN_WAY_I + (m*8));
 		}
+#else
+#ifdef CONFIG_SMP
+      /* If SMP defined, 
+         TF is running on Core #0. So, force execution on Core #0 */
+		cpu_set(0, local_cpu_mask);
+		sched_getaffinity(0, &saved_cpu_mask);
+		ret = sched_setaffinity(0, &local_cpu_mask);
+		if (ret != 0)
+		{
+			printk(KERN_ERR "sched_setaffinity #1 -> 0x%lX", ret);
+		}
+#endif
+		callGenericSMC(0xFFFFF100, 0x00000002, 0);
+#ifdef CONFIG_SMP
+		ret = sched_setaffinity(0, &saved_cpu_mask);
+		if (ret != 0)
+		{
+			printk(KERN_ERR "sched_setaffinity #2 -> 0x%lX", ret);
+		}
+#endif
+#endif
 	}
 
 	local_irq_restore(flags);
@@ -285,6 +317,11 @@ static void l2x0_enable(__u32 aux_val, __u32 aux_mask)
 	__u32 cache_id;
 	int ways;
 	const char *type;
+#ifdef CONFIG_SMP
+	long ret;
+	cpumask_t saved_cpu_mask;
+	cpumask_t local_cpu_mask = CPU_MASK_NONE;
+#endif
 
 	if (l2x0_disabled)
 		return;
@@ -324,6 +361,7 @@ static void l2x0_enable(__u32 aux_val, __u32 aux_mask)
 	 */
 	if (!(readl_relaxed(l2x0_base + L2X0_CTRL) & 1)) {
 
+#ifndef CONFIG_TRUSTED_FOUNDATIONS
 		/* l2x0 controller is disabled */
 		writel_relaxed(aux, l2x0_base + L2X0_AUX_CTRL);
 
@@ -331,6 +369,41 @@ static void l2x0_enable(__u32 aux_val, __u32 aux_mask)
 
 		/* enable L2X0 */
 		writel_relaxed(1, l2x0_base + L2X0_CTRL);
+      
+#else /* CONFIG_TRUSTED_FOUNDATIONS is defined */
+/*
+			ISSUE : Some registers of PL310 controler must be written from Secure context!
+						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 in cache (L2X0_INV_WAY==0x77C), mandatory at boot time.
+						- Tag and Data RAM Latency Control Registers (0x108 & 0x10C) must be written in Secure.
+
+			The following call are now called by a Secure driver.
+			We switch to Secure context and ask to Trusted Foundations to do the configuration and activation of L2.*/
+		/* l2x0 controller is disabled */
+
+#ifdef CONFIG_SMP
+      /* If SMP defined, 
+         TF is running on Core #0. So, force execution on Core #0 */
+		cpu_set(0, local_cpu_mask);
+		sched_getaffinity(0, &saved_cpu_mask);
+		ret = sched_setaffinity(0, &local_cpu_mask);
+		if (ret != 0)
+		{
+			printk(KERN_ERR "sched_setaffinity #1 -> 0x%lX", ret);
+		}
+#endif
+		callGenericSMC(0xFFFFF100, 0x00000001, 0);
+#ifdef CONFIG_SMP
+		ret = sched_setaffinity(0, &saved_cpu_mask);
+		if (ret != 0)
+		{
+			printk(KERN_ERR "sched_setaffinity #2 -> 0x%lX", ret);
+		}
+#endif
+#endif
 	}
 
 	/*printk(KERN_INFO "%s cache controller enabled\n", type);
diff --git a/arch/arm/mm/proc-v7.S b/arch/arm/mm/proc-v7.S
index de77d5b4271a..590f57dc1f70 100644
--- a/arch/arm/mm/proc-v7.S
+++ b/arch/arm/mm/proc-v7.S
@@ -238,6 +238,8 @@ __v7_setup:
 2:	ldr	r10, =0x00000c09		@ Cortex-A9 primary part number
 	teq	r0, r10
 	bne	3f
+#ifndef CONFIG_TRUSTED_FOUNDATIONS
+    /* c15,c0,0: read and write in Secure privileged modes, read only in Non-secure state. */
 	cmp	r6, #0x10			@ power ctrl reg added r1p0
 	mrcge	p15, 0, r10, c15, c0, 0		@ read power control register
 	orrge	r10, r10, #1			@ enable dynamic clock gating
@@ -248,7 +250,9 @@ __v7_setup:
 	orreq	r10, r10, #0x30			@ disable core clk gate on
 	mcreq	p15, 0, r10, c15, c0, 2		@ instr-side waits
 #endif
-#ifdef CONFIG_ARM_ERRATA_742230
+#endif
+
+#if defined(CONFIG_ARM_ERRATA_742230) && !defined(CONFIG_TRUSTED_FOUNDATIONS)
 	cmp	r6, #0x22			@ only present up to r2p2
 	mrcle	p15, 0, r10, c15, c0, 1		@ read diagnostic register
 	orrle	r10, r10, #1 << 4		@ set bit #4