SC_Semaphore rewritten

rpcs3/Emu/CPU/CPUThreadManager.cpp

@@ -108,6 +108,22 @@ CPUThread* CPUThreadManager::GetThread(u32 id)
 	return res;
 }
 
+void CPUThreadManager::NotifyThread(const u32 id)
+{
+	if (!id) return;
+
+	std::lock_guard<std::mutex> lock(m_mtx_thread);
+
+	for (u32 i = 0; i < m_threads.size(); i++)
+	{
+		if (m_threads[i]->GetId() == id)
+		{
+			m_threads[i]->Notify();
+			return;
+		}
+	}
+}
+
 void CPUThreadManager::Exec()
 {
 	std::lock_guard<std::mutex> lock(m_mtx_thread);

rpcs3/Emu/CPU/CPUThreadManager.h

@@ -17,6 +17,7 @@ public:
 
 	CPUThread& AddThread(CPUThreadType type);
 	void RemoveThread(const u32 id);
+	void NotifyThread(const u32 id);
 
 	std::vector<CPUThread*>& GetThreads() { return m_threads; }
 	s32 GetThreadNumById(CPUThreadType type, u32 id);

rpcs3/Emu/SysCalls/SysCalls.h

@@ -11,6 +11,7 @@
 #include "lv2/SC_Event_flag.h"
 #include "lv2/SC_Condition.h"
 #include "lv2/SC_Spinlock.h"
+#include "lv2/SC_Semaphore.h"
 #include "Emu/event.h"
 #include "Static.h"
 //#define SYSCALLS_DEBUG
@@ -144,12 +145,12 @@ extern int sys_event_flag_cancel(u32 eflag_id, mem32_t num);
 extern int sys_event_flag_get(u32 eflag_id, mem64_t flags);
 
 //sys_semaphore
-extern int sys_semaphore_create(u32 sem_addr, u32 attr_addr, int initial_val, int max_val);
+extern int sys_semaphore_create(mem32_t sem, mem_ptr_t<sys_semaphore_attribute> attr, int initial_count, int max_count);
-extern int sys_semaphore_destroy(u32 sem);
+extern int sys_semaphore_destroy(u32 sem_id);
-extern int sys_semaphore_wait(u32 sem, u64 timeout);
+extern int sys_semaphore_wait(u32 sem_id, u64 timeout);
-extern int sys_semaphore_trywait(u32 sem);
+extern int sys_semaphore_trywait(u32 sem_id);
-extern int sys_semaphore_post(u32 sem, int count);
+extern int sys_semaphore_post(u32 sem_id, int count);
-extern int sys_semaphore_get_value(u32 sem, u32 count_addr);
+extern int sys_semaphore_get_value(u32 sem_id, mem32_t count);
 
 //sys_lwcond
 extern int sys_lwcond_create(mem_ptr_t<sys_lwcond_t> lwcond, mem_ptr_t<sys_lwmutex_t> lwmutex, mem_ptr_t<sys_lwcond_attribute_t> attr);

rpcs3/Emu/SysCalls/lv2/SC_Lwmutex.cpp

@@ -190,6 +190,13 @@ bool SleepQueue::invalidate(u32 tid)
 	return false;
 }
 
+u32 SleepQueue::count()
+{
+	std::lock_guard<std::mutex> lock(m_mutex);
+
+	return list.size();
+}
+
 bool SleepQueue::finalize()
 {
 	if (!m_mutex.try_lock()) return false;

rpcs3/Emu/SysCalls/lv2/SC_Lwmutex.h

@@ -60,6 +60,7 @@ struct SleepQueue
 	u32 pop_prio(); // SYS_SYNC_PRIORITY
 	u32 pop_prio_inherit(); // (TODO)
 	bool invalidate(u32 tid);
+	u32 count();
 	bool finalize();
 };
 

rpcs3/Emu/SysCalls/lv2/SC_Semaphore.cpp

@@ -3,116 +3,203 @@
 #include "Emu/Memory/Memory.h"
 #include "Emu/System.h"
 #include "Emu/SysCalls/SysCalls.h"
