From 1475625705bf55f2d56e1b1a9fed246b9b43ef84 Mon Sep 17 00:00:00 2001
From: Elad <18193363+elad335@users.noreply.github.com>
Date: Sat, 16 Nov 2024 09:32:41 +0200
Subject: [PATCH] Implement lf_array::for_each

---
 Utilities/lockless.h     | 52 ++++++++++++++++++++++++++++++++++++++++
 rpcs3/util/vm_native.cpp | 32 ++++++++++---------------
 2 files changed, 65 insertions(+), 19 deletions(-)

diff --git a/Utilities/lockless.h b/Utilities/lockless.h
index 28d5d65246..7e10e107b8 100644
--- a/Utilities/lockless.h
+++ b/Utilities/lockless.h
@@ -72,6 +72,58 @@ public:
 		return *result;
 	}
 
+	template <typename F> requires (std::is_invocable_v<F, T&>)
+	auto for_each(F&& func, bool is_finite = true)
+	{
+		lf_array* _this = this;
+
+		using return_t = decltype(func(std::declval<T&>()));
+
+		for (usz i = 0; _this; i += N)
+		{
+			for (usz j = 0; j < N; j++)
+			{
+				if constexpr (std::is_void_v<return_t>)
+				{
+					std::invoke(func, _this->m_data[j]);
+				}
+				else
+				{
+					auto ret = std::invoke(func, _this->m_data[j]);
+
+					if (ret)
+					{
+						return std::make_pair(std::addressof(_this->m_data[j]), std::move(ret));
+					}
+				}
+			}
+
+			lf_array* next = m_next;
+
+			if (!next && !std::is_void_v<return_t> && !is_finite)
+			{
+				for (auto _new = new lf_array, ptr = _this; ptr;)
+				{
+					// Install the pointer. If failed, go deeper.
+					ptr = ptr->m_next.compare_and_swap(nullptr, _new);
+
+					if (!next)
+					{
+						// Determine the next pointer (if null then the new memory has been installed)
+						next = ptr ? ptr : _new;
+					}
+				}
+			}
+
+			_this = next;
+		}
+
+		if constexpr (!std::is_void_v<return_t>)
+		{
+			return std::make_pair(std::add_pointer_t<T>{}, return_t());
+		}
+	}
+
 	u64 size() const
 	{
 		u64 size_n = 0;
diff --git a/rpcs3/util/vm_native.cpp b/rpcs3/util/vm_native.cpp
index 8c9a5dd693..9f3d7cd02e 100644
--- a/rpcs3/util/vm_native.cpp
+++ b/rpcs3/util/vm_native.cpp
@@ -112,12 +112,8 @@ namespace utils
 			return false;
 		}
 
-		const u64 map_size = s_is_mapping.size();
-
-		for (u64 i = map_size - 1; i != umax; i--)
+		return s_is_mapping.for_each([addr](const map_info_t& info)
 		{
-			const auto& info = s_is_mapping[i];
-
 			if (info.state == 1)
 			{
 				if (addr >= info.addr && addr < info.addr + info.size)
@@ -125,24 +121,20 @@ namespace utils
 					return true;
 				}
 			}
-		}
 
-		return false;
+			return false;
+		}).second;
 	}
 
 	u64 unmap_mappping_memory(u64 addr, u64 size)
 	{
 		if (!addr || !size)
 		{
-			return false;
+			return 0;
 		}
 
-		const u64 map_size = s_is_mapping.size();
-
-		for (u64 i = map_size - 1; i != umax; i--)
+		return s_is_mapping.for_each([addr, size](map_info_t& info) -> u64
 		{
-			auto& info = s_is_mapping[i];
-
 			if (info.state == 1)
 			{
 				if (addr == info.addr && size == info.size)
@@ -153,9 +145,9 @@ namespace utils
 					}
 				}
 			}
-		}
 
-		return false;
+			return 0;
+		}).second;
 	}
 
 	bool map_mappping_memory(u64 addr, u64 size)
@@ -165,10 +157,8 @@ namespace utils
 			return false;
 		}
 
-		for (u64 i = 0;; i++)
+		ensure(s_is_mapping.for_each([addr, size](map_info_t& info)
 		{
-			auto& info = s_is_mapping[i];
-
 			if (!info.addr && info.state.compare_and_swap_test(0, 2))
 			{
 				info.addr = addr;
@@ -176,7 +166,11 @@ namespace utils
 				info.state = 1;
 				return true;
 			}
-		}
+
+			return false;
+		}, true).second);
+
+		return true;
 	}
 
 	bool is_memory_mappping_memory(const void* addr)