1 files changed, 389 insertions, 0 deletions
diff --git a/src/Dolphin/dsp/dsp_task.c b/src/Dolphin/dsp/dsp_task.c
new file mode 100644
index 0000000..2fe7f15
--- /dev/null
+++ b/src/Dolphin/dsp/dsp_task.c
@@ -0,0 +1,389 @@
+#include "dolphin/dsp.h"
+#include "dolphin/dsp_regs.h"
+
+DSPTaskInfo* __DSP_curr_task;
+DSPTaskInfo* __DSP_first_task;
+DSPTaskInfo* __DSP_last_task;
+DSPTaskInfo* __DSP_tmp_task;
+DSPTaskInfo* __DSP_rude_task;
+
+BOOL __DSP_rude_task_pending;
+
+void __DSPHandler(__OSInterrupt, OSContext* context) {
+  DSPTaskInfo* tmp_task;
+  OSContext exceptionContext;
+  u16 tmp;
+  u32 mail;
+
+  tmp = __DSPRegs[5];
+  tmp = (u16)(tmp & ~0x28) | 0x80;
+  __DSPRegs[5] = tmp;
+
+  OSClearContext(&exceptionContext);
+  OSSetCurrentContext(&exceptionContext);
+
+  while (!DSPCheckMailFromDSP())
+    ;
+  mail = DSPReadMailFromDSP();
+
+  if ((__DSP_curr_task->flags & DSP_TASK_FLAG_CANCEL) && (mail == 0xDCD10002)) {
+    mail = 0xDCD10003;
+  }
+
+  switch (mail) {
+  case 0xDCD10000:
+    __DSP_curr_task->state = DSP_TASK_STATE_RUN;
+
+    if (__DSP_curr_task->init_cb) {
+      (*(__DSP_curr_task->init_cb))((void*)(__DSP_curr_task));
+    }
+    break;
+  case 0xDCD10001:
+    __DSP_curr_task->state = DSP_TASK_STATE_RUN;
+    if (__DSP_curr_task->res_cb) {
+      (*(__DSP_curr_task->res_cb))((void*)(__DSP_curr_task));
+    }
+    break;
+  case 0xDCD10002:
+    if (__DSP_rude_task_pending) {
+
+      if (__DSP_curr_task == __DSP_rude_task) {
+        DSPSendMailToDSP(0xCDD10003);
+        while (DSPCheckMailToDSP()) {
+        }
+
+        __DSP_rude_task = NULL;
+        __DSP_rude_task_pending = FALSE;
+
+        if (__DSP_curr_task->res_cb) {
+          (*(__DSP_curr_task->res_cb))((void*)(__DSP_curr_task));
+        }
+
+        break;
+      } else {
+        DSPSendMailToDSP(0xCDD10001);
+        while (DSPCheckMailToDSP())
+          ;
+        __DSP_exec_task(__DSP_curr_task, __DSP_rude_task);
+
+        __DSP_curr_task->state = DSP_TASK_STATE_YIELD;
+        __DSP_curr_task = __DSP_rude_task;
+
+        __DSP_rude_task = NULL;
+        __DSP_rude_task_pending = FALSE;
+
+        break;
+      }
+    }
+
+    if (__DSP_curr_task->next == NULL) {
+
+      if (__DSP_curr_task == __DSP_first_task) {
+
+        DSPSendMailToDSP(0xCDD10003);
+        while (DSPCheckMailToDSP())
+          ;
+
+        if (__DSP_curr_task->res_cb) {
+          (*(__DSP_curr_task->res_cb))((void*)(__DSP_curr_task));
+        }
+
+      } else {
+        DSPSendMailToDSP(0xCDD10001);
+        while (DSPCheckMailToDSP()) {
+        }
+
+        __DSP_exec_task(__DSP_curr_task, __DSP_first_task);
+
+        __DSP_curr_task->state = DSP_TASK_STATE_YIELD;
+        __DSP_curr_task = __DSP_first_task;
+      }
+
+    } else {
+
+      DSPSendMailToDSP(0xCDD10001);
+      while (DSPCheckMailToDSP()) {
+      }
+
+      __DSP_exec_task(__DSP_curr_task, __DSP_curr_task->next);
+
+      __DSP_curr_task->state = DSP_TASK_STATE_YIELD;
+      __DSP_curr_task = __DSP_curr_task->next;
+    }
+    break;
+  case 0xDCD10003:
+    if (__DSP_rude_task_pending) {
+
+      if (__DSP_curr_task->done_cb) {
+        (*(__DSP_curr_task->done_cb))((void*)(__DSP_curr_task));
+      }
+
+      DSPSendMailToDSP(0xCDD10001);
+      while (DSPCheckMailToDSP())
+        ;
+
+      __DSP_exec_task(NULL, __DSP_rude_task);
+
+      __DSP_remove_task(__DSP_curr_task);
+      __DSP_curr_task = __DSP_rude_task;
+
+      __DSP_rude_task = NULL;
+      __DSP_rude_task_pending = FALSE;
+
+      break;
+    }
+
+    if (__DSP_curr_task->next == NULL) {
+
+      if (__DSP_curr_task == __DSP_first_task) {
+
+        if (__DSP_curr_task->done_cb) {
+          (*(__DSP_curr_task->done_cb))((void*)(__DSP_curr_task));
+        }
+
+        DSPSendMailToDSP(0xCDD10002);
+        while (DSPCheckMailToDSP())
+          ;
+
+        __DSP_curr_task->state = DSP_TASK_STATE_DONE;
+
+        __DSP_remove_task(__DSP_curr_task);
+
+      } else {
+
+        if (__DSP_curr_task->done_cb) {
+          (*(__DSP_curr_task->done_cb))((void*)(__DSP_curr_task));
+        }
+
+        DSPSendMailToDSP(0xCDD10001);
+        while (DSPCheckMailToDSP())
+          ;
+
+        __DSP_curr_task->state = DSP_TASK_STATE_DONE;
+        __DSP_exec_task(NULL, __DSP_first_task);
+
+        __DSP_curr_task = __DSP_first_task;
+        __DSP_remove_task(__DSP_last_task);
+      }
+
+    } else {
+      if (__DSP_curr_task->done_cb) {
+        (*(__DSP_curr_task->done_cb))((void*)(__DSP_curr_task));
+      }
+      DSPSendMailToDSP(0xCDD10001);
+      while (DSPCheckMailToDSP())
+        ;
+
+      __DSP_curr_task->state = DSP_TASK_STATE_DONE;
+      __DSP_exec_task(NULL, __DSP_curr_task->next);
+
+      __DSP_curr_task = __DSP_curr_task->next;
+      __DSP_remove_task(__DSP_curr_task->prev);
+    }
+    break;
+
+  case 0xDCD10004:
+
+    if (__DSP_curr_task->req_cb) {
+      (*(__DSP_curr_task->req_cb))((void*)(__DSP_curr_task));
+    }
+    break;
+  default:
+    break;
+  }
+  OSClearContext(&exceptionContext);
+  OSSetCurrentContext(context);
+}
+
+void __DSP_exec_task(DSPTaskInfo* curr, DSPTaskInfo* next) {
+  if (curr) {
+    DSPSendMailToDSP((u32)(curr->dram_mmem_addr));
+    while (DSPCheckMailToDSP())
+      ;
+    DSPSendMailToDSP((u32)(curr->dram_length));
+    while (DSPCheckMailToDSP())
+      ;
+    DSPSendMailToDSP((u32)(curr->dram_addr));
+    while (DSPCheckMailToDSP())
+      ;
+  } else {
+
+    DSPSendMailToDSP((u32)(0));
+    while (DSPCheckMailToDSP())
+      ;
+    DSPSendMailToDSP((u32)(0));
+    while (DSPCheckMailToDSP())
+      ;
+    DSPSendMailToDSP((u32)(0));
+    while (DSPCheckMailToDSP())
+      ;
+  }
+
+  DSPSendMailToDSP((u32)(next->iram_mmem_addr));
+  while (DSPCheckMailToDSP())
+    ;
+  DSPSendMailToDSP((u32)(next->iram_length));
+  while (DSPCheckMailToDSP())
+    ;
+  DSPSendMailToDSP((u32)(next->iram_addr));
+  while (DSPCheckMailToDSP())
+    ;
+
+  if (DSP_TASK_STATE_INIT == next->state) {
+    DSPSendMailToDSP((u32)(next->dsp_init_vector));
+    while (DSPCheckMailToDSP())
+      ;
+    DSPSendMailToDSP((u32)(0));
+    while (DSPCheckMailToDSP())
+      ;
+    DSPSendMailToDSP((u32)(0));
+    while (DSPCheckMailToDSP())
+      ;
+    DSPSendMailToDSP((u32)(0));
+    while (DSPCheckMailToDSP())
+      ;
+  } else {
+    DSPSendMailToDSP((u32)(next->dsp_resume_vector));
+    while (DSPCheckMailToDSP())
+      ;
+    DSPSendMailToDSP((u32)(next->dram_mmem_addr));
+    while (DSPCheckMailToDSP())
+      ;
+
+    DSPSendMailToDSP((u32)(next->dram_length));
+    while (DSPCheckMailToDSP())
+      ;
+
+    DSPSendMailToDSP((u32)(next->dram_addr));
+    while (DSPCheckMailToDSP())
+      ;
+  }
+}
+
+
+#define MSG_BASE 0x80F30000
+void __DSP_boot_task(DSPTaskInfo* task) {
+
+  volatile u32 mail;
+
+  while (!DSPCheckMailFromDSP())
+    ;
+
+  mail = DSPReadMailFromDSP();
+
+  DSPSendMailToDSP(MSG_BASE | 0xA001);
+  while (DSPCheckMailToDSP()) {
+  }
+  DSPSendMailToDSP((u32)(task->iram_mmem_addr));
+  while (DSPCheckMailToDSP()) {
+  }
+
+  DSPSendMailToDSP(MSG_BASE | 0xC002);
+  while (DSPCheckMailToDSP()) {
+  }
+  DSPSendMailToDSP((u32)(task->iram_addr & 0xffff));
+  while (DSPCheckMailToDSP()) {
+  }
+
+  DSPSendMailToDSP(MSG_BASE | 0xA002);
+  while (DSPCheckMailToDSP()) {
+  }
+  DSPSendMailToDSP(task->iram_length);
+  while (DSPCheckMailToDSP()) {
+  }
+
+  DSPSendMailToDSP(MSG_BASE | 0xB002);
+  while (DSPCheckMailToDSP()) {
+  }
+  DSPSendMailToDSP(0x00000000);
+  while (DSPCheckMailToDSP()) {
+  }
+
+  DSPSendMailToDSP(MSG_BASE | 0xD001);
+  while (DSPCheckMailToDSP()) {
+  }
+  DSPSendMailToDSP((u32)(0xffff & task->dsp_init_vector));
+  while (DSPCheckMailToDSP()) {
+  }
+
+  __DSP_debug_printf("DSP is booting task: 0x%08X\n", task);
+  __DSP_debug_printf("__DSP_boot_task()  : IRAM MMEM ADDR: 0x%08X\n", (u32)(task->iram_mmem_addr));
+  __DSP_debug_printf("__DSP_boot_task()  : IRAM DSP ADDR : 0x%08X\n", (u32)(task->iram_addr));
+  __DSP_debug_printf("__DSP_boot_task()  : IRAM LENGTH   : 0x%08X\n", (u32)(task->iram_length));
+  __DSP_debug_printf("__DSP_boot_task()  : DRAM MMEM ADDR: 0x%08X\n", (u32)(task->dram_length));
+  __DSP_debug_printf("__DSP_boot_task()  : Start Vector  : 0x%08X\n", (u32)(task->dsp_init_vector));
+}
+
+void __DSP_insert_task(DSPTaskInfo* task) {
+
+  DSPTaskInfo* temp;
+
+  if (__DSP_first_task == NULL) {
+    __DSP_first_task = __DSP_last_task = __DSP_curr_task = task;
+    task->next = task->prev = NULL;
+  } else {
+    temp = __DSP_first_task;
+
+    while (temp) {
+      if (task->priority < temp->priority) {
+        task->prev = temp->prev;
+        temp->prev = task;
+        task->next = temp;
+        if (task->prev == NULL) {
+          __DSP_first_task = task;
+        } else {
+          (task->prev)->next = task;
+        }
+        break;
+      }
+      temp = temp->next;
+    }
+
+    if (temp == NULL) {
+      __DSP_last_task->next = task;
+      task->next = NULL;
+      task->prev = __DSP_last_task;
+      __DSP_last_task = task;
+    }
+  }
+}
+
+void __DSP_add_task(DSPTaskInfo* task) {
+  if (__DSP_last_task == NULL) {
+    __DSP_first_task = __DSP_last_task = __DSP_curr_task = task;
+    task->next = task->prev = NULL;
+  } else {
+    __DSP_last_task->next = task;
+    task->next = NULL;
+    task->prev = __DSP_last_task;
+    __DSP_last_task = task;
+  }
+
+  task->state = DSP_TASK_STATE_INIT;
+
+  __DSP_debug_printf("__DSP_add_task() : Added task    : 0x%08X\n", task);
+}
+
+void __DSP_remove_task(DSPTaskInfo* task) {
+
+  task->flags = DSP_TASK_FLAG_CLEARALL;
+  task->state = DSP_TASK_STATE_DONE;
+
+  if (__DSP_first_task == task) {
+    if (task->next) {
+      __DSP_first_task = (task->next);
+      task->next->prev = NULL;
+    } else {
+      __DSP_first_task = __DSP_last_task = __DSP_curr_task = NULL;
+    }
+  } else if (__DSP_last_task == task) {
+    __DSP_last_task = (task->prev);
+    task->prev->next = NULL;
+    __DSP_curr_task = __DSP_first_task;
+
+  } else {
+    __DSP_curr_task = task->next;
+    task->prev->next = task->next;
+    task->next->prev = task->prev;
+  }
+}