diff --git a/thermosphere/src/spinlock.c b/thermosphere/src/spinlock.c index a5d34d1b3..c96e90550 100644 --- a/thermosphere/src/spinlock.c +++ b/thermosphere/src/spinlock.c @@ -19,7 +19,7 @@ void recursiveSpinlockLock(RecursiveSpinlock *lock) { - if (lock->tag != currentCoreCtx->coreId + 1) { + if (LIKELY(lock->tag != currentCoreCtx->coreId + 1)) { spinlockLock(&lock->lock); lock->tag = currentCoreCtx->coreId + 1; lock->count = 1; @@ -28,9 +28,25 @@ void recursiveSpinlockLock(RecursiveSpinlock *lock) } } +bool recursiveSpinlockTryLock(RecursiveSpinlock *lock) +{ + if (LIKELY(lock->tag != currentCoreCtx->coreId + 1)) { + if (!spinlockTryLock(&lock->lock)) { + return false; + } else { + lock->tag = currentCoreCtx->coreId + 1; + lock->count = 1; + return true; + } + } else { + ++lock->count; + return true; + } +} + void recursiveSpinlockUnlock(RecursiveSpinlock *lock) { - if (--lock->count == 0) { + if (LIKELY(--lock->count == 0)) { lock->tag = 0; spinlockUnlock(&lock->lock); } diff --git a/thermosphere/src/spinlock.h b/thermosphere/src/spinlock.h index 4afb2d228..12c0c685e 100644 --- a/thermosphere/src/spinlock.h +++ b/thermosphere/src/spinlock.h @@ -48,9 +48,13 @@ static inline void restoreInterruptFlags(u64 flags) SET_SYSREG(daif, flags); } +// spinlock_impl.s void spinlockLock(Spinlock *lock); +bool spinlockTryLock(Spinlock *lock); void spinlockUnlock(Spinlock *lock); + void recursiveSpinlockLock(RecursiveSpinlock *lock); +bool recursiveSpinlockTryLock(RecursiveSpinlock *lock); void recursiveSpinlockUnlock(RecursiveSpinlock *lock); static inline u64 spinlockLockMaskIrq(Spinlock *lock) diff --git a/thermosphere/src/spinlock_impl.s b/thermosphere/src/spinlock_impl.s index ffece8f38..86bdde4c8 100644 --- a/thermosphere/src/spinlock_impl.s +++ b/thermosphere/src/spinlock_impl.s @@ -29,17 +29,31 @@ FUNCTION spinlockLock mov w2, #1 sevl prfm pstl1keep, [x0] - l1: + 1: wfe - l2: - ldaxr w1, [x0] - cbnz w1, l1 - stxr w1, w2, [x0] - cbnz w1, l2 + 2: + ldaxr w1, [x0] + cbnz w1, 1b + stxr w1, w2, [x0] + cbnz w1, 2b + ret +END_FUNCTION + +FUNCTION spinlockTryLock + mov x1, x0 + mov w2, #1 + prfm pstl1strm, [x1] + 1: + ldaxr w0, [x1] + cbnz w0, 2f + stxr w0, w2, [x1] + cbnz w0, 1b + 2: + eor w0, w0, #1 ret END_FUNCTION FUNCTION spinlockUnlock - stlr wzr, [x0] + stlr wzr, [x0] ret END_FUNCTION