[PATCH v1 04/20] arm: socfpga: agilex5: Add low level initialization

[tien.fong.chee@intel.com]

From: Tien Fong Chee <tien.fong.chee@intel.com>

Create new low level initialization for Agile5 due to the new ARM core
composition and warm reset behavior.

Signed-off-by: Tien Fong Chee <tien.fong.chee@intel.com>
---
 arch/arm/mach-socfpga/Makefile                |  1 +
 .../include/mach/reset_manager_soc64.h        | 12 +++-
 arch/arm/mach-socfpga/lowlevel_init_agilex5.S | 57 +++++++++++++++++++
 3 files changed, 68 insertions(+), 2 deletions(-)
 create mode 100644 arch/arm/mach-socfpga/lowlevel_init_agilex5.S

diff --git a/arch/arm/mach-socfpga/Makefile b/arch/arm/mach-socfpga/Makefile
index 67c6a8dfec5..53c91d1d2a5 100644
--- a/arch/arm/mach-socfpga/Makefile
+++ b/arch/arm/mach-socfpga/Makefile
@@ -58,6 +58,7 @@ endif
 
 ifdef CONFIG_TARGET_SOCFPGA_AGILEX5
 obj-y	+= clock_manager_agilex5.o
+obj-y	+= lowlevel_init_agilex5.o
 obj-y	+= mailbox_s10.o
 obj-y	+= misc_soc64.o
 obj-y	+= mmu-arm64_s10.o
diff --git a/arch/arm/mach-socfpga/include/mach/reset_manager_soc64.h b/arch/arm/mach-socfpga/include/mach/reset_manager_soc64.h
index c8bb727aa2b..d373ec0dc70 100644
--- a/arch/arm/mach-socfpga/include/mach/reset_manager_soc64.h
+++ b/arch/arm/mach-socfpga/include/mach/reset_manager_soc64.h
@@ -1,15 +1,17 @@
 /* SPDX-License-Identifier: GPL-2.0 */
 /*
- *  Copyright (C) 2016-2019 Intel Corporation <www.intel.com>
+ *  Copyright (C) 2016-2024 Intel Corporation <www.intel.com>
  */
 
 #ifndef _RESET_MANAGER_SOC64_H_
 #define _RESET_MANAGER_SOC64_H_
 
+#ifndef __ASSEMBLY__
 void reset_deassert_peripherals_handoff(void);
 int cpu_has_been_warmreset(void);
 void print_reset_info(void);
 void socfpga_bridges_reset(int enable);
+#endif
 
 #define RSTMGR_SOC64_STATUS	0x00
 #define RSTMGR_SOC64_MPUMODRST	0x20
@@ -23,14 +25,20 @@ void socfpga_bridges_reset(int enable);
 #define RSTMGR_BRGMODRST_FPGA2SOC_MASK	0x00000004
 
 /* SDM, Watchdogs and MPU warm reset mask */
-#define RSTMGR_STAT_SDMWARMRST		BIT(1)
+#define RSTMGR_STAT_SDMWARMRST		0x2
 #define RSTMGR_STAT_MPU0RST_BITPOS	8
 #define RSTMGR_STAT_L4WD0RST_BITPOS	16
+#if IS_ENABLED(CONFIG_TARGET_SOCFPGA_AGILEX5)
+#define RSTMGR_STAT_L4WD0RST_BIT	0x1F0000
+#define RSTMGR_L4WD_MPU_WARMRESET_MASK	(RSTMGR_STAT_SDMWARMRST | \
+		RSTMGR_STAT_L4WD0RST_BIT)
+#else
 #define RSTMGR_L4WD_MPU_WARMRESET_MASK	(RSTMGR_STAT_SDMWARMRST | \
 		GENMASK(RSTMGR_STAT_MPU0RST_BITPOS + 3, \
 			RSTMGR_STAT_MPU0RST_BITPOS) | \
 		GENMASK(RSTMGR_STAT_L4WD0RST_BITPOS + 3, \
 			RSTMGR_STAT_L4WD0RST_BITPOS))
+#endif
 
 /*
  * SocFPGA Stratix10 reset IDs, bank mapping is as follows:
diff --git a/arch/arm/mach-socfpga/lowlevel_init_agilex5.S b/arch/arm/mach-socfpga/lowlevel_init_agilex5.S
new file mode 100644
index 00000000000..27430577719
--- /dev/null
+++ b/arch/arm/mach-socfpga/lowlevel_init_agilex5.S
@@ -0,0 +1,57 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Copyright (C) 2024 Intel Corporation. All rights reserved
+ */
+
+#include <asm-offsets.h>
+#include <config.h>
+#include <asm/arch/reset_manager_soc64.h>
+#include <asm/macro.h>
+#include <linux/linkage.h>
+
+ENTRY(lowlevel_init)
+	mov	x29, lr			/* Save LR */
+
+	/* Enable Async */
+	msr daifclr, #4
+
+#ifdef CONFIG_SPL_BUILD
+	branch_if_slave x0, 3f
+
+	/* Check rstmgr.stat for warm reset status */
+	ldr	w1, =SOCFPGA_RSTMGR_ADDRESS
+	ldr	w0, [x1]
+	/* Check whether any L4 watchdogs or SDM had triggered warm reset */
+	ldr	x2, =RSTMGR_L4WD_MPU_WARMRESET_MASK
+	ands	x0, x0, x2
+	/*
+	 * If current Reset Manager's status is warm reset just reload the
+	 * .data section by copying the data from data preserve section.
+	 * Otherwise, copy the .data section to the data preserve section to
+	 * keep an original copy of .data section. This ensure SPL is
+	 * reentrant after warm reset.
+	 */
+	b.ne	reload_data_section
+	/* Copy from .data to preserved .data to backup the SPL state */
+	ldr	x0, =__data_start
+	ldr	x1, =__preserve_data_start
+	ldr	x2, =__preserve_data_end
+	b	copy_loop
+reload_data_section:
+	/* Copy from preserved .data to .data to restore the SPL state */
+	ldr	x0, =__preserve_data_start
+	ldr	x1, =__data_start
+	ldr	x2, =__data_end
+copy_loop:
+	ldr	w3, [x0]
+	add	x0, x0, #4
+	str	w3, [x1]
+	add	x1, x1, #4
+	cmp	x1, x2
+	b.ne	copy_loop
+3:
+#endif
+
+	mov	lr, x29			/* Restore LR */
+	ret
+ENDPROC(lowlevel_init)
-- 
2.25.1