sys_semaphore...

rpcs3/Emu/Cell/lv2/sys_semaphore.cpp

@@ -13,7 +13,7 @@ logs::channel sys_semaphore("sys_semaphore", logs::level::notice);
 
 extern u64 get_system_time();
 
-s32 sys_semaphore_create(vm::ptr<u32> sem_id, vm::ptr<sys_semaphore_attribute_t> attr, s32 initial_val, s32 max_val)
+error_code sys_semaphore_create(vm::ptr<u32> sem_id, vm::ptr<sys_semaphore_attribute_t> attr, s32 initial_val, s32 max_val)
 {
 	sys_semaphore.warning("sys_semaphore_create(sem_id=*0x%x, attr=*0x%x, initial_val=%d, max_val=%d)", sem_id, attr, initial_val, max_val);
 
@@ -42,169 +42,232 @@ s32 sys_semaphore_create(vm::ptr<u32> sem_id, vm::ptr<sys_semaphore_attribute_t>
 		return CELL_EINVAL;
 	}
 
-	*sem_id = idm::make<lv2_obj, lv2_sema>(protocol, max_val, attr->name_u64, initial_val);
+	if (const u32 id = idm::make<lv2_obj, lv2_sema>(protocol, attr->name_u64, max_val, initial_val))
-
+	{
+		*sem_id = id;
 		return CELL_OK;
+	}
+
+	return CELL_EAGAIN;
 }
 
-s32 sys_semaphore_destroy(u32 sem_id)
+error_code sys_semaphore_destroy(u32 sem_id)
 {
 	sys_semaphore.warning("sys_semaphore_destroy(sem_id=0x%x)", sem_id);
 
-	LV2_LOCK;
+	const auto sem = idm::withdraw<lv2_obj, lv2_sema>(sem_id, [](lv2_sema& sema) -> CellError
+	{
+		if (sema.val < 0)
+		{
+			return CELL_EBUSY;
+		}
 
-	const auto sem = idm::get<lv2_obj, lv2_sema>(sem_id);
+		return {};
+	});
 
 	if (!sem)
 	{
 		return CELL_ESRCH;
 	}
 	
-	if (sem->sq.size())
+	if (sem.value)
 	{
-		return CELL_EBUSY;
+		return sem.value;
 	}
 
-	idm::remove<lv2_obj, lv2_sema>(sem_id);
-
 	return CELL_OK;
 }
 
-s32 sys_semaphore_wait(ppu_thread& ppu, u32 sem_id, u64 timeout)
+error_code sys_semaphore_wait(ppu_thread& ppu, u32 sem_id, u64 timeout)
 {
 	sys_semaphore.trace("sys_semaphore_wait(sem_id=0x%x, timeout=0x%llx)", sem_id, timeout);
 
 	const u64 start_time = get_system_time();
 
-	LV2_LOCK;
+	const auto sem = idm::get<lv2_obj, lv2_sema>(sem_id, [&](lv2_sema& sema)
+	{
+		const s32 val = sema.val;
 		
-	const auto sem = idm::get<lv2_obj, lv2_sema>(sem_id);
+		if (val > 0)
+		{
+			if (sema.val.compare_and_swap_test(val, val - 1))
+			{
+				return true;
+			}
+		}
+
+		semaphore_lock lock(sema.mutex);
+
+		if (sema.val-- <= 0)
+		{
+			sema.sq.emplace_back(&ppu);
+			return false;
+		}
+
+		return true;
+	});
 
 	if (!sem)
 	{
 		return CELL_ESRCH;
 	}
 
-	if (sem->value > 0)
+	if (sem.value)
 	{
-		sem->value--;
-		
 		return CELL_OK;
 	}
 
-	// add waiter; protocol is ignored in current implementation
+	// SLEEP
-	sleep_entry<cpu_thread> waiter(sem->sq, ppu);
 
 	while (!ppu.state.test_and_reset(cpu_flag::signal))
 	{
-		CHECK_EMU_STATUS;
-
 		if (timeout)
 		{
 			const u64 passed = get_system_time() - start_time;
 
 			if (passed >= timeout)
 			{
+				semaphore_lock lock(sem->mutex);
+
+				const s32 val = sem->val.fetch_op([](s32& val)
+				{
+					if (val < 0)
+					{
+						val++;
+					}
+				});
+
+				if (val >= 0)
+				{
+					timeout = 0;
+					continue;
+				}
+
+				verify(HERE), sem->unqueue(sem->sq, &ppu);
 				return CELL_ETIMEDOUT;
 			}
 
-			LV2_UNLOCK, thread_ctrl::wait_for(timeout - passed);
+			thread_ctrl::wait_for(timeout - passed);
 		}
 		else
 		{
-			LV2_UNLOCK, thread_ctrl::wait();
+			thread_ctrl::wait();
 		}
 	}
 
 	return CELL_OK;
 }
 
