diff --git a/lib/asm/non_matchings/unknown_072930/_MakeMotorData.s b/lib/asm/non_matchings/osMotor/_MakeMotorData.s similarity index 100% rename from lib/asm/non_matchings/unknown_072930/_MakeMotorData.s rename to lib/asm/non_matchings/osMotor/_MakeMotorData.s diff --git a/lib/asm/non_matchings/unknown_072930/osMotorInit.s b/lib/asm/non_matchings/osMotor/osMotorInit.s similarity index 78% rename from lib/asm/non_matchings/unknown_072930/osMotorInit.s rename to lib/asm/non_matchings/osMotor/osMotorInit.s index 0508ae1a..1c86ff94 100644 --- a/lib/asm/non_matchings/unknown_072930/osMotorInit.s +++ b/lib/asm/non_matchings/osMotor/osMotorInit.s @@ -49,15 +49,15 @@ glabel osMotorInit .L8007218C: /* 072D8C 8007218C 24010080 */ li $at, 128 /* 072D90 80072190 11C10003 */ beq $t6, $at, .L800721A0 -/* 072D94 80072194 3C048012 */ lui $a0, %hi(D_80123FF0) # $a0, 0x8012 +/* 072D94 80072194 3C048012 */ lui $a0, %hi(_motorstartbuf) # $a0, 0x8012 /* 072D98 80072198 10000026 */ b .L80072234 /* 072D9C 8007219C 2402000B */ li $v0, 11 .L800721A0: -/* 072DA0 800721A0 3C038012 */ lui $v1, %hi(D_80123FD0) # $v1, 0x8012 -/* 072DA4 800721A4 3C058012 */ lui $a1, %hi(D_80123FF0) # $a1, 0x8012 -/* 072DA8 800721A8 24A53FF0 */ addiu $a1, %lo(D_80123FF0) # addiu $a1, $a1, 0x3ff0 -/* 072DAC 800721AC 24633FD0 */ addiu $v1, %lo(D_80123FD0) # addiu $v1, $v1, 0x3fd0 -/* 072DB0 800721B0 24843FF0 */ addiu $a0, %lo(D_80123FF0) # addiu $a0, $a0, 0x3ff0 +/* 072DA0 800721A0 3C038012 */ lui $v1, %hi(_motorstopbuf) # $v1, 0x8012 +/* 072DA4 800721A4 3C058012 */ lui $a1, %hi(_motorstartbuf) # $a1, 0x8012 +/* 072DA8 800721A8 24A53FF0 */ addiu $a1, %lo(_motorstartbuf) # addiu $a1, $a1, 0x3ff0 +/* 072DAC 800721AC 24633FD0 */ addiu $v1, %lo(_motorstopbuf) # addiu $v1, $v1, 0x3fd0 +/* 072DB0 800721B0 24843FF0 */ addiu $a0, %lo(_motorstartbuf) # addiu $a0, $a0, 0x3ff0 /* 072DB4 800721B4 24020001 */ li $v0, 1 .L800721B8: /* 072DB8 800721B8 24630004 */ addiu $v1, $v1, 4 @@ -71,21 +71,21 @@ glabel osMotorInit /* 072DD8 800721D8 A082FFFC */ sb $v0, -4($a0) /* 072DDC 800721DC 1465FFF6 */ bne $v1, $a1, .L800721B8 /* 072DE0 800721E0 A060FFFC */ sb $zero, -4($v1) -/* 072DE4 800721E4 3C0F8012 */ lui $t7, %hi(D_80123ED0) # $t7, 0x8012 -/* 072DE8 800721E8 25EF3ED0 */ addiu $t7, %lo(D_80123ED0) # addiu $t7, $t7, 0x3ed0 -/* 072DEC 800721EC 3C068012 */ lui $a2, %hi(D_80123FF0) # $a2, 0x8012 +/* 072DE4 800721E4 3C0F8012 */ lui $t7, %hi(_MotorStartData) # $t7, 0x8012 +/* 072DE8 800721E8 25EF3ED0 */ addiu $t7, %lo(_MotorStartData) # addiu $t7, $t7, 0x3ed0 +/* 072DEC 800721EC 3C068012 */ lui $a2, %hi(_motorstartbuf) # $a2, 0x8012 /* 072DF0 800721F0 00101180 */ sll $v0, $s0, 6 /* 072DF4 800721F4 004F3821 */ addu $a3, $v0, $t7 /* 072DF8 800721F8 AFA2002C */ sw $v0, 0x2c($sp) -/* 072DFC 800721FC 24C63FF0 */ addiu $a2, %lo(D_80123FF0) # addiu $a2, $a2, 0x3ff0 +/* 072DFC 800721FC 24C63FF0 */ addiu $a2, %lo(_motorstartbuf) # addiu $a2, $a2, 0x3ff0 /* 072E00 80072200 02002025 */ move $a0, $s0 /* 072E04 80072204 0C01C7E0 */ jal _MakeMotorData /* 072E08 80072208 24050600 */ li $a1, 1536 /* 072E0C 8007220C 8FA2002C */ lw $v0, 0x2c($sp) -/* 072E10 80072210 3C188012 */ lui $t8, %hi(D_80123DD0) # $t8, 0x8012 -/* 072E14 80072214 27183DD0 */ addiu $t8, %lo(D_80123DD0) # addiu $t8, $t8, 0x3dd0 -/* 072E18 80072218 3C068012 */ lui $a2, %hi(D_80123FD0) # $a2, 0x8012 -/* 072E1C 8007221C 24C63FD0 */ addiu $a2, %lo(D_80123FD0) # addiu $a2, $a2, 0x3fd0 +/* 072E10 80072210 3C188012 */ lui $t8, %hi(_MotorStopData) # $t8, 0x8012 +/* 072E14 80072214 27183DD0 */ addiu $t8, %lo(_MotorStopData) # addiu $t8, $t8, 0x3dd0 +/* 072E18 80072218 3C068012 */ lui $a2, %hi(_motorstopbuf) # $a2, 0x8012 +/* 072E1C 8007221C 24C63FD0 */ addiu $a2, %lo(_motorstopbuf) # addiu $a2, $a2, 0x3fd0 /* 072E20 80072220 02002025 */ move $a0, $s0 /* 072E24 80072224 24050600 */ li $a1, 1536 /* 072E28 80072228 0C01C7E0 */ jal _MakeMotorData diff --git a/lib/asm/non_matchings/unknown_072930/osMotorStart.s b/lib/asm/non_matchings/osMotor/osMotorStart.s similarity index 95% rename from lib/asm/non_matchings/unknown_072930/osMotorStart.s rename to lib/asm/non_matchings/osMotor/osMotorStart.s index 165e2e22..fc0abd6c 100644 --- a/lib/asm/non_matchings/unknown_072930/osMotorStart.s +++ b/lib/asm/non_matchings/osMotor/osMotorStart.s @@ -8,8 +8,8 @@ glabel osMotorStart /* 072A70 80071E70 3C018013 */ lui $at, %hi(__osContLastCmd) # $at, 0x8013 /* 072A74 80071E74 A02ECD90 */ sb $t6, %lo(__osContLastCmd)($at) /* 072A78 80071E78 8DF80008 */ lw $t8, 8($t7) -/* 072A7C 80071E7C 3C088012 */ lui $t0, %hi(D_80123ED0) # $t0, 0x8012 -/* 072A80 80071E80 25083ED0 */ addiu $t0, %lo(D_80123ED0) # addiu $t0, $t0, 0x3ed0 +/* 072A7C 80071E7C 3C088012 */ lui $t0, %hi(_MotorStartData) # $t0, 0x8012 +/* 072A80 80071E80 25083ED0 */ addiu $t0, %lo(_MotorStartData) # addiu $t0, $t0, 0x3ed0 /* 072A84 80071E84 0018C980 */ sll $t9, $t8, 6 /* 072A88 80071E88 03282821 */ addu $a1, $t9, $t0 /* 072A8C 80071E8C 0C0335B0 */ jal __osSiRawStartDma diff --git a/lib/asm/non_matchings/unknown_072930/osMotorStop.s b/lib/asm/non_matchings/osMotor/osMotorStop.s similarity index 95% rename from lib/asm/non_matchings/unknown_072930/osMotorStop.s rename to lib/asm/non_matchings/osMotor/osMotorStop.s index 30c01635..0875e6b9 100644 --- a/lib/asm/non_matchings/unknown_072930/osMotorStop.s +++ b/lib/asm/non_matchings/osMotor/osMotorStop.s @@ -8,8 +8,8 @@ glabel osMotorStop /* 072948 80071D48 3C018013 */ lui $at, %hi(__osContLastCmd) # $at, 0x8013 /* 07294C 80071D4C A02ECD90 */ sb $t6, %lo(__osContLastCmd)($at) /* 072950 80071D50 8DF80008 */ lw $t8, 8($t7) -/* 072954 80071D54 3C088012 */ lui $t0, %hi(D_80123DD0) # $t0, 0x8012 -/* 072958 80071D58 25083DD0 */ addiu $t0, %lo(D_80123DD0) # addiu $t0, $t0, 0x3dd0 +/* 072954 80071D54 3C088012 */ lui $t0, %hi(_MotorStopData) # $t0, 0x8012 +/* 072958 80071D58 25083DD0 */ addiu $t0, %lo(_MotorStopData) # addiu $t0, $t0, 0x3dd0 /* 07295C 80071D5C 0018C980 */ sll $t9, $t8, 6 /* 072960 80071D60 03282821 */ addu $a1, $t9, $t0 /* 072964 80071D64 0C0335B0 */ jal __osSiRawStartDma diff --git a/lib/src/os/osMotor.c b/lib/src/os/osMotor.c index eab11065..4379d74d 100644 --- a/lib/src/os/osMotor.c +++ b/lib/src/os/osMotor.c @@ -3,8 +3,171 @@ #include "types.h" #include "macros.h" +#include "libultra_internal.h" +#include "controller.h" +#include "siint.h" -GLOBAL_ASM("lib/asm/non_matchings/unknown_072930/osMotorStop.s") -GLOBAL_ASM("lib/asm/non_matchings/unknown_072930/osMotorStart.s") -GLOBAL_ASM("lib/asm/non_matchings/unknown_072930/_MakeMotorData.s") -GLOBAL_ASM("lib/asm/non_matchings/unknown_072930/osMotorInit.s") +#if 1 +GLOBAL_ASM("lib/asm/non_matchings/osMotor/osMotorStop.s") +GLOBAL_ASM("lib/asm/non_matchings/osMotor/osMotorStart.s") +GLOBAL_ASM("lib/asm/non_matchings/osMotor/_MakeMotorData.s") +GLOBAL_ASM("lib/asm/non_matchings/osMotor/osMotorInit.s") +#else + +static void _MakeMotorData(int channel, u16 address, u8 *buffer, OSPifRam *mdata); +//u32 __osMotorinitialized[MAXCONTROLLERS] = {0, 0, 0, 0}; +extern OSPifRam _MotorStopData[MAXCONTROLLERS]; +extern OSPifRam _MotorStartData[MAXCONTROLLERS]; +extern u8 _motorstopbuf[32]; +extern u8 _motorstartbuf[32]; +s32 osMotorStop(OSPfs *pfs) +{ + int i; + s32 ret; + u8 *ptr; + __OSContRamReadFormat ramreadformat; + + __osSiGetAccess(); + + __osContLastCmd = CONT_CMD_WRITE_MEMPACK; + __osSiRawStartDma(OS_WRITE, &_MotorStopData[pfs->channel]); + osRecvMesg(pfs->queue, NULL, OS_MESG_BLOCK); + ret = __osSiRawStartDma(OS_READ, &__osPfsPifRam); + osRecvMesg(pfs->queue, NULL, OS_MESG_BLOCK); + ptr = (u8 *)&__osPfsPifRam; + + if (pfs->channel != 0) + for (i = 0; i < pfs->channel; i++) + ptr++; + + ramreadformat = *(__OSContRamReadFormat *)ptr; + ret = CHNL_ERR(ramreadformat); + if (ret == 0 && ramreadformat.datacrc != __osContDataCrc((u8*)&_motorstopbuf)) + { + ret = PFS_ERR_CONTRFAIL; + } + __osSiRelAccess(); + return ret; +} + +s32 osMotorStart(OSPfs *pfs) +{ + + int i; + s32 ret; + u8 *ptr; + __OSContRamReadFormat ramreadformat; + + __osSiGetAccess(); + + __osContLastCmd = CONT_CMD_WRITE_MEMPACK; + __osSiRawStartDma(OS_WRITE, &_MotorStartData[pfs->channel]); + osRecvMesg(pfs->queue, NULL, OS_MESG_BLOCK); + ret = __osSiRawStartDma(OS_READ, &__osPfsPifRam); + osRecvMesg(pfs->queue, NULL, OS_MESG_BLOCK); + ptr = (u8 *)&__osPfsPifRam; + + if (pfs->channel != 0) + for (i = 0; i < pfs->channel; i++) + ptr++; + + ramreadformat = *(__OSContRamReadFormat *)ptr; + ret = CHNL_ERR(ramreadformat); + if (ret == 0 && ramreadformat.datacrc != __osContDataCrc((u8*)&_motorstartbuf)) + { + ret = PFS_ERR_CONTRFAIL; + } + __osSiRelAccess(); + return ret; +} + +static void _MakeMotorData(int channel, u16 address, u8 *buffer, OSPifRam *mdata) +{ + u8 *ptr; + __OSContRamReadFormat ramreadformat; + int i; + + ptr = (u8 *)mdata->ramarray; + for (i = 0; i < ARRLEN(mdata->ramarray); i++) + mdata->ramarray[i] = 0; + mdata->pifstatus = CONT_CMD_EXE; + ramreadformat.dummy = CONT_CMD_NOP; + ramreadformat.txsize = CONT_CMD_WRITE_MEMPACK_TX; + ramreadformat.rxsize = CONT_CMD_WRITE_MEMPACK_RX; + ramreadformat.cmd = CONT_CMD_WRITE_MEMPACK; + + ramreadformat.address = (address << 0x5) | __osContAddressCrc(address); + ramreadformat.datacrc = CONT_CMD_NOP; + for (i = 0; i < ARRLEN(ramreadformat.data); i++) + + ramreadformat.data[i] = *buffer++; + if (channel != 0) + { + for (i = 0; i < channel; i++) + { + *ptr++ = 0; + } + } + *(__OSContRamReadFormat *)ptr = ramreadformat; + ptr += sizeof(__OSContRamReadFormat); + ptr[0] = CONT_CMD_END; +} + +s32 osMotorInit(OSMesgQueue *mq, OSPfs *pfs, int channel) +{ + int i; + s32 ret; + u8 temp[32]; + pfs->queue = mq; + pfs->channel = channel; + pfs->status = 0; + pfs->activebank = 128; + + for (i = 0; i < ARRLEN(temp); i++) + temp[i] = 254; + + ret = __osContRamWrite(mq, channel, 1024, temp, FALSE); + if (ret == 2) //TODO: remove magic constant + ret = __osContRamWrite(mq, channel, 1024, temp, FALSE); + if (ret != 0) + return ret; + + ret = __osContRamRead(mq, channel, 1024, temp); + if (ret == 2) + ret = PFS_ERR_CONTRFAIL; //is this right? + if (ret != 0) + return ret; + if (temp[31] == 254) + return PFS_ERR_DEVICE; + + for (i = 0; i < ARRLEN(temp); i++) + temp[i] = 128; + + ret = __osContRamWrite(mq, channel, 1024, temp, FALSE); + if (ret == 2) + ret = __osContRamWrite(mq, channel, 1024, temp, FALSE); + if (ret != 0) + return ret; + + ret = __osContRamRead(mq, channel, 1024, temp); + if (ret == 2) + ret = PFS_ERR_CONTRFAIL; + if (ret != 0) + return ret; + if (temp[31] != 128) + return PFS_ERR_DEVICE; + + //if (!__osMotorinitialized[channel]) + //{ + for (i = 0; i < ARRLEN(_motorstartbuf); i++) + { + _motorstartbuf[i] = 1; + _motorstopbuf[i] = 0; + } + _MakeMotorData(channel, 1536, _motorstartbuf, &_MotorStartData[channel]); + _MakeMotorData(channel, 1536, _motorstopbuf, &_MotorStopData[channel]); + //__osMotorinitialized[channel] = 1; + //} + return 0; +} +#endif diff --git a/src/memory.c b/src/memory.c index 43087210..5a6ee72e 100644 --- a/src/memory.c +++ b/src/memory.c @@ -4,6 +4,7 @@ #include "memory.h" #include "printf.h" #include "thread0_epc.h" +#include "controller.h" /************ .rodata ************/ @@ -37,10 +38,10 @@ s32 D_801235C4; FreeQueueSlot gFreeQueue[256]; s32 gFreeQueueCount; s32 gFreeQueueState; -s32 D_80123DD0[64]; -s32 D_80123ED0[64]; -s32 D_80123FD0[8]; -s32 D_80123FF0[8]; +OSPifRam _MotorStopData[MAXCONTROLLERS]; +OSPifRam _MotorStartData[MAXCONTROLLERS]; +u8 _motorstopbuf[32]; +u8 _motorstartbuf[32]; extern MemoryPoolSlot gMainMemoryPool;