4 files changed, 16 insertions, 8 deletions

diff --git a/src/arch/riscv/boot.c b/src/arch/riscv/boot.c
index e1dc61955b..04fba07234 100644
--- a/src/arch/riscv/boot.c
+++ b/src/arch/riscv/boot.c
@@ -19,6 +19,7 @@
 #include <arch/encoding.h>
 #include <rules.h>
 #include <console/console.h>
+#include <arch/smp/smp.h>
 
 /*
  * A pointer to the Flattened Device Tree passed to coreboot by the boot ROM.
@@ -28,7 +29,7 @@
  */
 const void *rom_fdt;
 
-void arch_prog_run(struct prog *prog)
+static void do_arch_prog_run(struct prog *prog)
 {
 	void (*doit)(void *) = prog_entry(prog);
 	void riscvpayload(const void *fdt, void *payload);
@@ -48,3 +49,8 @@ void arch_prog_run(struct prog *prog)
 
 	doit(prog_entry_arg(prog));
 }
+
+void arch_prog_run(struct prog *prog)
+{
+	smp_resume((void (*)(void *))do_arch_prog_run, prog);
+}
diff --git a/src/arch/riscv/bootblock.S b/src/arch/riscv/bootblock.S
index 277c3910c0..7f84215dac 100644
--- a/src/arch/riscv/bootblock.S
+++ b/src/arch/riscv/bootblock.S
@@ -24,13 +24,6 @@
 .global _estack
 .globl _start
 _start:
-	csrr a0, mhartid
-	li a3, 0
-	beq a0, a3, _hart_zero
-_hart_loop:
-	j _hart_loop
-_hart_zero:
-
 	# The boot ROM may pass the following arguments to coreboot:
 	#   a0: the value of mhartid
 	#   a1: a pointer to the flattened devicetree
@@ -59,6 +52,9 @@ _hart_zero:
 	csrr a0, mhartid
 	call hls_init
 
+	li   a0, CONFIG_RISCV_WORKING_HARTID
+	call smp_pause
+
 	# initialize entry of interrupt/exception
 	la t0, trap_entry
 	csrw mtvec, t0
diff --git a/src/arch/riscv/ramstage.S b/src/arch/riscv/ramstage.S
index c721a126c4..115a55f290 100644
--- a/src/arch/riscv/ramstage.S
+++ b/src/arch/riscv/ramstage.S
@@ -35,6 +35,9 @@ _start:
 	csrr a0, mhartid
 	call hls_init
 
+	li   a0, CONFIG_RISCV_WORKING_HARTID
+	call smp_pause
+
 	# initialize entry of interrupt/exception
 	la   t0, trap_entry
 	csrw mtvec, t0
diff --git a/src/arch/riscv/stages.c b/src/arch/riscv/stages.c
index 80754762bd..4fe040934e 100644
--- a/src/arch/riscv/stages.c
+++ b/src/arch/riscv/stages.c
@@ -27,10 +27,13 @@
 #include <arch/boot.h>
 #include <arch/encoding.h>
 #include <arch/stages.h>
+#include <arch/smp/smp.h>
 #include <rules.h>
 
 void stage_entry(void)
 {
+	smp_pause(CONFIG_RISCV_WORKING_HARTID);
+
 	/*
 	 * Save the FDT pointer before entering ramstage, because mscratch
 	 * might be overwritten in the trap handler, and there is code in