From 4a56873541eab436a20b02e596ad419b3fb950a8 Mon Sep 17 00:00:00 2001
From: "Unknown W. Brackets" <checkins@unknownbrackets.org>
Date: Thu, 17 Jan 2013 00:45:13 -0800
Subject: [PATCH] Properly wake semaphores in priority order.

---
 Core/HLE/sceKernelSemaphore.cpp | 39 +++++++--------------------------
 Core/HLE/sceKernelThread.cpp    |  5 +++++
 Core/HLE/sceKernelThread.h      |  1 +
 3 files changed, 14 insertions(+), 31 deletions(-)

diff --git a/Core/HLE/sceKernelSemaphore.cpp b/Core/HLE/sceKernelSemaphore.cpp
index 8fbeb4e674..9790f343ca 100644
--- a/Core/HLE/sceKernelSemaphore.cpp
+++ b/Core/HLE/sceKernelSemaphore.cpp
@@ -135,26 +135,6 @@ bool __KernelClearSemaThreads(Semaphore *s, int reason)
 	return wokeThreads;
 }
 
-std::vector<SceUID>::iterator __KernelSemaFindPriority(std::vector<SceUID> &waiting, std::vector<SceUID>::iterator begin)
-{
-	_dbg_assert_msg_(HLE, !waiting.empty(), "__KernelSemaFindPriority: Trying to find best of no threads.");
-
-	std::vector<SceUID>::iterator iter, end, best = waiting.end();
-	u32 best_prio = 0xFFFFFFFF;
-	for (iter = begin, end = waiting.end(); iter != end; ++iter)
-	{
-		u32 iter_prio = __KernelGetThreadPrio(*iter);
-		if (iter_prio < best_prio)
-		{
-			best = iter;
-			best_prio = iter_prio;
-		}
-	}
-
-	_dbg_assert_msg_(HLE, best != waiting.end(), "__KernelSemaFindPriority: Returning invalid best thread.");
-	return best;
-}
-
 int sceKernelCancelSema(SceUID id, int newCount, u32 numWaitThreadsPtr)
 {
 	DEBUG_LOG(HLE, "sceKernelCancelSema(%i)", id);
@@ -273,19 +253,16 @@ int sceKernelSignalSema(SceUID id, int signal)
 		s->ns.currentCount += signal;
 		DEBUG_LOG(HLE, "sceKernelSignalSema(%i, %i) (old: %i, new: %i)", id, signal, oldval, s->ns.currentCount);
 
-		bool wokeThreads = false;
-		std::vector<SceUID>::iterator iter, end, best;
-retry:
-		for (iter = s->waitingThreads.begin(), end = s->waitingThreads.end(); iter != end; ++iter)
-		{
-			if ((s->ns.attr & PSP_SEMA_ATTR_PRIORITY) != 0)
-				best = __KernelSemaFindPriority(s->waitingThreads, iter);
-			else
-				best = iter;
+		if ((s->ns.attr & PSP_SEMA_ATTR_PRIORITY) != 0)
+			std::stable_sort(s->waitingThreads.begin(), s->waitingThreads.end(), __KernelThreadSortPriority);
 
-			if (__KernelUnlockSemaForThread(s, *best, error, 0, wokeThreads))
+		bool wokeThreads = false;
+retry:
+		for (auto iter = s->waitingThreads.begin(), end = s->waitingThreads.end(); iter != end; ++iter)
+		{
+			if (__KernelUnlockSemaForThread(s, *iter, error, 0, wokeThreads))
 			{
-				s->waitingThreads.erase(best);
+				s->waitingThreads.erase(iter);
 				goto retry;
 			}
 		}
diff --git a/Core/HLE/sceKernelThread.cpp b/Core/HLE/sceKernelThread.cpp
index 6d8f426c30..c23fd232a9 100644
--- a/Core/HLE/sceKernelThread.cpp
+++ b/Core/HLE/sceKernelThread.cpp
@@ -1793,6 +1793,11 @@ u32 __KernelGetThreadPrio(SceUID id)
 	return 0;
 }
 
+bool __KernelThreadSortPriority(SceUID thread1, SceUID thread2)
+{
+	return __KernelGetThreadPrio(thread1) < __KernelGetThreadPrio(thread2);
+}
+
 //////////////////////////////////////////////////////////////////////////
 // WAIT/SLEEP ETC
 //////////////////////////////////////////////////////////////////////////
diff --git a/Core/HLE/sceKernelThread.h b/Core/HLE/sceKernelThread.h
index 90c845c9da..4d1aca0fa6 100644
--- a/Core/HLE/sceKernelThread.h
+++ b/Core/HLE/sceKernelThread.h
@@ -160,6 +160,7 @@ void __KernelSetupRootThread(SceUID moduleId, int args, const char *argp, int pr
 void __KernelStartIdleThreads();
 void __KernelReturnFromThread();  // Called as HLE function
 u32 __KernelGetThreadPrio(SceUID id);
+bool __KernelThreadSortPriority(SceUID thread1, SceUID thread2);
 
 void __KernelIdle();