padsetup OK (#58)

1 files changed, 37 insertions, 0 deletions

diff --git a/src/boot/O2/padsetup.c b/src/boot/O2/padsetup.c
new file mode 100644
index 0000000..bde4570
--- /dev/null
+++ b/src/boot/O2/padsetup.c
@@ -0,0 +1,37 @@
+#include "ultra64.h"
+
+s32 PadSetup_Init(OSMesgQueue* mq, u8* outMask, OSContStatus* status) {
+    s32 ret;
+    s32 i;
+
+    *outMask = 0xFF;
+    ret = osContInit(mq, outMask, status);
+    if (ret != 0) {
+        return ret;
+    }
+
+    if (*outMask == 0xFF) {
+        if (osContStartQuery(mq) != 0) {
+            return 1;
+        }
+
+        osRecvMesg(mq, NULL, OS_MESG_BLOCK);
+        osContGetQuery(status);
+
+        *outMask = 0;
+        for (i = 0; i < MAXCONTROLLERS; i++) {
+            switch (status[i].errno) {
+                case 0:
+                    if (status[i].type == CONT_TYPE_NORMAL) {
+                        *outMask |= 1 << i;
+                    }
+                    break;
+
+                default:
+                    break;
+            }
+        }
+    }
+
+    return 0;
+}