From 2290c389d630b4d39456796cb071b6cd37a42199 Mon Sep 17 00:00:00 2001
From: Nekotekina <nekotekina@gmail.com>
Date: Tue, 26 Nov 2019 00:01:05 +0300
Subject: [PATCH] vm: implement vm::try_access, vm::ptr::try_read/write

---
 rpcs3/Emu/Cell/lv2/sys_gpio.cpp |  6 ++---
 rpcs3/Emu/Memory/vm.cpp         | 41 +++++++++++++++++++++++++++++++++
 rpcs3/Emu/Memory/vm.h           |  3 +++
 rpcs3/Emu/Memory/vm_ptr.h       | 10 ++++++++
 4 files changed, 56 insertions(+), 4 deletions(-)

diff --git a/rpcs3/Emu/Cell/lv2/sys_gpio.cpp b/rpcs3/Emu/Cell/lv2/sys_gpio.cpp
index bef798c5ae..e76f36cd9e 100644
--- a/rpcs3/Emu/Cell/lv2/sys_gpio.cpp
+++ b/rpcs3/Emu/Cell/lv2/sys_gpio.cpp
@@ -15,14 +15,12 @@ error_code sys_gpio_get(u64 device_id, vm::ptr<u64> value)
 		return CELL_ESRCH;
 	}
 
-	if (!vm::check_addr(value.addr(), value.size(), vm::page_writable))
+	// Retail consoles dont have LEDs or DIPs switches, hence always sets 0 in paramenter
+	if (!value.try_write(0))
 	{
 		return CELL_EFAULT;
 	}
 
-	// Retail consoles dont have LEDs or DIPs switches, hence always sets 0 in paramenter
-	*value = 0;
-
 	return CELL_OK;
 }
 
diff --git a/rpcs3/Emu/Memory/vm.cpp b/rpcs3/Emu/Memory/vm.cpp
index 8c3c714fa8..acd5566444 100644
--- a/rpcs3/Emu/Memory/vm.cpp
+++ b/rpcs3/Emu/Memory/vm.cpp
@@ -1078,6 +1078,47 @@ namespace vm
 		return _map(addr, area_size, flags);
 	}
 
+	bool try_access(u32 addr, void* ptr, u32 size, bool is_write)
+	{
+		vm::reader_lock lock;
+
+		if (size == 0)
+		{
+			return true;
+		}
+
+		if (vm::check_addr(addr, size, is_write ? page_writable : page_readable))
+		{
+			void* src = vm::g_sudo_addr + addr;
+			void* dst = ptr;
+
+			if (is_write)
+				std::swap(src, dst);
+
+			if (size <= 16 && utils::popcnt32(size) == 1 && (addr & (size - 1)) == 0)
+			{
+				if (is_write)
+				{
+					switch (size)
+					{
+					case 1: atomic_storage<u8>::release(*static_cast<u8*>(dst), *static_cast<u8*>(src)); break;
+					case 2: atomic_storage<u16>::release(*static_cast<u16*>(dst), *static_cast<u16*>(src)); break;
+					case 4: atomic_storage<u32>::release(*static_cast<u32*>(dst), *static_cast<u32*>(src)); break;
+					case 8: atomic_storage<u64>::release(*static_cast<u64*>(dst), *static_cast<u64*>(src)); break;
+					case 16: _mm_store_si128(static_cast<__m128i*>(dst), _mm_loadu_si128(static_cast<__m128i*>(src))); break;
+					}
+
+					return true;
+				}
+			}
+
+			std::memcpy(dst, src, size);
+			return true;
+		}
+
+		return false;
+	}
+
 	inline namespace ps3_
 	{
 		void init()
diff --git a/rpcs3/Emu/Memory/vm.h b/rpcs3/Emu/Memory/vm.h
index cf69d302a4..153b49ebdd 100644
--- a/rpcs3/Emu/Memory/vm.h
+++ b/rpcs3/Emu/Memory/vm.h
@@ -216,6 +216,9 @@ namespace vm
 		g_base_addr[addr] = value;
 	}
 
+	// Read or write virtual memory in a safe manner, returns false on failure
+	bool try_access(u32 addr, void* ptr, u32 size, bool is_write);
+
 	inline namespace ps3_
 	{
 		// Convert specified PS3 address to a pointer of specified (possibly converted to BE) type
diff --git a/rpcs3/Emu/Memory/vm_ptr.h b/rpcs3/Emu/Memory/vm_ptr.h
index 01eea01347..6f6a6532f8 100644
--- a/rpcs3/Emu/Memory/vm_ptr.h
+++ b/rpcs3/Emu/Memory/vm_ptr.h
@@ -199,6 +199,16 @@ namespace vm
 			m_addr = vm::cast(m_addr, HERE) - count * size();
 			return *this;
 		}
+
+		bool try_read(std::conditional_t<std::is_void_v<T>, char&, std::add_lvalue_reference_t<std::remove_const_t<T>>> out) const
+		{
+			return vm::try_access(vm::cast(m_addr, HERE), &out, sizeof(T), false);
+		}
+
+		bool try_write(std::conditional_t<std::is_void_v<T>, const char&, std::add_lvalue_reference_t<const T>> _in) const
+		{
+			return vm::try_access(vm::cast(m_addr, HERE), const_cast<T*>(&_in), sizeof(T), true);
+		}
 	};
 
 	template<typename AT, typename RT, typename... T>