121 lines
2.9 KiB
C
121 lines
2.9 KiB
C
#include "global.h"
|
|
|
|
#define BANK_ADDR 0x400
|
|
#define MOTOR_ID 0x80
|
|
|
|
OSPifRam osPifBuffers[MAXCONTROLLERS];
|
|
|
|
s32 __osMotorAccess(OSPfs* pfs, u32 vibrate) {
|
|
s32 i;
|
|
s32 ret;
|
|
u8* buf = (u8*)&osPifBuffers[pfs->channel];
|
|
|
|
if (!(pfs->status & 8)) {
|
|
return 5;
|
|
}
|
|
|
|
__osSiGetAccess();
|
|
osPifBuffers[pfs->channel].status = 1;
|
|
buf += pfs->channel;
|
|
for (i = 0; i < BLOCKSIZE; i++) {
|
|
((__OSContRamHeader*)buf)->data[i] = vibrate;
|
|
}
|
|
|
|
__osContLastPoll = CONT_CMD_END;
|
|
__osSiRawStartDma(OS_WRITE, &osPifBuffers[pfs->channel]);
|
|
osRecvMesg(pfs->queue, NULL, OS_MESG_BLOCK);
|
|
__osSiRawStartDma(OS_READ, &osPifBuffers[pfs->channel]);
|
|
osRecvMesg(pfs->queue, NULL, OS_MESG_BLOCK);
|
|
|
|
ret = ((__OSContRamHeader*)buf)->rxsize & 0xC0;
|
|
if (!ret) {
|
|
if (!vibrate) {
|
|
if (((__OSContRamHeader*)buf)->datacrc != 0) {
|
|
ret = PFS_ERR_CONTRFAIL;
|
|
}
|
|
} else {
|
|
if (((__OSContRamHeader*)buf)->datacrc != 0xEB) {
|
|
ret = PFS_ERR_CONTRFAIL;
|
|
}
|
|
}
|
|
}
|
|
|
|
__osSiRelAccess();
|
|
|
|
return ret;
|
|
}
|
|
|
|
void _MakeMotorData(s32 channel, OSPifRam* buf) {
|
|
u8* bufptr = (u8*)buf;
|
|
__OSContRamHeader mempakwr;
|
|
s32 i;
|
|
|
|
mempakwr.unk_00 = 0xFF;
|
|
mempakwr.txsize = 0x23;
|
|
mempakwr.rxsize = 1;
|
|
mempakwr.poll = 3; // write mempak
|
|
mempakwr.hi = 0x600 >> 3;
|
|
mempakwr.lo = (u8)(__osContAddressCrc(0x600) | (0x600 << 5));
|
|
|
|
if (channel != 0) {
|
|
for (i = 0; i < channel; ++i) {
|
|
*bufptr++ = 0;
|
|
}
|
|
}
|
|
|
|
*(__OSContRamHeader*)bufptr = mempakwr;
|
|
bufptr += sizeof(mempakwr);
|
|
*bufptr = 0xFE;
|
|
}
|
|
|
|
s32 osMotorInit(OSMesgQueue* ctrlrqueue, OSPfs* pfs, s32 channel) {
|
|
s32 ret;
|
|
u8 sp24[BLOCKSIZE];
|
|
|
|
pfs->queue = ctrlrqueue;
|
|
pfs->channel = channel;
|
|
pfs->activebank = 0xFF;
|
|
pfs->status = 0;
|
|
|
|
ret = __osPfsSelectBank(pfs, 0xFE);
|
|
if (ret == 2) {
|
|
ret = __osPfsSelectBank(pfs, MOTOR_ID);
|
|
}
|
|
if (ret != 0) {
|
|
return ret;
|
|
}
|
|
ret = __osContRamRead(ctrlrqueue, channel, BANK_ADDR, sp24);
|
|
if (ret == 2) {
|
|
ret = 4; // "Controller pack communication error"
|
|
}
|
|
if (ret != 0) {
|
|
return ret;
|
|
}
|
|
if (sp24[BLOCKSIZE - 1] == 0xFE) {
|
|
return 0xB;
|
|
}
|
|
ret = __osPfsSelectBank(pfs, MOTOR_ID);
|
|
if (ret == 2) {
|
|
ret = 4; // "Controller pack communication error"
|
|
}
|
|
if (ret != 0) {
|
|
return ret;
|
|
}
|
|
ret = __osContRamRead(ctrlrqueue, channel, BANK_ADDR, sp24);
|
|
if (ret == 2) {
|
|
ret = 4; // "Controller pack communication error"
|
|
}
|
|
if (ret != 0) {
|
|
return ret;
|
|
}
|
|
if (sp24[BLOCKSIZE - 1] != MOTOR_ID) {
|
|
return 0xB;
|
|
}
|
|
if ((pfs->status & PFS_MOTOR_INITIALIZED) == 0) {
|
|
_MakeMotorData(channel, &osPifBuffers[channel]);
|
|
}
|
|
pfs->status = PFS_MOTOR_INITIALIZED;
|
|
|
|
return 0; // "Recognized rumble pak"
|
|
}
|