#include <regdef.h>
#include <asm.h>
#include <R4300.h>
#include <PR/bcp.h>
#include <PR/bbdebug.h>
#include "bbnand.h"
#include "test_vector.h"

/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
   Memory allocation
   0x7FFF80 --- 0x800000: store test parameter
   0x7fff00 --- 0x7fff80: inter-communication between bd and cpu
   0x600000 --- 0x7FFF00: store sp dmem/imem code  
 * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */

#define  TEST_PARAM_ADDR   (0x7FFF80 | K1BASE)
#define  RSP_RET_ADDR      (0x7FFFC0 | K1BASE)

#define  RDP_COMM_ADDR     (0x7FFF00 | K1BASE)
#define  RSP_CODE_TEST     0x00000001
#define  RDP_TEST          0x00000010
#define  AI_TEST           0x00000100
#define  VI_TEST           0x00001000
#define  UI_TEST           0x00010000
#define  PI_TEST           0x00100000
#define  PI2_TEST          0x00200000
#define  SI_TEST           0x08000000

#define  RDP_ONE_TEST      0x00000001
#define  RDP_LAST_TEST     0x00000002
#define  RDP_TEST_DONE     0x00000010

static inline int rspcode_test();
static inline int rdp_test();
static inline int vpll_init();
static inline int ai_test();
static inline int vi_test();
static inline int ui_test();
static inline int pi_test();
static inline int pi2_test();
static inline int si_test();

int main()
{
    int err=0;
    int param;

    param  = * ((int *) TEST_PARAM_ADDR);

    if (param & RSP_CODE_TEST) 
        err += rspcode_test();

    if (param & RDP_TEST) 
        err += rdp_test();

    if (param & AI_TEST) 
        err += ai_test();

    if (param & VI_TEST) 
        err += vi_test();

    if (param & UI_TEST) 
        err += ui_test();

    if (param & PI_TEST) 
        err += pi_test();

    if (param & PI2_TEST) 
        err += pi2_test();

    if (param & SI_TEST) 
        err += si_test();

    if (err) { 
        TEST_ERROR;
    } else {
        TEST_PASS;
    }

    for (;;) ;
    return 0;
}

/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
    RSP Test 
    Reading from DDR address at RSP_CODE_START
       4 Bytes: #of Test
       4 Bytes: Padding to 8 bytes align
       4 Bytes:  test i Dmem offset(from RSP_CODE_START)
       4 Bytes:  test i Dmem size
       4 Bytes:  test i Imem offset(from RSP_CODE_START)
       4 Bytes:  test i Imem size
       ... 
       code(dmem+imem) 
   Note: offset and size should be 8 bytes aligned
 * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
#define RSP_CODE_START 0x600000
#define RSP_DEVICE_BUSY (SP_STATUS_DMA_FULL | SP_STATUS_DMA_BUSY | SP_STATUS_IO_FULL)

static inline int rspcode_test()
{
    int i, num, err=0, ret;
    int *pdata;
    
    pdata = (int *) (RSP_CODE_START | K0BASE);
    num = *pdata;
    pdata += 2;
    IO_WRITE(0x04000f84, 0);
    for (i=0; i<num; i++) {
        if (pdata[1] > 0) { /* dmem data */
            IO_WRITE(SP_MEM_ADDR_REG, SP_DMEM_START);
            IO_WRITE(SP_DRAM_ADDR_REG, pdata[0] + RSP_CODE_START);
            IO_WRITE(SP_RD_LEN_REG, pdata[1] - 1);
            do {
                 ret =  IO_READ(SP_STATUS_REG);
            } while (ret & RSP_DEVICE_BUSY);  
        }
        pdata += 2;

        if (pdata[1] > 0) { /* imem data */
            IO_WRITE(SP_MEM_ADDR_REG, SP_IMEM_START);
            IO_WRITE(SP_DRAM_ADDR_REG, pdata[0] +  RSP_CODE_START);
            IO_WRITE(SP_RD_LEN_REG, pdata[1] - 1);
            do {
                 ret =  IO_READ(SP_STATUS_REG);
            } while (ret & RSP_DEVICE_BUSY);  
        } 
        pdata += 2;

        /* set PC to 0 and run it */
        IO_WRITE(SP_PC_REG, 0);
        IO_WRITE(SP_STATUS_REG, SP_CLR_BROKE | SP_CLR_HALT);
        do {
            ret = IO_READ(SP_STATUS_REG);
        } while (! (ret & SP_STATUS_BROKE));

        do {
            ret =  IO_READ(SP_STATUS_REG);
        }  while (ret & SP_STATUS_IO_FULL);

        ret = IO_READ(0x04000f84);
        if (ret != 0x0000feed && ret != 0xfeed0000) 
            err++;
    }

    IO_WRITE(RSP_RET_ADDR, num);
    return err;
}

