From 3d9b1e9dd9509ae936d542ddd0eb851f07392a64 Mon Sep 17 00:00:00 2001
From: Giovanni Di Sirio <gdisirio@gmail.com>
Date: Fri, 2 Apr 2021 17:45:42 +0000
Subject: [PATCH] SMP-related changes.

git-svn-id: svn://svn.code.sf.net/p/chibios/svn/trunk@14125 27425a3e-05d8-49a3-a47f-9c15f0e5edd8
---
 os/common/ports/ARMv6-M-RP2/chcore.h | 22 +++++++++++++++++++++-
 1 file changed, 21 insertions(+), 1 deletion(-)

diff --git a/os/common/ports/ARMv6-M-RP2/chcore.h b/os/common/ports/ARMv6-M-RP2/chcore.h
index f7d0ab985..53e65f682 100644
--- a/os/common/ports/ARMv6-M-RP2/chcore.h
+++ b/os/common/ports/ARMv6-M-RP2/chcore.h
@@ -195,10 +195,17 @@
  * @details Activating this option will make the Kernel use the PendSV
  *          handler for preemption instead of the NMI handler.
  */
-#ifndef CORTEX_ALTERNATE_SWITCH
+#if !defined(CORTEX_ALTERNATE_SWITCH)
 #define CORTEX_ALTERNATE_SWITCH         FALSE
 #endif
 
+/**
+ * @brief   Spinlock to be used by the port layer.
+ */
+#if !defined(PORT_SPINLOCK_NUMBER)
+#define PORT_SPINLOCK_NUMBER            31
+#endif
+
 /*===========================================================================*/
 /* Derived constants and error checks.                                       */
 /*===========================================================================*/
@@ -211,6 +218,10 @@
   #error "ChibiOS Cortex-M0 port not licensed"
 #endif
 
+#if (PORT_SPINLOCK_NUMBER < 0) || (PORT_SPINLOCK_NUMBER > 31)
+  #error "invalid PORT_SPINLOCK_NUMBER value"
+#endif
+
 /* Handling a GCC problem impacting ARMv6-M.*/
 #if defined(__GNUC__) && !defined(PORT_IGNORE_GCC_VERSION_CHECK)
   #if ( __GNUC__ > 5 ) && ( __GNUC__ < 10 )
@@ -534,6 +545,11 @@ __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
 }
 
 /**
@@ -542,6 +558,10 @@ __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
   __enable_irq();
 }