RISC-V: S-Mode: propagate Hart ID

Unlike other architectures we support, Linux must apparently be
booted on all cores by the bootloader. To achieve this, the bootloaders
running on the multiple cores synchronize via IPIs.

We will get there eventually, but for now, let's restrict barebox
to boot Linux on a single core. S-Mode firmware is passed hart (core) id
in a0. This is propagated via the thread pointer register, which is
unused by GCC and made available as:

 - cpuinfo output when running in S-Mode
 - $global.hartid
 - a0 when booting via bootm
 - /chosen/boot-hartid fixup: will come in handy when we gain EFI
   loading support
 - single /cpus/*/reg: All other CPU nodes are deleted via fixup

For M-Mode, we can query hart id via CSR. It's unknown whether erizo
supports it and we don't yet have exception support to handle it not
being available, so changes are only done for S-Mode for now.

Signed-off-by: Ahmad Fatoum <a.fatoum@pengutronix.de>
Link: https://lore.barebox.org/20210619045055.779-6-a.fatoum@pengutronix.de
Signed-off-by: Sascha Hauer <s.hauer@pengutronix.de>

10 files changed, 114 insertions, 11 deletions

diff --git a/arch/riscv/boards/hifive/lowlevel.c b/arch/riscv/boards/hifive/lowlevel.c
index 8a20f3c51d..5e8969bef1 100644
--- a/arch/riscv/boards/hifive/lowlevel.c
+++ b/arch/riscv/boards/hifive/lowlevel.c
@@ -4,23 +4,23 @@
 #include <asm/barebox-riscv.h>
 #include <debug_ll.h>
 
-static __always_inline void start_hifive(void *fdt)
+static __always_inline void start_hifive(unsigned long hartid, void *fdt)
 {
 	putc_ll('>');
 
-	barebox_riscv_supervisor_entry(0x80000000, SZ_128M, fdt);
+	barebox_riscv_supervisor_entry(0x80000000, SZ_128M, hartid, fdt);
 }
 
 ENTRY_FUNCTION(start_hifive_unmatched, a0, a1, a2)
 {
 	extern char __dtb_z_hifive_unmatched_a00_start[];
 
-	start_hifive(__dtb_z_hifive_unmatched_a00_start + get_runtime_offset());
+	start_hifive(a0, __dtb_z_hifive_unmatched_a00_start + get_runtime_offset());
 }
 
 ENTRY_FUNCTION(start_hifive_unleashed, a0, a1, a2)
 {
 	extern char __dtb_z_hifive_unleashed_a00_start[];
 
-	start_hifive(__dtb_z_hifive_unleashed_a00_start + get_runtime_offset());
+	start_hifive(a0, __dtb_z_hifive_unleashed_a00_start + get_runtime_offset());
 }
diff --git a/arch/riscv/boot/board-dt-2nd.c b/arch/riscv/boot/board-dt-2nd.c
index 48cb23ae5e..f31c48a906 100644
--- a/arch/riscv/boot/board-dt-2nd.c
+++ b/arch/riscv/boot/board-dt-2nd.c
@@ -40,7 +40,7 @@ static const struct fdt_device_id console_ids[] = {
 	{ /* sentinel */ }
 };
 
-ENTRY_FUNCTION(start_dt_2nd, a0, _fdt, a2)
+ENTRY_FUNCTION(start_dt_2nd, hartid, _fdt, a2)
 {
 	unsigned long membase, memsize, endmem, endfdt, uncompressed_len;
 	struct fdt_header *fdt = (void *)_fdt;
@@ -73,5 +73,5 @@ ENTRY_FUNCTION(start_dt_2nd, a0, _fdt, a2)
 	    _fdt < riscv_mem_stack_top(membase, endmem))
 		memsize = ALIGN_DOWN(_fdt - membase, SZ_1M);
 
-	barebox_riscv_supervisor_entry(membase, memsize, fdt);
+	barebox_riscv_supervisor_entry(membase, memsize, hartid, fdt);
 }
diff --git a/arch/riscv/cpu/core.c b/arch/riscv/cpu/core.c
index 982d378edd..bf6e6608ce 100644
--- a/arch/riscv/cpu/core.c
+++ b/arch/riscv/cpu/core.c
@@ -19,6 +19,9 @@
 #include <linux/err.h>
 #include <memory.h>
 #include <asm-generic/memory_layout.h>