static inline int rdp_test()
{
    int ret, err=0;
    int dp_start, dp_count, cur;

    for (; ;) {
        do {
            ret = IO_READ(RDP_COMM_ADDR);
        } while (ret == 0);
        if (ret == RDP_LAST_TEST) break;

        IO_WRITE(RDP_COMM_ADDR, 0);   
        IO_WRITE(RDP_COMM_ADDR+4, 0);   

        dp_start = IO_READ(RDP_COMM_ADDR+8) & 0x00ffffff;
        dp_count = IO_READ(RDP_COMM_ADDR+0xC) & 0x00ffffff;

        IO_WRITE(DPC_START_REG, dp_start);
        IO_WRITE(DPC_END_REG, dp_start+dp_count);
        
        do {
            cur = IO_READ(DPC_CURRENT_REG) & 0x00ffffff;
        } while (cur < dp_start+dp_count);

        do {
            cur = IO_READ(DPC_STATUS_REG);
        } while (cur & DPC_STATUS_PIPE_BUSY);

        IO_WRITE(RDP_COMM_ADDR+4, RDP_TEST_DONE);
    } 

    return err;
}

static inline int vpll_init()
{
    IO_WRITE(MI_AVCTRL_REG, 0x01194245);
    IO_WRITE(MI_AVCTRL_REG, 0x03194244); 
    IO_WRITE(MI_AVCTRL_REG, 0x03994244);
    IO_WRITE(MI_AVCTRL_REG, 0x03194244);
    IO_WRITE(MI_AVCTRL_REG, 0x03994244);
    IO_WRITE(MI_AVCTRL_REG, 0x03194244);

    return 0;
}

#define AI_INIT(DacRate, Bitrate) { IO_WRITE(AI_DACRATE_REG, DacRate); \
                                    IO_WRITE(AI_BITRATE_REG, Bitrate); \
                                    IO_WRITE(AI_CONTROL_REG, AI_CONTROL_DMA_ON); }
#define AI_DMA(addr, len) { IO_WRITE(AI_DRAM_ADDR_REG, addr); \
                            IO_WRITE(AI_LEN_REG, len); }
#define POLLING_SET(addr, data) { do { } while (!(IO_READ(addr) & (data)));}
#define POLLING_CLEAR(addr, data) { do {} while (IO_READ(addr) & (data));}
#define __REG(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 inline int wait_cycles(int cycles)
{
    int count, diff;

    count = getcp0reg(C0_COUNT);  
    do { 
       diff = getcp0reg(C0_COUNT) - count;
    } while (diff < cycles); 

    return 0;
}

static inline int ai_test()
{
    int err = 0, ret, stat, i;

    vpll_init();
    IO_WRITE(VI_CONTROL_REG, 0); /* init vi to get proper vclk */
 
    /*  ported from iosim test */

    /* 1st part */
    AI_INIT(0x86, 0x1);      /* 003 */
    AI_DMA(0, 0x14);         /* 043 */ 
    IO_READ(8);              /* 100 */
    POLLING_CLEAR(AI_STATUS_REG, AI_STATUS_DMA_BUSY);  /* 104 */
    IO_WRITE(AI_CONTROL_REG, AI_CONTROL_DMA_OFF);      /* 102 */
    IO_WRITE(0x10, 0x010f0A05);
    if (IO_READ(0x10) != 0x010f0A05) err++;
    POLLING_SET(MI_INTR_REG, MI_INTR_AI);             
    POLLING_CLEAR(AI_STATUS_REG, AI_STATUS_DMA_BUSY);
    IO_WRITE(AI_CONTROL_REG, AI_CONTROL_DMA_ON);
    POLLING_SET(MI_INTR_REG, MI_INTR_AI);             
    IO_WRITE(AI_STATUS_REG, 0); /* clear interrupt */
    POLLING_CLEAR(MI_INTR_REG, MI_INTR_AI);             
    AI_DMA(8, 8);
    POLLING_CLEAR(AI_STATUS_REG, AI_STATUS_DMA_BUSY);

    /* 2nd part */
    wait_cycles(3000);
    AI_INIT(0x90, 0x1);
    AI_DMA(0x20, 0x10);
    IO_READ(0x10);
    IO_WRITE(AI_CONTROL_REG, AI_CONTROL_DMA_OFF);      /* 102 */
    IO_WRITE(0x10, 0x10f0a050);
    if (IO_READ(0x10) != 0x10f0a050) err++;
    POLLING_SET(MI_INTR_REG, MI_INTR_AI);             
    POLLING_CLEAR(AI_STATUS_REG, AI_STATUS_DMA_BUSY);
    IO_WRITE(AI_CONTROL_REG, AI_CONTROL_DMA_ON);
    POLLING_SET(MI_INTR_REG, MI_INTR_AI);             
    IO_WRITE(AI_STATUS_REG, 1); /* clear interrupt */
    POLLING_CLEAR(MI_INTR_REG, MI_INTR_AI);             
    POLLING_CLEAR(AI_STATUS_REG, AI_STATUS_DMA_BUSY);
     
    /* 3rd part */ 
    wait_cycles(4000);
    IO_WRITE(VI_CONTROL_REG, 0); 

    for (i=0x20000; i>=8; i>>=1) {
        AI_INIT(0x86, 0x1);
        AI_DMA(0, i); 
        err++;
        do {
            stat = IO_READ(AI_STATUS_REG);
            ret = IO_READ(AI_LEN_REG);
            if (ret <= (i-8)) {
                err--;
                break;
            }
        } while (stat & AI_STATUS_DMA_BUSY);
        AI_INIT(0, 0);
    }

    IO_WRITE(RSP_RET_ADDR, 1);
    return err;
}

