From e65bee0d6ad4b52f83802a1550c00706fe2e5aed Mon Sep 17 00:00:00 2001 From: Michael Scire Date: Sat, 3 Nov 2018 22:05:42 -0700 Subject: [PATCH] libstratosphere: Implement message queues --- .../libstratosphere/include/stratosphere.hpp | 1 + .../include/stratosphere/hossynch.hpp | 39 ++- .../include/stratosphere/message_queue.hpp | 73 ++++++ .../libstratosphere/source/message_queue.cpp | 235 ++++++++++++++++++ 4 files changed, 345 insertions(+), 3 deletions(-) create mode 100644 stratosphere/libstratosphere/include/stratosphere/message_queue.hpp create mode 100644 stratosphere/libstratosphere/source/message_queue.cpp diff --git a/stratosphere/libstratosphere/include/stratosphere.hpp b/stratosphere/libstratosphere/include/stratosphere.hpp index 503f6f7f1..209e80e7e 100644 --- a/stratosphere/libstratosphere/include/stratosphere.hpp +++ b/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" diff --git a/stratosphere/libstratosphere/include/stratosphere/hossynch.hpp b/stratosphere/libstratosphere/include/stratosphere/hossynch.hpp index 73ca16d76..64df3eafb 100644 --- a/stratosphere/libstratosphere/include/stratosphere/hossynch.hpp +++ b/stratosphere/libstratosphere/include/stratosphere/hossynch.hpp @@ -16,6 +16,7 @@ #pragma once #include +#include #include class HosMutex { @@ -100,15 +101,15 @@ class HosCondVar { condvarInit(&cv); } - Result WaitTimeout(u64 timeout, HosMutex *hm) { - return WaitTimeout(timeout, hm->GetMutex()); + Result TimedWait(u64 timeout, HosMutex *hm) { + 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; + } +}; \ No newline at end of file diff --git a/stratosphere/libstratosphere/include/stratosphere/message_queue.hpp b/stratosphere/libstratosphere/include/stratosphere/message_queue.hpp new file mode 100644 index 000000000..f8b4c6591 --- /dev/null +++ b/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 . + */ + +#pragma once +#include +#include "hossynch.hpp" +#include + +class HosMessageQueue { + private: + HosMutex queue_lock; + HosCondVar cv_not_full; + HosCondVar cv_not_empty; + std::unique_ptr buffer; + size_t capacity; + + size_t count = 0; + size_t offset = 0; + public: + HosMessageQueue(size_t c) : capacity(c) { + this->buffer = std::make_unique(this->capacity); + } + + HosMessageQueue(std::unique_ptr 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; + } + +}; + diff --git a/stratosphere/libstratosphere/source/message_queue.cpp b/stratosphere/libstratosphere/source/message_queue.cpp new file mode 100644 index 000000000..dc24cd937 --- /dev/null +++ b/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 . + */ + +#include +#include +#include + + +void HosMessageQueue::Send(uintptr_t data) { + /* Acquire mutex, wait sendable. */ + std::scoped_lock 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 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 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 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 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 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 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 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 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 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 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 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]; +} \ No newline at end of file