1 files changed, 102 insertions, 7 deletions

diff --git a/src/core/memory.cpp b/src/core/memory.cpp
index 9d87045a0..7def00768 100644
--- a/src/core/memory.cpp
+++ b/src/core/memory.cpp
@@ -8,6 +8,7 @@
 #include <utility>
 
 #include "common/assert.h"
+#include "common/atomic_ops.h"
 #include "common/common_types.h"
 #include "common/logging/log.h"
 #include "common/page_table.h"
@@ -29,15 +30,12 @@ namespace Core::Memory {
 struct Memory::Impl {
     explicit Impl(Core::System& system_) : system{system_} {}
 
-    void SetCurrentPageTable(Kernel::Process& process) {
+    void SetCurrentPageTable(Kernel::Process& process, u32 core_id) {
         current_page_table = &process.PageTable().PageTableImpl();
 
         const std::size_t address_space_width = process.PageTable().GetAddressSpaceWidth();
 
-        system.ArmInterface(0).PageTableChanged(*current_page_table, address_space_width);
-        system.ArmInterface(1).PageTableChanged(*current_page_table, address_space_width);
-        system.ArmInterface(2).PageTableChanged(*current_page_table, address_space_width);
-        system.ArmInterface(3).PageTableChanged(*current_page_table, address_space_width);
+        system.ArmInterface(core_id).PageTableChanged(*current_page_table, address_space_width);
     }
 
     void MapMemoryRegion(Common::PageTable& page_table, VAddr base, u64 size, PAddr target) {
@@ -179,6 +177,22 @@ struct Memory::Impl {
         }
     }
 
+    bool WriteExclusive8(const VAddr addr, const u8 data, const u8 expected) {
+        return WriteExclusive<u8>(addr, data, expected);
+    }
+
+    bool WriteExclusive16(const VAddr addr, const u16 data, const u16 expected) {
+        return WriteExclusive<u16_le>(addr, data, expected);
+    }
+
+    bool WriteExclusive32(const VAddr addr, const u32 data, const u32 expected) {
+        return WriteExclusive<u32_le>(addr, data, expected);
+    }
+
+    bool WriteExclusive64(const VAddr addr, const u64 data, const u64 expected) {
+        return WriteExclusive<u64_le>(addr, data, expected);
+    }
+
     std::string ReadCString(VAddr vaddr, std::size_t max_length) {
         std::string string;
         string.reserve(max_length);
@@ -682,6 +696,67 @@ struct Memory::Impl {
         }
     }
 
+    template <typename T>
+    bool WriteExclusive(const VAddr vaddr, const T data, const T expected) {
+        u8* page_pointer = current_page_table->pointers[vaddr >> PAGE_BITS];
+        if (page_pointer != nullptr) {
+            // NOTE: Avoid adding any extra logic to this fast-path block
+            T volatile* pointer = reinterpret_cast<T volatile*>(&page_pointer[vaddr]);
+            return Common::AtomicCompareAndSwap(pointer, data, expected);
+        }
+
+        const Common::PageType type = current_page_table->attributes[vaddr >> PAGE_BITS];
+        switch (type) {
+        case Common::PageType::Unmapped:
+            LOG_ERROR(HW_Memory, "Unmapped Write{} 0x{:08X} @ 0x{:016X}", sizeof(data) * 8,
+                      static_cast<u32>(data), vaddr);
+            return true;
+        case Common::PageType::Memory:
+            ASSERT_MSG(false, "Mapped memory page without a pointer @ {:016X}", vaddr);
+            break;
+        case Common::PageType::RasterizerCachedMemory: {
+            u8* host_ptr{GetPointerFromRasterizerCachedMemory(vaddr)};
+            system.GPU().InvalidateRegion(vaddr, sizeof(T));
+            T volatile* pointer = reinterpret_cast<T volatile*>(&host_ptr);
+            return Common::AtomicCompareAndSwap(pointer, data, expected);
+            break;
+        }
+        default:
+            UNREACHABLE();
+        }
+        return true;
+    }
+
+    bool WriteExclusive128(const VAddr vaddr, const u128 data, const u128 expected) {
+        u8* const page_pointer = current_page_table->pointers[vaddr >> PAGE_BITS];
+        if (page_pointer != nullptr) {
+            // NOTE: Avoid adding any extra logic to this fast-path block
+            u64 volatile* pointer = reinterpret_cast<u64 volatile*>(&page_pointer[vaddr]);
+            return Common::AtomicCompareAndSwap(pointer, data, expected);
+        }
+
+        const Common::PageType type = current_page_table->attributes[vaddr >> PAGE_BITS];
+        switch (type) {
+        case Common::PageType::Unmapped:
+            LOG_ERROR(HW_Memory, "Unmapped Write{} 0x{:08X} @ 0x{:016X}{:016X}", sizeof(data) * 8,
+                      static_cast<u64>(data[1]), static_cast<u64>(data[0]), vaddr);
+            return true;
+        case Common::PageType::Memory:
+            ASSERT_MSG(false, "Mapped memory page without a pointer @ {:016X}", vaddr);
+            break;
+        case Common::PageType::RasterizerCachedMemory: {
+            u8* host_ptr{GetPointerFromRasterizerCachedMemory(vaddr)};
+            system.GPU().InvalidateRegion(vaddr, sizeof(u128));
+            u64 volatile* pointer = reinterpret_cast<u64 volatile*>(&host_ptr);
+            return Common::AtomicCompareAndSwap(pointer, data, expected);
+            break;
+        }
+        default:
+            UNREACHABLE();
+        }
+        return true;
+    }
+
     Common::PageTable* current_page_table = nullptr;
     Core::System& system;
 };
@@ -689,8 +764,8 @@ struct Memory::Impl {
 Memory::Memory(Core::System& system) : impl{std::make_unique<Impl>(system)} {}
 Memory::~Memory() = default;
 
-void Memory::SetCurrentPageTable(Kernel::Process& process) {
-    impl->SetCurrentPageTable(process);
+void Memory::SetCurrentPageTable(Kernel::Process& process, u32 core_id) {
+    impl->SetCurrentPageTable(process, core_id);
 }
 
 void Memory::MapMemoryRegion(Common::PageTable& page_table, VAddr base, u64 size, PAddr target) {
@@ -764,6 +839,26 @@ void Memory::Write64(VAddr addr, u64 data) {
     impl->Write64(addr, data);
 }
 
+bool Memory::WriteExclusive8(VAddr addr, u8 data, u8 expected) {
+    return impl->WriteExclusive8(addr, data, expected);
+}
+
+bool Memory::WriteExclusive16(VAddr addr, u16 data, u16 expected) {
+    return impl->WriteExclusive16(addr, data, expected);
+}
+
+bool Memory::WriteExclusive32(VAddr addr, u32 data, u32 expected) {
+    return impl->WriteExclusive32(addr, data, expected);
+}
+
+bool Memory::WriteExclusive64(VAddr addr, u64 data, u64 expected) {
+    return impl->WriteExclusive64(addr, data, expected);
+}
+
+bool Memory::WriteExclusive128(VAddr addr, u128 data, u128 expected) {
+    return impl->WriteExclusive128(addr, data, expected);
+}
+
 std::string Memory::ReadCString(VAddr vaddr, std::size_t max_length) {
     return impl->ReadCString(vaddr, max_length);
 }