From cac16d639cd59111a718062bd3d4991f9a346197 Mon Sep 17 00:00:00 2001
From: AzureMarker <mark.drobnak@gmail.com>
Date: Sat, 12 Feb 2022 21:06:27 -0800
Subject: [PATCH] Replace pthread_getpriority with more standard version (and
 add set fn)

In the case of the main thread we use a hack since there's no nice API
for getting the main thread's handle (see code).
---
 src/lib.rs | 117 ++++++++++++++++++++++++++++++++++++++++++++++++++---
 1 file changed, 112 insertions(+), 5 deletions(-)

diff --git a/src/lib.rs b/src/lib.rs
index 4d19f8f..7424de0 100644
--- a/src/lib.rs
+++ b/src/lib.rs
@@ -87,7 +87,7 @@ pub unsafe extern "C" fn pthread_join(
 
 #[no_mangle]
 pub unsafe extern "C" fn pthread_detach(thread: libc::pthread_t) -> libc::c_int {
-    if native == MAIN_THREAD_ID {
+    if thread == MAIN_THREAD_ID {
         // This is not a valid thread to detach
         return libc::EINVAL;
     }
@@ -102,11 +102,110 @@ pub unsafe extern "C" fn pthread_self() -> libc::pthread_t {
     ctru_sys::threadGetCurrent() as libc::pthread_t
 }
 
+unsafe fn get_main_thread_handle() -> Result<ctru_sys::Handle, ctru_sys::Result> {
+    // Unfortunately I don't know of any better way to get the main thread's
+    // handle, other than via svcGetThreadList and svcOpenThread.
+    // Experimentally, the main thread ID is always the first in the list.
+    let mut thread_ids = [0; 1];
+    let mut thread_ids_count = 0;
+    let result = ctru_sys::svcGetThreadList(
+        &mut thread_ids_count,
+        thread_ids.as_mut_ptr(),
+        thread_ids.len() as i32,
+        ctru_sys::CUR_PROCESS_HANDLE,
+    );
+    if ctru_sys::R_FAILED(result) {
+        return Err(result);
+    }
+
+    // Get the main thread's handle
+    let main_thread_id = thread_ids[0];
+    let mut main_thread_handle = 0;
+    let result = ctru_sys::svcOpenThread(
+        &mut main_thread_handle,
+        ctru_sys::CUR_PROCESS_HANDLE,
+        main_thread_id,
+    );
+    if ctru_sys::R_FAILED(result) {
+        return Err(result);
+    }
+
+    Ok(main_thread_handle)
+}
+
+unsafe fn get_thread_handle(native: libc::pthread_t) -> Result<ctru_sys::Handle, ctru_sys::Result> {
+    if native == MAIN_THREAD_ID {
+        get_main_thread_handle()
+    } else {
+        Ok(ctru_sys::threadGetHandle(native as ctru_sys::Thread))
+    }
+}
+
 #[no_mangle]
-pub unsafe extern "C" fn pthread_getpriority() -> libc::c_int {
+pub unsafe extern "C" fn pthread_getschedparam(
+    native: libc::pthread_t,
+    policy: *mut libc::c_int,
+    param: *mut libc::sched_param,
+) -> libc::c_int {
+    let handle = match get_thread_handle(native) {
+        Ok(handle) => handle,
+        Err(_) => return libc::ESRCH,
+    };
+
+    if handle == u32::MAX {
+        // The thread has already finished
+        return libc::ESRCH;
+    }
+
     let mut priority = 0;
-    ctru_sys::svcGetThreadPriority(&mut priority, ctru_sys::CUR_THREAD_HANDLE);
-    priority
+    let result = ctru_sys::svcGetThreadPriority(&mut priority, handle);
+    if ctru_sys::R_FAILED(result) {
+        // Some error occurred. This is the only error defined for this
+        // function, so return it. Maybe the thread exited while this function
+        // was exiting?
+        return libc::ESRCH;
+    }
+
+    (*param).sched_priority = priority;
+
+    // SCHED_FIFO is closest to how the cooperative app core works, while
+    // SCHED_RR is closest to how the preemptive sys core works.
+    // However, we don't have an API to get the current processor ID of a chosen
+    // thread (only the current), so we just always return SCHED_FIFO.
+    (*policy) = libc::SCHED_FIFO;
+
+    0
+}
+
+#[no_mangle]
+pub unsafe extern "C" fn pthread_setschedparam(
+    native: libc::pthread_t,
+    policy: libc::c_int,
+    param: *const libc::sched_param,
+) -> libc::c_int {
+    if policy != libc::SCHED_FIFO {
+        // We only accept SCHED_FIFO. See the note in pthread_getschedparam.
+        return libc::EINVAL;
+    }
+
+    let handle = match get_thread_handle(native) {
+        Ok(handle) => handle,
+        Err(_) => return libc::EINVAL,
+    };
+
+    if handle == u32::MAX {
+        // The thread has already finished
+        return libc::ESRCH;
+    }
+
+    let result = ctru_sys::svcSetThreadPriority(handle, (*param).sched_priority);
+    if ctru_sys::R_FAILED(result) {
+        // Probably the priority is out of the permissible bounds
+        // TODO: improve the error code by checking the result further?
+        return libc::EPERM;
+    }
+
+    0
 }
 
 /// Internal struct for storing pthread attribute data
@@ -120,12 +219,20 @@ struct PThreadAttr {
 
 impl Default for PThreadAttr {
     fn default() -> Self {
+        let thread_id = unsafe { libc::pthread_self() };
+        let mut policy = 0;
+        let mut sched_param = libc::sched_param { sched_priority: 0 };
+
+        unsafe { libc::pthread_getschedparam(thread_id, &mut policy, &mut sched_param) };
+
+        let priority = sched_param.sched_priority;
+
         PThreadAttr {
             stack_size: libc::PTHREAD_STACK_MIN,
 
             // If no priority value is specified, spawn with the same
             // priority as the current thread
-            priority: unsafe { pthread_getpriority() },
+            priority,
 
             // If no processor is specified, spawn on the default core.
             // (determined by the application's Exheader)