3 files changed, 21 insertions, 3 deletions

diff --git a/bsps/riscv/riscv/start/bsp_fatal_halt.c b/bsps/riscv/riscv/start/bsp_fatal_halt.c
index d9708661a7..fb0787c606 100644
--- a/bsps/riscv/riscv/start/bsp_fatal_halt.c
+++ b/bsps/riscv/riscv/start/bsp_fatal_halt.c
@@ -41,6 +41,9 @@ void _CPU_Fatal_halt( uint32_t source, CPU_Uint32ptr error )
 #if RISCV_ENABLE_HTIF_SUPPORT != 0
   htif_poweroff();
 #endif
+#if RISCV_ENABLE_MPFS_SUPPORT != 0
+  for(;;);
+#endif
 
   fdt = bsp_fdt_get();
   node = fdt_node_offset_by_compatible(fdt, -1, "sifive,test0");
diff --git a/bsps/riscv/riscv/start/bspsmp.c b/bsps/riscv/riscv/start/bspsmp.c
index 4f1b3c93cc..91f4f7b96a 100644
--- a/bsps/riscv/riscv/start/bspsmp.c
+++ b/bsps/riscv/riscv/start/bspsmp.c
@@ -49,7 +49,7 @@ void bsp_start_on_secondary_processor(Per_CPU_Control *cpu_self)
 
 uint32_t _CPU_SMP_Initialize(void)
 {
-  return riscv_hart_count;
+  return riscv_hart_count - RISCV_BOOT_HARTID;
 }
 
 bool _CPU_SMP_Start_processor(uint32_t cpu_index)
diff --git a/bsps/riscv/riscv/start/bspstart.c b/bsps/riscv/riscv/start/bspstart.c
index d33e9965f8..66e2934013 100644
--- a/bsps/riscv/riscv/start/bspstart.c
+++ b/bsps/riscv/riscv/start/bspstart.c
@@ -74,6 +74,10 @@ void *riscv_fdt_get_address(const void *fdt, int node)
   return (void *)(uintptr_t) addr;
 }
 
+#if RISCV_ENABLE_MPFS_SUPPORT != 0
+uint32_t riscv_hart_count;
+static uint32_t riscv_hart_phandles[5];
+#else
 #ifdef RTEMS_SMP
 uint32_t riscv_hart_count;
 
@@ -81,6 +85,7 @@ static uint32_t riscv_hart_phandles[CPU_MAXIMUM_PROCESSORS];
 #else
 static uint32_t riscv_hart_phandles[1];
 #endif
+#endif
 
 static void riscv_find_harts(void)
 {
@@ -146,9 +151,13 @@ static void riscv_find_harts(void)
     riscv_hart_phandles[hart_index] = phandle;
   }
 
+#if RISCV_ENABLE_MPFS_SUPPORT != 0
+  riscv_hart_count = max_hart_index + 1;
+#else
 #ifdef RTEMS_SMP
   riscv_hart_count = max_hart_index + 1;
 #endif
+#endif
 }
 
 uint32_t riscv_get_hart_index_by_phandle(uint32_t phandle)
@@ -166,7 +175,7 @@ uint32_t riscv_get_hart_index_by_phandle(uint32_t phandle)
 
 static uint32_t get_core_frequency(void)
 {
-#if RISCV_ENABLE_FRDME310ARTY_SUPPORT != 0
+#if RISCV_ENABLE_FRDME310ARTY_SUPPORT != 0 || RISCV_ENABLE_MPFS_SUPPORT != 0
   uint32_t node;
   const char *fdt;
   const char *tlclk;
@@ -177,7 +186,13 @@ static uint32_t get_core_frequency(void)
   node = fdt_node_offset_by_compatible(fdt, -1,"fixed-clock");
   tlclk = fdt_getprop(fdt, node, "clock-output-names", &len);
 
-  if (strcmp(tlclk,"tlclk") != 0) {
+#if RISCV_ENABLE_FRDME310ARTY_SUPPORT != 0
+  if (strcmp(tlclk,"tlclk") != 0)
+#endif
+#if RISCV_ENABLE_MPFS_SUPPORT != 0
+  if (strcmp(tlclk,"msspllclk") != 0)
+#endif
+  {
     bsp_fatal(RISCV_FATAL_NO_TLCLOCK_FREQUENCY_IN_DEVICE_TREE);
   }