From 883e9d367f6cd489666a562c83df0f56458c94b2 Mon Sep 17 00:00:00 2001
From: Nicolas Pitre <npitre@baylibre.com>
Date: Fri, 6 Jan 2023 18:39:04 -0500
Subject: [PATCH] riscv: translate CPU numbers to hartid values for IPI

Given the Zephyr CPU number is no longer tied to the hartid, we must
consider the actual hartid when sending an IPI to a given CPU. Since
those hartids can be anything, let's just save them in the cpu structure
as each CPU is brought online.

While at it, throw in some `get_hart_msip()` cleanups.

Signed-off-by: Nicolas Pitre <npitre@baylibre.com>
---
 arch/riscv/core/smp.c                 | 18 +++++++++---------
 arch/riscv/include/kernel_arch_func.h |  4 ++++
 include/zephyr/arch/riscv/structs.h   |  4 ++++
 3 files changed, 17 insertions(+), 9 deletions(-)

diff --git a/arch/riscv/core/smp.c b/arch/riscv/core/smp.c
index 4d9b7bca93f..c00b4190106 100644
--- a/arch/riscv/core/smp.c
+++ b/arch/riscv/core/smp.c
@@ -34,6 +34,10 @@ void arch_start_cpu(int cpu_num, k_thread_stack_t *stack, int sz,
 void z_riscv_secondary_cpu_init(int cpu_num)
 {
 	csr_write(mscratch, &_kernel.cpus[cpu_num]);
+#ifdef CONFIG_SMP
+	_kernel.cpus[cpu_num].arch.hartid = csr_read(mhartid);
+	_kernel.cpus[cpu_num].arch.online = true;
+#endif
 #ifdef CONFIG_THREAD_LOCAL_STORAGE
 	__asm__("mv tp, %0" : : "r" (z_idle_threads[cpu_num].tls));
 #endif
@@ -50,13 +54,9 @@ void z_riscv_secondary_cpu_init(int cpu_num)
 }
 
 #ifdef CONFIG_SMP
-static uintptr_t *get_hart_msip(int hart_id)
+static uint32_t *get_hart_msip(int hart_id)
 {
-#ifdef CONFIG_64BIT
-	return (uintptr_t *)(uint64_t)(RISCV_MSIP_BASE + (hart_id * 4));
-#else
-	return (uintptr_t *)(RISCV_MSIP_BASE + (hart_id * 4));
-#endif
+	return (uint32_t *)(unsigned long)(RISCV_MSIP_BASE + (hart_id * 4));
 }
 
 void arch_sched_ipi(void)
@@ -71,8 +71,8 @@ void arch_sched_ipi(void)
 	unsigned int num_cpus = arch_num_cpus();
 
 	for (i = 0U; i < num_cpus; i++) {
-		if (i != id) {
-			volatile uint32_t *r = (uint32_t *)get_hart_msip(i);
+		if (i != id && _kernel.cpus[i].arch.online) {
+			volatile uint32_t *r = get_hart_msip(_kernel.cpus[i].arch.hartid);
 			*r = 1U;
 		}
 	}
@@ -84,7 +84,7 @@ static void sched_ipi_handler(const void *unused)
 {
 	ARG_UNUSED(unused);
 
-	volatile uint32_t *r = (uint32_t *)get_hart_msip(_current_cpu->id);
+	volatile uint32_t *r = get_hart_msip(csr_read(mhartid));
 	*r = 0U;
 
 	z_sched_ipi();
diff --git a/arch/riscv/include/kernel_arch_func.h b/arch/riscv/include/kernel_arch_func.h
index 893ad0f276d..edbd956d20a 100644
--- a/arch/riscv/include/kernel_arch_func.h
+++ b/arch/riscv/include/kernel_arch_func.h
@@ -32,6 +32,10 @@ static ALWAYS_INLINE void arch_kernel_init(void)
 #if defined(CONFIG_SMP) || defined(CONFIG_USERSPACE)
 	csr_write(mscratch, &_kernel.cpus[0]);
 #endif
+#ifdef CONFIG_SMP
+	_kernel.cpus[0].arch.hartid = csr_read(mhartid);
+	_kernel.cpus[0].arch.online = true;
+#endif
 #ifdef CONFIG_RISCV_PMP
 	z_riscv_pmp_init();
 #endif
diff --git a/include/zephyr/arch/riscv/structs.h b/include/zephyr/arch/riscv/structs.h
index e52b902b932..4a1260ba525 100644
--- a/include/zephyr/arch/riscv/structs.h
+++ b/include/zephyr/arch/riscv/structs.h
@@ -14,6 +14,10 @@ struct _cpu_arch {
 	unsigned long user_exc_tmp0;
 	unsigned long user_exc_tmp1;
 #endif
+#ifdef CONFIG_SMP
+	unsigned long hartid;
+	bool online;
+#endif
 };
 
 #endif /* ZEPHYR_INCLUDE_RISCV_STRUCTS_H_ */