From f2e299880ad4c594420057e3dafeba5ff1145353 Mon Sep 17 00:00:00 2001
From: Ruslan Bukin <br@FreeBSD.org>
Date: Tue, 12 Jun 2018 16:47:33 +0000
Subject: [PATCH] Release secondary cores from WFI (wait for interrupt) by
 sending them an IPI.

This does not work however yet in QEMU. As a temporary workaround set
software interrupt pending bit manually on a local core to ensure WFI
doesn't halt the hart.

This is required to smpboot in QEMU.

Sponsored by:	DARPA, AFRL
---
 sys/riscv/riscv/mp_machdep.c | 14 ++++++++++++++
 1 file changed, 14 insertions(+)

diff --git a/sys/riscv/riscv/mp_machdep.c b/sys/riscv/riscv/mp_machdep.c
index adad7b4b533b..389ed9bb16d3 100644
--- a/sys/riscv/riscv/mp_machdep.c
+++ b/sys/riscv/riscv/mp_machdep.c
@@ -181,6 +181,7 @@ riscv64_cpu_attach(device_t dev)
 static void
 release_aps(void *dummy __unused)
 {
+	uintptr_t mask;
 	int cpu, i;
 
 	if (mp_ncpus == 1)
@@ -191,6 +192,14 @@ release_aps(void *dummy __unused)
 
 	atomic_store_rel_int(&aps_ready, 1);
 
+	/* Wake up the other CPUs */
+	mask = 0;
+
+	for (i = 1; i < mp_ncpus; i++)
+		mask |= (1 << i);
+
+	sbi_send_ipi(&mask);
+
 	printf("Release APs\n");
 
 	for (i = 0; i < 2000; i++) {
@@ -217,6 +226,11 @@ init_secondary(uint64_t cpu)
 	pcpup = &__pcpu[cpu];
 	__asm __volatile("mv gp, %0" :: "r"(pcpup));
 
+	/* Workaround: make sure wfi doesn't halt the hart */
+	intr_disable();
+	csr_set(sie, SIE_SSIE);
+	csr_set(sip, SIE_SSIE);
+
 	/* Spin until the BSP releases the APs */
 	while (!aps_ready)
 		__asm __volatile("wfi");