diff --git a/arch/risc-v/src/mpfs/mpfs_entrypoints.c b/arch/risc-v/src/mpfs/mpfs_entrypoints.c
index 45aa6f6d8e53b..64b6866ac35a7 100644
--- a/arch/risc-v/src/mpfs/mpfs_entrypoints.c
+++ b/arch/risc-v/src/mpfs/mpfs_entrypoints.c
@@ -121,22 +121,22 @@ void mpfs_jump_to_app(void)
       "csrw pmpcfg0, t0\n"
       "csrw pmpcfg2, zero\n"
 #endif
-#ifdef CONFIG_MPFS_OPENSBI
+      "slli t1, a0, 3\n"                     /* To entrypoint offset */
+      "la   t0, g_app_entrypoints\n"         /* Entrypoint table base */
+      "add  t0, t0, t1\n"                    /* Index in table */
+      "ld   a1, 0(t0)\n"                     /* Load the address from table */
+      "li   t1, 1\n"
+      "la   t2, g_cpus_booted\n"
       "ld   t0, g_hart_use_sbi\n"            /* Load sbi usage bitmask */
+      "amoadd.w.aqrl zero, t1, 0(t2)\n"      /* g_cpus_booted + 1 */
+#ifdef CONFIG_MPFS_OPENSBI
       "srl  t0, t0, a0\n"                    /* Shift right by this hart */
       "andi t0, t0, 1\n"                     /* Check the 0 bit */
       "beqz t0, 1f\n"                        /* If bit was 1, jump to sbi */
       "tail mpfs_opensbi_prepare_hart\n"
       "1:\n"
 #endif
-      "slli t1, a0, 3\n"                     /* To entrypoint offset */
-      "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 */
+      "jr   a1\n"                            /* Jump to entrypoint */
       :
 #ifdef CONFIG_MPFS_BOARD_PMP
       : "i" (ENTRY_STACK)
diff --git a/arch/risc-v/src/mpfs/mpfs_opensbi.c b/arch/risc-v/src/mpfs/mpfs_opensbi.c
index 64cd58ad3ece4..55a4ec4cfb487 100644
--- a/arch/risc-v/src/mpfs/mpfs_opensbi.c
+++ b/arch/risc-v/src/mpfs/mpfs_opensbi.c
@@ -596,16 +596,17 @@ static int mpfs_opensbi_ecall_handler(long funcid,
  *     - Calls the sbi_init() that will not return
  *
  * Input Parameters:
- *   None
+ *   a0 - hartid
+ *   a1 - next_addr
  *
  * Returned Value:
  *   None - this will never return
  *
  ****************************************************************************/
 
-void __attribute__((noreturn)) mpfs_opensbi_setup(void)
+void __attribute__((noreturn)) mpfs_opensbi_setup(uintptr_t hartid,
+                                                  uintptr_t next_addr)
 {
-  uint32_t hartid = current_hartid();
   size_t i;
 
   for (i = 0; i < nitems(mpfs_hart_index2id); i++)
@@ -618,7 +619,7 @@ void __attribute__((noreturn)) mpfs_opensbi_setup(void)
 
   csr_write(mscratch, &g_scratches[hartid].scratch);
   g_scratches[hartid].scratch.next_mode = PRV_S;
-  g_scratches[hartid].scratch.next_addr = mpfs_get_entrypt(hartid);
+  g_scratches[hartid].scratch.next_addr = next_addr;
   g_scratches[hartid].scratch.next_arg1 = 0;
 
   sbi_init(&g_scratches[hartid].scratch);
diff --git a/arch/risc-v/src/mpfs/mpfs_opensbi_utils.S b/arch/risc-v/src/mpfs/mpfs_opensbi_utils.S
index f9041f00983c6..225835cb0061b 100644
--- a/arch/risc-v/src/mpfs/mpfs_opensbi_utils.S
+++ b/arch/risc-v/src/mpfs/mpfs_opensbi_utils.S
@@ -63,7 +63,8 @@ mpfs_global_pointer:
  *   times.
  *
  * Input Parameters:
- *   None
+ *   a0 - hartid
+ *   a1 - next_addr
  *
  * Returned Value:
  *   None
@@ -87,9 +88,9 @@ mpfs_opensbi_prepare_hart:
 
   /* Setup stacks per hart, the stack top is the end of the hart's scratch */
 
-  csrr a0, CSR_MHARTID
+  csrr t0, CSR_MHARTID
   li   t1, SBI_SCRATCH_SIZE
-  mul  t0, a0, t1
+  mul  t0, t0, t1
   la   sp, g_scratches
   add  sp, sp, t0
   tail mpfs_opensbi_setup