SMP support. Mostly works, but occasionally craps out during boot.

ok drahn@

13 files changed, 502 insertions, 110 deletions

diff --git a/sys/arch/riscv64/compile/GENERIC.MP/Makefile b/sys/arch/riscv64/compile/GENERIC.MP/Makefile
new file mode 100644
index 00000000000..01b5f23410c
--- /dev/null
+++ b/sys/arch/riscv64/compile/GENERIC.MP/Makefile
@@ -0,0 +1 @@
+.include "../Makefile.inc"
diff --git a/sys/arch/riscv64/conf/GENERIC.MP b/sys/arch/riscv64/conf/GENERIC.MP
new file mode 100644
index 00000000000..1cd7770cadf
--- /dev/null
+++ b/sys/arch/riscv64/conf/GENERIC.MP
@@ -0,0 +1,9 @@
+#	$OpenBSD: GENERIC.MP,v 1.1 2021/06/29 21:27:52 kettenis Exp $
+
+include "arch/riscv64/conf/GENERIC"
+
+option	MULTIPROCESSOR
+#option	MP_LOCKDEBUG
+#option	WITNESS
+
+cpu*		at mainbus?
diff --git a/sys/arch/riscv64/dev/riscv_cpu_intc.c b/sys/arch/riscv64/dev/riscv_cpu_intc.c
index f00ae558d14..dcf4b51ba7e 100644
--- a/sys/arch/riscv64/dev/riscv_cpu_intc.c
+++ b/sys/arch/riscv64/dev/riscv_cpu_intc.c
@@ -1,4 +1,4 @@
-/*	$OpenBSD: riscv_cpu_intc.c,v 1.8 2021/05/14 06:48:52 jsg Exp $	*/
+/*	$OpenBSD: riscv_cpu_intc.c,v 1.9 2021/06/29 21:27:52 kettenis Exp $	*/
 
 /*
  * Copyright (c) 2020, Mars Li <mengshi.li.mars@gmail.com>
@@ -88,6 +88,12 @@ riscv_intc_attach(struct device *parent, struct device *self, void *aux)
 
 	riscv_intr_register_fdt(&intc_ic);
 
+#ifdef MULTIPROCESSOR
+	extern int ipi_intr(void *);
+	riscv_intc_intr_establish(IRQ_SOFTWARE_SUPERVISOR, 0,
+	    ipi_intr, NULL, NULL);
+#endif
+
 	/*
 	 * XXX right time to enable interrupts ??
 	 * might need to postpone untile autoconf is finished
diff --git a/sys/arch/riscv64/include/cpu.h b/sys/arch/riscv64/include/cpu.h
index fb4846558de..4730598a0c7 100644
--- a/sys/arch/riscv64/include/cpu.h
+++ b/sys/arch/riscv64/include/cpu.h
@@ -1,4 +1,4 @@
-/*	$OpenBSD: cpu.h,v 1.7 2021/06/02 00:39:27 cheloha Exp $	*/
+/*	$OpenBSD: cpu.h,v 1.8 2021/06/29 21:27:52 kettenis Exp $	*/
 
 /*
  * Copyright (c) 2019 Mike Larkin <mlarkin@openbsd.org>
@@ -78,25 +78,18 @@ struct cpu_info {
 	struct schedstate_percpu ci_schedstate; /* scheduler state */
 
 	u_int32_t		ci_cpuid;
-#if 0
-	uint64_t		ci_mpidr;
-	u_int			ci_acpi_proc_id;
-#endif
+	uint64_t		ci_hartid;
 	int			ci_node;
 	struct cpu_info		*ci_self;
 
 	struct proc		*ci_curproc;
 	struct pmap		*ci_curpm;
-#if 0
 	struct proc		*ci_fpuproc;
-#endif
 	u_int32_t		ci_randseed;
 
 	struct pcb		*ci_curpcb;
 	struct pcb		*ci_idle_pcb;
 
-	u_int32_t		ci_ctrl; /* The CPU control register */
-
 	uint64_t		ci_lasttb;
 	uint64_t		ci_nexttimerevent;
 	uint64_t		ci_nextstatevent;
