oot/src/libultra_code/osRumblePak.c

#include <ultra64.h>
#include <global.h>
#include <ultra64/controller.h>

#define BANK_ADDR 0x400
#define MOTOR_ID 0x80

OSPifRam osPifBuffers[MAXCONTROLLERS];

// func_800CF990 in 1.0
s32 osSetRumble(OSPfs* pfs, u32 vibrate) {
    s32 i;
    s32 ret;
    u8* buf;

    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 osSetUpMempakWrite(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)(osMempakAddrCRC(0x600) | (0x600 << 5));
    if (channel != 0) {
        for (i = 0; i < channel; ++i) {
            *bufptr++ = 0;
        }
    }
    *(__OSContRamHeader*)bufptr = mempakwr;
    bufptr += sizeof(mempakwr);
    *bufptr = 0xFE;
}

s32 osProbeRumblePak(OSMesgQueue* ctrlrqueue, OSPfs* pfs, u32 channel) {
    s32 ret;
    u8 sp24[BLOCKSIZE];

    pfs->queue = ctrlrqueue;
    pfs->channel = channel;
    pfs->activebank = 0xFF;
    pfs->status = 0;

    ret = func_80104C80(pfs, 0xFE);
    if (ret == 2) {
        ret = func_80104C80(pfs, MOTOR_ID);
    }
    if (ret != 0) {
        return ret;
    }
    ret = osReadMempak(ctrlrqueue, channel, BANK_ADDR, sp24);
    ret = ret;
    if (ret == 2) {
        ret = 4; // "Controller pack communication error"
    }
    if (ret != 0) {
        return ret;
    }
    if (sp24[BLOCKSIZE - 1] == 0xFE) {
        return 0xB;
    }
    ret = func_80104C80(pfs, MOTOR_ID);
    if (ret == 2) {
        ret = 4; // "Controller pack communication error"
    }
    if (ret != 0) {
        return ret;
    }
    ret = osReadMempak(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) {
        osSetUpMempakWrite(channel, &osPifBuffers[channel]);
    }
    pfs->status = PFS_MOTOR_INITIALIZED;
    return 0; // "Recognized rumble pak"
}