+#include "SC_Semaphore.h"
 
 SysCallBase sys_sem("sys_semaphore");
 
-struct semaphore_attr
+int sys_semaphore_create(mem32_t sem, mem_ptr_t<sys_semaphore_attribute> attr, int initial_count, int max_count)
-{
-	u32 protocol;
-	u32 pshared;
-	u64 ipc_key;
-	int flags;
-	u32 pad;
-	char name[8];
-};
-
-struct semaphore
-{
-	rSemaphore sem;
-	semaphore_attr attr;
-	int sem_count;
-
-	semaphore(int initial_count, int max_count, semaphore_attr attr)
-		: sem(initial_count, max_count)
-		, attr(attr)
-	{
-	}
-};
-
-int sys_semaphore_create(u32 sem_addr, u32 attr_addr, int initial_count, int max_count)
 {
 	sys_sem.Warning("sys_semaphore_create(sem_addr=0x%x, attr_addr=0x%x, initial_count=%d, max_count=%d)",
-		sem_addr, attr_addr, initial_count, max_count);
+		sem.GetAddr(), attr.GetAddr(), initial_count, max_count);
 
-	if(!Memory.IsGoodAddr(sem_addr) || !Memory.IsGoodAddr(attr_addr)) return CELL_EFAULT;
+	if (!sem.IsGood() || !attr.IsGood())
+	{
+		return CELL_EFAULT;
+	}
 
-	semaphore_attr attr = (semaphore_attr&)Memory[attr_addr];
+	if (max_count <= 0)
-	attr.protocol = re(attr.protocol);
+	{
-	attr.pshared = re(attr.pshared);
+		return CELL_EINVAL;
-	attr.ipc_key = re(attr.ipc_key);
+	}
-	attr.flags = re(attr.flags);
 	
-	sys_sem.Log("*** protocol = %d", attr.protocol);
+	if (attr->pshared.ToBE() != se32(0x200))
-	sys_sem.Log("*** pshared = %d", attr.pshared);
+	{
-	sys_sem.Log("*** ipc_key = 0x%llx", attr.ipc_key);
+		sys_sem.Error("Invalid pshared attribute(0x%x)", (u32)attr->pshared);
-	sys_sem.Log("*** flags = 0x%x", attr.flags);
+		return CELL_EINVAL;
-	sys_sem.Log("*** name = %s", attr.name);
+	}
 
-	Memory.Write32(sem_addr, sys_sem.GetNewId(new semaphore(initial_count, max_count, attr)));
+	switch (attr->protocol.ToBE())
+	{
+	case se32(SYS_SYNC_FIFO): break;
+	case se32(SYS_SYNC_PRIORITY): break;
+	case se32(SYS_SYNC_PRIORITY_INHERIT): sys_sem.Warning("TODO: SYS_SYNC_PRIORITY_INHERIT protocol"); break;
+	case se32(SYS_SYNC_RETRY): sys_sem.Error("Invalid SYS_SYNC_RETRY protocol"); return CELL_EINVAL;
+	default: sys_sem.Error("Unknown protocol attribute(0x%x)", (u32)attr->protocol); return CELL_EINVAL;
+	}
+
+	sem = sys_sem.GetNewId(new Semaphore(initial_count, max_count, attr->protocol, attr->name_u64));
+	ConLog.Write("*** semaphore created [%s] (protocol=0x%x): id = %d",
+		std::string(attr->name, 8).c_str(), (u32)attr->protocol, sem.GetValue());
 
 	return CELL_OK;
 }
 
-int sys_semaphore_destroy(u32 sem)
+int sys_semaphore_destroy(u32 sem_id)
 {
-	sys_sem.Log("sys_semaphore_destroy(sem=%d)", sem);
+	sys_sem.Warning("sys_semaphore_destroy(sem_id=%d)", sem_id);
 
-	if(!sys_sem.CheckId(sem)) return CELL_ESRCH;
+	Semaphore* sem;
+	if (!Emu.GetIdManager().GetIDData(sem_id, sem))
+	{
+		return CELL_ESRCH;
+	}
 	
-	Emu.GetIdManager().RemoveID(sem);
+	if (!sem->m_queue.finalize())
+	{
+		return CELL_EBUSY;
+	}
+
+	Emu.GetIdManager().RemoveID(sem_id);
 	return CELL_OK;
 }
 