@@ -110,25 +103,12 @@ struct cpu_info {
 #endif
 	int			ci_want_resched;
 
-	/* currently loaded fpu proc ctx */
-	struct proc		*ci_fpuproc;
-
-#if 0
-	void			(*ci_flush_bp)(void);
-
-	struct opp_table	*ci_opp_table;
-	volatile int		ci_opp_idx;
-	volatile int		ci_opp_max;
-	uint32_t		ci_cpu_supply;
-#endif
-
 #ifdef MULTIPROCESSOR
 	struct srp_hazard	ci_srp_hazards[SRP_HAZARD_NUM];
 	volatile int		ci_flags;
-#if 0
-	uint64_t		ci_ttbr1;
-	vaddr_t			ci_el1_stkend;
-#endif
+	uint64_t		ci_satp;
+	vaddr_t			ci_initstack_end;
+	int			ci_ipi_reason;
 
 	volatile int		ci_ddb_paused;
 #define CI_DDB_RUNNING		0
@@ -142,6 +122,7 @@ struct cpu_info {
 #ifdef GPROF
 	struct gmonparam	*ci_gmon;
 #endif
+
 	char			ci_panicbuf[512];
 };
 
@@ -169,6 +150,7 @@ extern struct cpu_info *cpu_info_list;
 
 #define cpu_number()	0
 #define CPU_IS_PRIMARY(ci)	1
+#define CPU_IS_RUNNING(ci)	1
 #define CPU_INFO_ITERATOR	int
 #define CPU_INFO_FOREACH(cii, ci) \
 	for (cii = 0, ci = curcpu(); ci != NULL; ci = NULL)
@@ -180,6 +162,7 @@ extern struct cpu_info *cpu_info_list;
 
 #define cpu_number()		(curcpu()->ci_cpuid)
 #define CPU_IS_PRIMARY(ci)	((ci) == &cpu_info_primary)
+#define CPU_IS_RUNNING(ci)	((ci)->ci_flags & CPUF_RUNNING)
 #define CPU_INFO_ITERATOR		int
 #define CPU_INFO_FOREACH(cii, ci)	for (cii = 0, ci = cpu_info_list; \
 					    ci != NULL; ci = ci->ci_next)
@@ -188,7 +171,9 @@ extern struct cpu_info *cpu_info_list;
 
 extern struct cpu_info *cpu_info[MAXCPUS];
 
-void cpu_boot_secondary_processors(void);
+void	cpu_boot_secondary_processors(void);
+void	cpu_startclock(void);
+
 #endif /* !MULTIPROCESSOR */
 
 #define CPU_BUSY_CYCLE()	do {} while (0)
diff --git a/sys/arch/riscv64/include/intr.h b/sys/arch/riscv64/include/intr.h
index b5501bc64ab..e0e9b8f3077 100644
--- a/sys/arch/riscv64/include/intr.h
+++ b/sys/arch/riscv64/include/intr.h
@@ -1,4 +1,4 @@
-/*	$OpenBSD: intr.h,v 1.5 2021/05/19 17:39:50 kettenis Exp $	*/
+/*	$OpenBSD: intr.h,v 1.6 2021/06/29 21:27:52 kettenis Exp $	*/
 
 /*
  * Copyright (c) 2001-2004 Opsycon AB  (www.opsycon.se / www.opsycon.com)
@@ -206,11 +206,10 @@ void	 riscv_intr_disable(void *);
 void	 riscv_intr_route(void *, int, struct cpu_info *);
 void	 riscv_intr_cpu_enable(void);
 
-void	 riscv_send_ipi(struct cpu_info *, int);
-extern void (*intr_send_ipi_func)(struct cpu_info *, int);
+#define IPI_NOP		0
+#define IPI_DDB		(1 << 0)
 
-#define riscv_IPI_NOP	0
-#define riscv_IPI_DDB	1
+void	intr_send_ipi(struct cpu_info *, int);
 
 #ifdef DIAGNOSTIC
 /*
diff --git a/sys/arch/riscv64/riscv64/cpu.c b/sys/arch/riscv64/riscv64/cpu.c
index 44c5fe83826..a6ca8927b46 100644
--- a/sys/arch/riscv64/riscv64/cpu.c
+++ b/sys/arch/riscv64/riscv64/cpu.c
@@ -1,4 +1,4 @@
-/*	$OpenBSD: cpu.c,v 1.8 2021/05/20 18:28:15 kettenis Exp $	*/
+/*	$OpenBSD: cpu.c,v 1.9 2021/06/29 21:27:53 kettenis Exp $	*/
 
 /*
  * Copyright (c) 2016 Dale Rahn <drahn@dalerahn.com>
@@ -19,11 +19,15 @@
 
 #include <sys/param.h>
 #include <sys/systm.h>
+#include <sys/atomic.h>
 #include <sys/malloc.h>
 #include <sys/device.h>
 #include <sys/sysctl.h>
 
+#include <uvm/uvm.h>
+
 #include <machine/fdt.h>
+#include <machine/sbi.h>
 
 #include <dev/ofw/openfirm.h>
 #include <dev/ofw/ofw_clock.h>
@@ -65,7 +69,7 @@ cpu_identify(struct cpu_info *ci)
 }
 
 #ifdef MULTIPROCESSOR
-int	cpu_hatch_secondary(struct cpu_info *ci, int, uint64_t);
+int	cpu_hatch_secondary(struct cpu_info *ci);
 #endif
 int	cpu_clockspeed(int *);
 
@@ -94,10 +98,11 @@ cpu_attach(struct device *parent, struct device *dev, void *aux)
 
 	KASSERT(faa->fa_nreg > 0);
 
-	if (faa->fa_reg[0].addr == boot_hart) {/* the primary cpu */
+	if (faa->fa_reg[0].addr == boot_hart) {
 		ci = &cpu_info_primary;
 #ifdef MULTIPROCESSOR
 		ci->ci_flags |= CPUF_RUNNING | CPUF_PRESENT | CPUF_PRIMARY;
+		csr_set(sie, SIE_SSIE);
 #endif
 	}
 #ifdef MULTIPROCESSOR
