armv8: Enable CPUECTLR.SMPEN for coherency

For A53, data coherency is enabled only when the CPUECTLR.SMPEN bit is
set. The SMPEN bit should be set before enabling the data cache.
If not enabled, the cache is not coherent with other cores and
data corruption could occur.

For A57/A72, SMPEN bit enables the processor to receive instruction
cache and TLB maintenance operations broadcast from other processors
in the cluster. This bit should be set before enabling the caches and
MMU, or performing any cache and TLB maintenance operations.

Signed-off-by: Mingkai Hu <mingkai.hu@nxp.com>
Signed-off-by: Gong Qianyu <Qianyu.Gong@nxp.com>
Signed-off-by: Mateusz Kulikowski <mateusz.kulikowski@gmail.com>
Signed-off-by: Hou Zhiqiang <Zhiqiang.Hou@nxp.com>
Reviewed-by: York Sun <york.sun@nxp.com>

arch/arm/cpu/armv8/Kconfig

@@ -3,6 +3,24 @@ if ARM64
 config ARMV8_MULTIENTRY
         bool "Enable multiple CPUs to enter into U-Boot"
 
+config ARMV8_SET_SMPEN
+        bool "Enable data coherency with other cores in cluster"
+        help
+	  Say Y here if there is not any trust firmware to set
+	  CPUECTLR_EL1.SMPEN bit before U-Boot.
+
+	  For A53, it enables data coherency with other cores in the
+	  cluster, and for A57/A72, it enables receiving of instruction
+	  cache and TLB maintenance operations.
+	  Cortex A53/57/72 cores require CPUECTLR_EL1.SMPEN set even
+	  for single core systems. Unfortunately write access to this
+	  register may be controlled by EL3/EL2 firmware. To be more
+	  precise, by default (if there is EL2/EL3 firmware running)
+	  this register is RO for NS EL1.
+	  This switch can be used to avoid writing to CPUECTLR_EL1,
+	  it can be safely enabled when EL2/EL3 initialized SMPEN bit
+	  or when CPU implementation doesn't include that register.
+
 config ARMV8_SPIN_TABLE
 	bool "Support spin-table enable method"
 	depends on ARMV8_MULTIENTRY && OF_LIBFDT

arch/arm/cpu/armv8/start.S

@@ -86,6 +86,17 @@ save_boot_params_ret:
 	msr	cpacr_el1, x0			/* Enable FP/SIMD */
 0:
 
+	/*
+	 * Enalbe SMPEN bit for coherency.
+	 * This register is not architectural but at the moment
+	 * this bit should be set for A53/A57/A72.
+	 */
+#ifdef CONFIG_ARMV8_SET_SMPEN
+	mrs     x0, S3_1_c15_c2_1               /* cpuactlr_el1 */
+	orr     x0, x0, #0x40
+	msr     S3_1_c15_c2_1, x0
+#endif
+
 	/* Apply ARM core specific erratas */
 	bl	apply_core_errata