#include "ultra64.h"
#include <bcp.h>
#include <os_bb.h>
#include <bbdebug.h>

#include "mon.h"
#include "salaunch.h"
#include "vidreset.h"

char version[] = VERSION;

#define DMA_QUEUE_SIZE  200

#define PRINTF          osSyncPrintf
#define REALLY_PRINTF   PRINTF

#define PROBE_IDE_RDB() \
        (IDE_WRITE(IDE_RDB_RDATA_REG+0,0xbabe), \
         IDE_WRITE(IDE_RDB_RDATA_REG+2,0xcafe), \
         ((IDE_READ(IDE_RDB_RDATA_REG+0) == 0xbabe) & \
          (IDE_READ(IDE_RDB_RDATA_REG+2) == 0xcafe)))

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

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

static OSMesg           SiMessages[DMA_QUEUE_SIZE];
static OSMesgQueue      SiMessageQ;
OSPiHandle              *handler;

/*
 * Local variables and routines
 */

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

u32 __osDisableInt(void);

extern void *__printfunc;
extern u32 __osBbHackFlags;
extern void __osBbVideoPllInit(s32 tvType);
void
boot(void)
{
    //IO_WRITE(MI_INTR_EMASK_REG, MI_INTR_MASK_CLR_BUT);
    /* Init the video PLL */
    __osBbVideoPllInit(OS_TV_NTSC);
    clear(0);
    vidReset(cfb);

    osInitialize();
    osCreateThread(&idleThread, 1, (void(*)(void *))idleproc, (void *)0,
                   idleThreadStack+STACKSIZE, 20);
    osStartThread(&idleThread);
}

#ifdef USB_RDB
extern u32 __osRdb_Usb_Active;
#endif

static void
idleproc(char *argv)            /* priority 20 */
{
    osCreateViManager(OS_PRIORITY_VIMGR);
    osViSetMode(&osViModeTable[OS_VI_NTSC_LPN1]);

    /*
     * Start PI Mgr for access to cartridge - start before the debugger
     */
    osCreatePiManager((OSPri) OS_PRIORITY_PIMGR, &PiMessageQ, PiMessages,
            DMA_QUEUE_SIZE);

    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, (OSPri)18);
    osStartThread(&mainThread);

    osSetThreadPri(0, OS_PRIORITY_IDLE);

#if 0
    for(;;) {
        osBbSetErrorLed(1);
        delay();
        osBbSetErrorLed(0);
        delay();
    }
#else
    for(;;) ;
#endif
}

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 u16 blockList[4096];             /* file block list */
static OSBbDirEnt dir[BB_INODE16_ENTRIES];
static u8 spare[BB_FL_SPARE_SIZE];
static u8 name[256], name2[256];
static OSBbStatFs sfs;

