lib: Implement hart hotplug

This patch adds support for hart hotplug in OpenSBI using a generic WFI
based approach. Hart hotplug can be achieved via SBI HSM extension which
allows supervisor mode software to start or stop any harts anytime.

Any platform wishes to implement platform specific hart hotplug must
implement both hart_start and hart_stop in addition to enable platform
feature SBI_PLATFORM_HAS_HART_HOTPLUG.

Signed-off-by: Atish Patra <atish.patra@wdc.com>
Reviewed-by: Anup Patel <anup.patel@wdc.com>

10 files changed, 343 insertions, 2 deletions

diff --git a/include/sbi/sbi_error.h b/include/sbi/sbi_error.h
index 4b7dd12..91ba2ee 100644
--- a/include/sbi/sbi_error.h
+++ b/include/sbi/sbi_error.h
@@ -28,6 +28,7 @@
 #define SBI_ETRAP	-13
 #define SBI_EUNKNOWN	-14
 #define SBI_ENOENT	-15
+#define SBI_EALREADY_STARTED	-16
 
 /* clang-format on */
 
diff --git a/include/sbi/sbi_hart.h b/include/sbi/sbi_hart.h
index 065cb0c..d88c7de 100644
--- a/include/sbi/sbi_hart.h
+++ b/include/sbi/sbi_hart.h
@@ -22,6 +22,8 @@ void sbi_hart_set_trap_info(struct sbi_scratch *scratch, void *data);
 
 void sbi_hart_delegation_dump(struct sbi_scratch *scratch);
 void sbi_hart_pmp_dump(struct sbi_scratch *scratch);
+int  sbi_hart_pmp_check_addr(struct sbi_scratch *scratch, unsigned long daddr,
+			     unsigned long attr);
 
 void __attribute__((noreturn)) sbi_hart_hang(void);
 