@@ -113,31 +118,18 @@ cpu_attach(struct device *parent, struct device *dev, void *aux)
 
 	ci->ci_dev = dev;
 	ci->ci_cpuid = dev->dv_unit;
+	ci->ci_hartid = faa->fa_reg[0].addr;
 	ci->ci_node = faa->fa_node;
 	ci->ci_self = ci;
 
 #ifdef MULTIPROCESSOR
 	if (ci->ci_flags & CPUF_AP) {
-		char buf[32];
-		uint64_t spinup_data = 0;
-		int spinup_method = 0;
 		int timeout = 10000;
-		int len;
-
-		len = OF_getprop(ci->ci_node, "enable-method",
-		    buf, sizeof(buf));
-		if (strcmp(buf, "psci") == 0) {
-			spinup_method = 1;
-		} else if (strcmp(buf, "spin-table") == 0) {
-			spinup_method = 2;
-			spinup_data = OF_getpropint64(ci->ci_node,
-			    "cpu-release-addr", 0);
-		}
 
 		sched_init_cpu(ci);
-		if (cpu_hatch_secondary(ci, spinup_method, spinup_data)) {
+		if (cpu_hatch_secondary(ci)) {
 			atomic_setbits_int(&ci->ci_flags, CPUF_IDENTIFY);
-			__asm volatile("dsb sy; sev");
+			membar_producer();
 
 			while ((ci->ci_flags & CPUF_IDENTIFIED) == 0 &&
 			    --timeout)
@@ -240,3 +232,110 @@ cpu_cache_nop_range(paddr_t pa, psize_t len)
 void (*cpu_dcache_wbinv_range)(paddr_t, psize_t) = cpu_cache_nop_range;
 void (*cpu_dcache_inv_range)(paddr_t, psize_t) = cpu_cache_nop_range;
 void (*cpu_dcache_wb_range)(paddr_t, psize_t) = cpu_cache_nop_range;
+
+#ifdef MULTIPROCESSOR
+
+void	cpu_hatch(void);
+
+void
+cpu_boot_secondary(struct cpu_info *ci)
+{
+	atomic_setbits_int(&ci->ci_flags, CPUF_GO);
+	membar_producer();
+
+	while ((ci->ci_flags & CPUF_RUNNING) == 0)
+		CPU_BUSY_CYCLE();
+}
+
+void
+cpu_boot_secondary_processors(void)
+{
+	struct cpu_info *ci;
+	CPU_INFO_ITERATOR cii;
+
+	CPU_INFO_FOREACH(cii, ci) {
+		if ((ci->ci_flags & CPUF_AP) == 0)
+			continue;
+		if (ci->ci_flags & CPUF_PRIMARY)
+			continue;
+
+		ci->ci_randseed = (arc4random() & 0x7fffffff) + 1;
+		cpu_boot_secondary(ci);
+	}
+}
+
+int
+cpu_hatch_secondary(struct cpu_info *ci)
+{
+	paddr_t start_addr, a1;
+	void *kstack;
+	int error;
+
+	kstack = km_alloc(USPACE, &kv_any, &kp_zero, &kd_waitok);
+	ci->ci_initstack_end = (vaddr_t)kstack + USPACE - 16;
+
+	pmap_extract(pmap_kernel(), (vaddr_t)cpu_hatch, &start_addr);
+	pmap_extract(pmap_kernel(), (vaddr_t)ci, &a1);
+
+	ci->ci_satp = pmap_kernel()->pm_satp;
+
+	error = sbi_hsm_hart_start(ci->ci_hartid, start_addr, a1);
+	return (error == SBI_SUCCESS);
+}
+
+void cpu_startclock(void);
+
+void
+cpu_start_secondary(void)
+{
+	struct cpu_info *ci = curcpu();
+	int s;
+
+	ci->ci_flags |= CPUF_PRESENT;
+	membar_producer();
+
+	while ((ci->ci_flags & CPUF_IDENTIFY) == 0)
+		membar_consumer();
+
+	cpu_identify(ci);
+
+	atomic_setbits_int(&ci->ci_flags, CPUF_IDENTIFIED);
+	membar_producer();
+
+	while ((ci->ci_flags & CPUF_GO) == 0)
+		membar_consumer();
+
+	s = splhigh();
+	riscv_intr_cpu_enable();
+	cpu_startclock();
+
+	csr_clear(sstatus, SSTATUS_FS_MASK);
+	csr_set(sie, SIE_SSIE);
+
+	nanouptime(&ci->ci_schedstate.spc_runtime);
+
+	atomic_setbits_int(&ci->ci_flags, CPUF_RUNNING);
+	membar_producer();
+
+	spllower(IPL_NONE);
+	intr_enable();
+
+	SCHED_LOCK(s);
+	cpu_switchto(NULL, sched_chooseproc());
+}
+
+void
+cpu_kick(struct cpu_info *ci)
+{
+	if (ci != curcpu())
+		intr_send_ipi(ci, IPI_NOP);
+}
+
+void
+cpu_unidle(struct cpu_info *ci)
+{
+	if (ci != curcpu())
+		intr_send_ipi(ci, IPI_NOP);
+}
+
+#endif
diff --git a/sys/arch/riscv64/riscv64/db_interface.c b/sys/arch/riscv64/riscv64/db_interface.c
index 82d184d974d..672444a8427 100644
--- a/sys/arch/riscv64/riscv64/db_interface.c
+++ b/sys/arch/riscv64/riscv64/db_interface.c
@@ -1,4 +1,4 @@
-/*	$OpenBSD: db_interface.c,v 1.3 2021/05/12 01:20:52 jsg Exp $	*/
+/*	$OpenBSD: db_interface.c,v 1.4 2021/06/29 21:27:53 kettenis Exp $	*/
 
 /*
  * Copyright (c) 1996 Scott K. Stevens
@@ -87,6 +87,22 @@ struct db_variable db_regs[] = {
 	{ "t6", (long *)&DDB_REGS->tf_t[6], FCN_NULL, }		/* x31 */
 };
 
+#ifdef MULTIPROCESSOR
+struct db_mutex ddb_mp_mutex = DB_MUTEX_INITIALIZER;
+volatile int ddb_state = DDB_STATE_NOT_RUNNING;
+volatile cpuid_t ddb_active_cpu;
+int		 db_switch_cpu;
+long             db_switch_to_cpu;
+
+void db_cpuinfo_cmd(db_expr_t, int, db_expr_t, char *);
+void db_startproc_cmd(db_expr_t, int, db_expr_t, char *);
+void db_stopproc_cmd(db_expr_t, int, db_expr_t, char *);
+void db_ddbproc_cmd(db_expr_t, int, db_expr_t, char *);
+void db_stopcpu(int cpu);
+void db_startcpu(int cpu);
+int db_enter_ddb(void);
+#endif
+
 extern label_t	*db_recover;
 
 struct db_variable *db_eregs = db_regs + nitems(db_regs);
@@ -100,6 +116,14 @@ kdb_trap(int type, db_regs_t *regs)
 {
 	int s;
 
+#ifdef MULTIPROCESSOR
+	db_mtx_enter(&ddb_mp_mutex);
+	if (ddb_state == DDB_STATE_EXITING)
+		ddb_state = DDB_STATE_NOT_RUNNING;
+	db_mtx_leave(&ddb_mp_mutex);
+	while (db_enter_ddb()) {
+#endif
+
 	switch (type) {
 	case T_BREAKPOINT:	/* breakpoint */
 	case -1:		/* keyboard interrupt */
@@ -123,6 +147,11 @@ kdb_trap(int type, db_regs_t *regs)
 
 	*regs = ddb_regs;
 
+#ifdef MULTIPROCESSOR
+		if (!db_switch_cpu)
+			ddb_state = DDB_STATE_EXITING;
+	}
+#endif
 	return (1);
 }
 #endif
@@ -199,8 +228,200 @@ db_enter(void)
 	asm("ebreak");
 }
 
+
+#ifdef MULTIPROCESSOR
+void
+db_cpuinfo_cmd(db_expr_t addr, int have_addr, db_expr_t count, char *modif)
+{
+	int i;
+
+	for (i = 0; i < MAXCPUS; i++) {
+		if (cpu_info[i] != NULL) {
+			db_printf("%c%4d: ", (i == cpu_number()) ? '*' : ' ',
+			    CPU_INFO_UNIT(cpu_info[i]));
+			switch(cpu_info[i]->ci_ddb_paused) {
+			case CI_DDB_RUNNING:
+				db_printf("running\n");
+				break;
+			case CI_DDB_SHOULDSTOP:
+				db_printf("stopping\n");
+				break;
+			case CI_DDB_STOPPED:
+				db_printf("stopped\n");
+				break;
+			case CI_DDB_ENTERDDB:
+				db_printf("entering ddb\n");
+				break;
+			case CI_DDB_INDDB:
+				db_printf("ddb\n");
+				break;
+			default:
+				db_printf("? (%d)\n",
+				    cpu_info[i]->ci_ddb_paused);
+				break;
+			}
+		}
+	}
+}
+
+void
+db_startproc_cmd(db_expr_t addr, int have_addr, db_expr_t count, char *modif)
+{
+	int i;
+
+	if (have_addr) {
+		if (addr >= 0 && addr < MAXCPUS &&
+		    cpu_info[addr] != NULL && addr != cpu_number())
+			db_startcpu(addr);
+		else
+			db_printf("Invalid cpu %d\n", (int)addr);
+	} else {
+		for (i = 0; i < MAXCPUS; i++) {
+			if (cpu_info[i] != NULL && i != cpu_number())
+				db_startcpu(i);
+		}
+	}
+}
+
+void
+db_stopproc_cmd(db_expr_t addr, int have_addr, db_expr_t count, char *modif)
+{
+	int i;
+
+	if (have_addr) {
+		if (addr >= 0 && addr < MAXCPUS &&
+		    cpu_info[addr] != NULL && addr != cpu_number())
+			db_stopcpu(addr);
+		else
+			db_printf("Invalid cpu %d\n", (int)addr);
+	} else {
+		for (i = 0; i < MAXCPUS; i++) {
+			if (cpu_info[i] != NULL && i != cpu_number())
+				db_stopcpu(i);
+		}
+	}
+}
+
+void
+db_ddbproc_cmd(db_expr_t addr, int have_addr, db_expr_t count, char *modif)
+{
+	if (have_addr) {
+		if (addr >= 0 && addr < MAXCPUS &&
+		    cpu_info[addr] != NULL && addr != cpu_number()) {
+			db_stopcpu(addr);
+			db_switch_to_cpu = addr;
+			db_switch_cpu = 1;
+			db_cmd_loop_done = 1;
+		} else {
+			db_printf("Invalid cpu %d\n", (int)addr);
+		}
+	} else {
+		db_printf("CPU not specified\n");
+	}
+}
+
+int
+db_enter_ddb(void)
+{
+	int i;
+
+	db_mtx_enter(&ddb_mp_mutex);
+
+	/* If we are first in, grab ddb and stop all other CPUs */
+	if (ddb_state == DDB_STATE_NOT_RUNNING) {
+		ddb_active_cpu = cpu_number();
+		ddb_state = DDB_STATE_RUNNING;
+		curcpu()->ci_ddb_paused = CI_DDB_INDDB;
+		db_mtx_leave(&ddb_mp_mutex);
+		for (i = 0; i < MAXCPUS; i++) {
+			if (cpu_info[i] != NULL && i != cpu_number() &&
+			    cpu_info[i]->ci_ddb_paused != CI_DDB_STOPPED) {
+				cpu_info[i]->ci_ddb_paused = CI_DDB_SHOULDSTOP;
+				intr_send_ipi(cpu_info[i], IPI_DDB);
+			}
+		}
+		return (1);
+	}
+
+	/* Leaving ddb completely.  Start all other CPUs and return 0 */
+	if (ddb_active_cpu == cpu_number() && ddb_state == DDB_STATE_EXITING) {
+		for (i = 0; i < MAXCPUS; i++) {
+			if (cpu_info[i] != NULL) {
+				cpu_info[i]->ci_ddb_paused = CI_DDB_RUNNING;
+			}
+		}
+		db_mtx_leave(&ddb_mp_mutex);
+		return (0);
+	}
+
+	/* We're switching to another CPU.  db_ddbproc_cmd() has made sure
+	 * it is waiting for ddb, we just have to set ddb_active_cpu. */
+	if (ddb_active_cpu == cpu_number() && db_switch_cpu) {
+		curcpu()->ci_ddb_paused = CI_DDB_SHOULDSTOP;
+		db_switch_cpu = 0;
+		ddb_active_cpu = db_switch_to_cpu;
+		cpu_info[db_switch_to_cpu]->ci_ddb_paused = CI_DDB_ENTERDDB;
+	}
+
+	/* Wait until we should enter ddb or resume */
+	while (ddb_active_cpu != cpu_number() &&
+	    curcpu()->ci_ddb_paused != CI_DDB_RUNNING) {
+		if (curcpu()->ci_ddb_paused == CI_DDB_SHOULDSTOP)
+			curcpu()->ci_ddb_paused = CI_DDB_STOPPED;
+		db_mtx_leave(&ddb_mp_mutex);
+
+		/* Busy wait without locking, we'll confirm with lock later */
+		while (ddb_active_cpu != cpu_number() &&
+		    curcpu()->ci_ddb_paused != CI_DDB_RUNNING)
+			CPU_BUSY_CYCLE();
+
+		db_mtx_enter(&ddb_mp_mutex);
+	}
+
+	/* Either enter ddb or exit */
+	if (ddb_active_cpu == cpu_number() && ddb_state == DDB_STATE_RUNNING) {
+		curcpu()->ci_ddb_paused = CI_DDB_INDDB;
+		db_mtx_leave(&ddb_mp_mutex);
+		return (1);
+	} else {
+		db_mtx_leave(&ddb_mp_mutex);
+		return (0);
+	}
+}
+
+void
+db_startcpu(int cpu)
+{
+	if (cpu != cpu_number() && cpu_info[cpu] != NULL) {
+		db_mtx_enter(&ddb_mp_mutex);
+		cpu_info[cpu]->ci_ddb_paused = CI_DDB_RUNNING;
+		db_mtx_leave(&ddb_mp_mutex);
+	}
+}
+
+void
+db_stopcpu(int cpu)
+{
+	db_mtx_enter(&ddb_mp_mutex);
+	if (cpu != cpu_number() && cpu_info[cpu] != NULL &&
+	    cpu_info[cpu]->ci_ddb_paused != CI_DDB_STOPPED) {
+		cpu_info[cpu]->ci_ddb_paused = CI_DDB_SHOULDSTOP;
+		db_mtx_leave(&ddb_mp_mutex);
+		intr_send_ipi(cpu_info[cpu], IPI_DDB);
+	} else {
+		db_mtx_leave(&ddb_mp_mutex);
+	}
+}
+#endif
+
 struct db_command db_machine_command_table[] = {
-	{ NULL, NULL, 0, NULL }
+#ifdef MULTIPROCESSOR
+	{ "cpuinfo",    db_cpuinfo_cmd,         0,      NULL },
+	{ "startcpu",   db_startproc_cmd,       0,      NULL },
+	{ "stopcpu",    db_stopproc_cmd,        0,      NULL },
+	{ "ddbcpu",     db_ddbproc_cmd,         0,      NULL },
+#endif
+	{ NULL, 	NULL, 			0,	NULL }
 };
 
 int
@@ -223,7 +444,17 @@ extern vaddr_t end;
 void
 db_machine_init(void)
 {
+#ifdef MULTIPROCESSOR
+	int i;
+#endif
+
 	db_machine_commands_install(db_machine_command_table);
+#ifdef MULTIPROCESSOR
+	for (i = 0; i < MAXCPUS; i++) {
+		if (cpu_info[i] != NULL)
+			cpu_info[i]->ci_ddb_paused = CI_DDB_RUNNING;
+	}
+#endif
 }
 
 db_addr_t
diff --git a/sys/arch/riscv64/riscv64/fpu.c b/sys/arch/riscv64/riscv64/fpu.c
index 369eced7f8b..38663f3b4c6 100644
--- a/sys/arch/riscv64/riscv64/fpu.c
+++ b/sys/arch/riscv64/riscv64/fpu.c
@@ -1,4 +1,4 @@
-/*	$OpenBSD: fpu.c,v 1.9 2021/06/19 22:11:08 kettenis Exp $	*/
+/*	$OpenBSD: fpu.c,v 1.10 2021/06/29 21:27:53 kettenis Exp $	*/
 
 /*
  * Copyright (c) 2020 Dale Rahn <drahn@openbsd.org>
@@ -32,9 +32,12 @@ fpu_clear(struct fpreg *fp)
 void
 fpu_discard(struct proc *p)
 {
-	if (p->p_addr->u_pcb.pcb_fpcpu == curcpu())
-		curcpu()->ci_fpuproc = NULL;
-	p->p_addr->u_pcb.pcb_fpcpu = NULL;
+	struct cpu_info *ci = curcpu();
+	
+	if (curpcb->pcb_fpcpu == ci && ci->ci_fpuproc == p) {
+		ci->ci_fpuproc = NULL;
+		curpcb->pcb_fpcpu = NULL;
+	}
 }
 
 void
diff --git a/sys/arch/riscv64/riscv64/genassym.cf b/sys/arch/riscv64/riscv64/genassym.cf
index 309b32a7ea7..ab6c8bd538d 100644
--- a/sys/arch/riscv64/riscv64/genassym.cf
+++ b/sys/arch/riscv64/riscv64/genassym.cf
@@ -1,4 +1,4 @@
-#	$OpenBSD: genassym.cf,v 1.3 2021/05/16 10:39:22 jsg Exp $
+#	$OpenBSD: genassym.cf,v 1.4 2021/06/29 21:27:53 kettenis Exp $
 #
 # Copyright (c) 2020 Brian Bamsch <bbamsch@google.com>
 # All rights reserved.
@@ -84,6 +84,8 @@ member	ci_want_resched
 ifdef MULTIPROCESSOR
 member	ci_srp_hazards
 member	ci_flags
+member	ci_satp
+member	ci_initstack_end
 member	ci_ddb_paused
 endif
 ifdef GPROF
diff --git a/sys/arch/riscv64/riscv64/intr.c b/sys/arch/riscv64/riscv64/intr.c
index 1e519b69e47..23f77d230a7 100644
--- a/sys/arch/riscv64/riscv64/intr.c
+++ b/sys/arch/riscv64/riscv64/intr.c
@@ -1,4 +1,4 @@
-/*	$OpenBSD: intr.c,v 1.7 2021/05/19 17:39:50 kettenis Exp $	*/
+/*	$OpenBSD: intr.c,v 1.8 2021/06/29 21:27:53 kettenis Exp $	*/
 
 /*
  * Copyright (c) 2011 Dale Rahn <drahn@openbsd.org>
@@ -18,10 +18,12 @@
 
 #include <sys/param.h>
 #include <sys/systm.h>
+#include <sys/atomic.h>
 #include <sys/malloc.h>
 
 #include <machine/cpu.h>
 #include <machine/intr.h>
+#include <machine/sbi.h>
 
 #include <dev/ofw/openfirm.h>
 
@@ -713,17 +715,36 @@ intr_barrier(void *cookie)
  * IPI implementation
  */
 
-void riscv_no_send_ipi(struct cpu_info *ci, int id);
-void (*intr_send_ipi_func)(struct cpu_info *, int) = riscv_no_send_ipi;
+#ifdef MULTIPROCESSOR
 
 void
-riscv_send_ipi(struct cpu_info *ci, int id)
+intr_send_ipi(struct cpu_info *ci, int reason)
 {
-	(*intr_send_ipi_func)(ci, id);
+	unsigned long hart_mask;
+
+	if (ci == curcpu() && reason == IPI_NOP)
+		return;
+
+	if (reason != IPI_NOP)
+		atomic_setbits_int(&ci->ci_ipi_reason, reason);
+
+	hart_mask = (1UL << ci->ci_hartid);
+	sbi_send_ipi(&hart_mask);
 }
 
-void
-riscv_no_send_ipi(struct cpu_info *ci, int id)
+int
+ipi_intr(void *frame)
 {
-	panic("riscv_send_ipi() called: no ipi function");
+	struct cpu_info *ci = curcpu();
+	int pending;
+
+	csr_clear(sip, SIP_SSIP);
+	pending = atomic_swap_uint(&ci->ci_ipi_reason, IPI_NOP);
+
+	if (pending & IPI_DDB)
+		db_enter();
+
+	return 1;
 }
+
+#endif
diff --git a/sys/arch/riscv64/riscv64/locore.S b/sys/arch/riscv64/riscv64/locore.S
index f804235e6b9..0916f401eb4 100644
--- a/sys/arch/riscv64/riscv64/locore.S
+++ b/sys/arch/riscv64/riscv64/locore.S
@@ -1,4 +1,4 @@
-/*	$OpenBSD: locore.S,v 1.11 2021/06/29 12:22:39 kettenis Exp $	*/
+/*	$OpenBSD: locore.S,v 1.12 2021/06/29 21:27:53 kettenis Exp $	*/
 
 /*-
  * Copyright (c) 2015-2018 Ruslan Bukin <br@bsdpad.com>
@@ -263,6 +263,17 @@ va:
 	call	_C_LABEL(initriscv)	// Off we go, defined in machdep.c
 	call	_C_LABEL(main)		//defined in openbsd/kern/init_main.c
 
+/*
+ * Get the physical address the kernel is loaded to. Returned in s9.
+ */
+get_physmem:
+	lla	t0, virt_map	/* physical address of virt_map */
+	ld	t1, 0(t0)	/* virtual address of virt_map */
+	sub	t1, t1, t0	/* calculate phys->virt delta */
+	li	t2, KERNBASE
+	sub	s9, t2, t1	/* s9 = physmem base */
+	ret
+
 	.data
 	.align	4
 initstack:
@@ -317,44 +328,31 @@ init_pt_va:
 	.quad pagetable_l2	/* XXX: Keep page tables VA */
 
 #ifdef MULTIPROCESSOR
-/*
- * mpentry(unsigned long)
- *
- * Called by a core / hart when it is being brought online.
- * XXX: [CMPE] This needs to be updated
- */
-ENTRY(mpentry)
-	/*
-	 * Calculate the offset to __riscv_boot_ap
-	 * for the current core, cpuid is in a0.
-	 */
-	li	t1, 4			//t1 = 4, each core occupies a word
-	mulw	t1, t1, a0		//t1 = a0*4, offset in Bytes for #a0 core
-	/* Get the pointer */
-	lla	t0, __riscv_boot_ap
-	add	t0, t0, t1		//t0 = starting addr for current core
 
-1:
-	/* Wait the kernel to be ready */
-	lw	t1, 0(t0)		//when kernel is ready, 0(t0) should NOT euqal 0
-	beqz	t1, 1b			//see __riscv_boot_ap
-
-	/* Setup stack pointer */	//now kernel is ready
-	lla	t0, secondary_stacks	//pa, size: #core x #pages/kernel x pg_size
-	li	t1, (PAGE_SIZE * KSTACK_PAGES)	// size of kernel stack for one core
-	mulw	t2, t1, a0			//offset for this hart
-	add	t0, t0, t2			//end of stack for this hart
-	add	t0, t0, t1			//start of stack for this hart
-	sub	t0, t0, s9			//s9 is phymem base, t0 is now relative addr
-	li	t1, KERNBASE			//t1 is virtual addr
-	add	sp, t0, t1			//now sp is set to the right virtual address.
+	.text
+	.globl cpu_hatch
+cpu_hatch:
+	ld	tp, CI_SELF(a1)
+	ld	a2, CI_SATP(a1)
+
+	/* Set the global pointer */
+.option push
+.option norelax
+	lla	gp, __global_pointer$
+.option pop
+
+	/* Setup stack pointer */
+	ld	sp, CI_INITSTACK_END(a1)
+
+	/* Get the kernel's load address */
+	jal	get_physmem
 
 	/* Setup supervisor trap vector */
-	lla	t0, mpva			//mpva is phymem addr of the handler array
-	sub	t0, t0, s9			//get the relative addr
+	lla	t0, mpva
+	sub	t0, t0, s9
 	li	t1, KERNBASE
-	add	t0, t0, t1			//get the virtual addr
-	csrw	stvec, t0			//set the CSR
+	add	t0, t0, t1
+	csrw	stvec, t0
 
 	/* Set page tables base register */
 	lla	s2, pagetable_l1
@@ -378,8 +376,12 @@ mpva:
 
 	/* Ensure sscratch is zero */
 	li	t0, 0
-	csrw	sscratch, t0		//Scratch Register for Supervisor Mode Trap Handler
+	csrw	sscratch, t0
+
+	/* Switch to real kernel page tables */
+	csrw	satp, a2
+	sfence.vma
+
+	call	_C_LABEL(cpu_start_secondary)
 
-	call	init_secondary
-END(mpentry)
 #endif
diff --git a/sys/arch/riscv64/riscv64/pmap.c b/sys/arch/riscv64/riscv64/pmap.c
index 3de3af22bfe..7418fdd2bc6 100644
--- a/sys/arch/riscv64/riscv64/pmap.c
+++ b/sys/arch/riscv64/riscv64/pmap.c
@@ -1,4 +1,4 @@
-/*	$OpenBSD: pmap.c,v 1.12 2021/05/18 12:26:31 deraadt Exp $	*/
+/*	$OpenBSD: pmap.c,v 1.13 2021/06/29 21:27:53 kettenis Exp $	*/
 
 /*
  * Copyright (c) 2019-2020 Brian Bamsch <bbamsch@google.com>
@@ -44,6 +44,20 @@ void pmap_free_asid(pmap_t);
 static inline void
 tlb_flush(pmap_t pm, vaddr_t va)
 {
+#ifdef MULTIPTOCESSOR
+	CPU_INFO_ITERATOR cii;
+	struct cpu_info *ci;
+	unsigned long hart_mask = 0;
+
+	CPU_INFO_FOREACH(cii, ci) {
+		if (ci == curcpu())
+			continue;
+		hart_mask |= (1UL << ci->ci_hartid);
+	}
+
+	sbi_remote_sfence_vma(&hart_mask, va, PAGE_SIZE);
+#endif
+
 	if (pm == pmap_kernel()) {
 		// Flush Translations for VA across all ASIDs
 		cpu_tlb_flush_page_all(va);
@@ -54,6 +68,25 @@ tlb_flush(pmap_t pm, vaddr_t va)
 	}
 }
 
+static inline void
+icache_flush(void)
+{
+#ifdef MULTIPROCESSOR
+	CPU_INFO_ITERATOR cii;
+	struct cpu_info *ci;
+	unsigned long hart_mask = 0;
+
+	CPU_INFO_FOREACH(cii, ci) {
+		if (ci == curcpu())
+			continue;
+		hart_mask |= (1UL << ci->ci_hartid);
+	}
+
+#endif
+
+	fence_i();
+}
+
 struct pmap kernel_pmap_;
 struct pmap pmap_tramp;
 
@@ -518,7 +551,7 @@ pmap_enter(pmap_t pm, vaddr_t va, paddr_t pa, vm_prot_t prot, int flags)
 		need_sync = ((pg->pg_flags & PG_PMAP_EXE) == 0);
 		atomic_setbits_int(&pg->pg_flags, PG_PMAP_EXE);
 		if (need_sync)
-			fence_i();
+			icache_flush();
 	}
 
 	error = 0;
@@ -1570,7 +1603,7 @@ pmap_init(void)
 void
 pmap_proc_iflush(struct process *pr, vaddr_t va, vsize_t len)
 {
-	fence_i();
+	icache_flush();
 }
 
 void
@@ -1765,7 +1798,7 @@ pmap_fault_fixup(pmap_t pm, vaddr_t va, vm_prot_t ftype)
 		need_sync = ((pg->pg_flags & PG_PMAP_EXE) == 0);
 		atomic_setbits_int(&pg->pg_flags, PG_PMAP_EXE);
 		if (need_sync)
-			fence_i();
+			icache_flush();
 	}
 
 	retcode = 1;
diff --git a/sys/arch/riscv64/riscv64/sig_machdep.c b/sys/arch/riscv64/riscv64/sig_machdep.c
index ec9c0393e27..e865a9eff1c 100644
--- a/sys/arch/riscv64/riscv64/sig_machdep.c
+++ b/sys/arch/riscv64/riscv64/sig_machdep.c
@@ -1,4 +1,4 @@
-/*	$OpenBSD: sig_machdep.c,v 1.6 2021/06/21 14:39:05 deraadt Exp $	*/
+/*	$OpenBSD: sig_machdep.c,v 1.7 2021/06/29 21:27:53 kettenis Exp $	*/
 
 /*
  * Copyright (c) 1990 The Regents of the University of California.
@@ -156,7 +156,8 @@ sendsig(sig_t catcher, int sig, sigset_t mask, const siginfo_t *ksip)
 	/* Save signal mask. */
 	frame.sf_sc.sc_mask = mask;
 
-	if (p->p_addr->u_pcb.pcb_flags & PCB_FPU) {
+	if (p->p_addr->u_pcb.pcb_flags & PCB_FPU &&
+	    (tf->tf_sstatus & SSTATUS_FS_MASK) == SSTATUS_FS_DIRTY) {
 		fpu_save(p, tf);
 		fpreg = &p->p_addr->u_pcb.pcb_fpstate;
 		for (i=0; i < 32; i++) {