padsetup OK (#383)

* padsetup OK

* Made tharo's suggestions

* Made arg name consistent

1 files changed, 32 insertions, 1 deletions

diff --git a/src/boot_O2/padsetup.c b/src/boot_O2/padsetup.c
index 5f83bd524..9cef6f196 100644
--- a/src/boot_O2/padsetup.c
+++ b/src/boot_O2/padsetup.c
@@ -1,3 +1,34 @@
 #include "global.h"
 
-#pragma GLOBAL_ASM("asm/non_matchings/boot/padsetup/func_80086620.s")
+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;
+}