Update port spinlock implementation

chinglee-iot · Aug 23, 2023 · 8725b6e · 8725b6e
1 parent b4210b2
commit 8725b6e
Show file tree

Hide file tree

Showing 2 changed files with 46 additions and 11 deletions.
diff --git a/portable/ThirdParty/GCC/RP2040/include/portmacro.h b/portable/ThirdParty/GCC/RP2040/include/portmacro.h
@@ -269,6 +269,7 @@
 typedef struct{
     uint32_t uxSpinlockNumber;
     volatile uint32_t uxSpinlockValue;
+    BaseType_t xOwnerCore;
 } SoftwareSpinlock_t;
 
 #define RP2040_SPINLOCK_NUMBER_ISR              ( PICO_SPINLOCK_ID_OS1 + 0 )
@@ -285,43 +286,50 @@ typedef struct{
 do {   \
     ( pxSpinlock )->uxSpinlockNumber = RP2040_SPINLOCK_NUMBER_EVENT_GROUP;  \
     ( pxSpinlock )->uxSpinlockValue = 0;   \
+    ( pxSpinlock )->xOwnerCore = -1;   \
 } while( 0 )
 
 #define portSPINLOCK_QUEUE_INIT( pxSpinlock ) \
 do {   \
     ( pxSpinlock )->uxSpinlockNumber = RP2040_SPINLOCK_NUMBER_QUEUE;  \
     ( pxSpinlock )->uxSpinlockValue = 0;   \
+    ( pxSpinlock )->xOwnerCore = -1;   \
 } while( 0 )
 
 #define  portSPINLOCK_STREAM_BUFFER_INIT( pxSpinlock ) \
 do {   \
     ( pxSpinlock )->uxSpinlockNumber = RP2040_SPINLOCK_NUMBER_STREAM_BUFFER;  \
     ( pxSpinlock )->uxSpinlockValue = 0;   \
+    ( pxSpinlock )->xOwnerCore = -1;   \
 } while( 0 )
 
 
 #define portSPINLOCK_KERNEL_TASK_INIT_STATIC \
 {   \
     .uxSpinlockNumber = RP2040_SPINLOCK_NUMBER_TASK,    \
-    .uxSpinlockValue = 0U                               \
+    .uxSpinlockValue = 0U,                               \
+    .xOwnerCore = -1                               \
 }
 
 #define portSPINLOCK_KERNEL_ISR_INIT_STATIC \
 {   \
     .uxSpinlockNumber = RP2040_SPINLOCK_NUMBER_ISR,    \
-    .uxSpinlockValue = 0U                               \
+    .uxSpinlockValue = 0U,                               \
+    .xOwnerCore = -1                               \
 }
 
 #define portSPINLOCK_TIMER_INIT_STATIC \
 {   \
     .uxSpinlockNumber = RP2040_SPINLOCK_NUMBER_TIMER,    \
-    .uxSpinlockValue = 0U                               \
+    .uxSpinlockValue = 0U,                               \
+    .xOwnerCore = -1                               \
 }
 
 #define portSPINLOCK_USER_INIT_STATIC \
 {   \
     .uxSpinlockNumber = RP2040_SPINLOCK_NUMBER_USER,    \
-    .uxSpinlockValue = 0U                               \
+    .uxSpinlockValue = 0U,                               \
+    .xOwnerCore = -1                               \
 }
 
 #define portSPINLOCK_NUMBER_TO_INDEX( x )       ( ( x ) - PICO_SPINLOCK_ID_OS1 )

diff --git a/portable/ThirdParty/GCC/RP2040/port.c b/portable/ThirdParty/GCC/RP2040/port.c
@@ -377,6 +377,19 @@ void vPortStartFirstTask( void )
         spin_lock_claim( configSMP_SPINLOCK_0 );
         spin_lock_claim( configSMP_SPINLOCK_1 );
 
+        #if portGRANULAR_LOCKING == 1
+            spin_lock_claim( RP2040_SPINLOCK_NUMBER_EVENT_GROUP );
+            spin_lock_init( RP2040_SPINLOCK_NUMBER_EVENT_GROUP );
+            spin_lock_claim( RP2040_SPINLOCK_NUMBER_QUEUE );
+            spin_lock_init( RP2040_SPINLOCK_NUMBER_QUEUE );
+            spin_lock_claim( RP2040_SPINLOCK_NUMBER_STREAM_BUFFER );
+            spin_lock_init( RP2040_SPINLOCK_NUMBER_STREAM_BUFFER );
+            spin_lock_claim( RP2040_SPINLOCK_NUMBER_TIMER );
+            spin_lock_init( RP2040_SPINLOCK_NUMBER_TIMER );
+            spin_lock_claim( RP2040_SPINLOCK_NUMBER_USER );
+            spin_lock_init( RP2040_SPINLOCK_NUMBER_USER );
+        #endif
+
         #if portRUNNING_ON_BOTH_CORES
             ucPrimaryCoreNum = configTICK_CORE;
             configASSERT( get_core_num() == 0) ; // we must be started on core 0
@@ -1153,14 +1166,21 @@ void vPortSpinlockTake( portSPINLOCK_TYPE *pxSpinlock )
     {
         for(;;)
         {
-            vPortRecursiveLock(uxSpinlockIndex, spin_lock_instance(pxSpinlock->uxSpinlockNumber), pdTRUE);
-            if( pxSpinlock->uxSpinlockValue == 0U )
+            spin_lock_unsafe_blocking( spin_lock_instance(pxSpinlock->uxSpinlockNumber) );
+            if( pxSpinlock->xOwnerCore == portGET_CORE_ID() )
+            {
+                pxSpinlock->uxSpinlockValue = pxSpinlock->uxSpinlockValue + 1;
+                spin_unlock_unsafe( spin_lock_instance(pxSpinlock->uxSpinlockNumber) );
+                break;
+            }
+            else if( pxSpinlock->uxSpinlockValue == 0U )
             {
                 pxSpinlock->uxSpinlockValue = 1;
-                vPortRecursiveLock(uxSpinlockIndex, spin_lock_instance(pxSpinlock->uxSpinlockNumber), pdFALSE);
+                pxSpinlock->xOwnerCore = portGET_CORE_ID();
+                spin_unlock_unsafe( spin_lock_instance(pxSpinlock->uxSpinlockNumber) );
                 break;
             }
-            vPortRecursiveLock(uxSpinlockIndex, spin_lock_instance(pxSpinlock->uxSpinlockNumber), pdFALSE);
+            spin_unlock_unsafe( spin_lock_instance(pxSpinlock->uxSpinlockNumber) );
         }
     }
 }
@@ -1174,8 +1194,15 @@ void vPortSpinlockRelease( portSPINLOCK_TYPE *pxSpinlock )
     }
     else
     {
-        vPortRecursiveLock(uxSpinlockIndex, spin_lock_instance(pxSpinlock->uxSpinlockNumber), pdTRUE);
-        pxSpinlock->uxSpinlockValue = 0;
-        vPortRecursiveLock(uxSpinlockIndex, spin_lock_instance(pxSpinlock->uxSpinlockNumber), pdFALSE);
+        spin_lock_unsafe_blocking( spin_lock_instance(pxSpinlock->uxSpinlockNumber) );
+        if( pxSpinlock->xOwnerCore == portGET_CORE_ID() )
+        {
+            pxSpinlock->uxSpinlockValue = pxSpinlock->uxSpinlockValue - 1U;
+            if( pxSpinlock->uxSpinlockValue == 0 )
+            {
+                pxSpinlock->xOwnerCore = -1;
+            }
+        }
+        spin_unlock_unsafe( spin_lock_instance(pxSpinlock->uxSpinlockNumber) );
     }
 }