-s32 sys_semaphore_trywait(u32 sem_id)
+error_code sys_semaphore_trywait(u32 sem_id)
 {
 	sys_semaphore.trace("sys_semaphore_trywait(sem_id=0x%x)", sem_id);
 
-	LV2_LOCK;
+	const auto sem = idm::check<lv2_obj, lv2_sema>(sem_id, [&](lv2_sema& sema)
+	{
+		const s32 val = sema.val;
 		
-	const auto sem = idm::get<lv2_obj, lv2_sema>(sem_id);
+		if (val > 0)
+		{
+			if (sema.val.compare_and_swap_test(val, val - 1))
+			{
+				return true;
+			}
+		}
+
+		return false;
+	});
 
 	if (!sem)
 	{
 		return CELL_ESRCH;
 	}
 
-	if (sem->value <= 0 || sem->sq.size())
+	if (!sem.value)
 	{
-		return CELL_EBUSY;
+		return not_an_error(CELL_EBUSY);
 	}
 
-	sem->value--;
-
 	return CELL_OK;
 }
 
-s32 sys_semaphore_post(u32 sem_id, s32 count)
+error_code sys_semaphore_post(u32 sem_id, s32 count)
 {
 	sys_semaphore.trace("sys_semaphore_post(sem_id=0x%x, count=%d)", sem_id, count);
 
-	LV2_LOCK;
-
-	const auto sem = idm::get<lv2_obj, lv2_sema>(sem_id);
-
-	if (!sem)
-	{
-		return CELL_ESRCH;
-	}
-
 	if (count < 0)
 	{
 		return CELL_EINVAL;
 	}
 
-	// get comparable values considering waiting threads
+	const auto sem = idm::get<lv2_obj, lv2_sema>(sem_id, [&](lv2_sema& sema)
-	const u64 new_value = sem->value + count;
-	const u64 max_value = sem->max + sem->sq.size();
-
-	if (new_value > max_value)
 	{
-		return CELL_EBUSY;
+		const s32 val = sema.val;
+
+		if (val >= 0 && count <= sema.max - val)
+		{
+			if (sema.val.compare_and_swap_test(val, val + count))
+			{
+				return true;
+			}
 		}
 
-	// wakeup as much threads as possible
+		return false;
-	while (count && !sem->sq.empty())
+	});
-	{
-		count--;
-
-		const auto thread = sem->sq.front();
-		thread->set_signal();
-
-		sem->sq.pop_front();
-	}
-
-	// add the rest to the value
-	sem->value += count;
-
-	return CELL_OK;
-}
-
-s32 sys_semaphore_get_value(u32 sem_id, vm::ptr<s32> count)
-{
-	sys_semaphore.trace("sys_semaphore_get_value(sem_id=0x%x, count=*0x%x)", sem_id, count);
-
-	LV2_LOCK;
-
-	if (!count)
-	{
-		return CELL_EFAULT;
-	}
-
-	const auto sem = idm::get<lv2_obj, lv2_sema>(sem_id);
 
 	if (!sem)
 	{
 		return CELL_ESRCH;
 	}
 
-	*count = sem->value;
+	if (sem.value)
+	{
+		return CELL_OK;
+	}
+	else
+	{
+		semaphore_lock lock(sem->mutex);
+
+		const s32 val = sem->val.fetch_op([=](s32& val)
+		{
+			if (val + s64{count} <= sem->max)
+			{
+				val += count;
+			}
+		});
+
+		if (val + s64{count} > sem->max)
+		{
+			return not_an_error(CELL_EBUSY);
+		}
+
+		// Wake threads
+		for (s32 i = std::min<s32>(-std::min<s32>(val, 0), count); i > 0; i--)
+		{
+			const auto cpu = verify(HERE, sem->schedule<ppu_thread>(sem->sq, sem->protocol));
+
+			cpu->set_signal();
+		}
+	}
+
+	return CELL_OK;
+}
+
+error_code sys_semaphore_get_value(u32 sem_id, vm::ptr<s32> count)
+{
+	sys_semaphore.trace("sys_semaphore_get_value(sem_id=0x%x, count=*0x%x)", sem_id, count);
+
+	if (!count)
+	{
+		return CELL_EFAULT;
+	}
+
+	if (!idm::check<lv2_obj, lv2_sema>(sem_id, [=](lv2_sema& sema)
+	{
+		*count = std::max<s32>(0, sema.val);
+	}))
+	{
+		return CELL_ESRCH;
+	}
 
 	return CELL_OK;
 }

