From 6ea8b3ef60bba657ca10c7e62dd849be9217faf3 Mon Sep 17 00:00:00 2001
From: bunnei <bunneidev@gmail.com>
Date: Sat, 5 May 2018 23:03:01 -0400
Subject: thread: Implement ChangeCore function.

---
 src/core/hle/kernel/thread.cpp | 53 +++++++++++++++++++++++++++++++++++++++++-
 src/core/hle/kernel/thread.h   |  6 +++++
 2 files changed, 58 insertions(+), 1 deletion(-)

diff --git a/src/core/hle/kernel/thread.cpp b/src/core/hle/kernel/thread.cpp
index 0a5441684..c370776e8 100644
--- a/src/core/hle/kernel/thread.cpp
+++ b/src/core/hle/kernel/thread.cpp
@@ -273,7 +273,7 @@ ResultVal<SharedPtr<Thread>> Thread::Create(std::string name, VAddr entry_point,
     thread->name = std::move(name);
     thread->callback_handle = wakeup_callback_handle_table.Create(thread).Unwrap();
     thread->owner_process = owner_process;
-    thread->scheduler = Core::System().GetInstance().Scheduler(static_cast<size_t>(processor_id));
+    thread->scheduler = Core::System().GetInstance().Scheduler(processor_id);
     thread->scheduler->AddThread(thread, priority);
 
     // Find the next available TLS index, and mark it as used
@@ -415,6 +415,57 @@ void Thread::UpdatePriority() {
         lock_owner->UpdatePriority();
 }
 
+static s32 GetNextProcessorId(u64 mask) {
+    s32 processor_id{};
+    for (s32 index = 0; index < Core::NUM_CPU_CORES; ++index) {
+        if (mask & (1ULL << index)) {
+            if (!Core::System().GetInstance().Scheduler(index)->GetCurrentThread()) {
+                // Core is enabled and not running any threads, use this one
+                return index;
+            }
+
+            // Core is enabled, but running a thread, less ideal
+            processor_id = index;
+        }
+    }
+
+    return processor_id;
+}
+
+void Thread::ChangeCore(u32 core, u64 mask) {
+    const s32 new_processor_id{GetNextProcessorId(mask)};
+
+    ASSERT(ideal_core == core); // We're not doing anything with this yet, so assert the expected
+    ASSERT(new_processor_id < Core::NUM_CPU_CORES);
+
+    if (new_processor_id == processor_id) {
+        // Already running on ideal core, nothing to do here
+        return;
+    }
+
+    ASSERT(status != THREADSTATUS_RUNNING); // Unsupported
+
+    processor_id = new_processor_id;
+    ideal_core = core;
+    mask = mask;
+
+    // Add thread to new core's scheduler
+    auto& next_scheduler = Core::System().GetInstance().Scheduler(new_processor_id);
+    next_scheduler->AddThread(this, current_priority);
+
+    if (status == THREADSTATUS_READY) {
+        // If the thread was ready, unschedule from the previous core and schedule on the new core
+        scheduler->UnscheduleThread(this, current_priority);
+        next_scheduler->ScheduleThread(this, current_priority);
+    }
+
+    // Remove thread from previous core's scheduler
+    scheduler->RemoveThread(this);
+
+    // Change thread's scheduler
+    scheduler = next_scheduler;
+}
+
 ////////////////////////////////////////////////////////////////////////////////////////////////////
 
 /**
diff --git a/src/core/hle/kernel/thread.h b/src/core/hle/kernel/thread.h
index 0a3bb1183..3dda548ad 100644
--- a/src/core/hle/kernel/thread.h
+++ b/src/core/hle/kernel/thread.h
@@ -120,6 +120,9 @@ public:
     /// Recalculates the current priority taking into account priority inheritance.
     void UpdatePriority();
 
+    /// Changes the core that the thread is running or scheduled to run on.
+    void ChangeCore(u32 core, u64 mask);
+
     /**
      * Gets the thread's thread ID
      * @return The thread's ID
@@ -244,6 +247,9 @@ public:
 
     std::shared_ptr<Scheduler> scheduler;
 
+    u32 ideal_core{0xFFFFFFFF};
+    u64 mask{0x1};
+
 private:
     Thread();
     ~Thread() override;
-- 
cgit v1.2.3