#include "ultra64.h"
#include <PR/bcp.h>
#include "os_bb.h"

#include "fsrw.h"


#define DMA_QUEUE_SIZE	200
#define PRINTF		osSyncPrintf
#define MSG_FAULT	0x10


/*
 * Thread and stack structures
 */
char   bootStack[STACKSIZE] __attribute__ ((aligned (8)));

static OSThread idleThread;
static char     idleThreadStack[STACKSIZE] __attribute__ ((aligned (8)));

static OSThread mainThread;
static char     mainThreadStack[STACKSIZE] __attribute__ ((aligned (8)));

static OSThread excepThread;
static char     excepThreadStack[STACKSIZE] __attribute__ ((aligned (8)));


/*
 * Message queues and message buffers used by this app 
 */
static OSMesg           PiMessages[DMA_QUEUE_SIZE];
static OSMesgQueue      PiMessageQ;

static OSMesg           SiMessages[DMA_QUEUE_SIZE];
static OSMesgQueue      SiMessageQ;

/*
 * Local variables and routines
 */

static void		idleproc(char *);
static void		mainproc(char *);
static void		excepproc(char *);

void
boot(void)
{
    osInitialize();
    osCreateThread(&idleThread, 1, (void(*)(void *))idleproc, (void *)0,
                   idleThreadStack+STACKSIZE, 8);
    osStartThread(&idleThread);
}


static void
idleproc(char *argv)		/* priority 8 */
{
    osCreateViManager(OS_PRIORITY_VIMGR);
    osViSetMode(&osViModeTable[OS_VI_NTSC_LPN1]);

    /*
     * Start PI Mgr for access to cartridge - start before the debugger
     */
    osCreatePiManager((OSPri) OS_PRIORITY_PIMGR, &PiMessageQ, PiMessages,
            DMA_QUEUE_SIZE);

    osCreateMesgQueue(&SiMessageQ, SiMessages, DMA_QUEUE_SIZE);
    osSetEventMesg(OS_EVENT_SI, &SiMessageQ, (OSMesg)DMA_QUEUE_SIZE);

    /*
     * The main thread's priority must be the same or lower than the original
     * idle's thread priority. This allows the idle thread to change its
     * priority to 0 before the main thread starts execution.
     */
    osCreateThread(&mainThread, 3, (void(*)(void *))mainproc, argv,
           mainThreadStack+STACKSIZE/8, (OSPri)7);
    osStartThread(&mainThread);

    osCreateThread(&excepThread, 3, (void(*)(void *))excepproc, argv,
           excepThreadStack+STACKSIZE/8, (OSPri)7);
    osStartThread(&excepThread);

    osSetThreadPri(0, OS_PRIORITY_IDLE);
    for(;;);                                    /* idle thread */
}

#define __REG(x) "$" #x
#define __REG1(x) #x
#define getcp0reg(source)          	                        \
({ int __res;                                                   \
        __asm__ __volatile__(                                   \
	".set\tpush\n\t"					\
	".set\treorder\n\t"					\
        "mfc0\t%0,"__REG(source)"\n\t"                          \
	".set\tpop"						\
        : "=r" (__res));                                        \
        __res;})

static OSMesgQueue	retraceMessageQ;
static OSMesg		dummyMessage, retraceMessageBuf;
static OSBbFs fs;
#define NDIR 100
static OSBbStatFs fsb;
#define NBLOCKS 	128
static u8 blocks[NBLOCKS*BB_FL_BLOCK_SIZE] __attribute__((aligned(16)));
static u32 error_shadow;