-int sys_semaphore_wait(u32 sem, u64 timeout)
+int sys_semaphore_wait(u32 sem_id, u64 timeout)
 {
-	sys_sem.Log("sys_semaphore_wait(sem=0x%x, timeout=0x%llx)", sem, timeout);
+	sys_sem.Log("sys_semaphore_wait(sem_id=%d, timeout=%lld)", sem_id, timeout);
 
-	semaphore* sem_data = nullptr;
+	Semaphore* sem;
-	if(!sys_sem.CheckId(sem, sem_data)) return CELL_ESRCH;
+	if (!Emu.GetIdManager().GetIDData(sem_id, sem))
+	{
+		return CELL_ESRCH;
+	}
 
-	sem_data->sem_count = 0;  // Reset internal counter for sys_semaphore_get_value.
+	const u32 tid = GetCurrentPPUThread().GetId();
-	sem_data->sem.WaitTimeout(timeout ? timeout : INFINITE);
+	const u64 start_time = get_system_time();
 
+	{
+		std::lock_guard<std::mutex> lock(sem->m_mutex);
+		if (sem->m_value > 0)
+		{
+			sem->m_value--;
+			return CELL_OK;
+		}
+		sem->m_queue.push(tid);
+	}
+
+	while (true)
+	{
+		if (Emu.IsStopped())
+		{
+			ConLog.Warning("sys_semaphore_wait(%d) aborted", sem_id);
 			return CELL_OK;
 		}
 
-int sys_semaphore_trywait(u32 sem)
+		if (timeout && get_system_time() - start_time > timeout)
 		{
-	sys_sem.Log("sys_semaphore_trywait(sem=%d)", sem);
+			return CELL_ETIMEDOUT;
+		}
 
-	semaphore* sem_data = nullptr;
+		if (tid == sem->signal)
-	if(!sys_sem.CheckId(sem, sem_data)) return CELL_ESRCH;
+		{
-
+			std::lock_guard<std::mutex> lock(sem->m_mutex);
-	sem_data->sem_count = 0;  // Reset internal counter for sys_semaphore_get_value.
-	if(sem_data->sem.TryWait()) return 1;
 
+			sem->signal = 0;
+			// TODO: notify signaler
 			return CELL_OK;
 		}
 
-int sys_semaphore_post(u32 sem, int count)
+		SM_Sleep();
-{
+	}
-	sys_sem.Log("sys_semaphore_post(sem=%d, count=%d)", sem, count);
+}
 
-	semaphore* sem_data = nullptr;
+int sys_semaphore_trywait(u32 sem_id)
-	if(!sys_sem.CheckId(sem, sem_data)) return CELL_ESRCH;
-
-	while(count--)
 {
-		sem_data->sem_count++;  // Increment internal counter for sys_semaphore_get_value.
+	sys_sem.Log("sys_semaphore_trywait(sem_id=%d)", sem_id);
-		sem_data->sem.Post();
+
+	Semaphore* sem;
+	if (!Emu.GetIdManager().GetIDData(sem_id, sem))
+	{
+		return CELL_ESRCH;
+	}
+
+	std::lock_guard<std::mutex> lock(sem->m_mutex);
+
+	if (sem->m_value > 0)
+	{
+		sem->m_value--;
+		return CELL_OK;
+	}
+	else
+	{
+		return CELL_EBUSY;
+	}
+}
+
+int sys_semaphore_post(u32 sem_id, int count)
+{
+	sys_sem.Log("sys_semaphore_post(sem_id=%d, count=%d)", sem_id, count);
+
+	Semaphore* sem;
+	if (!Emu.GetIdManager().GetIDData(sem_id, sem))
+	{
+		return CELL_ESRCH;
+	}
+
+	if (count < 0)
+	{
+		return CELL_EINVAL;
+	}
+
+	if (count + sem->m_value - sem->m_queue.count() > sem->max)
+	{
+		return CELL_EINVAL;
+	}
+
+	while (count > 0)
+	{
+		if (Emu.IsStopped())
+		{
+			ConLog.Warning("sys_semaphore_post(%d) aborted", sem_id);
+			return CELL_OK;
+		}
+
+		std::lock_guard<std::mutex> lock(sem->m_mutex);
+
+		if (sem->signal && sem->m_queue.count())
+		{
+			SM_Sleep();
+			continue;
+		}
+
+		if (u32 target = (sem->protocol == SYS_SYNC_FIFO) ? sem->m_queue.pop() : sem->m_queue.pop_prio())
+		{
+			count--;
+			sem->signal = target;
+			Emu.GetCPU().NotifyThread(target);
+		}
+		else
+		{
+			sem->m_value += count;
+			count = 0;
+		}
 	}
 
 	return CELL_OK;
 }
 
-int sys_semaphore_get_value(u32 sem, u32 count_addr)
+int sys_semaphore_get_value(u32 sem_id, mem32_t count)
 {
-	sys_sem.Log("sys_semaphore_get_value(sem=%d, count_addr=0x%x)", sem, count_addr);
+	sys_sem.Log("sys_semaphore_get_value(sem_id=%d, count_addr=0x%x)", sem_id, count.GetAddr());
 
-	semaphore* sem_data = nullptr;
+	Semaphore* sem;
-	if(!sys_sem.CheckId(sem, sem_data)) return CELL_ESRCH;
+	if (!Emu.GetIdManager().GetIDData(sem_id, sem))
+	{
+		return CELL_ESRCH;
+	}
 
-	Memory.Write32(count_addr, sem_data->sem_count);
+	std::lock_guard<std::mutex> lock(sem->m_mutex);
+	
+	count = sem->m_value;
 
 	return CELL_OK;
 }

rpcs3/Emu/SysCalls/lv2/SC_Semaphore.h

@@ -0,0 +1,36 @@
+#pragma once
+
+struct sys_semaphore_attribute
+{
+	be_t<u32> protocol;
+	be_t<u32> pshared; // undefined
+	be_t<u64> ipc_key; // undefined
+	be_t<int> flags; // undefined
+	be_t<u32> pad; // not used
+	union
+	{
+		char name[8];
+		u64 name_u64;
+	};
+};
+
+struct Semaphore
+{
+	std::mutex m_mutex;
+	SleepQueue m_queue;
+	int m_value;
+	u32 signal;
+
+	const int max;
+	const u32 protocol;
+	const u64 name;
+
+	Semaphore(int initial_count, int max_count, u32 protocol, u64 name)
+		: m_value(initial_count)
+		, signal(0)
+		, max(max_count)
+		, protocol(protocol)
+		, name(name)
+	{
+	}
+};

rpcs3/emucore.vcxproj

@@ -321,6 +321,7 @@
     <ClInclude Include="Emu\SysCalls\lv2\SC_Memory.h" />
     <ClInclude Include="Emu\SysCalls\lv2\SC_Mutex.h" />
     <ClInclude Include="Emu\SysCalls\lv2\SC_Rwlock.h" />
+    <ClInclude Include="Emu\SysCalls\lv2\SC_Semaphore.h" />
     <ClInclude Include="Emu\SysCalls\lv2\SC_Spinlock.h" />
     <ClInclude Include="Emu\SysCalls\lv2\SC_SPU_Thread.h" />
     <ClInclude Include="Emu\SysCalls\lv2\SC_Time.h" />

rpcs3/emucore.vcxproj.filters

@@ -1048,5 +1048,8 @@
     <ClInclude Include="..\Utilities\SSemaphore.h">
       <Filter>Utilities</Filter>
     </ClInclude>
+    <ClInclude Include="Emu\SysCalls\lv2\SC_Semaphore.h">
+      <Filter>Emu\SysCalls\lv2</Filter>
+    </ClInclude>
   </ItemGroup>
 </Project>