[v5 03/18] arm: socfpga: soc64: Override 'lowlevel_init' to support ATF

[Siew Chin Lim <elly.siew.chin.lim@intel.com>]

From: Chee Hong Ang <chee.hong.ang@intel.com>

Override 'lowlevel_init' to make sure secondary CPUs trapped
in ATF instead of SPL. After ATF is initialized, it will signal
the secondary CPUs to jump from SPL to ATF waiting to be 'activated'
by Linux OS via PSCI call.

Signed-off-by: Chee Hong Ang <chee.hong.ang@intel.com>
---
 arch/arm/mach-socfpga/Makefile                     |  2 ++
 .../arm/mach-socfpga/lowlevel_init_soc64.S         | 41 ++++++++--------------
 2 files changed, 17 insertions(+), 26 deletions(-)
 copy board/cortina/presidio-asic/lowlevel_init.S => arch/arm/mach-socfpga/lowlevel_init_soc64.S (66%)

diff --git a/arch/arm/mach-socfpga/Makefile b/arch/arm/mach-socfpga/Makefile
index 418f543b20..c63162a5c6 100644
--- a/arch/arm/mach-socfpga/Makefile
+++ b/arch/arm/mach-socfpga/Makefile
@@ -29,6 +29,7 @@ endif
 
 ifdef CONFIG_TARGET_SOCFPGA_STRATIX10
 obj-y	+= clock_manager_s10.o
+obj-y	+= lowlevel_init_soc64.o
 obj-y	+= mailbox_s10.o
 obj-y	+= misc_s10.o
 obj-y	+= mmu-arm64_s10.o
@@ -41,6 +42,7 @@ endif
 
 ifdef CONFIG_TARGET_SOCFPGA_AGILEX
 obj-y	+= clock_manager_agilex.o
+obj-y	+= lowlevel_init_soc64.o
 obj-y	+= mailbox_s10.o
 obj-y	+= misc_s10.o
 obj-y	+= mmu-arm64_s10.o
diff --git a/board/cortina/presidio-asic/lowlevel_init.S b/arch/arm/mach-socfpga/lowlevel_init_soc64.S
similarity index 66%
copy from board/cortina/presidio-asic/lowlevel_init.S
copy to arch/arm/mach-socfpga/lowlevel_init_soc64.S
index 4450a5df79..612ea8a037 100644
--- a/board/cortina/presidio-asic/lowlevel_init.S
+++ b/arch/arm/mach-socfpga/lowlevel_init_soc64.S
@@ -1,43 +1,31 @@
-/* SPDX-License-Identifier: GPL-2.0+ */
 /*
- * Copyright (C) 2020 Cortina-Access
+ * Copyright (C) 2020 Intel Corporation. All rights reserved
  *
+ * SPDX-License-Identifier:    GPL-2.0
  */
 
-
 #include <asm-offsets.h>
 #include <config.h>
 #include <linux/linkage.h>
 #include <asm/macro.h>
-#include <asm/armv8/mmu.h>
 
-	.globl lowlevel_init
-lowlevel_init:
+ENTRY(lowlevel_init)
 	mov	x29, lr			/* Save LR */
 
-#if defined(CONFIG_SOC_CA7774)
-	/* Enable SMPEN in CPUECTLR */
-	mrs     x0, s3_1_c15_c2_1
-	tst     x0, #0x40
-        b.ne    skip_smp_setup
-	orr     x0, x0, #0x40
-	msr     s3_1_c15_c2_1, x0
-skip_smp_setup:
-#endif
-
-#if defined(CONFIG_SOC_CA8277B)
-	/* Enable CPU Timer */
-	ldr x0, =CONFIG_SYS_TIMER_BASE
-	mov x1, #1
-	str w1, [x0]
-#endif
-
 #if defined(CONFIG_GICV2) || defined(CONFIG_GICV3)
+#if defined(CONFIG_SPL_BUILD) && defined(CONFIG_SPL_ATF)
+wait_for_atf:
+	ldr	x4, =CPU_RELEASE_ADDR
+	ldr	x5, [x4]
+	cbz	x5, slave_wait_atf
+	br	x5
+slave_wait_atf:
+	branch_if_slave x0, wait_for_atf
+#else
 	branch_if_slave x0, 1f
-#ifndef CONFIG_TARGET_VENUS
+#endif
 	ldr	x0, =GICD_BASE
 	bl	gic_init_secure
-#endif
 1:
 #if defined(CONFIG_GICV3)
 	ldr	x0, =GICR_BASE
@@ -54,7 +42,7 @@ skip_smp_setup:
 
 	/*
 	 * Slave should wait for master clearing spin table.
-	 * This sync prevent salves observing incorrect
+	 * This sync prevent slaves observing incorrect
 	 * value of spin table and jumping to wrong place.
 	 */
 #if defined(CONFIG_GICV2) || defined(CONFIG_GICV3)
@@ -85,3 +73,4 @@ lowlevel_in_el1:
 2:
 	mov	lr, x29			/* Restore LR */
 	ret
+ENDPROC(lowlevel_init)
-- 
2.13.0