From 4d6735f6221a0307c99d721e37c59a6f5a9d665d Mon Sep 17 00:00:00 2001
From: Ville Juven <ville.juven@unikie.com>
Date: Mon, 9 Dec 2024 11:59:54 +0200
Subject: [PATCH] mpfs_entrypoints.c: Add simple ACK mechanism for CPU boot

CPUs will acknowledge that they have booted, the primary CPU handling the
boot can then wait for others to complete their boot, before booting
itself.
---
 arch/risc-v/src/mpfs/mpfs_entrypoints.c | 25 +++++++++++++++++++++++++
 arch/risc-v/src/mpfs/mpfs_entrypoints.h | 16 ++++++++++++++++
 2 files changed, 41 insertions(+)

diff --git a/arch/risc-v/src/mpfs/mpfs_entrypoints.c b/arch/risc-v/src/mpfs/mpfs_entrypoints.c
index 71c659220cbc1..46ccf90d57184 100644
--- a/arch/risc-v/src/mpfs/mpfs_entrypoints.c
+++ b/arch/risc-v/src/mpfs/mpfs_entrypoints.c
@@ -29,6 +29,7 @@
 #include <stdint.h>
 #include <stdbool.h>
 
+#include <nuttx/atomic.h>
 #include <nuttx/compiler.h>
 
 #include <sys/types.h>
@@ -90,6 +91,8 @@ uint8_t g_mpfs_boot_stacks[ENTRY_STACK * ENTRYPT_CNT]
   aligned_data(STACK_ALIGNMENT);
 #endif
 
+static int g_cpus_booted;
+
 /****************************************************************************
  * Public Functions
  ****************************************************************************/
@@ -128,6 +131,9 @@ void mpfs_jump_to_app(void)
       "la   t0, g_app_entrypoints\n"         /* Entrypoint table base */
       "add  t0, t0, t1\n"                    /* Index in table */
       "ld   t0, 0(t0)\n"                     /* Load the address from table */
+      "li   t1, 1\n"
+      "la   t2, g_cpus_booted\n"
+      "amoadd.w.aqrl zero, t1, 0(t2)\n"      /* g_cpus_booted + 1 */
       "jr   t0\n"                            /* Jump to entrypoint */
       :
 #ifdef CONFIG_MPFS_BOARD_PMP
@@ -246,4 +252,23 @@ bool mpfs_get_use_sbi(uint64_t hartid)
   return false;
 }
 
+/****************************************************************************
+ * Name: mpfs_cpus_booted
+ *
+ * Description:
+ *   Get amount of CPUs that have completed boot.
+ *
+ * Input Parameters:
+ *   None.
+ *
+ * Returned value:
+ *   Amount of CPUs that have booted.
+ *
+ ****************************************************************************/
+
+int mpfs_cpus_booted(void)
+{
+  return atomic_load(&g_cpus_booted);
+}
+
 #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 794b01974a691..3296c3a8a3fa4 100644
--- a/arch/risc-v/src/mpfs/mpfs_entrypoints.h
+++ b/arch/risc-v/src/mpfs/mpfs_entrypoints.h
@@ -102,6 +102,22 @@ int mpfs_set_use_sbi(uint64_t hartid, bool use_sbi);
 
 bool mpfs_get_use_sbi(uint64_t hartid);
 
+/****************************************************************************
+ * Name: mpfs_cpus_booted
+ *
+ * Description:
+ *   Get amount of CPUs that have completed boot.
+ *
+ * Input Parameters:
+ *   None.
+ *
+ * Returned value:
+ *   Amount of CPUs that have booted.
+ *
+ ****************************************************************************/
+
+int mpfs_cpus_booted(void);
+
 #if defined(__cplusplus)
 }
 #endif