riscv/mpfs: Add boilerplate code for SMP

arch/risc-v/Kconfig

@@ -203,6 +203,7 @@ config ARCH_CHIP_MPFS
 	select ARCH_RV_ISA_C
 	select ARCH_HAVE_FPU
 	select ARCH_HAVE_DPFPU
+	select ARCH_HAVE_MULTICPU
 	select ARCH_HAVE_MPU
 	select ARCH_MMU_TYPE_SV39
 	select ARCH_HAVE_ADDRENV

arch/risc-v/src/mpfs/hardware/mpfs_clint.h

@@ -39,4 +39,6 @@
 
 #define MPFS_CLINT_MTIME     (MPFS_CLINT_BASE + 0xbff8)
 
+#define RISCV_IPI            MPFS_CLINT_MSIP0
+
 #endif /* __ARCH_RISCV_SRC_MPFS_HARDWARE_MPFS_CLINT_H */

arch/risc-v/src/mpfs/mpfs_head.S

@@ -29,6 +29,7 @@
 #include "chip.h"
 #include "mpfs_memorymap.h"
 #include "riscv_internal.h"
+#include "riscv_macros.S"
 
 /****************************************************************************
  * Public Symbols
@@ -183,21 +184,17 @@ __start:
 
   /* Set stack pointer to the idle thread stack */
 
+#ifdef CONFIG_SMP
+  addi a1, a0, -1        /* Only cores 1...3 can participate in SMP */
+  riscv_set_inital_sp MPFS_IDLESTACK_BASE, SMP_STACK_SIZE, a1
+#else
   la   sp, MPFS_IDLESTACK_TOP
+#endif
 
   /* Jump to __mpfs_start with mhartid in a0 */
 
   tail __mpfs_start
 
-  /* We shouldn't return from __mpfs_start
-   * in case of return, loop forever. nop's added so can be seen in debugger
-   */
-
-1:
-  nop
-  nop
-  j 1b
-
   .global _init
   .global _fini
 

arch/risc-v/src/mpfs/mpfs_irq.c

@@ -33,6 +33,8 @@
 #include <nuttx/irq.h>
 
 #include "riscv_internal.h"
+#include "riscv_ipi.h"
+
 #include "mpfs.h"
 #include "mpfs_plic.h"
 
@@ -50,28 +52,9 @@ void up_irqinitialize(void)
 
   up_irq_save();
 
-  /* Complete possibly claimed IRQs in PLIC (for current hart) in case
-   * of warm reboot, e.g. after a crash in the middle of IRQ handler.
-   * This has no effect on non-claimed or disabled interrupts.
-   */
-
-  uintptr_t claim_address = mpfs_plic_get_claimbase();
-
-  for (int irq = MPFS_IRQ_EXT_START; irq < NR_IRQS; irq++)
-    {
-      putreg32(irq - MPFS_IRQ_EXT_START, claim_address);
-    }
-
-  /* Disable all global interrupts for current hart */
+  /* Initialize PLIC for current hart */
 
-  uintptr_t iebase = mpfs_plic_get_iebase();
-
-  putreg32(0x0, iebase + 0);
-  putreg32(0x0, iebase + 4);
-  putreg32(0x0, iebase + 8);
-  putreg32(0x0, iebase + 12);
-  putreg32(0x0, iebase + 16);
-  putreg32(0x0, iebase + 20);
+  mpfs_plic_init_hart(riscv_mhartid());
 
   /* Colorize the interrupt stack for debug purposes */
 
@@ -80,21 +63,17 @@ void up_irqinitialize(void)
   riscv_stack_color(g_intstackalloc, intstack_size);
 #endif
 
-  /* Set priority for all global interrupts to 1 (lowest) */
-
-  for (int id = 1; id <= NR_IRQS; id++)
-    {
-      putreg32(1, (uintptr_t)(MPFS_PLIC_PRIORITY + (4 * id)));
-    }
+  /* Attach the common interrupt handler */
 
-  /* Set irq threshold to 0 (permits all global interrupts) */
+  riscv_exception_attach();
 
-  uintptr_t threshold_address = mpfs_plic_get_thresholdbase();
-  putreg32(0, threshold_address);
+#ifdef CONFIG_SMP
+  /* Clear IPI for CPU0 */
 
