From af118bc04587bfa8149ca8f4ad9457dc781e1f40 Mon Sep 17 00:00:00 2001 From: Mr-Wiseguy <68165316+Mr-Wiseguy@users.noreply.github.com> Date: Thu, 3 Mar 2022 00:16:07 -0500 Subject: [PATCH] Some pi and controller functions (#13) * Some pi and controller functions * Fixed formatting issues in pidma.c and pimgr.c * Changed devmgr.c label to match mdebug name --- src/io/cartrominit.c | 65 +++++++++++++++++++++ src/io/contramread.c | 65 +++++++++++++++++++++ src/io/contramwrite.c | 73 +++++++++++++++++++++++ src/io/controller.h | 3 +- src/io/devmgr.c | 4 +- src/io/motor.c | 133 ++++++++++++++++++++++++++++++++++++++++++ src/io/pi.c | 13 +++++ src/io/piacs.c | 24 ++++++++ src/io/pidma.c | 30 ++++++++++ src/io/pigetcmdq.c | 10 ++++ src/io/pigetstat.c | 9 +++ src/io/pigettype.c | 8 +++ src/io/piint.h | 3 + src/io/pimgr.c | 53 +++++++++++++++++ src/io/pirawdma.c | 2 +- 15 files changed, 491 insertions(+), 4 deletions(-) create mode 100644 src/io/cartrominit.c create mode 100644 src/io/contramread.c create mode 100644 src/io/contramwrite.c create mode 100644 src/io/motor.c create mode 100644 src/io/pi.c create mode 100644 src/io/piacs.c create mode 100644 src/io/pidma.c create mode 100644 src/io/pigetcmdq.c create mode 100644 src/io/pigetstat.c create mode 100644 src/io/pigettype.c create mode 100644 src/io/pimgr.c diff --git a/src/io/cartrominit.c b/src/io/cartrominit.c new file mode 100644 index 0000000..9cd1617 --- /dev/null +++ b/src/io/cartrominit.c @@ -0,0 +1,65 @@ +#include "macros.h" +#include "PR/os_internal.h" +#include "PR/R4300.h" +#include "PR/rcp.h" + +OSPiHandle __CartRomHandle ALIGNED(8); +OSPiHandle *osCartRomInit(void) +{ + u32 value; + u32 saveMask; + static int first = 1; + register u32 stat; + u32 latency; + u32 pulse; + u32 pageSize; + u32 relDuration; + + __osPiGetAccess(); + + if (!first) { + __osPiRelAccess(); + return &__CartRomHandle; + } + + first = 0; + __CartRomHandle.type = DEVICE_TYPE_CART; + __CartRomHandle.baseAddress = PHYS_TO_K1(PI_DOM1_ADDR2); + __CartRomHandle.domain = PI_DOMAIN1; + __CartRomHandle.speed = 0; + + bzero(&__CartRomHandle.transferInfo, sizeof(__OSTranxInfo)); + + while (stat = IO_READ(PI_STATUS_REG), stat & (PI_STATUS_DMA_BUSY | PI_STATUS_IO_BUSY)) { + ; + } + + latency = IO_READ(PI_BSD_DOM1_LAT_REG); + pageSize = IO_READ(PI_BSD_DOM1_PGS_REG); + relDuration = IO_READ(PI_BSD_DOM1_RLS_REG); + pulse = IO_READ(PI_BSD_DOM1_PWD_REG); + + IO_WRITE(PI_BSD_DOM1_LAT_REG, 0xFF); + IO_WRITE(PI_BSD_DOM1_PGS_REG, 0); + IO_WRITE(PI_BSD_DOM1_RLS_REG, 3); + IO_WRITE(PI_BSD_DOM1_PWD_REG, 0xFF); + + value = IO_READ(__CartRomHandle.baseAddress); + __CartRomHandle.latency = value & 0xFF; + __CartRomHandle.pageSize = (value >> 0x10) & 0xF; + __CartRomHandle.relDuration = (value >> 0x14) & 0xF; + __CartRomHandle.pulse = (value >> 8) & 0xFF; + + IO_WRITE(PI_BSD_DOM1_LAT_REG, latency); + IO_WRITE(PI_BSD_DOM1_PGS_REG, pageSize); + IO_WRITE(PI_BSD_DOM1_RLS_REG, relDuration); + IO_WRITE(PI_BSD_DOM1_PWD_REG, pulse); + + saveMask = __osDisableInt(); + __CartRomHandle.next = __osPiTable; + __osPiTable = &__CartRomHandle; + __osRestoreInt(saveMask); + __osPiRelAccess(); + + return &__CartRomHandle; +} diff --git a/src/io/contramread.c b/src/io/contramread.c new file mode 100644 index 0000000..fc069c2 --- /dev/null +++ b/src/io/contramread.c @@ -0,0 +1,65 @@ +#include "PR/os_internal.h" +#include "PR/rcp.h" +#include "controller.h" +#include "siint.h" + +s32 __osPfsLastChannel = -1; + +s32 __osContRamRead(OSMesgQueue* mq, int channel, u16 address, u8* buffer) { + s32 ret; + s32 i; + u8* ptr; + s32 retry = 2; + + __osSiGetAccess(); + do { + ptr = (u8*)&__osPfsPifRam; + + if ((__osContLastCmd != 2) || (__osPfsLastChannel != channel)) { + __osContLastCmd = 2; + __osPfsLastChannel = channel; + + for (i = 0; i < channel; i++) { + *ptr++ = 0; + } + + __osPfsPifRam.pifstatus = CONT_CMD_EXE; + ((__OSContRamReadFormat*)ptr)->dummy = CONT_CMD_NOP; + ((__OSContRamReadFormat*)ptr)->txsize = CONT_CMD_READ_MEMPACK_TX; + ((__OSContRamReadFormat*)ptr)->rxsize = CONT_CMD_READ_MEMPACK_RX; + ((__OSContRamReadFormat*)ptr)->cmd = CONT_CMD_READ_MEMPACK; + ((__OSContRamReadFormat*)ptr)->datacrc = 0xFF; + ptr[sizeof(__OSContRamReadFormat)] = CONT_CMD_END; + } else { + ptr += channel; + } + + ((__OSContRamReadFormat*)ptr)->addrh = address >> 3; + ((__OSContRamReadFormat*)ptr)->addrl = (s8)(__osContAddressCrc(address) | (address << 5)); + __osSiRawStartDma(OS_WRITE, &__osPfsPifRam); + osRecvMesg(mq, NULL, OS_MESG_BLOCK); + __osSiRawStartDma(OS_READ, &__osPfsPifRam); + osRecvMesg(mq, NULL, OS_MESG_BLOCK); + + ret = (((__OSContRamReadFormat*)ptr)->rxsize & 0xC0) >> 4; + if (!ret) { + if (__osContDataCrc(ptr + 6) != ((__OSContRamReadFormat*)ptr)->datacrc) { + ret = __osPfsGetStatus(mq, channel); + if (ret) { + break; + } + ret = PFS_ERR_CONTRFAIL; + } else { + bcopy(ptr + 6, buffer, BLOCKSIZE); + } + } else { + ret = PFS_ERR_NOPACK; + } + if (ret != PFS_ERR_CONTRFAIL) { + break; + } + } while (0 <= retry--); + __osSiRelAccess(); + + return ret; +} \ No newline at end of file diff --git a/src/io/contramwrite.c b/src/io/contramwrite.c new file mode 100644 index 0000000..47eb566 --- /dev/null +++ b/src/io/contramwrite.c @@ -0,0 +1,73 @@ +#include "PR/os_internal.h" +#include "PR/rcp.h" +#include "controller.h" +#include "siint.h" + +extern s32 __osPfsLastChannel; + +s32 __osContRamWrite(OSMesgQueue* mq, int channel, u16 address, u8* buffer, int force) { + s32 ret = 0; + s32 i; + u8* ptr; + s32 retry = 2; + u8 crc; + + if ((force != 1) && (address < PFS_LABEL_AREA) && (address != 0)) { + return 0; + } + + __osSiGetAccess(); + + do { + ptr = (u8*)(&__osPfsPifRam); + + if (__osContLastCmd != CONT_CMD_WRITE_MEMPACK || __osPfsLastChannel != channel) { + __osContLastCmd = CONT_CMD_WRITE_MEMPACK; + __osPfsLastChannel = channel; + + for (i = 0; i < channel; i++) { + *ptr++ = 0; + } + + __osPfsPifRam.pifstatus = CONT_CMD_EXE; + + ((__OSContRamReadFormat*)ptr)->dummy = CONT_CMD_NOP; + ((__OSContRamReadFormat*)ptr)->txsize = CONT_CMD_WRITE_MEMPACK_TX; + ((__OSContRamReadFormat*)ptr)->rxsize = CONT_CMD_WRITE_MEMPACK_RX; + ((__OSContRamReadFormat*)ptr)->cmd = CONT_CMD_WRITE_MEMPACK; + ((__OSContRamReadFormat*)ptr)->datacrc = 0xFF; + + ptr[sizeof(__OSContRamReadFormat)] = CONT_CMD_END; + } else { + ptr += channel; + } + + ((__OSContRamReadFormat*)ptr)->addrh = address >> 3; + ((__OSContRamReadFormat*)ptr)->addrl = ((address << 5) | __osContAddressCrc(address)); + + bcopy(buffer, ((__OSContRamReadFormat*)ptr)->data, BLOCKSIZE); + + ret = __osSiRawStartDma(OS_WRITE, &__osPfsPifRam); + crc = __osContDataCrc(buffer); + osRecvMesg(mq, (OSMesg*)NULL, OS_MESG_BLOCK); + + ret = __osSiRawStartDma(OS_READ, &__osPfsPifRam); + osRecvMesg(mq, (OSMesg*)NULL, OS_MESG_BLOCK); + + ret = ((((__OSContRamReadFormat*)ptr)->rxsize & 0xC0) >> 4); + if (!ret) { + if (crc != ((__OSContRamReadFormat*)ptr)->datacrc) { + if ((ret = __osPfsGetStatus(mq, channel))) { + break; + } else { + ret = PFS_ERR_CONTRFAIL; + } + } + } else { + ret = PFS_ERR_NOPACK; + } + } while ((ret == PFS_ERR_CONTRFAIL) && (retry-- >= 0)); + __osSiRelAccess(); + + return ret; +} \ No newline at end of file diff --git a/src/io/controller.h b/src/io/controller.h index cc1413d..80cf4aa 100755 --- a/src/io/controller.h +++ b/src/io/controller.h @@ -53,7 +53,8 @@ typedef struct /* 0x1 */ u8 txsize; /* 0x2 */ u8 rxsize; /* 0x3 */ u8 cmd; - /* 0x4 */ u16 address; + /* 0x4 */ u8 addrh; + /* 0x5 */ u8 addrl; /* 0x6 */ u8 data[BLOCKSIZE]; /* 0x26 */ u8 datacrc; } __OSContRamReadFormat; diff --git a/src/io/devmgr.c b/src/io/devmgr.c index 76e1a04..2d22774 100644 --- a/src/io/devmgr.c +++ b/src/io/devmgr.c @@ -40,7 +40,7 @@ void __osDevMgrMain(void *args) __osResetGlobalIntMask(OS_IM_PI); __osEPiRawWriteIo(mb->piHandle, LEO_BM_CTL, (info->bmCtlShadow | 0x80000000)); -doMessageSend: +readblock1: osRecvMesg(dm->evtQueue, &em, OS_MESG_BLOCK); info = &mb->piHandle->transferInfo; blockInfo = &info->block[info->blockNum]; @@ -64,7 +64,7 @@ doMessageSend: if (messageSend == 1 && mb->piHandle->transferInfo.block[0].errStatus == LEO_ERROR_GOOD) { messageSend = 0; - goto doMessageSend; + goto readblock1; } osSendMesg(dm->acsQueue, NULL, OS_MESG_NOBLOCK); diff --git a/src/io/motor.c b/src/io/motor.c new file mode 100644 index 0000000..7f26dfc --- /dev/null +++ b/src/io/motor.c @@ -0,0 +1,133 @@ +#include "macros.h" +#include "PR/os_internal.h" +#include "controller.h" +#include "siint.h" + +static OSPifRam __MotorDataBuf[MAXCONTROLLERS]; + +s32 __osMotorAccess(OSPfs* pfs, u32 vibrate) { + int i; + s32 ret; + u8* ptr = (u8*)&__MotorDataBuf[pfs->channel]; + + if (!(pfs->status & 8)) { + return 5; + } + + __osSiGetAccess(); + __MotorDataBuf[pfs->channel].pifstatus = CONT_CMD_EXE; + ptr += pfs->channel; + for (i = 0; i < BLOCKSIZE; i++) { + ((__OSContRamReadFormat*)ptr)->data[i] = vibrate; + } + + __osContLastCmd = CONT_CMD_END; + __osSiRawStartDma(OS_WRITE, &__MotorDataBuf[pfs->channel]); + osRecvMesg(pfs->queue, NULL, OS_MESG_BLOCK); + __osSiRawStartDma(OS_READ, &__MotorDataBuf[pfs->channel]); + osRecvMesg(pfs->queue, NULL, OS_MESG_BLOCK); + + ret = ((__OSContRamReadFormat*)ptr)->rxsize & 0xC0; + if (!ret) { + if (!vibrate) { + if (((__OSContRamReadFormat*)ptr)->datacrc != 0) { + ret = PFS_ERR_CONTRFAIL; + } + } else { + if (((__OSContRamReadFormat*)ptr)->datacrc != 0xEB) { + ret = PFS_ERR_CONTRFAIL; + } + } + } + + __osSiRelAccess(); + + return ret; +} + +static void _MakeMotorData(int channel, OSPifRam *mdata) { + u8 *ptr = (u8 *)mdata->ramarray; + __OSContRamReadFormat ramreadformat; + int i; + + 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.addrh = 0x600 >> 3; + ramreadformat.addrl = (u8)(__osContAddressCrc(0x600) | (0x600 << 5)); + + 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) +{ + s32 ret; + u8 temp[32]; + + pfs->queue = mq; + pfs->channel = channel; + pfs->activebank = 0xFF; + pfs->status = 0; + + ret = __osPfsSelectBank(pfs, 0xFE); + + if (ret == 2) { + ret = __osPfsSelectBank(pfs, 0x80); + } + + if (ret != 0) { + return ret; + } + + ret = __osContRamRead(mq, channel, 0x400, temp); + + if (ret == 2) { + ret = PFS_ERR_CONTRFAIL; + } + + if (ret != 0) { + return ret; + } + + if (temp[31] == 254) { + return PFS_ERR_DEVICE; + } + + ret = __osPfsSelectBank(pfs, 0x80); + if (ret == 2) { + ret = PFS_ERR_CONTRFAIL; + } + + 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] != 0x80) { + return PFS_ERR_DEVICE; + } + + if (!(pfs->status & PFS_MOTOR_INITIALIZED)) { + _MakeMotorData(channel, &__MotorDataBuf[channel]); + } + + pfs->status = PFS_MOTOR_INITIALIZED; + return 0; +} diff --git a/src/io/pi.c b/src/io/pi.c new file mode 100644 index 0000000..1c350c5 --- /dev/null +++ b/src/io/pi.c @@ -0,0 +1,13 @@ + +#include "PR/os_internal.h" +#include "piint.h" + +// TODO: this comes from a header +#ident "$Revision: 1.17 $" + +int __osPiDeviceBusy() { + register u32 stat = IO_READ(PI_STATUS_REG); + if (stat & (PI_STATUS_DMA_BUSY | PI_STATUS_IO_BUSY)) + return 1; + return 0; +} diff --git a/src/io/piacs.c b/src/io/piacs.c new file mode 100644 index 0000000..03ab11e --- /dev/null +++ b/src/io/piacs.c @@ -0,0 +1,24 @@ +#include "macros.h" +#include "PR/os_internal.h" + +#define PI_Q_BUF_LEN 1 +u32 __osPiAccessQueueEnabled = 0; +OSMesgQueue __osPiAccessQueue ALIGNED(8); +static OSMesg piAccessBuf[PI_Q_BUF_LEN]; + +void __osPiCreateAccessQueue(void) { + __osPiAccessQueueEnabled = 1; + osCreateMesgQueue(&__osPiAccessQueue, piAccessBuf, PI_Q_BUF_LEN); + osSendMesg(&__osPiAccessQueue, NULL, OS_MESG_NOBLOCK); +} + +void __osPiGetAccess(void) { + OSMesg dummyMesg; + if (!__osPiAccessQueueEnabled) + __osPiCreateAccessQueue(); + osRecvMesg(&__osPiAccessQueue, &dummyMesg, OS_MESG_BLOCK); +} + +void __osPiRelAccess(void) { + osSendMesg(&__osPiAccessQueue, NULL, OS_MESG_NOBLOCK); +} diff --git a/src/io/pidma.c b/src/io/pidma.c new file mode 100644 index 0000000..077f13a --- /dev/null +++ b/src/io/pidma.c @@ -0,0 +1,30 @@ +#include "PR/os_internal.h" +#include "piint.h" + +s32 osPiStartDma(OSIoMesg *mb, s32 priority, s32 direction, u32 devAddr, void *dramAddr, u32 size, OSMesgQueue *mq) { + register s32 ret; + if (!__osPiDevMgr.active) { + return -1; + } + + if (direction == OS_READ) { + mb->hdr.type = OS_MESG_TYPE_DMAREAD; + } else { + mb->hdr.type = OS_MESG_TYPE_DMAWRITE; + } + + mb->hdr.pri = priority; + mb->hdr.retQueue = mq; + mb->dramAddr = dramAddr; + mb->devAddr = devAddr; + mb->size = size; + mb->piHandle = NULL; + + if (priority == OS_MESG_PRI_HIGH) { + ret = osJamMesg(osPiGetCmdQueue(), (OSMesg)mb, OS_MESG_NOBLOCK); + } else { + ret = osSendMesg(osPiGetCmdQueue(), (OSMesg)mb, OS_MESG_NOBLOCK); + } + + return ret; +} diff --git a/src/io/pigetcmdq.c b/src/io/pigetcmdq.c new file mode 100644 index 0000000..809cdb2 --- /dev/null +++ b/src/io/pigetcmdq.c @@ -0,0 +1,10 @@ +#include "PR/os_internal.h" +#include "piint.h" + +OSMesgQueue *osPiGetCmdQueue(void) { + if (!__osPiDevMgr.active) { + return NULL; + } + + return __osPiDevMgr.cmdQueue; +} diff --git a/src/io/pigetstat.c b/src/io/pigetstat.c new file mode 100644 index 0000000..43e9a27 --- /dev/null +++ b/src/io/pigetstat.c @@ -0,0 +1,9 @@ +#include "PR/os_internal.h" +#include "piint.h" + +// TODO: this comes from a header +#ident "$Revision: 1.17 $" + +u32 osPiGetStatus() { + return IO_READ(PI_STATUS_REG); +} diff --git a/src/io/pigettype.c b/src/io/pigettype.c new file mode 100644 index 0000000..f918306 --- /dev/null +++ b/src/io/pigettype.c @@ -0,0 +1,8 @@ +#include "PR/os_internal.h" + +// TODO: this comes from a header +#ident "$Revision: 1.17 $" + +s32 osPiGetDeviceType() { + return osRomType; +} diff --git a/src/io/piint.h b/src/io/piint.h index 1389f6b..734fdde 100644 --- a/src/io/piint.h +++ b/src/io/piint.h @@ -105,6 +105,9 @@ void __osDevMgrMain(void *); void __osPiCreateAccessQueue(void); void __osPiRelAccess(void); void __osPiGetAccess(void); +s32 __osPiRawStartDma(s32, u32 , void *, u32 ); +s32 __osPiRawWriteIo(u32, u32); +s32 __osPiRawReadIo(u32, u32 *); s32 __osEPiRawWriteIo(OSPiHandle *, u32 , u32); s32 __osEPiRawReadIo(OSPiHandle *, u32 , u32 *); s32 __osEPiRawStartDma(OSPiHandle *, s32 , u32 , void *, u32 ); diff --git a/src/io/pimgr.c b/src/io/pimgr.c new file mode 100644 index 0000000..c757532 --- /dev/null +++ b/src/io/pimgr.c @@ -0,0 +1,53 @@ +#include "macros.h" +#include "PR/os_internal.h" +#include "piint.h" + +OSDevMgr __osPiDevMgr = {0}; +OSPiHandle *__osPiTable = NULL; +OSPiHandle __Dom1SpeedParam ALIGNED(8); +OSPiHandle __Dom2SpeedParam ALIGNED(8); +OSPiHandle *__osCurrentHandle[2] ALIGNED(8) = {&__Dom1SpeedParam, &__Dom2SpeedParam}; +static OSThread piThread; +static char piThreadStack[OS_PIM_STACKSIZE]; +static OSMesgQueue piEventQueue; +static OSMesg piEventBuf[1]; + +void osCreatePiManager(OSPri pri, OSMesgQueue *cmdQ, OSMesg *cmdBuf, s32 cmdMsgCnt) { + u32 savedMask; + OSPri oldPri; + OSPri myPri; + + if (!__osPiDevMgr.active) { + osCreateMesgQueue(cmdQ, cmdBuf, cmdMsgCnt); + osCreateMesgQueue(&piEventQueue, (OSMesg*)piEventBuf, 1); + + if (!__osPiAccessQueueEnabled) { + __osPiCreateAccessQueue(); + } + + osSetEventMesg(OS_EVENT_PI, &piEventQueue, (OSMesg)0x22222222); + oldPri = -1; + myPri = osGetThreadPri(NULL); + + if (myPri < pri) { + oldPri = myPri; + osSetThreadPri(NULL, pri); + } + + savedMask = __osDisableInt(); + __osPiDevMgr.active = 1; + __osPiDevMgr.thread = &piThread; + __osPiDevMgr.cmdQueue = cmdQ; + __osPiDevMgr.evtQueue = &piEventQueue; + __osPiDevMgr.acsQueue = &__osPiAccessQueue; + __osPiDevMgr.dma = __osPiRawStartDma; + __osPiDevMgr.edma = __osEPiRawStartDma; + osCreateThread(&piThread, 0, __osDevMgrMain, &__osPiDevMgr, &piThreadStack[OS_PIM_STACKSIZE], pri); + osStartThread(&piThread); + __osRestoreInt(savedMask); + + if (oldPri != -1) { + osSetThreadPri(NULL, oldPri); + } + } +} diff --git a/src/io/pirawdma.c b/src/io/pirawdma.c index 28b19a5..d766064 100644 --- a/src/io/pirawdma.c +++ b/src/io/pirawdma.c @@ -3,7 +3,7 @@ // TODO: this comes from a header #ident "$Revision: 1.17 $" -s32 __osPiRawStartDma(s32 direction, u32 devAddr, void* dramAddr, size_t size) { +s32 __osPiRawStartDma(s32 direction, u32 devAddr, void* dramAddr, u32 size) { register u32 stat; WAIT_ON_IOBUSY(stat);