#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <time.h>
#include "libsys.h"
#include "cfiflash.h"
#include "hpi.h"

static volatile UINT32 _g_uart_msg;
static volatile UINT32 _g_rst;
static volatile UINT32 _g_sus;
static volatile UINT32 _g_cfg;

void uart_handler(void)
{
	volatile UINT32 *pUART_stat = (UINT32*)SYS_UART_STAT;
	volatile UINT32 *pUART_data = (UINT32*)SYS_UART_DATA;

	while((SYS_PS2_BIT_RX_AVAIL & *pUART_stat))
	{
		_g_uart_msg = *pUART_data;
	}

}

void PrintDevRegs(UINT devid)
{
	int i;
	UINT32 reg_base1, reg_base2, reg_addr;

	if ((devid < 1) || (devid > 2))
		return;

	printf("********************************************************\n");
	printf("Device %d status\n", devid);
	printf("********************************************************\n");
	reg_base1 = 0x0200 + (devid-1)*0x80;
	reg_base2 = 0xC080 + (devid-1)*0x20;
	reg_addr = reg_base2 + 4;
	printf("Port select (%4.4X)   : %4.4X\n", reg_addr, hpi_read_reg(reg_addr));

	reg_addr = reg_base2 + 10;
	printf("USB control (%4.4X)   : %4.4X\n", reg_addr, hpi_read_reg(reg_addr));

	reg_addr = reg_base2 + 12;
	printf("Int. enable (%4.4X)   : %4.4X\n", reg_addr, hpi_read_reg(reg_addr));

	reg_addr = reg_base2 + 14;
	printf("Address     (%4.4X)   : %4.4X\n", reg_addr, hpi_read_reg(reg_addr));

	reg_addr = reg_base2 + 16;
	printf("Status      (%4.4X)   : %4.4X\n", reg_addr, hpi_read_reg(reg_addr));

	reg_addr = reg_base2 + 18;
	printf("Frame # reg (%4.4X)   : %4.4X\n", reg_addr, hpi_read_reg(reg_addr));

	reg_addr = reg_base2 + 20;
	printf("SOF/EOP cnt (%4.4X)   : %4.4X\n", reg_addr, hpi_read_reg(reg_addr));

	for (i=0; i < 8; i++)
	{
		printf("EP# %d\n", i);
		reg_addr = reg_base1 + 16*i + 0;
		printf("Control (%4.4X)       : %4.4X\n", reg_addr, hpi_read_reg(reg_addr));

		reg_addr = reg_base1 + 16*i + 2;
		printf("Address (%4.4X)       : %4.4X\n", reg_addr, hpi_read_reg(reg_addr));

		reg_addr = reg_base1 + 16*i + 4;
		printf("Count (%4.4X)         : %4.4X\n", reg_addr, hpi_read_reg(reg_addr));

		reg_addr = reg_base1 + 16*i + 6;
		printf("Status (%4.4X)        : %4.4X\n", reg_addr, hpi_read_reg(reg_addr));

		reg_addr = reg_base1 + 16*i + 8;
		printf("Count result (%4.4X)  : %4.4X\n", reg_addr, hpi_read_reg(reg_addr));

	}
}

typedef struct _sdev_descr_t
{
	UINT8 size;					// length
	UINT8 type;					// desc type
	UINT16 usb_spec;			// USB spec
	UINT8 devclass;				// device class
	UINT8 subclass;				// sub class
	UINT8 protocol;				// protocol
	UINT8 max_packet_size;		// max packet size for endpoint 0
	UINT16 vendor_id;			// Vendor ID
	UINT16 product_id;			// Product ID
	UINT16 device_id;			// device release number
	UINT8 man_str_index;		// index of manufacture string
	UINT8 prod_str_index;		// index of product string
	UINT8 sn_str_index;			// index of serial number string
	UINT8 nconfs;				// number of configurations
} __attribute__ ((__packed__)) dev_descr_t;

typedef struct _sconf_descr_t
{
	UINT8 size;					// length of this config
	UINT8 type;					// desc type
	UINT16 total_size;			// Total configuration desc length including this config and following descriptions
	UINT8 nifaces;				// Number of interface descriptions following this
	UINT8 conf_id;				// config number
	UINT8 conf_str_index;		// index of string describing config
	UINT8 attr;					// attributes (e.g. bus powered)
	UINT8 current;				// 2mA x <current> (max. 250 => 500mA)
} __attribute__ ((__packed__)) conf_descr_t;

