diff --git a/portable/ThirdParty/GCC/RP2040/include/portmacro.h b/portable/ThirdParty/GCC/RP2040/include/portmacro.h index 9b8c0dea83..23a0df2f72 100644 --- 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 index 27f2838d36..dad8edea8d 100644 --- 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) ); } }