#include "ultra64.h"
#include "os_internal.h"
#include "PR/bcp.h"

#include "soctest.h"


#define DMA_QUEUE_SIZE	200
#define PRINTF		osSyncPrintf
#define MSG_FAULT	0x10
#define BACKDOOR_PRINT      0x046fffe0

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

/*
 * Local variables and routines
 */

static void		idleproc(char *);
static void		mainproc(char *);
inline void 	message(const char* s);

void
boot(void)
{
	message("ROM Start ...\n");
    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);

    /*
     * 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 */
}

inline void message(const char* s) 
{
	while(*s) {
		IDE_WRITE(BACKDOOR_PRINT, *s);
		s++;
	}
}


static void
memtest(u8* p, int n)
{
    int i;
    for(i = 0; i < n; i++) {
    	p[i] = ~i;
    }
    for(i = 0; i < n; i++) {
        if (p[i] != ((~i)&0xff))
	    PRINTF("v0[%d] result %02x expected %02x\n", i, p[i], (~i)&0xff);
    }
}

static void 
mainproc(char *argv)			/* priority of 7 */
{
    PRINTF("\n=> mainproc...\n");

if (0) {
u32 c = __osGetConfig();
__osSetConfig(c|02);
PRINTF("config %x\n", __osGetConfig());
}
    /* assume in secure mode */

    PRINTF("PI ...\n");
    PRINTF("=> gpio  ...\n");
    /* power on */
    IO_WRITE(PI_GPIO_REG, PI_GPIO_POWER_BIT|(1 << PI_GPIO_ENABLE_SHIFT));
    /* power off */
    //IO_WRITE(PI_GPIO_REG, 0|(1 << PI_GPIO_ENABLE_SHIFT));
    /* led on */
    //IO_WRITE(PI_GPIO_REG, PI_GPIO_ERROR_BIT|(2 << PI_GPIO_ENABLE_SHIFT));
    /* led off */
    //IO_WRITE(PI_GPIO_REG, PI_GPIO_ERROR_BIT|(2 << PI_GPIO_ENABLE_SHIFT));
    /* box id */
    PRINTF("=> box id 0x%04x\n", IO_READ(PI_GPIO_REG) >> PI_GPIO_ID_SHIFT);

    PRINTF("MI ...\n");
    PRINTF("=> virage 0 ...\n");
    memtest((u8*)PHYS_TO_K1(VIRAGE0_RAM_START), VIRAGE0_RAM_END-VIRAGE0_RAM_START);
    memtest((u8*)PHYS_TO_K0(VIRAGE0_RAM_START), VIRAGE0_RAM_END-VIRAGE0_RAM_START);

    PRINTF("=> virage 1 ...\n");
    memtest((u8*)PHYS_TO_K1(VIRAGE1_RAM_START), VIRAGE1_RAM_END-VIRAGE1_RAM_START);
    memtest((u8*)PHYS_TO_K0(VIRAGE1_RAM_START), VIRAGE1_RAM_END-VIRAGE1_RAM_START);

    PRINTF("=> virage 2 ...\n");
	IO_WRITE(VIRAGE2_CTRL_REG, 0x0);
    memtest((u8*)PHYS_TO_K1(VIRAGE2_RAM_START), VIRAGE2_RAM_END-VIRAGE2_RAM_START);
    memtest((u8*)PHYS_TO_K0(VIRAGE2_RAM_START), VIRAGE2_RAM_END-VIRAGE2_RAM_START);

    PRINTF("=> isram ...\n");
    memtest((u8*)PHYS_TO_K1(INTERNAL_RAM_START), INTERNAL_RAM_END-INTERNAL_RAM_START);
    memtest((u8*)PHYS_TO_K0(INTERNAL_RAM_START), INTERNAL_RAM_END-INTERNAL_RAM_START);

    PRINTF("=> bram ...\n");
    memtest((u8*)PHYS_TO_K1(BOOT_RAM_HI_START), BOOT_RAM_HI_END-BOOT_RAM_HI_START);
    memtest((u8*)PHYS_TO_K0(BOOT_RAM_HI_START), BOOT_RAM_HI_END-BOOT_RAM_HI_START);
    
    /*XXX? brom*/
    PRINTF("=> done\n");

    /* power off */
    IO_WRITE(PI_GPIO_REG, 0|(1 << PI_GPIO_ENABLE_SHIFT));
    for(;;) ;
}