From 4841a4a0bf7c2ab92493d64c22442c4e584a7b99 Mon Sep 17 00:00:00 2001
From: kyleburnette <kyle@kyleburnette.com>
Date: Tue, 2 Nov 2021 14:35:57 -0700
Subject: [PATCH] padsetup OK (#383)

* padsetup OK

* Made tharo's suggestions

* Made arg name consistent
---
 include/functions.h        |  2 +-
 src/boot_O2/padsetup.c     | 33 ++++++++++++++++++++++++++++++++-
 tools/disasm/functions.txt |  2 +-
 3 files changed, 34 insertions(+), 3 deletions(-)

diff --git a/include/functions.h b/include/functions.h
index 7f71bc4cbe..fedeaf0f57 100644
--- a/include/functions.h
+++ b/include/functions.h
@@ -168,7 +168,7 @@ void func_800863AC(u32 param_1, s32 param_2, s32 param_3, UNK_PTR param_4);
 // void func_800864EC(UNK_TYPE1 param_1, UNK_TYPE1 param_2, UNK_TYPE1 param_3, UNK_TYPE1 param_4, UNK_TYPE4 param_5);
 // void func_80086588(void);
 void SystemArena_Init(u32 base, u32 size);
-s32 func_80086620(OSMesgQueue* param_1, PadMgr* param_2, OSContStatus* param_3);
+s32 PadSetup_Init(OSMesgQueue* mq, u8* outMask, OSContStatus* status);
 // void func_80086760(void);
 // void func_80086794(void);
 // void func_800867B4(void);
diff --git a/src/boot_O2/padsetup.c b/src/boot_O2/padsetup.c
index 5f83bd5246..9cef6f196f 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;
+}
diff --git a/tools/disasm/functions.txt b/tools/disasm/functions.txt
index c158bd87f4..4348bf5b58 100644
--- a/tools/disasm/functions.txt
+++ b/tools/disasm/functions.txt
@@ -165,7 +165,7 @@
     0x800864EC:("func_800864EC",),
     0x80086588:("func_80086588",),
     0x800865F8:("SystemArena_Init",),
-    0x80086620:("func_80086620",),
+    0x80086620:("PadSetup_Init",),
     0x80086760:("func_80086760",),
     0x80086794:("func_80086794",),
     0x800867B4:("func_800867B4",),