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

#include "gngsys.h"
#include "nu64sys.h"
#include "graph.h"
#include "os_bbsa.h"
#include "sched.h"

#define DMA_QUEUE_SIZE	200

extern u32 __osBbIsBb;

/*
 * Thread and stack structures
 */
char   bootStack[STACKSIZE] __attribute__ ((aligned (8)));

static OSThread idleThread, bgThread;
static char     idleThreadStack[STACKSIZE] __attribute__ ((aligned (8)));
static char     bgThreadStack[STACKSIZE] __attribute__ ((aligned (8)));
static char     gpSchedStack[OS_SC_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;

/*
 * Local variables and routines
 */

static void		idleproc(char *);
static void		mainproc(char *);
static void		bgproc(char *);

#define ERR_ON    IO_WRITE(PI_GPIO_REG, \
                          ((PI_GPIO_ERROR_BIT | PI_GPIO_POWER_BIT) << PI_GPIO_ENABLE_SHIFT) | \
                           PI_GPIO_POWER_BIT)
#define MAX_MESGS        20
OSMesgQueue              bgMsgQ, siMessageQ;
OSMesg                   bgMsgBuf[MAX_MESGS], bdretMsg;
OSSched                  gSc;
OSScClient               gGfxClient;
#define SCHEDULER_PRIORITY      13
#define BG_PRIORITY             10
#define NUM_FIELDS              1
#define BUT_DEBOUCE_TIME        5000
#define BUT_STABLE_TIME         1000000
#define BUT_RELEASED (!((IO_READ(MI_EINTR_REG)&MI_EINTR_BUTTON_PRESSED)))
#define BUT_STATE_CHANGED \
             (IO_READ(MI_EINTR_REG)&MI_INTR_BUT)
#define BUT_CLEAR_STATE_CHANGED \
        IO_WRITE(MI_INTR_EMASK_REG, MI_INTR_MASK_CLR_BUT)
static          OSBbFs fs;

static void              bgproc(char *argv)
{
     u64 end;
     int  key=CONT_L | CONT_G | CONT_R;
     OSContStatus     sdata[MAXCONTROLLERS];
     OSContPad        ctrl_data[MAXCONTROLLERS];
     char pattern;

     do{
          while(BUT_STATE_CHANGED) {
              BUT_CLEAR_STATE_CHANGED;
              
              end = OS_CYCLES_TO_USEC(osGetTime()) + BUT_DEBOUCE_TIME;
              while (OS_CYCLES_TO_USEC(osGetTime()) < end);
          }
      } while(!BUT_RELEASED || BUT_STATE_CHANGED);

     IO_WRITE(MI_INTR_EMASK_REG, MI_INTR_MASK_CLR_BUT);
     IO_WRITE(MI_INTR_EMASK_REG, MI_INTR_MASK_SET_BUT);

     osCreateMesgQueue(&bgMsgQ, bgMsgBuf, MAX_MESGS);
     osSetEventMesg(OS_EVENT_PRENMI, &bgMsgQ, (OSMesg) OS_EVENT_PRENMI); 
     osSetEventMesg(OS_EVENT_SI, &bgMsgQ, (OSMesg) OS_EVENT_SI);
     osContInit(&bgMsgQ, &pattern, &sdata[0]);

     for (;;) {  
         if (IO_READ(MI_EINTR_REG) & MI_EINTR_BUT_STATUS) 
              my_poweroff();

         osContStartReadData(&bgMsgQ);
         osRecvMesg(&bgMsgQ, &bdretMsg, OS_MESG_BLOCK);
         if (bdretMsg == OS_EVENT_PRENMI) my_poweroff();
 
         osContGetReadData(ctrl_data);
         if ((ctrl_data[0].button & key) == key) {
             osBbCardInit();
             osBbFInit(&fs);

             osBbFDelete("errlog.gng");
             my_poweroff();
         }
     }
}

void
boot(void)
{
    /* Init the video PLL */
    __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 */
{
   u64 end;
   osUsbInit();

    osCreateViManager(OS_PRIORITY_VIMGR);
#if	SCREEN_LOW
    osViSetMode(&osViModeTable[OS_VI_NTSC_LPN1]);
#else
    osViSetMode(&osViModeTable[OS_VI_NTSC_HPF1]);
#endif

   
   end = OS_CYCLES_TO_USEC(osGetTime()) + 1000000;
   while (OS_CYCLES_TO_USEC(osGetTime()) < end);
    /*
     * 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);

    /* osCreateThread(&bgThread, 1, (void(*)(void *))bgproc, (void *)0,
                   bgThreadStack+STACKSIZE, BG_PRIORITY);
    osStartThread(&bgThread); */

    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;
OSMesg      siMessageBuf;

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;
    }
}


#define ERR_ON    IO_WRITE(PI_GPIO_REG, \
                          ((PI_GPIO_ERROR_BIT | PI_GPIO_POWER_BIT) << PI_GPIO_ENABLE_SHIFT) | \
                           PI_GPIO_POWER_BIT)
int my_poweroff()
{
    __osDisableInt();
    osBbPowerOff();

    /* Busy loop to let CPU spin */
    for (; ;);
}

static OSBbFs fs;

static void 
mainproc(char *argv)		{
    int ret;
    char buf[128];

    //osCreateMesgQueue(&siMessageQ, &siMessageBuf, 1);
    //osSetEventMesg(OS_EVENT_SI, &siMessageQ, dummyMessage);

    osCreateMesgQueue(&retraceMessageQ, &retraceMessageBuf, 1);
    osViSetEvent(&retraceMessageQ, dummyMessage, 1);
    osViBlack(1);
    osViSwapBuffer(cfb);
    clear(0x0);
    osViBlack(0);
    osWritebackDCacheAll();
    osViSwapBuffer(cfb);

    IO_WRITE(PI_FLASH_CONFIG_REG, 0xa207071f);
    osBbCardInit();
    printstr(white, 12, 4, "Secure launch");
    osWritebackDCacheAll();

    if (osBbFInit(&fs) < 0) {
        printstr(red, 6, 5, "empty card?");   
        goto exit;
    }

    if (gng_loadticket("bbgng.tik") != BB_SYSAPP_PASS) {
        printstr(red, 6, 5, "Cannot open tik file");   
        goto exit;
    }

    ret = verify_sysapp_tik();
    if (ret  != BB_SYSAPP_PASS) {
        sprintf(buf, "Wrong tik file");
        printstr(red, 6, 5, buf);
        goto exit;
    }

    ret =  verify_load_app("bbgng.aes");
    if (ret < 0) {
        sprintf(buf, "cannot verified this rom file.");
        printstr(red, 6, 5, buf);
        goto exit;
    } else printstr(yellow, 6, 5, "Verified done");

exit:
    osWritebackDCacheAll();
    for (;;);
}