#define VI_PARAM_ADDR       0x7FE000
#define VI_COMM_ADDR        (RDP_COMM_ADDR)
#define VI_COMM_START_ADDR  (VI_COMM_ADDR + 4)
#define VI_COMM_SETUP_ADDR  (VI_COMM_ADDR + 0x10)
#define VI_COMM_RUN_ADDR    (VI_COMM_ADDR + 8)
#define VI_COMM_END_ADDR    (VI_COMM_ADDR + 0xC)
#define VI_ONE_TEST         4
#define VI_TEST_DONE        8
#define VI_LAST_TEST        0x20

static inline int vi_test()
{
    int err = 0, ret, tid, a1, a2, a3, offset, exit_code;
   
    vpll_init();
    for (; ;) {
        do {
            ret = IO_READ(VI_COMM_ADDR);
        } while (ret == 0);
        if (ret == VI_LAST_TEST) break;

        IO_WRITE(VI_COMM_START_ADDR, 1);

        do { /* Wait for backdoor setup */
            ret = IO_READ(VI_COMM_SETUP_ADDR);
        } while (ret == 0);
        
        offset = VI_PARAM_ADDR;
        for (; ;) {
            tid = IO_READ(offset);
            a1 = IO_READ(offset + 4);
            exit_code = 0;

            switch (tid) {
                case 103:
                    a2 = IO_READ(offset + 8);
                    IO_WRITE(a1, a2);
                    if (a1 == 0x04400000) IO_WRITE(VI_COMM_RUN_ADDR, 1);
                    if (IO_READ(a1) != a2) err++;
                    break;
                case 100: 
                    IO_READ(a1);
                    break;
                case 102:
                    a2 = IO_READ(offset + 8);
                    IO_WRITE(a1, a2);
                    break;
                case 107:
                    a2 = IO_READ(offset + 8);
                    a3 = IO_READ(offset + 0xc);
                    do { /* read until */
                        ret = IO_READ(a1);
                    } while ((ret & a2) != a3);
                    break;
                default: 
                    exit_code = 1;
                    break;
            }
            if (exit_code)  break;
            offset += 16;
        }

        IO_WRITE(VI_COMM_END_ADDR, VI_TEST_DONE);
    }

    return err;
}

#define UI_RESET_REG_ADDR (0x700000 | K0BASE)
#define UI_REG_TEST_ADDR  (0x701000 | K0BASE)
#define UI_COMM_ADDR      RDP_COMM_ADDR
#define UI_COMM_RUN_ADDR  (UI_COMM_ADDR + 4)
#define UI_COMM_END_ADDR  (UI_COMM_ADDR + 8)
#define UI_COMM_BD0_ADDR  (UI_COMM_ADDR + 0xC)
#define UI_COMM_BD1_ADDR  (UI_COMM_ADDR + 0x10)
#define UI_ONE_TEST       0x1000
#define UI_LAST_TEST      0x4000
#define UI_PARAM_ADDR     0x7FE000

static inline int do_sram_test(unsigned int start, unsigned int end)
{
    int err=0, data, addr, k;

    /* change to cache space */
    start |= K1BASE;
    end |= K1BASE;

         /* walk through address line */
    *((unsigned int *) start) = 0x12345678;
    for (addr=start, k=4; (addr+k)<end; k<<=1) {
        data = (k & 0xff) | ((~k & 0xff) << 8);
        data |= (~data << 16);

        *((unsigned int *) (addr + k)) = data;
    }
    *((unsigned int *) (end -4)) = 0xbabecafe;

    if ( *((unsigned int *) start) != 0x12345678 )  err++;
    for (addr=start, k=4; (addr+k)<end; k<<=1) {
         data = (k & 0xff) | ((~k & 0xff) << 8);
         data |= (~data << 16);
         if (*((unsigned int *) (addr + k)) != data)  err++;
    }
    if ( *((unsigned int *) (end-4)) != 0xbabecafe )  err++;

                /* 1 Walk through data line */
    for (k=0; k<32; k++) {
        data = 1 << k;
        *((unsigned int *) start) = data;
        if ( *((unsigned int *) start) != data) err++;
    }

    return err;
}

static inline int ui_test()
{
    int err = 0;

#ifndef SIMPLE_UI_VECTOR_ONLY
    unsigned int *p, i, j, ret, offset, exit_code, tid, a1, a2, a3;

    IO_WRITE(USB1_SECURE_MODE_REG, USB_SECURE_MODE_OFF);
#endif
    IO_WRITE(USB0_SECURE_MODE_REG, USB_SECURE_MODE_OFF);

#ifndef SIMPLE_UI_VECTOR_ONLY
    p = (unsigned int *) UI_RESET_REG_ADDR;
    for (i=0, j=1; i<p[0]; i++, j+=2) {
        if (IO_READ(p[j]) != p[j+1]) {
            err++;
            break;
        }
    } /* Check for reg coming out reset */

    /* Ui sram test */
    err += do_sram_test(USB0_BDT_ADDR, USB0_BDT_ADDR+512);
    err += do_sram_test(USB1_BDT_ADDR, USB1_BDT_ADDR+512);

    p = (unsigned int *) UI_REG_TEST_ADDR;
    for (i=0, j=1; i<p[0]; i++, j+=3) {
        IO_WRITE(p[j], p[j+1]);
        ret = IO_READ(p[j]);
        if ((ret & p[j+2]) != (p[j+1] & p[j+2])) {
            err++;
            break;
        }
    }  /* Register tests */

        /* set up BDT */
    IO_WRITE(USB0_BDT_PAGE_01_REG, (USB0_BDT_ADDR>>8)&0xff);
    IO_WRITE(USB0_BDT_PAGE_02_REG, (USB0_BDT_ADDR>>16)&0xff);
    IO_WRITE(USB0_BDT_PAGE_03_REG, (USB0_BDT_ADDR>>24)&0xff);

    IO_WRITE(USB1_BDT_PAGE_01_REG, (USB1_BDT_ADDR>>8)&0xff);
    IO_WRITE(USB1_BDT_PAGE_02_REG, (USB1_BDT_ADDR>>16)&0xff);
    IO_WRITE(USB1_BDT_PAGE_03_REG, (USB1_BDT_ADDR>>24)&0xff);

    /* enable UI interrupt here */
    IO_WRITE(MI_INTR_EMASK_REG, MI_INTR_MASK_SET_USB0);
    IO_WRITE(MI_INTR_EMASK_REG, MI_INTR_MASK_SET_USB1);
 
#endif

    IO_WRITE(USB0_CTL_REG, 0x1B);
    IO_WRITE(USB0_OTG_CTRL_REG, 0x3C);

#ifndef SIMPLE_UI_VECTOR_ONLY
    IO_WRITE(USB1_CTL_REG, 0x1B);
    IO_WRITE(USB1_OTG_CTRL_REG, 0x3C);
#endif

    wait_cycles(8000);

    IO_WRITE(USB0_CTL_REG, 0x9);
    IO_WRITE(USB0_ERR_STAT_REG, 0xff);  /* clear error */
    IO_WRITE(USB0_INT_STAT_REG, 0x0e);
    IO_WRITE(USB0_INT_ENB_REG, 0x0e);

#ifndef SIMPLE_UI_VECTOR_ONLY
    IO_WRITE(USB1_CTL_REG, 0x9);
    IO_WRITE(USB1_ERR_STAT_REG, 0xff);  /* clear error */
    IO_WRITE(USB1_INT_STAT_REG, 0x0e);
    IO_WRITE(USB1_INT_ENB_REG, 0x0e);
    POLLING_SET(USB1_INT_STAT_REG, 0x4); /* waiting for SOF interrupt*/            
#endif
    POLLING_SET(USB0_INT_STAT_REG, 0x4); /* waiting for SOF interrupt*/            

#ifndef SIMPLE_UI_VECTOR_ONLY
    /* All ui transactions */
    IO_WRITE(UI_COMM_END_ADDR, 1);  /* Ask for starting */
    for ( ; ; )  {
        do {
            ret = IO_READ(UI_COMM_ADDR);
        } while (ret == 0);
        if (ret == UI_LAST_TEST) break;
        IO_WRITE(UI_COMM_ADDR, 0);
     
        for (offset=UI_PARAM_ADDR; ; offset+=16) {
            tid = IO_READ(offset);
            a1 = IO_READ(offset + 4);
            exit_code = 0;

            switch (tid) {
                case 103:
                    a2 = IO_READ(offset + 8);
                    IO_WRITE(a1, a2);
                    if (IO_READ(a1) != a2) err++;
                    break;
                case 100: 
                    IO_READ(a1);
                    break;
                case 102:
                    a2 = IO_READ(offset + 8);
                    if (a1 == USB0_TOKEN_REG) IO_WRITE(UI_COMM_BD0_ADDR, 1);
                    if (a1 == USB1_TOKEN_REG) IO_WRITE(UI_COMM_BD1_ADDR, 1);
                    IO_WRITE(a1, a2);
                    break;
                case 109:
                    a2 = IO_READ(offset + 8);
                    IO_WRITE(a1, a2 & IO_READ(a1));
                    break;
                case 107:
                    a2 = IO_READ(offset + 8);
                    a3 = IO_READ(offset + 0xc);
                    do { /* read until */
                        ret = IO_READ(a1);
                    } while ((ret & a2) != a3);
                    break;
                default: 
                    exit_code = 1;
                    break;
            }
            if (exit_code)  break;
        }
        IO_WRITE(UI_COMM_END_ADDR, 1);   /* One test is done */
    }

#endif
    IO_WRITE(RSP_RET_ADDR, 1);
    return err;
}

/** PI FLASH Test **/
#define PI_AES_ADDR       (0x580000 | K0BASE)
#define PI_AES_IV_ADDR    (0x58F000 | K0BASE)
#define PI_RAND_TAB_ADDR  (0x590000 | K0BASE)

static void atb_setup(unsigned int base, unsigned int fladdr, unsigned int size) 
{
    int i, j;

    IO_WRITE(PI_ATBU_REG, PI_ATBU_IV |                          /* IV */
                          (0<<PI_ATBU_DEV_SHIFT) |              /* device */
                          PI_ATBU_PERM_PIO | PI_ATBU_PERM_DMA | /* perm */
                          (0<<PI_ATBU_SIZE_SHIFT));             /* size */
    IO_WRITE(PI_ATB_BUFFER_LO_REG,
             0 | (((base>>14)-1) << PI_ATBL_VADDR_SHIFT) );
    IO_WRITE(PI_ATBU_REG, (0<<PI_ATBU_DEV_SHIFT) |              /* device */
                          PI_ATBU_PERM_PIO | PI_ATBU_PERM_DMA | /* perm */
                          (0<<PI_ATBU_SIZE_SHIFT));             /* size */
    for(i = 1; i < size/(16*1024)+1; i++) {
        IO_WRITE(PI_ATB_BUFFER_LO_REG+i*4,
                 ((fladdr+i-1) << PI_ATBL_PADDR_SHIFT) |
                 (((base+(i-1)*16*1024)>>14) << PI_ATBL_VADDR_SHIFT) );
    }
    /* map rest of entries with same last entry */
    for(j = i; j < PI_ATB_NUM_ENTRIES; j++)
        IO_WRITE(PI_ATB_BUFFER_LO_REG+j*4,
                 ((fladdr+i-1) << PI_ATBL_PADDR_SHIFT) |
                 (((base+(i-1)*16*1024)>>14) << PI_ATBL_VADDR_SHIFT) );
}

static inline int pi_test()
{
    int err = 0, i, j;
    unsigned int *aes_ekey, *aes_iv, *tab;

#ifdef TOSHIBA_FLASH
    IO_WRITE(PI_FLASH_CONFIG_REG, 7 << 28 |  // end of cycle time - 2
                                  5 << 24 |  // read data sample time - 1
                                  0x3e << 16 | // RE active time
                                  0x1f << 8 |  // WE active time
                                  0x3f << 0);  // CLE/ALE active time
#else
    IO_WRITE(PI_FLASH_CONFIG_REG, 4 << 28 |  // end of cycle time - 2
                                  3 << 24 |  // read data sample time - 1
                                  15 << 16 | // RE active time
                                  15 << 8 |  // WE active time
                                  63 << 0);  // CLE/ALE active time
#endif

    /* compatible mode dma */
    aes_ekey = (unsigned int *) PI_AES_ADDR;
    aes_iv = (unsigned int *) PI_AES_IV_ADDR;
    tab = (unsigned int *) PI_RAND_TAB_ADDR;
    
    IO_WRITE(PI_FLASH_CTRL_REG, PI_FLASH_CTRL_RDPH  |
                                0xf << PI_FLASH_CTRL_ADPH_SHIFT |
                                0x0 << PI_FLASH_CTRL_CMD_SHIFT |
                                PI_FLASH_CTRL_WRDY |
                                0 << PI_FLASH_CTRL_BUF_SHIFT |
                                0 << PI_FLASH_CTRL_DEV_SHIFT |
                                PI_FLASH_CTRL_ECC |
                                0x3ff);

        /* setup aes key */
    for(i = 0; i < 11; i++)
        for(j = 0; j < 4; j++)
            IO_WRITE(PI_AES_EKEY_REG+(i*4+j)*4, aes_ekey[(10-i)*4+j]);

    for(i = 0; i < 4; i++)
        IO_WRITE(PI_AES_INIT_REG+4*i, aes_iv[i]); 

    IO_WRITE(PI_AES_CTRL_REG, 0);
    atb_setup(PI_DOM1_ADDR1, 0, 32*1024);
    invalDCache((void*)K0BASE, 32*1024);

    for (i = 1; i < tab[0]; i+=2) {
        IO_WRITE(PI_DRAM_ADDR_REG, K0_TO_PHYS(K0BASE)+tab[i]);
        IO_WRITE(PI_CART_ADDR_REG, PI_DOM1_ADDR1+tab[i]); 
        IO_WRITE(PI_WR_LEN_REG, tab[i+1]-1); 

        while(IO_READ(PI_STATUS_REG) & (PI_STATUS_IO_BUSY | PI_STATUS_DMA_BUSY)) {
            if (IO_READ(PI_STATUS_REG) & PI_STATUS_ERROR)  {
               err=100;
               goto out;
            }
        }
    }

#ifdef CPU_CHECKSUM         
    for(j = 0; j < (32*1024)/4; j++) {
        unsigned x = *((volatile unsigned *)K0BASE+j);
        csum += (x >> 24) + ((x >> 16)&0xff) + ((x >> 8)&0xff) + (x&0xff);
    }
    IO_WRITE(PI_IDE3_BASE_REG, csum);
    if (csum != 0x3844) err++;
#endif

      /* Two ide write */
    IO_WRITE(PI_IDE3_BASE_REG, 0);
    IO_WRITE(PI_IDE3_BASE_REG, 0xffff);

      /* Gpio signal */
    IO_WRITE(PI_GPIO_REG, (0x0<<PI_GPIO_ENABLE_SHIFT) | (0x0<<PI_GPIO_DATA_SHIFT));
    IO_WRITE(PI_GPIO_REG, (0xf<<PI_GPIO_ENABLE_SHIFT) | (0xf<<PI_GPIO_DATA_SHIFT));

       /* erase old value in PI buffer */
    for (i=0; i<512; i+=4) 
        IO_WRITE(PI_BUFFER_0_START+i, i);

    invalDCache((void*)(K0BASE+0x1000), 512);
    IO_WRITE(PI_DRAM_ADDR_REG, 0x1000);
    IO_WRITE(PI_CART_ADDR_REG, PI_BUFFER_0_START);
    IO_WRITE(PI_DMA_BUFFER_RD_REG, 511);
    do {
    } while (IO_READ(PI_STATUS_REG) & PI_STATUS_DMA_BUSY);

    tab = (unsigned int *) (K0BASE | 0x1000);
    for (i=j=0; i<512; i+=4, j++) 
        if (IO_READ(PI_BUFFER_0_START+i) != tab[j]) {
            err++;
            break;
        }

out:
    IO_WRITE(RSP_RET_ADDR, 1);
    return err;
}

static inline int flash_status(unsigned dev, unsigned offset, unsigned buffer) 
{
    IO_WRITE(PI_FLASH_ADDR_REG, offset << PI_FLASH_PAGE_ADDR_SHIFT);
    IO_WRITE(PI_FLASH_CTRL_REG, PI_FLASH_CTRL_START |
                                PI_FLASH_CTRL_RDPH  |
                                NAND_CMD_STATUS << PI_FLASH_CTRL_CMD_SHIFT |
                                buffer << PI_FLASH_CTRL_BUF_SHIFT |
                                dev << PI_FLASH_CTRL_DEV_SHIFT |
                                1);
    do {
        if (IO_READ(MI_EINTR_REG)&MI_INTR_MD) return -1;
    } while(IO_READ(PI_FLASH_CTRL_REG)&PI_FLASH_CTRL_BUSY);
    return IO_READ(PI_BUFFER_BASE_REG+buffer*512+offset) >> 24;
}

static inline int erase_block(unsigned dev, unsigned int addr) 
{
    IO_WRITE(PI_FLASH_ADDR_REG, addr << PI_FLASH_PAGE_ADDR_SHIFT);
    IO_WRITE(PI_FLASH_CTRL_REG, PI_FLASH_CTRL_START |
                                0xe << PI_FLASH_CTRL_ADPH_SHIFT |
                                NAND_CMD_ERASE_0 << PI_FLASH_CTRL_CMD_SHIFT |
                                dev << PI_FLASH_CTRL_DEV_SHIFT |
                                PI_FLASH_CTRL_MCMD);
    do {
        if (IO_READ(MI_EINTR_REG)&MI_INTR_MD) return 1;
    } while(IO_READ(PI_FLASH_CTRL_REG)&PI_FLASH_CTRL_BUSY);
    IO_WRITE(PI_FLASH_CTRL_REG, PI_FLASH_CTRL_START |
                                0 << PI_FLASH_CTRL_ADPH_SHIFT |
                                NAND_CMD_ERASE_1 << PI_FLASH_CTRL_CMD_SHIFT |
                                PI_FLASH_CTRL_WRDY |
                                dev << PI_FLASH_CTRL_DEV_SHIFT);
    do {
        if (IO_READ(MI_EINTR_REG)&MI_INTR_MD) return 1;
    } while(IO_READ(PI_FLASH_CTRL_REG)&PI_FLASH_CTRL_BUSY);
    return flash_status(dev, 0, 0) != (NAND_STATUS_CMD_PASS|NAND_STATUS_READY|NAND_STATUS_WRITE_OK);
}

