[PATCH v1 18/34] arm: socfpga: soc64: Perform warm reset after L2 reset in SPL

[alif.zakuan.yuslaimi at altera.com]

From: Alif Zakuan Yuslaimi <alif.zakuan.yuslaimi at altera.com>

SPL checks for a magic word in the system manager's scratch
register to determine if an L2 reset has occurred. If detected,
SPL places all slave CPUs (CPU1–3) into WFI mode. The master
CPU (CPU0) then initiates a warm reset by writing to the RMR_EL3
system register and also enters WFI mode.

This warm reset flow is handled entirely within the HPS. The
function `socfpga_sysreset_request()` triggers the warm
reset, and upon SPL re-entry, the updated `lowlevel_init_soc64.S`
handles the necessary initialization.

Signed-off-by: Alif Zakuan Yuslaimi <alif.zakuan.yuslaimi at altera.com>
---
 .../include/mach/system_manager_soc64.h       |  2 +
 arch/arm/mach-socfpga/lowlevel_init_soc64.S   | 95 +++++++++++++++++++
 include/configs/socfpga_soc64_common.h        |  5 +
 3 files changed, 102 insertions(+)

diff --git a/arch/arm/mach-socfpga/include/mach/system_manager_soc64.h b/arch/arm/mach-socfpga/include/mach/system_manager_soc64.h
index 0871cf949e5..713fa2c517b 100644
--- a/arch/arm/mach-socfpga/include/mach/system_manager_soc64.h
+++ b/arch/arm/mach-socfpga/include/mach/system_manager_soc64.h
@@ -148,6 +148,8 @@ void populate_sysmgr_pinmux(void);
  * Bit[30] reserved for FSBL to update the DDR init progress
  * 1 - means in progress, 0 - haven't started / DDR is up running.
  *
+ * Bit[19] store ATF CPU0 ON OFF value.
+ *
  * Bit[18] reserved for SDM to configure ACF
  * Bit[17:1] - Setting by Linux EDAC.
  * Bit[1](ECC_OCRAM), Bit[16](ECC_DDR0), Bit[17](ECC_DDR1)
diff --git a/arch/arm/mach-socfpga/lowlevel_init_soc64.S b/arch/arm/mach-socfpga/lowlevel_init_soc64.S
index 8926c2d1d9c..b39565f591d 100644
--- a/arch/arm/mach-socfpga/lowlevel_init_soc64.S
+++ b/arch/arm/mach-socfpga/lowlevel_init_soc64.S
@@ -12,15 +12,110 @@
 ENTRY(lowlevel_init)
 	mov	x29, lr			/* Save LR */
 
+#ifdef CONFIG_XPL_BUILD
+	/* Check for L2 reset magic word */
+	ldr	x4, =L2_RESET_DONE_REG
+	ldr	x5, [x4]
+	ldr	x1, =L2_RESET_DONE_STATUS
+	cmp	x1, x5
+	/* No L2 reset, skip warm reset */
+	b.ne	skipwarmreset
+	/* Put all slaves CPUs into WFI mode */
+	branch_if_slave x0, put_cpu_in_wfi
+	/* L2 reset completed */
+	str	xzr, [x4]
+	/* Clear previous CPU release address */
+	ldr	x4, =CPU_RELEASE_ADDR
+	str	wzr, [x4]
+	/* Master CPU (CPU0) request for warm reset */
+	mrs	x1, rmr_el3
+	orr	x1, x1, #0x02
+	msr	rmr_el3, x1
+	isb
+	dsb	sy
+put_cpu_in_wfi:
+	wfi
+	b	put_cpu_in_wfi
+skipwarmreset:
+#endif
+
 #if defined(CONFIG_GICV2) || defined(CONFIG_GICV3)
 #if defined(CONFIG_XPL_BUILD) && defined(CONFIG_SPL_ATF)
+
+	/*
+	 * In ATF flow, need to clear the old CPU address when cold reset
+	 * being triggered, but shouldn't clear CPU address if it is reset
+	 * by CPU-ON, so that the core can correctly jump to ATF code after
+	 * reset by CPU-ON. CPU-ON trigger the reset via mpumodrst.
+	 *
+	 * Hardware will set 1 to core*_irq in mpurststat register in
+	 * reset manager if the core is reset by mpumodrst.
+	 *
+	 * The following code will check the mpurststat to identify if the
+	 * core is reset by mpumodrst, and it will skip CPU address clearing
+	 * if the core is reset by mpumodrst. At last, the code need to clear
+	 * the core*_irq by set it to 1. So that it can reflect the correct
+	 * and latest status in next reset.
+	 */
+
+	/* Check if it is a master core off/on from kernel using boot scratch
+	 * cold register 8 bit 19. This bit is set by ATF.
+	 */
+	ldr	x4, =BOOT_SCRATCH_COLD8
+	ldr	x5, [x4]
+	and	x6, x5, #0x80000
+	cbnz	x6, wait_for_atf_master
+
+	/* Retrieve mpurststat register in reset manager */
+	ldr	x4, =SOCFPGA_RSTMGR_ADDRESS
+	ldr	w5, [x4, #0x04]
+
+	/* Set mask based on current core id */
+	mrs	x0, mpidr_el1
+	and	x1, x0, #0xF
+	ldr	x2, =0x00000100
+	lsl	x2, x2, x1
+
+	/* Skip if core*_irq register is set */
+	and	x6, x5, x2
+	cbnz	x6, skip_clear_cpu_address
+
+	/*
+	 * Reach here means core*_irq is 0, means the core is
+	 * reset by cold, warm or watchdog reset.
+	 * Clear previous CPU release address
+	 */
+	ldr	x4, =CPU_RELEASE_ADDR
+	str	wzr, [x4]
+	b	skip_clear_core_irq
+
+skip_clear_cpu_address:
+	/* Clear core*_irq register by writing 1 */
+	ldr	x4, =SOCFPGA_RSTMGR_ADDRESS
+	str	w2, [x4, #0x04]
+
+skip_clear_core_irq:
+	/* Master CPU (CPU0) does not need to wait for atf */
+	branch_if_master x0, master_cpu
+
 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
+
+wait_for_atf_master:
+	ldr	x4, =CPU_RELEASE_ADDR
+	ldr	x5, [x4]
+	cbz	x5, master_wait_atf
+	br	x5
+master_wait_atf:
+	branch_if_master x0, wait_for_atf_master
+
+master_cpu:
 #else
 	branch_if_slave x0, 1f
 #endif
diff --git a/include/configs/socfpga_soc64_common.h b/include/configs/socfpga_soc64_common.h
index b8448ba6636..312c6fafc6c 100644
--- a/include/configs/socfpga_soc64_common.h
+++ b/include/configs/socfpga_soc64_common.h
@@ -24,6 +24,11 @@
  */
 #define L2_RESET_DONE_REG		0xFFD12218
 
+/* sysmgr.boot_scratch_cold8 bit 17 (1bit) will be used to check whether CPU0
+ * is being powered off/on from kernel
+ */
+#define BOOT_SCRATCH_COLD8		0xFFD12220
+
 /* Magic word to indicate L2 reset is completed */
 #define L2_RESET_DONE_STATUS		0x1228E5E7
 
-- 
2.35.3