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

@@ -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;
+}

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;
+}

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;
+}

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;

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);

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;
+}

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;
+}

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);
+}

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;
+}

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;
+}

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);
+}

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;
+}

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 );

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);
+        }
+    }
+}

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);