elfloader,RISC-V: Fix early boot issues

- Set a valid stack before calling any C function so that memory and
  registers don't get corrupted.
- Keep track of the DTB address so that it's still in a1 when main is
  called even if we needed to switch to a different HART.
- If the HSM extention isn't present, then try and call main on the
  current HART.
- Suspend HARTs that exceed CONFIG_MAX_NUM_NODES.

Signed-off-by: Kent McLeod <[email protected]>

elfloader-tool/src/arch-riscv/crt0.S

@@ -17,6 +17,7 @@
 /* SBI commands */
 #define SBI_HSM_BASE 0x48534DULL
 #define SBI_HSM_BASE_HART_START 0
+#define SBI_HSM_BASE_HART_STOP 1
 #define SBI_EXT_BASE 0x10
 #define SBI_EXT_BASE_PROBE_EXT 3
 
@@ -54,6 +55,9 @@ _start:
   mv s0, a0 /* preserve a0 (hart id) in s0 */
   mv s2, a1 /* preserve a1 (dtb) in s2 */
 
+  /* Attach the stack to sp before calling any C functions */
+  la sp, (elfloader_stack_alloc + BIT(12))
+
 #ifdef CONFIG_IMAGE_BINARY
 /* Clear the BSS before we get to do anything more specific */
   jal clear_bss
@@ -69,10 +73,12 @@ _start:
   li a6, SBI_EXT_BASE_PROBE_EXT
   li a0, SBI_HSM_BASE
   ecall /* call SBI to probe for HSM extension */
-  mv a2, a0 /* move SBI call generic return code to s2 as we need a0 */
+  mv a2, a0 /* move SBI call generic return code to a2 as we need a0 */
+  mv a3, a1 /* move SBI call error return code to a3 as we need a1 */
   mv a0, s0 /* restore a0 to hold hart ID passed by the boot loader */
+  mv a1, s2 /* restore a1 to hold dtb address passed by the boot loader */
   bnez a2, _start1 /* goto _start1 if SBI did not return SBI_SUCCESS (0) */
-  beqz a1, _start1 /* goto _start1 if HSM extension is missing */
+  beqz a3, _start1 /* goto _start1 if HSM extension is missing */
 
   /* Update global bool variable to tell boot code the HSM extension exists. */
   la t1, hsm_exists
@@ -88,8 +94,8 @@ hsm_switch_hart:
   li a7, SBI_HSM_BASE
   li a6, SBI_HSM_BASE_HART_START
   li a0, CONFIG_FIRST_HART_ID /* hart id to start */
+  mv a2, a1 /* dtb address to be passed in a1 when new hart starts is 3rd parameter */
   la a1, _start1 /* where to start the hart */
-  li a2, 0 /* logical hart_id to be passed in a1 when new hart starts */
   ecall /* call SBI to start hart FIRST_HART_ID */
 
   /* Since we are not the designated primary hart, continue the boot process as
@@ -100,16 +106,17 @@ hsm_switch_hart:
 
 
 _start1: /* a0 must hold current hard ID passed by bootloader */
-
+         /* a1 must hold dtb address passed by bootloader */
 .option push
 .option norelax
 1:auipc gp, %pcrel_hi(__global_pointer$)
   addi  gp, gp, %pcrel_lo(1b)
 .option pop
 
-  li s0, CONFIG_FIRST_HART_ID
-  bne  a0, s0, secondary_harts
-
+  /* Attach the stack to sp before calling any C functions */
+  /* This HART may be a different HART to the one that started at _start
+   * If we've switched HARTs then the other HART will get a different stack
+   * region in secondary_harts. */
   la sp, (elfloader_stack_alloc + BIT(12))
   /* The C code expects the registers to be set up as:
    *   a0 = hart id
@@ -144,7 +151,7 @@ secondary_harts:
   amoadd.w t0, t2, (a1)
   /* now a1 has the logical core id */
   li t2, CONFIG_MAX_NUM_NODES
-  bge t0, t2, spin_hart
+  bge t0, t2, hsm_suspend_hart
 
   mv a1, t0
   slli t0, t0, 12
@@ -153,6 +160,13 @@ secondary_harts:
   la s0, secondary_entry
   jr s0
 #endif
+/* If we get here then the HSM extension exists and the current
+ * HART is not going to be used and needs to be suspended. */
+hsm_suspend_hart:
+  li a7, SBI_HSM_BASE
+  li a6, SBI_HSM_BASE_HART_STOP
+  ecall /* call SBI to suspend current HART */
+
 spin_hart:
   wfi
   j spin_hart