#include "ultra64.h"
#include <bcp.h>
#include <os_bb.h>
#include <PR/bbskapi.h>

#include "sysapp.h"
#include "mon.h"

#ifdef FBDEBUG
#include <libfb.h>
#define printstr fbPrintf
#endif

extern s32 osBbCardChange(void);
extern s32 osBbCardClearChange(void);

char version[] = VERSION;

#define IS_SECMODE() ( IO_READ(VIRAGE0_NMS_CRSTO_0_REG) ? 1:0 )

#define BYTE(x)	((u8)((x)&0xFF))

/* For handling file system access */
static OSBbFs fs;
static u8 block[BB_FL_BLOCK_SIZE] __attribute__((aligned(16)));
/* block status list */
static u8 blist[BB_FL_BLOCK_SIZE] __attribute__((aligned(16)));
static u8 spare[BB_FL_SPARE_SIZE];
static u8 name[256];

/* Taken from salaunch.c */
/* * * * * * *  Heart beat part * * * * * * * * * * * */
#define HEART_BEAT_PRI   9
#define HEART_BEAT_RATE  500000    /*  1 sec */

static OSThread heartThread;
static u64      heartThreadStack[STACKSIZE/sizeof(u64)] __attribute__ ((aligned (8)));
OSMesg          heartMessageBuf;
OSMesgQueue     heartMessageQ;
static int init_heart_beat = 1;

static void blinkled(int delay) {
    osBbSetErrorLed(1);
    __osBbDelay(delay);
    osBbSetErrorLed(0);
    __osBbDelay(delay);    
}

static void heartproc(void  *arg)
{
    int on = 1;
    OSTimer  timer;

    while (1) {
        osBbSetErrorLed(on);
        osSetTimer(&timer, OS_USEC_TO_CYCLES((u64) HEART_BEAT_RATE), 0, &heartMessageQ, NULL);
        osRecvMesg(&heartMessageQ, NULL, OS_MESG_BLOCK);
        on ^= 1;
    }
}

void start_heart_beat()
{
    if (init_heart_beat) { /* create heartbeat thread the first time */
        osCreateMesgQueue(&heartMessageQ, &heartMessageBuf, 1);
        osCreateThread(&heartThread, 9, heartproc, NULL,
                   heartThreadStack+STACKSIZE/sizeof(u64), HEART_BEAT_PRI);
        init_heart_beat = 0;
    }
    osStartThread(&heartThread);
}

void stop_heart_beat()
{
    osStopThread(&heartThread);
    osBbSetErrorLed(0);
}

/* Copied from uiBbidCheck from viewer */
/* * * * * * * * Write sysid file * * * * * * * * * * * * * */
int uiBbidCheck(void)
{
    char ftmp[] = "temp.tmp";
    char idsys[] = "id.sys";
    BbId bbid;
    BbId cardid;

    int ret = 0; 
    s32 ifd;

    if ((ret = skGetId(&bbid)) < 0) {;
        PRINTF("ERROR skGetId %d\n", ret);
        ret = VW_ERR_FATAL;
        goto exit;
    }
    PRINTF("bbid=%d\n", bbid);

    if(osBbFInit(&fs)<0){
        PRINTF("ERROR osBbFInit()\n");
        ret = VW_ERR_FS;
        goto exit;
    }
    PRINTF("FS init done\n");

    bzero(block, sizeof(block));
#ifdef _BBID_GUARD
    ifd = osBbFOpen(idsys, "r");
    if (ifd>=0) {
        if (osBbFRead(ifd, 0, block, sizeof(block))<0) {
            osBbFClose(ifd);
            ret = VW_ERR_FS;
        } else {
            cardid = *((u32*)block);
            PRINTF("cardid=%d\n", cardid);
            osBbFClose(ifd);
            if (bbid!=cardid) 
                ret = VW_ERR_ID_MISMATCH;
        }
    } 
    else if (ifd==BBFS_ERR_ENTRY) {
#endif
        bcopy(&bbid, block, 4);
        osBbFDelete(ftmp);
        osBbFCreate(ftmp, 1, sizeof(block));
        if ((ifd = osBbFOpen(ftmp, "w"))<0) {
            PRINTF("temp.tmp creation failed\n", ifd);
            ret = VW_ERR_FS;
        } else {
            if(osBbFWrite(ifd, 0, block, sizeof(block))<0){
                PRINTF("failed to write bbid\n");
                osBbFClose(ifd);
                osBbFDelete(ftmp);
                ret = VW_ERR_FS;
            }
            osBbFClose(ifd);
            osBbFRename(ftmp, idsys);
        }
#ifdef _BBID_GUARD
    } else {
        PRINTF("id.sys open failed: %d\n", ifd);
        ret = VW_ERR_FS;
    } 
#endif

exit: 
    return ret;
}

