From 8736de4c68147b17db2f09450a4796d2d2e0e925 Mon Sep 17 00:00:00 2001
From: Mathias Kraus <elboberido@m-hias.de>
Date: Wed, 24 Jul 2024 17:04:05 +0200
Subject: [PATCH] iox-#2301 Add SpinLockMutex

---
 .../popo/building_blocks/locking_policy.hpp   | 95 ++++++++++++++++++-
 .../popo/building_blocks/locking_policy.cpp   | 12 ++-
 2 files changed, 101 insertions(+), 6 deletions(-)

diff --git a/iceoryx_posh/include/iceoryx_posh/internal/popo/building_blocks/locking_policy.hpp b/iceoryx_posh/include/iceoryx_posh/internal/popo/building_blocks/locking_policy.hpp
index 973662da1a..17f1be3f5e 100644
--- a/iceoryx_posh/include/iceoryx_posh/internal/popo/building_blocks/locking_policy.hpp
+++ b/iceoryx_posh/include/iceoryx_posh/internal/popo/building_blocks/locking_policy.hpp
@@ -16,12 +16,105 @@
 #ifndef IOX_POSH_POPO_BUILDING_BLOCKS_LOCKING_POLICY_HPP
 #define IOX_POSH_POPO_BUILDING_BLOCKS_LOCKING_POLICY_HPP
 
+#include "iox/detail/adaptive_wait.hpp"
 #include "iox/mutex.hpp"
 
 namespace iox
 {
 namespace popo
 {
+class SpinlockMutex
+{
+  public:
+    SpinlockMutex()
+        : m_flag(ATOMIC_FLAG_INIT)
+        , m_recursive{Recursive{0, 0}}
+    {
+    }
+
+    expected<void, MutexLockError> lock() noexcept
+    {
+        pid_t tid = gettid();
+
+        auto recursive = m_recursive.load();
+
+        if (recursive.tid == tid)
+        {
+            recursive.count += 1;
+            m_recursive.store(recursive);
+
+            return ok();
+        }
+
+        detail::adaptive_wait spinner;
+        spinner.wait_loop([this] { return this->m_flag.test_and_set(std::memory_order_acquire); });
+
+        m_recursive.store(Recursive{tid, 1});
+
+        return ok();
+    }
+
+    expected<void, MutexUnlockError> unlock() noexcept
+    {
+        pid_t tid = gettid();
+
+        auto recursive = m_recursive.load();
+
+        if (recursive.tid == tid)
+        {
+            recursive.count -= 1;
+
+            if (recursive.count == 0)
+            {
+                recursive.tid = 0;
+                m_recursive.store(recursive);
+                m_flag.clear(std::memory_order_release);
+            }
+            else
+            {
+                m_recursive.store(recursive);
+            }
+
+            return ok();
+        }
+
+
+        return err(MutexUnlockError::UNKNOWN_ERROR);
+    }
+
+    expected<MutexTryLock, MutexTryLockError> try_lock() noexcept
+    {
+        pid_t tid = gettid();
+
+        auto recursive = m_recursive.load();
+
+        if (recursive.tid == tid)
+        {
+            recursive.count += 1;
+            m_recursive.store(recursive);
+            return ok(MutexTryLock::LOCK_SUCCEEDED);
+        }
+
+        if (!m_flag.test_and_set(std::memory_order_acquire))
+        {
+            m_recursive.store(Recursive{tid, 1});
+
+            return ok(MutexTryLock::LOCK_SUCCEEDED);
+        }
+        return ok(MutexTryLock::FAILED_TO_ACQUIRE_LOCK);
+    }
+
+    struct Recursive
+    {
+        pid_t tid;
+        uint32_t count;
+    };
+
+  private:
+    std::atomic_flag m_flag;
+    std::atomic<Recursive> m_recursive;
+};
+
 class ThreadSafePolicy
 {
   public:
@@ -33,7 +126,7 @@ class ThreadSafePolicy
     bool tryLock() const noexcept;
 
   private:
-    mutable optional<mutex> m_mutex;
+    mutable optional<SpinlockMutex> m_mutex;
 };
 
 class SingleThreadedPolicy
diff --git a/iceoryx_posh/source/popo/building_blocks/locking_policy.cpp b/iceoryx_posh/source/popo/building_blocks/locking_policy.cpp
index db1796b422..84e0fb612f 100644
--- a/iceoryx_posh/source/popo/building_blocks/locking_policy.cpp
+++ b/iceoryx_posh/source/popo/building_blocks/locking_policy.cpp
@@ -25,11 +25,13 @@ namespace popo
 {
 ThreadSafePolicy::ThreadSafePolicy() noexcept
 {
-    MutexBuilder()
-        .isInterProcessCapable(true)
-        .mutexType(MutexType::RECURSIVE)
-        .create(m_mutex)
-        .expect("Failed to create Mutex");
+    /// @todo iox-#2301 fix this with a proper spin-lock implementation
+    // MutexBuilder()
+    //     .isInterProcessCapable(true)
+    //     .mutexType(MutexType::RECURSIVE)
+    //     .create(m_mutex)
+    //     .expect("Failed to create Mutex");
+    m_mutex.emplace();
 }
 
 void ThreadSafePolicy::lock() const noexcept