static void 
mainproc(char *argv) {
    s32 i, j, l, fd, rv;
    u32 sum, sum2, t1, t2;
    extern u8 __osBbCardMultiplane;
    extern u8 __osBbCardNoEcc;
    u8 spare[BB_FL_SPARE_SIZE], save;
    osCreateMesgQueue(&retraceMessageQ, &retraceMessageBuf, 1);
    osViSetEvent(&retraceMessageQ, dummyMessage, 1);

    PRINTF("\n=> mainproc...\n");
    PRINTF("init\n");
    if ((rv = osBbFInit(&fs)) < 0) {
	PRINTF("osBbFInit failed %d\n", rv);
	goto out;
    }
    if ((rv = osBbFStatFs(&fsb)) < 0) {
	PRINTF("osBbFStatFs failed %d\n", rv);
	goto out;
    }
    PRINTF("osBbFFStatFS files %d free files %d blocks %d free blocks %d\n", fsb.files, fsb.freeFiles, fsb.blocks, fsb.freeBlocks);

    /* make a big file */
    sum = 0;
    for(i = 0; i < NBLOCKS; i++) {
	for(j = 0; j < BB_FL_BLOCK_SIZE; j++) {
	    blocks[i*BB_FL_BLOCK_SIZE+j] = (j&1)?j : ((i<<4)|(j>>8));
	    sum += blocks[i*BB_FL_BLOCK_SIZE+j];
	}
    }
    if ((fd = osBbFCreate("big.c", 1, sizeof blocks)) < 0) {
	PRINTF("osBbFCreate big.c failed %d\n", fd);
	goto out;
    }

    for(l = 0; l < 2; l++) {
	t1 = getcp0reg(C0_COUNT);
	if ((rv = osBbFWrite(fd, 0, blocks, sizeof blocks)) < 0) {
	    PRINTF("osBbFWrite big.c failed %d\n", rv);
	    goto out;
	}
	t2 = getcp0reg(C0_COUNT);
	PRINTF("write speed (%s multiplane) %.2f MB/s\n", __osBbCardMultiplane ? "with" : "without", sizeof(blocks)/(float)OS_CYCLES_TO_USEC(t2-t1)*96.f/62.5f);

	bzero(blocks, sizeof blocks);
	if ((rv = osBbFRead(fd, 0, blocks, sizeof blocks)) < 0 || rv != sizeof blocks) {
	    PRINTF("osBbFRead big.c failed %d\n", rv);
	    goto out;
	}
	sum2 = 0;
	for(j = 0; j < NBLOCKS; j++) {
	    int k;
	    for(k = 0; k < BB_FL_BLOCK_SIZE; k++) {
		sum2 += blocks[j*BB_FL_BLOCK_SIZE+k];
	    }
	}
	if (sum != sum2) {
	    PRINTF("osBbFRead big.c sum mismatch read %x expected %x\n", sum2, sum);
	    goto out;
	}
	__osBbCardMultiplane ^= 1;
    }
    
    t1 = getcp0reg(C0_COUNT);
    if ((rv = osBbFRead(fd, 0, blocks, sizeof blocks)) < 0) {
	PRINTF("osBbFRead big.c failed %d\n", rv);
	goto out;
    }
    t2 = getcp0reg(C0_COUNT);
    PRINTF("read speed %.2f MB/s\n", sizeof(blocks)/(float)OS_CYCLES_TO_USEC(t2-t1)*96.f/62.5f);

//#define TEST_ECC
#define ERR_TYPE	PI_ERROR_SYS_INTR
//#define ERR_TYPE	PI_ERROR_KERNEL_INTR
#ifdef TEST_ECC
    IO_WRITE(PI_ERROR_REG, ERR_TYPE);
    PRINTF ("test single bit error\n");
    bzero(blist, sizeof blist);
    osBbFStat(fd, &sb, blist, sizeof blist);
    osBbAtbSetup(PI_DOM1_ADDR2, blist, sizeof blist/sizeof blist[0]);
    osBbCardReadBlock(0, blist[0], blocks, spare);
#define LOC	(BB_FL_BLOCK_SIZE-100)
    save = blocks[LOC];
    blocks[LOC] ^= 1;
    __osBbCardNoEcc = 1;
    if ((rv = osBbCardWriteBlock(0, blist[0], blocks, spare)) < 0) {
	PRINTF("osBbCardWriteBlock %d failed %d\n", blist[0], rv);
	goto out;
    }
    if ((rv = osBbCardReadBlock(0, blist[0], blocks, 0)) < 0) {
	PRINTF("osBbCardReadBlock %d failed %d\n", blist[0], rv);
	goto out;
    }
    if (save != blocks[LOC]) {
	PRINTF("single bit error not corrected %x %x\n", save, blocks[LOC]);
	goto out;
    }
    if ((IO_READ(PI_FLASH_CTRL_REG) & PI_FLASH_CTRL_SBERR) == 0) {
	PRINTF("single bit error not detected, ctrl reg %x\n", IO_READ(PI_FLASH_CTRL_REG));
	goto out;
    }

    /* now go through pi dma with pio */
    IO_WRITE(PI_FLASH_CTRL_REG, PI_FLASH_CTRL_RDPH  |
				0xf << PI_FLASH_CTRL_ADPH_SHIFT |
				0 << PI_FLASH_CTRL_CMD_SHIFT |
				PI_FLASH_CTRL_WRDY |
				PI_FLASH_CTRL_ECC |
				1023);
    IO_READ(PI_DOM1_ADDR2+LOC);
    if ((IO_READ(PI_ERROR_REG) & PI_ERROR_CORRECTABLE_ECC) == 0) {
	PRINTF("single bit error not detected on pio, error reg %x\n", IO_READ(PI_ERROR_REG));
	goto out;
    }

    IO_WRITE(PI_ERROR_REG, ERR_TYPE);
    if ((IO_READ(PI_ERROR_REG) & PI_ERROR_CORRECTABLE_ECC) != 0) {
	PRINTF("single bit error bit not cleared, error reg %x\n", IO_READ(PI_ERROR_REG));
	goto out;
    }

    /* now go through pi dma with dma */
    IO_WRITE(PI_DRAM_ADDR_REG, osVirtualToPhysical(block));
    IO_WRITE(PI_CART_ADDR_REG, PI_DOM1_ADDR2);
    IO_WRITE(PI_WR_LEN_REG, BB_FL_BLOCK_SIZE-1);
    while(IO_READ(PI_STATUS_REG)&PI_STATUS_DMA_BUSY) ;
    osInvalDCache(block, BB_FL_BLOCK_SIZE);

    if ((IO_READ(PI_ERROR_REG) & PI_ERROR_CORRECTABLE_ECC) == 0) {
	PRINTF("single bit error not detected on dma, error reg %x\n", IO_READ(PI_ERROR_REG));
	goto out;
    }
    IO_WRITE(PI_ERROR_REG, ERR_TYPE);

    PRINTF("test double bit error\n");
    blocks[LOC] ^= 1;
    blocks[LOC+1] ^= 4;
    if ((rv = osBbCardWriteBlock(0, blist[0], blocks, spare)) < 0) {
	PRINTF("osBbCardWriteBlock %d failed %d\n", blist[0], rv);
	goto out;
    }
    if ((rv = osBbCardReadBlock(0, blist[0], blocks, 0)) != BBFS_ERR_FAIL) {
	PRINTF("osBbCardReadBlock %d failed %d\n", blist[0], rv);
	goto out;
    }
    if (save != (blocks[LOC]^0x1)) {
	PRINTF("double bit error not read correctly %x %x\n", save, blocks[LOC]);
	goto out;
    }
    /* now go through pi dma */
    IO_WRITE(PI_FLASH_CTRL_REG, PI_FLASH_CTRL_RDPH  |
				0xf << PI_FLASH_CTRL_ADPH_SHIFT |
				0 << PI_FLASH_CTRL_CMD_SHIFT |
				PI_FLASH_CTRL_WRDY |
				PI_FLASH_CTRL_ECC |
				1023);
#if 0
//PRINTF("do pio\n");
    IO_READ(PI_DOM1_ADDR2+LOC);
//PRINTF("done pio\n");
    if (error_shadow & PI_ERROR_UNCORRECTABLE_ECC) == 0) {
	PRINTF("double bit error not detected, error reg %x\n", IO_READ(PI_ERROR_REG));
	goto out;
    }
    if ((IO_READ(PI_STATUS_REG) & PI_STATUS_IO_BUSY) == 0) {
	PRINTF("io busy not set, status reg %x\n", IO_READ(PI_STATUS_REG));
	goto out;
    }
    IO_WRITE(PI_STATUS_REG, PI_STATUS_IO_BUSY);
    IO_WRITE(PI_STATUS_REG, 0);
