From da966bedfdffb039069b496c0274e4b0bfbaf6e4 Mon Sep 17 00:00:00 2001
From: squidbus <175574877+squidbus@users.noreply.github.com>
Date: Wed, 28 Aug 2024 03:13:33 -0700
Subject: [PATCH] kernel: Reimplement POSIX semaphores using
 std::counting_semaphore. (#600)

---
 CMakeLists.txt                                |   4 -
 .../libraries/kernel/thread_management.cpp    | 135 +++++++++---------
 src/core/libraries/kernel/thread_management.h |   7 +
 3 files changed, 78 insertions(+), 68 deletions(-)

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 6450eaf1..04740784 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -95,10 +95,6 @@ check_symbol_exists(pthread_mutex_timedlock "pthread.h" HAVE_PTHREAD_MUTEX_TIMED
 if(HAVE_PTHREAD_MUTEX_TIMEDLOCK OR WIN32)
     add_compile_options(-DHAVE_PTHREAD_MUTEX_TIMEDLOCK)
 endif()
-check_symbol_exists(sem_timedwait "semaphore.h" HAVE_SEM_TIMEDWAIT)
-if(HAVE_SEM_TIMEDWAIT OR WIN32)
-    add_compile_options(-DHAVE_SEM_TIMEDWAIT)
-endif()
 
 if (CMAKE_CXX_COMPILER_ID STREQUAL "Clang" OR CMAKE_CXX_COMPILER_ID STREQUAL "AppleClang")
     # libc++ requires -fexperimental-library to enable std::jthread and std::stop_token support.
diff --git a/src/core/libraries/kernel/thread_management.cpp b/src/core/libraries/kernel/thread_management.cpp
index 567ac867..605d0d29 100644
--- a/src/core/libraries/kernel/thread_management.cpp
+++ b/src/core/libraries/kernel/thread_management.cpp
@@ -2,8 +2,8 @@
 // SPDX-License-Identifier: GPL-2.0-or-later
 
 #include <mutex>
+#include <semaphore>
 #include <thread>
-#include <semaphore.h>
 #include "common/alignment.h"
 #include "common/assert.h"
 #include "common/error.h"
@@ -1374,90 +1374,97 @@ int PS4_SYSV_ABI posix_pthread_detach(ScePthread thread) {
     return pthread_detach(thread->pth);
 }
 
-int PS4_SYSV_ABI posix_sem_init(sem_t* sem, int pshared, unsigned int value) {
-    int result = sem_init(sem, pshared, value);
-    if (result == -1) {
-        SetPosixErrno(errno);
+int PS4_SYSV_ABI posix_sem_init(PthreadSemInternal** sem, int pshared, unsigned int value) {
+    if (value > ORBIS_KERNEL_SEM_VALUE_MAX) {
+        SetPosixErrno(EINVAL);
+        return -1;
     }
-    return result;
+    if (sem != nullptr) {
+        *sem = new PthreadSemInternal{
+            .semaphore = std::counting_semaphore<ORBIS_KERNEL_SEM_VALUE_MAX>{value},
+            .value = {static_cast<int>(value)},
+        };
+    }
+    return 0;
 }
 
-int PS4_SYSV_ABI posix_sem_wait(sem_t* sem) {
-    int result = sem_wait(sem);
-    if (result == -1) {
-        SetPosixErrno(errno);
+int PS4_SYSV_ABI posix_sem_wait(PthreadSemInternal** sem) {
+    if (sem == nullptr || *sem == nullptr) {
+        SetPosixErrno(EINVAL);
+        return -1;
     }
-    return result;
+    (*sem)->semaphore.acquire();
+    --(*sem)->value;
+    return 0;
 }
 
-int PS4_SYSV_ABI posix_sem_trywait(sem_t* sem) {
-    int result = sem_trywait(sem);
-    if (result == -1) {
-        SetPosixErrno(errno);
+int PS4_SYSV_ABI posix_sem_trywait(PthreadSemInternal** sem) {
+    if (sem == nullptr || *sem == nullptr) {
+        SetPosixErrno(EINVAL);
+        return -1;
     }
-    return result;
+    if (!(*sem)->semaphore.try_acquire()) {
+        SetPosixErrno(EAGAIN);
+        return -1;
+    }
+    --(*sem)->value;
+    return 0;
 }
 
-#ifndef HAVE_SEM_TIMEDWAIT
-int sem_timedwait(sem_t* sem, const struct timespec* abstime) {
-    int rc;
-    while ((rc = sem_trywait(sem)) == EAGAIN) {
-        struct timespec curr_time;
-        clock_gettime(CLOCK_REALTIME, &curr_time);
-
-        s64 remaining_ns = 0;
-        remaining_ns +=
-            (static_cast<s64>(abstime->tv_sec) - static_cast<s64>(curr_time.tv_sec)) * 1000000000L;
-        remaining_ns += static_cast<s64>(abstime->tv_nsec) - static_cast<s64>(curr_time.tv_nsec);
-
-        if (remaining_ns <= 0) {
-            return ETIMEDOUT;
-        }
-
-        struct timespec sleep_time;
-        sleep_time.tv_sec = 0;
-        if (remaining_ns < 5000000L) {
-            sleep_time.tv_nsec = remaining_ns;
-        } else {
-            sleep_time.tv_nsec = 5000000;
-        }
-
-        nanosleep(&sleep_time, nullptr);
+int PS4_SYSV_ABI posix_sem_timedwait(PthreadSemInternal** sem, const timespec* t) {
+    if (sem == nullptr || *sem == nullptr) {
+        SetPosixErrno(EINVAL);
+        return -1;
     }
-    return rc;
-}
-#endif
 
-int PS4_SYSV_ABI posix_sem_timedwait(sem_t* sem, const timespec* t) {
-    int result = sem_timedwait(sem, t);
-    if (result == -1) {
-        SetPosixErrno(errno);
+    using std::chrono::duration_cast;
+    using std::chrono::nanoseconds;
+    using std::chrono::seconds;
+    using std::chrono::system_clock;
+
+    const system_clock::time_point time{
+        duration_cast<system_clock::duration>(seconds{t->tv_sec} + nanoseconds{t->tv_nsec})};
+    if (!(*sem)->semaphore.try_acquire_until(time)) {
+        SetPosixErrno(ETIMEDOUT);
+        return -1;
     }
-    return result;
+    --(*sem)->value;
+    return 0;
 }
 
-int PS4_SYSV_ABI posix_sem_post(sem_t* sem) {
-    int result = sem_post(sem);
-    if (result == -1) {
-        SetPosixErrno(errno);
+int PS4_SYSV_ABI posix_sem_post(PthreadSemInternal** sem) {
+    if (sem == nullptr || *sem == nullptr) {
+        SetPosixErrno(EINVAL);
+        return -1;
     }
-    return result;
+    if ((*sem)->value == ORBIS_KERNEL_SEM_VALUE_MAX) {
+        SetPosixErrno(EOVERFLOW);
+        return -1;
+    }
+    ++(*sem)->value;
+    (*sem)->semaphore.release();
+    return 0;
 }
 
-int PS4_SYSV_ABI posix_sem_destroy(sem_t* sem) {
-    int result = sem_destroy(sem);
-    if (result == -1) {
-        SetPosixErrno(errno);
+int PS4_SYSV_ABI posix_sem_destroy(PthreadSemInternal** sem) {
+    if (sem == nullptr || *sem == nullptr) {
+        SetPosixErrno(EINVAL);
+        return -1;
     }
-    return result;
+    delete *sem;
+    *sem = nullptr;
+    return 0;
 }
 
-int PS4_SYSV_ABI posix_sem_getvalue(sem_t* sem, int* sval) {
-    int result = sem_getvalue(sem, sval);
-    if (result == -1) {
-        SetPosixErrno(errno);
+int PS4_SYSV_ABI posix_sem_getvalue(PthreadSemInternal** sem, int* sval) {
+    if (sem == nullptr || *sem == nullptr) {
+        SetPosixErrno(EINVAL);
+        return -1;
     }
-    return result;
+    if (sval) {
+        *sval = (*sem)->value;
+    }
+    return 0;
 }
 
 int PS4_SYSV_ABI posix_pthread_attr_getstacksize(const pthread_attr_t* attr, size_t* size) {
diff --git a/src/core/libraries/kernel/thread_management.h b/src/core/libraries/kernel/thread_management.h
index 3679af84..27cd7929 100644
--- a/src/core/libraries/kernel/thread_management.h
+++ b/src/core/libraries/kernel/thread_management.h
@@ -5,6 +5,7 @@
 
 #include <atomic>
 #include <mutex>
+#include <semaphore>
 #include <string>
 #include <vector>
 #include <pthread.h>
@@ -19,6 +20,7 @@ namespace Libraries::Kernel {
 constexpr int ORBIS_KERNEL_PRIO_FIFO_DEFAULT = 700;
 constexpr int ORBIS_KERNEL_PRIO_FIFO_HIGHEST = 256;
 constexpr int ORBIS_KERNEL_PRIO_FIFO_LOWEST = 767;
+constexpr int ORBIS_KERNEL_SEM_VALUE_MAX = 0x7FFFFFFF;
 
 constexpr int ORBIS_PTHREAD_MUTEX_ERRORCHECK = 1;
 constexpr int ORBIS_PTHREAD_MUTEX_RECURSIVE = 2;
@@ -109,6 +111,11 @@ struct PthreadRwInternal {
     std::string name;
 };
 
+struct PthreadSemInternal {
+    std::counting_semaphore<ORBIS_KERNEL_SEM_VALUE_MAX> semaphore;
+    std::atomic<s32> value;
+};
+
 class PThreadPool {
 public:
     ScePthread Create();