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

#include "usbtest.h"

#define DEBUG	1
#ifdef DEBUG
extern int dramPrintf(const char *fmt, ...);
extern void drambufswitch(void);
#define	PRINTF	dramPrintf
#define	printf	dramPrintf
#else
#define	PRINTF	osSyncPrintf
#endif

#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)));


/*
 * USB specific data structures
 */
#define	MAX_USB_DEVICES	4

static OSUsbInfo UsbInfo[MAX_USB_DEVICES];

/*
 * 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 *arg)		/* priority 8 */
{
    s32 nctlr;

    /*
     * Initialize USB and start manager threads for both controllers
     */
    message("Calling osUsbInit ...\n");
    nctlr = osUsbInit();
    if (nctlr == 0) {
    	message("osUsbInit returns 0 ...\n");
	PRINTF("No USB controllers present\n");
    } else if (nctlr > OS_USB_MAX_CONTROLLERS) {
	PRINTF("More USB controllers than possible (%d)\n", nctlr);
    }

    /*
     * 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, arg,
           mainThreadStack+STACKSIZE, (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++;
	}
}

#if 0
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);
    }
}
#endif /* 0 */

#define	ECHO_DATA_SIZE	1024
unsigned char outbuf[ECHO_DATA_SIZE];
unsigned char inbuf[ECHO_DATA_SIZE];

void
do_echo_test(s32 which, OSUsbHandle handle)
{
    s32 i, nc, errs;

    PRINTF("USB%d: test Echo Device in loopback mode\n", which);

    for (i = 0; i < sizeof outbuf; i++) {
	    outbuf[i] = (u8)i;
    }
    bzero((void *)inbuf, sizeof inbuf);

    nc = osUsbDevWrite(handle, &outbuf[0], sizeof outbuf, (u64)0);
    PRINTF("USB%d: devWrite returns %d\n", which, nc);

    nc = osUsbDevRead(handle, &inbuf[0], sizeof inbuf, (u64)0);
    PRINTF("USB%d: devRead returns %d\n", which, nc);

    if (nc < sizeof inbuf) {
        PRINTF("USB%d: devRead short (expected %d)\n", which, sizeof inbuf);
    }

    errs = 0;
    for (i = 0; i < nc; i++)
	if (inbuf[i] != outbuf[i]) {
	    errs++;
            PRINTF("USB%d: echo[%d] is 0x%x, should be 0x%x\n", 
	        which, i, inbuf[i], outbuf[i]);
	}

    if (errs > 0)
        PRINTF("USB%d: %d echo errs total\n", which, errs);

    return;
}

#include "rdb.h"

void
do_rdb_test(s32 which, OSUsbHandle handle)
{
    unsigned char data[4];
    unsigned char in[64];
    int i, rv;

    PRINTF("USB%d: test RDB device in loopback mode\n", which);

    data[0] = RDB_TYPE_HtoG_DATA << 2 | 3;
    data[1] = 0xa1;
    data[2] = 0xb2;
    data[3] = 0xc3;

    rv = osUsbDevWrite(handle, data, 4, 0);
    PRINTF("USB0: DevWrite 4 to RDB wrote %d\n", rv);

    bzero((void *)in, sizeof in);
    rv = osUsbDevRead(handle, in, sizeof in, 0);
    PRINTF("USB0: DevRead from RDB got %d\nRet data: ", rv);

    for (i = 0; i < rv; i++)
	PRINTF("%02x ", in[i]);
    
    PRINTF("\n");

    /*
     * Now try big buffers
     */
    do_echo_test(which, handle);
    return;
}

void
do_msd_test(s32 which, OSUsbHandle handle)
{
    PRINTF("USB%d: No recognized device found \n", which);
    return;
}

static void 
mainproc(char *arg)			/* priority of 7 */
{
    s32 ndevs;
    s32 which = 1;	/* 1 is always host, 0 goes both ways */
    OSUsbHandle handle;

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

    /*
     * Call USB device query for the designated controller
     */
    PRINTF("Call DevQuery for controller %d\n", which);
    for (;;) {
    	ndevs = osUsbDevQuery(which, &UsbInfo[0], MAX_USB_DEVICES);
	if (ndevs > 0) {
    	    PRINTF("USB Controller %d has %d devices\n", which, ndevs);
	    break;
	}
	/* Device initialization not done, chill out and try again */
	osYieldThread();
    }

    if (ndevs > 0) {
	PRINTF("USB%d: found device vend 0x%x, prod 0x%x, class 0x%x\n",
	    which,
	    UsbInfo[0].ua_vendor,
	    UsbInfo[0].ua_product,
	    UsbInfo[0].ua_class);
    }
   
    handle = NULL;
    if (osUsbDevGetHandle(which, &UsbInfo[0], &handle)) {
	    PRINTF("USB%d: GetHandle fails\n", which);
    }
    PRINTF("USB%d: GetHandle returns 0x%x\n", which, handle);

    /*
     * Assume the echo device for now
     */
    if (UsbInfo[0].ua_vendor == 0xa305 &&
        UsbInfo[0].ua_product == 0x0100) {
       do_echo_test(which, handle);
    } else if (UsbInfo[0].ua_vendor == 0x3dbb &&
        UsbInfo[0].ua_product == 0xdbbb) {
       do_rdb_test(which, handle);
    } else {
       do_msd_test(which, handle);
    }

    /*XXX? brom*/
    PRINTF("=> done\n");

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