#endif
//PRINTF("do dma\n");
    error_shadow = 0;
    /* now go through pi dma with dma */
    IO_WRITE(PI_DRAM_ADDR_REG, osVirtualToPhysical(block));
    IO_WRITE(PI_CART_ADDR_REG, PI_DOM1_ADDR2);
    IO_WRITE(PI_WR_LEN_REG, BB_FL_BLOCK_SIZE-1);
    while(IO_READ(PI_STATUS_REG)&PI_STATUS_DMA_BUSY) ;
    osInvalDCache(block, BB_FL_BLOCK_SIZE);
//PRINTF("done dma\n");

    if ((error_shadow & PI_ERROR_UNCORRECTABLE_ECC) == 0) {
	PRINTF("double bit error not detected on dma, error reg %x\n", IO_READ(PI_ERROR_REG));
	goto out;
    }

    __osBbCardNoEcc = 0;
    if ((rv = osBbCardWriteBlock(0, blist[0], blocks, 0)) < 0) {
	PRINTF("osBbCardWriteBlock %d failed %d\n", blist[0], rv);
	goto out;
    }
#endif
    if ((rv = osBbFClose(fd)) < 0) {
	PRINTF("osBbFClose big.c failed %d\n", rv);
	goto out;
    }
    if ((rv = osBbFDelete("big.c")) < 0) {
	PRINTF("osBbFDelete big.c failed %d\n", rv);
	goto out;
    }
    PRINTF("all tests PASSED  \n");
