Implement IOP semaphores. (#1747)

* IOP: Rename exitThread, too close to ExitThread.

* IOP: Implement Semaphores

game/overlord/iso_queue.cpp

@@ -82,12 +82,11 @@ void InitBuffers() {
   sStrBuffer[N_STR_BUFFERS - 1].header.next = nullptr;
   sFreeStrBuffer = &sStrBuffer[0];
 
-  // TODO - this has options
   SemaParam params;
-  params.attr = 1;
+  params.attr = SA_THPRI;
   params.max_count = 1;
-  params.option = 1;
-  params.init_count = 0;
+  params.init_count = 1;
+  params.option = 0;
   sSema = CreateSema(&params);
 
   if (sSema < 0) {
@@ -332,8 +331,8 @@ VagCommand* GetVAGCommand() {
     }
 
     // wait for VAG semaphore
-    // while (WaitSema(sSema)) {
-    //}
+    while (WaitSema(sSema)) {
+    }
 
     // try to get something.
     for (s32 i = 0; i < N_VAG_CMDS; i++) {
@@ -344,24 +343,24 @@ VagCommand* GetVAGCommand() {
         if (vag_cmd_cnt > max_vag_cmd_cnt) {
           max_vag_cmd_cnt = vag_cmd_cnt;
         }
-        // SignalSema(sSema);
+        SignalSema(sSema);
         return &vag_cmds[i];
       }
     }
 
-    // SignalSema(sSema);
+    SignalSema(sSema);
   }
 }
 
 void FreeVAGCommand(VagCommand* cmd) {
   s32 idx = cmd - vag_cmds;
   if (idx >= 0 && idx < N_VAG_CMDS && ((vag_cmd_used >> (idx & 0x1f)) & 1)) {
-    // while (WaitSema(sSema)) {
-    // }
+    while (WaitSema(sSema)) {
+    }
 
     vag_cmd_used &= ~(1 << (idx & 0x1f));
     vag_cmd_cnt--;
-    // SignalSema(sSema);
+    SignalSema(sSema);
   } else {
     printf("[OVERLORD] Invalid FreeVAGCommand!\n");
   }

game/overlord/srpc.cpp

@@ -93,21 +93,22 @@ u32 Thread_Loader() {
 void* RPC_Player(unsigned int /*fno*/, void* data, int size) {
   if (gSoundEnable) {
     gFreeMem = QueryTotalFreeMemSize();
-    // if (!PollSema(gSema)) {
-    if (gMusic) {
-      if (!gMusicPause && !LookupSound(666)) {
-        Sound* music = AllocateSound();
-        if (music != nullptr) {
-          gMusicFade = 0;
-          gMusicFadeDir = 1;
-          SetMusicVol();
-          music->sound_handle = snd_PlaySoundVolPanPMPB(gMusic, 0, 0x400, -1, 0, 0);
-          music->id = 666;
-          music->is_music = 1;
+    if (!PollSema(gSema)) {
+      if (gMusic) {
+        if (!gMusicPause && !LookupSound(666)) {
+          Sound* music = AllocateSound();
+          if (music != nullptr) {
+            gMusicFade = 0;
+            gMusicFadeDir = 1;
+            SetMusicVol();
+            music->sound_handle = snd_PlaySoundVolPanPMPB(gMusic, 0, 0x400, -1, 0, 0);
+            music->id = 666;
+            music->is_music = 1;
+          }
         }
       }
+      SignalSema(gSema);
     }
-    //}
 
     SetMusicVol();
     Sound* music = LookupSound(666);
@@ -396,8 +397,8 @@ void* RPC_Loader(unsigned int /*fno*/, void* data, int size) {
           printf("IOP language: %s\n", gLanguage);  // added.
         } break;
         case SoundCommand::LOAD_MUSIC: {
-          // while (WaitSema(gSema))
-          //   ;
+          while (WaitSema(gSema))
+            ;
           if (gMusic) {
             gMusicFadeDir = -1;
             while (gMusicFade) {
@@ -408,14 +409,14 @@ void* RPC_Loader(unsigned int /*fno*/, void* data, int size) {
             gMusic = 0;
           }
           LoadMusic(cmd->load_bank.bank_name, &gMusic);
-          //  SignalSema(gSema);
+          SignalSema(gSema);
         } break;
         case SoundCommand::LIST_SOUNDS: {
           PrintActiveSounds();
         } break;
         case SoundCommand::UNLOAD_MUSIC: {
-          // while (WaitSema(gSema))
-          //   ;
+          while (WaitSema(gSema))
+            ;
           if (gMusic) {
             gMusicFadeDir = -1;
             while (gMusicFade) {
@@ -425,7 +426,7 @@ void* RPC_Loader(unsigned int /*fno*/, void* data, int size) {
             snd_ResolveBankXREFS();
             gMusic = 0;
           }
-          // SignalSema(gSema);
+          SignalSema(gSema);
         } break;
         default:
           ASSERT_MSG(false, fmt::format("Unhandled RPC Loader command {}", (int)cmd->command));

game/overlord/ssound.cpp

@@ -134,7 +134,7 @@ void InitSound_Overlord() {
   snd_SetPlayBackMode(2);
 
   SemaParam sema;
-  sema.attr = 1;
+  sema.attr = SA_THPRI;
   sema.init_count = 1;
   sema.max_count = 1;
   sema.option = 0;

game/sce/iop.cpp

@@ -206,20 +206,19 @@ void SleepThread() {
 }
 
 s32 CreateSema(SemaParam* param) {
-  (void)param;
-  return iop->kernel.CreateSema();
+  return iop->kernel.CreateSema(param->attr, param->option, param->max_count, param->init_count);
 }
 
 s32 WaitSema(s32 sema) {
-  (void)sema;
-  ASSERT(false);  // nyi
-  return 0;
+  return iop->kernel.WaitSema(sema);
 }
 
 s32 SignalSema(s32 sema) {
-  (void)sema;
-  ASSERT(false);  // nyi
-  return 0;
+  return iop->kernel.SignalSema(sema);
+}
+
+s32 PollSema(s32 sema) {
+  return iop->kernel.PollSema(sema);
 }
 
 s32 WakeupThread(s32 thid) {

game/sce/iop.h

@@ -20,11 +20,16 @@
 #define SCECdComplete 0x02
 #define SCECdNotReady 0x06
 #define KE_OK 0
+#define KE_SEMA_ZERO -419
+#define KE_SEMA_OVF -420
 #define KE_MBOX_NOMSG -424
 #define KE_WAIT_DELETE -425
 
 #define TH_C 0x02000000
 
+#define SA_THFIFO 0
+#define SA_THPRI 1
+
 class IOP;
 
 namespace iop {
@@ -81,9 +86,9 @@ struct ThreadParam {
 
 struct SemaParam {
   uint32_t attr;
+  uint32_t option;
   int32_t init_count;
   int32_t max_count;
-  uint32_t option;
 };
 
 // void PS2_RegisterIOP(IOP *iop);
@@ -131,6 +136,7 @@ void GetSystemTime(SysClock* time);
 s32 CreateSema(SemaParam* param);
 s32 WaitSema(s32 sema);
 s32 SignalSema(s32 sema);
+s32 PollSema(s32 sema);
 
 void FlushDcache();
 

game/system/IOP_Kernel.cpp

@@ -75,7 +75,7 @@ void IOP_Kernel::DelayThread(u32 usec) {
   _currentThread->waitType = IopThread::Wait::Delay;
   _currentThread->resumeTime =
       time_point_cast<microseconds>(steady_clock::now()) + microseconds(usec);
-  exitThread();
+  leaveThread();
 }
 
 /*!
@@ -85,7 +85,7 @@ void IOP_Kernel::SleepThread() {
   ASSERT(_currentThread);
 
   _currentThread->state = IopThread::State::Suspend;
-  exitThread();
+  leaveThread();
 }
 
 /*!
@@ -96,10 +96,66 @@ void IOP_Kernel::WakeupThread(s32 id) {
   threads.at(id).state = IopThread::State::Ready;
 }
 
+s32 IOP_Kernel::WaitSema(s32 id) {
+  auto& sema = semas.at(id);
+  if (sema.count > 0) {
+    sema.count--;
+    return KE_OK;
+  }
+
+  sema.wait_list.push_back(_currentThread);
+  _currentThread->state = IopThread::State::Wait;
+  _currentThread->waitType = IopThread::Wait::Semaphore;
+  leaveThread();
+
+  return KE_OK;
+}
+
+s32 IOP_Kernel::SignalSema(s32 id) {
+  auto& sema = semas.at(id);
+
+  if (sema.count >= sema.maxCount) {
+    return KE_SEMA_OVF;
+  }
+
+  if (sema.wait_list.empty()) {
+    sema.count++;
+    return KE_OK;
+  }
+
+  IopThread* to_run = nullptr;
+
+  if (sema.attr == Semaphore::attribute::fifo) {
+    to_run = sema.wait_list.front();
+    sema.wait_list.pop_front();
+  } else {
+    auto it =
+        std::max_element(sema.wait_list.begin(), sema.wait_list.end(),
+                         [](IopThread*& a, IopThread*& b) { return a->priority < b->priority; });
+    to_run = *it;
+    sema.wait_list.erase(it);
+  }
+
+  to_run->waitType = IopThread::Wait::None;
+  to_run->state = IopThread::State::Ready;
+  return KE_OK;
+}
+
+s32 IOP_Kernel::PollSema(s32 id) {
+  auto& sema = semas.at(id);
+  if (sema.count > 0) {
+    sema.count--;
+    ASSERT(sema.count >= 0);
+    return KE_OK;
+  }
+
+  return KE_SEMA_ZERO;
+}
+
 /*!
  * Return to kernel from a thread, not to be called from the kernel thread.
  */
-void IOP_Kernel::exitThread() {
+void IOP_Kernel::leaveThread() {
   IopThread* oldThread = _currentThread;
   co_switch(kernel_thread);
 

game/system/IOP_Kernel.h

@@ -5,6 +5,7 @@
 
 #include <atomic>
 #include <condition_variable>
+#include <list>
 #include <mutex>
 #include <queue>
 #include <string>
@@ -79,11 +80,17 @@ struct IopThread {
 };
 
 struct Semaphore {
-  u32 option;
-  u32 attr;
-  s32 count;
-  s32 maxCount;
-  s32 initCount;
+  enum class attribute { fifo, prio };
+  Semaphore(attribute attr, s32 option, s32 init_count, s32 max_count)
+      : attr(attr), option(option), count(init_count), initCount(init_count), maxCount(max_count) {}
+
+  attribute attr{attribute::fifo};
+  u32 option{0};
+  s32 count{0};
+  s32 initCount{0};
+  s32 maxCount{0};
+
+  std::list<IopThread*> wait_list;
 };
 
 class IOP_Kernel {
@@ -93,6 +100,7 @@ class IOP_Kernel {
     threads.reserve(16);
     CreateThread("null-thread", nullptr, 0);
     CreateMbx();
+    CreateSema(0, 0, 0, 0);
     kernel_thread = co_active();
   }
 
@@ -155,7 +163,15 @@ class IOP_Kernel {
     return 0;
   }
 
-  s32 CreateSema() { return 1; }
+  s32 CreateSema(s32 attr, s32 option, s32 init_count, s32 max_count) {
+    s32 id = semas.size();
+    semas.emplace_back((Semaphore::attribute)attr, option, init_count, max_count);
+    return id;
+  }
+
+  s32 WaitSema(s32 id);
+  s32 SignalSema(s32 id);
+  s32 PollSema(s32 id);
 
   void read_disc_sectors(u32 sector, u32 sectors, void* buffer);
   bool sif_busy(u32 id);
@@ -170,7 +186,7 @@ class IOP_Kernel {
 
  private:
   void runThread(IopThread* thread);
-  void exitThread();
+  void leaveThread();
   void updateDelay();
   void processWakeups();