[PATCH 3/3] arch: arm: mach-socfpga: Reload SoC64 SPL state after warm reset

[Jit Loon Lim <jit.loon.lim@intel.com>]

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

When the system boot from cold reset, SPL will copy its .data section
into this backup section to keep an original copy of .data section.
When the system has been warm reset, SPL will reload the original .data
section from this backup section to restore the original state of SPL.
This is required to make sure SPL still run in fresh state after
warm reset.

Signed-off-by: Chee Hong Ang <chee.hong.ang@intel.com>
Signed-off-by: Jit Loon Lim <jit.loon.lim@intel.com>
---
 arch/arm/mach-socfpga/lowlevel_init_soc64.S | 37 +++++++++++++++++++++
 1 file changed, 37 insertions(+)

diff --git a/arch/arm/mach-socfpga/lowlevel_init_soc64.S b/arch/arm/mach-socfpga/lowlevel_init_soc64.S
index 875927cc4d..07adf62ea8 100644
--- a/arch/arm/mach-socfpga/lowlevel_init_soc64.S
+++ b/arch/arm/mach-socfpga/lowlevel_init_soc64.S
@@ -71,6 +71,43 @@ lowlevel_in_el1:
 #endif /* CONFIG_ARMV8_MULTIENTRY */
 
 2:
+
+#ifdef CONFIG_SPL_BUILD
+	branch_if_slave x0, 3f
+
+	/* Check rstmgr.stat for warm reset status */
+	ldr	x1, =SOCFPGA_RSTMGR_ADDRESS
+	ldr	x0, [x1]
+	/* Check whether any L4 watchdogs or MPUs had triggered warm reset */
+	ldr	x2, =0x000F0F00
+	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.26.2