include: sbi_platform: Introduce HART index to HART id table

A platform can have discontinuous and/or sparse HART ids so we
cannot always assume a set of HARTs with continuous HART ids.

This patch adds support for discontinuous and sparse HART ids by
introducing HART index to HART id table. This table has platform
hart_count entries and it maps HART index to HART id.

The HART index to HART id table has only two restrictions:
1. HART index < sbi_platform hart_count
2. HART id < SBI_HARTMASK_MAX_BITS

Example1:
Let's say we have a platform with 2 HART ids 11 and 22, for such a
a platform:
hart_count = 2
hart_index2id[0] = 11
hart_index2id[1] = 22

Example2:
Let's say we have a platform with 5 HARTs ids 0, 1, 2, 3, and 4
but out of these HART with id 0 is not usable so for such a platform:
hart_count = 5
hart_index2id[0] = -1U
hart_index2id[1] = 1
hart_index2id[2] = 2
hart_index2id[3] = 3
hart_index2id[4] = 4
OR
hart_count = 4
hart_index2id[0] = 1
hart_index2id[1] = 2
hart_index2id[2] = 3
hart_index2id[3] = 4

With HART index to HART id table in place, the hart_disabled()
callback is now redundant so we remove it as well.

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

firmware/fw_base.S

@@ -351,6 +351,7 @@ _start_warm:
 	csrw	CSR_MIE, zero
 	csrw	CSR_MIP, zero
 
+	/* Find HART count and HART stack size */
 	la	a4, platform
 #if __riscv_xlen == 64
 	lwu	s7, SBI_PLATFORM_HART_COUNT_OFFSET(a4)
@@ -359,12 +360,29 @@ _start_warm:
 	lw	s7, SBI_PLATFORM_HART_COUNT_OFFSET(a4)
 	lw	s8, SBI_PLATFORM_HART_STACK_SIZE_OFFSET(a4)
 #endif
+	REG_L	s9, SBI_PLATFORM_HART_INDEX2ID_OFFSET(a4)
 
-	/* HART ID should be within expected limit */
+	/* Find HART id */
 	csrr	s6, CSR_MHARTID
-	bge	s6, s7, _start_hang
 
-	/* find the scratch space for this hart */
+	/* Find HART index */
+	beqz	s9, 3f
+	li	a4, 0
+1:
+#if __riscv_xlen == 64
+	lwu	a5, (s9)
+#else
+	lw	a5, (s9)
+#endif
+	beq	a5, s6, 2f
+	add	s9, s9, 4
+	add	a4, a4, 1
+	blt	a4, s7, 1b
+	li	a4, -1
+2:	add	s6, a4, zero
+3:	bge	s6, s7, _start_hang
+
+	/* Find the scratch space based on HART index */
 	la	tp, _fw_end
 	mul	a5, s7, s8
 	add	tp, tp, a5
@@ -411,6 +429,7 @@ _link_end:
 _hartid_to_scratch:
 	/*
 	 * a0 -> HART ID (passed by caller)
+	 * a1 -> HART Index (passed by caller)
 	 * t0 -> HART Stack Size
 	 * t1 -> HART Stack End
 	 * t2 -> Temporary
@@ -423,7 +442,7 @@ _hartid_to_scratch:
 	lw	t0, SBI_PLATFORM_HART_STACK_SIZE_OFFSET(t2)
 	lw	t2, SBI_PLATFORM_HART_COUNT_OFFSET(t2)
 #endif
-	sub	t2, t2, a0
+	sub	t2, t2, a1
 	mul	t2, t2, t0
 	la	t1, _fw_end
 	add	t1, t1, t2

include/sbi/sbi_platform.h

@@ -33,6 +33,8 @@
 #define SBI_PLATFORM_OPS_OFFSET (0x58)
 /** Offset of firmware_context in struct sbi_platform */
 #define SBI_PLATFORM_FIRMWARE_CONTEXT_OFFSET (0x58 + __SIZEOF_POINTER__)