+#include <globalvar.h>
+#include <magicvar.h>
+#include <asm/system.h>
 #include <io.h>
 
 static int riscv_request_stack(void)
@@ -30,6 +33,31 @@ coredevice_initcall(riscv_request_stack);
 
 static struct device_d timer_dev;
 
+static s64 hartid;
+
+BAREBOX_MAGICVAR(global.hartid, "RISC-V hartid");
+
+static int riscv_fixup_cpus(struct device_node *root, void *context)
+{
+	struct device_node *cpus_node, *np, *tmp;
+
+	cpus_node = of_find_node_by_name(root, "cpus");
+	if (!cpus_node)
+		return 0;
+
+	for_each_child_of_node_safe(cpus_node, tmp, np) {
+		u32 cpu_index;
+
+		if (of_property_read_u32(np, "reg", &cpu_index))
+			continue;
+
+		if (cpu_index != hartid)
+			of_delete_node(np);
+	}
+
+	return 0;
+}
+
 static int riscv_probe(struct device_d *parent)
 {
 	int ret;
@@ -46,7 +74,11 @@ static int riscv_probe(struct device_d *parent)
 			return ret;
 	}
 
-	return 0;
+	hartid = riscv_hartid();
+	if (hartid >= 0)
+		globalvar_add_simple_uint64("hartid", &hartid, "%llu");
+
+	return of_register_fixup(riscv_fixup_cpus, NULL);
 }
 
 static struct of_device_id riscv_dt_ids[] = {
diff --git a/arch/riscv/include/asm/barebox-riscv.h b/arch/riscv/include/asm/barebox-riscv.h
index f4081a71f0..bbe6cd0406 100644
--- a/arch/riscv/include/asm/barebox-riscv.h
+++ b/arch/riscv/include/asm/barebox-riscv.h
@@ -33,8 +33,10 @@ void __noreturn __naked barebox_riscv_entry(unsigned long membase, unsigned long
 #define barebox_riscv_machine_entry(membase, memsize, boarddata) \
 	barebox_riscv_entry(membase, memsize, boarddata, RISCV_M_MODE)
 
-#define barebox_riscv_supervisor_entry(membase, memsize, boarddata) \
-	barebox_riscv_entry(membase, memsize, boarddata, RISCV_S_MODE)
+#define barebox_riscv_supervisor_entry(membase, memsize, hartid, boarddata) do { \
+	__asm__ volatile("mv tp, %0\n" : : "r"(hartid)); \
+	barebox_riscv_entry(membase, memsize, boarddata, RISCV_S_MODE); \
+} while (0)
 
 unsigned long riscv_mem_ramoops_get(void);
 unsigned long riscv_mem_endmem_get(void);
diff --git a/arch/riscv/include/asm/system.h b/arch/riscv/include/asm/system.h
index 3d57a7d191..adf856f9e9 100644
--- a/arch/riscv/include/asm/system.h
+++ b/arch/riscv/include/asm/system.h
@@ -26,6 +26,22 @@ static inline enum riscv_mode __riscv_mode(u32 flags)
 	return flags & RISCV_MODE_MASK;
 }
 
+static inline long __riscv_hartid(u32 flags)
+{
+	long hartid = -1;
+
+	switch (__riscv_mode(flags)) {
+	case RISCV_S_MODE:
+		__asm__ volatile("mv %0, tp\n" : "=r"(hartid) :);
+		break;
+	default:
+		/* unimplemented */
+		break;
+	}
+
+	return hartid;
+}
+
 #ifndef __PBL__
 extern unsigned barebox_riscv_pbl_flags;
 
@@ -34,6 +50,10 @@ static inline enum riscv_mode riscv_mode(void)
 	return __riscv_mode(barebox_riscv_pbl_flags);
 }
 
+static inline long riscv_hartid(void)
+{
+	return __riscv_hartid(barebox_riscv_pbl_flags);
+}
 #endif
 
 #endif
diff --git a/arch/riscv/lib/bootm.c b/arch/riscv/lib/bootm.c
index b3e41de4a8..835ff345e3 100644
--- a/arch/riscv/lib/bootm.c
+++ b/arch/riscv/lib/bootm.c
@@ -3,11 +3,13 @@
 
 #include <common.h>
 #include <bootm.h>
