#include "ultra64.h"
#include "os_bb.h"
#include "bcp.h"

#include "pidma.h"
#include "nu64sys.h"
#include "graph.h"


#define DMA_QUEUE_SIZE	200
#define PRINTF		osSyncPrintf
//#define IDE_PRINT

extern u32 __osBbIsBb;

/*
 * 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], dmaMessageBuf;
static OSMesgQueue      PiMessageQ, dmaMessageQ;

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);
#if	SCREEN_LOW
    osViSetMode(&osViModeTable[OS_VI_NTSC_LPN1]);
#else
    osViSetMode(&osViModeTable[OS_VI_NTSC_HPF1]);
#endif

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

    osCreateMesgQueue(&dmaMessageQ, &dmaMessageBuf, 1);
    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 */
}

#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 u8 testbuf[1024*1024] __attribute__((aligned(16)));
static OSPiHandle* handler;
static OSIoMesg dmaIOMessageBuf;

u32
sum(u8* p, u32 len) {
    u32 i;
    u32 s = 0;
    for(i = 0; i < len; i++)
	s += p[i];
    return s;
}

static void
run(void) {
    int i;
    extern u8 _dataSegmentRomStart, _dataSegmentRomEnd;
    u32 t1, t2, s0 = 0, s, fail = 0;
    handler = osCartRomInit();
    for(i = 0; i < 100000; i++) {
#ifndef IDE_PRINT
        char buf[512];
#endif
	dmaIOMessageBuf.hdr.pri      = OS_MESG_PRI_NORMAL;
	dmaIOMessageBuf.hdr.retQueue = &dmaMessageQ;
	dmaIOMessageBuf.dramAddr     = (char*)testbuf;
	dmaIOMessageBuf.devAddr      = (u32)&_dataSegmentRomStart;
	dmaIOMessageBuf.size         = &_dataSegmentRomEnd-&_dataSegmentRomStart;
#if 1
	/* clear memory just to be sure */
	bzero(testbuf, sizeof testbuf);
	osWritebackDCacheAll();
#endif
	osInvalDCache(testbuf, sizeof testbuf);
	t1 = getcp0reg(C0_COUNT);
        osEPiStartDma(handler, &dmaIOMessageBuf, OS_READ);
#ifdef IDE_PRINT
	//PRINTF("wait dma: %d\n", i);
#else
        sprintf(buf, "wait dma: %d", i);
        printstr(white, 3, 5, buf);
#endif
        (void)osRecvMesg(&dmaMessageQ, NULL, OS_MESG_BLOCK);
	t2 = getcp0reg(C0_COUNT);
	s = sum(testbuf, sizeof testbuf);
	if (!s0) s0 = s;
	if (s != s0) {
	    u32 j;
#ifdef IDE_PRINT
	    PRINTF("sum failed, expected %08x computed %08x\n", s0, s);
#else
	    sprintf(buf, "fail: e %08x c %08x", s0, s);
            printstr(white, 3, 7, buf);
	    sprintf(buf, "pi_error: %08x", IO_READ(PI_ERROR_REG));
            printstr(white, 3, 8, buf);
#endif
	    for(j = 0; j < dmaIOMessageBuf.size; j+=4)  {
		u32 x;
		u32 y = *((u32*)(testbuf+j));
		osEPiReadIo(handler, dmaIOMessageBuf.devAddr+j, &x);
		if (x != y) {
#ifdef IDE_PRINT
		    PRINTF("woff %08x e %08x g %08x\n", j, x, y);
#else
		    sprintf(buf, "woff %08x e %08x g %08x", j, x, y);
		    printstr(white, 3, 9, buf);
#endif
		}
	    }
	    fail++;
	}
#ifdef IDE_PRINT
	PRINTF("%.2f MB/s sum %x pass %ld fail %ld\n", sizeof(testbuf)/(float)OS_CYCLES_TO_USEC(t2-t1)*80.f/62.5f, s, i-fail+1, fail);
#else
	sprintf(buf, "%.2f MB/s sum %x fail %ld", sizeof(testbuf)/(float)OS_CYCLES_TO_USEC(t2-t1)*80.f/62.5f, s, fail);
        printstr(white, 3, 6, buf);
#endif
    }
    IO_READ(MI_SEC_MODE_REG);
}

static u16 cfb[SCREEN_WD*SCREEN_HT] __attribute__((aligned(64)));
static void clear(u16 bg) {
    int i;
    for (i = 0; i < SCREEN_WD*SCREEN_HT; ++i) {
            cfb[i] = bg;
    }
}

static void 
mainproc(char *argv)		{
    osCreateMesgQueue(&retraceMessageQ, &retraceMessageBuf, 1);
    osViSetEvent(&retraceMessageQ, dummyMessage, 1);
    osViBlack(1);
    osViSwapBuffer(cfb);
    clear(0x0);
    osViBlack(0);
    osWritebackDCacheAll();
    osViSwapBuffer(cfb);

    if (__osBbIsBb) {
      IO_WRITE(PI_FLASH_CONFIG_REG, 0xa207071f);
    }
    run();
    for(;;) ;
}