typedef struct _siface_descr_t
{
	UINT8 size;					// length of this config
	UINT8 type;					// desc type
	UINT8 base;					// base number
	UINT8 alt;					// alt
	UINT8 neps;					// number of endpoint description following this
	UINT8 iface_class;			// interface class (vendor)
	UINT8 subclass;				// subclass
	UINT8 iface_proto;			// interface proto (vendor)
	UINT8 iface_str_index;
} __attribute__ ((__packed__)) iface_descr_t;

typedef struct _sep_descr_t
{
	UINT8 size;					// length of this config
	UINT8 type;					// type (endpoint)
	UINT8 type_num;				// type/number (Host use WriteFile)
	UINT8 bulk;					// Bulk
	UINT16 pkt_size;			// packet size
	UINT8 interval;				// interval
} __attribute__ ((__packed__)) ep_descr_t;

typedef struct _sotg_descr_t
{
	UINT8 size;					// length of this config
	UINT8 type;					// type (OTG)
	UINT8 hnp_srp;				// HNP|SRP support
} __attribute__ ((__packed__)) otg_descr_t;

typedef struct _sstr_descr_hdr_t
{
	UINT8 size;					// length of this config
	UINT8 type;					// type
} __attribute__ ((__packed__)) str_descr_hdr_t;

typedef struct _scfg_inst_t
{
	usb_irp_t *pIRP_rx;
	usb_irp_t *pIRP_tx;
} cfg_inst_t;

UINT32 conf_write(UINT8 *pConfDescr, UINT32 neps, ep_descr_t *pEP)
{
	UINT32 pos, size;
	conf_descr_t cfg;
	iface_descr_t iface;
	otg_descr_t otg_descr;

	// Fill config description
	cfg.size = sizeof(conf_descr_t);
	cfg.type = 2;
	cfg.total_size = sizeof(conf_descr_t) + sizeof(iface_descr_t) + neps*sizeof(ep_descr_t) + sizeof(otg_descr_t);
	cfg.nifaces = 1;	// can handle only one interface per config
	cfg.conf_id = 1;
	cfg.conf_str_index = 0;
	cfg.attr = 0x80;
	cfg.current = 50;

	// Copy config description
	pos = 0;
	size = sizeof(conf_descr_t);
	memcpy(&pConfDescr[pos], &cfg, size);

	// Fill interface description
	iface.size = sizeof(iface_descr_t);
	iface.type = 4;
	iface.base = 0;
	iface.alt = 0;
	iface.neps = neps;
	iface.iface_class = 0;
	iface.subclass = 0;
	iface.iface_proto = 0;
	iface.iface_str_index = 0;

	// Copy interface description
	pos += size;
	size = sizeof(iface_descr_t);
	memcpy(&pConfDescr[pos], &iface, size);

	// Copy N end point descriptions
	pos += size;
	size = neps*sizeof(ep_descr_t);
	memcpy(&pConfDescr[pos], pEP, size);

	// Fill OTG description
	otg_descr.size = sizeof(otg_descr_t);
	otg_descr.type = 9;		// OTG
	otg_descr.hnp_srp = 3;	// HNP|SRP supported

	// Copy OTG description
	pos += size;
	size = sizeof(ep_descr_t);
	memcpy(&pConfDescr[pos], &otg_descr, size);

	pos += size;

	return pos;
}

