Equeue: HrTimer fixes (#2987)

* initial changes

* tmp

* impl

* support wait for multiple timers

* cleanup

src/core/libraries/kernel/equeue.cpp

@@ -125,7 +125,6 @@ int EqueueInternal::WaitForEvents(SceKernelEvent* ev, int num, u32 micros) {
                                          .count();
             count = WaitForSmallTimer(ev, num, std::max(0l, long(micros - time_waited)));
         }
-        small_timer_event.event.data = 0;
     }
 
     if (ev->flags & SceKernelEvent::Flags::OneShot) {
@@ -179,39 +178,46 @@ int EqueueInternal::GetTriggeredEvents(SceKernelEvent* ev, int num) {
 }
 
 bool EqueueInternal::AddSmallTimer(EqueueEvent& ev) {
-    // We assume that only one timer event (with the same ident across calls)
-    // can be posted to the queue, based on observations so far. In the opposite case,
-    // the small timer storage and wait logic should be reworked.
-    ASSERT(!HasSmallTimer() || small_timer_event.event.ident == ev.event.ident);
-    ev.time_added = std::chrono::steady_clock::now();
-    small_timer_event = std::move(ev);
+    SmallTimer st;
+    st.event = ev.event;
+    st.added = std::chrono::steady_clock::now();
+    st.interval = std::chrono::microseconds{ev.event.data};
+    {
+        std::scoped_lock lock{m_mutex};
+        m_small_timers[st.event.ident] = std::move(st);
+    }
     return true;
 }
 
 int EqueueInternal::WaitForSmallTimer(SceKernelEvent* ev, int num, u32 micros) {
-    int count{};
-
-    ASSERT(num == 1);
+    ASSERT(num >= 1);
 
     auto curr_clock = std::chrono::steady_clock::now();
     const auto wait_end_us = (micros == 0) ? std::chrono::steady_clock::time_point::max()
                                            : curr_clock + std::chrono::microseconds{micros};
-
+    int count = 0;
     do {
         curr_clock = std::chrono::steady_clock::now();
         {
             std::scoped_lock lock{m_mutex};
-            if ((curr_clock - small_timer_event.time_added) >
-                std::chrono::microseconds{small_timer_event.event.data}) {
-                ev[count++] = small_timer_event.event;
-                small_timer_event.event.data = 0;
-                break;
+            for (auto it = m_small_timers.begin(); it != m_small_timers.end() && count < num;) {
+                const SmallTimer& st = it->second;
+
+                if (curr_clock - st.added >= st.interval) {
+                    ev[count++] = st.event;
+                    it = m_small_timers.erase(it);
+                } else {
+                    ++it;
+                }
             }
+
+            if (count > 0)
+                return count;
         }
         std::this_thread::yield();
     } while (curr_clock < wait_end_us);
 
-    return count;
+    return 0;
 }
 
 bool EqueueInternal::EventExists(u64 id, s16 filter) {
@@ -326,6 +332,11 @@ s32 PS4_SYSV_ABI sceKernelAddHRTimerEvent(SceKernelEqueue eq, int id, timespec*
     // `HrTimerSpinlockThresholdUs`) and fall back to boost asio timers if the time to tick is
     // large. Even for large delays, we truncate a small portion to complete the wait
     // using the spinlock, prioritizing precision.
+
+    if (eq->EventExists(event.event.ident, event.event.filter)) {
+        eq->RemoveEvent(id, SceKernelEvent::Filter::HrTimer);
+    }
+
     if (total_us < HrTimerSpinlockThresholdUs) {
         return eq->AddSmallTimer(event) ? ORBIS_OK : ORBIS_KERNEL_ERROR_ENOMEM;
     }

src/core/libraries/kernel/equeue.h

@@ -9,6 +9,7 @@
 #include <vector>
 #include <boost/asio/steady_timer.hpp>
 
+#include <unordered_map>
 #include "common/rdtsc.h"
 #include "common/types.h"
 
@@ -135,6 +136,12 @@ private:
 };
 
 class EqueueInternal {
+    struct SmallTimer {
+        SceKernelEvent event;
+        std::chrono::steady_clock::time_point added;
+        std::chrono::microseconds interval;
+    };
+
 public:
     explicit EqueueInternal(std::string_view name) : m_name(name) {}
 
@@ -151,13 +158,14 @@ public:
     int GetTriggeredEvents(SceKernelEvent* ev, int num);
 
     bool AddSmallTimer(EqueueEvent& event);
-    bool HasSmallTimer() const {
-        return small_timer_event.event.data != 0;
+    bool HasSmallTimer() {
+        std::scoped_lock lock{m_mutex};
+        return !m_small_timers.empty();
     }
     bool RemoveSmallTimer(u64 id) {
-        if (HasSmallTimer() && small_timer_event.event.ident == id) {
-            small_timer_event = {};
-            return true;
+        if (HasSmallTimer()) {
+            std::scoped_lock lock{m_mutex};
+            return m_small_timers.erase(id) > 0;
         }
         return false;
     }
@@ -170,8 +178,8 @@ private:
     std::string m_name;
     std::mutex m_mutex;
     std::vector<EqueueEvent> m_events;
-    EqueueEvent small_timer_event{};
     std::condition_variable m_cond;
+    std::unordered_map<u64, SmallTimer> m_small_timers;
 };
 
 u64 PS4_SYSV_ABI sceKernelGetEventData(const SceKernelEvent* ev);