Implement shared_mutex and atomic_op

Some parts (initially written by me) are taken from RPCS3 and relicensed.

CMakeLists.txt

@@ -59,6 +59,7 @@ add_library(obj.orbis-kernel OBJECT
   src/sys/sys_vm_unix.cpp
 
   src/utils/Logs.cpp
+  src/utils/SharedMutex.cpp
 )
 
 target_link_libraries(obj.orbis-kernel PUBLIC orbis::kernel::config)

include/orbis/KernelContext.hpp

@@ -1,6 +1,7 @@
 #pragma once
 #include "orbis/thread/Process.hpp"
 #include "utils/LinkedNode.hpp"
+#include "utils/SharedMutex.hpp"
 
 #include <algorithm>
 #include <mutex>
@@ -89,7 +90,7 @@ public:
   }
 
 private:
-  mutable std::mutex m_proc_mtx;
+  mutable shared_mutex m_proc_mtx;
   utils::LinkedNode<Process> *m_processes = nullptr;
   std::vector<EventListener *> m_event_listeners;
 };

include/orbis/thread/Process.hpp

@@ -3,6 +3,7 @@
 #include "orbis-config.hpp"
 #include "orbis/module/Module.hpp"
 #include "orbis/utils/IdMap.hpp"
+#include "orbis/utils/SharedMutex.hpp"
 #include "../thread/types.hpp"
 #include "../thread/Thread.hpp"
 
