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

#include "reencrypt.h"


#define DMA_QUEUE_SIZE	200
#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)));


/*
 * 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 */
{
    osCreateViManager(OS_PRIORITY_VIMGR);
    /*
     * 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 OSMesgQueue	retraceMessageQ;
static OSMesg		dummyMessage, retraceMessageBuf;

static OSBbFs fs;
static OSBbStatBuf sb;
static u8 buffer[BB_FL_BLOCK_SIZE*32];

static void 
mainproc(char *argv)		{
    s32 rv, sfd, dfd;
    u32 i, len;
    u16 sum0 = 0, sum = 0, sum2 = 0;
    PRINTF("\n=> mainproc...\n");
    osCreateMesgQueue(&retraceMessageQ, &retraceMessageBuf, 1);
    osViSetEvent(&retraceMessageQ, dummyMessage, 1);

    if ((rv = osBbFInit(&fs)) < 0) {
	PRINTF("osBbFInit failed %d\n", rv);
	goto out;
    }
    if ((sfd = osBbFOpen("test.aes", "r")) < 0) {
	PRINTF("osBbFOpen test.aes failed %d\n", sfd);
	goto out;
    }
    if ((dfd = osBbFCreate("test.cry", 1, 0)) < 0) {
	PRINTF("osBbFOpen test.cry failed %d\n", dfd);
	goto out;
    }
    if ((rv = osBbFStat(sfd, &sb, NULL, 0)) < 0) {
	PRINTF("osBbFStat failed %d\n", rv);
	goto out;
    }
    for(i = 0; i < sb.size; i += len) {
	int j;
	len = sizeof buffer;
	if (len > sb.size-i) len = sb.size-i;
	if ((rv = osBbFRead(sfd, i, buffer, len)) < 0) {
	    PRINTF("osBbFRead failed at offset %d %d\n", i, rv);
	    goto out;
	}
	for(j = 0; j < len; j++) {
	    sum0 += buffer[j];
	    buffer[j] ^= 0xa5;
	}
    }
    for(i = 0; i < sb.size; i += len) {
	int j;
	len = sizeof buffer;
	if (sb.size-i < len) len = sb.size-i;
	if ((rv = osBbFRead(sfd, i? sizeof buffer : 0, buffer, len)) < 0) {
	    PRINTF("osBbFRead failed at offset 0 %d\n", rv);
	    goto out;
	}
	for(j = 0; j < len; j++) {
	    sum += buffer[j];
	    buffer[j] ^= 0xa5;
	}
	if ((rv = osBbFShuffle(sfd, dfd, i!=0, buffer, len)) < 0) {
	    PRINTF("osBbFShuffle failed at offset %d %d\n", i, rv);
	    goto out;
	}
    }
    osBbFClose(sfd);
    osBbFClose(dfd);
    if ((rv = osBbFRename("test.cry", "test.aes" )) < 0) {
	PRINTF("osBbFRename test.aes failed %d\n", rv);
	goto out;
    }
    if ((sfd = osBbFOpen("test.aes", "r")) < 0) {
	PRINTF("osBbFOpen test.aes failed %d\n", sfd);
	goto out;
    }
    if ((rv = osBbFStat(sfd, &sb, NULL, 0)) < 0) {
	PRINTF("osBbFStat failed %d\n", rv);
	goto out;
    }
PRINTF("size %d\n", sb.size);
    for(i = 0; i < sb.size; i += len) {
	int j;
	len = sizeof buffer;
	if (len > sb.size-i) len = sb.size-i;
	if ((rv = osBbFRead(sfd, i, buffer, len)) < 0) {
	    PRINTF("osBbFRead failed at offset %d %d\n", i, rv);
	    goto out;
	}
	for(j = 0; j < len; j++) {
	    buffer[j] ^= 0xa5;
	    sum2 += buffer[j];
	}
    }
    osBbFClose(sfd);
    if (sum != sum2 || sum != sum0) {
	PRINTF("check sum failed %04x %04x %04x\n", sum, sum2, sum0, sum0);
    }
out:
    IO_WRITE(PI_GPIO_REG, 0|(1 << PI_GPIO_ENABLE_SHIFT));
    osExit();
    for(;;) ;
}