From 79e968112ebeeb4c656263a434f6fbffc8f533d9 Mon Sep 17 00:00:00 2001
From: Ye Li <ye.li@nxp.com>
Date: Mon, 13 Mar 2017 14:03:53 +0800
Subject: MLK-14417 imx: Enable ACTLR.SMP bit for all i.MX cortex-a7 platforms

According to the Cortex-A7 TRM, for ACTLR.SMP bit "You must ensure this bit
is set to 1 before the caches and MMU are enabled, or any cache and TLB
maintenance operations are performed".
ROM sets this bit in normal boot flow, but when in serial download mode, it is not set.
Here we add it in u-boot as a common flow for all i.MX cortex-a7 platforms,
including mx7d, mx6ul/ull and mx7ulp.

Signed-off-by: Ye Li <ye.li@nxp.com>
(cherry picked from commit 14990af03450f3e1898135c86fd8b93328007617)
---
 arch/arm/cpu/armv7/mx7/soc.c |  7 -------
 arch/arm/imx-common/cache.c  | 40 ++++++++++++++++++++++++++++++++++++++++
 2 files changed, 40 insertions(+), 7 deletions(-)

diff --git a/arch/arm/cpu/armv7/mx7/soc.c b/arch/arm/cpu/armv7/mx7/soc.c
index 332d977..89a339c 100644
--- a/arch/arm/cpu/armv7/mx7/soc.c
+++ b/arch/arm/cpu/armv7/mx7/soc.c
@@ -480,13 +480,6 @@ int mmc_get_env_dev(void)
 
 void s_init(void)
 {
-#if !defined CONFIG_SPL_BUILD
-	/* Enable SMP mode for CPU0, by setting bit 6 of Auxiliary Ctl reg */
-	asm volatile(
-			"mrc p15, 0, r0, c1, c0, 1\n"
-			"orr r0, r0, #1 << 6\n"
-			"mcr p15, 0, r0, c1, c0, 1\n");
-#endif
 	/* clock configuration. */
 	clock_init();
 
diff --git a/arch/arm/imx-common/cache.c b/arch/arm/imx-common/cache.c
index af14702..e00431a 100644
--- a/arch/arm/imx-common/cache.c
+++ b/arch/arm/imx-common/cache.c
@@ -1,5 +1,6 @@
 /*
  * Copyright 2015-2016 Freescale Semiconductor, Inc.
+ * Copyright 2017 NXP
  *
  * SPDX-License-Identifier:     GPL-2.0+
  */
@@ -10,6 +11,34 @@
 #include <asm/io.h>
 #include <asm/imx-common/sys_proto.h>
 
+static void enable_ca7_smp(void)
+{
+	uint32_t val;
+
+	/* Read MIDR */
+	asm volatile ("mrc p15, 0, %0, c0, c0, 0\n\t" : "=r"(val));
+	val = (val >> 4);
+	val &= 0xf;
+
+	/* Only set the SMP for Cortex A7 */
+	if (val == 0x7) {
+		/* Read auxiliary control register */
+		asm volatile ("mrc p15, 0, %0, c1, c0, 1\n\t" : "=r"(val));
+
+		if (val & (1 << 6))
+			return;
+
+		/* Enable SMP */
+		val |= (1 << 6);
+
+		/* Write auxiliary control register */
+		asm volatile ("mcr p15, 0, %0, c1, c0, 1\n\t" : : "r"(val));
+
+		DSB;
+		ISB;
+	}
+}
+
 #ifndef CONFIG_SYS_DCACHE_OFF
 void enable_caches(void)
 {
@@ -21,6 +50,9 @@ void enable_caches(void)
 	/* Avoid random hang when download by usb */
 	invalidate_dcache_all();
 
+	/* Set ACTLR.SMP bit for Cortex-A7 */
+	enable_ca7_smp();
+
 	/* Enable D-cache. I-cache is already enabled in start.S */
 	dcache_enable();
 
@@ -32,6 +64,14 @@ void enable_caches(void)
 					IRAM_SIZE,
 					option);
 }
+#else
+void enable_caches(void)
+{
+	/* Set ACTLR.SMP bit for Cortex-A7, even the caches are disabled by u-boot */
+	enable_ca7_smp();
+	
+	puts("WARNING: Caches not enabled\n");
+}
 #endif
 
 #ifndef CONFIG_SYS_L2CACHE_OFF
-- 
cgit v1.1