-  /* Attach the common interrupt handler */
+  riscv_ipi_clear(0);
 
-  riscv_exception_attach();
+  up_enable_irq(RISCV_IRQ_SOFT);
+#endif
 
 #ifndef CONFIG_SUPPRESS_INTERRUPTS
 

arch/risc-v/src/mpfs/mpfs_plic.c

@@ -52,93 +52,196 @@
 #endif
 
 /****************************************************************************
- * Public Functions
+ * Private Functions
  ****************************************************************************/
 
 /****************************************************************************
- * Name: mpfs_plic_get_iebase
+ * Name: get_iebase
  *
  * Description:
- *   Context aware way to query PLIC interrupt enable base address
+ *   Get base address for interrupt enable bits for a specific hart.
+ *
+ * Input Parameters:
+ *   hartid - Hart ID to query.
  *
  * Returned Value:
- *   Interrupt enable base address
+ *   Interrupt enable base address.
  *
  ****************************************************************************/
 
-uintptr_t mpfs_plic_get_iebase(void)
+static uintptr_t get_iebase(uintptr_t hartid)
 {
   uintptr_t iebase;
-  uintptr_t hart_id = riscv_mhartid();
 
-  if (hart_id == 0)
+  if (hartid == 0)
     {
       iebase = MPFS_PLIC_H0_MIE0;
     }
   else
     {
       iebase = MPFS_PLIC_H1_MIE0 + MPFS_PLIC_IEPRIV_OFFSET +
-        (hart_id - 1) * MPFS_HART_MIE_OFFSET;
+        (hartid - 1) * MPFS_HART_MIE_OFFSET;
     }
 
   return iebase;
 }
 
 /****************************************************************************
- * Name: mpfs_plic_get_claimbase
+ * Name: get_claimbase
  *
  * Description:
- *   Context aware way to query PLIC interrupt claim base address
+ *   Get base address for interrupt claim for a specific hart.
+ *
+ * Input Parameters:
+ *   hartid - Hart ID to query.
  *
  * Returned Value:
- *   Interrupt enable claim address
+ *   Interrupt enable claim address.
  *
  ****************************************************************************/
 
-uintptr_t mpfs_plic_get_claimbase(void)
+uintptr_t get_claimbase(uintptr_t hartid)
 {
   uintptr_t claim_address;
-  uintptr_t hart_id = riscv_mhartid();
 
-  if (hart_id == 0)
+  if (hartid == 0)
     {
       claim_address = MPFS_PLIC_H0_MCLAIM;
     }
   else
     {
       claim_address = MPFS_PLIC_H1_MCLAIM + MPFS_PLIC_CLAIMPRIV_OFFSET +
-        (hart_id - 1) * MPFS_PLIC_NEXTHART_OFFSET;
+        (hartid - 1) * MPFS_PLIC_NEXTHART_OFFSET;
     }
 
   return claim_address;
 }
 
 /****************************************************************************
- * Name: mpfs_plic_get_thresholdbase
+ * Name: get_thresholdbase
  *
  * Description:
- *   Context aware way to query PLIC interrupt threshold base address
+ *   Get base address for interrupt threshold for a specific hart.
+ *
+ * Input Parameters:
+ *   hartid - Hart ID to query.
  *
  * Returned Value:
- *   Interrupt enable threshold address
+ *   Interrupt enable threshold address.
  *
  ****************************************************************************/
 
