New sys_event_queue/port LV2 syscalls

AlexAltea · Oct 23, 2014 · e401a41 · e401a41
1 parent 9e565ef
commit e401a41
Show file tree

Hide file tree

Showing 6 changed files with 85 additions and 26 deletions.
diff --git a/nucleus/cpu/ppu/interpreter/ppu_interpreter.cpp b/nucleus/cpu/ppu/interpreter/ppu_interpreter.cpp
@@ -61,7 +61,7 @@ void initRotateMask()
         return;
     }
     for (u32 mb = 0; mb < 64; mb++) {
-        for(u32 me = 0; me < 64; me++) {
+        for (u32 me = 0; me < 64; me++) {
             const u64 mask = (~0ULL >> mb) ^ ((me >= 63) ? 0 : ~0ULL >> (me + 1));
             rotateMask[mb][me] = mb > me ? ~mask : mask;
         }

diff --git a/nucleus/syscalls/lv2.cpp b/nucleus/syscalls/lv2.cpp
@@ -68,6 +68,7 @@ LV2::LV2(u32 fw_type)
         m_syscalls[0x06E] = {wrap(sys_cond_signal_to), LV2_NONE};
         m_syscalls[0x072] = {wrap(sys_semaphore_get_value), LV2_NONE};
         m_syscalls[0x076] = {wrap(sys_event_flag_clear), LV2_NONE};
+        m_syscalls[0x082] = {wrap(sys_event_queue_receive), LV2_NONE};
         m_syscalls[0x084] = {wrap(sys_event_flag_cancel), LV2_NONE};
         m_syscalls[0x08B] = {wrap(sys_event_flag_get), LV2_NONE};
         m_syscalls[0x08D] = {wrap(sys_timer_usleep), LV2_NONE};

diff --git a/nucleus/syscalls/lv2/sys_event.cpp b/nucleus/syscalls/lv2/sys_event.cpp
@@ -8,6 +8,8 @@
 #include "nucleus/syscalls/lv2.h"
 #include "nucleus/emulator.h"
 
+#include <algorithm>
+
 /**
  * LV2: Event flags
  */
@@ -192,8 +194,15 @@ s32 sys_event_flag_get(u32 eflag_id, be_t<u64>* flags)
  */
 s32 sys_event_port_create(be_t<u32>* eport_id, s32 port_type, u64 name)
 {
+    // Check requisites
+    if (eport_id == nucleus.memory.ptr(0)) {
+        return CELL_EFAULT;
+    }
+
     // Create event queue
     auto* eport = new sys_event_port_t();
+    eport-> type = port_type;
+    eport->name_value = name;
 
     *eport_id = nucleus.lv2.objects.add(eport, SYS_EVENT_PORT_OBJECT);
     return CELL_OK;
@@ -207,39 +216,56 @@ s32 sys_event_port_destroy(u32 eport_id)
     return CELL_OK;
 }
 
-s32 sys_event_port_connect_local(u32 eport_id, u32 event_queue_id)
+s32 sys_event_port_connect_local(u32 eport_id, u32 equeue_id)
 {
-    auto* eport = nucleus.lv2.objects.get<sys_event_queue_t>(eport_id);
+    auto* eport = nucleus.lv2.objects.get<sys_event_port_t>(eport_id);
+    auto* equeue = nucleus.lv2.objects.get<sys_event_queue_t>(equeue_id);
 
     // Check requisites
-    if (!eport) {
+    if (!eport || !equeue) {
         return CELL_ESRCH;
     }
+    if (!eport->type != SYS_EVENT_PORT_LOCAL) {
+        return CELL_EINVAL;
+    }
+    if (!eport->equeue) {
+        return CELL_EISCONN;
+    }
 
+    eport->equeue = equeue;
     return CELL_OK;
 }
 
 s32 sys_event_port_disconnect(u32 eport_id)
 {
-    auto* eport = nucleus.lv2.objects.get<sys_event_queue_t>(eport_id);
+    auto* eport = nucleus.lv2.objects.get<sys_event_port_t>(eport_id);
 
     // Check requisites
     if (!eport) {
         return CELL_ESRCH;
     }
 
+    eport->equeue = nullptr;
     return CELL_OK;
 }
 
 s32 sys_event_port_send(u32 eport_id, u64 data1, u64 data2, u64 data3)
 {
-    auto* eport = nucleus.lv2.objects.get<sys_event_queue_t>(eport_id);
+    auto* eport = nucleus.lv2.objects.get<sys_event_port_t>(eport_id);
 
     // Check requisites
     if (!eport) {
         return CELL_ESRCH;
     }
 
+    sys_event_t evt;
+    evt.source = eport->name_value;
+    evt.data1 = data1;
+    evt.data2 = data2;
+    evt.data3 = data3;
+
+    eport->equeue->queue.push(evt);
+    eport->equeue->cv.notify_one();
     return CELL_OK;
 }
 
@@ -273,7 +299,7 @@ s32 sys_event_queue_destroy(u32 equeue_id, s32 mode)
     return CELL_OK;
 }
 
-s32 sys_event_queue_receive(u32 equeue_id, sys_event_t* dummy_event, u64 timeout)
+s32 sys_event_queue_receive(u32 equeue_id, sys_event_t* evt, u64 timeout)
 {
     auto* equeue = nucleus.lv2.objects.get<sys_event_queue_t>(equeue_id);
 
@@ -282,10 +308,26 @@ s32 sys_event_queue_receive(u32 equeue_id, sys_event_t* dummy_event, u64 timeout
         return CELL_ESRCH;
     }
 
+    std::unique_lock<std::mutex> lock(equeue->mutex);
+
+    if (equeue->queue.empty()) {
+        // Wait until condition or timeout is met
+        if (timeout == 0) {
+            equeue->cv.wait(lock, [&]{ return equeue->queue.size(); });
+        } else {
+            auto rel_time = std::chrono::microseconds(timeout);
+            if (!equeue->cv.wait_for(lock, rel_time, [&]{ return equeue->queue.size(); })) {
+                return CELL_ETIMEDOUT;
+            }
+        }
+    }
+
+    *evt = equeue->queue.front();
+    equeue->queue.pop();
     return CELL_OK;
 }
 
-s32 sys_event_queue_tryreceive(u32 equeue_id, sys_event_t* event_array, s32 size, be_t<u32>* number)
+s32 sys_event_queue_tryreceive(u32 equeue_id, sys_event_t* event_array, s32 size, be_t<s32>* number)
 {
     auto* equeue = nucleus.lv2.objects.get<sys_event_queue_t>(equeue_id);
 
@@ -294,6 +336,15 @@ s32 sys_event_queue_tryreceive(u32 equeue_id, sys_event_t* event_array, s32 size
         return CELL_ESRCH;
     }
 
+    std::unique_lock<std::mutex> lock(equeue->mutex);
+
+    s32 eventsReceived = std::max((s32)equeue->queue.size(), size);
+    *number = eventsReceived;
+
+    for (u32 i = 0; i < eventsReceived; i++) {
+        event_array[i] = equeue->queue.front();
+        equeue->queue.pop();
+    }
     return CELL_OK;
 }
 

diff --git a/nucleus/syscalls/lv2/sys_event.h b/nucleus/syscalls/lv2/sys_event.h
@@ -9,6 +9,7 @@
 
 #include <condition_variable>
 #include <mutex>
+#include <queue>
 
 // Constants
 enum
@@ -17,6 +18,8 @@ enum
 	SYS_EVENT_FLAG_WAIT_OR        = 0x02,
 	SYS_EVENT_FLAG_WAIT_CLEAR     = 0x10,
 	SYS_EVENT_FLAG_WAIT_CLEAR_ALL = 0x20,
+
+    SYS_EVENT_PORT_LOCAL          = 0x01,
 };
 
 // Classes
@@ -58,13 +61,18 @@ struct sys_event_queue_t
 {
     std::mutex mutex;
     std::condition_variable cv;
+    std::queue<sys_event_t> queue;
     sys_event_queue_attr_t attr;
 };
 
 struct sys_event_port_t
 {
-    sys_event_queue_t* queue = nullptr;
-    s8 name[8];
+    sys_event_queue_t* equeue = nullptr;
+    u32 type;
+    union {
+        s8 name[8];
+        u64 name_value;
+    };
 };
 
 // SysCalls
@@ -79,12 +87,12 @@ s32 sys_event_flag_get(u32 eflag_id, be_t<u64>* flags);
 
 s32 sys_event_port_create(be_t<u32>* eport_id, s32 port_type, u64 name);
 s32 sys_event_port_destroy(u32 eport_id);
-s32 sys_event_port_connect_local(u32 eport_id, u32 event_queue_id);
+s32 sys_event_port_connect_local(u32 eport_id, u32 equeue_id);
 s32 sys_event_port_disconnect(u32 eport_id);
 s32 sys_event_port_send(u32 eport_id, u64 data1, u64 data2, u64 data3);
 
 s32 sys_event_queue_create(be_t<u32>* equeue_id, sys_event_queue_attr_t* attr, u64 event_queue_key, s32 size);
 s32 sys_event_queue_destroy(u32 equeue_id, s32 mode);
 s32 sys_event_queue_receive(u32 equeue_id, sys_event_t* dummy_event, u64 timeout);
-s32 sys_event_queue_tryreceive(u32 equeue_id, sys_event_t* event_array, s32 size, be_t<u32>* number);
+s32 sys_event_queue_tryreceive(u32 equeue_id, sys_event_t* event_array, s32 size, be_t<s32>* number);
 s32 sys_event_queue_drain(u32 equeue_id);
diff --git a/nucleus/syscalls/lv2/sys_process.cpp b/nucleus/syscalls/lv2/sys_process.cpp
@@ -30,6 +30,9 @@ s32 sys_process_get_paramsfo(u8* buffer)
 
 s32 sys_process_get_sdk_version(u32 pid, be_t<u32>* version)
 {
+    if (!version) {
+        return CELL_EFAULT;
+    }
     *version = nucleus.lv2.proc_param.sdk_version;
     return CELL_OK;
 }
diff --git a/nucleus/syscalls/lv2/sys_semaphore.cpp b/nucleus/syscalls/lv2/sys_semaphore.cpp
@@ -112,24 +112,20 @@ s32 sys_semaphore_wait(u32 sem_id, u64 timeout)
         return CELL_ESRCH;
     }
 
-    // If semaphore count is positive, decrement it and continue
     std::unique_lock<std::mutex> lock(semaphore->mutex);
-    if (semaphore->count > 0) {
-        semaphore->count--;
-        return CELL_OK;
-    }
 
-    // Wait until condition or timeout is met
-    if (timeout == 0) {
-        semaphore->cv.wait(lock, [&]{ return semaphore->count > 0; });
-    } else {
-        auto rel_time = std::chrono::microseconds(timeout);
-        semaphore->cv.wait_for(lock, rel_time, [&]{ return semaphore->count > 0; });
+    if (semaphore->count <= 0) {
+        // Wait until condition or timeout is met
+        if (timeout == 0) {
+            semaphore->cv.wait(lock, [&]{ return semaphore->count > 0; });
+        } else {
+            auto rel_time = std::chrono::microseconds(timeout);
+            if (!semaphore->cv.wait_for(lock, rel_time, [&]{ return semaphore->count > 0; })) {
+                return CELL_ETIMEDOUT;
+            }
+        }
     }
 
-    if (semaphore->count == 0) {
-        return CELL_ETIMEDOUT;
-    }
     semaphore->count--;
     return CELL_OK;
 }