out:
    IO_WRITE(PI_GPIO_REG, 0|(1 << PI_GPIO_ENABLE_SHIFT));
    IO_READ(MI_SEC_MODE_REG);
    for(;;) ;
}

static void 
excepproc(char *argv) {
    OSMesgQueue piErrEventQueue;
    OSIoMesg piErrMsg;
#define NUM_MESG 2
    OSMesg piErrEventBuf[NUM_MESG];
    OSIoMesg m;

    PRINTF("\n=> excepproc...\n");
    osCreateMesgQueue(&piErrEventQueue, piErrEventBuf, NUM_MESG);
    osSetEventMesg(OS_EVENT_PI_ERR, &piErrEventQueue, (OSMesg)&piErrMsg);
    while(1) {
	IO_WRITE(MI_INTR_EMASK_REG, MI_INTR_MASK_SET_PI_ERR);
	osRecvMesg(&piErrEventQueue, (OSMesg)&m, OS_MESG_BLOCK);
	PRINTF("PI interrupt: ERROR %x STATUS %x\n", error_shadow = IO_READ(PI_ERROR_REG), IO_READ(PI_STATUS_REG));
	IO_WRITE(PI_ERROR_REG, ERR_TYPE);
	if (IO_READ(PI_STATUS_REG) & PI_STATUS_DMA_BUSY)
	    IO_WRITE(PI_STATUS_REG, PI_STATUS_DMA_BUSY);
	if (IO_READ(PI_STATUS_REG) & PI_STATUS_IO_BUSY)
	    IO_WRITE(PI_STATUS_REG, PI_STATUS_IO_BUSY);
    }
}