diff --git a/include/sbi/sbi_hsm.h b/include/sbi/sbi_hsm.h
new file mode 100644
index 0000000..2566afb
--- /dev/null
+++ b/include/sbi/sbi_hsm.h
@@ -0,0 +1,30 @@
+/*
+ * SPDX-License-Identifier: BSD-2-Clause
+ *
+ * Copyright (c) 2020 Western Digital Corporation or its affiliates.
+ *
+ * Authors:
+ *   Atish Patra <atish.patra@wdc.com>
+ */
+
+#ifndef __SBI_HSM_H__
+#define __SBI_HSM_H__
+
+#include <sbi/sbi_types.h>
+
+/** Hart state values **/
+#define SBI_HART_STOPPED	0
+#define SBI_HART_STOPPING	1
+#define SBI_HART_STARTING	2
+#define SBI_HART_STARTED	3
+
+int sbi_hsm_init(struct sbi_scratch *scratch, u32 hartid, bool cold_boot);
+void __noreturn sbi_hsm_exit(struct sbi_scratch *scratch);
+
+int sbi_hsm_hart_start(struct sbi_scratch *scratch, u32 hartid,
+			   ulong saddr, ulong priv);
+int sbi_hsm_hart_stop(struct sbi_scratch *scratch, bool exitnow);
+int sbi_hsm_hart_get_state(struct sbi_scratch *scratch, u32 hartid);
+bool sbi_hsm_hart_started(struct sbi_scratch *scratch, u32 hartid);
+void sbi_hsm_prepare_next_jump(struct sbi_scratch *scratch, u32 hartid);
+#endif
diff --git a/include/sbi/sbi_platform.h b/include/sbi/sbi_platform.h
index 16ee05c..462006e 100644
--- a/include/sbi/sbi_platform.h
+++ b/include/sbi/sbi_platform.h
@@ -135,6 +135,14 @@ struct sbi_platform_operations {
 	/** Exit platform timer for current HART */
 	void (*timer_exit)(void);
 
+	/** Bringup the given hart from previous stage **/
+	int (*hart_start)(u32 hartid, ulong saddr, ulong priv);
+	/**
+	 *  Stop the current hart from running. This call doesn't expect to
+	 *  return if success.
+	 */
+	int (*hart_stop)(void);
+
 	/** Reboot the platform */
 	int (*system_reboot)(u32 type);
 	/** Shutdown or poweroff the platform */
@@ -285,6 +293,41 @@ static inline u32 sbi_platform_hart_stack_size(const struct sbi_platform *plat)
 }
 
 /**
+ * Bringup a given hart from previous stage. Platform should implement this
+ * operation if they support a custom mechanism to start a hart. Otherwise,
+ * a generic WFI based approach will be used to start/stop a hart in OpenSBI.
+ *
+ * @param plat pointer to struct sbi_platform
+ * @param hartid Hart ID
+ * @param saddr  Physical address in supervisor mode for hart to jump after
+ *		 OpenSBI
+ * @param priv	 A private context data from the caller
+ *
+ * @return 0 if sucessful and negative error code on failure
+ */
+static inline int sbi_platform_hart_start(const struct sbi_platform *plat,
+					  u32 hartid, ulong saddr, ulong priv)
+{
+	if (plat && sbi_platform_ops(plat)->hart_start)
+		return sbi_platform_ops(plat)->hart_start(hartid, saddr, priv);
+	return SBI_ENOTSUPP;
+}
+
+/**
+ * Stop the current hart in OpenSBI.
+ *
+ * @param plat pointer to struct sbi_platform
+ *
+ * @return Negative error code on failure. It doesn't return on success.
+ */
+static inline int sbi_platform_hart_stop(const struct sbi_platform *plat)
+{
+	if (plat && sbi_platform_ops(plat)->hart_stop)
+		return sbi_platform_ops(plat)->hart_stop();
+	return SBI_ENOTSUPP;
+}
+
+/**
  * Early initialization for current HART
  *
  * @param plat pointer to struct sbi_platform
diff --git a/lib/sbi/objects.mk b/lib/sbi/objects.mk
index fac980f..dc52a9f 100644
--- a/lib/sbi/objects.mk
+++ b/lib/sbi/objects.mk
@@ -22,6 +22,7 @@ libsbi-objs-y += sbi_emulate_csr.o
 libsbi-objs-y += sbi_fifo.o
 libsbi-objs-y += sbi_hfence.o
 libsbi-objs-y += sbi_hart.o
+libsbi-objs-y += sbi_hsm.o
 libsbi-objs-y += sbi_illegal_insn.o
 libsbi-objs-y += sbi_init.o
 libsbi-objs-y += sbi_ipi.o
diff --git a/lib/sbi/sbi_hart.c b/lib/sbi/sbi_hart.c
index 1d62862..3de46a2 100644
--- a/lib/sbi/sbi_hart.c
+++ b/lib/sbi/sbi_hart.c
@@ -166,6 +166,31 @@ void sbi_hart_pmp_dump(struct sbi_scratch *scratch)
 	}
 }
 
+int sbi_hart_pmp_check_addr(struct sbi_scratch *scratch, unsigned long addr,
+			    unsigned long attr)
+{
+	unsigned long prot, size, l2l, i, tempaddr;
+	const struct sbi_platform *plat = sbi_platform_ptr(scratch);
+
+	if (!sbi_platform_has_pmp(plat))
+		return SBI_OK;
+
+	for (i = 0; i < PMP_COUNT; i++) {
+		pmp_get(i, &prot, &tempaddr, &l2l);
+		if (!(prot & PMP_A))
+			continue;
+		if (l2l < __riscv_xlen)
+			size = (1UL << l2l);
+		else
+			size = 0;
+		if (tempaddr <= addr && addr <= tempaddr + size)
+			if (!(prot & attr))
+				return SBI_INVALID_ADDR;
+	}
+
+	return SBI_OK;
+}
+
 static int pmp_init(struct sbi_scratch *scratch, u32 hartid)
 {
 	u32 i, count;
diff --git a/lib/sbi/sbi_hsm.c b/lib/sbi/sbi_hsm.c
new file mode 100644
index 0000000..f3207ee
--- /dev/null
+++ b/lib/sbi/sbi_hsm.c
@@ -0,0 +1,222 @@
+/*
+ * SPDX-License-Identifier: BSD-2-Clause
+ *
+ * Copyright (c) 2020 Western Digital Corporation or its affiliates.
+ *
+ * Authors:
+ *   Atish Patra <atish.patra@wdc.com>
+ */
+
+#include <sbi/riscv_asm.h>
+#include <sbi/riscv_barrier.h>
+#include <sbi/riscv_encoding.h>
+#include <sbi/riscv_atomic.h>
+#include <sbi/sbi_bits.h>
+#include <sbi/sbi_console.h>
+#include <sbi/sbi_error.h>
+#include <sbi/sbi_hart.h>
+#include <sbi/sbi_hsm.h>
+#include <sbi/sbi_init.h>
+#include <sbi/sbi_ipi.h>
+#include <sbi/sbi_platform.h>
+#include <sbi/sbi_system.h>
+#include <sbi/sbi_timer.h>
+#include <sbi/sbi_console.h>
+
+static unsigned long hart_data_offset;
+
+/** Per hart specific data to manage state transition **/
+struct sbi_hsm_data {
+	atomic_t state;
+};
+
+int sbi_hsm_hart_get_state(struct sbi_scratch *scratch, u32 hartid)
+{
+	struct sbi_hsm_data *hdata;
+	u32 hstate;
+
+	if (hartid != sbi_current_hartid())
+		scratch = sbi_hart_id_to_scratch(scratch, hartid);
+
+	hdata = sbi_scratch_offset_ptr(scratch, hart_data_offset);
+	hstate = atomic_read(&hdata->state);
+
+	return hstate;
+}
+
+int sbi_hsm_hart_started(struct sbi_scratch *scratch, u32 hartid)
+{
+
+	if (sbi_hsm_hart_get_state(scratch, hartid) == SBI_HART_STARTED)
+		return TRUE;
+	else
+		return FALSE;
+}
+
+void sbi_hsm_prepare_next_jump(struct sbi_scratch *scratch, u32 hartid)
+{
+	u32 oldstate;
+	struct sbi_hsm_data *hdata = sbi_scratch_offset_ptr(scratch,
+							hart_data_offset);
+
+	oldstate = arch_atomic_cmpxchg(&hdata->state, SBI_HART_STARTING,
+				       SBI_HART_STARTED);
+	if (oldstate != SBI_HART_STARTING)
+		sbi_hart_hang();
+}
+
+static void sbi_hsm_hart_wait(struct sbi_scratch *scratch, u32 hartid)
+{
+	unsigned long saved_mie;
+	const struct sbi_platform *plat = sbi_platform_ptr(scratch);
+	struct sbi_hsm_data *hdata = sbi_scratch_offset_ptr(scratch,
+							hart_data_offset);
+	/* Save MIE CSR */
+	saved_mie = csr_read(CSR_MIE);
+
+	/* Set MSIE bit to receive IPI */
+	csr_set(CSR_MIE, MIP_MSIP);
+
+	/* Wait for hart_add call*/
+	while (atomic_read(&hdata->state) != SBI_HART_STARTING) {
+		wfi();
+	};
+
+	/* Restore MIE CSR */
+	csr_write(CSR_MIE, saved_mie);
+
+	/* Clear current HART IPI */
+	sbi_platform_ipi_clear(plat, hartid);
+}
+
+int sbi_hsm_init(struct sbi_scratch *scratch, u32 hartid, bool cold_boot)
+{
+	struct sbi_scratch *rscratch;
+	struct sbi_hsm_data *hdata;
+	u32 hart_count, i;
+	const struct sbi_platform *plat = sbi_platform_ptr(scratch);
+
+	if (cold_boot) {
+		hart_data_offset = sbi_scratch_alloc_offset(sizeof(*hdata),
+								    "HART_DATA");
+		if (!hart_data_offset)
+			return SBI_ENOMEM;
+		hart_count = sbi_platform_hart_count(plat);
+
+		/* Initialize hart state data for every hart */
+		for (i = 0; i < hart_count; i++) {
+			rscratch = sbi_hart_id_to_scratch(scratch, i);
+			hdata = sbi_scratch_offset_ptr(rscratch,
+						       hart_data_offset);
+			ATOMIC_INIT(&hdata->state,
+			(i == hartid) ? SBI_HART_STARTING : SBI_HART_STOPPED);
+		}
+	} else {
+		sbi_hsm_hart_wait(scratch, hartid);
+	}
+
+	return 0;
+}
+
+void __noreturn sbi_hsm_exit(struct sbi_scratch *scratch)
+{
+	u32 hstate;
+	const struct sbi_platform *plat = sbi_platform_ptr(scratch);
+	struct sbi_hsm_data *hdata = sbi_scratch_offset_ptr(scratch,
+							hart_data_offset);
+	void (*jump_warmboot)(void) = (void (*)(void))scratch->warmboot_addr;
+
+	hstate = arch_atomic_cmpxchg(&hdata->state, SBI_HART_STOPPING,
+				     SBI_HART_STOPPED);
+	if (hstate != SBI_HART_STOPPING)
+		goto fail_exit;
+
+	if (sbi_platform_has_hart_hotplug(plat)) {
+		sbi_platform_hart_stop(plat);
+		/* It should never reach here */
+		goto fail_exit;
+	}
+
+	/**
+	 * As platform is lacking support for hotplug, directly jump to warmboot
+	 * and wait for interrupts in warmboot. We do it preemptively in order
+	 * preserve the hart states and reuse the code path for hotplug.
+	 */
+	jump_warmboot();
+
+fail_exit:
+	/* It should never reach here */
+	sbi_printf("ERR: Failed stop hart [%u]\n", sbi_current_hartid());
+	sbi_hart_hang();
+}
+
+int sbi_hsm_hart_start(struct sbi_scratch *scratch, u32 hartid,
+		       ulong saddr, ulong priv)
+{
+	unsigned long init_count;
+	unsigned int hstate;
+	int rc;
+	const struct sbi_platform *plat = sbi_platform_ptr(scratch);
+	struct sbi_scratch *rscratch = sbi_hart_id_to_scratch(scratch, hartid);
+	struct sbi_hsm_data *hdata = sbi_scratch_offset_ptr(rscratch,
+							hart_data_offset);
+
+	if (sbi_platform_hart_disabled(plat, hartid))
+		return SBI_EINVAL;
+	hstate = arch_atomic_cmpxchg(&hdata->state, SBI_HART_STOPPED,
+				     SBI_HART_STARTING);
+	if (hstate == SBI_HART_STARTED)
+		return SBI_EALREADY_STARTED;
+
+	/**
+	 * if a hart is already transition to start or stop, another start call
+	 * is considered as invalid request.
+	 */
+	if (hstate != SBI_HART_STOPPED)
+		return SBI_EINVAL;
+
+	rc = sbi_hart_pmp_check_addr(scratch, saddr, PMP_X);
+	if (rc)
+		return rc;
+	//TODO: We also need to check saddr for valid physical address as well.
+
+	init_count = sbi_init_count(hartid);
+	rscratch->next_arg1 = priv;
+	rscratch->next_addr = saddr;
+
+	if (sbi_platform_has_hart_hotplug(plat) ||
+	   (sbi_platform_has_hart_secondary_boot(plat) && !init_count)) {
+		return sbi_platform_hart_start(plat, hartid,
+					     scratch->warmboot_addr, priv);
+	} else {
+		sbi_platform_ipi_send(plat, hartid);
+	}
+
+	return 0;
+}
+
+int sbi_hsm_hart_stop(struct sbi_scratch *scratch, bool exitnow)
+{
+	int oldstate;
+	u32 hartid = sbi_current_hartid();
+	const struct sbi_platform *plat = sbi_platform_ptr(scratch);
+	struct sbi_hsm_data *hdata = sbi_scratch_offset_ptr(scratch,
+							hart_data_offset);
+
+	if (sbi_platform_hart_disabled(plat, hartid) ||
+	    !sbi_hsm_hart_started(scratch, hartid))
+		return SBI_EINVAL;
+
+	oldstate = arch_atomic_cmpxchg(&hdata->state, SBI_HART_STARTED,
+				     SBI_HART_STOPPING);
+	if (oldstate != SBI_HART_STARTED) {
+		sbi_printf("%s: ERR: The hart is in invalid state [%u]\n",
+			   __func__, oldstate);
+		return SBI_DENIED;
+	}
+
+	if (exitnow)
+		sbi_exit(scratch);
+
+	return 0;
+}
diff --git a/lib/sbi/sbi_init.c b/lib/sbi/sbi_init.c
index 48bc653..1f113b9 100644
--- a/lib/sbi/sbi_init.c
+++ b/lib/sbi/sbi_init.c
@@ -12,6 +12,7 @@
 #include <sbi/sbi_console.h>
 #include <sbi/sbi_ecall.h>
 #include <sbi/sbi_hart.h>