-uintptr_t mpfs_plic_get_thresholdbase(void)
+uintptr_t get_thresholdbase(uintptr_t hartid)
 {
   uintptr_t threshold_address;
-  uintptr_t hart_id = riscv_mhartid();
 
-  if (hart_id == 0)
+  if (hartid == 0)
     {
       threshold_address = MPFS_PLIC_H0_MTHRESHOLD;
     }
   else
     {
       threshold_address = MPFS_PLIC_H1_MTHRESHOLD +
           MPFS_PLIC_THRESHOLDPRIV_OFFSET +
-          (hart_id - 1) * MPFS_PLIC_NEXTHART_OFFSET;
+          (hartid - 1) * MPFS_PLIC_NEXTHART_OFFSET;
     }
 
   return threshold_address;
 }
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: mpfs_plic_init_hart
+ *
+ * Description:
+ *   Initialize current hart's PLIC.
+ *
+ * Input Parameters:
+ *   hartid - Hart ID to init.
+ *
+ * Returned Value:
+ *   None.
+ *
+ ****************************************************************************/
+
+void mpfs_plic_init_hart(uintptr_t hartid)
+{
+  /* Disable all global interrupts for current hart */
+
+  uintptr_t iebase = get_iebase(hartid);
+
+  putreg32(0x0, iebase + 0);
+  putreg32(0x0, iebase + 4);
+  putreg32(0x0, iebase + 8);
+  putreg32(0x0, iebase + 12);
+  putreg32(0x0, iebase + 16);
+  putreg32(0x0, iebase + 20);
+
+  /* Complete possibly claimed IRQs in PLIC (for current hart) in case
+   * of warm reboot, e.g. after a crash in the middle of IRQ handler.
+   * This has no effect on non-claimed or disabled interrupts.
+   */
+
+  uintptr_t claim_address = get_claimbase(hartid);
+
+  for (int irq = MPFS_IRQ_EXT_START; irq < NR_IRQS; irq++)
+    {
+      putreg32(irq - MPFS_IRQ_EXT_START, claim_address);
+    }
+
+  /* Set irq threshold to 0 (permits all global interrupts) */
+
+  uintptr_t threshold_address = get_thresholdbase(hartid);
+  putreg32(0, threshold_address);
+}
+
+/****************************************************************************
+ * Name: mpfs_plic_get_iebase
+ *
+ * Description:
+ *   Context aware way to query PLIC interrupt enable base address
+ *
+ * Returned Value:
+ *   Interrupt enable base address
+ *
+ ****************************************************************************/
+
+uintptr_t mpfs_plic_get_iebase(void)
+{
+  return get_iebase(riscv_mhartid());
+}
+
+/****************************************************************************
+ * Name: mpfs_plic_get_claimbase
+ *
+ * Description:
+ *   Context aware way to query PLIC interrupt claim base address
+ *
+ * Returned Value:
+ *   Interrupt enable claim address
+ *
+ ****************************************************************************/
+
+uintptr_t mpfs_plic_get_claimbase(void)
+{
+  return get_claimbase(riscv_mhartid());
+}
+
+/****************************************************************************
+ * Name: mpfs_plic_get_thresholdbase
+ *
+ * Description:
+ *   Context aware way to query PLIC interrupt threshold base address
+ *
+ * Returned Value:
+ *   Interrupt enable threshold address
+ *
+ ****************************************************************************/
+
+uintptr_t mpfs_plic_get_thresholdbase(void)
+{
+  return get_thresholdbase(riscv_mhartid());
+}

arch/risc-v/src/mpfs/mpfs_plic.h

@@ -31,6 +31,22 @@
  * Public Function Prototypes
  ****************************************************************************/
 
+/****************************************************************************
+ * Name: mpfs_plic_init_hart
+ *
+ * Description:
+ *   Initialize current hart's PLIC.
+ *
+ * Input Parameters:
+ *   hartid - Hart ID to init.
+ *
+ * Returned Value:
+ *   None.
+ *
+ ****************************************************************************/
+
+void mpfs_plic_init_hart(uintptr_t hartid);
+
 /****************************************************************************
  * Name: mpfs_plic_get_iebase
  *

arch/risc-v/src/mpfs/mpfs_shead.S

@@ -95,5 +95,13 @@ __start:
 
   /* Set stack pointer and jump to start */
 
-  la sp, MPFS_IDLESTACK_TOP
-  j __mpfs_start
+#ifdef CONFIG_SMP
+  addi a1, a0, -1        /* Only cores 1...3 can participate in SMP */
+  riscv_set_inital_sp MPFS_IDLESTACK_BASE, SMP_STACK_SIZE, a1
+#else
+  la   sp, MPFS_IDLESTACK_TOP
+#endif
+
+  /* Jump to __mpfs_start with mhartid in a0 */
+
+  tail __mpfs_start