rpcs3/Emu/Cell/lv2/sys_semaphore.h

@@ -22,18 +22,24 @@ struct lv2_sema final : lv2_obj
 	static const u32 id_base = 0x96000000;
 
 	const u32 protocol;
-	const s32 max;
+	const u32 shared;
+	const u64 key;
 	const u64 name;
+	const s32 flags;
+	const s32 max;
 
-	atomic_t<s32> value;
+	semaphore<> mutex;
+	atomic_t<s32> val;
+	std::deque<cpu_thread*> sq;
 
-	sleep_queue<cpu_thread> sq;
+	lv2_sema(u32 protocol, u64 name, s32 max, s32 value)
-
-	lv2_sema(u32 protocol, s32 max, u64 name, s32 value)
 		: protocol(protocol)
-		, max(max)
+		, shared(0)
+		, key(0)
+		, flags(0)
 		, name(name)
-		, value(value)
+		, max(max)
+		, val(value)
 	{
 	}
 };
@@ -41,10 +47,11 @@ struct lv2_sema final : lv2_obj
 // Aux
 class ppu_thread;
 
-// SysCalls
+// Syscalls
-s32 sys_semaphore_create(vm::ps3::ptr<u32> sem_id, vm::ps3::ptr<sys_semaphore_attribute_t> attr, s32 initial_val, s32 max_val);
+
-s32 sys_semaphore_destroy(u32 sem_id);
+error_code sys_semaphore_create(vm::ps3::ptr<u32> sem_id, vm::ps3::ptr<sys_semaphore_attribute_t> attr, s32 initial_val, s32 max_val);
-s32 sys_semaphore_wait(ppu_thread& ppu, u32 sem_id, u64 timeout);
+error_code sys_semaphore_destroy(u32 sem_id);
-s32 sys_semaphore_trywait(u32 sem_id);
+error_code sys_semaphore_wait(ppu_thread& ppu, u32 sem_id, u64 timeout);
-s32 sys_semaphore_post(u32 sem_id, s32 count);
+error_code sys_semaphore_trywait(u32 sem_id);
-s32 sys_semaphore_get_value(u32 sem_id, vm::ps3::ptr<s32> count);
+error_code sys_semaphore_post(u32 sem_id, s32 count);
+error_code sys_semaphore_get_value(u32 sem_id, vm::ps3::ptr<s32> count);

rpcs3/Emu/Cell/lv2/sys_sync.h

@@ -52,6 +52,56 @@ struct lv2_obj
 
 	static const u32 id_step = 0x100;
 	static const u32 id_count = 8192;
+
+	// Find and remove the object from the container (deque or vector)
+	template <typename T, typename E>
+	static bool unqueue(std::deque<T*>& queue, const E& object)
+	{
+		for (auto found = queue.cbegin(), end = queue.cend(); found != end; found++)
+		{
+			if (*found == object)
+			{
+				queue.erase(found);
+				return true;
+			}
+		}
+
+		return false;
+	}
+
+	template <typename E, typename T>
+	static T* schedule(std::deque<T*>& queue, u32 protocol)
+	{
+		if (queue.empty())
+		{
+			return nullptr;
+		}
+
+		if (protocol == SYS_SYNC_FIFO)
+		{
+			const auto res = queue.front();
+			queue.pop_front();
+			return res;
+		}
+
+		u32 prio = -1;
+		auto it = queue.cbegin();
+
+		for (auto found = it, end = queue.cend(); found != end; found++)
+		{
+			const u32 _prio = static_cast<E*>(*found)->prio;
+
+			if (_prio < prio)
+			{
+				it = found;
+				prio = _prio;
+			}
+		}
+
+		const auto res = *it;
+		queue.erase(it);
+		return res;
+	}
 };
 
 // Temporary implementation for LV2_UNLOCK (TODO: remove it)

rpcs3/Gui/KernelExplorer.cpp

@@ -214,7 +214,7 @@ void KernelExplorer::Update()
 		{
 			auto& sema = static_cast<lv2_sema&>(obj);
 			m_tree->AppendItem(node, fmt::format("Semaphore: ID = 0x%08x \"%s\", Count = %d, Max Count = %d, Waiters = %#zu", id, +name64(sema.name),
-				sema.value.load(), sema.max, sema.sq.size()));
+				sema.val.load(), sema.max, sema.sq.size()));
 			break;
 		}
 		case SYS_LWCOND_OBJECT: