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

#include "access.h"
#include "access_util.h"

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


#define PRINTF osSyncPrintf

/*
 * 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 void		idleproc(char *);
static void		mainproc(char *);
static void             run();

/* to print to screen */

void
boot(void)
{
   __osBbVideoPllInit(OS_TV_NTSC);

    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

    /*
     * 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 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(0);
    osViBlack(0);
    osWritebackDCacheAll();
    osViSwapBuffer(cfb);

    run();
    for(;;) ;
}

static void run(void) 
{
  
    int i;
    int x;
    u32 addr;
    
    /* fill virage 0, 1, 2 SRAMs with data */
    /* virage 0 */
    seed = osGetCount();
    for (i=0; i<64; i++) {
        testdata[i] = myrand(); 
    }

    addr = VIRAGE0_CTRL_REG & 0xffff0000;
    x = sram_write_test(testdata, addr, 16);
    if(x != PASS){
        sprintf(buf, "FAILED secure write sram virage 0");
        printstr(white, 3, 1, buf);
        osWritebackDCacheAll();
    }
    else{
        sprintf(buf, "PASSED secure write sram virage 0");
        printstr(white, 3, 1, buf);
        osWritebackDCacheAll();
    }
    /* virage 1 */

    addr = VIRAGE1_CTRL_REG & 0xffff0000;
    x = sram_write_test(testdata, addr, 16);
    if(x != PASS){
        sprintf(buf, "FAILED secure write sram virage 1");
        printstr(white, 3, 2, buf);
        osWritebackDCacheAll();
    }
    else{
        sprintf(buf, "PASSED secure write sram virage 1");
        printstr(white, 3, 2, buf);
        osWritebackDCacheAll();
    }
    /* virage 2 */
 
    /* take it out of keep mode if it is */
    IO_WRITE(VIRAGE2_CTRL_REG, 0x0);

    for(i=0 ; i< 100; i++){
        ;
    }

    addr = VIRAGE2_CTRL_REG & 0xffff0000;
    x = sram_write_test(testdata, addr, 64);
    if(x != PASS){
        sprintf(buf, "FAILED secure write sram virage 2");
        printstr(white, 3, 3, buf);
        osWritebackDCacheAll();
    }
    else{
        sprintf(buf, "PASSED secure write sram virage 2");
        printstr(white, 3, 3, buf);
        osWritebackDCacheAll();
    }
    

    /* write test on internal sram, part by part 32 K */
    addr = INTERNAL_RAM_START;
    for(i =0; i < ISRAM_SIZE/64; i ++){
        x = sram_write_test(testdata, addr + (i*64), 64);
    
        if(x != PASS){
            sprintf(buf, "FAILED secure write ISRAM");
            printstr(white, 3, 4, buf);
            osWritebackDCacheAll();
        }
        else{
            sprintf(buf, "PASSED secure write ISRAM");
            printstr(white, 3, 4, buf);
            osWritebackDCacheAll();
        }
    }

    /* call with expect_fail = false */
    if(checkAtb(FALSE) != PASS){
        sprintf(buf, "FAILED secude mod ATB regs");
        printstr(white, 3, 5, buf);
        osWritebackDCacheAll();
    }
    else{
        sprintf(buf, "PASSED secure mod ATB regs");
        printstr(white, 3, 5, buf);
        osWritebackDCacheAll();
    }


    /* write test on internal sram, part by part 32 K */
    addr = PI_BUFFER_1_START;
    for(i =0; i < PI_BUFFER_DATA_SIZE/64; i ++){
        x = sram_write_test(testdata, addr + (i*64), 64);
    
        if(x != PASS){
            sprintf(buf, "FAILED secure write PI buf");
            printstr(white, 3, 5, buf);
            osWritebackDCacheAll();
        }
        else{
            sprintf(buf, "PASSED secure write PI buf");
            printstr(white, 3, 5, buf);
            osWritebackDCacheAll();
        }
    }
    

    /* turn on and off led, gpio access test */
    IO_WRITE(PI_GPIO_REG, 
             ((PI_GPIO_POWER_BIT| PI_GPIO_ERROR_BIT) << PI_GPIO_ENABLE_SHIFT)| PI_GPIO_POWER_BIT);
    
    /*off */
    IO_WRITE(PI_GPIO_REG, 
             ((PI_GPIO_POWER_BIT| PI_GPIO_ERROR_BIT) << PI_GPIO_ENABLE_SHIFT)| PI_GPIO_POWER_BIT | PI_GPIO_ERROR_BIT);


    /* test PI buffer access */
    

/********************/

    exitSecureMode();

/********************/

/*
    sprintf(buf, "Exited Secure Mode");
    printstr(white, 3, 8, buf);
    osWritebackDCacheAll();
*/

    addr = VIRAGE0_CTRL_REG & 0xffff0000;
#if 0
    x = sram_write_test(testdata, addr, 16);
    if(x != PASS){
        sprintf(buf, "FAIL nonsecure write sram virage 0");
        printstr(white, 3, 5, buf);
        osWritebackDCacheAll();
    }
    else{
        sprintf(buf, "PASS nonsecure write sram virage 0");
        printstr(white, 3, 5, buf);
        osWritebackDCacheAll();
    }
#endif

    for(i= 0; i < 64; i++){
        testdata[i] = 0x0;
    }

    x = sram_read_test(testdata, addr, 16);
    if (x != PASS){
        sprintf(buf, "FAIL nonsecure read sram virage 0");
        printstr(white, 3, 6, buf);
        osWritebackDCacheAll();
    }
    else{
        sprintf(buf, "PASS nonsecure read sram virage 0");
        printstr(white, 3, 6, buf);
        osWritebackDCacheAll();
    }

    addr = VIRAGE1_CTRL_REG & 0xffff0000;

    x = sram_read_test(testdata, addr, 16);
    if (x != PASS){
        sprintf(buf, "FAIL nonsecure read sram virage 1");
        printstr(white, 3, 7, buf);
        osWritebackDCacheAll();
    }
    else{
        sprintf(buf, "PASS nonsecure read sram virage 1");
        printstr(white, 3, 7, buf);
        osWritebackDCacheAll();
    }

    
    addr = VIRAGE2_CTRL_REG & 0xffff0000;

    x = sram_read_test(testdata, addr, 64);
    if (x != PASS){
        sprintf(buf, "FAIL nonsecure read sram virage 2");
        printstr(white, 3, 8, buf);
        osWritebackDCacheAll();
    }
    else{
        sprintf(buf, "PASS nonsecure read sram virage 2");
        printstr(white, 3, 8, buf);
        osWritebackDCacheAll();
    }
    
    addr = INTERNAL_RAM_START;
    /* read test on internal sram, part by part 32 K */
    for(i =0; i < ISRAM_SIZE/64; i ++){
        x = sram_read_test(testdata, addr + (i*64), 64);
    
        if(x != PASS){
            sprintf(buf, "FAIL insecure read ISRAM");
            printstr(white, 3, 9, buf);
            osWritebackDCacheAll();
        }
        else{
            sprintf(buf, "PASS insecure read ISRAM");
            printstr(white, 3, 9, buf);
            osWritebackDCacheAll();
        }
    }

       
    addr = BOOT_RAM_HI_START;
    /* read test on internal sram, part by part 32 K */
    for(i =0; i < BSRAM_SIZE/64; i ++){
        x = sram_read_test(testdata, addr + (i*64), 64);
    
        if(x != PASS){
            sprintf(buf, "FAIL insecure read BSRAM");
            printstr(white, 3, 10, buf);
            osWritebackDCacheAll();
        }
        else{
            sprintf(buf, "PASS insecure read BSRAM");
            printstr(white, 3, 10, buf);
            osWritebackDCacheAll();
        }
    }

    if(checkAes() != PASS){
        sprintf(buf, "FAIL insecure mod AES regs");
        printstr(white, 3, 11, buf);
        osWritebackDCacheAll();
    }
    else{
        sprintf(buf, "PASS insecure mod AES regs");
        printstr(white, 3, 11, buf);
        osWritebackDCacheAll();
    }

    if(checkAtb(TRUE) != PASS){
        sprintf(buf, "FAIL insecude mod ATB regs");
        printstr(white, 3, 12, buf);
        osWritebackDCacheAll();
    }
    else{
        sprintf(buf, "PASS insecure mod ATB regs");
        printstr(white, 3, 12, buf);
        osWritebackDCacheAll();
    }

    
    if(checkFlash() != PASS){
        sprintf(buf, "FAIL insecude mod flash regs");
        printstr(white, 3, 13, buf);
        osWritebackDCacheAll();
    }
    else{
        sprintf(buf, "PASS insecure mod flash regs");
        printstr(white, 3, 13, buf);
        osWritebackDCacheAll();
    }

    if(checkBufDma() != PASS){
        sprintf(buf, "FAIL insecure mod bufdma regs");
        printstr(white, 3, 14, buf);
        osWritebackDCacheAll();
    }
    else{
        sprintf(buf, "PASS insecure mod bufdma regs");
        printstr(white, 3, 14, buf);
        osWritebackDCacheAll();
    }


    /* wait to write on top of screen */
    for(i =0; i< 1000; i++){
        ;
    }

    
    if(checkIDEacc() != PASS){
        sprintf(buf, "FAIL insecure mod ide conf regs");
        printstr(white, 3, 1, buf);
        osWritebackDCacheAll();
    }
    else{
        sprintf(buf, "PASS insecure mod ide conf regs");
        printstr(white, 3, 1, buf);
        osWritebackDCacheAll();
    }

    /* write test on internal sram, part by part 32 K */
    addr = PI_BUFFER_1_START;
    for(i =0; i < PI_BUFFER_DATA_SIZE/64; i ++){
        x = sram_write_test(testdata, addr + (i*64), 64);
    
        if(x != PASS){
            sprintf(buf, "FAILED insecure write PI buf");
            printstr(white, 3, 2, buf);
            osWritebackDCacheAll();
        }
        else{
            sprintf(buf, "PASSED insecure write PI buf");
            printstr(white, 3, 2, buf);
            osWritebackDCacheAll();
        }
    }

    
    /* turn on and off led, gpio access test */
    IO_WRITE(PI_GPIO_REG, 
             ((PI_GPIO_POWER_BIT| PI_GPIO_ERROR_BIT) << PI_GPIO_ENABLE_SHIFT)| PI_GPIO_POWER_BIT);
    
    /*off */
    IO_WRITE(PI_GPIO_REG, 
             ((PI_GPIO_POWER_BIT| PI_GPIO_ERROR_BIT) << PI_GPIO_ENABLE_SHIFT)| PI_GPIO_POWER_BIT | PI_GPIO_ERROR_BIT);

    for(;;){
        ;
    }
    return 0;
}