Index: sys/riscv/riscv/mp_machdep.c
===================================================================
--- sys/riscv/riscv/mp_machdep.c
+++ sys/riscv/riscv/mp_machdep.c
@@ -97,6 +97,7 @@
 #endif
 static device_t cpu_list[MAXCPU];
 
+void mpentry(u_long hartid);
 void init_secondary(uint64_t);
 
 static struct mtx ap_boot_mtx;
@@ -297,7 +298,7 @@
 	struct pcpu *pc;
 	int cpu;
 
-	for (cpu = 1; cpu < mp_ncpus; cpu++) {
+	for (cpu = 1; cpu <= mp_maxid; cpu++) {
 		if (bootstacks[cpu] != NULL) {
 			pc = pcpu_find(cpu);
 			while (atomic_load_ptr(&pc->pc_curpcb) == NULL)
@@ -399,9 +400,11 @@
 cpu_init_fdt(u_int id, phandle_t node, u_int addr_size, pcell_t *reg)
 {
 	struct pcpu *pcpup;
+	vm_paddr_t start_addr;
 	uint64_t hart;
 	u_int cpuid;
 	int naps;
+	int error;
 
 	/* Check if this hart supports MMU. */
 	if (OF_getproplen(node, "mmu-type") < 0)
@@ -441,6 +444,27 @@
 	if (cpuid > mp_maxid)
 		return (0);
 
+	/*
+	 * Depending on the SBI implementation, APs are waiting either in
+	 * locore.S or to be activated explicitly, via SBI call.
+	 */
+	if (sbi_probe_extension(SBI_EXT_ID_HSM) != 0) {
+		start_addr = pmap_kextract((vm_offset_t)mpentry);
+		error = sbi_hsm_hart_start(hart, start_addr, 0);
+		if (error != 0) {
+			/* Do not proceed for INVARIANTS kernels. */
+			KASSERT(false,
+			    ("AP %u (hart %lu) failed to start, error %d\n",
+			    cpuid, hart, error));
+			mp_ncpus--;
+
+			/* Send a warning to the user and continue. */
+			printf("AP %u (hart %lu) failed to start, error %d\n",
+			    cpuid, hart, error);
+			return (0);
+		}
+	}
+
 	pcpup = &__pcpu[cpuid];
 	pcpu_init(pcpup, cpuid, sizeof(struct pcpu));
 	pcpup->pc_hart = hart;