static inline int read_page(unsigned dev, unsigned addr, int which_buf) 
{
    IO_WRITE(PI_FLASH_ADDR_REG, addr << PI_FLASH_PAGE_ADDR_SHIFT);
    IO_WRITE(PI_FLASH_CTRL_REG, PI_FLASH_CTRL_START |
                                PI_FLASH_CTRL_RDPH  |
                                0xf << PI_FLASH_CTRL_ADPH_SHIFT |
                                NAND_CMD_READ_0 << PI_FLASH_CTRL_CMD_SHIFT |
                                PI_FLASH_CTRL_WRDY |
                                which_buf << PI_FLASH_CTRL_BUF_SHIFT |
                                dev << PI_FLASH_CTRL_DEV_SHIFT |
                                PI_FLASH_CTRL_ECC |
                                528);
    do {
        if (IO_READ(MI_EINTR_REG)&MI_INTR_MD) return 1;
    } while(IO_READ(PI_FLASH_CTRL_REG)&PI_FLASH_CTRL_BUSY);
    return (IO_READ(PI_FLASH_CTRL_REG) & PI_FLASH_CTRL_DBERR) ? 1 : 0;
}

static inline int write_page(unsigned dev, unsigned addr, int which_buf) 
{
    IO_WRITE(PI_FLASH_ADDR_REG, addr << PI_FLASH_PAGE_ADDR_SHIFT);
    IO_WRITE(PI_FLASH_CTRL_REG, PI_FLASH_CTRL_START |
                                PI_FLASH_CTRL_WDPH  |
                                0xf << PI_FLASH_CTRL_ADPH_SHIFT |
                                NAND_CMD_DATA_INPUT << PI_FLASH_CTRL_CMD_SHIFT |                                which_buf << PI_FLASH_CTRL_BUF_SHIFT |
                                dev << PI_FLASH_CTRL_DEV_SHIFT |
                                PI_FLASH_CTRL_ECC |
                                528);
    do {
        if (IO_READ(MI_EINTR_REG)&MI_INTR_MD) return 1;
    } while(IO_READ(PI_FLASH_CTRL_REG)&PI_FLASH_CTRL_BUSY);
    IO_WRITE(PI_FLASH_CTRL_REG, PI_FLASH_CTRL_START |
                                NAND_CMD_PROGRAM << PI_FLASH_CTRL_CMD_SHIFT |
                                PI_FLASH_CTRL_WRDY |
                                dev << PI_FLASH_CTRL_DEV_SHIFT);
    do {
        if (IO_READ(MI_EINTR_REG)&MI_INTR_MD) return 1;
    } while(IO_READ(PI_FLASH_CTRL_REG)&PI_FLASH_CTRL_BUSY);
    return flash_status(dev, 0, which_buf) != (NAND_STATUS_CMD_PASS|NAND_STATUS_WRITE_OK|NAND_STATUS_READY);
}

static inline int regrw(int reg, int v)
{
    IO_WRITE(reg, v);
    return (IO_READ(reg) != v) ? 1 : 0;
}