/* * * *  Check SUM calculations * * * */
unsigned int
dosum(u8* p, int n, unsigned int s) {
    int i;
    for(i = 0; i < n; i++)
        s += p[i];
    return s;
}

int
cmp_cksum(char *file, int length, unsigned int sum)
{
    unsigned int calc = 0;
    int len;
    int n, off;
    OSBbStatBuf sb;
    int fd, rv;

    fd = osBbFOpen(file, "r");
    if (fd < 0)
        return fd;
    if ((rv = osBbFStat(fd, &sb, NULL, 0)) < 0)
        return rv;
  
    len = MIN(sb.size, length);
    off = 0;
    while (len > 0) {
        bzero(block, sizeof block);
        osInvalDCache(block, sizeof block);
        n = (len > sizeof block) ? sizeof block : len;
        if ((rv = osBbFRead(fd, off, block, sizeof block)) < 0)
            return rv;
        len -= n;
        off += sizeof block;
        calc = dosum(block, n, calc);
    }
    //PRINTF("cmp_cksum: %s, len %d, calcsum = 0x%x\n", file, MIN(sb.size,length), calc);
    return (sum == calc) ? 0 : -1;
}

char *cause = (char *)(0x80500000-4096);
#define CAUSE_PRINTF(fmt, args...) PRINTF(fmt, ## args); 

/*
 * State variables for BBC interface
 */
int cardseqno;
int cardpresent;
int cardchanged;

/* 
 * Supported libbbc requests 
 * REQ_SET_LEDS
 * REQ_SET_TIME
 * REQ_READ_BLOCK_SP, REQ_READ_BLOCK
 * REQ_WRITE_BLOCK_SP, REQ_WRITE_BLOCK
 * REQ_SCAN_BLOCKS
 * REQ_FS_INIT
 * REQ_CKSUM_FILE
 * REQ_SET_SEQNO, REQ_GET_SEQNO
 * REQ_CARD_SIZE
 * REG_GET_BBID
 * REG_GET_SIG
 */
