thermopshere: add spinlock try lock

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);
     }

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)

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