UINT32 str_write(UINT8 *pStrDescr, UINT8 *pStr0, UINT8 *pStr1, UINT8 *pStr2)
{
	int i;
	UINT32 pos, size, strsize;
	str_descr_hdr_t *pStrHdr;
	UINT16 *pStr;

	pos = 0;
	size = sizeof(str_descr_hdr_t);
	strsize = 2;
	pStrHdr = (str_descr_hdr_t*)&pStrDescr[pos];
	pStrHdr->size = size + strsize;
	pStrHdr->type = 3;

	pos += size;
	pStr = (UINT16*)&pStrDescr[pos];

	*pStr = 0x0409;

	// String 0
	pos += size;
	size = sizeof(str_descr_hdr_t);
	strsize = strlen(pStr0);

	pStrHdr = (str_descr_hdr_t*)&pStrDescr[pos];
	pStrHdr->size = sizeof(str_descr_hdr_t) + 2*strsize;
	pStrHdr->type = 3;

	pos += size;
	size = 2*strsize;
	pStr = (UINT16*)&pStrDescr[pos];

	for (i=0; i < strsize; i++)
		pStr[i] = (UINT16)pStr0[i];

	// String 1
	pos += size;
	size = sizeof(str_descr_hdr_t);
	strsize = strlen(pStr1);

	pStrHdr = (str_descr_hdr_t*)&pStrDescr[pos];
	pStrHdr->size = sizeof(str_descr_hdr_t) + 2*strsize;
	pStrHdr->type = 3;

	pos += size;
	size = 2*strsize;
	pStr = (UINT16*)&pStrDescr[pos];

	for (i=0; i < strsize; i++)
		pStr[i] = (UINT16)pStr1[i];

	// String 2
	pos += size;
	size = sizeof(str_descr_hdr_t);
	strsize = strlen(pStr2);

	pStrHdr = (str_descr_hdr_t*)&pStrDescr[pos];
	pStrHdr->size = sizeof(str_descr_hdr_t) + 2*strsize;
	pStrHdr->type = 3;

	pos += size;
	size = 2*strsize;
	pStr = (UINT16*)&pStrDescr[pos];

	for (i=0; i < strsize; i++)
		pStr[i] = (UINT16)pStr2[i];

	pos += size;

	return pos;
}

void ep0_func(void *pArg)
{
	UINT32 result;
	UINT16 buf[USB_MAX_IMG_SIZE/2], ep_cntres, ep_status, ep_count, tsize;
	usb_irp_t *pIRP = (usb_irp_t*)pArg;

//	printf("Callback with pArg = %8.8X\n", (UINT32)pArg);
	ep_status = hpi_read_reg (SUSB2_REG_EP_STATUS0);
	ep_cntres = hpi_read_reg (SUSB2_REG_EP_CNTRES0);
	ep_count = hpi_read_reg (SUSB2_REG_EP_COUNT0);

	if (ep_status & 0x10)
	{
		printf("Setup-flag detected. Status = %4.4X\n", ep_status);
	}
}

void ep1_func(void *pArg)
{
	UINT32 result;
	UINT16 buf[USB_MAX_IMG_SIZE/2], ep_cntres, ep_status, ep_count, tsize;
	usb_irp_t *pIRP = (usb_irp_t*)pArg;

//	printf("Callback with pArg = %8.8X\n", (UINT32)pArg);
	ep_status = hpi_read_reg (SUSB2_REG_EP_STATUS1);
	ep_cntres = hpi_read_reg (SUSB2_REG_EP_CNTRES1);
	ep_count = hpi_read_reg (SUSB2_REG_EP_COUNT1);

	tsize = pIRP->tsize;
	if (ep_status & 0x0020)
	{
		printf("Length exception result %4.4X with count = %d\n", ep_status, (INT16)ep_cntres);
		if (ep_status & 0x0400)
			tsize = (UINT16)((INT16)tsize - (INT16)ep_cntres);
		if (ep_status & 0x0800)
			tsize = (UINT16)((INT16)tsize - (INT16)ep_cntres);

	}
	result = hpi_read_ram(pIRP->cy_req.addr, buf, tsize);
	usb_irp_enqueue(pIRP);
	fifo_write(&pIRP->fifo, (UINT8*)buf, result, 0, FIFO_BLOCK);

//	printf("Read %d bytes\n", result);

}

void ep2_func(void *pArg)
{
	UINT32 result;
	UINT16 buf[USB_MAX_IMG_SIZE/2], ep_cntres, ep_status, ep_count, tsize;
	usb_irp_t *pIRP = (usb_irp_t*)pArg;

//	printf("Callback with pArg = %8.8X\n", (UINT32)pArg);
	ep_status = hpi_read_reg (SUSB2_REG_EP_STATUS2);
	ep_cntres = hpi_read_reg (SUSB2_REG_EP_CNTRES2);
	ep_count = hpi_read_reg (SUSB2_REG_EP_COUNT2);

	tsize = pIRP->tsize;
	if (ep_status & 0x0020)
	{
		printf("Length exception result %4.4X with count = %d\n", ep_status, (INT16)ep_cntres);
		if (ep_status & 0x0400)
			tsize = (UINT16)((INT16)tsize - (INT16)ep_cntres);
		if (ep_status & 0x0800)
			tsize = (UINT16)((INT16)tsize - (INT16)ep_cntres);
	}

	pIRP->tx_in_progress = 0;
	result = fifo_read(&pIRP->fifo, (UINT8*)buf, pIRP->tsize, 0, FIFO_NONBLOCK);
	if (!result)
		return;

	pIRP->cy_req.size = result;
	hpi_write_ram(pIRP->cy_req.addr, (UINT16*)buf, result);

	usb_irp_enqueue(pIRP);

//	printf("Wrote %d bytes\n", result);

}