+#include <sbi/sbi_hsm.h>
 #include <sbi/sbi_ipi.h>
 #include <sbi/sbi_platform.h>
 #include <sbi/sbi_system.h>
@@ -85,6 +86,10 @@ static void __noreturn init_coldboot(struct sbi_scratch *scratch, u32 hartid)
 	if (!init_count_offset)
 		sbi_hart_hang();
 
+	rc = sbi_hsm_init(scratch, hartid, TRUE);
+	if (rc)
+		sbi_hart_hang();
+
 	rc = sbi_system_early_init(scratch, TRUE);
 	if (rc)
 		sbi_hart_hang();
@@ -131,6 +136,7 @@ static void __noreturn init_coldboot(struct sbi_scratch *scratch, u32 hartid)
 	init_count = sbi_scratch_offset_ptr(scratch, init_count_offset);
 	(*init_count)++;
 
+	sbi_hsm_prepare_next_jump(scratch, hartid);
 	sbi_hart_switch_mode(hartid, scratch->next_arg1, scratch->next_addr,
 			     scratch->next_mode, FALSE);
 }
@@ -146,6 +152,10 @@ static void __noreturn init_warmboot(struct sbi_scratch *scratch, u32 hartid)
 	if (!init_count_offset)
 		sbi_hart_hang();
 