static inline int pi2_test()
{
    int err = 0, i;

    IO_WRITE(PI_FLASH_CONFIG_REG, 7 << 28 |  // end of cycle time - 2
                                  5 << 24 |  // read data sample time - 1
                                  0x3e << 16 | // RE active time
                                  0x1f << 8 |  // WE active time
                                  0x3f << 0);  // CLE/ALE active time

    if (erase_block(0, 0x1fe)) err++;
    if (read_page(0, 1, 0)) err++;
    if (write_page(0, 0x1ff, 0)) err++;

         /* overwrite data in pi buffer 1 */
    for (i = 0; i < 512/4; i++) IO_WRITE(PI_BUFFER_1_START+i*4, 0);
    for(i = 0; i < 16/4; i++) IO_WRITE(PI_BUFFER_1_OOB_START+i*4, 0);
    if (read_page(0, 0x1ff, 1)) err++;

        /* compare the result */
    for (i = 4; i < 512/4; i++) 
        if (IO_READ(PI_BUFFER_1_START+i*4) != IO_READ(PI_BUFFER_0_START+i*4))
            err++;

    for (i = 0; i < 16/4; i++) 
        if (IO_READ(PI_BUFFER_1_OOB_START+i*4) != 
            IO_READ(PI_BUFFER_0_OOB_START+i*4))
            err++;

#ifdef TOSHIBA_FLASH
    if (erase_block(0, 1)) err++; 
    if (write_page(0, 1, 1)) err++;
    for (i = 0; i < 512/4; i++) IO_WRITE(PI_BUFFER_0_START+i*4, 0);
    for(i = 0; i < 16/4; i++) IO_WRITE(PI_BUFFER_0_OOB_START+i*4, 0);
    if (read_page(0, 1, 0)) err++;
#endif

            /* compare the result */
    for (i = 4; i < 512/4; i++) 
        if (IO_READ(PI_BUFFER_1_START+i*4) != IO_READ(PI_BUFFER_0_START+i*4))
            err++;

    for (i = 0; i < 16/4; i++) 
        if (IO_READ(PI_BUFFER_1_OOB_START+i*4) != 
            IO_READ(PI_BUFFER_0_OOB_START+i*4))
            err++;
 
    err += do_sram_test(0x4610420, 0x4610800);

    err += regrw(PI_DRAM_ADDR_REG, 0x03FFFFFC);
    err += regrw(PI_DRAM_ADDR_REG, 0);
    err += regrw(PI_CART_ADDR_REG, 0x3FFFFFFC);
    err += regrw(PI_CART_ADDR_REG, 0);
    
    IO_WRITE(PI_AES_CTRL_REG, 0xFFFFFFFF);
    IO_WRITE(PI_AES_CTRL_REG, 0);

    err += regrw(PI_FLASH_CONFIG_REG, 0xFFFFFFFF);
    err += regrw(PI_FLASH_CONFIG_REG, 0);

    IO_WRITE(PI_FLASH_CTRL_REG, 0xFFFFFFFF);
    IO_WRITE(PI_FLASH_CTRL_REG, 0xFFFFFFF0);

    IO_WRITE(PI_STATUS_REG, 0);
    IO_WRITE(PI_STATUS_REG, 0xFFFFFFFF);

    for (i=2; i<0x20000; i<<=1) { /*Write IDE space */
        IDE_WRITE(PI_IDE0_BASE_REG+i, 0xA5A5);
        IDE_WRITE(PI_IDE1_BASE_REG+i, 0xF00F);
        IDE_WRITE(PI_IDE2_BASE_REG+i, 0xBABE);
        IDE_WRITE(PI_IDE3_BASE_REG+i, 0xDEAD);
    }

    IO_WRITE(RSP_RET_ADDR, 1);
    return err;
}

#define  SI_COMM_ADDR     0x7fff00
#define  SI_COMM_END_ADDR 0x7fff20
#define  SI_ONE_TEST      0x1
#define  SI_LAST_TEST     0x2
#define  SI_TEST_DONE     0x10
#define  SI_PARAM_ADDR    0x7C0000

static inline int si_test()
{
    int err = 0, ret, tid, a1, a2, a3, exit_code, offset;

    /* Take jctrl out of reset */
    IO_WRITE(SI_CTRL_REG, SI_CTRL_JCRST);
    wait_cycles(40000); 
    IO_WRITE(SI_CTRL_REG, 0);

    for ( ; ; )  {
        do {
            ret = IO_READ(SI_COMM_ADDR);
        } while (ret == 0);

        if (ret == SI_LAST_TEST) break;
        IO_WRITE(SI_COMM_ADDR, 0); /* reset SI test */

        for (offset=SI_PARAM_ADDR; ; offset+=16) {
            tid = IO_READ(offset);
            a1 = IO_READ(offset + 4);
            exit_code = 0;

            switch (tid) {
                case 103:
                    a2 = IO_READ(offset + 8);
                    IO_WRITE(a1, a2);
                    if (IO_READ(a1) != a2) err++;
                    break;
                case 100:
                    IO_READ(a1);
                    break;
                case 101:
                    a2 = IO_READ(offset + 8);
                    if (IO_READ(a1) != a2) err++;
                    break;
                case 102:
                    a2 = IO_READ(offset + 8);
                    IO_WRITE(a1, a2);
                    break;
                case 109:
                    a2 = IO_READ(offset + 8);
                    IO_WRITE(a1, a2 & IO_READ(a1));
                    break;
                case 209:
                    a2 = IO_READ(offset + 8);
                    IO_WRITE(a1, a2 | IO_READ(a1));
                    break;
                case 107:
                    a2 = IO_READ(offset + 8);
                    a3 = IO_READ(offset + 0xc);
                    do { /* read until */
                        ret = IO_READ(a1);
                    } while ((ret & a2) != a3);
                    break;
                case 0x14:
                    wait_cycles(a1);
                    break;
                default:
                    exit_code = 1;
                    break;
            }
            if (exit_code)  break;
        }
        IO_WRITE(SI_COMM_END_ADDR, 1);   /* One test is done */
    }

    IO_WRITE(RSP_RET_ADDR, 1); 
    return err;
}