From 82ef3813bd7af8bb328f5e9ddb7f0ccfaad7cb51 Mon Sep 17 00:00:00 2001
From: Jukka Laitinen <jukkax@ssrc.tii.ae>
Date: Wed, 24 May 2023 07:09:52 +0300
Subject: [PATCH] arch/risc-v/src/mpfs: Make mpfs_hart_index2id table
 modifiable by bootloader

This is actually the same table as entrypoints, so just use the same data, which
can be set before booting any of the harts

Signed-off-by: Jukka Laitinen <jukkax@ssrc.tii.ae>
---
 arch/risc-v/src/mpfs/mpfs_entrypoints.c | 24 ++++++++++++++++++
 arch/risc-v/src/mpfs/mpfs_entrypoints.h | 16 ++++++++++++
 arch/risc-v/src/mpfs/mpfs_opensbi.c     | 33 +++++++------------------
 3 files changed, 49 insertions(+), 24 deletions(-)

diff --git a/arch/risc-v/src/mpfs/mpfs_entrypoints.c b/arch/risc-v/src/mpfs/mpfs_entrypoints.c
index 52c8415e4c0..71c659220cb 100644
--- a/arch/risc-v/src/mpfs/mpfs_entrypoints.c
+++ b/arch/risc-v/src/mpfs/mpfs_entrypoints.c
@@ -222,4 +222,28 @@ int mpfs_set_use_sbi(uint64_t hartid, bool use_sbi)
   return ERROR;
 }
 
+/****************************************************************************
+ * Name: mpfs_get_use_sbi
+ *
+ * Description:
+ *   Get if hart boots via SBI.
+ *
+ * Input Parameters:
+ *   hartid - hart id to check
+ *
+ * Returned value:
+ *   true if SBI is used, false otherwise
+ *
+ ****************************************************************************/
+
+bool mpfs_get_use_sbi(uint64_t hartid)
+{
+  if (hartid < ENTRYPT_CNT)
+    {
+      return (g_hart_use_sbi & (1 << hartid)) != 0;
+    }
+
+  return false;
+}
+
 #endif /* CONFIG_MPFS_BOOTLOADER */
diff --git a/arch/risc-v/src/mpfs/mpfs_entrypoints.h b/arch/risc-v/src/mpfs/mpfs_entrypoints.h
index 949972db74f..794b01974a6 100644
--- a/arch/risc-v/src/mpfs/mpfs_entrypoints.h
+++ b/arch/risc-v/src/mpfs/mpfs_entrypoints.h
@@ -86,6 +86,22 @@ int mpfs_set_entrypt(uint64_t hartid, uintptr_t entry);
 
 int mpfs_set_use_sbi(uint64_t hartid, bool use_sbi);
 
+/****************************************************************************
+ * Name: mpfs_get_use_sbi
+ *
+ * Description:
+ *   Get if hart boots via SBI.
+ *
+ * Input Parameters:
+ *   hartid - hart id to check
+ *
+ * Returned value:
+ *   true if SBI is used, false otherwise
+ *
+ ****************************************************************************/
+
+bool mpfs_get_use_sbi(uint64_t hartid);
+
 #if defined(__cplusplus)
 }
 #endif
diff --git a/arch/risc-v/src/mpfs/mpfs_opensbi.c b/arch/risc-v/src/mpfs/mpfs_opensbi.c
index d0d277ba05f..8cf776fa634 100644
--- a/arch/risc-v/src/mpfs/mpfs_opensbi.c
+++ b/arch/risc-v/src/mpfs/mpfs_opensbi.c
@@ -45,6 +45,8 @@
 #include <mpfs_ihc.h>
 #endif
 
+#include <sys/param.h>
+
 /****************************************************************************
  * Pre-processor Definitions
  ****************************************************************************/
@@ -184,30 +186,7 @@ static struct aclint_mswi_data mpfs_mswi =
  * Unused hart is marked with -1.  Mpfs will always have the hart0 unused.
  */
 
-static const u32 mpfs_hart_index2id[MPFS_HART_COUNT] =
-{
-  [0] = -1,
-#ifdef CONFIG_MPFS_HART1_SBI
-  [1] = 1,
-#else
-  [1] = -1,
-#endif
-#ifdef CONFIG_MPFS_HART2_SBI
-  [2] = 2,
-#else
-  [2] = -1,
-#endif
-#ifdef CONFIG_MPFS_HART3_SBI
-  [3] = 3,
-#else
-  [3] = -1,
-#endif
-#ifdef CONFIG_MPFS_HART4_SBI
-  [4] = 4,
-#else
-  [4] = -1,
-#endif
-};
+static u32 mpfs_hart_index2id[MPFS_HART_COUNT];
 
 static const struct sbi_platform platform =
 {
@@ -602,6 +581,12 @@ static int mpfs_opensbi_ecall_handler(long funcid,
 void __attribute__((noreturn)) mpfs_opensbi_setup(void)
 {
   uint32_t hartid = current_hartid();
+  size_t i;
+
+  for (i = 0; i < nitems(mpfs_hart_index2id); i++)
+    {
+      mpfs_hart_index2id[i] = mpfs_get_use_sbi(i) ? i : -1;
+    }
 
   sbi_console_set_device(&mpfs_console);
   mpfs_opensbi_scratch_setup(hartid);