+	rc = sbi_hsm_init(scratch, hartid, FALSE);
+	if (rc)
+		sbi_hart_hang();
+
 	rc = sbi_system_early_init(scratch, FALSE);
 	if (rc)
 		sbi_hart_hang();
@@ -179,6 +189,7 @@ static void __noreturn init_warmboot(struct sbi_scratch *scratch, u32 hartid)
 	init_count = sbi_scratch_offset_ptr(scratch, init_count_offset);
 	(*init_count)++;
 
+	sbi_hsm_prepare_next_jump(scratch, hartid);
 	sbi_hart_switch_mode(hartid, scratch->next_arg1,
 			     scratch->next_addr,
 			     scratch->next_mode, FALSE);
@@ -260,5 +271,5 @@ void __noreturn sbi_exit(struct sbi_scratch *scratch)
 
 	sbi_platform_final_exit(plat);
 
-	sbi_hart_hang();
+	sbi_hsm_exit(scratch);
 }
diff --git a/lib/sbi/sbi_ipi.c b/lib/sbi/sbi_ipi.c
index d3b48fe..006844b 100644
--- a/lib/sbi/sbi_ipi.c
+++ b/lib/sbi/sbi_ipi.c
@@ -14,6 +14,7 @@
 #include <sbi/sbi_bitops.h>
 #include <sbi/sbi_error.h>
 #include <sbi/sbi_hart.h>
