lib: Add IPI extension in SBI

This patch adds new IPI extension which replaces ipi related
v0.1 extensions. This also adds a new API for ipi sending as trap
handling is not necessary in v0.2 SBI IPI related extensions.

It also modifies the IPI sending code which now accepts hart mask as a value
instead of S-mode virtual address. Thus, the caller should set it to exact hart
mask value everytime.

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

5 files changed, 161 insertions, 25 deletions

diff --git a/include/sbi/sbi_bitops.h b/include/sbi/sbi_bitops.h
index ece6ec4..9d99299 100644
--- a/include/sbi/sbi_bitops.h
+++ b/include/sbi/sbi_bitops.h
@@ -95,4 +95,78 @@ static inline int __ffs(unsigned long word)
  */
 #define ffz(x) __ffs(~(x))
 
+/**
+ * fls - find last (most-significant) bit set
+ * @x: the word to search
+ *
+ * This is defined the same way as ffs.
+ * Note fls(0) = 0, fls(1) = 1, fls(0x80000000) = 32.
+ */
+
+static inline int fls(int x)
+{
+	int r = 32;
+
+	if (!x)
+		return 0;
+	if (!(x & 0xffff0000u)) {
+		x <<= 16;
+		r -= 16;
+	}
+	if (!(x & 0xff000000u)) {
+		x <<= 8;
+		r -= 8;
+	}
+	if (!(x & 0xf0000000u)) {
+		x <<= 4;
+		r -= 4;
+	}
+	if (!(x & 0xc0000000u)) {
+		x <<= 2;
+		r -= 2;
+	}
+	if (!(x & 0x80000000u)) {
+		x <<= 1;
+		r -= 1;
+	}
+	return r;
+}
+
+/**
+ * __fls - find last (most-significant) set bit in a long word
+ * @word: the word to search
+ *
+ * Undefined if no set bit exists, so code should check against 0 first.
+ */
+static inline unsigned long __fls(unsigned long word)
+{
+	int num = BITS_PER_LONG - 1;
+
+#if BITS_PER_LONG == 64
+	if (!(word & (~0ul << 32))) {
+		num -= 32;
+		word <<= 32;
+	}
+#endif
+	if (!(word & (~0ul << (BITS_PER_LONG-16)))) {
+		num -= 16;
+		word <<= 16;
+	}
+	if (!(word & (~0ul << (BITS_PER_LONG-8)))) {
+		num -= 8;
+		word <<= 8;
+	}
+	if (!(word & (~0ul << (BITS_PER_LONG-4)))) {
+		num -= 4;
+		word <<= 4;
+	}
+	if (!(word & (~0ul << (BITS_PER_LONG-2)))) {
+		num -= 2;
+		word <<= 2;
+	}
+	if (!(word & (~0ul << (BITS_PER_LONG-1))))
+		num -= 1;
+	return num;
+}
+
 #endif
diff --git a/include/sbi/sbi_ipi.h b/include/sbi/sbi_ipi.h
index 5f7192a..109e9af 100644
--- a/include/sbi/sbi_ipi.h
+++ b/include/sbi/sbi_ipi.h
@@ -27,9 +27,8 @@ struct sbi_ipi_data {
 	unsigned long ipi_type;
 };
 
-int sbi_ipi_send_many(struct sbi_scratch *scratch,
-		      struct sbi_trap_info *uptrap,
-		      ulong *pmask, u32 event, void *data);
+int sbi_ipi_send_many(struct sbi_scratch *scratch, ulong hmask,
+		      ulong hbase, u32 event, void *data);
 
 void sbi_ipi_clear_smode(struct sbi_scratch *scratch);
 
diff --git a/lib/sbi/sbi_ecall.c b/lib/sbi/sbi_ecall.c
index d472165..891cdbe 100644
--- a/lib/sbi/sbi_ecall.c
+++ b/lib/sbi/sbi_ecall.c
@@ -17,6 +17,7 @@
 #include <sbi/sbi_timer.h>
 #include <sbi/sbi_tlb.h>
 #include <sbi/sbi_trap.h>
+#include <sbi/sbi_unpriv.h>
 #include <sbi/sbi_hart.h>
 #include <sbi/sbi_version.h>
 #include <sbi/riscv_asm.h>
@@ -35,6 +36,22 @@ u16 sbi_ecall_version_minor(void)
 	return SBI_ECALL_VERSION_MINOR;
 }
 
