SMP done, to be validated.

git-svn-id: svn://svn.code.sf.net/p/chibios/svn/trunk@14130 27425a3e-05d8-49a3-a47f-9c15f0e5edd8

os/common/ports/ARMv6-M-RP2/chcore.c

@@ -76,7 +76,7 @@ void NMI_Handler(void) {
   /* Writing back the modified PSP value.*/
   __set_PSP((uint32_t)ctxp);
 
-  /* Restoring the normal interrupts status.*/
+  /* Restoring the normal interrupts status, releasing the spinlock.*/
   port_unlock_from_isr();
 }
 #endif /* !CORTEX_ALTERNATE_SWITCH */
@@ -100,9 +100,69 @@ void PendSV_Handler(void) {
 
   /* Writing back the modified PSP value.*/
   __set_PSP((uint32_t)ctxp);
+
+#if CH_CFG_SMP_MODE != FALSE
+   /* Interrupts have been re-enabled in the ASM part but the spinlock is
+      still taken, releasing it.*/
+   port_spinlock_release();
+#endif
 }
 #endif /* CORTEX_ALTERNATE_SWITCH */
 
+#if (CH_CFG_SMP_MODE != FALSE) || defined(__DOXYGEN__)
+/**
+ * @brief   FIFO interrupt handler for core 0.
+ *
+ * @isr
+ */
+CH_IRQ_HANDLER(RP_SIO_IRQ_PROC0_HANDLER) {
+
+  CH_IRQ_PROLOGUE();
+
+  while ((SIO->FIFO_ST & SIO_FIFO_ST_VLD) != 0U) {
+    uint32_t message = SIO->FIFO_RD;
+#if defined(PORT_HANDLE_FIFO_MESSAGE)
+    if (message != PORT_FIFO_RESCHEDULE_MESSAGE) {
+      PORT_FIFO_RESCHEDULE_MESSAGE(1U, message);
+    }
+#else
+    (void)message;
+#endif
+  }
+
+  /* In case the other core is in WFE.*/
+  __SEV();
+
+  CH_IRQ_EPILOGUE();
+}
+
+/**
+ * @brief   FIFO interrupt handler for core 1.
+ *
+ * @isr
+ */
+CH_IRQ_HANDLER(RP_SIO_IRQ_PROC1_HANDLER) {
+
+  CH_IRQ_PROLOGUE();
+
+  while ((SIO->FIFO_ST & SIO_FIFO_ST_VLD) != 0U) {
+    uint32_t message = SIO->FIFO_RD;
+#if defined(PORT_HANDLE_FIFO_MESSAGE)
+    if (message != PORT_FIFO_RESCHEDULE_MESSAGE) {
+      PORT_FIFO_RESCHEDULE_MESSAGE(0U, message);
+    }
+#else
+    (void)message;
+#endif
+  }
+
+  /* In case the other core is in WFE.*/
+  __SEV();
+
+  CH_IRQ_EPILOGUE();
+}
+#endif /* CH_CFG_SMP_MODE != FALSE */
+
 /*===========================================================================*/
 /* Module exported functions.                                                */
 /*===========================================================================*/