+#include <sbi/sbi_hsm.h>
 #include <sbi/sbi_init.h>
 #include <sbi/sbi_ipi.h>
 #include <sbi/sbi_platform.h>
@@ -153,7 +154,7 @@ void sbi_ipi_clear_smode(struct sbi_scratch *scratch)
 
 static void sbi_ipi_process_halt(struct sbi_scratch *scratch)
 {
-	sbi_exit(scratch);
+	sbi_hsm_hart_stop(scratch, TRUE);
 }
 
 static struct sbi_ipi_event_ops ipi_halt_ops = {
diff --git a/lib/sbi/sbi_system.c b/lib/sbi/sbi_system.c
index 0fd4999..c6aa36c 100644
--- a/lib/sbi/sbi_system.c
+++ b/lib/sbi/sbi_system.c
@@ -9,6 +9,7 @@
  */
 
 #include <sbi/sbi_hart.h>
+#include <sbi/sbi_hsm.h>
 #include <sbi/sbi_platform.h>
 #include <sbi/sbi_system.h>
 #include <sbi/sbi_ipi.h>
@@ -42,6 +43,8 @@ void __noreturn sbi_system_reboot(struct sbi_scratch *scratch, u32 type)
 	sbi_ipi_send_halt(scratch,
 			  sbi_hart_available_mask() & ~current_hartid_mask, 0);
 
+	sbi_hsm_hart_stop(scratch, FALSE);
+
 	/* Platform specific reooot */
 	sbi_platform_system_reboot(sbi_platform_ptr(scratch), type);
 
@@ -57,6 +60,8 @@ void __noreturn sbi_system_shutdown(struct sbi_scratch *scratch, u32 type)
 	sbi_ipi_send_halt(scratch,
 			  sbi_hart_available_mask() & ~current_hartid_mask, 0);
 
+	sbi_hsm_hart_stop(scratch, FALSE);
+
 	/* Platform specific shutdown */
 	sbi_platform_system_shutdown(sbi_platform_ptr(scratch), type);