+static int sbi_load_hart_mask_unpriv(struct sbi_scratch *scratch, ulong *pmask,
+				     ulong *hmask, struct sbi_trap_info *uptrap)
+{
+	ulong mask = 0;
+
+	if (pmask) {
+		mask = sbi_load_ulong(pmask, scratch, uptrap);
+		if (uptrap->cause)
+			return SBI_ETRAP;
+	} else {
+		mask = sbi_hart_available_mask();
+	}
+	*hmask = mask;
+	return 0;
+}
+
 int sbi_check_extension(struct sbi_scratch *scratch, unsigned long extid,
 			unsigned long *out_val)
 {
@@ -49,7 +66,8 @@ int sbi_check_extension(struct sbi_scratch *scratch, unsigned long extid,
 
 	if ((extid >= SBI_EXT_0_1_SET_TIMER && extid <= SBI_EXT_0_1_SHUTDOWN) ||
 	    (extid == SBI_EXT_BASE) ||
-	    (extid == SBI_EXT_TIME)) {
+	    (extid == SBI_EXT_TIME) ||
+	    (extid == SBI_EXT_IPI)) {
 		*out_val = 1;
 	} else if (extid >= SBI_EXT_VENDOR_START &&
 		   extid <= SBI_EXT_VENDOR_END) {
@@ -130,12 +148,27 @@ int sbi_ecall_time_handler(struct sbi_scratch *scratch, unsigned long funcid,
 	return ret;
 }
 
+int sbi_ecall_ipi_handler(struct sbi_scratch *scratch, unsigned long funcid,
+			   unsigned long *args, unsigned long *tval)
+{
+	int ret = 0;
+
+	if (funcid == SBI_EXT_IPI_SEND_IPI)
+		ret = sbi_ipi_send_many(scratch, args[0], args[1],
+					SBI_IPI_EVENT_SOFT, NULL);
+	else
+		ret = SBI_ENOTSUPP;
+
+	return ret;
+}
+
 int sbi_ecall_0_1_handler(struct sbi_scratch *scratch, unsigned long extid,
 			   unsigned long *args, struct sbi_trap_info *out_trap)
 {
 	int ret = 0;
 	struct sbi_tlb_info tlb_info;
 	u32 source_hart = sbi_current_hartid();
+	ulong hmask = 0;
 
 	switch (extid) {
 	case SBI_EXT_0_1_SET_TIMER:
@@ -156,25 +189,35 @@ int sbi_ecall_0_1_handler(struct sbi_scratch *scratch, unsigned long extid,
 		sbi_ipi_clear_smode(scratch);
 		break;
 	case SBI_EXT_0_1_SEND_IPI:
-		ret = sbi_ipi_send_many(scratch, out_trap, (ulong *)args[0],
-					SBI_IPI_EVENT_SOFT, NULL);
+		ret = sbi_load_hart_mask_unpriv(scratch, (ulong *)args[0],
+						&hmask, out_trap);
+		if (ret != SBI_ETRAP)
+			ret = sbi_ipi_send_many(scratch, hmask, 0,
+					       SBI_IPI_EVENT_SOFT, NULL);
 		break;
 	case SBI_EXT_0_1_REMOTE_FENCE_I:
 		tlb_info.start  = 0;
 		tlb_info.size  = 0;
 		tlb_info.type  = SBI_ITLB_FLUSH;
 		tlb_info.shart_mask = 1UL << source_hart;
-		ret = sbi_ipi_send_many(scratch, out_trap, (ulong *)args[0],
-					SBI_IPI_EVENT_FENCE, &tlb_info);
+		ret = sbi_load_hart_mask_unpriv(scratch, (ulong *)args[0],
+						&hmask, out_trap);
+		if (ret != SBI_ETRAP)
+			ret = sbi_ipi_send_many(scratch, hmask, 0,
+						SBI_IPI_EVENT_FENCE,
+						&tlb_info);
 		break;
 	case SBI_EXT_0_1_REMOTE_SFENCE_VMA:
 		tlb_info.start = (unsigned long)args[1];
 		tlb_info.size  = (unsigned long)args[2];
 		tlb_info.type  = SBI_TLB_FLUSH_VMA;
 		tlb_info.shart_mask = 1UL << source_hart;
-
-		ret = sbi_ipi_send_many(scratch, out_trap, (ulong *)args[0],
-					SBI_IPI_EVENT_FENCE, &tlb_info);
+		ret = sbi_load_hart_mask_unpriv(scratch, (ulong *)args[0],
+						&hmask, out_trap);
+		if (ret != SBI_ETRAP)
+			ret = sbi_ipi_send_many(scratch, hmask, 0,
+						SBI_IPI_EVENT_FENCE,
+						&tlb_info);
 		break;
 	case SBI_EXT_0_1_REMOTE_SFENCE_VMA_ASID:
 		tlb_info.start = (unsigned long)args[1];
@@ -183,9 +226,12 @@ int sbi_ecall_0_1_handler(struct sbi_scratch *scratch, unsigned long extid,
 		tlb_info.type  = SBI_TLB_FLUSH_VMA_ASID;
 		tlb_info.shart_mask = 1UL << source_hart;
 
-		ret = sbi_ipi_send_many(scratch, out_trap, (ulong *)args[0],
-					SBI_IPI_EVENT_FENCE,
-					&tlb_info);
+		ret = sbi_load_hart_mask_unpriv(scratch, (ulong *)args[0],
+						&hmask, out_trap);
+		if (ret != SBI_ETRAP)
+			ret = sbi_ipi_send_many(scratch, hmask, 0,
+						SBI_IPI_EVENT_FENCE,
+						&tlb_info);
 		break;
 	case SBI_EXT_0_1_SHUTDOWN:
 		sbi_system_shutdown(scratch, 0);
@@ -225,6 +271,8 @@ int sbi_ecall_handler(u32 hartid, ulong mcause, struct sbi_trap_regs *regs,
 					     args, &out_val, &trap);
 	else if (extension_id == SBI_EXT_TIME)
 		ret = sbi_ecall_time_handler(scratch, func_id, args);
+	else if (extension_id == SBI_EXT_IPI)
+		ret = sbi_ecall_ipi_handler(scratch, func_id, args, &out_val);
 	else if (extension_id >= SBI_EXT_VENDOR_START &&
 		extension_id <= SBI_EXT_VENDOR_END) {
 		ret = sbi_ecall_vendor_ext_handler(scratch, extension_id,
diff --git a/lib/sbi/sbi_ipi.c b/lib/sbi/sbi_ipi.c
index 8dc78a5..c235927 100644
--- a/lib/sbi/sbi_ipi.c
+++ b/lib/sbi/sbi_ipi.c
@@ -11,13 +11,13 @@
 #include <sbi/riscv_asm.h>
 #include <sbi/riscv_atomic.h>
 #include <sbi/riscv_barrier.h>
+#include <sbi/sbi_bitops.h>
 #include <sbi/sbi_error.h>
 #include <sbi/sbi_hart.h>
 #include <sbi/sbi_ipi.h>
 #include <sbi/sbi_platform.h>
 #include <sbi/sbi_tlb.h>
 #include <sbi/sbi_trap.h>
-#include <sbi/sbi_unpriv.h>
 
 static unsigned long ipi_data_off;
 
@@ -53,20 +53,34 @@ static int sbi_ipi_send(struct sbi_scratch *scratch, u32 hartid, u32 event,
 	return 0;
 }
 
-int sbi_ipi_send_many(struct sbi_scratch *scratch,
-		      struct sbi_trap_info *uptrap,
-		      ulong *pmask, u32 event, void *data)
+/**
+ * As this this function only handlers scalar values of hart mask, it must be
+ * set to all online harts if the intention is to send IPIs to all the harts.
+ * If hmask is zero, no IPIs will be sent.
+ */
+int sbi_ipi_send_many(struct sbi_scratch *scratch, ulong hmask, ulong hbase,
+			u32 event, void *data)
 {
 	ulong i, m;
 	ulong mask = sbi_hart_available_mask();
+	ulong tempmask;
 	u32 hartid = sbi_current_hartid();
+	unsigned long last_bit = __fls(mask);
+
+	if (hbase > last_bit)
+		/* hart base is not available */
+		return SBI_EINVAL;
+	/**
+	 * FIXME: This check is valid only ULONG size. This is oka for now as
+	 * avaialble hart mask can support upto ULONG size only.
+	 */
+	tempmask = hmask << hbase;
+	tempmask = ~mask & tempmask;
+	if (tempmask)
+		/* at least one of the hart in hmask is not available */
+		return SBI_EINVAL;
 
-	if (pmask) {
-		mask &= sbi_load_ulong(pmask, scratch, uptrap);
-		if (uptrap->cause)
-			return SBI_ETRAP;
-	}
-
+	mask &= (hmask << hbase);
 	/* Send IPIs to every other hart on the set */
 	for (i = 0, m = mask; m; i++, m >>= 1)
 		if ((m & 1UL) && (i != hartid))
diff --git a/lib/sbi/sbi_system.c b/lib/sbi/sbi_system.c
index 2cb30d4..cdc85d6 100644
--- a/lib/sbi/sbi_system.c
+++ b/lib/sbi/sbi_system.c
@@ -39,7 +39,8 @@ sbi_system_shutdown(struct sbi_scratch *scratch, u32 type)
 
 	/* If that fails (or is not implemented) send an IPI on every
 	 * hart to hang and then hang the current hart */
-	sbi_ipi_send_many(scratch, NULL, NULL, SBI_IPI_EVENT_HALT, NULL);
+	sbi_ipi_send_many(scratch, sbi_hart_available_mask(), 0,
+			  SBI_IPI_EVENT_HALT, NULL);
 
 	sbi_hart_hang();
 }