add scePthreadSetaffinity and emulate affinity (#2885)

* add implementation

* fix preprocessor

* fixes squidbus's comments

* fix clang

* comment became fucked up?

* fix removed return

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

@@ -289,7 +289,12 @@ int PS4_SYSV_ABI posix_pthread_create_name_np(PthreadT* thread, const PthreadAtt
     /* Create 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 (attr != nullptr && *attr != nullptr && (*attr)->cpuset != nullptr) {
+        new_thread->SetAffinity((*attr)->cpuset);
+    }
     if (ret) {
         *thread = nullptr;
     }
@@ -521,6 +526,69 @@ int PS4_SYSV_ABI posix_pthread_setcancelstate(PthreadCancelState state,
     return 0;
 }
 
+int Pthread::SetAffinity(const Cpuset* cpuset) {
+    const auto processor_count = std::thread::hardware_concurrency();
+    if (processor_count < 8) {
+        return 0;
+    }
+    if (cpuset == nullptr) {
+        return POSIX_EINVAL;
+    }
+
+    u64 mask = cpuset->bits;
+
+    uintptr_t handle = native_thr.GetHandle();
+    if (handle == 0) {
+        return POSIX_ESRCH;
+    }
+
+    // We don't use this currently because some games gets performance problems
+    // when applying affinity even on strong hardware
+    /*
+    #ifdef _WIN64
+        DWORD_PTR affinity_mask = static_cast<DWORD_PTR>(mask);
+        if (!SetThreadAffinityMask(reinterpret_cast<HANDLE>(handle), affinity_mask)) {
+            return POSIX_EINVAL;
+        }
+
+    #elif defined(__linux__)
+        cpu_set_t cpu_set;
+        CPU_ZERO(&cpu_set);
+
+        u64 mask = cpuset->bits;
+        for (int cpu = 0; cpu < std::min(64, CPU_SETSIZE); ++cpu) {
+            if (mask & (1ULL << cpu)) {
+                CPU_SET(cpu, &cpu_set);
+            }
+        }
+
+        int result =
+            pthread_setaffinity_np(static_cast<pthread_t>(handle), sizeof(cpu_set_t), &cpu_set);
+        if (result != 0) {
+            return POSIX_EINVAL;
+        }
+    #endif
+    */
+    return 0;
+}
+
+int PS4_SYSV_ABI posix_pthread_setaffinity_np(PthreadT thread, size_t cpusetsize,
+                                              const Cpuset* cpusetp) {
+    if (thread == nullptr || cpusetp == nullptr) {
+        return POSIX_EINVAL;
+    }
+    thread->attr.cpusetsize = cpusetsize;
+    return thread->SetAffinity(cpusetp);
+}
+
+int PS4_SYSV_ABI scePthreadSetaffinity(PthreadT thread, const Cpuset mask) {
+    int result = posix_pthread_setaffinity_np(thread, 0x10, &mask);
+    if (result != 0) {
+        return ErrnoToSceKernelError(result);
+    }
+    return 0;
+}
+
 void RegisterThread(Core::Loader::SymbolsResolver* sym) {
     // Posix
     LIB_FUNCTION("Z4QosVuAsA0", "libScePosix", 1, "libkernel", 1, 1, posix_pthread_once);
@@ -544,6 +612,7 @@ void RegisterThread(Core::Loader::SymbolsResolver* sym) {
     LIB_FUNCTION("Z4QosVuAsA0", "libkernel", 1, "libkernel", 1, 1, posix_pthread_once);
     LIB_FUNCTION("EotR8a3ASf4", "libkernel", 1, "libkernel", 1, 1, posix_pthread_self);
     LIB_FUNCTION("OxhIB8LB-PQ", "libkernel", 1, "libkernel", 1, 1, posix_pthread_create);
+    LIB_FUNCTION("5KWrg7-ZqvE", "libkernel", 1, "libkernel", 1, 1, posix_pthread_setaffinity_np);
 
     // Orbis
     LIB_FUNCTION("14bOACANTBo", "libkernel", 1, "libkernel", 1, 1, ORBIS(posix_pthread_once));
@@ -566,6 +635,7 @@ void RegisterThread(Core::Loader::SymbolsResolver* sym) {
     LIB_FUNCTION("W0Hpm2X0uPE", "libkernel", 1, "libkernel", 1, 1, ORBIS(posix_pthread_setprio));
     LIB_FUNCTION("rNhWz+lvOMU", "libkernel", 1, "libkernel", 1, 1, _sceKernelSetThreadDtors);
     LIB_FUNCTION("6XG4B33N09g", "libkernel", 1, "libkernel", 1, 1, sched_yield);
+    LIB_FUNCTION("bt3CTBKmGyI", "libkernel", 1, "libkernel", 1, 1, scePthreadSetaffinity)
 }
 
 } // namespace Libraries::Kernel

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

@@ -332,6 +332,8 @@ struct Pthread {
             return true;
         }
     }
+
+    int SetAffinity(const Cpuset* cpuset);
 };
 using PthreadT = Pthread*;