Implement scheduling of IOP threads. (#1689)

* Use sleepthread in RPC loop

* Keep a pointer to current IOP thread

* Implement IOP thread scheduling based on priority

And implement DelayThread as an actual delay.

* Run IOP flat out

* Use information from scheduler in wait_run_iop

* Lock sif mutex in set_rpc_queue

* always use kernel dispatch with wait_run

* Loop in dispatch until no thread is ready

* Use timestamp for next wakeup

instead of duration

* Wrap IOP thread entrypoints for safety

Libco threads are not supposed to return from their entrypoint

* Use a queue for IOP thread wakeups from EE thread

game/runtime.cpp

@@ -244,7 +244,7 @@ void iop_runner(SystemThreadInterface& iface) {
   bool complete = false;
   start_overlord_wrapper(iop.overlord_argc, iop.overlord_argv, &complete);  // todo!
   while (complete == false) {
-    iop.kernel.dispatchAll();
+    iop.wait_run_iop(iop.kernel.dispatch());
   }
 
   // unblock the EE, the overlord is set up!
@@ -252,10 +252,9 @@ void iop_runner(SystemThreadInterface& iface) {
 
   // IOP Kernel loop
   while (!iface.get_want_exit() && !iop.want_exit) {
-    // the IOP kernel just runs at full blast, so we only run the IOP when the EE is waiting on the
-    // IOP. Each time the EE is waiting on the IOP, it will run an iteration of the IOP kernel.
-    iop.wait_run_iop();
-    iop.kernel.dispatchAll();
+    // The IOP scheduler informs us of how many microseconds are left until it has something to do.
+    // So we can wait for that long or until something else needs it to wake up.
+    iop.wait_run_iop(iop.kernel.dispatch());
   }
 }
 }  // namespace

game/sce/iop.cpp

@@ -86,7 +86,14 @@ void* AllocSysMemory(int type, unsigned long size, void* addr) {
  * Create a new thread
  */
 s32 CreateThread(ThreadParam* param) {
-  return iop->kernel.CreateThread(param->name, (void (*)())param->entry);
+  return iop->kernel.CreateThread(param->name, (void (*)())param->entry, param->initPriority);
+}
+
+/*!
+ * Exit current thread
+ */
+s32 ExitThread() {
+  return iop->kernel.ExitThread();
 }
 
 /*!
@@ -158,8 +165,7 @@ int sceCdMmode(int media) {
 }
 
 void DelayThread(u32 usec) {
-  iop->kernel.SuspendThread();
-  (void)usec;
+  iop->kernel.DelayThread(usec);
 }
 
 int sceCdBreak() {

game/sce/iop.h

@@ -96,6 +96,7 @@ void CpuEnableIntr();
 void SleepThread();
 void DelayThread(u32 usec);
 s32 CreateThread(ThreadParam* param);
+s32 ExitThread();
 s32 StartThread(s32 thid, u32 arg);
 s32 WakeupThread(s32 thid);
 

game/sce/sif_ee.cpp

@@ -84,12 +84,12 @@ s32 sceSifCallRpc(sceSifClientData* bd,
   ASSERT(!end_para);
   ASSERT(mode == 1);  // async
   iop->kernel.sif_rpc(bd->rpcd.id, fno, mode, send, ssize, recv, rsize);
-  iop->signal_run_iop(false);
+  iop->signal_run_iop();
   return 0;
 }
 
 s32 sceSifCheckStatRpc(sceSifRpcData* bd) {
-  iop->signal_run_iop(false);
+  iop->signal_run_iop();
   return iop->kernel.sif_busy(bd->id);
 }
 

game/system/IOP_Kernel.cpp

@@ -7,15 +7,46 @@
 
 #include "game/sce/iop.h"
 
+using namespace std::chrono;
+
+/*
+** wrap thread entry points to ensure they don't return into libco
+*/
+static void (*thread_entry)() = nullptr;
+static cothread_t wrap_return;
+void IopThread::functionWrapper() {
+  void (*f)() = thread_entry;
+  co_switch(wrap_return);
+  if (f != nullptr) {
+    f();
+  }
+  // libco threads must not return
+  while (true) {
+    iop::ExitThread();
+  }
+}
+
+/*
+** -----------------------------------------------------------------------------
+** Functions callable by threads
+** -----------------------------------------------------------------------------
+*/
+
 /*!
  * Create a new thread.  Will not run the thread.
  */
-s32 IOP_Kernel::CreateThread(std::string name, void (*func)()) {
+s32 IOP_Kernel::CreateThread(std::string name, void (*func)(), u32 priority) {
   u32 ID = (u32)_nextThID++;
   ASSERT(ID == threads.size());
 
   // add entry
-  threads.emplace_back(name, func, ID);
+  threads.emplace_back(name, func, ID, priority);
+
+  // enter the function wrapper so it can put the actual thread enry on its stack
+  // to call it when the thread is eventually started
+  thread_entry = func;
+  wrap_return = co_active();
+  co_switch(threads.at(ID).thread);
 
   return ID;
 }
@@ -27,37 +58,23 @@ void IOP_Kernel::StartThread(s32 id) {
   threads.at(id).state = IopThread::State::Ready;
 }
 
-/*!
- * Run a thread (call from kernel)
- */
-void IOP_Kernel::runThread(s32 id) {
-  ASSERT(_currentThread == -1);  // should run in the kernel thread
-  _currentThread = id;
-  threads.at(id).state = IopThread::State::Run;
-  co_switch(threads.at(id).thread);
-  _currentThread = -1;
+s32 IOP_Kernel::ExitThread() {
+  ASSERT(_currentThread);
+  _currentThread->state = IopThread::State::Dormant;
+
+  return 0;
 }
 
 /*!
- * Return to kernel from a thread, not to be called from the kernel thread.
+ * Put a thread in Wait state for desired amount of usecs.
  */
-void IOP_Kernel::exitThread() {
-  s32 oldThread = getCurrentThread();
-  co_switch(kernel_thread);
+void IOP_Kernel::DelayThread(u32 usec) {
+  ASSERT(_currentThread);
 
-  // check kernel resumed us correctly
-  ASSERT(_currentThread == oldThread);
-}
-
-/*!
- * Suspend a thread (call from user thread).  Will simply allow other threads to run.
- * Like yield
- * This does not match the behaviour of any real IOP function.
- */
-void IOP_Kernel::SuspendThread() {
-  ASSERT(getCurrentThread() >= 0);
-
-  threads.at(getCurrentThread()).state = IopThread::State::Ready;
+  _currentThread->state = IopThread::State::Wait;
+  _currentThread->waitType = IopThread::Wait::Delay;
+  _currentThread->resumeTime =
+      time_point_cast<microseconds>(steady_clock::now()) + microseconds(usec);
   exitThread();
 }
 
@@ -65,9 +82,9 @@ void IOP_Kernel::SuspendThread() {
  * Sleep a thread.  Must be explicitly woken up.
  */
 void IOP_Kernel::SleepThread() {
-  ASSERT(getCurrentThread() >= 0);
+  ASSERT(_currentThread);
 
-  threads.at(getCurrentThread()).state = IopThread::State::Suspend;
+  _currentThread->state = IopThread::State::Suspend;
   exitThread();
 }
 
@@ -80,21 +97,114 @@ void IOP_Kernel::WakeupThread(s32 id) {
 }
 
 /*!
- * Dispatch all IOP threads.
- * Currently does no scheduling, on the real IOP the highest priority therad that is Ready
- * will always be scheduled.
+ * Return to kernel from a thread, not to be called from the kernel thread.
  */
-void IOP_Kernel::dispatchAll() {
-  for (s64 i = 0; i < threads.size(); i++) {
-    if (threads[i].state == IopThread::State::Ready) {
-      // printf("[IOP Kernel] Dispatch %s (%ld)\n", threads[i].name.c_str(), i);
-      runThread(i);
-      // printf("[IOP Kernel] back to kernel!\n");
+void IOP_Kernel::exitThread() {
+  IopThread* oldThread = _currentThread;
+  co_switch(kernel_thread);
+
+  // check kernel resumed us correctly
+  ASSERT(_currentThread == oldThread);
+}
+
+/*
+** -----------------------------------------------------------------------------
+** Kernel functions.
+** -----------------------------------------------------------------------------
+*/
+
+/*!
+ * Run a thread (call from kernel)
+ */
+void IOP_Kernel::runThread(IopThread* thread) {
+  ASSERT(_currentThread == nullptr);  // should run in the kernel thread
+  _currentThread = thread;
+  thread->state = IopThread::State::Run;
+  co_switch(thread->thread);
+  _currentThread = nullptr;
+}
+
+/*!
+** Update wait states for delayed threads
+*/
+void IOP_Kernel::updateDelay() {
+  for (auto& t : threads) {
+    if (t.waitType == IopThread::Wait::Delay) {
+      if (steady_clock::now() > t.resumeTime) {
+        t.waitType = IopThread::Wait::None;
+        t.state = IopThread::State::Ready;
+      }
     }
   }
 }
 
+time_stamp IOP_Kernel::nextWakeup() {
+  time_stamp lowest = time_point_cast<microseconds>(steady_clock::now()) + microseconds(1000);
+
+  for (auto& t : threads) {
+    if (t.waitType == IopThread::Wait::Delay) {
+      if (t.resumeTime < lowest) {
+        lowest = t.resumeTime;
+      }
+    }
+  }
+
+  return lowest;
+}
+
+/*!
+** Get next thread to run.
+** i.e. Highest prio in ready state.
+*/
+IopThread* IOP_Kernel::schedNext() {
+  IopThread* highest_prio = nullptr;
+
+  for (auto& t : threads) {
+    if (t.state == IopThread::State::Ready) {
+      if (highest_prio == nullptr) {
+        highest_prio = &t;
+      }
+
+      // Lower number = higher priority
+      if (t.priority < highest_prio->priority) {
+        highest_prio = &t;
+      }
+    }
+  }
+
+  return highest_prio;
+};
+
+void IOP_Kernel::processWakeups() {
+  std::scoped_lock lock(wakeup_mtx);
+  while (!wakeup_queue.empty()) {
+    WakeupThread(wakeup_queue.front());
+    wakeup_queue.pop();
+  }
+}
+
+/*!
+ * Run the next IOP thread.
+ */
+time_stamp IOP_Kernel::dispatch() {
+  updateDelay();
+  processWakeups();
+
+  IopThread* next = schedNext();
+  while (next != nullptr) {
+    // printf("[IOP Kernel] Dispatch %s (%d)\n", next->name.c_str(), next->thID);
+    runThread(next);
+    updateDelay();
+    next = schedNext();
+    // printf("[IOP Kernel] back to kernel!\n");
+  }
+
+  // printf("[IOP Kernel] No runnable threads\n");
+  return nextWakeup();
+}
+
 void IOP_Kernel::set_rpc_queue(iop::sceSifQueueData* qd, u32 thread) {
+  sif_mtx.lock();
   for (const auto& r : sif_records) {
     ASSERT(!(r.qd == qd || r.thread_to_wake == thread));
   }
@@ -102,6 +212,7 @@ void IOP_Kernel::set_rpc_queue(iop::sceSifQueueData* qd, u32 thread) {
   rec.thread_to_wake = thread;
   rec.qd = qd;
   sif_records.push_back(rec);
+  sif_mtx.unlock();
 }
 
 typedef void* (*sif_rpc_handler)(unsigned int, void*, int);
@@ -158,6 +269,11 @@ void IOP_Kernel::sif_rpc(s32 rpcChannel,
   rec->cmd.started = false;
   rec->cmd.finished = false;
 
+  {
+    std::scoped_lock lock(wakeup_mtx);
+    wakeup_queue.push(rec->thread_to_wake);
+  }
+
   sif_mtx.unlock();
 }
 
@@ -199,7 +315,8 @@ void IOP_Kernel::rpc_loop(iop::sceSifQueueData* qd) {
         sif_mtx.unlock();
       }
     }
-    SuspendThread();
+
+    SleepThread();
   }
 }
 

game/system/IOP_Kernel.h

@@ -9,6 +9,7 @@
 #include <queue>
 #include <string>
 #include <thread>
+#include <utility>
 #include <vector>
 
 #include "common/common_types.h"
@@ -23,6 +24,8 @@ namespace iop {
 struct sceSifQueueData;
 }
 
+using time_stamp = std::chrono::time_point<std::chrono::steady_clock, std::chrono::microseconds>;
+
 struct SifRpcCommand {
   bool started = true;
   bool finished = true;
@@ -57,17 +60,21 @@ struct IopThread {
     Delay,
   };
 
-  IopThread(std::string n, void (*f)(), s32 ID) : name(n), function(f), thID(ID) {
-    thread = co_create(0x300000, f);
+  IopThread(std::string n, void (*f)(), s32 ID, u32 priority)
+      : name(std::move(n)), function(f), priority(priority), thID(ID) {
+    thread = co_create(0x300000, functionWrapper);
   }
 
   ~IopThread() { co_delete(thread); }
 
+  static void functionWrapper();
   std::string name;
   void (*function)();
   cothread_t thread;
   State state = State::Dormant;
   Wait waitType = Wait::None;
+  time_stamp resumeTime = {};
+  u32 priority = 0;
   s32 thID = -1;
 };
 
@@ -84,19 +91,20 @@ class IOP_Kernel {
   IOP_Kernel() {
     // this ugly hack
     threads.reserve(16);
-    CreateThread("null-thread", nullptr);
+    CreateThread("null-thread", nullptr, 0);
     CreateMbx();
     kernel_thread = co_active();
   }
 
   ~IOP_Kernel();
 
-  s32 CreateThread(std::string n, void (*f)());
+  s32 CreateThread(std::string n, void (*f)(), u32 priority);
+  s32 ExitThread();
   void StartThread(s32 id);
-  void SuspendThread();
+  void DelayThread(u32 usec);
   void SleepThread();
   void WakeupThread(s32 id);
-  void dispatchAll();
+  time_stamp dispatch();
   void set_rpc_queue(iop::sceSifQueueData* qd, u32 thread);
   void rpc_loop(iop::sceSifQueueData* qd);
   void shutdown();
@@ -104,7 +112,10 @@ class IOP_Kernel {
   /*!
    * Get current thread ID.
    */
-  s32 getCurrentThread() { return _currentThread; }
+  s32 getCurrentThread() {
+    ASSERT(_currentThread);
+    return _currentThread->thID;
+  }
 
   /*!
    * Create a message box
@@ -158,18 +169,25 @@ class IOP_Kernel {
                s32 recvSize);
 
  private:
-  void runThread(s32 id);
+  void runThread(IopThread* thread);
   void exitThread();
+  void updateDelay();
+  void processWakeups();
+
+  IopThread* schedNext();
+  time_stamp nextWakeup();
+
   cothread_t kernel_thread;
   s32 _nextThID = 0;
-  s32 _currentThread = {-1};
+  IopThread* _currentThread = nullptr;
   std::vector<IopThread> threads;
   std::vector<std::queue<void*>> mbxs;
   std::vector<SifRecord> sif_records;
   std::vector<Semaphore> semas;
+  std::queue<int> wakeup_queue;
   bool mainThreadSleep = false;
   FILE* iso_disc_file = nullptr;
-  std::mutex sif_mtx;
+  std::mutex sif_mtx, wakeup_mtx;
 };
 
 #endif  // JAK_IOP_KERNEL_H

game/system/iop_thread.cpp

@@ -55,33 +55,22 @@ void* IOP::iop_alloc(int size) {
   return mem;
 }
 
-void IOP::wait_run_iop() {
-  std::unique_lock<std::mutex> lk(iters_mutex);
-  if (iop_iters_des > iop_iters_act) {
-    iop_iters_act++;
-    return;
-  }
-
-  iop_run_cv.wait(lk, [&] { return iop_iters_des > iop_iters_act; });
-  iop_iters_act++;
+void IOP::wait_run_iop(
+    std::chrono::time_point<std::chrono::steady_clock, std::chrono::microseconds> wakeup) {
+  std::unique_lock<std::mutex> lk(run_cv_mutex);
+  iop_run_cv.wait_until(lk, wakeup);
 }
 
 void IOP::kill_from_ee() {
   want_exit = true;
-  signal_run_iop(true);
+  signal_run_iop();
 }
 
-void IOP::signal_run_iop(bool force) {
-  std::unique_lock<std::mutex> lk(iters_mutex);
-  if (iop_iters_act == iop_iters_des || force) {
-    iop_iters_des++;  // todo, tune this
-    if (iop_iters_des - iop_iters_act > 500) {
-      iop_iters_des = iop_iters_act + 500;
-    }
-    iop_run_cv.notify_all();
-  }
+void IOP::signal_run_iop() {
+  std::unique_lock<std::mutex> lk(run_cv_mutex);
+  iop_run_cv.notify_all();
 }
 
 IOP::~IOP() {
   reset_allocator();
-}
+}

game/system/iop_thread.h

@@ -19,8 +19,10 @@ class IOP {
   void wait_for_overlord_start_cmd();
   void wait_for_overlord_init_finish();
   void signal_overlord_init_finish();
-  void signal_run_iop(bool force);
-  void wait_run_iop();
+  void signal_run_iop();
+
+  void wait_run_iop(
+      std::chrono::time_point<std::chrono::steady_clock, std::chrono::microseconds> duration);
   void kill_from_ee();
 
   void set_ee_main_mem(u8* mem) { ee_main_mem = mem; }
@@ -33,14 +35,12 @@ class IOP {
 
   IOP_Kernel kernel;
   u8* ee_main_mem = nullptr;
-  u64 iop_iters_des = 0;
-  u64 iop_iters_act = 0;
   bool want_exit = false;
 
  private:
   std::vector<void*> allocations;
   std::condition_variable cv;
-  std::mutex iop_mutex, iters_mutex;
+  std::mutex iop_mutex, run_cv_mutex;
   bool overlord_init_done = false;
   std::condition_variable iop_run_cv;
 };