+#include <asm/system.h>
 
 static int do_bootm_linux(struct image_data *data)
 {
 	void (*fn)(unsigned long a0, unsigned long dtb, unsigned long a2);
 	phys_addr_t devicetree;
+	long hartid = riscv_hartid();
 
 	fn = booti_load_image(data, &devicetree);
 	if (IS_ERR(fn))
@@ -15,7 +17,7 @@ static int do_bootm_linux(struct image_data *data)
 
 	shutdown_barebox();
 
-	fn(0, devicetree, 0);
+	fn(hartid >= 0 ? hartid : 0, devicetree, 0);
 
 	return -EINVAL;
 }
diff --git a/arch/riscv/lib/cpuinfo.c b/arch/riscv/lib/cpuinfo.c
index 16305e6c4d..2d9cee2a62 100644
--- a/arch/riscv/lib/cpuinfo.c
+++ b/arch/riscv/lib/cpuinfo.c
@@ -34,6 +34,7 @@ static int do_cpuinfo(int argc, char *argv[])
 
 	switch (mode) {
 	case RISCV_S_MODE:
+		printf("Hart ID=%lu\n", riscv_hartid());
 		if (!IS_ENABLED(CONFIG_RISCV_SBI))
 			break;
 		printf("SBI specification v%lu.%lu detected\n",
diff --git a/common/globalvar.c b/common/globalvar.c
index 8bb5015ce4..9e5a99f793 100644
--- a/common/globalvar.c
+++ b/common/globalvar.c
@@ -565,6 +565,27 @@ int globalvar_add_simple_int(const char *name, int *value,
 	return 0;
 }
 
+int globalvar_add_simple_uint64(const char *name, u64 *value,
+				const char *format)
+{
+	struct param_d *p;
+	int ret;
+
+	ret = globalvar_remove_unqualified(name);
+	if (ret)
+		return ret;
+
+	p = dev_add_param_uint64(&global_device, name, NULL, NULL,
+		value, format, NULL);
+
+	if (IS_ERR(p))
+		return PTR_ERR(p);
+
+	globalvar_nv_sync(name);
+
+	return 0;
+}
+
 int globalvar_add_bool(const char *name,
 		       int (*set)(struct param_d *, void *),
 		       int *value, void *priv)
diff --git a/common/oftree.c b/common/oftree.c
index 5eaa63ad7e..1fcc5277c5 100644
--- a/common/oftree.c
+++ b/common/oftree.c
@@ -232,7 +232,24 @@ static int of_fixup_bootargs(struct device_node *root, void *unused)
 			return err;
 	}
 
-	return of_fixup_bootargs_bootsource(root, node);
+	err = of_fixup_bootargs_bootsource(root, node);
+	if (err)
+		return err;
+
+	if (IS_ENABLED(CONFIG_RISCV)) {
+		const char *hartid;
+
+		hartid = getenv("global.hartid");
+		if (hartid) {
+			unsigned long id;
+
+			err = kstrtoul(hartid, 10, &id);
+			if (!err)
+				err = of_property_write_u32(node, "boot-hartid", id);
+		}
+	}
+
+	return err;
 }
 
 static int of_register_bootargs_fixup(void)
diff --git a/include/globalvar.h b/include/globalvar.h
index 84bee9102c..476bb920f3 100644
--- a/include/globalvar.h
+++ b/include/globalvar.h
@@ -20,6 +20,8 @@ void globalvar_set(const char *name, const char *val);
 int globalvar_add_simple_string(const char *name, char **value);
 int globalvar_add_simple_int(const char *name, int *value,
 			     const char *format);
+int globalvar_add_simple_uint64(const char *name, u64 *value,
+				const char *format);
 int globalvar_add_bool(const char *name,
 		       int (*set)(struct param_d *, void *),
 		       int *value, void *priv);
@@ -55,6 +57,12 @@ static inline int globalvar_add_simple_int(const char *name,
 	return 0;
 }
 
+static inline int globalvar_add_simple_uint64(const char *name,
+		u64 *value, const char *format)
+{
+	return 0;
+}
+
 static inline int globalvar_add_bool(const char *name,
 		int (*set)(struct param_d *, void *),
 		int *value, void *priv)