8 files changed, 97 insertions, 57 deletions

diff --git a/src/core/hle/kernel/kernel.cpp b/src/core/hle/kernel/kernel.cpp
index bb3e312a7..4cf9cee42 100644
--- a/src/core/hle/kernel/kernel.cpp
+++ b/src/core/hle/kernel/kernel.cpp
@@ -68,6 +68,12 @@ struct KernelCore::Impl {
         InitializeSuspendThreads();
     }
 
+    void InitializeCores() {
+        for (auto& core : cores) {
+            core.Initialize(current_process->Is64BitProcess());
+        }
+    }
+
     void Shutdown() {
         next_object_id = 0;
         next_kernel_process_id = Process::InitialKIPIDMin;
@@ -116,7 +122,7 @@ struct KernelCore::Impl {
             Core::MakeExclusiveMonitor(system.Memory(), Core::Hardware::NUM_CPU_CORES);
         for (std::size_t i = 0; i < Core::Hardware::NUM_CPU_CORES; i++) {
             schedulers[i] = std::make_unique<Kernel::Scheduler>(system, i);
-            cores.emplace_back(system, i, *schedulers[i], interrupts[i]);
+            cores.emplace_back(i, system, *schedulers[i], interrupts);
         }
     }
 
@@ -181,6 +187,7 @@ struct KernelCore::Impl {
         if (process == nullptr) {
             return;
         }
+
         const u32 core_id = GetCurrentHostThreadID();
         if (core_id < Core::Hardware::NUM_CPU_CORES) {
             system.Memory().SetCurrentPageTable(*process, core_id);
@@ -372,6 +379,10 @@ void KernelCore::Initialize() {
     impl->Initialize(*this);
 }
 
+void KernelCore::InitializeCores() {
+    impl->InitializeCores();
+}
+
 void KernelCore::Shutdown() {
     impl->Shutdown();
 }
@@ -486,12 +497,12 @@ const Core::ExclusiveMonitor& KernelCore::GetExclusiveMonitor() const {
 }
 
 void KernelCore::InvalidateAllInstructionCaches() {
-    auto& threads = GlobalScheduler().GetThreadList();
-    for (auto& thread : threads) {
-        if (!thread->IsHLEThread()) {
-            auto& arm_interface = thread->ArmInterface();
-            arm_interface.ClearInstructionCache();
+    if (!IsMulticore()) {
+        for (auto& physical_core : impl->cores) {
+            physical_core.ArmInterface().ClearInstructionCache();
         }
+    } else {
+        ASSERT_MSG(false, "UNIMPLEMENTED!!!!!!!!!!!");
     }
 }
 
diff --git a/src/core/hle/kernel/kernel.h b/src/core/hle/kernel/kernel.h
index 16285c3f0..a9fdc5860 100644
--- a/src/core/hle/kernel/kernel.h
+++ b/src/core/hle/kernel/kernel.h
@@ -74,6 +74,9 @@ public:
     /// Resets the kernel to a clean slate for use.
     void Initialize();
 
+    /// Initializes the CPU cores.
+    void InitializeCores();
+
     /// Clears all resources in use by the kernel instance.
     void Shutdown();
 
diff --git a/src/core/hle/kernel/physical_core.cpp b/src/core/hle/kernel/physical_core.cpp
index 6e04d025f..50aca5752 100644
--- a/src/core/hle/kernel/physical_core.cpp
+++ b/src/core/hle/kernel/physical_core.cpp
@@ -4,21 +4,43 @@
 
 #include "common/spin_lock.h"
 #include "core/arm/cpu_interrupt_handler.h"
+#include "core/arm/dynarmic/arm_dynarmic_32.h"
+#include "core/arm/dynarmic/arm_dynarmic_64.h"
 #include "core/core.h"
+#include "core/hle/kernel/kernel.h"
 #include "core/hle/kernel/physical_core.h"
 #include "core/hle/kernel/scheduler.h"
 
 namespace Kernel {
 
-PhysicalCore::PhysicalCore(Core::System& system, std::size_t id, Kernel::Scheduler& scheduler,
-                           Core::CPUInterruptHandler& interrupt_handler)
-    : interrupt_handler{interrupt_handler},
-      core_index{id}, scheduler{scheduler}, guard{std::make_unique<Common::SpinLock>()} {}
+PhysicalCore::PhysicalCore(std::size_t core_index, Core::System& system,
+                           Kernel::Scheduler& scheduler, Core::CPUInterrupts& interrupts)
+    : core_index{core_index}, system{system}, scheduler{scheduler},
+      interrupts{interrupts}, guard{std::make_unique<Common::SpinLock>()} {}
 
 PhysicalCore::~PhysicalCore() = default;
 
+void PhysicalCore::Initialize([[maybe_unused]] bool is_64_bit) {
+#ifdef ARCHITECTURE_x86_64
+    auto& kernel = system.Kernel();
+    if (is_64_bit) {
+        arm_interface = std::make_unique<Core::ARM_Dynarmic_64>(
+            system, interrupts, kernel.IsMulticore(), kernel.GetExclusiveMonitor(), core_index);
+    } else {
+        arm_interface = std::make_unique<Core::ARM_Dynarmic_32>(
+            system, interrupts, kernel.IsMulticore(), kernel.GetExclusiveMonitor(), core_index);
+    }
+#else
+#error Platform not supported yet.
+#endif
+}
+
+void PhysicalCore::Run() {
+    arm_interface->Run();
+}
+
 void PhysicalCore::Idle() {
-    interrupt_handler.AwaitInterrupt();
+    interrupts[core_index].AwaitInterrupt();
 }
 
 void PhysicalCore::Shutdown() {
@@ -26,18 +48,18 @@ void PhysicalCore::Shutdown() {
 }
 
 bool PhysicalCore::IsInterrupted() const {
-    return interrupt_handler.IsInterrupted();
+    return interrupts[core_index].IsInterrupted();
 }
 
 void PhysicalCore::Interrupt() {
     guard->lock();
-    interrupt_handler.SetInterrupt(true);
+    interrupts[core_index].SetInterrupt(true);
     guard->unlock();
 }
 
 void PhysicalCore::ClearInterrupt() {
     guard->lock();
-    interrupt_handler.SetInterrupt(false);
+    interrupts[core_index].SetInterrupt(false);
     guard->unlock();
 }
 
diff --git a/src/core/hle/kernel/physical_core.h b/src/core/hle/kernel/physical_core.h
index d7a7a951c..ace058a5a 100644
--- a/src/core/hle/kernel/physical_core.h
+++ b/src/core/hle/kernel/physical_core.h
@@ -4,9 +4,12 @@
 
 #pragma once
 
+#include <array>
 #include <cstddef>
 #include <memory>
 
+#include "core/arm/arm_interface.h"
+
 namespace Common {
 class SpinLock;
 }
@@ -16,7 +19,6 @@ class Scheduler;
 } // namespace Kernel
 
 namespace Core {
-class ARM_Interface;
 class CPUInterruptHandler;
 class ExclusiveMonitor;
 class System;
@@ -26,8 +28,8 @@ namespace Kernel {
 
 class PhysicalCore {
 public:
-    PhysicalCore(Core::System& system, std::size_t id, Kernel::Scheduler& scheduler,
-                 Core::CPUInterruptHandler& interrupt_handler);
+    PhysicalCore(std::size_t core_index, Core::System& system, Kernel::Scheduler& scheduler,
+                 Core::CPUInterrupts& interrupts);
     ~PhysicalCore();
 
     PhysicalCore(const PhysicalCore&) = delete;
@@ -36,7 +38,14 @@ public:
     PhysicalCore(PhysicalCore&&) = default;
     PhysicalCore& operator=(PhysicalCore&&) = default;
 
+    /// Initialize the core for the specified parameters.
+    void Initialize(bool is_64_bit);
+
+    /// Execute current jit state
+    void Run();
+
     void Idle();
+
     /// Interrupt this physical core.
     void Interrupt();
 
@@ -49,6 +58,14 @@ public:
     // Shutdown this physical core.
     void Shutdown();
 
+    Core::ARM_Interface& ArmInterface() {
+        return *arm_interface;
+    }
+
+    const Core::ARM_Interface& ArmInterface() const {
+        return *arm_interface;
+    }
+
     bool IsMainCore() const {
         return core_index == 0;
     }
@@ -70,10 +87,12 @@ public:
     }
 
 private:
-    Core::CPUInterruptHandler& interrupt_handler;
-    std::size_t core_index;
+    const std::size_t core_index;
+    Core::System& system;
     Kernel::Scheduler& scheduler;
+    Core::CPUInterrupts& interrupts;
     std::unique_ptr<Common::SpinLock> guard;
+    std::unique_ptr<Core::ARM_Interface> arm_interface;
 };
 
 } // namespace Kernel
diff --git a/src/core/hle/kernel/scheduler.cpp b/src/core/hle/kernel/scheduler.cpp
index 6b7db5372..0805e9914 100644
--- a/src/core/hle/kernel/scheduler.cpp
+++ b/src/core/hle/kernel/scheduler.cpp
@@ -621,11 +621,14 @@ void Scheduler::OnThreadStart() {
 void Scheduler::Unload() {
     Thread* thread = current_thread.get();
     if (thread) {
-        thread->SetContinuousOnSVC(false);
         thread->last_running_ticks = system.CoreTiming().GetCPUTicks();
         thread->SetIsRunning(false);
+        if (thread->IsContinuousOnSVC() && !thread->IsHLEThread()) {
+            system.ArmInterface(core_id).ExceptionalExit();
+            thread->SetContinuousOnSVC(false);
+        }
         if (!thread->IsHLEThread() && !thread->HasExited()) {
-            Core::ARM_Interface& cpu_core = thread->ArmInterface();
+            Core::ARM_Interface& cpu_core = system.ArmInterface(core_id);
             cpu_core.SaveContext(thread->GetContext32());
             cpu_core.SaveContext(thread->GetContext64());
             // Save the TPIDR_EL0 system register in case it was modified.
@@ -652,12 +655,11 @@ void Scheduler::Reload() {
             system.Kernel().MakeCurrentProcess(thread_owner_process);
         }
         if (!thread->IsHLEThread()) {
-            Core::ARM_Interface& cpu_core = thread->ArmInterface();
+            Core::ARM_Interface& cpu_core = system.ArmInterface(core_id);
             cpu_core.LoadContext(thread->GetContext32());
             cpu_core.LoadContext(thread->GetContext64());
             cpu_core.SetTlsAddress(thread->GetTLSAddress());
             cpu_core.SetTPIDR_EL0(thread->GetTPIDR_EL0());
-            cpu_core.ChangeProcessorID(this->core_id);
             cpu_core.ClearExclusiveState();
         }
     }
@@ -679,12 +681,11 @@ void Scheduler::SwitchContextStep2() {
             system.Kernel().MakeCurrentProcess(thread_owner_process);
         }
         if (!selected_thread->IsHLEThread()) {
-            Core::ARM_Interface& cpu_core = selected_thread->ArmInterface();
+            Core::ARM_Interface& cpu_core = system.ArmInterface(core_id);
             cpu_core.LoadContext(selected_thread->GetContext32());
             cpu_core.LoadContext(selected_thread->GetContext64());
             cpu_core.SetTlsAddress(selected_thread->GetTLSAddress());
             cpu_core.SetTPIDR_EL0(selected_thread->GetTPIDR_EL0());
-            cpu_core.ChangeProcessorID(this->core_id);
             cpu_core.ClearExclusiveState();
         }
     }
@@ -715,11 +716,14 @@ void Scheduler::SwitchContext() {
         if (new_thread != nullptr && new_thread->IsSuspendThread()) {
             previous_thread->SetWasRunning(true);
         }
-        previous_thread->SetContinuousOnSVC(false);
         previous_thread->last_running_ticks = system.CoreTiming().GetCPUTicks();
         previous_thread->SetIsRunning(false);
+        if (previous_thread->IsContinuousOnSVC() && !previous_thread->IsHLEThread()) {
+            system.ArmInterface(core_id).ExceptionalExit();
+            previous_thread->SetContinuousOnSVC(false);
+        }
         if (!previous_thread->IsHLEThread() && !previous_thread->HasExited()) {
-            Core::ARM_Interface& cpu_core = previous_thread->ArmInterface();
+            Core::ARM_Interface& cpu_core = system.ArmInterface(core_id);
             cpu_core.SaveContext(previous_thread->GetContext32());
             cpu_core.SaveContext(previous_thread->GetContext64());
             // Save the TPIDR_EL0 system register in case it was modified.
diff --git a/src/core/hle/kernel/svc.cpp b/src/core/hle/kernel/svc.cpp
index e3b770d66..95d6e2b4d 100644
--- a/src/core/hle/kernel/svc.cpp
+++ b/src/core/hle/kernel/svc.cpp
@@ -2639,6 +2639,9 @@ void Call(Core::System& system, u32 immediate) {
     auto& kernel = system.Kernel();
     kernel.EnterSVCProfile();
 
+    auto* thread = system.CurrentScheduler().GetCurrentThread();
+    thread->SetContinuousOnSVC(true);
+
     const FunctionDef* info = system.CurrentProcess()->Is64BitProcess() ? GetSVCInfo64(immediate)
                                                                         : GetSVCInfo32(immediate);
     if (info) {
@@ -2652,6 +2655,12 @@ void Call(Core::System& system, u32 immediate) {
     }
 
     kernel.ExitSVCProfile();
+
+    if (!thread->IsContinuousOnSVC()) {
+        auto* host_context = thread->GetHostContext().get();
+        host_context->Rewind();
+    }
+
     system.EnterDynarmicProfile();
 }
 
diff --git a/src/core/hle/kernel/thread.cpp b/src/core/hle/kernel/thread.cpp
index da0cb26b6..3abe12810 100644
--- a/src/core/hle/kernel/thread.cpp
+++ b/src/core/hle/kernel/thread.cpp
@@ -12,7 +12,6 @@
 #include "common/fiber.h"
 #include "common/logging/log.h"
 #include "common/thread_queue_list.h"
-#include "core/arm/arm_interface.h"
 #include "core/core.h"
 #include "core/cpu_manager.h"
 #include "core/hardware_properties.h"
@@ -62,7 +61,6 @@ void Thread::Stop() {
             // Mark the TLS slot in the thread's page as free.
             owner_process->FreeTLSRegion(tls_address);
         }
-        arm_interface.reset();
         has_exited = true;
     }
     global_handle = 0;
@@ -217,22 +215,9 @@ ResultVal<std::shared_ptr<Thread>> Thread::Create(Core::System& system, ThreadTy
         thread->tls_address = 0;
     }
 
-    thread->arm_interface.reset();
+    // TODO(peachum): move to ScheduleThread() when scheduler is added so selected core is used
+    // to initialize the context
     if ((type_flags & THREADTYPE_HLE) == 0) {
-#ifdef ARCHITECTURE_x86_64
-        if (owner_process && !owner_process->Is64BitProcess()) {
-            thread->arm_interface = std::make_unique<Core::ARM_Dynarmic_32>(
-                system, kernel.Interrupts(), kernel.IsMulticore(), kernel.GetExclusiveMonitor(),
-                processor_id);
-        } else {
-            thread->arm_interface = std::make_unique<Core::ARM_Dynarmic_64>(
-                system, kernel.Interrupts(), kernel.IsMulticore(), kernel.GetExclusiveMonitor(),
-                processor_id);
-        }
-#else
-#error Platform not supported yet.
-#endif
-
         ResetThreadContext32(thread->context_32, static_cast<u32>(stack_top),
                              static_cast<u32>(entry_point), static_cast<u32>(arg));
         ResetThreadContext64(thread->context_64, stack_top, entry_point, arg);
@@ -268,14 +253,6 @@ VAddr Thread::GetCommandBufferAddress() const {
     return GetTLSAddress() + command_header_offset;
 }
 
-Core::ARM_Interface& Thread::ArmInterface() {
-    return *arm_interface;
-}
-
-const Core::ARM_Interface& Thread::ArmInterface() const {
-    return *arm_interface;
-}
-
 void Thread::SetStatus(ThreadStatus new_status) {
     if (new_status == status) {
         return;
diff --git a/src/core/hle/kernel/thread.h b/src/core/hle/kernel/thread.h
index 8daf79fac..20e86fb81 100644
--- a/src/core/hle/kernel/thread.h
+++ b/src/core/hle/kernel/thread.h
@@ -248,10 +248,6 @@ public:
 
     void SetSynchronizationResults(SynchronizationObject* object, ResultCode result);
 
-    Core::ARM_Interface& ArmInterface();
-
-    const Core::ARM_Interface& ArmInterface() const;
-
     SynchronizationObject* GetSignalingObject() const {
         return signaling_object;
     }
@@ -586,7 +582,6 @@ private:
     Common::SpinLock context_guard{};
     ThreadContext32 context_32{};
     ThreadContext64 context_64{};
-    std::unique_ptr<Core::ARM_Interface> arm_interface{};
     std::shared_ptr<Common::Fiber> host_context{};
 
     u64 thread_id = 0;