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

#include "card.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)));


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

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

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

static OSMesgQueue	retraceMessageQ;
static OSMesg		dummyMessage, retraceMessageBuf;
static u8 block[1024*16];
static u16 blockList[16];
static u8 blocks[BB_FL_BLOCK_SIZE*16];

static void 
mainproc(char *argv)		{
    u32 type, mfg, pass = 0;
    u8 status;
    s32 rv, i;
    osCreateMesgQueue(&retraceMessageQ, &retraceMessageBuf, 1);
    osViSetEvent(&retraceMessageQ, dummyMessage, 1);

    PRINTF("\n=> mainproc...\n");
#if 1
retest:
    PRINTF("card init\n");
    osBbCardInit();
    PRINTF("test card ops\n");
    if ((rv == osBbCardStatus(0, &status)) < 0) {
	PRINTF("osBbCardStatus failed %d\n", rv);
	goto out;
    }
    if ((rv = osBbCardReadId(0, &mfg, &type)) < 0 || mfg != 0xec || type != 0x76) {
	PRINTF("osBbCardReadId failed %d (%x %x)\n", rv, mfg, type);
	goto out;
    }
    PRINTF("type %x mfg %x\n", type, mfg);
    if ((rv = osBbCardBlocks(0)) != 4096) {
	PRINTF("osBbBlocks failed %d\n", rv);
	goto out;
    }
    if ((rv = osBbCardEraseBlock(0, 0)) < 0) {
	PRINTF("osBbCardEraseBlock failed %d\n", rv);
	goto out;
    }
    if ((rv = osBbCardStatus(0, &status)) < 0 || status != 0xc0) {
	PRINTF("erase status %x != expected 0xc0\n", status);
	goto out;
    }
    strcpy(block, "babecafe");
    if ((rv = osBbCardWriteBlock(0, 0, block, 0)) < 0) {
	PRINTF("osBbWriteBlock failed %d\n", rv);
	goto out;
    }
    if ((rv = osBbCardStatus(0, &status)) < 0 || status != 0xc0) {
	PRINTF("write status %x != expected 0xc0\n", status);
	goto out;
    }
    bzero(block, sizeof block);
    if ((rv = osBbCardReadBlock(0, 0, block, 0)) < 0) {
	PRINTF("osBbReadBlock failed %d\n", rv);
	goto out;
    }
    if (bcmp(block, "babecafe", 9) != 0) {
	PRINTF("osBbReadBlock contents failed %02x%02x%02x%02x%02x%02x%02x%02x%02x\n", block[0], block[1], block[2], block[3], block[4], block[5], block[6], block[7], block[8]);
	goto out;
    }
    for(i = 0; i < 16; i++) blockList[i] = i;
    for(i = 0; i < BB_FL_BLOCK_SIZE*16; i++) {
	blocks[i] = 255-(i >> 10);
    }
    if ((rv = osBbCardEraseBlocks(0, blockList, 16)) < 0) {
	PRINTF("erase blocks failed %d\n", rv);
	goto out;
    }
    if ((rv = osBbCardWriteBlocks(0, blockList, 16, blocks, 0)) < 0) {
	PRINTF("write blocks failed %d\n", rv);
	goto out;
    }
    for(i = 0; i < 16; i ++) {
	s32 j;
	if ((rv = osBbCardReadBlock(0, blockList[i], block, 0)) < 0) {
	    PRINTF("read block %d failed %d\n", i, rv);
	    goto out;
	}
	for(j = 0; j < BB_FL_BLOCK_SIZE; j++) {
	    if (block[j] != 255-((i*BB_FL_BLOCK_SIZE+j)>>10)) {
		PRINTF("read mismatch (%d, %d), expected %x read %x\n", i, j, 255-((i*BB_FL_BLOCK_SIZE+j)>>10), block[j]);
		goto out;
	    }
	}
    }

    /* unplug card */
    IDE_WRITE(0x046fffe4, 1);
    PRINTF("test card unplugged ops\n");
    if ((rv = osBbCardStatus(0, &status)) != BBCARD_ERR_NO_CARD ||
	(rv = osBbCardUnhappy()) != BBCARD_ERR_NO_CARD ||
	(rv = osBbCardReadId(0, &mfg, &type)) != BBCARD_ERR_NO_CARD ||
	(rv = osBbCardBlocks(0)) != BBCARD_ERR_NO_CARD ||
	(rv = osBbCardEraseBlock(0, 0)) != BBCARD_ERR_NO_CARD ||
	(rv = osBbCardWriteBlock(0, 0, block, 0)) != BBCARD_ERR_NO_CARD ||
	(rv = osBbCardReadBlock(0, 0, block, 0)) != BBCARD_ERR_NO_CARD ||
	(rv = osBbCardEraseBlocks(0, blockList, 16)) != BBCARD_ERR_NO_CARD ||
	(rv = osBbCardWriteBlocks(0, blockList, 16, blocks, 0)) != BBCARD_ERR_NO_CARD) {
	PRINTF("osBbCardXxxx (no card) failed %d\n", rv);
	goto out;
    }

    /* plug card back in */
    IDE_WRITE(0x046fffe4, 0);
    PRINTF("test card changed ops\n");
    if ((rv = osBbCardStatus(0, &status)) != BBCARD_ERR_CHANGED ||
	(rv = osBbCardUnhappy()) != BBCARD_ERR_CHANGED ||
	(rv = osBbCardReadId(0, &mfg, &type)) != BBCARD_ERR_CHANGED ||
#if 0
	(rv = osBbCardBlocks(0)) != BBCARD_ERR_CHANGED ||
	(rv = osBbCardEraseBlock(0, 0)) != BBCARD_ERR_CHANGED ||
	(rv = osBbCardWriteBlock(0, 0, block, 0)) != BBCARD_ERR_CHANGED ||
	(rv = osBbCardReadBlock(0, 0, block, 0)) != BBCARD_ERR_CHANGED ||
	(rv = osBbCardEraseBlocks(0, blockList, 16)) != BBCARD_ERR_CHANGED ||
#endif
	(rv = osBbCardWriteBlocks(0, blockList, 16, blocks, 0)) != BBCARD_ERR_CHANGED) {
	PRINTF("osBbCardXxxx (changed) failed %d\n", rv);
	goto out;
    }

    /* reset the CHANGE flag */
    pass++;
    if (pass == 1) goto retest;
#endif

    PRINTF("test bad blocks\n");
    /* test flash failures -- assume block 17 and 19 are bad */
    osBbCardInit();
    blockList[0] = 16;
    blockList[1] = 17;
    blockList[2] = 18;
    blockList[3] = 19;
    status = -1;
    if ((rv = osBbCardEraseBlocks(0, blockList, 4)) != BBCARD_ERR_FAIL ||
	 osBbCardStatus(0, &status) != 0 ||
	 status != (NAND_STATUS_CMD_FAIL|NAND_STATUS_WRITE_OK|NAND_STATUS_READY|0x14)) {
	PRINTF("osBbCardEraseBlocks (bad block) failed %d status %x\n", rv, status);
	goto out;
    }
    status = -1;
    if ((rv = osBbCardWriteBlocks(0, blockList, 4, blocks, 0)) != BBCARD_ERR_FAIL ||
	 osBbCardStatus(0, &status) != 0 ||
	 status != (NAND_STATUS_CMD_FAIL|NAND_STATUS_WRITE_OK|NAND_STATUS_READY|0x14)) {
	PRINTF("osBbCardWriteBlocks (bad block) failed %d status %x\n", rv, status);
	goto out;
    }
    if ((rv = osBbCardEraseBlock(0, 17)) != BBCARD_ERR_FAIL ||
	 osBbCardStatus(0, &status) != 0 ||
	 status != (NAND_STATUS_CMD_FAIL|NAND_STATUS_WRITE_OK|NAND_STATUS_READY|0x4)) {
	PRINTF("osBbCardEraseBlock (bad block) failed %d status %x\n", rv, status);
	goto out;
    }
    status = -1;
    if ((rv = osBbCardWriteBlock(0, 17, blocks, 0)) != BBCARD_ERR_FAIL ||
	 osBbCardStatus(0, &status) != 0 ||
	 status != (NAND_STATUS_CMD_FAIL|NAND_STATUS_WRITE_OK|NAND_STATUS_READY|0x4)) {
	PRINTF("osBbCardWriteBlock (bad block) failed %d status %x\n", rv, status);
	goto out;
    }
    
out:
    /* power off */
    IO_WRITE(PI_GPIO_REG, 0|(1 << PI_GPIO_ENABLE_SHIFT));
    for(;;) ;
}