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

#include "pidmabug.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 */
{
    /*
     * 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 */
}

static u8 *good = (u8*)0x80300000;
static u8 *test = (u8*)0x800ea000; 
static u8 *scratch = (u8 *)0x80400000;
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 int
doit(u32 offset, u32 doffset, u32 size)
{
    int j, fail = 0;
    int n, sum;
    extern u8 _dataSegmentRomStart;

#if 0
    PRINTF("doff %x off %x size %d\n", doffset, offset, size);
#endif
    dmaIOMessageBuf.hdr.pri      = OS_MESG_PRI_NORMAL;
    dmaIOMessageBuf.hdr.retQueue = &dmaMessageQ;
    dmaIOMessageBuf.dramAddr     = (char*) test + offset;
    dmaIOMessageBuf.devAddr      = (u32)&_dataSegmentRomStart + doffset;
    dmaIOMessageBuf.size         = size;
    /* clear memory just to be sure */
    bzero(test + offset, size);
    osWritebackDCacheAll();
    osInvalDCache(test + offset, size);
    osEPiStartDma(handler, &dmaIOMessageBuf, OS_READ);

    /* have cpu steal memory bandwidth, while dma is busy */

    sum = 0;
    for(n = 0; n < 256; n++)
	sum += scratch[n << 4];

    (void)osRecvMesg(&dmaMessageQ, NULL, OS_MESG_BLOCK);

    dmaIOMessageBuf.hdr.pri      = OS_MESG_PRI_NORMAL;
    dmaIOMessageBuf.hdr.retQueue = &dmaMessageQ;
    dmaIOMessageBuf.dramAddr     = (char*) good;
    dmaIOMessageBuf.devAddr      = (u32)&_dataSegmentRomStart + doffset;
    dmaIOMessageBuf.size         = size;
    /* clear memory just to be sure */
    bzero(good, size);
    osWritebackDCacheAll();
    osInvalDCache(good, size);
    osEPiStartDma(handler, &dmaIOMessageBuf, OS_READ);

    (void)osRecvMesg(&dmaMessageQ, NULL, OS_MESG_BLOCK);

    if (bcmp(good, test + offset, size)) {
	fail = 1;
#ifdef DIFF_PRINT
        u32 *p = good;
        u32 *q = test + offset;
        PRINTF("Bad!, dev=%08x, dram=%08x\n", dmaIOMessageBuf.devAddr, test+offset);
        for (j=0; j<size; j+=4,p++,q++) {
            if (*p != *q) {
                PRINTF("0x%08x: %08x != %08x\n", p, *p, *q);
            }
        }
#endif
    }
    return(fail);
}

static u16 tab[32*8];

static u16 cfb[640*480] __attribute__((aligned(64)));
static int count = 0;
static char buf[64];

static void
run(void)
{
    int i, n;
    u32 offset;
    u32 doffset;
    u32 len;
    u16 *tp;

    /* turn on vi to steal memory bandwidth */

    osCreateViManager(OS_PRIORITY_VIMGR);
    osViSetMode(&osViModeTable[OS_VI_NTSC_HPF2]);
    fbInit(FB_LOW_RES);
    osViSwapBuffer(cfb);
    osViSwapBuffer(cfb);
    fbClear();

    handler = osCartRomInit();
    bzero(tab, sizeof(tab));

    /* run dma with varying offsets */
    /* record failure statistics */

    for(n = 0; n < 200; n++) {
#ifdef IDE_PRINT
        if( !(n & 15)) 
            PRINTF("run %d\n", n);
#endif
      for(i = 0; i < 32*8; i++) {
	offset = (i << 3) & 0xff;
	doffset = ((i >> 4) & 0x0e) | ((i & 0xf) << 10);
	len = 256 + 16*3 - offset;
        len = 640;
	tab[i] += doit(offset, doffset, len);
	count += tab[i];
      }		
    }

    /* report failures */
    for(i = 0, tp = tab; tp < tab + 32*8; tp += 16, i += 16) {
#ifdef IDE_PRINT
	PRINTF("%03d %02x %02x %02x %02x %02x %02x %02x %02x"
		   " %02x %02x %02x %02x %02x %02x %02x %02x\n", i,
		tp[0], tp[1], tp[2], tp[3], tp[4], tp[5], tp[6], tp[7],
		tp[8], tp[9], tp[10], tp[11], tp[12], tp[13], tp[14], tp[15]
            );
#endif
    }
#ifdef IDE_PRINT    
    PRINTF("Done\n");
#endif
}

static void 
mainproc(char *argv)		{

#ifdef IDE_PRINT
    PRINTF("\n=> mainproc...\n");
#endif
    if (__osBbIsBb) {
        // IO_WRITE(PI_FLASH_CONFIG_REG, 0xa207071f);
        IO_WRITE(PI_FLASH_CONFIG_REG, 0x753e3eff);
    }
    run();
    sprintf(buf, "Count: %d", count);
    fbPrintStr(FB_WHITE, 3, 5, buf);
    osWritebackDCacheAll();
    for(;;) ;
}