void rst_func(void *pArg)
{
	volatile int *pLED = (int*)SYS_LED_PORT;

//	printf("Callback with pArg = %8.8X\n", (UINT32)pArg);
	sputs("SUSB_RST_MSG\n");
///	hpi_comm_reset();

	_g_rst = 1;

	*pLED = 0x40000000 + 1;

}

void sof_func(void *pArg)
{
	volatile int *pLED = (int*)SYS_LED_PORT;

//	printf("Callback with pArg = %8.8X\n", (UINT32)pArg);
	sputs("SUSB_SOF_MSG\n");

	*pLED = 0x40000000 + 2;

}

void cfg_func(void *pArg)
{
	volatile int *pLED = (int*)SYS_LED_PORT;
	cfg_inst_t *pCFG = (cfg_inst_t*)pArg;
	usb_irp_t *pIRP_rx = (usb_irp_t*)pCFG->pIRP_rx;
	usb_irp_t *pIRP_tx = (usb_irp_t*)pCFG->pIRP_tx;

//	printf("Callback with pArg = %8.8X\n", (UINT32)pArg);
	sputs("SUSB_CFG_MSG\n");
	_g_cfg = 1;
	usb_irp_enqueue(pIRP_rx);
	fifo_flush(&pIRP_rx->fifo);
	fifo_flush(&pIRP_tx->fifo);
	pIRP_tx->tx_in_progress = 0;
	*pLED = 0;

}

void sus_func(void *pArg)
{
	volatile int *pLED = (int*)SYS_LED_PORT;

//	printf("Callback with pArg = %8.8X\n", (UINT32)pArg);
	sputs("SUSB_SUS_MSG\n");

	*pLED = 0x40000000 + 3;

	_g_sus = 1;

}

void id_func(void *pArg)
{
	volatile int *pLED = (int*)SYS_LED_PORT;

//	printf("Callback with pArg = %8.8X\n", (UINT32)pArg);
	sputs("SUSB_ID_MSG\n");

	*pLED = 0x40000000 + 4;

}

void vbus_func(void *pArg)
{
	volatile int *pLED = (int*)SYS_LED_PORT;

//	printf("Callback with pArg = %8.8X\n", (UINT32)pArg);
	sputs("SUSB_VBUS_MSG\n");

	*pLED = 0x40000000 + 5;

}

UINT32 usb_init_descr(void)
{
	UINT32 result;

	UINT16 buffer16[0x100];
	UINT16 addr;
	dev_descr_t dev_descr = {sizeof(dev_descr_t), 1, 0x0200, 0xFF, 0, 0, 8, 0x04B4, 0x7200, 0x0099, 1, 2, 3, 1};
	ep_descr_t ep_descr[2] = {{sizeof(ep_descr_t), 5, 1, 2, 64, 0}, {sizeof(ep_descr_t), 5, 0x82, 2, 64, 0}};

	// Set device descriptor
	addr = 0xf516;	// default
	addr = 0xA00;
	hpi_write_ram(addr, (UINT16*)&dev_descr, sizeof(dev_descr_t));
	hpi_write_reg(2*SUSB2_DEVICE_DESCRIPTOR_VEC, addr);

	// Set new configuration descriptor

	// Write config and interface descriptor
	result = conf_write((UINT8*)buffer16, sizeof(ep_descr)/sizeof(ep_descr_t), ep_descr);

	addr = 0xf528;	// default
	addr = 0xB00;
	hpi_write_ram(addr, (UINT16*)buffer16, result);
	hpi_write_reg(2*SUSB2_CONFIGURATION_DESCRIPTOR_VEC, addr);

	// Set string description
	result = str_write((UINT8*)buffer16, "JDI Inc.", "JDI-USB (MIPS)", "311070");
	addr = 0xf528;	// default
	addr = 0xC00;
	hpi_write_ram(addr, (UINT16*)buffer16, result);
	hpi_write_reg(2*SUSB2_STRING_DESCRIPTOR_VEC, addr);

	return 0;
}