@@ -20,7 +21,7 @@ struct Process {
   sysentvec *sysent = nullptr;
   ProcessState state = ProcessState::NEW;
   Process *parentProcess = nullptr;
-  std::mutex mtx;
+  shared_mutex mtx;
   void (*onSysEnter)(Thread *thread, int id, uint64_t *args, int argsCount) = nullptr;
   void (*onSysExit)(Thread *thread, int id, uint64_t *args, int argsCount, SysResult result) = nullptr;
   ptr<void> processParam = nullptr;

include/orbis/utils/AtomicOp.hpp

@@ -0,0 +1,74 @@
+#pragma once
+
+#include <atomic>
+#include <utility>
+#include <functional>
+
+namespace orbis {
+inline namespace utils {
+// Atomic operation; returns old value, or pair of old value and return value
+// (cancel op if evaluates to false)
+template <typename T, typename F, typename RT = std::invoke_result_t<F, T &>>
+inline std::conditional_t<std::is_void_v<RT>, T, std::pair<T, RT>>
+atomic_fetch_op(std::atomic<T> &v, F func) {
+  T _new, old = v.load();
+  while (true) {
+    _new = old;
+    if constexpr (std::is_void_v<RT>) {
+      std::invoke(func, _new);
+      if (v.compare_exchange_strong(old, _new)) [[likely]] {
+        return old;
+      }
+    } else {
+      RT ret = std::invoke(func, _new);
+      if (!ret || v.compare_exchange_strong(old, _new)) [[likely]] {
+        return {old, std::move(ret)};
+      }
+    }
+  }
+}
+
+// Atomic operation; returns function result value, function is the lambda
+template <typename T, typename F, typename RT = std::invoke_result_t<F, T &>>
+inline RT atomic_op(std::atomic<T> &v, F func) {
+  T _new, old = v.load();
+  while (true) {
+    _new = old;
+    if constexpr (std::is_void_v<RT>) {
+      std::invoke(func, _new);
+      if (v.compare_exchange_strong(old, _new)) [[likely]] {
+        return;
+      }
+    } else {
+      RT result = std::invoke(func, _new);
+      if (v.compare_exchange_strong(old, _new)) [[likely]] {
+        return result;
+      }
+    }
+  }
+}
+
+#if defined(__ATOMIC_HLE_ACQUIRE) && defined(__ATOMIC_HLE_RELEASE)
+static constexpr int s_hle_ack = __ATOMIC_SEQ_CST | __ATOMIC_HLE_ACQUIRE;
+static constexpr int s_hle_rel = __ATOMIC_SEQ_CST | __ATOMIC_HLE_RELEASE;
+#else
+static constexpr int s_hle_ack = __ATOMIC_SEQ_CST;
+static constexpr int s_hle_rel = __ATOMIC_SEQ_CST;
+#endif
+
+template <typename T>
+inline bool compare_exchange_hle_acq(std::atomic<T> &dest, T &comp, T exch) {
+  static_assert(sizeof(T) == 4 || sizeof(T) == 8);
+  static_assert(std::atomic<T>::is_always_lock_free);
+  return __atomic_compare_exchange(reinterpret_cast<T *>(&dest), &comp, &exch,
+                                   false, s_hle_ack, s_hle_ack);
+}
+
+template <typename T>
+inline T fetch_add_hle_rel(std::atomic<T> &dest, T value) {
+  static_assert(sizeof(T) == 4 || sizeof(T) == 8);
+  static_assert(std::atomic<T>::is_always_lock_free);
+  return __atomic_fetch_add(reinterpret_cast<T *>(&dest), value, s_hle_rel);
+}
+} // namespace utils
+} // namespace orbis

include/orbis/utils/SharedMutex.hpp

@@ -0,0 +1,144 @@
+#pragma once
+
+#include <atomic>
+#include <mutex>
+#include <orbis/utils/AtomicOp.hpp>
+
+namespace orbis {
+inline namespace utils {
+// IPC-ready shared mutex, using only writer lock is recommended
+struct shared_mutex final {
+  enum : unsigned {
+    c_one = 1u << 14, // Fixed-point 1.0 value (one writer)
+    c_sig = 1u << 30,
+    c_err = 1u << 31,
+  };
+
+  std::atomic<unsigned> m_value{};
+
+  void impl_lock_shared(unsigned val);
+  void impl_unlock_shared(unsigned old);
+  void impl_wait();
+  void impl_signal();
+  void impl_lock(unsigned val);
+  void impl_unlock(unsigned old);
+  void impl_lock_upgrade();
+
+public:
+  constexpr shared_mutex() = default;
+
+  bool try_lock_shared() {
+    // Conditional increment
+    unsigned value = m_value.load();
+    return value < c_one - 1 &&
+           m_value.compare_exchange_strong(value, value + 1);
+  }
+
+  // Lock with HLE acquire hint
+  void lock_shared() {
+    unsigned value = m_value.load();
+    if (value < c_one - 1) [[likely]] {
+      unsigned old = value;
+      if (compare_exchange_hle_acq(m_value, old, value + 1)) [[likely]] {
+        return;
+      }
+    }
+
+    impl_lock_shared(value + 1);
+  }
+
+  // Unlock with HLE release hint
+  void unlock_shared() {
+    const unsigned value = fetch_add_hle_rel(m_value, -1u);
+    if (value >= c_one) [[unlikely]] {
+      impl_unlock_shared(value);
+    }
+  }
+
+  bool try_lock() {
+    unsigned value = 0;
+    return m_value.compare_exchange_strong(value, c_one);
+  }
+
+  // Lock with HLE acquire hint
+  void lock() {
+    unsigned value = 0;
+    if (!compare_exchange_hle_acq(m_value, value, +c_one)) [[unlikely]] {
+      impl_lock(value);
+    }
+  }
+
+  // Unlock with HLE release hint
+  void unlock() {
+    const unsigned value = fetch_add_hle_rel(m_value, 0u - c_one);
+    if (value != c_one) [[unlikely]] {
+      impl_unlock(value);
+    }
+  }
+
+  bool try_lock_upgrade() {
+    unsigned value = m_value.load();
+
+    // Conditional increment, try to convert a single reader into a writer,
+    // ignoring other writers
+    return (value + c_one - 1) % c_one == 0 &&
+           m_value.compare_exchange_strong(value, value + c_one - 1);
+  }
+
+  void lock_upgrade() {
+    if (!try_lock_upgrade()) [[unlikely]] {
+      impl_lock_upgrade();
+    }
+  }
+
+  void lock_downgrade() {
+    // Convert to reader lock (can result in broken state)
+    m_value -= c_one - 1;
+  }
+
+  // Check whether can immediately obtain an exclusive (writer) lock
+  bool is_free() const { return m_value.load() == 0; }
+
+  // Check whether can immediately obtain a shared (reader) lock
+  bool is_lockable() const { return m_value.load() < c_one - 1; }
+};
+
+// Simplified shared (reader) lock implementation.
+class reader_lock final {
+  shared_mutex &m_mutex;
+  bool m_upgraded = false;
+
+public:
+  reader_lock(const reader_lock &) = delete;
+  reader_lock &operator=(const reader_lock &) = delete;
+  explicit reader_lock(shared_mutex &mutex) : m_mutex(mutex) {
+    m_mutex.lock_shared();
+  }
+
+  // One-way lock upgrade; note that the observed state could have been changed
+  void upgrade() {
+    if (!m_upgraded) {
+      m_mutex.lock_upgrade();
+      m_upgraded = true;
+    }
+  }
+
+  // Try to upgrade; if it succeeds, the observed state has NOT been changed
+  bool try_upgrade() {
+    return m_upgraded || (m_upgraded = m_mutex.try_lock_upgrade());
+  }
+
+  ~reader_lock() { m_upgraded ? m_mutex.unlock() : m_mutex.unlock_shared(); }
+};
+
+class writer_lock final {
+  shared_mutex &m_mutex;
+
+public:
+  writer_lock(const writer_lock &) = delete;
+  writer_lock &operator=(const writer_lock &) = delete;
+  explicit writer_lock(shared_mutex &mutex) : m_mutex(mutex) { m_mutex.lock(); }
+  ~writer_lock() { m_mutex.unlock(); }
+};
+} // namespace utils
+} // namespace orbis

src/utils/SharedMutex.cpp

@@ -0,0 +1,151 @@
+#include "utils/SharedMutex.hpp"
+#include <linux/futex.h>
+#include <syscall.h>
+#include <unistd.h>
+#include <xmmintrin.h>
+
+static void busy_wait(unsigned long long cycles = 3000) {
+  const auto stop = __builtin_ia32_rdtsc() + cycles;
+  do
+    _mm_pause();
+  while (__builtin_ia32_rdtsc() < stop);
+}
+
+namespace orbis::utils {
+void shared_mutex::impl_lock_shared(unsigned val) {
+  if (val >= c_err)
+    std::abort(); // "shared_mutex underflow"
+
+  // Try to steal the notification bit
+  unsigned _old = val;
+  if (val & c_sig && m_value.compare_exchange_strong(_old, val - c_sig + 1)) {
+    return;
+  }
+
+  for (int i = 0; i < 10; i++) {
+    if (try_lock_shared()) {
+      return;
+    }
+
+    unsigned old = m_value;
+
+    if (old & c_sig && m_value.compare_exchange_strong(old, old - c_sig + 1)) {
+      return;
+    }
+
+    busy_wait();
+  }
+
+  // Acquire writer lock and downgrade
+  const unsigned old = m_value.fetch_add(c_one);
+
+  if (old == 0) {
+    lock_downgrade();
+    return;
+  }
+
+  if ((old % c_sig) + c_one >= c_sig)
+    std::abort; // "shared_mutex overflow"
+  impl_wait();
+  lock_downgrade();
+}
+void shared_mutex::impl_unlock_shared(unsigned old) {
+  if (old - 1 >= c_err)
+    std::abort(); // "shared_mutex underflow"
+
+  // Check reader count, notify the writer if necessary
+  if ((old - 1) % c_one == 0) {
+    impl_signal();
+  }
+}
+void shared_mutex::impl_wait() {
+  while (true) {
+    const auto [old, ok] = atomic_fetch_op(m_value, [](unsigned &value) {
+      if (value >= c_sig) {
+        value -= c_sig;
+        return true;
+      }
+
+      return false;
+    });
+
+    if (ok) {
+      break;
+    }
+
+    syscall(SYS_futex, &m_value, FUTEX_WAIT, old, 0, 0, 0);
+  }
+}
+void shared_mutex::impl_signal() {
+  m_value += c_sig;
+  syscall(SYS_futex, &m_value, FUTEX_WAKE, 1, 0, 0, 0);
+}
+void shared_mutex::impl_lock(unsigned val) {
+  if (val >= c_err)
+    std::abort(); // "shared_mutex underflow"
+
+  // Try to steal the notification bit
+  unsigned _old = val;
+  if (val & c_sig &&
+      m_value.compare_exchange_strong(_old, val - c_sig + c_one)) {
+    return;
+  }
+
+  for (int i = 0; i < 10; i++) {
+    busy_wait();
+
+    unsigned old = m_value;
+
+    if (!old && try_lock()) {
+      return;
+    }
+
+    if (old & c_sig &&
+        m_value.compare_exchange_strong(old, old - c_sig + c_one)) {
+      return;
+    }
+  }
+
+  const unsigned old = m_value.fetch_add(c_one);
+
+  if (old == 0) {
+    return;
+  }
+
+  if ((old % c_sig) + c_one >= c_sig)
+    std::abort(); // "shared_mutex overflow"
+  impl_wait();
+}
+void shared_mutex::impl_unlock(unsigned old) {
+  if (old - c_one >= c_err)
+    std::abort(); // "shared_mutex underflow"
+
+  // 1) Notify the next writer if necessary
+  // 2) Notify all readers otherwise if necessary (currently indistinguishable
+  // from writers)
+  if (old - c_one) {
+    impl_signal();
+  }
+}
+void shared_mutex::impl_lock_upgrade() {
+  for (int i = 0; i < 10; i++) {
+    busy_wait();
+
+    if (try_lock_upgrade()) {
+      return;
+    }
+  }
+
+  // Convert to writer lock
+  const unsigned old = m_value.fetch_add(c_one - 1);
+
+  if ((old % c_sig) + c_one - 1 >= c_sig)
+    std::abort(); // "shared_mutex overflow"
+
+  if (old % c_one == 1) {
+    return;
+  }
+
+  impl_wait();
+}
+} // namespace orbis::utils