diff --git a/libraries/libmesosphere/include/mesosphere/arch/arm64/kern_cpu.hpp b/libraries/libmesosphere/include/mesosphere/arch/arm64/kern_cpu.hpp index 5cd51a910..c8b898f1b 100644 --- a/libraries/libmesosphere/include/mesosphere/arch/arm64/kern_cpu.hpp +++ b/libraries/libmesosphere/include/mesosphere/arch/arm64/kern_cpu.hpp @@ -149,6 +149,26 @@ namespace ams::kern::arch::arm64::cpu { return true; } + ALWAYS_INLINE bool CanAccessAtomic(KProcessAddress addr, bool privileged = false) { + const uintptr_t va = GetInteger(addr); + + u64 phys_addr; + if (privileged) { + __asm__ __volatile__("at s1e1w, %[va]" :: [va]"r"(va) : "memory"); + } else { + __asm__ __volatile__("at s1e0w, %[va]" :: [va]"r"(va) : "memory"); + } + InstructionMemoryBarrier(); + + u64 par = GetParEl1(); + + if (par & 0x1) { + return false; + } + + return (par >> BITSIZEOF(par) - BITSIZEOF(u8)) == 0xFF; + } + /* Synchronization helpers. */ NOINLINE void SynchronizeAllCores(); diff --git a/libraries/libmesosphere/include/mesosphere/arch/arm64/kern_userspace_memory_access.hpp b/libraries/libmesosphere/include/mesosphere/arch/arm64/kern_userspace_memory_access.hpp index db67b0acb..6783d399d 100644 --- a/libraries/libmesosphere/include/mesosphere/arch/arm64/kern_userspace_memory_access.hpp +++ b/libraries/libmesosphere/include/mesosphere/arch/arm64/kern_userspace_memory_access.hpp @@ -39,6 +39,9 @@ namespace ams::kern::arch::arm64 { static bool ClearMemoryAligned64Bit(void *dst, size_t size); static bool ClearMemorySize32Bit(void *dst); + static bool UpdateIfEqualAtomic(s32 *out, s32 *address, s32 compare_value, s32 new_value); + static bool DecrementIfLessThanAtomic(s32 *out, s32 *address, s32 compare); + static bool StoreDataCache(uintptr_t start, uintptr_t end); static bool FlushDataCache(uintptr_t start, uintptr_t end); static bool InvalidateDataCache(uintptr_t start, uintptr_t end); diff --git a/libraries/libmesosphere/include/mesosphere/kern_k_condition_variable.hpp b/libraries/libmesosphere/include/mesosphere/kern_k_condition_variable.hpp index 3c8928157..57aca7def 100644 --- a/libraries/libmesosphere/include/mesosphere/kern_k_condition_variable.hpp +++ b/libraries/libmesosphere/include/mesosphere/kern_k_condition_variable.hpp @@ -20,6 +20,8 @@ namespace ams::kern { + extern KThread g_cv_arbiter_compare_thread; + class KConditionVariable { public: using ThreadTree = typename KThread::ConditionVariableThreadTreeType; diff --git a/libraries/libmesosphere/include/mesosphere/kern_k_thread.hpp b/libraries/libmesosphere/include/mesosphere/kern_k_thread.hpp index 0439ae9f5..83aed923b 100644 --- a/libraries/libmesosphere/include/mesosphere/kern_k_thread.hpp +++ b/libraries/libmesosphere/include/mesosphere/kern_k_thread.hpp @@ -147,7 +147,7 @@ namespace ams::kern { KLightLock *waiting_lock{}; uintptr_t condvar_key{}; uintptr_t entrypoint{}; - KProcessAddress arbiter_key{}; + KProcessAddress address_key{}; KProcess *parent{}; void *kernel_stack_top{}; u32 *light_ipc_data{}; @@ -175,7 +175,7 @@ namespace ams::kern { KThread *lock_owner{}; ConditionVariableThreadTree *condvar_tree{}; uintptr_t debug_params[3]{}; - u32 arbiter_value{}; + u32 address_key_value{}; u32 suspend_request_flags{}; u32 suspend_allowed_flags{}; Result wait_result; @@ -304,6 +304,7 @@ namespace ams::kern { NOINLINE KThreadContext *GetContextForSchedulerLoop(); constexpr uintptr_t GetConditionVariableKey() const { return this->condvar_key; } + constexpr uintptr_t GetAddressArbiterKey() const { return this->condvar_key; } constexpr void SetupForConditionVariableCompare(uintptr_t cv_key, int priority) { this->condvar_key = cv_key; @@ -314,6 +315,11 @@ namespace ams::kern { this->condvar_tree = nullptr; } + constexpr void SetupForAddressArbiterCompare(uintptr_t address, int priority) { + this->condvar_key = address; + this->priority = priority; + } + constexpr void SetAddressArbiter(ConditionVariableThreadTree *tree, uintptr_t address) { this->condvar_tree = tree; this->condvar_key = address; @@ -349,10 +355,10 @@ namespace ams::kern { void RemoveWaiter(KThread *thread); KThread *RemoveWaiterByKey(s32 *out_num_waiters, KProcessAddress key); - constexpr KProcessAddress GetAddressKey() const { return this->arbiter_key; } - constexpr u32 GetAddressKeyValue() const { return this->arbiter_value; } - constexpr void SetAddressKey(KProcessAddress key) { this->arbiter_key = key; } - constexpr void SetAddressKey(KProcessAddress key, u32 val) { this->arbiter_key = key; this->arbiter_value = val; } + constexpr KProcessAddress GetAddressKey() const { return this->address_key; } + constexpr u32 GetAddressKeyValue() const { return this->address_key_value; } + constexpr void SetAddressKey(KProcessAddress key) { this->address_key = key; } + constexpr void SetAddressKey(KProcessAddress key, u32 val) { this->address_key = key; this->address_key_value = val; } constexpr void SetLockOwner(KThread *owner) { this->lock_owner = owner; } constexpr KThread *GetLockOwner() const { return this->lock_owner; } diff --git a/libraries/libmesosphere/source/arch/arm64/kern_userspace_memory_access_asm.s b/libraries/libmesosphere/source/arch/arm64/kern_userspace_memory_access_asm.s index d210466c9..5752a93b2 100644 --- a/libraries/libmesosphere/source/arch/arm64/kern_userspace_memory_access_asm.s +++ b/libraries/libmesosphere/source/arch/arm64/kern_userspace_memory_access_asm.s @@ -428,6 +428,67 @@ _ZN3ams4kern4arch5arm6415UserspaceAccess20ClearMemorySize32BitEPv: mov x0, #1 ret +/* ams::kern::arch::arm64::UserspaceAccess::UpdateIfEqualAtomic(s32 *out, s32 *address, s32 compare_value, s32 new_value) */ +.section .text._ZN3ams4kern4arch5arm6415UserspaceAccess19UpdateIfEqualAtomicEPiS4_ii, "ax", %progbits +.global _ZN3ams4kern4arch5arm6415UserspaceAccess19UpdateIfEqualAtomicEPiS4_ii +.type _ZN3ams4kern4arch5arm6415UserspaceAccess19UpdateIfEqualAtomicEPiS4_ii, %function +.balign 0x10 +_ZN3ams4kern4arch5arm6415UserspaceAccess19UpdateIfEqualAtomicEPiS4_ii: + /* Load the value from the address. */ + ldaxr w4, [x1] + + /* Compare it to the desired one. */ + cmp w4, w2 + + /* If equal, we want to try to write the new value. */ + b.eq 1f + + /* Otherwise, clear our exclusive hold and finish. */ + clrex + b 2f + +1: /* Try to store. */ + stlxr w5, w3, [x1] + + /* If we failed to store, try again. */ + cbnz w5, _ZN3ams4kern4arch5arm6415UserspaceAccess19UpdateIfEqualAtomicEPiS4_ii + +2: /* We're done. */ + str w4, [x0] + mov x0, #1 + ret + +/* ams::kern::arch::arm64::UserspaceAccess::DecrementIfLessThanAtomic(s32 *out, s32 *address, s32 compare) */ +.section .text._ZN3ams4kern4arch5arm6415UserspaceAccess25DecrementIfLessThanAtomicEPiS4_i, "ax", %progbits +.global _ZN3ams4kern4arch5arm6415UserspaceAccess25DecrementIfLessThanAtomicEPiS4_i +.type _ZN3ams4kern4arch5arm6415UserspaceAccess25DecrementIfLessThanAtomicEPiS4_i, %function +.balign 0x10 +_ZN3ams4kern4arch5arm6415UserspaceAccess25DecrementIfLessThanAtomicEPiS4_i: + /* Load the value from the address. */ + ldaxr w3, [x1] + + /* Compare it to the desired one. */ + cmp w3, w2 + + /* If less than, we want to try to decrement. */ + b.lt 1f + + /* Otherwise, clear our exclusive hold and finish. */ + clrex + b 2f + +1: /* Decrement and try to store. */ + sub w4, w3, #1 + stlxr w5, w4, [x1] + + /* If we failed to store, try again. */ + cbnz w5, _ZN3ams4kern4arch5arm6415UserspaceAccess25DecrementIfLessThanAtomicEPiS4_i + +2: /* We're done. */ + str w3, [x0] + mov x0, #1 + ret + /* ams::kern::arch::arm64::UserspaceAccess::StoreDataCache(uintptr_t start, uintptr_t end) */ .section .text._ZN3ams4kern4arch5arm6415UserspaceAccess14StoreDataCacheEmm, "ax", %progbits .global _ZN3ams4kern4arch5arm6415UserspaceAccess14StoreDataCacheEmm diff --git a/libraries/libmesosphere/source/kern_k_address_arbiter.cpp b/libraries/libmesosphere/source/kern_k_address_arbiter.cpp index 629e57839..dfcd40883 100644 --- a/libraries/libmesosphere/source/kern_k_address_arbiter.cpp +++ b/libraries/libmesosphere/source/kern_k_address_arbiter.cpp @@ -19,28 +19,212 @@ namespace ams::kern { namespace { - constinit KThread g_arbiter_compare_thread; - ALWAYS_INLINE bool ReadFromUser(s32 *out, KProcessAddress address) { return UserspaceAccess::CopyMemoryFromUserSize32Bit(out, GetVoidPointer(address)); } + ALWAYS_INLINE bool DecrementIfLessThan(s32 *out, KProcessAddress address, s32 value) { + KScopedInterruptDisable di; + + if (!cpu::CanAccessAtomic(address)) { + return false; + } + + return UserspaceAccess::DecrementIfLessThanAtomic(out, GetPointer(address), value); + } + + ALWAYS_INLINE bool UpdateIfEqual(s32 *out, KProcessAddress address, s32 value, s32 new_value) { + KScopedInterruptDisable di; + + if (!cpu::CanAccessAtomic(address)) { + return false; + } + + return UserspaceAccess::UpdateIfEqualAtomic(out, GetPointer(address), value, new_value); + } + } Result KAddressArbiter::Signal(uintptr_t addr, s32 count) { - MESOSPHERE_UNIMPLEMENTED(); + /* Perform signaling. */ + s32 num_waiters = 0; + { + KScopedSchedulerLock sl; + g_cv_arbiter_compare_thread.SetupForAddressArbiterCompare(addr, -1); + + auto it = this->tree.nfind(g_cv_arbiter_compare_thread); + while ((it != this->tree.end()) && (count <= 0 || num_waiters < count) && (it->GetAddressArbiterKey() == addr)) { + KThread *target_thread = std::addressof(*it); + target_thread->SetSyncedObject(nullptr, ResultSuccess()); + + AMS_ASSERT(target_thread->IsWaitingForAddressArbiter()); + target_thread->Wakeup(); + + it = this->tree.erase(it); + target_thread->ClearAddressArbiter(); + ++num_waiters; + } + } + return ResultSuccess(); } Result KAddressArbiter::SignalAndIncrementIfEqual(uintptr_t addr, s32 value, s32 count) { - MESOSPHERE_UNIMPLEMENTED(); + /* Perform signaling. */ + s32 num_waiters = 0; + { + KScopedSchedulerLock sl; + g_cv_arbiter_compare_thread.SetupForAddressArbiterCompare(addr, -1); + + auto it = this->tree.nfind(g_cv_arbiter_compare_thread); + + /* Check the userspace value. */ + s32 user_value; + R_UNLESS(UpdateIfEqual(std::addressof(user_value), addr, value, value + 1), svc::ResultInvalidCurrentMemory()); + R_UNLESS(user_value == value, svc::ResultInvalidState()); + + while ((it != this->tree.end()) && (count <= 0 || num_waiters < count) && (it->GetAddressArbiterKey() == addr)) { + KThread *target_thread = std::addressof(*it); + target_thread->SetSyncedObject(nullptr, ResultSuccess()); + + AMS_ASSERT(target_thread->IsWaitingForAddressArbiter()); + target_thread->Wakeup(); + + it = this->tree.erase(it); + target_thread->ClearAddressArbiter(); + ++num_waiters; + } + } + return ResultSuccess(); } Result KAddressArbiter::SignalAndModifyByWaitingCountIfEqual(uintptr_t addr, s32 value, s32 count) { - MESOSPHERE_UNIMPLEMENTED(); + /* Perform signaling. */ + s32 num_waiters = 0; + { + KScopedSchedulerLock sl; + g_cv_arbiter_compare_thread.SetupForAddressArbiterCompare(addr, -1); + + auto it = this->tree.nfind(g_cv_arbiter_compare_thread); + + /* Determine the updated value. */ + s32 new_value; + if (count <= 0) { + if ((it != this->tree.end()) && (it->GetAddressArbiterKey() == addr)) { + new_value = value - 1; + } else { + new_value = value + 1; + } + } else { + auto tmp_it = it; + int tmp_num_waiters = 0; + while ((tmp_it != this->tree.end()) && (tmp_it->GetAddressArbiterKey() == addr) && (tmp_num_waiters < count + 1)) { + ++tmp_num_waiters; + ++tmp_it; + } + + if (tmp_num_waiters == 0) { + new_value = value + 1; + } else if (tmp_num_waiters <= count) { + new_value = value - 1; + } else { + new_value = value; + } + } + + /* Check the userspace value. */ + s32 user_value; + bool succeeded; + if (value != new_value) { + succeeded = UpdateIfEqual(std::addressof(user_value), addr, value, new_value); + } else { + succeeded = ReadFromUser(std::addressof(user_value), addr); + } + + R_UNLESS(succeeded, svc::ResultInvalidCurrentMemory()); + R_UNLESS(user_value == value, svc::ResultInvalidState()); + + while ((it != this->tree.end()) && (count <= 0 || num_waiters < count) && (it->GetAddressArbiterKey() == addr)) { + KThread *target_thread = std::addressof(*it); + target_thread->SetSyncedObject(nullptr, ResultSuccess()); + + AMS_ASSERT(target_thread->IsWaitingForAddressArbiter()); + target_thread->Wakeup(); + + it = this->tree.erase(it); + target_thread->ClearAddressArbiter(); + ++num_waiters; + } + } + return ResultSuccess(); } Result KAddressArbiter::WaitIfLessThan(uintptr_t addr, s32 value, bool decrement, s64 timeout) { - MESOSPHERE_UNIMPLEMENTED(); + /* Prepare to wait. */ + KThread *cur_thread = GetCurrentThreadPointer(); + KHardwareTimer *timer; + + { + KScopedSchedulerLockAndSleep slp(std::addressof(timer), cur_thread, timeout); + + /* Check that the thread isn't terminating. */ + if (cur_thread->IsTerminationRequested()) { + slp.CancelSleep(); + return svc::ResultTerminationRequested(); + } + + /* Set the synced object. */ + cur_thread->SetSyncedObject(nullptr, ams::svc::ResultTimedOut()); + + /* Read the value from userspace. */ + s32 user_value; + bool succeeded; + if (decrement) { + succeeded = DecrementIfLessThan(std::addressof(user_value), addr, value); + } else { + succeeded = ReadFromUser(std::addressof(user_value), addr); + } + + if (!succeeded) { + slp.CancelSleep(); + return svc::ResultInvalidCurrentMemory(); + } + + /* Check that the value is less than the specified one. */ + if (user_value >= value) { + slp.CancelSleep(); + return svc::ResultInvalidState(); + } + + /* Check that the timeout is non-zero. */ + if (timeout == 0) { + slp.CancelSleep(); + return svc::ResultTimedOut(); + } + + /* Set the arbiter. */ + cur_thread->SetAddressArbiter(std::addressof(this->tree), addr); + this->tree.insert(*cur_thread); + cur_thread->SetState(KThread::ThreadState_Waiting); + } + + /* Cancel the timer wait. */ + if (timer != nullptr) { + timer->CancelTask(cur_thread); + } + + /* Remove from the address arbiter. */ + { + KScopedSchedulerLock sl; + + if (cur_thread->IsWaitingForAddressArbiter()) { + this->tree.erase(this->tree.iterator_to(*cur_thread)); + cur_thread->ClearAddressArbiter(); + } + } + + /* Get the result. */ + KSynchronizationObject *dummy; + return cur_thread->GetWaitResult(std::addressof(dummy)); } Result KAddressArbiter::WaitIfEqual(uintptr_t addr, s32 value, s64 timeout) { diff --git a/libraries/libmesosphere/source/kern_k_condition_variable.cpp b/libraries/libmesosphere/source/kern_k_condition_variable.cpp index 50df876cb..3d2ba92fe 100644 --- a/libraries/libmesosphere/source/kern_k_condition_variable.cpp +++ b/libraries/libmesosphere/source/kern_k_condition_variable.cpp @@ -17,9 +17,9 @@ namespace ams::kern { - namespace { + constinit KThread g_cv_arbiter_compare_thread; - constinit KThread g_cv_compare_thread; + namespace { ALWAYS_INLINE bool ReadFromUser(u32 *out, KProcessAddress address) { return UserspaceAccess::CopyMemoryFromUserSize32Bit(out, GetVoidPointer(address)); @@ -131,9 +131,9 @@ namespace ams::kern { int num_waiters = 0; { KScopedSchedulerLock sl; - g_cv_compare_thread.SetupForConditionVariableCompare(cv_key, -1); + g_cv_arbiter_compare_thread.SetupForConditionVariableCompare(cv_key, -1); - auto it = this->tree.nfind(g_cv_compare_thread); + auto it = this->tree.nfind(g_cv_arbiter_compare_thread); while ((it != this->tree.end()) && (count <= 0 || num_waiters < count) && (it->GetConditionVariableKey() == cv_key)) { KThread *target_thread = std::addressof(*it);