+/** Offset of hart_index2id in struct sbi_platform */
+#define SBI_PLATFORM_HART_INDEX2ID_OFFSET (0x58 + (__SIZEOF_POINTER__ * 2))
 
 #define SBI_PLATFORM_TLB_RANGE_FLUSH_LIMIT_DEFAULT		(1UL << 12)
 
@@ -136,9 +138,6 @@ struct sbi_platform_operations {
 	/** Exit platform timer for current HART */
 	void (*timer_exit)(void);
 
-	/** Check whether given hart is disabled */
-	bool (*hart_disabled)(u32 hartid);
-
 	/** Bringup the given hart from previous stage **/
 	int (*hart_start)(u32 hartid, ulong saddr, ulong priv);
 	/**
@@ -190,6 +189,22 @@ struct sbi_platform {
 	unsigned long platform_ops_addr;
 	/** Pointer to system firmware specific context */
 	unsigned long firmware_context;
+	/**
+	 * HART index to HART id table
+	 *
+	 * For used HART index <abc>:
+	 *     hart_index2id[<abc>] = some HART id
+	 * For unused HART index <abc>:
+	 *     hart_index2id[<abc>] = -1U
+	 *
+	 * If hart_index2id == NULL then we assume identity mapping
+	 *     hart_index2id[<abc>] = <abc>
+	 *
+	 * We have only two restrictions:
+	 * 1. HART index < sbi_platform hart_count
+	 * 2. HART id < SBI_HARTMASK_MAX_BITS
+	 */
+	const u32 *hart_index2id;
 } __packed;
 
 /** Get pointer to sbi_platform for sbi_scratch pointer */
@@ -282,22 +297,46 @@ static inline u32 sbi_platform_hart_stack_size(const struct sbi_platform *plat)
 }
 
 /**
- * Check whether the given HART is disabled
+ * Get HART index for the given HART
  *
  * @param plat pointer to struct sbi_platform
  * @param hartid HART ID
  *
- * @return TRUE if HART is disabled and FALSE otherwise
+ * @return 0 <= value < hart_count for valid HART otherwise -1U
  */
-static inline bool sbi_platform_hart_disabled(const struct sbi_platform *plat,
-					      u32 hartid)
+static inline u32 sbi_platform_hart_index(const struct sbi_platform *plat,
+					  u32 hartid)
 {
-	if (plat) {
-		if (sbi_platform_hart_count(plat) <= hartid)
-			return TRUE;
-		if (sbi_platform_ops(plat)->hart_disabled)
-			return sbi_platform_ops(plat)->hart_disabled(hartid);
+	u32 i;
+
+	if (!plat)
+		return -1U;
+	if (plat->hart_index2id) {
+		for (i = 0; i < plat->hart_count; i++) {
+			if (plat->hart_index2id[i] == hartid)
+				return i;
+		}
+		return -1U;
 	}
+
+	return hartid;
+}
+
+/**
+ * Check whether given HART is invalid
+ *
+ * @param plat pointer to struct sbi_platform
+ * @param hartid HART ID
+ *
+ * @return TRUE if HART is invalid and FALSE otherwise
+ */
+static inline bool sbi_platform_hart_invalid(const struct sbi_platform *plat,
+					     u32 hartid)
+{
+	if (!plat)
+		return TRUE;
+	if (plat->hart_count <= sbi_platform_hart_index(plat, hartid))
+		return TRUE;
 	return FALSE;
 }
 

lib/sbi/sbi_init.c

@@ -279,7 +279,7 @@ void __noreturn sbi_init(struct sbi_scratch *scratch)
 	const struct sbi_platform *plat = sbi_platform_ptr(scratch);
 
 	if ((SBI_HARTMASK_MAX_BITS <= hartid) ||
-	    sbi_platform_hart_disabled(plat, hartid))
+	    sbi_platform_hart_invalid(plat, hartid))
 		sbi_hart_hang();
 
 	if (atomic_add_return(&coldboot_lottery, 1) == 1)
