1 files changed, 185 insertions, 0 deletions
diff --git a/arm9/lib/NitroSDK/src/PXI_fifo.c b/arm9/lib/NitroSDK/src/PXI_fifo.c
new file mode 100644
index 00000000..35db1c7f
--- /dev/null
+++ b/arm9/lib/NitroSDK/src/PXI_fifo.c
@@ -0,0 +1,185 @@
+#include "function_target.h"
+#include "consts.h"
+#include "OS_interrupt.h"
+#include "OS_system.h"
+#include "PXI_fifo.h"
+#include "systemWork.h"
+
+static u16 FifoCtrlInit = 0;
+static PXIFifoCallback FifoRecvCallbackTable[PXI_MAX_FIFO_TAG];
+
+static inline PXIFifoStatus PXIi_SetToFifo(u32 data);
+
+ARM_FUNC void PXI_InitFifo(void)
+{
+    OSSystemWork *p = OS_GetSystemWork();
+    OSIntrMode enabled = OS_DisableInterrupts();
+    s32 i;
+
+    if (!FifoCtrlInit)
+    {
+        FifoCtrlInit = TRUE;
+
+        p->pxiHandleChecker[PXI_PROC_ARM9] = 0UL;
+
+        for (i = 0; i < PXI_MAX_FIFO_TAG; i++)
+        {
+            FifoRecvCallbackTable[i] = NULL;
+        }
+
+        reg_PXI_SUBP_FIFO_CNT = (REG_PXI_SUBP_FIFO_CNT_SEND_CL_MASK
+                                 | REG_PXI_SUBP_FIFO_CNT_RECV_RI_MASK | REG_PXI_SUBP_FIFO_CNT_E_MASK | REG_PXI_SUBP_FIFO_CNT_ERR_MASK);
+
+        (void)OS_ResetRequestIrqMask(OS_IE_SPFIFO_RECV);
+        (void)OS_SetIrqFunction(OS_IE_SPFIFO_RECV, PXIi_HandlerRecvFifoNotEmpty);
+        (void)OS_EnableIrqMask(OS_IE_SPFIFO_RECV);
+
+        {
+            s32 timeout;
+            s32 c;
+
+            for (i = 0; ; i++)
+            {
+                c = reg_PXI_SUBPINTF & 15;
+                reg_PXI_SUBPINTF = (u16)(c << 8);
+
+                if (c == 0 && i > 4)
+                {
+                    break;
+                }
+
+                for (timeout = 1000; (reg_PXI_SUBPINTF & 15) == c; timeout--)
+                {
+                    if (timeout == 0)
+                    {
+                        i = 0;
+                        break;
+                    }
+                }
+            }
+        }
+    }
+    (void)OS_RestoreInterrupts(enabled);
+}
+
+ARM_FUNC void PXI_SetFifoRecvCallback(s32 fifotag, PXIFifoCallback callback)
+{
+    OSSystemWork *p = OS_GetSystemWork();
+    OSIntrMode enabled = OS_DisableInterrupts();
+
+    FifoRecvCallbackTable[fifotag] = callback;
+    if (callback)
+    {
+        p->pxiHandleChecker[PXI_PROC_ARM9] |= (1UL << fifotag);
+    }
+    else
+    {
+        p->pxiHandleChecker[PXI_PROC_ARM9] &= ~(1UL << fifotag);
+    }
+    (void)OS_RestoreInterrupts(enabled);
+}
+
+ARM_FUNC BOOL PXI_IsCallbackReady(s32 fifotag, PXIProc proc)
+{
+    OSSystemWork *p = OS_GetSystemWork();
+
+    return (p->pxiHandleChecker[proc] & (1UL << fifotag)) ? TRUE : FALSE;
+}
+
+ARM_FUNC s32 PXI_SendWordByFifo(s32 fifotag, u32 data, BOOL err)
+{
+    PXIFifoMessage fifomsg;
+
+    fifomsg.e.tag = (PXIFifoTag)fifotag;
+    fifomsg.e.err = (u32)err;
+    fifomsg.e.data = data;
+
+    return PXIi_SetToFifo(fifomsg.raw);
+}
+
+static inline PXIFifoStatus PXIi_SetToFifo(u32 data)
+{
+    if (reg_PXI_SUBP_FIFO_CNT & REG_PXI_SUBP_FIFO_CNT_ERR_MASK)
+    {
+        reg_PXI_SUBP_FIFO_CNT |= (REG_PXI_SUBP_FIFO_CNT_E_MASK | REG_PXI_SUBP_FIFO_CNT_ERR_MASK);
+        return PXI_FIFO_FAIL_SEND_ERR;
+    }
+
+    OSIntrMode enabled = OS_DisableInterrupts();
+    if (reg_PXI_SUBP_FIFO_CNT & REG_PXI_SUBP_FIFO_CNT_SEND_FULL_MASK)
+    {
+        (void)OS_RestoreInterrupts(enabled);
+        return PXI_FIFO_FAIL_SEND_FULL;
+    }
+
+    reg_PXI_SEND_FIFO = data;
+    (void)OS_RestoreInterrupts(enabled);
+    return PXI_FIFO_SUCCESS;
+}
+
+static inline PXIFifoStatus PXIi_GetFromFifo(u32 *data_buf)
+{
+    if (reg_PXI_SUBP_FIFO_CNT & REG_PXI_SUBP_FIFO_CNT_ERR_MASK)
+    {
+        reg_PXI_SUBP_FIFO_CNT |= (REG_PXI_SUBP_FIFO_CNT_E_MASK | REG_PXI_SUBP_FIFO_CNT_ERR_MASK);
+        return PXI_FIFO_FAIL_RECV_ERR;
+    }
+
+    OSIntrMode enabled = OS_DisableInterrupts();
+    if (reg_PXI_SUBP_FIFO_CNT & REG_PXI_SUBP_FIFO_CNT_RECV_EMP_MASK)
+    {
+        (void)OS_RestoreInterrupts(enabled);
+        return PXI_FIFO_FAIL_RECV_EMPTY;
+    }
+
+
+    *data_buf = reg_PXI_RECV_FIFO;
+    (void)OS_RestoreInterrupts(enabled);
+
+    return PXI_FIFO_SUCCESS;
+}
+
+ARM_FUNC void PXIi_HandlerRecvFifoNotEmpty(void)
+{
+    PXIFifoMessage fifomsg;
+    PXIFifoStatus ret_code;
+    PXIFifoTag tag;
+
+    while (TRUE)
+    {
+        ret_code = PXIi_GetFromFifo(&fifomsg.raw);
+
+        if (ret_code == PXI_FIFO_FAIL_RECV_EMPTY)
+        {
+            break;
+        }
+        if (ret_code == PXI_FIFO_FAIL_RECV_ERR)
+        {
+            continue;
+        }
+
+        tag = (PXIFifoTag)fifomsg.e.tag;
+
+        if (tag)
+        {
+            if (FifoRecvCallbackTable[tag])
+            {
+                (FifoRecvCallbackTable[tag])(tag, fifomsg.e.data, (BOOL)fifomsg.e.err);
+            }
+            else
+            {
+                if (fifomsg.e.err)
+                {
+                }
+                else
+                {
+                    fifomsg.e.err = TRUE;
+                    (void)PXIi_SetToFifo(fifomsg.raw);
+                }
+            }
+        }
+        else
+        {
+        }
+    }
+}