diff --git a/include/functions.h b/include/functions.h index a2a42e6d3..ab8384a06 100644 --- a/include/functions.h +++ b/include/functions.h @@ -351,7 +351,7 @@ void _Putfld(_Pft* px, va_list* pap, u8 code, u8* ac); s32 osVoiceClearDictionary(OSVoiceHandle* hd, u8 words); void osUnmapTLBAll(void); s32 osEPiStartDma(OSPiHandle* pihandle, OSIoMesg* mb, s32 direction); -s32 __osVoiceContRead2(OSMesgQueue* mq, s32 port, s32, u8 dst[2]); +s32 __osVoiceContRead2(OSMesgQueue* mq, s32 port, u16, u8 dst[2]); u8 __osVoiceContDataCrc(u8* data, u32 arg1); const char* strchr(const char* __s, s32 __c); size_t strlen(const char* __s); diff --git a/src/libultra/voice/voicecontread2.c b/src/libultra/voice/voicecontread2.c index 38350ee5f..8e70d4dee 100644 --- a/src/libultra/voice/voicecontread2.c +++ b/src/libultra/voice/voicecontread2.c @@ -1,3 +1,69 @@ #include "global.h" -#pragma GLOBAL_ASM("asm/non_matchings/boot/voicecontread2/__osVoiceContRead2.s") +extern s32 D_80097FB0; + +s32 __osVoiceContRead2(OSMesgQueue* mq, s32 port, u16 arg2, u8* dst) { + u8* ptr; + u8 sp63; + s32 errorCode; + s32 var = 2; + s32 i; + + __osSiGetAccess(); + + do { + + ptr = (u8*)&__osPfsPifRam; + + if ((__osContLastCmd != 0xB) || (port != D_80097FB0)) { + __osContLastCmd = 0xB; + D_80097FB0 = port; + + for (i = 0; i < port; i++, *ptr++ = 0) { + ; + } + + __osPfsPifRam.pifstatus = 1; + + ptr[0] = 0xFF; + ptr[1] = 3; + ptr[2] = 3; + ptr[3] = 0xB; + ptr[8] = 0xFF; + ptr[9] = 0xFE; + } else { + ptr = (u8*)&__osPfsPifRam + port; + } + + ptr[4] = arg2 >> 3; + ptr[5] = __osContAddressCrc(arg2) | (arg2 << 5); + + __osSiRawStartDma(OS_WRITE, &__osPfsPifRam); + 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[8] != __osVoiceContDataCrc(&ptr[6], 2)) { + errorCode = __osVoiceGetStatus(mq, port, &sp63); + if (errorCode != 0) { + break; + } + + errorCode = CONT_ERR_CONTRFAIL; + } else { + bcopy(&ptr[6], dst, 2); + } + } else { + errorCode = CONT_ERR_NO_CONTROLLER; + } + + } while ((errorCode == CONT_ERR_CONTRFAIL) && (var-- >= 0)); + + __osSiRelAccess(); + + return errorCode; +} +