1 files changed, 54 insertions, 0 deletions
diff --git a/src/libultra_code_O2/osContStartReadData.c b/src/libultra_code_O2/osContStartReadData.c
new file mode 100644
index 000000000..59ead072c
--- /dev/null
+++ b/src/libultra_code_O2/osContStartReadData.c
@@ -0,0 +1,54 @@
+#include "global.h"
+
+s32 osContStartReadData(OSMesgQueue* mq) {
+    s32 ret;
+    __osSiGetAccess();
+    if (__osContLastPoll != 1) {
+        __osPackReadData();
+        __osSiRawStartDma(OS_WRITE, &__osPifInternalBuff);
+        osRecvMesg(mq, NULL, OS_MESG_BLOCK);
+    }
+    ret = __osSiRawStartDma(OS_READ, &__osPifInternalBuff);
+    __osContLastPoll = CONT_CMD_READ_BUTTON;
+    __osSiRelAccess();
+    return ret;
+}
+
+void osContGetReadData(OSContPad* contData) {
+    u8* bufptr;
+    __OSContReadHeader read;
+    s32 i;
+    bufptr = (u8*)(&__osPifInternalBuff);
+    for (i = 0; i < __osMaxControllers; i++, bufptr += sizeof(read), contData++) {
+        read = *((__OSContReadHeader*)bufptr);
+        contData->errno = (read.rxsize & 0xC0) >> 4;
+        if (contData->errno == 0) {
+            contData->button = read.button;
+            contData->stick_x = read.joyX;
+            contData->stick_y = read.joyY;
+        }
+    };
+}
+
+void __osPackReadData() {
+    u8* bufptr;
+    __OSContReadHeader read;
+    s32 i;
+    bufptr = (u8*)(&__osPifInternalBuff);
+    for (i = 0; i < 0xF; i++) {
+        __osPifInternalBuff.ram[i] = 0;
+    }
+    __osPifInternalBuff.status = 1;
+    read.align = 0xFF;
+    read.txsize = 1;
+    read.rxsize = 4;
+    read.poll = CONT_CMD_READ_BUTTON;
+    read.button = 0xFFFF;
+    read.joyX = 0xFF;
+    read.joyY = 0xFF;
+    for (i = 0; i < __osMaxControllers; i++) {
+        *((__OSContReadHeader*)bufptr) = read;
+        bufptr += sizeof(read);
+    }
+    *((u8*)bufptr) = CONT_CMD_END;
+}