The way to Unity, pt.1 (#1659)

src/core/libraries/kernel/threads/pthread.cpp

@@ -206,6 +206,7 @@ static void RunThread(void* arg) {
     DebugState.AddCurrentThreadToGuestList();
 
     /* Run the current thread's start routine with argument: */
+    curthread->native_thr.Initialize();
     void* ret = Core::ExecuteGuest(curthread->start_routine, curthread->arg);
 
     /* Remove thread from tracking */
@@ -280,7 +281,7 @@ int PS4_SYSV_ABI posix_pthread_create_name_np(PthreadT* thread, const PthreadAtt
     (*thread) = new_thread;
 
     /* Create thread */
-    new_thread->native_thr = Core::Thread();
+    new_thread->native_thr = Core::NativeThread();
     int ret = new_thread->native_thr.Create(RunThread, new_thread, &new_thread->attr);
     ASSERT_MSG(ret == 0, "Failed to create thread with error {}", ret);
     if (ret) {
@@ -412,6 +413,33 @@ int PS4_SYSV_ABI posix_pthread_getschedparam(PthreadT pthread, SchedPolicy* poli
     return 0;
 }
 
+int PS4_SYSV_ABI posix_pthread_setschedparam(PthreadT pthread, SchedPolicy policy,
+                                             const SchedParam* param) {
+    if (pthread == nullptr || param == nullptr) {
+        return POSIX_EINVAL;
+    }
+
+    auto* thread_state = ThrState::Instance();
+    if (pthread == g_curthread) {
+        g_curthread->lock.lock();
+    } else if (int ret = thread_state->FindThread(pthread, /*include dead*/ 0); ret != 0) {
+        return ret;
+    }
+
+    if (pthread->attr.sched_policy == policy &&
+        (policy == SchedPolicy::Other || pthread->attr.prio == param->sched_priority)) {
+        pthread->attr.prio = param->sched_priority;
+        pthread->lock.unlock();
+        return 0;
+    }
+
+    // TODO: _thr_setscheduler
+    pthread->attr.sched_policy = policy;
+    pthread->attr.prio = param->sched_priority;
+    pthread->lock.unlock();
+    return 0;
+}
+
 int PS4_SYSV_ABI scePthreadGetprio(PthreadT thread, int* priority) {
     SchedParam param;
     SchedPolicy policy;
@@ -495,6 +523,7 @@ void RegisterThread(Core::Loader::SymbolsResolver* sym) {
     LIB_FUNCTION("lZzFeSxPl08", "libScePosix", 1, "libkernel", 1, 1, posix_pthread_setcancelstate);
     LIB_FUNCTION("a2P9wYGeZvc", "libScePosix", 1, "libkernel", 1, 1, posix_pthread_setprio);
     LIB_FUNCTION("FIs3-UQT9sg", "libScePosix", 1, "libkernel", 1, 1, posix_pthread_getschedparam);
+    LIB_FUNCTION("Xs9hdiD7sAA", "libScePosix", 1, "libkernel", 1, 1, posix_pthread_setschedparam);
     LIB_FUNCTION("6XG4B33N09g", "libScePosix", 1, "libkernel", 1, 1, sched_yield);
 
     // Posix-Kernel

src/core/libraries/kernel/threads/pthread.h

@@ -259,7 +259,7 @@ struct Pthread {
     int refcount;
     PthreadEntryFunc start_routine;
     void* arg;
-    Core::Thread native_thr;
+    Core::NativeThread native_thr;
     PthreadAttr attr;
     bool cancel_enable;
     bool cancel_pending;

src/core/memory.cpp

@@ -328,7 +328,7 @@ int MemoryManager::MapFile(void** out_addr, VAddr virtual_addr, size_t size, Mem
     }
 
     // Map the file.
-    impl.MapFile(mapped_addr, size, offset, std::bit_cast<u32>(prot), fd);
+    impl.MapFile(mapped_addr, size_aligned, offset, std::bit_cast<u32>(prot), fd);
 
     // Add virtual memory area
     auto& new_vma = CarveVMA(mapped_addr, size_aligned)->second;
@@ -512,9 +512,8 @@ int MemoryManager::VirtualQuery(VAddr addr, int flags,
     info->is_flexible.Assign(vma.type == VMAType::Flexible);
     info->is_direct.Assign(vma.type == VMAType::Direct);
     info->is_stack.Assign(vma.type == VMAType::Stack);
-    info->is_pooled.Assign(vma.type == VMAType::Pooled);
-    info->is_committed.Assign(vma.type != VMAType::Free && vma.type != VMAType::Reserved &&
-                              vma.type != VMAType::PoolReserved);
+    info->is_pooled.Assign(vma.type == VMAType::PoolReserved);
+    info->is_committed.Assign(vma.type == VMAType::Pooled);
     vma.name.copy(info->name.data(), std::min(info->name.size(), vma.name.size()));
     if (vma.type == VMAType::Direct) {
         const auto dmem_it = FindDmemArea(vma.phys_base);
@@ -585,6 +584,7 @@ void MemoryManager::NameVirtualRange(VAddr virtual_addr, size_t size, std::strin
                "Range provided is not fully contained in vma");
     it->second.name = name;
 }
+
 VAddr MemoryManager::SearchFree(VAddr virtual_addr, size_t size, u32 alignment) {
     // If the requested address is below the mapped range, start search from the lowest address
     auto min_search_address = impl.SystemManagedVirtualBase();
@@ -691,7 +691,7 @@ MemoryManager::DMemHandle MemoryManager::Split(DMemHandle dmem_handle, size_t of
     new_area.size -= offset_in_area;
 
     return dmem_map.emplace_hint(std::next(dmem_handle), new_area.base, new_area);
-};
+}
 
 int MemoryManager::GetDirectMemoryType(PAddr addr, int* directMemoryTypeOut,
                                        void** directMemoryStartOut, void** directMemoryEndOut) {

src/core/thread.cpp

@@ -4,45 +4,125 @@
 #include "libraries/kernel/threads/pthread.h"
 #include "thread.h"
 
+#include "core/libraries/kernel/threads/pthread.h"
+
 #ifdef _WIN64
 #include <windows.h>
+#include "common/ntapi.h"
 #else
 #include <pthread.h>
 #endif
 
 namespace Core {
 
-Thread::Thread() : native_handle{0} {}
-
-Thread::~Thread() {}
-
-int Thread::Create(ThreadFunc func, void* arg, const ::Libraries::Kernel::PthreadAttr* attr) {
 #ifdef _WIN64
-    native_handle = CreateThread(nullptr, 0, (LPTHREAD_START_ROUTINE)func, arg, 0, nullptr);
-    return native_handle ? 0 : -1;
-#else
+#define KGDT64_R3_DATA (0x28)
+#define KGDT64_R3_CODE (0x30)
+#define KGDT64_R3_CMTEB (0x50)
+#define RPL_MASK (0x03)
+
+#define INITIAL_FPUCW (0x037f)
+#define INITIAL_MXCSR_MASK (0xffbf)
+#define EFLAGS_INTERRUPT_MASK (0x200)
+
+void InitializeTeb(INITIAL_TEB* teb, const ::Libraries::Kernel::PthreadAttr* attr) {
+    teb->StackBase = (void*)((u64)attr->stackaddr_attr + attr->stacksize_attr);
+    teb->StackLimit = nullptr;
+    teb->StackAllocationBase = attr->stackaddr_attr;
+}
+
+void InitializeContext(CONTEXT* ctx, ThreadFunc func, void* arg,
+                       const ::Libraries::Kernel::PthreadAttr* attr) {
+    /* Note: The stack has to be reversed */
+    ctx->Rsp = (u64)attr->stackaddr_attr + attr->stacksize_attr;
+    ctx->Rbp = (u64)attr->stackaddr_attr + attr->stacksize_attr;
+    ctx->Rcx = (u64)arg;
+    ctx->Rip = (u64)func;
+
+    ctx->SegGs = KGDT64_R3_DATA | RPL_MASK;
+    ctx->SegEs = KGDT64_R3_DATA | RPL_MASK;
+    ctx->SegDs = KGDT64_R3_DATA | RPL_MASK;
+    ctx->SegCs = KGDT64_R3_CODE | RPL_MASK;
+    ctx->SegSs = KGDT64_R3_DATA | RPL_MASK;
+    ctx->SegFs = KGDT64_R3_CMTEB | RPL_MASK;
+
+    ctx->EFlags = 0x3000 | EFLAGS_INTERRUPT_MASK;
+    ctx->MxCsr = INITIAL_MXCSR;
+
+    ctx->FltSave.ControlWord = INITIAL_FPUCW;
+    ctx->FltSave.MxCsr = INITIAL_MXCSR;
+    ctx->FltSave.MxCsr_Mask = INITIAL_MXCSR_MASK;
+
+    ctx->ContextFlags =
+        CONTEXT_CONTROL | CONTEXT_INTEGER | CONTEXT_SEGMENTS | CONTEXT_FLOATING_POINT;
+}
+#endif
+
+NativeThread::NativeThread() : native_handle{0} {}
+
+NativeThread::~NativeThread() {}
+
+int NativeThread::Create(ThreadFunc func, void* arg, const ::Libraries::Kernel::PthreadAttr* attr) {
+#ifndef _WIN64
     pthread_t* pthr = reinterpret_cast<pthread_t*>(&native_handle);
     pthread_attr_t pattr;
     pthread_attr_init(&pattr);
     pthread_attr_setstack(&pattr, attr->stackaddr_attr, attr->stacksize_attr);
     return pthread_create(pthr, &pattr, (PthreadFunc)func, arg);
+#else
+    CLIENT_ID clientId{};
+    INITIAL_TEB teb{};
+    CONTEXT ctx{};
+
+    clientId.UniqueProcess = GetCurrentProcess();
+    clientId.UniqueThread = GetCurrentThread();
+
+    InitializeTeb(&teb, attr);
+    InitializeContext(&ctx, func, arg, attr);
+
+    return NtCreateThread(&native_handle, THREAD_ALL_ACCESS, nullptr, GetCurrentProcess(),
+                          &clientId, &ctx, &teb, false);
 #endif
 }
 
-void Thread::Exit() {
+void NativeThread::Exit() {
     if (!native_handle) {
         return;
     }
 
+    tid = 0;
+
 #ifdef _WIN64
-    CloseHandle(native_handle);
+    NtClose(native_handle);
     native_handle = nullptr;
 
-    // We call this assuming the thread has finished execution.
-    ExitThread(0);
+    /* The Windows kernel will free the stack
+       given at thread creation via INITIAL_TEB
+       (StackAllocationBase) upon thread termination.
+
+       In earlier Windows versions (NT4 to Windows Server 2003),
+       you could get around this via disabling FreeStackOnTermination
+       on the TEB. This has been removed since then.
+
+       To avoid this, we must forcefully set the TEB
+       deallocation stack pointer to NULL so ZwFreeVirtualMemory fails
+       in the kernel and our stack is not freed.
+     */
+    auto* teb = reinterpret_cast<TEB*>(NtCurrentTeb());
+    teb->DeallocationStack = nullptr;
+
+    NtTerminateThread(nullptr, 0);
 #else
     pthread_exit(nullptr);
 #endif
 }
 
+void NativeThread::Initialize() {
+#if _WIN64
+    tid = GetCurrentThreadId();
+#else
+    tid = (u64)pthread_self();
+#endif
+}
+
 } // namespace Core

src/core/thread.h

@@ -11,27 +11,34 @@ struct PthreadAttr;
 
 namespace Core {
 
-class Thread {
-public:
-    using ThreadFunc = void (*)(void*);
-    using PthreadFunc = void* (*)(void*);
+using ThreadFunc = void (*)(void*);
+using PthreadFunc = void* (*)(void*);
 
-    Thread();
-    ~Thread();
+class NativeThread {
+public:
+    NativeThread();
+    ~NativeThread();
 
     int Create(ThreadFunc func, void* arg, const ::Libraries::Kernel::PthreadAttr* attr);
     void Exit();
 
+    void Initialize();
+
     uintptr_t GetHandle() {
         return reinterpret_cast<uintptr_t>(native_handle);
     }
 
+    u64 GetTid() {
+        return tid;
+    }
+
 private:
-#if _WIN64
+#ifdef _WIN64
     void* native_handle;
 #else
     uintptr_t native_handle;
 #endif
+    u64 tid;
 };
 
 } // namespace Core