libstratosphere: Implement message queues

stratosphere/libstratosphere/include/stratosphere.hpp

@@ -20,6 +20,7 @@
 #include "stratosphere/scope_guard.hpp"
 
 #include "stratosphere/hossynch.hpp"
+#include "stratosphere/message_queue.hpp"
 #include "stratosphere/iwaitable.hpp"
 #include "stratosphere/event.hpp"
 

stratosphere/libstratosphere/include/stratosphere/hossynch.hpp

@@ -16,6 +16,7 @@
  
 #pragma once
 #include <switch.h>
+#include <switch/arm/counter.h>
 #include <mutex>
 
 class HosMutex {
@@ -100,15 +101,15 @@ class HosCondVar {
             condvarInit(&cv);
         }
         
-        Result WaitTimeout(u64 timeout, HosMutex *hm) {
+        Result TimedWait(u64 timeout, HosMutex *hm) {
-            return WaitTimeout(timeout, hm->GetMutex());
+            return TimedWait(timeout, hm->GetMutex());
         }
         
         Result Wait(HosMutex *hm) {
             return Wait(hm->GetMutex());
         }
         
-        Result WaitTimeout(u64 timeout, Mutex *m) {
+        Result TimedWait(u64 timeout, Mutex *m) {
             return condvarWaitTimeout(&cv, m, timeout);
         }
         
@@ -153,3 +154,35 @@ class HosSemaphore {
             return semaphoreTryWait(&s);
         }
 };
+
+class TimeoutHelper {
+    private:
+        u64 end_tick;
+    public:
+        TimeoutHelper(u64 ns) {
+            /* Special case zero-time timeouts. */
+            if (ns == 0) {
+                end_tick = 0;
+                return;
+            }
+            
+            u64 cur_tick = armGetSystemTick();
+            this->end_tick = cur_tick + NsToTick(ns) + 1;
+        }
+        
+        static inline u64 NsToTick(u64 ns) {
+            return (ns * 12) / 625;
+        }
+        
+        static inline u64 TickToNs(u64 tick) {
+            return (tick * 625) / 12;
+        }
+        
+        bool TimedOut() {
+            if (this->end_tick == 0) {
+                return true;
+            }
+            
+            return armGetSystemTick() >= this->end_tick;
+        }
+};

stratosphere/libstratosphere/include/stratosphere/message_queue.hpp

@@ -0,0 +1,73 @@
+/*
+ * Copyright (c) 2018 Atmosphère-NX
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+ 
+#pragma once
+#include <switch.h>
+#include "hossynch.hpp"
+#include <memory>
+
+class HosMessageQueue {
+    private:
+        HosMutex queue_lock;
+        HosCondVar cv_not_full;
+        HosCondVar cv_not_empty;
+        std::unique_ptr<uintptr_t[]> buffer;
+        size_t capacity;
+        
+        size_t count = 0;
+        size_t offset = 0;
+    public:
+        HosMessageQueue(size_t c) : capacity(c) {
+            this->buffer = std::make_unique<uintptr_t[]>(this->capacity);
+        }
+        
+        HosMessageQueue(std::unique_ptr<uintptr_t[]> buf, size_t c) : buffer(std::move(buf)), capacity(c) { }
+        
+        /* Sending (FIFO functionality) */
+        void Send(uintptr_t data);
+        bool TrySend(uintptr_t data);
+        bool TimedSend(uintptr_t data, u64 timeout);
+        
+        /* Sending (LIFO functionality) */
+        void SendNext(uintptr_t data);
+        bool TrySendNext(uintptr_t data);
+        bool TimedSendNext(uintptr_t data, u64 timeout);
+        
+        /* Receive functionality */
+        void Receive(uintptr_t *out);
+        bool TryReceive(uintptr_t *out);
+        bool TimedReceive(uintptr_t *out, u64 timeout);
+        
+        /* Peek functionality */
+        void Peek(uintptr_t *out);
+        bool TryPeek(uintptr_t *out);
+        bool TimedPeek(uintptr_t *out, u64 timeout);
+    private:
+        void SendInternal(uintptr_t data);
+        void SendNextInternal(uintptr_t data);
+        uintptr_t ReceiveInternal();
+        uintptr_t PeekInternal();
+        
+        bool IsFull() {
+            return this->count >= this->capacity;
+        }
+        
+        bool IsEmpty() {
+            return this->count == 0;
+        }
+
+};
+

stratosphere/libstratosphere/source/message_queue.cpp

@@ -0,0 +1,235 @@
+/*
+ * Copyright (c) 2018 Atmosphère-NX
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+ 
+#include <mutex>
+#include <switch.h>
+#include <stratosphere.hpp>
+
+        
+void HosMessageQueue::Send(uintptr_t data) {
+    /* Acquire mutex, wait sendable. */
+    std::scoped_lock<HosMutex> lock(this->queue_lock);
+    
+    while (this->IsFull()) {
+        this->cv_not_full.Wait(&this->queue_lock);
+    }
+    
+    /* Send, signal. */
+    this->SendInternal(data);
+    this->cv_not_empty.WakeAll();
+}
+
+bool HosMessageQueue::TrySend(uintptr_t data) {
+    std::scoped_lock<HosMutex> lock(this->queue_lock);
+    if (this->IsFull()) {
+        return false;
+    }
+    
+    /* Send, signal. */
+    this->SendInternal(data);
+    this->cv_not_empty.WakeAll();
+    return true;
+}
+
+bool HosMessageQueue::TimedSend(uintptr_t data, u64 timeout) {
+    std::scoped_lock<HosMutex> lock(this->queue_lock);
+    TimeoutHelper timeout_helper(timeout);
+    
+    while (this->IsFull()) {
+        if (timeout_helper.TimedOut()) {
+            return false;
+        }
+        
+        this->cv_not_full.TimedWait(timeout, &this->queue_lock);
+    }
+    
+    /* Send, signal. */
+    this->SendInternal(data);
+    this->cv_not_empty.WakeAll();
+    return true;
+}
+
+void HosMessageQueue::SendNext(uintptr_t data) {
+    /* Acquire mutex, wait sendable. */
+    std::scoped_lock<HosMutex> lock(this->queue_lock);
+    
+    while (this->IsFull()) {
+        this->cv_not_full.Wait(&this->queue_lock);
+    }
+    
+    /* Send, signal. */
+    this->SendNextInternal(data);
+    this->cv_not_empty.WakeAll();
+}
+
+bool HosMessageQueue::TrySendNext(uintptr_t data) {
+    std::scoped_lock<HosMutex> lock(this->queue_lock);
+    if (this->IsFull()) {
+        return false;
+    }
+    
+    /* Send, signal. */
+    this->SendNextInternal(data);
+    this->cv_not_empty.WakeAll();
+    return true;
+}
+
+bool HosMessageQueue::TimedSendNext(uintptr_t data, u64 timeout) {
+    std::scoped_lock<HosMutex> lock(this->queue_lock);
+    TimeoutHelper timeout_helper(timeout);
+    
+    while (this->IsFull()) {
+        if (timeout_helper.TimedOut()) {
+            return false;
+        }
+        
+        this->cv_not_full.TimedWait(timeout, &this->queue_lock);
+    }
+    
+    /* Send, signal. */
+    this->SendNextInternal(data);
+    this->cv_not_empty.WakeAll();
+    return true;
+}
+
+void HosMessageQueue::Receive(uintptr_t *out) {
+    /* Acquire mutex, wait receivable. */
+    std::scoped_lock<HosMutex> lock(this->queue_lock);
+    
+    while (this->IsEmpty()) {
+        this->cv_not_empty.Wait(&this->queue_lock);
+    }
+    
+    /* Receive, signal. */
+    *out = this->ReceiveInternal();
+    this->cv_not_full.WakeAll();
+}
+bool HosMessageQueue::TryReceive(uintptr_t *out) {
+    /* Acquire mutex, wait receivable. */
+    std::scoped_lock<HosMutex> lock(this->queue_lock);
+    
+    if (this->IsEmpty()) {
+        return false;
+    }
+    
+    /* Receive, signal. */
+    *out = this->ReceiveInternal();
+    this->cv_not_full.WakeAll();
+    return true;
+}
+
+bool HosMessageQueue::TimedReceive(uintptr_t *out, u64 timeout) {
+    std::scoped_lock<HosMutex> lock(this->queue_lock);
+    TimeoutHelper timeout_helper(timeout);
+    
+    while (this->IsEmpty()) {
+        if (timeout_helper.TimedOut()) {
+            return false;
+        }
+        
+        this->cv_not_empty.TimedWait(timeout, &this->queue_lock);
+    }
+    
+    /* Receive, signal. */
+    *out = this->ReceiveInternal();
+    this->cv_not_full.WakeAll();
+    return true;
+}
+
+void HosMessageQueue::Peek(uintptr_t *out) {
+    /* Acquire mutex, wait receivable. */
+    std::scoped_lock<HosMutex> lock(this->queue_lock);
+    
+    while (this->IsEmpty()) {
+        this->cv_not_empty.Wait(&this->queue_lock);
+    }
+    
+    /* Peek. */
+    *out = this->PeekInternal();
+}
+
+bool HosMessageQueue::TryPeek(uintptr_t *out) {
+    /* Acquire mutex, wait receivable. */
+    std::scoped_lock<HosMutex> lock(this->queue_lock);
+    
+    if (this->IsEmpty()) {
+        return false;
+    }
+    
+    /* Peek. */
+    *out = this->PeekInternal();
+    return true;
+}
+
+bool HosMessageQueue::TimedPeek(uintptr_t *out, u64 timeout) {
+    std::scoped_lock<HosMutex> lock(this->queue_lock);
+    TimeoutHelper timeout_helper(timeout);
+    
+    while (this->IsEmpty()) {
+        if (timeout_helper.TimedOut()) {
+            return false;
+        }
+        
+        this->cv_not_empty.TimedWait(timeout, &this->queue_lock);
+    }
+    
+    /* Peek. */
+    *out = this->PeekInternal();
+    return true;
+}
+
+void HosMessageQueue::SendInternal(uintptr_t data) {
+    /* Ensure we don't corrupt the queue, but this should never happen. */
+    if (this->count >= this->capacity) {
+        std::abort();
+    }
+    
+    /* Write data to tail of queue. */
+    this->buffer[(this->count++ + this->offset) % this->capacity] = data;
+}
+
+void HosMessageQueue::SendNextInternal(uintptr_t data) {
+    /* Ensure we don't corrupt the queue, but this should never happen. */
+    if (this->count >= this->capacity) {
+        std::abort();
+    }
+    
+    /* Write data to head of queue. */
+    this->offset = (this->offset + this->capacity - 1) % this->capacity;
+    this->buffer[this->offset] = data;
+    this->count++;
+}
+
+uintptr_t HosMessageQueue::ReceiveInternal() {
+    /* Ensure we don't corrupt the queue, but this should never happen. */
+    if (this->count == 0) {
+        std::abort();
+    }
+    
+    uintptr_t data = this->buffer[this->offset];
+    this->offset = (this->offset + 1) % this->capacity;
+    this->count--;
+    return data;
+}
+
+uintptr_t HosMessageQueue::PeekInternal() {
+    /* Ensure we don't corrupt the queue, but this should never happen. */
+    if (this->count == 0) {
+        std::abort();
+    }
+    
+    return this->buffer[this->offset];
+}