1 files changed, 142 insertions, 0 deletions
diff --git a/src/Dolphin/dvd/dvdqueue.c b/src/Dolphin/dvd/dvdqueue.c
new file mode 100644
index 0000000..a381ccf
--- /dev/null
+++ b/src/Dolphin/dvd/dvdqueue.c
@@ -0,0 +1,142 @@
+#include "dolphin/DVDPriv.h"
+
+#define MAX_QUEUES 4
+typedef struct {
+  DVDCommandBlock* next;
+  DVDCommandBlock* prev;
+} DVDQueue;
+
+static DVDQueue WaitingQueue[MAX_QUEUES];
+
+void __DVDClearWaitingQueue(void) {
+  u32 i;
+
+  for (i = 0; i < MAX_QUEUES; i++) {
+    DVDCommandBlock* q;
+
+    q = (DVDCommandBlock*)&(WaitingQueue[i]);
+    q->next = q;
+    q->prev = q;
+  }
+}
+
+BOOL __DVDPushWaitingQueue(s32 prio, DVDCommandBlock* block) {
+  BOOL enabled;
+  DVDCommandBlock* q;
+
+  enabled = OSDisableInterrupts();
+
+  q = (DVDCommandBlock*)&(WaitingQueue[prio]);
+
+  q->prev->next = block;
+  block->prev = q->prev;
+  block->next = q;
+  q->prev = block;
+
+  OSRestoreInterrupts(enabled);
+
+  return TRUE;
+}
+
+static DVDCommandBlock* PopWaitingQueuePrio(s32 prio) {
+  DVDCommandBlock* tmp;
+  BOOL enabled;
+  DVDCommandBlock* q;
+
+  enabled = OSDisableInterrupts();
+
+  q = (DVDCommandBlock*)&(WaitingQueue[prio]);
+
+  tmp = q->next;
+  q->next = tmp->next;
+  tmp->next->prev = q;
+
+  OSRestoreInterrupts(enabled);
+
+  tmp->next = (DVDCommandBlock*)NULL;
+  tmp->prev = (DVDCommandBlock*)NULL;
+
+  return tmp;
+}
+
+DVDCommandBlock* __DVDPopWaitingQueue(void) {
+  u32 i;
+  BOOL enabled;
+  DVDCommandBlock* q;
+
+  enabled = OSDisableInterrupts();
+
+  for (i = 0; i < MAX_QUEUES; i++) {
+    q = (DVDCommandBlock*)&(WaitingQueue[i]);
+    if (q->next != q) {
+      OSRestoreInterrupts(enabled);
+      return PopWaitingQueuePrio((s32)i);
+    }
+  }
+
+  OSRestoreInterrupts(enabled);
+
+  return (DVDCommandBlock*)NULL;
+}
+
+BOOL __DVDCheckWaitingQueue(void) {
+  u32 i;
+  BOOL enabled;
+  DVDCommandBlock* q;
+
+  enabled = OSDisableInterrupts();
+
+  for (i = 0; i < MAX_QUEUES; i++) {
+    q = (DVDCommandBlock*)&(WaitingQueue[i]);
+    if (q->next != q) {
+      OSRestoreInterrupts(enabled);
+      return TRUE;
+    }
+  }
+
+  OSRestoreInterrupts(enabled);
+
+  return FALSE;
+}
+
+BOOL __DVDDequeueWaitingQueue(DVDCommandBlock* block) {
+  BOOL enabled;
+  DVDCommandBlock* prev;
+  DVDCommandBlock* next;
+
+  enabled = OSDisableInterrupts();
+
+  prev = block->prev;
+  next = block->next;
+
+  if ((prev == (DVDCommandBlock*)NULL) || (next == (DVDCommandBlock*)NULL)) {
+    OSRestoreInterrupts(enabled);
+    return FALSE;
+  }
+
+  prev->next = next;
+  next->prev = prev;
+
+  OSRestoreInterrupts(enabled);
+
+  return TRUE;
+}
+
+BOOL __DVDIsBlockInWaitingQueue(DVDCommandBlock* block) {
+  u32 i;
+  DVDCommandBlock* start;
+  DVDCommandBlock* q;
+
+  for (i = 0; i < MAX_QUEUES; i++) {
+    start = (DVDCommandBlock*)&(WaitingQueue[i]);
+
+    if (start->next != start) {
+      for (q = start->next; q != start; q = q->next) {
+        if (q == block)
+          return TRUE;
+      }
+    }
+  }
+
+  return FALSE;
+}