#include "ultra64.h"
#include <bcp.h>
#include <os_bb.h>
#include <bbdebug.h>

#include "mon.h"

char version[] = VERSION;

#define DMA_QUEUE_SIZE	200

#define PRINTF		osSyncPrintf
#define REALLY_PRINTF	PRINTF

#define PROBE_IDE_RDB() \
	(IDE_WRITE(IDE_RDB_RDATA_REG+0,0xbabe), \
	 IDE_WRITE(IDE_RDB_RDATA_REG+2,0xcafe), \
	 ((IDE_READ(IDE_RDB_RDATA_REG+0) == 0xbabe) & \
	  (IDE_READ(IDE_RDB_RDATA_REG+2) == 0xcafe)))

#define IS_SECMODE() ( IO_READ(VIRAGE0_NMS_CRSTO_0_REG) ? 1:0 )

/*
 * 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;
OSPiHandle              *handler;

//static u8 rdbSendBuf[64] __attribute__((aligned(16)));

/*
 * Local variables and routines
 */

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

u32 __osDisableInt(void);

extern void *__printfunc;
extern u32 __osBbHackFlags;
void
boot(void)
{
#ifndef USB_RDB
    IO_WRITE(MI_INTR_EMASK_REG, MI_INTR_MASK_CLR_BUT);
#endif
    /* Init the video PLL */
    __osBbVideoPllInit(OS_TV_NTSC);

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


static void
delay(void) {
    int k;
    for(k = 0; k < 100000; k++)
	;
}

#ifdef USB_RDB
extern u32 __osRdb_Usb_Active;
#endif

static void
idleproc(char *argv)		/* priority 8 */
{
#if 0
    osCreateViManager(OS_PRIORITY_VIMGR);
    osViSetMode(&osViModeTable[OS_VI_NTSC_LPN1]);
    osInitRdb(rdbSendBuf, sizeof rdbSendBuf);
#endif

    /*
     * 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, (OSPri)7);
    osStartThread(&mainThread);

    osSetThreadPri(0, OS_PRIORITY_IDLE);

#if 0
    for(;;) {
	osBbSetErrorLed(1);
	delay();
	osBbSetErrorLed(0);
	delay();
    }
#else
    for(;;) ;
#endif
}

//static OSMesgQueue	retraceMessageQ;
//static OSMesg		dummyMessage, retraceMessageBuf;

static OSBbFs fs;
static u8 block[BB_FL_BLOCK_SIZE] __attribute__((aligned(16)));
					/* block status list */
static u8 blist[BB_FL_BLOCK_SIZE] __attribute__((aligned(16)));
static u16 blockList[4096];		/* file block list */
static OSBbDirEnt dir[BB_INODE16_ENTRIES];
static u8 spare[16];
static u8 name[256], name2[256];
static u32 args[64];
static OSBbStatFs sfs;

void sum(u8* p, int n) {
    unsigned short s = 0;
    int i;
    for(i = 0; i < n; i++) {
	s += p[i];
//	PRINTF("%02x ", p[i]);
    }
    PRINTF("sum %04x\n", s);
}

u16 data[1024], res[1024];

void dbg_test(void) {
    u16 i, j;

    for(i = 0; i < 16; i++) {
	j = 1 << i;
	IDE_WRITE(IDE_RDB_DATA_REG+8, j);
	IDE_READ(IDE_RDB_DATA_REG+8);
	IDE_WRITE(IDE_RDB_DATA_REG+10, ~j);
	IDE_READ(IDE_RDB_DATA_REG+10);
	if (IDE_READ(IDE_RDB_DATA_REG+8) != j || IDE_READ(IDE_RDB_DATA_REG+10) != ~j)
	    PRINTF("err %04x %04x %04x\n", j, IDE_READ(IDE_RDB_DATA_REG+8), IDE_READ(IDE_RDB_DATA_REG+10));
    }
}

char *cause = (char *)(0x80500000-4096);
#define CAUSE_PRINTF(fmt, args...) \
    if (has_rdb) \
        PRINTF(fmt, ## args); \
    else \
        cause += sprintf(cause, fmt, ## args)

static void 
mainproc(char *argv)		{
    unsigned int hbuf[2], rbuf[2], addr, size;
    void (*f)(), *sparep;
    s32 rv = 0, i, n, len, off, fd;
    int has_rdb = 0;
    u16 s;
#if 0
    osCreateMesgQueue(&retraceMessageQ, &retraceMessageBuf, 1);
    osViSetEvent(&retraceMessageQ, dummyMessage, 1);
#endif

#ifdef USB_RDB
    if (__osBbIsBb) {
	extern u32 __osRdb_Usb_Active;
	if ((rv = osBbUsbInit()) < 0) {
	    /* now what? */
            has_rdb = 0;
	} else {
            has_rdb = __osRdb_Usb_Active;
	}
    }
#else
    has_rdb = PROBE_IDE_RDB();
#endif

    if (__osBbIsBb) {
        *cause = 0;
        if(IS_SECMODE()){
	    u32 sm = IO_READ(MI_SEC_MODE_REG);
	    if (sm & (MI_SEC_MODE_MD_TRAP|MI_SEC_MODE_BUT_TRAP|MI_SEC_MODE_TRAP|MI_SEC_MODE_FATAL|MI_SEC_MODE_TIMER|MI_SEC_MODE_APP)) {
	        /* decode secure mode trap */
	        const static char* tname[] = {
		    "application", "timer", "pi error", "mi error", "button", "module",
	        };
	        CAUSE_PRINTF("secure trap ");
	        for(i = 0; i < 6; i++) {
		    if (sm & (1<<(i+2))) {
		        CAUSE_PRINTF("%s ", tname[i]);
		        if (i == 2) {
		            CAUSE_PRINTF("0x%x edata 0x%x ", (int)IO_READ(PI_ERROR_REG), (int)IO_READ(PI_EDATA_REG));
                            IO_WRITE(PI_STATUS_REG, 3);
                            IO_WRITE(PI_ERROR_REG, 0);
                        } else if (i == 3) {
			    CAUSE_PRINTF("info 0x%x addr 0x%x data 0x%x ",  (int)IO_READ(MI_ERROR_INFO_REG), (int)IO_READ(MI_ERROR_ADDR_REG), (int)IO_READ(MI_ERROR_DATA_REG));
		        }
		    }
	        }
	        CAUSE_PRINTF("pc 0x%x  mode 0x%x\n", (int)IO_READ(VIRAGE0_RAM_START), (int)sm);
            }
        }
    }

#ifndef USB_RDB
    if (!has_rdb) {
	__printfunc=NULL;
	osBbFInit(&fs); /* sure hope this works */
	fd = osBbFOpen("launch.aes", "r");
	if (fd >= 0) {
            OSBbStatBuf sb;
            OSBbLaunchMetaData md;
            __osBbHackFlags = __osBbIsBb == 1 ? 1 : 0;
            if (osBbFStat(fd, &sb, (u16*)block, 8192) >= 0) {
                bzero(&md, sizeof(OSBbLaunchMetaData));
                md.memSize = 0x400000;
                md.tvType = OS_TV_NTSC;
                md.romBase = PI_DOM1_ADDR2;
                osBbExecFile(fd, "launch.aes", &md, block);
            }
	}
    }
#endif
    PRINTF("bbmon++ (%s) ...\n", version);
    if (__osBbIsBb) {
	u32 mfg, type;

	osBbCardInit();
	rv = osBbCardReadId(0, &mfg, &type);
	PRINTF("card id %x %x rv %x %x %x %d\n", mfg, type, rv, IO_READ(PI_BUFFER_BASE_REG), IO_READ(PI_FLASH_CONFIG_REG), osBbCardBlocks(0));
	if ((rv = osBbFInit(&fs)) < 0)
	    PRINTF("osBbFInit failed %d\n", rv);
    }

    for(;;) {
	//PRINTF("read ...\n");
	osReadHost(hbuf, sizeof hbuf);
	//PRINTF("cmd %x\n", hbuf[0]);
	rbuf[0] = 255-hbuf[0];  rbuf[1] = 0; sparep = 0;
	switch(hbuf[0]) {
	case REQ_PING:
PRINTF("ping\n");
	    osWriteHost(rbuf, sizeof rbuf);
	    break;
	case REQ_POWER_OFF:
PRINTF("power off\n");
	    osWriteHost(rbuf, sizeof rbuf);
	    IO_WRITE(PI_GPIO_REG, 0|(1 << PI_GPIO_ENABLE_SHIFT));
	    break;
#ifndef USB_RDB
	case REQ_WRITE_MEM:
	    osReadHost(&addr, sizeof addr);
PRINTF("write mem %x %d\n", addr, hbuf[1]);
	    osReadHost((void*)addr, (hbuf[1]+3)&~3);
sum((void*)addr, hbuf[1]);
	    osWriteHost(rbuf, sizeof rbuf);
	    break;
	case REQ_READ_MEM:
	    osReadHost(&addr, sizeof addr);
PRINTF("read mem %x %d\n", addr, hbuf[1]);
	    osWriteHost(rbuf, sizeof rbuf);
	    osWriteHost((void*)addr, (hbuf[1]+3)&~3);
	    break;
	case REQ_JUMP:
	    osWritebackDCacheAll();
	    osInvalICache((void*)K0BASE, 64*1024);
	    osWriteHost(rbuf, sizeof rbuf);
PRINTF("jump %x\n", hbuf[1]);
delay();
	    __osDisableInt();
	    f = (void*)hbuf[1];
	    (*f)();
	    break;
#endif	/* !USB_RDB */
	case REQ_WRITE_BLOCK_SP:
	    sparep = spare;
	    /* fall through */
	case REQ_WRITE_BLOCK:
PRINTF("write block %d\n", hbuf[1]);
	    osReadHost(block, sizeof block);
	    if (sparep) osReadHost(spare, sizeof spare);
	    if ((rv = osBbCardEraseBlock(0, hbuf[1])) < 0)
    		PRINTF("erase block %d failed %d\n", hbuf[1], rv);
	    if ((rv = osBbCardWriteBlock(0, hbuf[1], block, sparep)) < 0)
    		PRINTF("write block %d failed %d\n", hbuf[1], rv);
	    rbuf[1] = rv;
	    osWriteHost(rbuf, sizeof rbuf);
	    break;
	case REQ_READ_BLOCK_SP:
	    sparep = spare;
	    /* fall through */
	case REQ_READ_BLOCK:
PRINTF("read block %d\n", hbuf[1]);
	    rv = osBbCardReadBlock(0, hbuf[1], block, spare);
	    rbuf[1] = rv;
	    osWriteHost(rbuf, sizeof rbuf);
	    osWriteHost(block, sizeof block);
	    if (sparep)
		osWriteHost(spare, sizeof spare);
	    break;
	case REQ_READ_DIR:
	    rv = osBbFReadDir(dir, sizeof dir/sizeof dir[0]);
	    rbuf[1] = rv;
PRINTF("read dir %d\n", rv);
	    osWriteHost(rbuf, sizeof rbuf);
	    for(i = 0; i < rv; ++i) {
		osWriteHost(dir[i].name, 16);
		osWriteHost(&dir[i].size, 4);
	    }
	    osBbFStatFs(&sfs);
PRINTF("fs files %d free %d   blocks %d free %d\n", sfs.files, sfs.freeFiles, sfs.blocks, sfs.freeBlocks);
	    break;
	case REQ_WRITE_FILE:
	    osReadHost(&size, sizeof size);
	    size = (size+3)&~3;
	    if (size > sizeof(name))
		size = sizeof(name);
	    osReadHost(name, size);
	    size = (hbuf[1]+BB_FL_BLOCK_SIZE-1)&~(BB_FL_BLOCK_SIZE-1);
	    len = (hbuf[1]+3)&~3;
PRINTF("write %s len %d\n", name, hbuf[1]);
	    fd = osBbFCreate(name, 1, size);
PRINTF("create %d\n", fd);
	    rbuf[1] = fd < 0 ? fd : 0;
	    osWriteHost(rbuf, sizeof rbuf); /* first response */
	    if (fd >= 0) {
		off = 0;
		while(fd >= 0 && len > 0) {
		    n = len > sizeof block ? sizeof block : len;
		    if (n != sizeof block) bzero(block, sizeof block);
		    osReadHost(block, n);
//PRINTF("wb %d\n", off);
//sum(block, sizeof block);
if (((off>>14)&15) == 0) PRINTF(".");
		    if ((rv = osBbFWrite(fd, off, block, sizeof block)) < 0) {
			PRINTF("osBbFWrite failed %d\n", rv); 
			break;
		    }
		    len -= n;
		    off += sizeof block;
		}
		osBbFClose(fd);
		rbuf[1] = fd < 0 ? fd : rv < 0 ? rv : 0;
		osWriteHost(rbuf, sizeof rbuf); /* second response */
	    }
	    break;
	case REQ_READ_FILE:
	    size = (hbuf[1]+3)&~3;
	    if (size > sizeof(name))
		size = sizeof(name);
	    osReadHost(name, size);
PRINTF("read %s\n", name);
	    fd = osBbFOpen(name, "r");
	    if (fd < 0) {
		rbuf[1] = fd;
		len = 0;
	    } else {
		OSBbStatBuf sb;
		if ((rv = osBbFStat(fd, &sb, blockList, sizeof blockList/sizeof blockList[0])) < 0) {
		    PRINTF("osBbFStat failed %d\n", rv);
		    len = 0;
		} else
		    len = rbuf[1] = sb.size;
		PRINTF("blocks %d %d %d %d\n", blockList[0], blockList[1], blockList[2], blockList[3]);
	    }
	    osWriteHost(rbuf, sizeof rbuf);
	    off = 0;
	    while(fd >= 0 && len > 0) {
		n = len > sizeof block ? sizeof block : len;
		if ((rv = osBbFRead(fd, off, block, sizeof block)) < 0) {
		    PRINTF("osBbFRead failed %d\n", rv); 
		    break;
		}
		PRINTF(".");
		osWriteHost(block, n);
		len -= n;
		off += sizeof block;
	    }
	    PRINTF("\n");
	    osBbFClose(fd);
	    break;
	case REQ_DELETE_FILE:
	    size = (hbuf[1]+3)&~3;
	    if (size > sizeof(name))
		size = sizeof(name);
	    osReadHost(name, size);
PRINTF("delete %s\n", name);
	    rv = osBbFDelete(name);
	    rbuf[1] = rv;
	    osWriteHost(rbuf, sizeof rbuf);
	    break;
	case REQ_RENAME_FILE:
	    size = (hbuf[1]+3)&~3;
	    if (size > sizeof(name))
		size = sizeof(name);
	    osReadHost(name, size);
	    osReadHost(&len, 4);
	    len = (len+3)&~3;
	    if (len > sizeof(name2))
		len = sizeof(name2);
	    osReadHost(name2, len);
PRINTF("rename %s %s\n", name, name2);
	    rbuf[1] = osBbFRename(name, name2);
	    osWriteHost(rbuf, sizeof rbuf);
	    break;
#ifndef USB_RDB
	case REQ_EXEC_FILE:
	    size = (hbuf[1]+3)&~3;
	    if (size > sizeof(name))
		size = sizeof(name);
	    osReadHost(name, size);
	    osReadHost(&size, 4);
	    bzero(args, sizeof args);
	    if (size > sizeof(args))
	        size = sizeof(args);
	    osReadHost(args, size);
PRINTF("exec %s %s\n", name, args);
	    if (__osBbIsBb) {
		IO_WRITE(USB0_SECURE_MODE_REG, USB_SECURE_MODE_OFF);
		for(i = 0; i < size/4; i++)
		    IO_WRITE(RAMROM_APP_WRITE_ADDR+4*i, args[i]);
	    }

	    fd = osBbFOpen(name, "r");
	    if (fd < 0) {
		rbuf[1] = fd;
	    }
	    osWriteHost(rbuf, sizeof rbuf);
	    if (fd >= 0) {
                OSBbStatBuf sb;
                OSBbLaunchMetaData md;
		__osBbHackFlags = __osBbIsBb == 1 ? 1 : 0;
                if (osBbFStat(fd, &sb, (u16*)block, 8192) >= 0) {
                    bzero(&md, sizeof(OSBbLaunchMetaData));
                    md.memSize = 0x400000;
                    md.tvType = OS_TV_NTSC;
                    md.romBase = PI_DOM1_ADDR2;
                    osBbExecFile(fd, name, &md, block);
                }
            }
	    break;
#endif	/* !USB_RDB */
	case REQ_SUM_FILE:
	    size = (hbuf[1]+3)&~3;
	    if (size > sizeof(name))
		size = sizeof(name);
	    osReadHost(name, size);
PRINTF("sum %s\n", name);
	    fd = osBbFOpen(name, "r");
	    if (fd < 0) {
		rbuf[1] = fd;
		len = 0;
	    } else {
		OSBbStatBuf sb;
		if ((rv = osBbFStat(fd, &sb, blockList, sizeof blockList/sizeof blockList[0])) < 0) {
		    PRINTF("osBbFStat failed %d\n", rv);
		    len = 0;
		} else
		    len = rbuf[1] = sb.size;
	    }
	    off = 0;
	    s = 0;
	    while(fd >= 0 && len > 0) {
		n = len > sizeof block ? sizeof block : len;
		if ((rv = osBbFRead(fd, off, block, sizeof block)) < 0) {
		    PRINTF("osBbFRead failed %d\n", rv); 
		    break;
		}
		for(i = 0; i < sizeof block; i++)
		    s += block[i];
		len -= n;
		off += sizeof block;
	    }
	    rbuf[1] = s;
	    osWriteHost(rbuf, sizeof rbuf);
	    PRINTF("%04x\n", s);
	    osBbFClose(fd);
	    break;
	case REQ_SCAN_BLOCKS:
	    PRINTF("bad blocks (%d):\n", osBbCardBlocks(0));
	    for(i = 0; i < osBbCardBlocks(0); i++) {
		u8 s;
		rv = osBbCardReadBlock(0, i, block, spare);
		blist[i] =  (rv < 0) | (spare[BB_FL_BLOCK_STATUS_OFF] != 0xff);
#if 1
		osBbCardStatus(0, &s);
		PRINTF("%s", blist[i] ? "@" : ".");
#else
		if (i % 256 == 0) PRINTF(".");
		if (blist[i])
		    PRINTF("%4d ", i);
#endif
	    }
	    PRINTF("\n");
	    rbuf[1] = i;
	    osWriteHost(rbuf, sizeof rbuf);
	    osWriteHost(blist, i);
	    break;
	case REQ_FS_INIT:
	    rv = osBbFInit(&fs);
PRINTF("fsinit %d\n", rv);
	    rbuf[1] = rv;
	    osWriteHost(rbuf, sizeof rbuf);
	    break;
	case REQ_DBG_TEST:
	    //dbg_test();
	    osWriteHost(rbuf, sizeof rbuf);
	    break;
	default:
	    PRINTF("unknown request %x\n", hbuf[0]);
	    break;
	}
    }
}