void 
monproc(char *argv)
{
    unsigned int hbuf[2], rbuf[2], size = 0;
    void *sparep;
    s32 rv = 0, i;
#ifdef FBDEBUG
    u32 rvbad = 0, cmd = 0;
    u8 tmp[256];
#endif

    if (__osBbIsBb) {
        *cause = 0;
        if(IS_SECMODE()){
            u32 sm = IO_READ(MI_SEC_MODE_REG);
            if (sm & (MI_SEC_MODE_MD_TRAP|MI_SEC_MODE_BUT_TRAP|MI_SEC_MODE_TRAP|
                      MI_SEC_MODE_FATAL|MI_SEC_MODE_TIMER|MI_SEC_MODE_APP)) {
                /* decode secure mode trap */
#ifdef FBDEBUG
                const static char* tname[] = {
                    "application", "timer", "pi error", "mi error", "button", "module",
                };
#endif
                CAUSE_PRINTF("secure trap ");
                for(i = 0; i < 6; i++) {
                    if (sm & (1<<(i+2))) {
                        CAUSE_PRINTF("%s ", tname[i]);
                        if (i == 2) {
                            CAUSE_PRINTF("0x%x edata 0x%x ", (int)IO_READ(PI_ERROR_REG),
                                         (int)IO_READ(PI_EDATA_REG));
                            IO_WRITE(PI_STATUS_REG, 3);
                            IO_WRITE(PI_ERROR_REG, 0);
                        } else if (i == 3) {
                            CAUSE_PRINTF("info 0x%x addr 0x%x data 0x%x ", 
                                         (int)IO_READ(MI_ERROR_INFO_REG),
                                         (int)IO_READ(MI_ERROR_ADDR_REG),
                                         (int)IO_READ(MI_ERROR_DATA_REG));
                        }
                    }
                }
                CAUSE_PRINTF("pc 0x%x  mode 0x%x\n", (int)IO_READ(VIRAGE0_RAM_START), (int)sm);
            }
        }
    }

    if (__osBbIsBb) {
        osBbRtcInit();
        osBbCardInit();
        if ((rv = osBbFInit(&fs)) < 0)
            PRINTF("osBbFInit failed %d\n", rv);
    }

    cardpresent = osBbCardClearChange();

    /* Indicate we are entering mon mode */
    blinkled(100000);

    for(;;) {
        u8 stat;
loop_start:
        if (rv < 0) {
            /* Previous USB read/write gave error, indicating reset */
            /* Reset LED state to off */
            stop_heart_beat();
        }

#ifdef FBDEBUG
        if (rv < 0) rvbad++;
        sprintf(tmp, "Read cmd, last=%d, rv=%d, bad=%d  ", cmd, rv, rvbad);
        printstr(fbWhite, 3, 12, tmp);
        osWritebackDCacheAll();
#endif
        rv = osBbReadHost(hbuf, sizeof hbuf);
        if (rv < 0) {
#ifdef FBDEBUG
            sprintf(tmp, "Error reading: rv=%d", rv);
            printstr(fbWhite, 3, 13, tmp);
            osWritebackDCacheAll();
#endif
            /* Error reading: just try again */
            goto loop_start; 
        } 

#ifdef FBDEBUG
        sprintf(tmp, "processing cmd=%d rv=%d, bad=%d   ", hbuf[0], rv, rvbad);
        printstr(fbWhite, 3, 12, tmp);
        osWritebackDCacheAll();
        PRINTF("cmd %d\n", hbuf[0]);
        cmd = hbuf[0];
#endif

        /* Check card status */
#ifdef FBDEBUG
        printstr(fbWhite, 3, 13, "Check card status   ");
        osWritebackDCacheAll();
#endif
        (void) osBbCardStatus(0, &stat);
        cardchanged = osBbCardChange();
        if (cardchanged) {
            cardseqno = 0;
            cardpresent = osBbCardClearChange();
            if (cardpresent)
                osBbCardInit();
        }
        
        /* Process BBC commands */
        rbuf[0] = 255-hbuf[0];  rbuf[1] = 0; sparep = 0;
        switch(hbuf[0]) {

        case REQ_CKSUM_FILE:
        {
            int len, csum;
            
            size = (hbuf[1]+3)&~3;
            /* prevent buffer overflow */
            if (size > sizeof(name))
                size = sizeof(name);
            if ((rv = osBbReadHost(name, size)) < 0) goto loop_start;
            /* make sure name is null terminated */
            name[sizeof(name)-1] = '\0';
            if ((rv = osBbReadHost(hbuf, sizeof hbuf)) < 0) goto loop_start;
            csum = hbuf[0];
            len = hbuf[1];
            rbuf[1] = cmp_cksum(name, len, csum);
            if ((rv = osBbWriteHost(rbuf, sizeof rbuf)) < 0) goto loop_start;
            break;
        }

        case REQ_SET_LEDS:
            stop_heart_beat();
            if ((hbuf[1] & 3) == 0 || (hbuf[1] & 3) == 1)
                osBbSetErrorLed(0);
            else if ((hbuf[1] & 3) == 2)
                osBbSetErrorLed(1);
            else if ((hbuf[1] & 3) == 3)
                start_heart_beat();
            rbuf[1] = 0;
            if ((rv = osBbWriteHost(rbuf, sizeof rbuf)) < 0) goto loop_start;
            break;

        case REQ_SET_TIME:
        {
            u8 year, month, day, dow, hour, min, sec;
            
            year = BYTE(hbuf[1]>>24);
            month = BYTE(hbuf[1]>>16);
            day = BYTE(hbuf[1]>>8);
            dow = BYTE(hbuf[1]);
            rbuf[1] = 0;
            if ((rv = osBbWriteHost(rbuf, sizeof rbuf)) < 0) goto loop_start;
            if ((rv = osBbReadHost(hbuf, 4)) < 0) { goto loop_start; }
            hour = BYTE(hbuf[0]>>16);
            min = BYTE(hbuf[0]>>8);
            sec = BYTE(hbuf[0]);
            osBbRtcSet(year, month, day, dow, hour, min, sec);
            break;
        }

        case REQ_WRITE_BLOCK_SP:
            sparep = spare;
            /* fall through */
        case REQ_WRITE_BLOCK:
            /* PRINTF("write block %d\n", hbuf[1]); */
            if ((rv = osBbReadHost(block, sizeof block)) < 0) goto loop_start;
            if (sparep)
                if ((rv = osBbReadHost(spare, sizeof spare)) < 0) goto loop_start;
#ifdef BBCARD_WRITE_ENABLE
            if ((rv = osBbCardEraseBlock(0, hbuf[1])) < 0)
                PRINTF("erase block %d failed %d\n", hbuf[1], rv);
            if ((rv = osBbCardWriteBlock(0, hbuf[1], block, sparep)) < 0)
                PRINTF("write block %d failed %d\n", hbuf[1], rv);
#else
            rv = 0;
#endif
            rbuf[1] = rv;
            if ((rv = osBbWriteHost(rbuf, sizeof rbuf)) < 0) goto loop_start;
            break;

        case REQ_READ_BLOCK_SP:
            sparep = spare;
            /* fall through */
        case REQ_READ_BLOCK:
            /* PRINTF("read block %d\n", hbuf[1]); */
            rv = osBbCardReadBlock(0, hbuf[1], block, spare);
            rbuf[1] = rv;
            if ((rv = osBbWriteHost(rbuf, sizeof rbuf)) < 0) goto loop_start;
            if ((rv = osBbWriteHost(block, sizeof block)) < 0) goto loop_start;
            if (sparep) {
                if ((rv = osBbWriteHost(spare, sizeof spare)) < 0) goto loop_start;
            }
            break;

        case REQ_SCAN_BLOCKS:
	{
	    u32 nblks;

            /* 
             * Scan for blocks that are marked bad by the manufacturer
             * Returns a list of status bytes indicating if the block is bad
             * or not.  The size of the list is returned in the 2nd byte
             * of the cmd response.
             */

            /* PRINTF("bad blocks (%d):\n", osBbCardBlocks(0)); */
            nblks = osBbCardBlocks(0);
            for(i = 0; i < nblks; i++) {
                spare[BB_FL_BLOCK_STATUS_OFF] = 0xff;
                rv = osBbCardReadBlock(0, i, block, spare);
                /* Check the spare area to see if the manufacturer has 
                   marked the block as bad */
                blist[i] = spare[BB_FL_BLOCK_STATUS_OFF] != 0xff;
                /* PRINTF("%s", blist[i] ? "@" : "."); */
            }
            /* PRINTF("\n"); */
            rbuf[1] = i;
            if ((rv = osBbWriteHost(rbuf, sizeof rbuf)) < 0) goto loop_start;
            if ((rv = osBbWriteHost(blist, i)) < 0) goto loop_start;
            break;
	}

        case REQ_FS_INIT:
            rv = osBbFInit(&fs);
            /* PRINTF("fsinit %d\n", rv); */
            rbuf[1] = rv;
            if ((rv = osBbWriteHost(rbuf, sizeof rbuf)) < 0) goto loop_start;
            break;

        case REQ_CARD_SIZE:
            rbuf[1] = osBbCardBlocks(0);
            if ((rv = osBbWriteHost(rbuf, sizeof rbuf)) < 0) goto loop_start;
            break;

        case REQ_SET_SEQNO:
            cardseqno = (cardpresent ? (int)hbuf[1] : 0);
            rbuf[1] = cardseqno;
            if ((rv = osBbWriteHost(rbuf, sizeof rbuf)) < 0) goto loop_start;
            break;

        case REQ_GET_SEQNO:
            rbuf[1] = cardpresent ? cardseqno : -1;
            if ((rv = osBbWriteHost(rbuf, sizeof rbuf)) < 0) goto loop_start;
            break;

        case REQ_GET_BBID:
            skGetId((BbId *)&rbuf[1]);
            if ((rv = osBbWriteHost(rbuf, sizeof rbuf)) < 0) goto loop_start;
            break;

        case REQ_GET_SIG:
        {
            BbShaHash hash;
            BbEccSig sig;
            int len;
            
            len = hbuf[1];

            if (len == sizeof(hash)) {
                /* PRINTF("REG_GET_SIG reading hash\n"); */
                if ((rv = osBbReadHost(hash, len)) < 0) goto loop_start;
                skSignHash(hash, sig);
                
                /* PRINTF("REG_GET_SIG writing response to host\n"); */
                rbuf[1] = sizeof sig;
                if ((rv = osBbWriteHost(rbuf, sizeof rbuf)) < 0) goto loop_start;
                if ((rv = osBbWriteHost((u8*) sig, sizeof sig)) < 0) goto loop_start;
                /* PRINTF("REG_GET_SIG done\n"); */
            }
            else {
                PRINTF("Bad hash length: expected %d, got %d\n", sizeof hash, len);
                rbuf[1] = -1;
                if ((rv = osBbWriteHost(rbuf, sizeof rbuf)) < 0) goto loop_start;
                
                /* Read out len of data so USB will still be in sync */
                while (len > 0) {
                    int n = (len > sizeof(block))? sizeof(block):len;
                    if ((rv = osBbReadHost(block, n)) < 0) goto loop_start;
                    len -= n;
                }
            }

            break;
        }

        default:
            PRINTF("unknown request %d\n", hbuf[0]);
            rbuf[1] = -1;
            rv = osBbWriteHost(rbuf, sizeof rbuf);
            break;
        }
    }
}