UINT32 usb_force_reconnect(UINT32 port)
{
	UINT32 result;
	UINT16 addr;

	if (port >= USB_MAX_NUM_PORTS)
		return -1;

	if (port == 0)
	{
		// R1 = 0 : Full speed
		// R2 = 1 : SIE1
		addr = hpi_read_reg(SUSB1_REG_USBCTRL);
		hpi_write_reg(SUSB1_REG_USBCTRL, addr | 0x0010);
		result = hpi_comm_exec_int(SUSB_INIT_INT, 2, R1, 0, R2, 1);
		hpi_write_reg(SUSB1_REG_USBCTRL, addr);
	}
	else
	{
		// R1 = 0 : Full speed
		// R2 = 2 : SIE2
		addr = hpi_read_reg(SUSB2_REG_USBCTRL);
		hpi_write_reg(SUSB2_REG_USBCTRL, addr | 0x0010);
		result = hpi_comm_exec_int(SUSB_INIT_INT, 2, R1, 0, R2, 2);
		hpi_write_reg(SUSB2_REG_USBCTRL, addr);
	}
	return result;
}

UINT32 usb_recv(usb_irp_t *pObj, UINT8 *pData, UINT32 len, UINT32 timeout)
{
	UINT32 result;

	result = fifo_read(&pObj->fifo, pData, len, timeout, FIFO_BLOCK);

	return result;
}

UINT32 usb_send(usb_irp_t *pObj, UINT8 *pData, UINT32 len, UINT32 timeout)
{
	UINT32 result;
	UINT8 buf[USB_MAX_IMG_SIZE];

	// Fill-up FIFO
	result = fifo_write(&pObj->fifo, pData, len, 0, FIFO_NONBLOCK);
	if (!result)
		return 0;

	// Trigger TX
	if (!pObj->tx_in_progress)
	{
		pObj->cy_req.size = fifo_read(&pObj->fifo, (UINT8*)buf, pObj->tsize, 0, FIFO_NONBLOCK);
		hpi_write_ram(pObj->cy_req.addr, (UINT16*)buf, pObj->cy_req.size);
		usb_irp_enqueue(pObj);
	}

	// If necessary, write rest to FIFO
	result += fifo_write(&pObj->fifo, &pData[result], len - result, timeout, FIFO_BLOCK);

	return result;

}

#define TEST_SIZE				(2*1024*1024)
#define TRANSFER_SIZE			64
#define FLASH_IMAGE_SIZE		(2*1024*1024)

