lv2: Rewrite yield command

* Return immediatly if current thread is found on the last position on g_ppu queue.
* Avoid unqueue and requeue: use rotate instead.
* Same applies when lowering the priority of thread. (increasing value)

rpcs3/Emu/Cell/lv2/lv2.cpp

@@ -1071,36 +1071,25 @@ void lv2_obj::awake_unlocked(cpu_thread* cpu, u32 prio)
 	// Check thread type
 	AUDIT(!cpu || cpu->id_type() == 1);
 
-	if (prio < INT32_MAX)
+	if (prio <= 0x7fffffff)
 	{
-        // Priority set
-        if (static_cast<ppu_thread*>(cpu)->prio.exchange(prio) == prio || !unqueue(g_ppu, cpu))
-        {
-            return;
-        }
-	}
-	else if (prio == -4)
-	{
-		// Yield command
-		const u64 start_time = get_guest_system_time();
+		// Priority set
+		const u32 _prio = static_cast<ppu_thread*>(cpu)->prio.exchange(prio);
 
-		for (std::size_t i = 0, pos = -1; i < g_ppu.size(); i++)
+		if (_prio == prio)
 		{
-			if (g_ppu[i] == cpu)
-			{
-				pos = i;
-				prio = g_ppu[i]->prio;
-			}
-			else if (i == pos + 1 && prio != -4 && g_ppu[i]->prio != prio)
-			{
-				return;
-			}
+			return;
 		}
 
-		unqueue(g_ppu, cpu);
-		unqueue(g_pending, cpu);
-
-		static_cast<ppu_thread*>(cpu)->start_time = start_time;
+		if (_prio < prio)
+		{
+			// Yield
+			prio = -4;
+		}
+		else if (!unqueue(g_ppu, cpu))
+		{
+			return;
+		}
 	}
 
 	const auto emplace_thread = [](cpu_thread* const cpu)
@@ -1134,7 +1123,43 @@ void lv2_obj::awake_unlocked(cpu_thread* cpu, u32 prio)
 		LOG_TRACE(PPU, "awake(): %s", cpu->id);
 	};
 
-	if (cpu)
+	if (prio == -4)
+	{
+		// Yield command
+		decltype(g_ppu.end()) current;
+		size_t to_rotate = 0;
+
+		for (auto it = g_ppu.begin(), end = g_ppu.end(); it != end; it++)
+		{
+			const auto thread = *it;
+
+			if (to_rotate)
+			{
+				if (thread->prio > prio)
+				{
+					break;
+				}
+
+				to_rotate++;
+			}
+			else if (thread == cpu)
+			{
+				current = it;
+				prio = thread->prio;
+				to_rotate++;
+			}
+		}
+
+		if (to_rotate <= 1)
+		{
+			return;
+		}
+
+		// Put the current thread at the back of the range
+		std::rotate(current, current + 1, current + to_rotate);
+		static_cast<ppu_thread*>(cpu)->start_time = get_guest_system_time();
+	}
+	else if (cpu)
 	{
 		// Emplace current thread
 		emplace_thread(cpu);