firmware: Handle overlapping load and link addresses in relocation

The old code may corrupt the code of the waiting hart hence this patch
keeps waiting HART within relocation code range at time of relocation.

Signed-off-by: Xiang W <wxjstz@126.com>
Acked-by: Anup Patel <anup.patel@wdc.com>

1 files changed, 39 insertions, 21 deletions

diff --git a/firmware/fw_base.S b/firmware/fw_base.S
index 6270bbc..213396a 100644
--- a/firmware/fw_base.S
+++ b/firmware/fw_base.S
@@ -48,7 +48,7 @@ _start:
 	 * that is, for mhartid != 0
 	 */
 	csrr	a6, CSR_MHARTID
-	blt	zero, a6, _wait_for_boot_hart
+	blt	zero, a6, _wait_relocate_copy_done
 
 	/* Save load address */
 	la	t0, _load_start
@@ -72,7 +72,7 @@ _relocate:
 	blt	t2, t0, _relocate_copy_to_upper
 _relocate_copy_to_lower:
 	ble	t1, t2, _relocate_copy_to_lower_loop
-	la	t3, _boot_hart_done
+	la	t3, _boot_status
 	BRANGE	t2, t1, t3, _start_hang
 	la	t3, _relocate
 	la	t5, _relocate_done
@@ -88,7 +88,7 @@ _relocate_copy_to_lower_loop:
 	jr	t4
 _relocate_copy_to_upper:
 	ble	t3, t0, _relocate_copy_to_upper_loop
-	la	t2, _boot_hart_done
+	la	t2, _boot_status
 	BRANGE	t0, t3, t2, _start_hang
 	la	t2, _relocate
 	la	t5, _relocate_done
@@ -102,8 +102,34 @@ _relocate_copy_to_upper_loop:
 	REG_S	t2, 0(t1)
 	blt	t0, t1, _relocate_copy_to_upper_loop
 	jr	t4
+_wait_relocate_copy_done:
+	la	t0, _start
+	la	t1, _link_start
+	REG_L	t1, 0(t1)
+	la	t2, _boot_status
+	sub	t2, t2, t0
+	add	t2, t2, t1
+	la	t3, _wait_for_boot_hart
+	sub	t3, t3, t0
+	add	t3, t3, t1
+1:
+	/* waitting for relocate copy done (_boot_status == 1) */
+	li	t4, 1
+	REG_L	t5, 0(t2)
+	/* Reduce the bus traffic so that boot hart may proceed faster */
+	nop
+	nop
+	nop
+	bne	t4, t5, 1b
+	jr	t3
 _relocate_done:
 
+	/* mark relocate copy done */
+	la	t0, _boot_status
+	li	t1, 1
+	REG_S	t1, 0(t0)
+	fence	rw, rw
+
 	/* At this point we are running from link address */
 
 	/* Reset all registers for boot HART */
@@ -274,31 +300,23 @@ _fdt_reloc_again:
 	blt	t1, t2, _fdt_reloc_again
 _fdt_reloc_done:
 
-	/* Update boot hart flag */
-	la	a4, _boot_hart_done
-	la	a5, _start_warm
-	REG_S	a5, (a4)
-	fence	rw, rw
-	la	t0, _link_start
-	REG_L	t0, 0(t0)
-	sub	a4, a4, t0
-	la	t0, _load_start
-	REG_L	t0, 0(t0)
-	add	a4, a4, t0
-	REG_S	a5, (a4)
+	/* mark boot hart done */
+	li	t0, 2
+	la	t1, _boot_status
+	REG_S	t0, 0(t1)
 	fence	rw, rw
 	j	_start_warm
 
-	/* Wait for boot hart */
+	/* waitting for boot hart done (_boot_status == 2) */
 _wait_for_boot_hart:
-	la	a4, _boot_hart_done
-	REG_L	a5, 0(a4)
+	li	t0, 2
+	la	t1, _boot_status
+	REG_L	t1, 0(t1)
 	/* Reduce the bus traffic so that boot hart may proceed faster */
 	nop
 	nop
 	nop
-	beqz	a5, _wait_for_boot_hart
-	jr	a5
+	bne	t0, t1, _wait_for_boot_hart
 
 _start_warm:
 	/* Reset all registers for non-boot HARTs */
@@ -352,7 +370,7 @@ _start_warm:
 	j	_start_hang
 
 	.align 3
-_boot_hart_done:
+_boot_status:
 	RISCV_PTR	0
 _load_start:
 	RISCV_PTR	_fw_start