int main(void)
{
	int i;
	UINT32 result, sync, cmd, len, buf_count, led_count, remain, tx_cnt, flash_offset;
	UINT8 buffer8[2048];
	UINT16 buffer16[0x2000];
	UINT16 addr;
	volatile UINT32 *pLED = (UINT32*)SYS_LED_PORT;
	volatile UINT32 *pUART0_stat = (UINT32*)SYS_UART0_STAT;
	UINT32 start, end;
	UINT32 *pFlash32;
	UINT8 *pFlash8;
	UINT32 buffer32[16], *pBuf;
	UINT64 *pBuf64;
	usb_irp_t irp_rx, irp_tx;
	fifo_t fifo_rx, fifo_tx;
	cfg_inst_t cfg_inst;
	flash_t flash;
	volatile UINT32 *pVGA_ctrl = (UINT32*)SYS_VGA_CTRL;
	volatile UINT32 *pVGA_moffs = (UINT32*)SYS_VGA_MOFFS;

	_g_rst = 0;
	_g_sus = 0;
	_g_cfg = 0;

	usb_init();

	// enable EP#0 callback
	usb_callback_register(1, SUSB_EP0_MSG, ep0_func, NULL);

	// enable EP#1 callback
	usb_irp_register(&irp_rx, 1, 1, TRANSFER_SIZE, 1);
	usb_callback_register(1, SUSB_EP1_MSG, ep1_func, &irp_rx);

	// enable EP#2 callback
	usb_irp_register(&irp_tx, 1, 2, TRANSFER_SIZE, 0);
	usb_callback_register(1, SUSB_EP2_MSG, ep2_func, &irp_tx);

	usb_callback_register(1, SUSB_RST_MSG, rst_func, NULL);
	usb_callback_register(1, SUSB_SOF_MSG, sof_func, NULL);
	cfg_inst.pIRP_rx = &irp_rx;
	cfg_inst.pIRP_tx = &irp_tx;
	usb_callback_register(1, SUSB_CFG_MSG, cfg_func, &cfg_inst);
	usb_callback_register(1, SUSB_SUS_MSG, sus_func, NULL);
	usb_callback_register(1, SUSB_ID_MSG, id_func, NULL);
	usb_callback_register(1, SUSB_VBUS_MSG, vbus_func, NULL);

	printf("HPI-Test\n\n");

	if (IS_ERROR(hpi_init()))
	{
		sputs("hpi_init(): error\n");
		return 1;
	}

	printf("cy67k3_reset\n");
	cy67k3_reset();

	while(!_g_rst);

	printf("hpi_comm_reset\n");
	hpi_comm_reset();

	while(!_g_sus);

	printf("usb_init_descr\n");
	usb_init_descr();
	printf("hpi_comm_exec_int\n");
	hpi_comm_exec_int(SUSB_INIT_INT, 2, R1, 0, R2, 2);
	printf("usb_force_reconnect\n");
	usb_force_reconnect(1);

	while(!_g_cfg);

	printf("USB-Ready\n");

	printf("CY-Read..");
	start = clock();
	for (i=0; i < TEST_SIZE; i+=0x2000)
		hpi_read_ram(0x2000, buffer16, 0x2000);

	end = clock();
	printf("done (%.2f Mbyte/s)\n", (double)TEST_SIZE/(1000*(end-start)));

	printf("CY-Write..");
	start = clock();
	for (i=0; i < TEST_SIZE; i+=0x2000)
		hpi_write_ram(0x2000, buffer16, 0x2000);

	end = clock();
	printf("done (%.2f Mbyte/s)\n", (double)TEST_SIZE/(1000*(end-start)));

	sputs("HPI-Status   : ");
	print_word(cy67k3_read_HPI_STATUS());
	sputs("\n");

	sputs("HPI BP       : ");
	print_word(hpi_read_reg(HPI_REG_BP));
	sputs("\n");

	sputs("HPI IRR      : ");
	print_word(hpi_read_reg(HPI_REG_INTROUTE));
	sputs("\n");

	sputs("CPU Revision : ");
	print_word(hpi_comm_read_ctrl_reg(USB_REG_REVISON));
	sputs("\n");

	hpi_comm_write_ctrl_reg(USB_REG_SPEED, 0x0000, LOGIC_DIRECT);
	sputs("CPU Speed    : ");
	print_word(hpi_comm_read_ctrl_reg(USB_REG_SPEED));
	sputs("\n");

//	usb_init_descr();
//	usb_force_reconnect(1);

	// enable RX interrupt
	*pUART0_stat |= (1 << 6);
	interrupt_register(3, uart_handler);
	interrupt_enable(3);

	// Enable EP#1
	hpi_write_reg(SUSB2_REG_EP_COUNT1, TRANSFER_SIZE);
	hpi_write_reg(SUSB2_REG_EP_CTRL1, 2);
	hpi_write_reg(SUSB2_REG_EP_CNTRES1, 0);
	hpi_write_reg(SUSB2_REG_EP_STATUS1, 0);

	// Enable EP#2
	hpi_write_reg(SUSB2_REG_EP_COUNT2, TRANSFER_SIZE);
	hpi_write_reg(SUSB2_REG_EP_CTRL2, 2);
	hpi_write_reg(SUSB2_REG_EP_CNTRES2, 0);
	hpi_write_reg(SUSB2_REG_EP_STATUS2, 0);

	led_count = 0;
	buf_count = 0;

	pBuf64 = (UINT64*)malloc(FLASH_IMAGE_SIZE);
	pBuf = (UINT32*)pBuf64;

	*pVGA_moffs = (UINT32)pBuf64;
	*pVGA_ctrl |= SYS_VGA_BIT_MSTEN;

	if (IS_ERROR(flash_find(&flash, SYS_FLASH_IO)))
	{
		printf("Cannot find flash device. Exit now!\n");
		return 1;
	}

	// Get flash offset by value of DIP-switch
	flash_offset = flash_get_offset_by_blocknum(&flash, flash.info.nblocks-FLASH_IMAGE_SIZE/flash.info.blocksize);
	printf("flash_offset: %08X\n", flash_offset);
	pFlash32 = (UINT32*)(SYS_FLASH_IO + flash_offset);
	pFlash8 = (UINT8*)pFlash32;

	while(1)
	{
		switch (_g_uart_msg)
		{
			case '1':
			_g_uart_msg = 0;
			PrintDevRegs(1);
			printf("\nSIEmsg1: %4.4X\n", hpi_read_reg(HPI_REG_SIE1MSG));
			break;

			case '2':
			_g_uart_msg = 0;
			PrintDevRegs(2);
			printf("\nSIEmsg2: %4.4X\n", hpi_read_reg(HPI_REG_SIE2MSG));
			break;

			default:
			break;
		}
		result = usb_recv(&irp_rx, (UINT8*)&sync, 4, 100);
		if (!result)
			continue;

		*pLED = led_count++;

		if (sync == 0xC24755AA)
		{
			printf("Sync found\n");
			result = usb_recv(&irp_rx, (UINT8*)&cmd, 4, 1000);
			if (!result)
				continue;

			result = usb_recv(&irp_rx, (UINT8*)&len, 4, 1000);
			if (!result)
				continue;

			printf("Command %d with len %d found\n",cmd, len);
			switch(cmd)
			{
				case 0x00000001:
				result = usb_recv(&irp_rx, NULL, len, 1000);
				if (result)
				{
					printf("Read %d bytes\n", result);
				}
				break;

				case 0x00000002:
				remain = len;
				tx_cnt = 0;
				while(remain)
				{
					len = sizeof(buffer8);
					if (remain < len)
						len = remain;

					for (i=0; i < len; i++)
						buffer8[i] = buf_count+i;

					result = usb_send(&irp_tx, buffer8, len, 1000);
					if (!result)
						break;
					tx_cnt += result;
					remain -= result;
					buf_count++;
				}
				printf("Wrote %d bytes\n", tx_cnt);
				break;

				case 0x00000003:
				remain = len;
				tx_cnt = 0;
				while(remain)
				{
					len = 256;
					if (remain < len)
						len = remain;

					result = usb_send(&irp_tx, (UINT8*)&pFlash8[tx_cnt], len, 1000);
					if (!result)
						break;
					tx_cnt += result;
					remain -= result;
					buf_count++;
				}
				printf("Wrote %d bytes\n", tx_cnt);
				break;

				case 0x00000004:
				*pVGA_ctrl &= ~SYS_VGA_BIT_MSTEN;
				result = usb_recv(&irp_rx, (UINT8*)pBuf, len, 1000);
				if (result)
				{
					printf("Read %d bytes\n", result);
				}

				len = result;
				if (len > FLASH_IMAGE_SIZE)
				{
					printf("Cannot write flash: image to large!\n");
					break;
				}

				printf("Flash erase...");
				result = flash_erase(&flash, flash_offset, len);
				if (IS_ERROR(result))
				{
					printf("failed (%08X)\n", result);
					break;
				}
				printf("done\n");

				printf("Flash write...");
				result = flash_program(&flash, flash_offset, (UINT8*)pBuf, len);
				if (IS_ERROR(result))
				{
					printf("failed (%08X)\n", result);
					break;
				}
				printf("done\n");

				printf("Flash verify...");
				result = flash_verify(&flash, flash_offset, (UINT8*)pBuf, len);
				if (IS_ERROR(result))
				{
					printf("failed (%08X)\n", result);
					break;
				}
				printf("passed\n");
				*pVGA_ctrl |= SYS_VGA_BIT_MSTEN;
				break;

				case 0x000000FF:
				usb_force_reconnect(1);
				break;

				default:
				break;
			}
		}
	}

	return 0;
}

//			pMem = (UINT8*)0x40000000;
//			result = fifo_write(&fifo_tx, pMem, 640, 0, FIFO_NONBLOCK);
//			pMem += result;
//			usb_irp_enqueue(&irp_tx);