1 files changed, 88 insertions, 0 deletions
diff --git a/src/libultra_code_O2/osContInit.c b/src/libultra_code_O2/osContInit.c
new file mode 100644
index 000000000..8f10e854d
--- /dev/null
+++ b/src/libultra_code_O2/osContInit.c
@@ -0,0 +1,88 @@
+#include "global.h"
+
+OSPifRam __osPifInternalBuff;
+u8 __osContLastPoll;
+u8 __osMaxControllers; // always 4
+
+OSTimer __osEepromTimer;
+OSMesgQueue __osEepromTimerMsgQ;
+OSMesg __osEepromTimerMsg;
+
+u32 gOSContInitialized = 0;
+
+#define HALF_SECOND OS_USEC_TO_CYCLES(500000)
+s32 osContInit(OSMesgQueue* mq, u8* ctlBitfield, OSContStatus* status) {
+    OSMesg mesg;
+    s32 ret = 0;
+    OSTime currentTime;
+    OSTimer timer;
+    OSMesgQueue timerqueue;
+
+    if (gOSContInitialized) {
+        return 0;
+    }
+    gOSContInitialized = 1;
+    currentTime = osGetTime();
+    if (HALF_SECOND > currentTime) {
+        osCreateMesgQueue(&timerqueue, &mesg, 1);
+        osSetTimer(&timer, HALF_SECOND - currentTime, 0, &timerqueue, &mesg);
+        osRecvMesg(&timerqueue, &mesg, OS_MESG_BLOCK);
+    }
+    __osMaxControllers = MAXCONTROLLERS;
+    __osPackRequestData(CONT_CMD_REQUEST_STATUS);
+    ret = __osSiRawStartDma(OS_WRITE, &__osPifInternalBuff);
+    osRecvMesg(mq, &mesg, OS_MESG_BLOCK);
+    ret = __osSiRawStartDma(OS_READ, &__osPifInternalBuff);
+    osRecvMesg(mq, &mesg, OS_MESG_BLOCK);
+    __osContGetInitData(ctlBitfield, status);
+    __osContLastPoll = CONT_CMD_REQUEST_STATUS;
+    __osSiCreateAccessQueue();
+    osCreateMesgQueue(&__osEepromTimerMsgQ, &__osEepromTimerMsg, 1);
+    return ret;
+}
+
+void __osContGetInitData(u8* ctlBitfield, OSContStatus* status) {
+    u8* bufptr;
+    __OSContRequestHeader req;
+    s32 i;
+    u8 bitfieldTemp = 0;
+    bufptr = (u8*)(&__osPifInternalBuff);
+    for (i = 0; i < __osMaxControllers; i++, bufptr += sizeof(req), status++) {
+        req = *((__OSContRequestHeader*)bufptr);
+        status->errno = (req.rxsize & 0xC0) >> 4;
+        if (status->errno) {
+            continue;
+        }
+        status->type = req.typel << 8 | req.typeh;
+        status->status = req.status;
+        bitfieldTemp |= 1 << i;
+    }
+    *ctlBitfield = bitfieldTemp;
+}
+
+void __osPackRequestData(u8 poll) {
+    u8* bufptr;
+    __OSContRequestHeader req;
+    s32 i;
+    for (i = 0; i < 0xF; i++) {
+        __osPifInternalBuff.ram[i] = 0;
+    }
+    __osPifInternalBuff.status = 1;
+
+    bufptr = &__osPifInternalBuff;
+
+    req.align = 0xFF;
+    req.txsize = 1;
+    req.rxsize = 3;
+    req.poll = poll;
+    req.typeh = 0xFF;
+    req.typel = 0xFF;
+    req.status = 0xFF;
+    req.align1 = 0xFF;
+
+    for (i = 0; i < __osMaxControllers; i++) {
+        *((__OSContRequestHeader*)bufptr) = req;
+        bufptr += sizeof(req);
+    }
+    *((u8*)bufptr) = 254;
+}