[U-Boot] [PATCH 2/3] imx: Enable ACTLR.SMP bit for Cortex-A7 platforms

[Peng Fan]

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 set it for mx6/7/7ulp. The code will check whether
the PE is A7 or not.

Signed-off-by: Peng Fan <peng.fan at nxp.com>
Cc: Stefano Babic <sbabic at denx.de>
Cc: Fabio Estevam <fabio.estevam at nxp.com>
---
 arch/arm/include/asm/mach-imx/sys_proto.h |  1 +
 arch/arm/mach-imx/cache.c                 | 28 ++++++++++++++++++++++++++++
 arch/arm/mach-imx/mx6/soc.c               |  2 ++
 arch/arm/mach-imx/mx7/soc.c               |  9 ++-------
 arch/arm/mach-imx/mx7ulp/soc.c            |  2 ++
 5 files changed, 35 insertions(+), 7 deletions(-)

diff --git a/arch/arm/include/asm/mach-imx/sys_proto.h b/arch/arm/include/asm/mach-imx/sys_proto.h
index 046df62..26db1cc 100644
--- a/arch/arm/include/asm/mach-imx/sys_proto.h
+++ b/arch/arm/include/asm/mach-imx/sys_proto.h
@@ -87,6 +87,7 @@ static inline u8 imx6_is_bmode_from_gpr9(void)
 u32 imx6_src_get_boot_mode(void);
 #endif /* CONFIG_MX6 */
 
+void enable_actlr_smp(void);
 u32 get_nr_cpus(void);
 u32 get_cpu_rev(void);
 u32 get_cpu_speed_grade_hz(void);
diff --git a/arch/arm/mach-imx/cache.c b/arch/arm/mach-imx/cache.c
index c5279a7..e02f1a5 100644
--- a/arch/arm/mach-imx/cache.c
+++ b/arch/arm/mach-imx/cache.c
@@ -10,6 +10,34 @@
 #include <asm/io.h>
 #include <asm/mach-imx/sys_proto.h>
 
+void enable_actlr_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)
 {
diff --git a/arch/arm/mach-imx/mx6/soc.c b/arch/arm/mach-imx/mx6/soc.c
index af31673..0ef4b29 100644
--- a/arch/arm/mach-imx/mx6/soc.c
+++ b/arch/arm/mach-imx/mx6/soc.c
@@ -576,6 +576,8 @@ void s_init(void)
 	u32 mask528;
 	u32 reg, periph1, periph2;
 
+	enable_actlr_smp();
+
 	if (is_mx6sx() || is_mx6ul() || is_mx6ull())
 		return;
 
diff --git a/arch/arm/mach-imx/mx7/soc.c b/arch/arm/mach-imx/mx7/soc.c
index ec74b4c..4307ae0 100644
--- a/arch/arm/mach-imx/mx7/soc.c
+++ b/arch/arm/mach-imx/mx7/soc.c
@@ -447,13 +447,8 @@ 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
+	enable_actlr_smp();
+
 	/* clock configuration. */
 	clock_init();
 
diff --git a/arch/arm/mach-imx/mx7ulp/soc.c b/arch/arm/mach-imx/mx7ulp/soc.c
index 454665a..71bfe36 100644
--- a/arch/arm/mach-imx/mx7ulp/soc.c
+++ b/arch/arm/mach-imx/mx7ulp/soc.c
@@ -100,6 +100,8 @@ void init_wdog(void)
 
 void s_init(void)
 {
+	enable_actlr_smp();
+
 	/* Disable wdog */
 	init_wdog();
 
-- 
2.6.2