sleepq: Separate and make faster

CMakeLists.txt

@@ -219,6 +219,8 @@ set(KERNEL_LIB src/core/libraries/kernel/threads/condvar.cpp
                src/core/libraries/kernel/threads/pthread_spec.cpp
                src/core/libraries/kernel/threads/rwlock.cpp
                src/core/libraries/kernel/threads/semaphore.cpp
+               src/core/libraries/kernel/threads/sleepq.cpp
+               src/core/libraries/kernel/threads/sleepq.h
                src/core/libraries/kernel/threads/stack.cpp
                src/core/libraries/kernel/threads/tcb.cpp
                src/core/libraries/kernel/threads/pthread.h

src/core/address_space.cpp

@@ -1,8 +1,8 @@
 // SPDX-FileCopyrightText: Copyright 2024 shadPS4 Emulator Project
 // SPDX-License-Identifier: GPL-2.0-or-later
 
-#include <boost/icl/separate_interval_set.hpp>
 #include <map>
+#include <boost/icl/separate_interval_set.hpp>
 #include "common/alignment.h"
 #include "common/arch.h"
 #include "common/assert.h"

src/core/libraries/kernel/threads/condvar.cpp

@@ -7,6 +7,7 @@
 #include "core/libraries/error_codes.h"
 #include "core/libraries/kernel/kernel.h"
 #include "core/libraries/kernel/threads/pthread.h"
+#include "core/libraries/kernel/threads/sleepq.h"
 #include "core/libraries/libs.h"
 
 namespace Libraries::Kernel {
@@ -21,59 +22,6 @@ static constexpr PthreadCondAttr PhreadCondattrDefault = {
     .c_clockid = ClockId::Realtime,
 };
 
-static std::mutex sc_lock;
-static std::unordered_map<void*, SleepQueue*> sc_table;
-
-void SleepqAdd(void* wchan, Pthread* td) {
-    auto [it, is_new] = sc_table.try_emplace(wchan, td->sleepqueue);
-    if (!is_new) {
-        it->second->sq_freeq.push_front(td->sleepqueue);
-    }
-    td->sleepqueue = nullptr;
-    td->wchan = wchan;
-    it->second->sq_blocked.push_back(td);
-}
-
-int SleepqRemove(SleepQueue* sq, Pthread* td) {
-    std::erase(sq->sq_blocked, td);
-    if (sq->sq_blocked.empty()) {
-        td->sleepqueue = sq;
-        sc_table.erase(td->wchan);
-        td->wchan = nullptr;
-        return 0;
-    } else {
-        td->sleepqueue = sq->sq_freeq.front();
-        sq->sq_freeq.pop_front();
-        td->wchan = nullptr;
-        return 1;
-    }
-}
-
-void SleepqDrop(SleepQueue* sq, void (*callback)(Pthread*, void*), void* arg) {
-    if (sq->sq_blocked.empty()) {
-        return;
-    }
-
-    sc_table.erase(sq);
-    Pthread* td = sq->sq_blocked.front();
-    sq->sq_blocked.pop_front();
-
-    callback(td, arg);
-
-    td->sleepqueue = sq;
-    td->wchan = nullptr;
-
-    auto sq2 = sq->sq_freeq.begin();
-    for (Pthread* td : sq->sq_blocked) {
-        callback(td, arg);
-        td->sleepqueue = *sq2;
-        td->wchan = nullptr;
-        ++sq2;
-    }
-    sq->sq_blocked.clear();
-    sq->sq_freeq.clear();
-}
-
 static int CondInit(PthreadCondT* cond, const PthreadCondAttrT* cond_attr, const char* name) {
     auto* cvp = new PthreadCond{};
     if (cvp == nullptr) {
@@ -154,7 +102,7 @@ int PthreadCond::Wait(PthreadMutexT* mutex, const OrbisKernelTimespec* abstime,
     Pthread* curthread = g_curthread;
     ASSERT_MSG(curthread->wchan == nullptr, "Thread was already on queue.");
     // _thr_testcancel(curthread);
-    sc_lock.lock();
+    SleepqLock(this);
 
     /*
      * set __has_user_waiters before unlocking mutex, this allows
@@ -171,32 +119,32 @@ int PthreadCond::Wait(PthreadMutexT* mutex, const OrbisKernelTimespec* abstime,
 
     int error = 0;
     for (;;) {
-        curthread->wake_sema.try_acquire();
+        void(curthread->wake_sema.try_acquire());
-        sc_lock.unlock();
+        SleepqUnlock(this);
 
         //_thr_cancel_enter2(curthread, 0);
         int error = curthread->Sleep(abstime, usec) ? 0 : POSIX_ETIMEDOUT;
         //_thr_cancel_leave(curthread, 0);
 
-        sc_lock.lock();
+        SleepqLock(this);
         if (curthread->wchan == nullptr) {
             error = 0;
             break;
         } else if (curthread->ShouldCancel()) {
-            SleepQueue* sq = sc_table[this];
+            SleepQueue* sq = SleepqLookup(this);
             has_user_waiters = SleepqRemove(sq, curthread);
-            sc_lock.unlock();
+            SleepqUnlock(this);
             curthread->mutex_obj = nullptr;
             mp->CvLock(recurse);
             return 0;
         } else if (error == POSIX_ETIMEDOUT) {
-            SleepQueue* sq = sc_table[this];
+            SleepQueue* sq = SleepqLookup(this);
             has_user_waiters = SleepqRemove(sq, curthread);
             break;
         }
         UNREACHABLE();
     }
-    sc_lock.unlock();
+    SleepqUnlock(this);
     curthread->mutex_obj = nullptr;
     mp->CvLock(recurse);
     return error;
@@ -230,14 +178,13 @@ int PS4_SYSV_ABI posix_pthread_cond_reltimedwait_np(PthreadCondT* cond, PthreadM
 int PthreadCond::Signal() {
     Pthread* curthread = g_curthread;
 
-    sc_lock.lock();
+    SleepqLock(this);
-    auto it = sc_table.find(this);
+    SleepQueue* sq = SleepqLookup(this);
-    if (it == sc_table.end()) {
+    if (sq == nullptr) {
-        sc_lock.unlock();
+        SleepqUnlock(this);
         return 0;
     }
 
-    SleepQueue* sq = it->second;
     Pthread* td = sq->sq_blocked.front();
     PthreadMutex* mp = td->mutex_obj;
     has_user_waiters = SleepqRemove(sq, td);
@@ -253,7 +200,7 @@ int PthreadCond::Signal() {
         waddr = &td->wake_sema;
     }
 
-    sc_lock.unlock();
+    SleepqUnlock(this);
     if (waddr != nullptr) {
         waddr->release();
     }
@@ -293,16 +240,16 @@ int PthreadCond::Broadcast() {
         }
     };
 
-    sc_lock.lock();
+    SleepqLock(this);
-    auto it = sc_table.find(this);
+    SleepQueue* sq = SleepqLookup(this);
-    if (it == sc_table.end()) {
+    if (sq == nullptr) {
-        sc_lock.unlock();
+        SleepqUnlock(this);
         return 0;
     }
 
-    SleepqDrop(it->second, drop_cb, &ba);
+    SleepqDrop(sq, drop_cb, &ba);
     has_user_waiters = 0;
-    sc_lock.unlock();
+    SleepqUnlock(this);
 
     for (int i = 0; i < ba.count; i++) {
         ba.waddrs[i]->release();

src/core/libraries/kernel/threads/pthread.h

@@ -8,7 +8,6 @@
 #include <mutex>
 #include <semaphore>
 #include <shared_mutex>
-#include <boost/intrusive/list.hpp>
 
 #include "common/enum.h"
 #include "core/libraries/kernel/time.h"
@@ -23,9 +22,6 @@ namespace Libraries::Kernel {
 
 struct Pthread;
 
-using ListBaseHook =
-    boost::intrusive::list_base_hook<boost::intrusive::link_mode<boost::intrusive::normal_link>>;
-
 enum class PthreadMutexFlags : u32 {
     TypeMask = 0xff,
     Defered = 0x200,
@@ -46,7 +42,7 @@ enum class PthreadMutexProt : u32 {
     Protect = 2,
 };
 
-struct PthreadMutex : public ListBaseHook {
+struct PthreadMutex {
     std::timed_mutex m_lock;
     PthreadMutexFlags m_flags;
     Pthread* m_owner;
@@ -240,14 +236,7 @@ using PthreadEntryFunc = void* PS4_SYSV_ABI (*)(void*);
 
 constexpr u32 TidTerminated = 1;
 
-struct SleepQueue {
+struct SleepQueue;
-    std::list<Pthread*> sq_blocked;
-    std::forward_list<SleepQueue*> sq_freeq;
-    std::list<SleepQueue*> sq_hash;
-    std::forward_list<SleepQueue*> sq_flink;
-    void* sq_wchan;
-    int sq_type;
-};
 
 struct SchedParam {
     int sched_priority;

src/core/libraries/kernel/threads/sleepq.cpp

@@ -0,0 +1,101 @@
+// SPDX-FileCopyrightText: Copyright 2024 shadPS4 Emulator Project
+// SPDX-License-Identifier: GPL-2.0-or-later
+
+#include <array>
+#include "common/spin_lock.h"
+#include "core/libraries/kernel/threads/pthread.h"
+#include "core/libraries/kernel/threads/sleepq.h"
+
+namespace Libraries::Kernel {
+
+static constexpr int HASHSHIFT = 9;
+static constexpr int HASHSIZE = (1 << HASHSHIFT);
+#define SC_HASH(wchan)                                                                             \
+    ((u32)((((uintptr_t)(wchan) >> 3) ^ ((uintptr_t)(wchan) >> (HASHSHIFT + 3))) & (HASHSIZE - 1)))
+#define SC_LOOKUP(wc) &sc_table[SC_HASH(wc)]
+
+struct SleepQueueChain {
+    Common::SpinLock sc_lock;
+    SleepqList sc_queues;
+    int sc_type;
+};
+
+static std::array<SleepQueueChain, HASHSIZE> sc_table{};
+
+void SleepqLock(void* wchan) {
+    SleepQueueChain* sc = SC_LOOKUP(wchan);
+    sc->sc_lock.lock();
+}
+
+void SleepqUnlock(void* wchan) {
+    SleepQueueChain* sc = SC_LOOKUP(wchan);
+    sc->sc_lock.unlock();
+}
+
+SleepQueue* SleepqLookup(void* wchan) {
+    SleepQueueChain* sc = SC_LOOKUP(wchan);
+    for (auto& sq : sc->sc_queues) {
+        if (sq.sq_wchan == wchan) {
+            return std::addressof(sq);
+        }
+    }
+    return nullptr;
+}
+
+void SleepqAdd(void* wchan, Pthread* td) {
+    SleepQueue* sq = SleepqLookup(wchan);
+    if (sq != nullptr) {
+        sq->sq_freeq.push_front(*td->sleepqueue);
+    } else {
+        SleepQueueChain* sc = SC_LOOKUP(wchan);
+        sq = td->sleepqueue;
+        sc->sc_queues.push_front(*sq);
+        sq->sq_wchan = wchan;
+        /* sq->sq_type = type; */
+    }
+    td->sleepqueue = NULL;
+    td->wchan = wchan;
+    sq->sq_blocked.push_front(td);
+}
+
+int SleepqRemove(SleepQueue* sq, Pthread* td) {
+    std::erase(sq->sq_blocked, td);
+    if (sq->sq_blocked.empty()) {
+        td->sleepqueue = sq;
+        sq->unlink();
+        td->wchan = nullptr;
+        return 0;
+    } else {
+        td->sleepqueue = std::addressof(sq->sq_freeq.front());
+        sq->sq_freeq.pop_front();
+        td->wchan = nullptr;
+        return 1;
+    }
+}
+
+void SleepqDrop(SleepQueue* sq, void (*callback)(Pthread*, void*), void* arg) {
+    if (sq->sq_blocked.empty()) {
+        return;
+    }
+
+    sq->unlink();
+    Pthread* td = sq->sq_blocked.front();
+    sq->sq_blocked.pop_front();
+
+    callback(td, arg);
+
+    td->sleepqueue = sq;
+    td->wchan = nullptr;
+
+    auto sq2 = sq->sq_freeq.begin();
+    for (Pthread* td : sq->sq_blocked) {
+        callback(td, arg);
+        td->sleepqueue = std::addressof(*sq2);
+        td->wchan = nullptr;
+        ++sq2;
+    }
+    sq->sq_blocked.clear();
+    sq->sq_freeq.clear();
+}
+
+} // namespace Libraries::Kernel

src/core/libraries/kernel/threads/sleepq.h

@@ -0,0 +1,38 @@
+// SPDX-FileCopyrightText: Copyright 2024 shadPS4 Emulator Project
+// SPDX-License-Identifier: GPL-2.0-or-later
+
+#include <forward_list>
+#include <list>
+#include <boost/intrusive/list.hpp>
+#include <boost/intrusive/list_hook.hpp>
+#include "common/types.h"
+
+namespace Libraries::Kernel {
+
+struct Pthread;
+
+using ListBaseHook =
+    boost::intrusive::list_base_hook<boost::intrusive::link_mode<boost::intrusive::auto_unlink>>;
+
+using SleepqList = boost::intrusive::list<SleepQueue, boost::intrusive::constant_time_size<false>>;
+
+struct SleepQueue : public ListBaseHook {
+    std::list<Pthread*> sq_blocked;
+    SleepqList sq_freeq;
+    void* sq_wchan;
+    int sq_type;
+};
+
+void SleepqLock(void* wchan);
+
+void SleepqUnlock(void* wchan);
+
+SleepQueue* SleepqLookup(void* wchan);
+
+void SleepqAdd(void* wchan, Pthread* td);
+
+int SleepqRemove(SleepQueue* sq, Pthread* td);
+
+void SleepqDrop(SleepQueue* sq, void (*callback)(Pthread*, void*), void* arg);
+
+} // namespace Libraries::Kernel

src/core/libraries/kernel/threads/thread_state.cpp

@@ -6,6 +6,7 @@
 #include "common/scope_exit.h"
 #include "core/libraries/error_codes.h"
 #include "core/libraries/kernel/threads/pthread.h"
+#include "core/libraries/kernel/threads/sleepq.h"
 #include "core/libraries/kernel/threads/thread_state.h"
 #include "core/memory.h"
 #include "core/tls.h"