@@ -322,7 +322,7 @@ void __noreturn sbi_exit(struct sbi_scratch *scratch)
 	u32 hartid			= current_hartid();
 	const struct sbi_platform *plat = sbi_platform_ptr(scratch);
 
-	if (sbi_platform_hart_disabled(plat, hartid))
+	if (sbi_platform_hart_invalid(plat, hartid))
 		sbi_hart_hang();
 
 	sbi_platform_early_exit(plat);

lib/sbi/sbi_scratch.c

@@ -19,7 +19,7 @@ struct sbi_scratch *hartid_to_scratch_table[SBI_HARTMASK_MAX_BITS] = { 0 };
 static spinlock_t extra_lock = SPIN_LOCK_INITIALIZER;
 static unsigned long extra_offset = SBI_SCRATCH_EXTRA_SPACE_OFFSET;
 
-typedef struct sbi_scratch *(*hartid2scratch)(ulong hartid);
+typedef struct sbi_scratch *(*hartid2scratch)(ulong hartid, ulong hartindex);
 
 int sbi_scratch_init(struct sbi_scratch *scratch)
 {
@@ -27,10 +27,11 @@ int sbi_scratch_init(struct sbi_scratch *scratch)
 	const struct sbi_platform *plat = sbi_platform_ptr(scratch);
 
 	for (i = 0; i < SBI_HARTMASK_MAX_BITS; i++) {
-		if (sbi_platform_hart_disabled(plat, i))
+		if (sbi_platform_hart_invalid(plat, i))
 			continue;
 		hartid_to_scratch_table[i] =
-			((hartid2scratch)scratch->hartid_to_scratch)(i);
+			((hartid2scratch)scratch->hartid_to_scratch)(i,
+					sbi_platform_hart_index(plat, i));
 	}
 
 	return 0;

lib/utils/fdt/fdt_helper.c

@@ -30,7 +30,7 @@ void fdt_cpu_fixup(void *fdt)
 		sbi_sprintf(cpu_node, "/cpus/cpu@%d", i);
 		cpu_offset = fdt_path_offset(fdt, cpu_node);
 
-		if (sbi_platform_hart_disabled(plat, i))
+		if (sbi_platform_hart_invalid(plat, i))
 			fdt_setprop_string(fdt, cpu_offset, "status",
 					   "disabled");
 

platform/sifive/fu540/platform.c

@@ -165,10 +165,12 @@ static int fu540_timer_init(bool cold_boot)
 	return clint_warm_timer_init();
 }
 
-static bool fu540_hart_disabled(u32 hartid)
-{
-	return (FU540_HARITD_DISABLED & (1UL << hartid)) ? TRUE : FALSE;
-}
+static u32 fu540_hart_index2id[FU540_HART_COUNT - 1] = {
+	[0] = 1,
+	[1] = 2,
+	[2] = 3,
+	[3] = 4,
+};
 
 static int fu540_system_down(u32 type)
 {
@@ -192,7 +194,6 @@ const struct sbi_platform_operations platform_ops = {
 	.timer_event_stop	= clint_timer_event_stop,
 	.timer_event_start	= clint_timer_event_start,
 	.timer_init		= fu540_timer_init,
-	.hart_disabled		= fu540_hart_disabled,
 	.system_reboot		= fu540_system_down,
 	.system_shutdown	= fu540_system_down
 };
@@ -202,7 +203,8 @@ const struct sbi_platform platform = {
 	.platform_version	= SBI_PLATFORM_VERSION(0x0, 0x01),
 	.name			= "SiFive Freedom U540",
 	.features		= SBI_PLATFORM_DEFAULT_FEATURES,
-	.hart_count		= FU540_HART_COUNT,
+	.hart_count		= (FU540_HART_COUNT - 1),
+	.hart_index2id		= fu540_hart_index2id,
 	.hart_stack_size	= SBI_PLATFORM_DEFAULT_HART_STACK_SIZE,
 	.platform_ops_addr	= (unsigned long)&platform_ops
 };