@@ -126,6 +186,19 @@ void port_init(os_instance_t *oip) {
   /* Activating timer for this instance.*/
   port_timer_enable(oip);
 
+#if CH_CFG_SMP_MODE != FALSE
+  /* FIFO handlers for each core.*/
+  if (core_id == 0U) {
+    NVIC_SetPriority(15, CORTEX_MAX_KERNEL_PRIORITY);
+  }
+  else if (core_id == 1U) {
+    NVIC_SetPriority(16, CORTEX_MAX_KERNEL_PRIORITY);
+  }
+  else {
+    chDbgAssert(false, "unexpected core id");
+  }
+#endif
+
 #if CORTEX_ALTERNATE_SWITCH == TRUE
   /* Initializing PendSV for the current core, it is only required in
      the alternate switch mode.*/
@@ -143,6 +216,7 @@ void __port_irq_epilogue(uint32_t lr) {
   if (lr != 0xFFFFFFF1U) {
     struct port_extctx *ectxp;
 
+    /* Entering system critical zone.*/
     port_lock_from_isr();
 
     /* The extctx structure is pointed by the PSP register.*/
@@ -175,4 +249,22 @@ void __port_irq_epilogue(uint32_t lr) {
   }
 }
 
+#if (CH_CFG_SMP_MODE != FALSE) || defined(__DOXYGEN__)
+/**
+ * @brief   Takes the kernel spinlock.
+ */
+void __port_spinlock_take(void) {
+
+  port_spinlock_take();
+}
+
+/**
+ * @brief   Releases the kernel spinlock.
+ */
+void __port_spinlock_release(void) {
+
+  port_spinlock_release();
+}
+#endif /* CH_CFG_SMP_MODE != FALSE */
+
 /** @} */

os/common/ports/ARMv6-M-RP2/chcore.h

@@ -99,6 +99,11 @@
 #define PORT_INSTANCE_EXTRA_FIELDS                                          \
   /* Core associated to this OS instance.*/                                 \
   uint32_t              core_id;
+
+/**
+ * @brief   Reschedule message sent through IPC FIFOs.
+ */
+#define PORT_FIFO_RESCHEDULE_MESSAGE    0xFFFFFFFFU
 /** @} */
 
 /**
@@ -120,7 +125,7 @@
  * @details This minimum priority level is calculated from the number of
  *          priority bits supported by the specific Cortex-Mx implementation.
  */
-#define CORTEX_MINIMUM_PRIORITY         (CORTEX_PRIORITY_LEVELS - 1)
+#define CORTEX_MINIMUM_PRIORITY         (CORTEX_PRIORITY_LEVELS - 1U)
 
 /**
  * @brief   Maximum priority level.
@@ -134,7 +139,7 @@
  *          this handler always has the highest priority that cannot preempt
  *          the kernel.
  */
-#define CORTEX_PRIORITY_PENDSV          0
+#define CORTEX_PRIORITY_PENDSV          0U
 
 /**
  * @brief   Priority level to priority mask conversion macro.
@@ -488,6 +493,10 @@ extern "C" {
   void __port_thread_start(void);
   void __port_switch_from_isr(void);
   void __port_exit_from_isr(void);
+#if (CH_CFG_SMP_MODE != FALSE) || defined(__DOXYGEN__)
+  void __port_spinlock_take(void);
+  void __port_spinlock_release_inline(void);
+#endif /* CH_CFG_SMP_MODE != FALSE */
 #ifdef __cplusplus
 }
 #endif
@@ -502,6 +511,45 @@ extern "C" {
 /* Module inline functions.                                                  */
 /*===========================================================================*/
 
+#if (CH_CFG_SMP_MODE != FALSE) || defined(__DOXYGEN__)
+/**
+ * @brief   Triggers an inter-core notification.
+ *
+ * @param[in] oip       pointer to the @p os_instance_t structure
+ */
+__STATIC_INLINE void port_notify_instance(os_instance_t *oip) {
+
+  (void)oip;
+
+  /* Waiting for space into the FIFO.*/
+  while ((SIO->FIFO_ST & SIO_FIFO_ST_RDY) == 0U) {
+    __WFE();
+  }
+
+  /* Sending a reschedule order to the other core.*/
+  SIO->FIFO_WR = PORT_FIFO_RESCHEDULE_MESSAGE;
+}
+
+/**
+ * @brief   Takes the kernel spinlock.
+ */
+__STATIC_INLINE void port_spinlock_take(void) {
+
+  while (SIO->SPINLOCK[PORT_SPINLOCK_NUMBER] == 0U) {
+  }
+  __DMB();
+}
+
+/**
+ * @brief   Releases the kernel spinlock.
+ */
+__STATIC_INLINE void port_spinlock_release(void) {
+
+  __DMB();
+  SIO->SPINLOCK[PORT_SPINLOCK_NUMBER] = (uint32_t)SIO;
+}
+#endif /* CH_CFG_SMP_MODE != FALSE */
+
 /**
  * @brief   Returns a word encoding the current interrupts status.
  *
@@ -538,6 +586,7 @@ __STATIC_INLINE bool port_is_isr_context(void) {
   return (bool)((__get_IPSR() & 0x1FFU) != 0U);
 }
 
+#if (CH_CFG_SMP_MODE != FALSE) || defined(__DOXYGEN__)
 /**
  * @brief   Kernel-lock action.
  * @details In this port this function disables interrupts globally.
@@ -545,11 +594,7 @@ __STATIC_INLINE bool port_is_isr_context(void) {
 __STATIC_INLINE void port_lock(void) {
 
   __disable_irq();
-#if CH_CFG_SMP_MODE == TRUE
-  while (SIO->SPINLOCK[PORT_SPINLOCK_NUMBER] == 0U) {
-  }
-  __DMB();
-#endif
+  port_spinlock_take();
 }
 
 /**
@@ -558,12 +603,20 @@ __STATIC_INLINE void port_lock(void) {
  */
 __STATIC_INLINE void port_unlock(void) {
 
-#if CH_CFG_SMP_MODE == TRUE
-  __DMB();
-  SIO->SPINLOCK[PORT_SPINLOCK_NUMBER] = (uint32_t)SIO;
-#endif
+  port_spinlock_release();
   __enable_irq();
 }
+#else /* CH_CFG_SMP_MODE == FALSE */
+__STATIC_INLINE void port_lock(void) {
+
+  __disable_irq();
+}
+
+__STATIC_INLINE void port_unlock(void) {
+
+  __enable_irq();
+}
+#endif /* CH_CFG_SMP_MODE == FALSE */
 
 /**
  * @brief   Kernel-lock action from an interrupt handler.
@@ -635,27 +688,6 @@ __STATIC_INLINE uint32_t port_get_core_id(void) {
   return SIO->CPUID;
 }
 
-/**
- * @brief   Triggers an inter-core notification.
- *
- * @param[in] oip       pointer to the @p os_instance_t structure
- */
-__STATIC_INLINE void port_notify_instance(os_instance_t *oip) {
-
-  (void)oip;
-#if 0
-  /* Waiting for space into the FIFO.*/
-  while ((SIO->FIFO_ST & SIO_FIFO_ST_RDY) == 0U) {
-    __WFE();
-  }
-
-  /* Note, the constant 0xFFFFFFFFU must not be handled as a message but
-     just discarded by the ISR, it is meant to just trigger a reschedule
-     check.*/
-  SIO->FIFO_WR = 0xFFFFFFFFU;
-#endif
-}
-
 #endif /* !defined(_FROM_ASM_) */
 
 #endif /* CHCORE_H */

os/common/ports/ARMv6-M-RP2/compilers/GCC/chcoreasm.S

@@ -111,6 +111,9 @@ __port_thread_start:
 #endif
 #if CH_DBG_STATISTICS
                 bl      __stats_stop_measure_crit_thd
+#endif
+#if CH_CFG_SMP_MODE != FALSE
+                bl      __port_spinlock_release
 #endif
                 cpsie   i
                 mov     r0, r5