void sum(u8* p, int n) {
    unsigned short s = 0;
    int i;
    for(i = 0; i < n; i++) {
        s += p[i];
//      PRINTF("%02x ", p[i]);
    }
    PRINTF("sum %04x\n", s);
}

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;

    fd = osBbFOpen(file, "r");
    if (fd < 0)
        return -1;
    if (osBbFStat(fd, &sb, blockList, sizeof blockList/sizeof blockList[0]) < 0)
        return -1;
  
    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 (osBbFRead(fd, off, block, sizeof block) < 0)
            return -1;
        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...) \
    if (has_rdb) \
        PRINTF(fmt, ## args); \
    else \
        cause += sprintf(cause, fmt, ## args)

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

static void 
mainproc(char *argv)
{
    unsigned int hbuf[2], rbuf[2], size;
    void *sparep;
    s32 rv = 0, i, n, len, off, fd;
    int has_rdb = 0;
    u16 s, first_time_call=1;

    try_to_launch_gng();

#ifdef USB_RDB
    if (__osBbIsBb) {
        extern u32 __osRdb_Usb_Active;
        if ((rv = osBbUsbInit()) < 0) {
            /* now what? */
            has_rdb = 0;
        } else {
            has_rdb = __osRdb_Usb_Active;
            /* wait 800ms after usb init for printf to work */
            __osBbDelay(800000);
        }
    }
#else
    has_rdb = PROBE_IDE_RDB();
#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 */
                const static char* tname[] = {
                    "application", "timer", "pi error", "mi error", "button", "module",
                };
                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 0
    if (!has_rdb) {
        __printfunc=NULL;
        osBbFInit(&fs); /* sure hope this works */
        fd = osBbFOpen("launch.aes", "r");
        if (fd >= 0) {
            OSBbStatBuf sb;
            OSBbLaunchMetaData md;
            __osBbHackFlags = __osBbIsBb == 1 ? 1 : 0;
            if (osBbFStat(fd, &sb, (u16*)block, sizeof block/sizeof u16) >= 0) {
                bzero(&md, sizeof(OSBbLaunchMetaData));
                md.memSize = 0x400000;
                md.tvType = OS_TV_NTSC;
                md.romBase = PI_DOM1_ADDR2;
                osBbExecFile(fd, "launch.aes", &md, block);
            }
        }
    }
    PRINTF("bbmon++ (%s) ...\n", version);
#endif
    if (__osBbIsBb) {
        u32 mfg, type;

        osBbRtcInit();
        osBbCardInit();
        rv = osBbCardReadId(0, &mfg, &type);
#if 0
        PRINTF("card id %x %x rv %x box id %x %x %x blks %d\n", mfg, type, rv,
            osBbGetBoxId(), IO_READ(PI_BUFFER_BASE_REG), 
            IO_READ(PI_FLASH_CONFIG_REG), osBbCardBlocks(0));
#endif
        if ((rv = osBbFInit(&fs)) < 0)
            PRINTF("osBbFInit failed %d\n", rv);
    }

    cardpresent = osBbCardClearChange();

    for(;;) {
        u8 stat;

loop_start:
        //PRINTF("read ...\n");
        rv = osBbReadHost(hbuf, sizeof hbuf);
        //PRINTF("cmd %d\n", hbuf[0]);

        if (rv < 0) {
            /* Error reading: just try again */
            goto loop_start; 
        } 

        if (first_time_call) {
            delete_gng_tmpfile();
            write_sysid_file();
            first_time_call = 0;
        }

        (void) osBbCardStatus(0, &stat);
        cardchanged = osBbCardChange();
        if (cardchanged) {
            cardseqno = 0;
            cardpresent = osBbCardClearChange();
            if (cardpresent)
                osBbCardInit();
        }
        
        rbuf[0] = 255-hbuf[0];  rbuf[1] = 0; sparep = 0;
        switch(hbuf[0]) {
        case REQ_PING:
PRINTF("ping\n");
#ifdef RTC_DEBUG
            {
                char *dname[] = {"Unk", "Mon", "Tue", "Wed", "Thu", "Fri", "Sat", "Sun"};
                u8 yr, mon, day, dow, hr, min, sec;
                osBbRtcGet(&yr, &mon, &day, &dow, &hr, &min, &sec);
                osSyncPrintf("RTC = %s %02d/%02d/%02d %02d:%02d:%02d\n",
                    dname[dow], mon, day, yr, hr, min, sec);
            }
#endif /* RTC_DEBUG */
            if ((rv = osBbWriteHost(rbuf, sizeof rbuf)) < 0) goto loop_start;
            break;
        case REQ_POWER_OFF:
PRINTF("power off\n");
             if ((rv = osBbWriteHost(rbuf, sizeof rbuf)) < 0) goto loop_start;
            osBbPowerOff();
            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_READ_DIR:
            PRINTF("REQ_READ_DIR\n");
            rv = osBbFReadDir(dir, sizeof dir/sizeof dir[0]);
            rbuf[1] = rv;
            PRINTF("read dir %d\n", rv);
            if ((rv = osBbWriteHost(rbuf, sizeof rbuf)) < 0) goto loop_start;
            for(i = 0; i < rbuf[1]; ++i) {
                if ((rv = osBbWriteHost(dir[i].name, 16)) < 0) goto loop_start;
                if ((rv = osBbWriteHost(&dir[i].size, 4)) < 0) goto loop_start;
            }
            PRINTF("Doing osBbFStat\n");
            osBbFStatFs(&sfs);
//PRINTF("fs files %d free %d   blocks %d free %d\n", sfs.files, sfs.freeFiles, sfs.blocks, sfs.freeBlocks);
            break;
        case REQ_WRITE_FILE:
            if ((rv = osBbReadHost(&size, sizeof size)) < 0) goto loop_start;
            size = (size+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';
            size = (hbuf[1]+BB_FL_BLOCK_SIZE-1)&~(BB_FL_BLOCK_SIZE-1);
            len = (hbuf[1]+3)&~3;
//PRINTF("write %s len %d\n", name, hbuf[1]);
#ifdef BBCARD_WRITE_ENABLE
            fd = osBbFCreate(name, 1, size);
#else
            fd = 0xff;     /* Fake file handle */
#endif
//PRINTF("create %d\n", fd);
            rbuf[1] = fd < 0 ? fd : 0;
            /* first response */
            if ((rv = osBbWriteHost(rbuf, sizeof rbuf)) < 0) goto loop_start;
            if (fd >= 0) {
                off = 0;
                while(fd >= 0 && len > 0) {
                    n = len > sizeof block ? sizeof block : len;
                    if (n != sizeof block) bzero(block, sizeof block);
                    if ((rv = osBbReadHost(block, n)) < 0) goto loop_start;
//PRINTF("wb %d\n", off);
//sum(block, sizeof block);
if (((off>>14)&15) == 0) PRINTF(".");
                    osWritebackDCache(block, sizeof block);
#ifdef BBCARD_WRITE_ENABLE
                    if ((rv = osBbFWrite(fd, off, block, sizeof block)) < 0) {
                        PRINTF("osBbFWrite failed %d\n", rv); 
                        break;
                    }
#else
                    rv = 0;
#endif
                    len -= n;
                    off += sizeof block;
                }
#ifdef BBCARD_WRITE_ENABLE
                osBbFClose(fd);
#endif
                rbuf[1] = fd < 0 ? fd : rv < 0 ? rv : 0;
                /* second response */
                if ((rv = osBbWriteHost(rbuf, sizeof rbuf)) < 0) goto loop_start;
            }
            break;
        case REQ_READ_FILE:
            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';
//PRINTF("read %s\n", name);
            fd = osBbFOpen(name, "r");
            if (fd < 0) {
                rbuf[1] = fd;
                len = 0;
            } else {
                OSBbStatBuf sb;
                if ((rv = osBbFStat(fd, &sb, blockList, sizeof blockList/sizeof blockList[0])) < 0) {
                    PRINTF("osBbFStat failed %d\n", rv);
                    len = 0;
                } else
                    len = rbuf[1] = sb.size;
                PRINTF("blocks %d %d %d %d\n", blockList[0], blockList[1], blockList[2], blockList[3]);
            }
            if ((rv = osBbWriteHost(rbuf, sizeof rbuf)) < 0) goto loop_start;
            off = 0;
            while(fd >= 0 && len > 0) {
                n = len > sizeof block ? sizeof block : len;
                osInvalDCache(block, sizeof block);
                if ((rv = osBbFRead(fd, off, block, sizeof block)) < 0) {
                    PRINTF("osBbFRead failed %d\n", rv); 
                    break;
                }
                PRINTF(".");
                if ((rv = osBbWriteHost(block, n)) < 0) goto loop_start;
                len -= n;
                off += sizeof block;
            }
            PRINTF("\n");
            osBbFClose(fd);
            break;
        case REQ_DELETE_FILE:
            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';
//PRINTF("delete %s\n", name);

#ifdef BBCARD_WRITE_ENABLE
            rv = osBbFDelete(name);
#else
            rv = 0;
#endif 
            rbuf[1] = rv;
            if ((rv = osBbWriteHost(rbuf, sizeof rbuf)) < 0) goto loop_start;
            break;
        case REQ_RENAME_FILE:
            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(&len, 4)) < 0) goto loop_start;
            /* prevent buffer overflow */
            len = (len+3)&~3;
            if (len > sizeof(name2))
                len = sizeof(name2);
            if ((rv = osBbReadHost(name2, len)) < 0) goto loop_start;
            /* make sure name is null terminated */
            name2[sizeof(name2)-1] = '\0';
//PRINTF("rename %s %s\n", name, name2);
#ifdef BBCARD_WRITE_ENABLE
            rbuf[1] = osBbFRename(name, name2);
#else
            rbuf[1] = 0;
#endif
            if ((rv = osBbWriteHost(rbuf, sizeof rbuf)) < 0) goto loop_start;
            break;
        case REQ_SUM_FILE:
            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';
//PRINTF("sum %s\n", name);
            fd = osBbFOpen(name, "r");
            if (fd < 0) {
                rbuf[1] = fd;
                len = 0;
            } else {
                OSBbStatBuf sb;
                if ((rv = osBbFStat(fd, &sb, blockList, sizeof blockList/sizeof blockList[0])) < 0) {
                    PRINTF("osBbFStat failed %d\n", rv);
                    len = 0;
                } else
                    len = rbuf[1] = sb.size;
            }
            off = 0;
            s = 0;
            while(fd >= 0 && len > 0) {
                n = len > sizeof block ? sizeof block : len;
                osInvalDCache(block, sizeof block);
                if ((rv = osBbFRead(fd, off, block, sizeof block)) < 0) {
                    PRINTF("osBbFRead failed %d\n", rv); 
                    break;
                }
                for(i = 0; i < sizeof block; i++)
                    s += block[i];
                len -= n;
                off += sizeof block;
            }
            rbuf[1] = s;
            if ((rv = osBbWriteHost(rbuf, sizeof rbuf)) < 0) goto loop_start;
            //PRINTF("%04x\n", s);
            osBbFClose(fd);
            break;
        case REQ_SCAN_BLOCKS:
            /* 
             * 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));
            for(i = 0; i < osBbCardBlocks(0); i++) {
                u8 s;
                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;
#if 1
                osBbCardStatus(0, &s);
                //PRINTF("%s", blist[i] ? "@" : ".");
#else
                if (i % 256 == 0) PRINTF(".");
                if (blist[i])
                    PRINTF("%4d ", i);
#endif
            }
            //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_FREE_BLKS:
            osBbFStatFs(&sfs);
            rbuf[1] = sfs.freeBlocks;
            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_STAT_FILE:
            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';
            fd = osBbFOpen(name, "r");
            if (fd < 0) {
                rbuf[1] = fd;
                if ((rv = osBbWriteHost(rbuf, sizeof rbuf)) < 0) goto loop_start;
            } else {
                OSBbStatBuf sb;
                if ((rv = osBbFStat(fd, &sb, blockList, sizeof blockList/sizeof blockList[0])) < 0) {
                    PRINTF("osBbFStat failed %d\n", rv);
                    rbuf[1] = rv;
                    if ((rv = osBbWriteHost(rbuf, sizeof rbuf)) < 0) goto loop_start;
                } else {
                    rbuf[1] = fd;
                    if ((rv = osBbWriteHost(rbuf, sizeof rbuf)) < 0) goto loop_start;
                    if ((rv = osBbWriteHost(&sb, sizeof sb)) < 0) goto loop_start;
                }
            }
            break;
        case REQ_READ_FILE_BLK:
            {
            int off, len, n;
            int resp[3];
            OSBbStatBuf sb;

            fd = hbuf[1];
            if ((rv = osBbReadHost(hbuf, sizeof hbuf)) < 0) goto loop_start;
            off = hbuf[0];
            len = hbuf[1];
            resp[0] = rbuf[0];
            resp[1] = cardseqno;
            if ((rv = osBbFStat(fd, &sb, blockList, sizeof blockList/sizeof blockList[0])) < 0) {
                PRINTF("osBbFStat failed %d\n", rv);
                resp[2] = rv;
                if ((rv = osBbWriteHost(resp, sizeof resp)) < 0) goto loop_start;
            } else {
                resp[2] = MIN(sb.size - off, len);
                len = resp[2];
                if ((rv = osBbWriteHost(resp, sizeof resp)) < 0) goto loop_start;
                while (len > 0) {
                    n = len > sizeof block ? sizeof block : len;
                    osInvalDCache(block, sizeof block);
                    if ((rv = osBbFRead(fd, off, block, sizeof block)) < 0) {
                        PRINTF("osBbFRead failed %d\n", rv); 
                        break;
                    }
                    if ((rv = osBbWriteHost(block, n)) < 0) goto loop_start;
                    len -= n;
                    off += sizeof block;
                }
            }
            }
            break;
        case REQ_WRITE_FILE_BLK:
            {
            int off, len, n, tot, seqno;
            int resp[3];

            fd = hbuf[1];
            if ((rv = osBbReadHost(resp, sizeof resp)) < 0) goto loop_start;
            off = resp[0];
            len = resp[1];
            seqno = resp[2];
            resp[0] = rbuf[0];
            if (seqno != cardseqno) {
                cardseqno = 0;
                resp[1] = 0;
                if ((rv = osBbWriteHost(resp, 8)) < 0) goto loop_start;
            } else {
                resp[1] = cardseqno;
                if ((rv = osBbWriteHost(resp, 8)) < 0) goto loop_start;
                tot = 0;
                while (len > 0) {
                    n = (len > sizeof block) ? sizeof block : len;
                    bzero(block, sizeof block);
                    if ((rv = osBbReadHost(block, n)) < 0) goto loop_start;
                    //PRINTF("Write file blk %d\n", BB_FL_BYTE_TO_BLOCK(off));
                    osWritebackDCache(block, sizeof block);
#ifdef BBCARD_WRITE_ENABLE
                    if ((rv = osBbFWrite(fd, off, block, sizeof block)) < 0) {
                        PRINTF("osBbFWrite failed %d\n", rv); 
                        break;
                    }
#else 
                    rv = 0;
#endif
                    len -= n;
                    off += n;
                    tot += n;
                }
                resp[2] = tot;
                if ((rv = osBbWriteHost(resp, 12)) < 0) goto loop_start;
                //PRINTF("Write file done (after resp)\n");
            }
            }
            break;
        case REQ_CREATE_FILE:
        {
            int resp[3];
            int len, fd, seqno;

            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(resp, 8)) < 0) goto loop_start;
            len = (resp[1]+BB_FL_BLOCK_SIZE-1)&~(BB_FL_BLOCK_SIZE-1);
            seqno = resp[0];
            resp[0] = rbuf[0];
            if (seqno != cardseqno) {
                cardseqno = 0;
                resp[1] = 0;
                resp[2] = -1;
                if ((rv = osBbWriteHost(resp, 12)) < 0) goto loop_start;
            } else {
#ifdef BBCARD_WRITE_ENABLE
                fd = osBbFCreate(name, 1, len);
#else
                fd = 0xff;
#endif
                resp[1] = cardseqno;
                resp[2] = fd;
                if ((rv = osBbWriteHost(resp, 12)) < 0) goto loop_start;
            }
            break;
        }
        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;
#define BYTE(x) ((u8)((x)&0xFF))
        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_GET_BBID:
        PRINTF("REQ_GET_BBID\n");
        skGetId(&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;
        }
    }
}