From 83453f6cd31172185714e7cd5b2f09b3a81d5f6f Mon Sep 17 00:00:00 2001
From: engineer124 <engineer124engineer124@gmail.com>
Date: Wed, 10 Nov 2021 00:21:29 +1100
Subject: [PATCH] Match __osVoiceContWrite4, Write is very similar to Read

---
 include/functions.h                  |  2 +-
 src/libultra/voice/voicecontread2.c  |  4 +-
 src/libultra/voice/voicecontread36.c |  4 +-
 src/libultra/voice/voicecontwrite4.c | 67 +++++++++++++++++++++++++++-
 4 files changed, 71 insertions(+), 6 deletions(-)

diff --git a/include/functions.h b/include/functions.h
index 0043a3544..87e3ba2be 100644
--- a/include/functions.h
+++ b/include/functions.h
@@ -470,7 +470,7 @@ s32 osVoiceControlGain(OSVoiceHandle* hd, s32 analog, s32 digital);
 s32 osVoiceStartReadData(OSVoiceHandle* hd);
 u32* osViGetCurrentFramebuffer(void);
 s32 __osSpSetPc(void* pc);
-s32 __osVoiceContWrite4(OSMesgQueue* hd, s32 arg1, s32 arg2, OSVoiceData* voiceData);
+s32 __osVoiceContWrite4(OSMesgQueue* mq, s32 port, u16 arg2, u8* dst[4]);
 void __osGetHWIntrRoutine(s32 idx, OSMesgQueue** outQueue, OSMesg* outMsg);
 void __osSetHWIntrRoutine(s32 idx, OSMesgQueue* queue, OSMesg msg);
 u32 __osGetWatchLo(void);
diff --git a/src/libultra/voice/voicecontread2.c b/src/libultra/voice/voicecontread2.c
index b3a3a7fbc..ce52e2be0 100644
--- a/src/libultra/voice/voicecontread2.c
+++ b/src/libultra/voice/voicecontread2.c
@@ -1,9 +1,9 @@
 #include "global.h"
 
 s32 __osVoiceContRead2(OSMesgQueue* mq, s32 port, u16 arg2, u8 dst[2]) {
-    u8* ptr;
-    u8 status;
     s32 errorCode;
+    u8 status;
+    u8* ptr;
     s32 retryCount = 2;
     s32 i;
 
diff --git a/src/libultra/voice/voicecontread36.c b/src/libultra/voice/voicecontread36.c
index a3b330428..66067d239 100644
--- a/src/libultra/voice/voicecontread36.c
+++ b/src/libultra/voice/voicecontread36.c
@@ -1,9 +1,9 @@
 #include "global.h"
 
 s32 __osVoiceContRead36(OSMesgQueue* mq, s32 port, u16 arg2, u8 dst[36]) {
-    u8* ptr;
-    u8 status;
     s32 errorCode;
+    u8 status;
+    u8* ptr;
     s32 retryCount = 2;
     s32 i;
 
diff --git a/src/libultra/voice/voicecontwrite4.c b/src/libultra/voice/voicecontwrite4.c
index c28e2d762..09158c043 100644
--- a/src/libultra/voice/voicecontwrite4.c
+++ b/src/libultra/voice/voicecontwrite4.c
@@ -1,3 +1,68 @@
 #include "global.h"
 
-#pragma GLOBAL_ASM("asm/non_matchings/boot/voicecontwrite4/__osVoiceContWrite4.s")
+s32 __osVoiceContWrite4(OSMesgQueue* mq, s32 port, u16 arg2, u8* dst[4]) {
+    s32 errorCode;
+    u8 status;
+    u8* ptr;
+    s32 retryCount = 2;
+    s32 i;
+    u8 temp_s2;
+
+    __osSiGetAccess();
+
+    do {
+
+        ptr = (u8*)&__osPfsPifRam;
+
+        if ((__osContLastCmd != 0xC) || (port != D_80097FB0)) {
+            __osContLastCmd = 0xC;
+            D_80097FB0 = port;
+
+            for (i = 0; i < port; i++, *ptr++ = 0) {
+                ;
+            }
+
+            __osPfsPifRam.pifstatus = CONT_CMD_READ_BUTTON;
+
+            ptr[0] = 0xFF;
+            ptr[1] = 7;
+            ptr[2] = 1;
+            ptr[3] = 0xC;
+            ptr[0xA] = 0xFF;
+            ptr[0xB] = 0xFE;
+        } else {
+            ptr = (u8*)&__osPfsPifRam + port;
+        }
+
+        ptr[4] = arg2 >> 3;
+        ptr[5] = __osContAddressCrc(arg2) | (arg2 << 5);
+
+        bcopy(dst, &ptr[6], 4);
+        
+        __osSiRawStartDma(OS_WRITE, &__osPfsPifRam);
+        temp_s2 = __osVoiceContDataCrc(dst, 4);
+        osRecvMesg(mq, NULL, OS_MESG_BLOCK);
+        __osSiRawStartDma(OS_READ, &__osPfsPifRam);
+        osRecvMesg(mq, NULL, OS_MESG_BLOCK);
+
+        errorCode = (ptr[2] & 0xC0) >> 4;
+
+        if (errorCode == 0) {
+            if (ptr[0xA] != (0, temp_s2)) {
+                errorCode = __osVoiceGetStatus(mq, port, &status);
+                if (errorCode != 0) {
+                    break;
+                }
+
+                errorCode = CONT_ERR_CONTRFAIL;
+            }
+        } else {
+            errorCode = CONT_ERR_NO_CONTROLLER;
+        }
+
+    } while ((errorCode == CONT_ERR_CONTRFAIL) && (retryCount-- >= 0));
+
+    __osSiRelAccess();
+
+    return errorCode;
+}