#include <stdio.h>
#include <stdlib.h>
#include <stdarg.h>
#include <string.h>
#include "libsys.h"
#include "gpio.h"
#include "hpi.h"

static volatile uint32_t _g_int_active;
static volatile uint32_t _g_mbx_return;
static volatile uint32_t _g_mbx_in_flag;

extern gpio_if_t _g_gpio;

// ---------------------------------------------------------
// Globals
// ---------------------------------------------------------
typedef struct _susb_t
{
	fp_t fptr_ept[USB_MAX_NUM_EPT];
	void *aptr_ept[USB_MAX_NUM_EPT];

	fp_t fptr_rst;
	void *aptr_rst;

	fp_t fptr_sof;
	void *aptr_sof;

	fp_t fptr_cfg;
	void *aptr_cfg;

	fp_t fptr_sus;
	void *aptr_sus;

	fp_t fptr_id;
	void *aptr_id;

	fp_t fptr_vbus;
	void *aptr_vbus;

} usb_t;

static usb_t _g_usb[USB_MAX_NUM_PORTS];
//static usb_irp_t *_g_usb_irp_ptr[USB_MAX_NUM_PORTS][USB_MAX_NUM_EPT];

// ---------------------------------------------------------
// HPI ISR
// ---------------------------------------------------------
void cy67k3_isr(struct xcptcontext *xcp)
{
	int32_t i;
	uint16_t sie_msg, hpi_status;
	uint32_t port, siemsg_handled, int_handled;
	_g_int_active = 1;
	hpi_status = cy67k3_read_HPI_STATUS();

#ifdef HPI_DEBUG
	sputs("--------------------------------------------------------\n");
	sputs("USB-Interrupt(4)\n");
	sputs("HPI Status   : ");
	print_word(hpi_status);
	sputs("\n");
#endif

//	sputs("MAILBOX (REG): ");

//	cy67k3_write(HPI_ADDRESS, HPI_REG_MAILBOX);
//	print_word(cy67k3_read_HPI_DATA());
//	sputs("\n");

	int_handled = 0;
	if (hpi_status & HPI_STATUS_MBX_IN)
	{
		int_handled = 1;
		_g_mbx_return = cy67k3_read_HPI_MAILBOX();
		_g_mbx_in_flag = 1;
#ifdef HPI_DEBUG
		sputs("MAILBOX (HPI): ");
		print_word(_g_mbx_return);
		sputs("\n");
#endif
	}

	if (hpi_status & HPI_STATUS_RESET1)
	{
		int_handled = 1;
		hpi_read_reg(SUSB1_REG_STATUS);
		hpi_comm_exec_int(SUSB_INIT_INT, 2, R1, 0, R2, 1);
#ifdef HPI_DEBUG
		sputs("RESET 1\n");
#endif
	}

	if (hpi_status & HPI_STATUS_RESET2)
	{
		int_handled = 1;
		hpi_read_reg(SUSB2_REG_STATUS);
		hpi_comm_exec_int(SUSB_INIT_INT, 2, R1, 0, R2, 2);
#ifdef HPI_DEBUG
		sputs("RESET 2\n");
#endif
	}

	if (hpi_status & HPI_STATUS_DONE1)
	{
		int_handled = 1;
#ifdef HPI_DEBUG
		sputs("DONE 1\n");
#endif
	}

	if (hpi_status & HPI_STATUS_DONE2)
	{
		int_handled = 1;
#ifdef HPI_DEBUG
		sputs("DONE 2\n");
#endif
	}

	if (hpi_status & HPI_STATUS_SOFEOP1)
	{
		int_handled = 1;
		hpi_read_reg(SUSB1_REG_STATUS);
#ifdef HPI_DEBUG
		sputs("SOF/EOP 1\n");
#endif
	}

	if (hpi_status & HPI_STATUS_SOFEOP2)
	{
		int_handled = 1;
		hpi_read_reg(SUSB2_REG_STATUS);
#ifdef HPI_DEBUG
		sputs("SOF/EOP 2\n");
#endif
	}

	port = 0;
	siemsg_handled = 0;
	if (hpi_status & HPI_STATUS_SIEMSG1)
	{
		int_handled = 1;
		sie_msg = hpi_read_reg(HPI_REG_SIE1MSG);

		// Reset message register
		hpi_write_reg(HPI_REG_SIE1MSG, 0x0000);

		if (sie_msg & SUSB_RST_MSG)
		{
			siemsg_handled = 1;
			if (_g_usb[port].fptr_rst)
				(_g_usb[port].fptr_rst)(_g_usb[port].aptr_rst);
		}

		if (sie_msg & SUSB_SOF_MSG)
		{
			siemsg_handled = 1;
			if (_g_usb[port].fptr_sof)
				(_g_usb[port].fptr_sof)(_g_usb[port].aptr_sof);
		}

		if (sie_msg & SUSB_CFG_MSG)
		{
			siemsg_handled = 1;
			if (_g_usb[port].fptr_cfg)
				(_g_usb[port].fptr_cfg)(_g_usb[port].aptr_cfg);
		}

		if (sie_msg & SUSB_SUS_MSG)
		{
			siemsg_handled = 1;
			if (_g_usb[port].fptr_sus)
				(_g_usb[port].fptr_sus)(_g_usb[port].aptr_sus);
		}

		if (sie_msg & SUSB_ID_MSG)
		{
			siemsg_handled = 1;
			if (_g_usb[port].fptr_id)
				(_g_usb[port].fptr_id)(_g_usb[port].aptr_id);
		}

		if (sie_msg & SUSB_VBUS_MSG)
		{
			siemsg_handled = 1;
			if (_g_usb[port].fptr_vbus)
				(_g_usb[port].fptr_vbus)(_g_usb[port].aptr_vbus);
		}

		for (i=0; i < 8; i++)
		{
			if (sie_msg & (SUSB_EP0_MSG << i))
			{
				siemsg_handled = 1;
				if (_g_usb[port].fptr_ept[i])
					(_g_usb[port].fptr_ept[i])(_g_usb[port].aptr_ept[i]);
			}
		}
		if (!siemsg_handled)
		{
#ifdef HPI_DEBUG
			sputs("Unhandled SIE1 message ");
			print_word(sie_msg);
			sputs("!\n");
#endif
		}

	}
	port = 1;
	siemsg_handled = 0;
	if (hpi_status & HPI_STATUS_SIEMSG2)
	{
		int_handled = 1;
		sie_msg = hpi_read_reg(HPI_REG_SIE2MSG);

		// Reset message register
		hpi_write_reg(HPI_REG_SIE2MSG, 0x0000);

		if (sie_msg & SUSB_RST_MSG)
		{
			siemsg_handled = 1;
			if (_g_usb[port].fptr_rst)
				(_g_usb[port].fptr_rst)(_g_usb[port].aptr_rst);
		}

		if (sie_msg & SUSB_SOF_MSG)
		{
			siemsg_handled = 1;
			if (_g_usb[port].fptr_sof)
				(_g_usb[port].fptr_sof)(_g_usb[port].aptr_sof);
		}

		if (sie_msg & SUSB_CFG_MSG)
		{
			siemsg_handled = 1;
			if (_g_usb[port].fptr_cfg)
				(_g_usb[port].fptr_cfg)(_g_usb[port].aptr_cfg);
		}

		if (sie_msg & SUSB_SUS_MSG)
		{
			siemsg_handled = 1;
			if (_g_usb[port].fptr_sus)
				(_g_usb[port].fptr_sus)(_g_usb[port].aptr_sus);
		}

		if (sie_msg & SUSB_ID_MSG)
		{
			siemsg_handled = 1;
			if (_g_usb[port].fptr_id)
				(_g_usb[port].fptr_id)(_g_usb[port].aptr_id);
		}

		if (sie_msg & SUSB_VBUS_MSG)
		{
			siemsg_handled = 1;
			if (_g_usb[port].fptr_vbus)
				(_g_usb[port].fptr_vbus)(_g_usb[port].aptr_vbus);
		}

		for (i=0; i < 8; i++)
		{
			if (sie_msg & (SUSB_EP0_MSG << i))
			{
				siemsg_handled = 1;
				if (_g_usb[port].fptr_ept[i])
					(_g_usb[port].fptr_ept[i])(_g_usb[port].aptr_ept[i]);
			}
		}
		if (!siemsg_handled)
		{
#ifdef HPI_DEBUG
			sputs("Unhandled SIE2 message ");
			print_word(sie_msg);
			sputs("!\n");
#endif
		}

	}
	if (!int_handled)
	{
#ifdef HPI_DEBUG
		sputs("Unhandled interrupt (status = ");
		print_word(hpi_status);
		sputs(")!\n");
#endif
	}

#ifdef HPI_DEBUG
	sputs("--------------------------------------------------------\n");
#endif
	_g_int_active = 0;
}

// ---------------------------------------------------------
// CY7C67300 low-level routines
// ---------------------------------------------------------
void cy67k3_reset(void)
{
	int i;

	gpio_write(&_g_gpio, GPIO_0_MASK_USB, GPIO_0_ALIGN_USB, GPIO_DATA_OFFSET, GPIO_0_USB_RST | GPIO_0_USB_INTEN);

	for (i=0; i <160; i++)
	{
		__asm __volatile
		(
			".set	noreorder\n"
			"nop\n"
			"nop\n"
			"nop\n"
			"nop\n"
			".set	reorder\n"
		);
	}

	gpio_write(&_g_gpio, GPIO_0_USB_RST, GPIO_0_ALIGN_USB, GPIO_DATA_OFFSET, ~GPIO_0_USB_RST);

}

uint16_t cy67k3_read_HPI_DATA(void)
{
	volatile uint32_t *pHpi = (uint32_t*)SYS_USB_DATA;

	return (uint16_t)*pHpi;
}

uint16_t cy67k3_read_HPI_MAILBOX(void)
{
	volatile uint32_t *pHpi = (uint32_t*)SYS_USB_MBX;

	return (uint16_t)*pHpi;
}

uint16_t cy67k3_read_HPI_ADDRESS(void)
{
	volatile uint32_t *pHpi = (uint32_t*)SYS_USB_ADDR;

	return (uint16_t)*pHpi;
}

uint16_t cy67k3_read_HPI_STATUS(void)
{
	volatile uint32_t *pHpi = (uint32_t*)SYS_USB_STATUS;

	return (uint16_t)*pHpi;
}

void cy67k3_write_HPI_DATA(uint16_t data)
{
	volatile uint32_t *pHpi = (uint32_t*)SYS_USB_DATA;

	*pHpi = (uint32_t)data;
}

void cy67k3_write_HPI_MAILBOX(uint16_t data)
{
	volatile uint32_t *pHpi = (uint32_t*)SYS_USB_MBX;

	*pHpi = (uint32_t)data;
}

void cy67k3_write_HPI_ADDRESS(uint16_t data)
{
	volatile uint32_t *pHpi = (uint32_t*)SYS_USB_ADDR;

	*pHpi = (uint32_t)data;
}

void cy67k3_write_HPI_STATUS(uint16_t data)
{
	volatile uint32_t *pHpi = (uint32_t*)SYS_USB_STATUS;

	*pHpi = (uint32_t)data;
}

// ---------------------------------------------------------
// HPI API
// ---------------------------------------------------------
uint32_t hpi_init(void)
{
	int i;
	uint16_t reg;

	// ToDo: Enable HPI interrupt
	_g_int_active = 0;

	hpi_write_reg(HPI_REG_SIE1MSG, 0x0000);
	hpi_write_reg(HPI_REG_SIE2MSG, 0x0000);
	hpi_write_reg(SUSB1_REG_STATUS, 0xFFFF);
	hpi_write_reg(SUSB2_REG_STATUS, 0xFFFF);

	// Init 1
	for(i=0; i < USB_MAX_NUM_EPT; i++)
	{
		hpi_write_reg(SUSB1_REG_EP_ADDR0 + 16*i, 0);
		hpi_write_reg(SUSB1_REG_EP_CNTRES0 + 16*i, 0);
		hpi_write_reg(SUSB1_REG_EP_COUNT0 + 16*i, 0);
		hpi_write_reg(SUSB1_REG_EP_CTRL0 + 16*i, 0);
		hpi_write_reg(SUSB1_REG_EP_STATUS0 + 16*i, 0);
	}
	// Init 2
	for(i=0; i < USB_MAX_NUM_EPT; i++)
	{
		hpi_write_reg(SUSB2_REG_EP_ADDR0 + 16*i, 0);
		hpi_write_reg(SUSB2_REG_EP_CNTRES0 + 16*i, 0);
		hpi_write_reg(SUSB2_REG_EP_COUNT0 + 16*i, 0);
		hpi_write_reg(SUSB2_REG_EP_CTRL0 + 16*i, 0);
		hpi_write_reg(SUSB2_REG_EP_STATUS0 + 16*i, 0);
	}

	hpi_write_reg(HPI_REG_INTROUTE, 0x0000);
	reg = hpi_read_reg(HPI_REG_INTROUTE);
//	hpi_write_reg(HPI_REG_INTROUTE, reg | 0x0202);	// Route reset {1,2} to HPI only
//	hpi_write_reg(HPI_REG_INTROUTE, reg | 0x2800);	// Route SOF/EOP {1,2} to HPI only
//	hpi_write_reg(HPI_REG_INTROUTE, reg | 0x1400);	// Route SOF/EOP {1,2} to CY16 only
	hpi_write_reg(HPI_REG_INTROUTE, reg | 0x3C00);	// Route SOF/EOP {1,2} to HPI and CY16


	interrupt_register(4, cy67k3_isr);
	interrupt_enable(4);
	gpio_write(&_g_gpio, GPIO_0_USB_INTEN, GPIO_0_ALIGN_USB, GPIO_DATA_OFFSET, GPIO_0_USB_INTEN);

	return HPI_NOERROR;
}

void hpi_mbx_write(uint16_t msgcode)
{
	_g_mbx_in_flag = 0;
	cy67k3_write_HPI_MAILBOX(msgcode);
}

uint32_t hpi_mbx_read(void)
{
	uint32_t err;
	uint16_t cy_return;

	if (_g_int_active)
	{
		while (!(cy67k3_read_HPI_STATUS() & HPI_STATUS_MBX_IN));
		cy_return = cy67k3_read_HPI_MAILBOX();
	}
	else
	{
		while(!_g_mbx_in_flag);
		cy_return = _g_mbx_return;
	}

	switch (cy_return)
	{
		case COMM_ACK:
			err = cy_return;
		break;

		case COMM_NAK:
		case COMM_ASYNC:
			err = HPI_ERR_PREFIX_MBX | cy_return;
		break;

		default:
			err = HPI_ERR_PREFIX_MBX | cy_return;
		break;
	}
	return err;
}

uint32_t hpi_check_addr(uint16_t addr)
{
	if ((addr >= 0x0000) && (addr < 0x4000))
		return 0;

	if ((addr >= 0xC080) && (addr < 0xC0BC))
		return 0;

	if (addr >= 0xE000)
		return 0;

	sputs("Invalid address for HPI direct access! Use LCP-command instead!\n");
	return HPI_ERR_INVPARAM;
}

uint32_t hpi_read_reg(uint16_t addr)
{
	uint32_t result;

	result = hpi_check_addr(addr);
	if (IS_ERROR(result))
		return result;

	cy67k3_write_HPI_ADDRESS(addr);
	return cy67k3_read_HPI_DATA();
}

uint32_t hpi_write_reg(uint16_t addr, uint16_t data)
{
	uint32_t result;

	result = hpi_check_addr(addr);
	if (IS_ERROR(result))
		return result;

	cy67k3_write_HPI_ADDRESS(addr);
	cy67k3_write_HPI_DATA(data);
}

uint32_t hpi_write_ram(uint16_t addr, uint16_t *pData, uint32_t len)
{
	int i, num_words;

	if (addr > 0x3FFE)
		return HPI_ERR_INVPARAM;

	if (addr & 1)				// Not 16bit aligned?
		return HPI_ERR_INVPARAM;

	cy67k3_write_HPI_ADDRESS(addr);

	num_words = len/2;
	if (len % 2)
		num_words++;

	for (i=0; i < num_words; i++)
		cy67k3_write_HPI_DATA(pData[i]);

	return len;
}

uint32_t hpi_read_ram(uint16_t addr, uint16_t *pData, uint32_t len)
{
	int i, num_words;

	if (addr > 0x3FFE)
		return HPI_ERR_INVPARAM;

	if (addr & 1)				// Not 16bit aligned?
		return HPI_ERR_INVPARAM;

	cy67k3_write_HPI_ADDRESS(addr);

	num_words = len/2;
	if (len % 2)
		num_words++;

	for (i=0; i < num_words; i++)
		pData[i] = cy67k3_read_HPI_DATA();

	return len;
}

uint32_t hpi_comm_reset(void)
{
	hpi_mbx_write(COMM_RESET);

	return hpi_mbx_read();
}

uint32_t hpi_comm_jump2code(uint16_t addr)
{
	cy67k3_write_HPI_ADDRESS(addr);
	hpi_mbx_write(COMM_JUMP2CODE);

	return hpi_mbx_read();
}

uint32_t hpi_comm_callcode(uint16_t addr)
{
	hpi_write_reg(COMM_CODE_ADDR, addr);

	hpi_mbx_write(COMM_CALL_CODE);

	return hpi_mbx_read();
}

uint32_t hpi_comm_write_ctrl_reg(uint16_t addr, uint16_t data, uint16_t logic)
{

	hpi_write_reg(COMM_CTRL_REG_ADDR, addr);
	hpi_write_reg(COMM_CTRL_REG_DATA, data);
	hpi_write_reg(COMM_CTRL_REG_LOGIC, logic);

	hpi_mbx_write(COMM_WRITE_CTRL_REG);

	return hpi_mbx_read();
}

uint32_t hpi_comm_read_ctrl_reg(uint16_t addr)
{
	uint32_t result;

	hpi_write_reg(COMM_CTRL_REG_ADDR, addr);

	hpi_mbx_write(COMM_READ_CTRL_REG);

	result = hpi_mbx_read();
	if (IS_ERROR(result))
		return result;

	cy67k3_write_HPI_ADDRESS(COMM_CTRL_REG_DATA);
	return (uint32_t)cy67k3_read_HPI_DATA();

}

uint32_t hpi_comm_write_xmem(uint16_t addr, uint8_t *pData, uint16_t len)
{
	return HPI_ERR_NOTIMPL;

}

uint32_t hpi_comm_read_xmem(uint16_t addr, uint8_t *pData, uint16_t len)
{
	return HPI_ERR_NOTIMPL;

}

typedef struct _sreg_param_t
{
	uint32_t id;
	uint16_t val;

} reg_param_t;

uint32_t hpi_comm_exec_int(uint16_t intnum, uint32_t nargs, ...)
{
	int i;
	uint32_t result;
	va_list args;
	reg_param_t reg_param[MAX_NUM_REGS];

	va_start(args, nargs);

	if (nargs > MAX_NUM_REGS)
	{
		fprintf(stderr, "hpi_comm_exec_int(): Number (%d), of arguments exceeds %d!\n", nargs, MAX_NUM_REGS);
		return HPI_ERR_INVPARAM;
	}

	for (i=0; i < nargs; i++)
	{
		reg_param[i].id = va_arg(args, uint32_t);
		if (reg_param[i].id > MAX_NUM_REGS)
		{
			fprintf(stderr, "hpi_comm_exec_int(): Invalid register R%d!\n", reg_param[i].id);
			return HPI_ERR_INVPARAM;
		}

		reg_param[i].val = (uint16_t)va_arg(args, uint32_t);
	}
	va_end(args);

	hpi_write_reg(COMM_INT_NUM, intnum);

	for (i=0; i < nargs; i++)
	{
		hpi_write_reg(COMM_R0 + 2*reg_param[i].id, reg_param[i].val);
	}

	hpi_mbx_write(COMM_EXEC_INT);

	result = hpi_mbx_read();
	if (IS_ERROR(result))
		return result;

	return (uint32_t)hpi_read_reg(COMM_R0);
}

uint32_t usb_init(void)
{
	int32_t i;

	for (i=0; i < USB_MAX_NUM_PORTS; i++)
	{
		memset(&_g_usb[i], 0, sizeof(usb_t));
	}
	return 0;
}

uint32_t usb_callback_register(uint32_t port, uint32_t type, fp_t pFunc, void *pArg)
{
	uint32_t result;
	if (port >= USB_MAX_NUM_PORTS)
		return -1;

	result = 0;
	switch (type)
	{
		case SUSB_EP0_MSG:
		_g_usb[port].fptr_ept[0] = pFunc;
		_g_usb[port].aptr_ept[0] = pArg;
		break;

		case SUSB_EP1_MSG:
		_g_usb[port].fptr_ept[1] = pFunc;
		_g_usb[port].aptr_ept[1] = pArg;
		break;

		case SUSB_EP2_MSG:
		_g_usb[port].fptr_ept[2] = pFunc;
		_g_usb[port].aptr_ept[2] = pArg;
		break;

		case SUSB_EP3_MSG:
		_g_usb[port].fptr_ept[3] = pFunc;
		_g_usb[port].aptr_ept[3] = pArg;
		break;

		case SUSB_EP4_MSG:
		_g_usb[port].fptr_ept[4] = pFunc;
		_g_usb[port].aptr_ept[4] = pArg;
		break;

		case SUSB_EP5_MSG:
		_g_usb[port].fptr_ept[5] = pFunc;
		_g_usb[port].aptr_ept[5] = pArg;
		break;

		case SUSB_EP6_MSG:
		_g_usb[port].fptr_ept[6] = pFunc;
		_g_usb[port].aptr_ept[6] = pArg;
		break;

		case SUSB_EP7_MSG:
		_g_usb[port].fptr_ept[7] = pFunc;
		_g_usb[port].aptr_ept[7] = pArg;
		break;

		case SUSB_RST_MSG:
		_g_usb[port].fptr_rst = pFunc;
		_g_usb[port].aptr_rst = pArg;
		break;

		case SUSB_SOF_MSG:
		_g_usb[port].fptr_sof = pFunc;
		_g_usb[port].aptr_sof = pArg;
		break;

		case SUSB_CFG_MSG:
		_g_usb[port].fptr_cfg = pFunc;
		_g_usb[port].aptr_cfg = pArg;
		break;

		case SUSB_SUS_MSG:
		_g_usb[port].fptr_sus = pFunc;
		_g_usb[port].aptr_sus = pArg;
		break;

		case SUSB_ID_MSG:
		_g_usb[port].fptr_id = pFunc;
		_g_usb[port].aptr_id = pArg;
		break;

		case SUSB_VBUS_MSG:
		_g_usb[port].fptr_vbus = pFunc;
		_g_usb[port].aptr_vbus = pArg;
		break;

		default:
		result = -1;
		break;
	}

	return result;
}

uint32_t usb_irp_register(usb_irp_t *pIRP, uint32_t port, uint32_t ept, uint32_t size, uint32_t is_out)
{

	if (!pIRP)
		return -1;

	if (port >= USB_MAX_NUM_PORTS)
		return -1;

	if (ept >= USB_MAX_NUM_EPT)
		return -1;

	if (size >= USB_MAX_IMG_SIZE)
		return -1;

	memset(pIRP, 0, sizeof(usb_irp_t));
	pIRP->is_out = is_out;
	pIRP->is_in = !is_out;
	pIRP->tsize = size;
	pIRP->port = port;
	pIRP->ept = ept;
	pIRP->cy_irp_addr = USB_IRP_BASE + port*USB_MAX_NUM_EPT*sizeof(cy_req_t) + ept*sizeof(cy_req_t);
	pIRP->cy_req.next = 0;
	pIRP->cy_req.addr = USB_IMG_BASE + port*USB_MAX_NUM_EPT*USB_MAX_IMG_SIZE + ept*USB_MAX_IMG_SIZE;
	pIRP->cy_req.size = size;
	pIRP->cy_req.callback = 0;
	fifo_alloc(&pIRP->fifo, 8*USB_MAX_IMG_SIZE);

	return 0;
}

uint32_t usb_irp_enqueue(usb_irp_t *pIRP)
{
	uint32_t result;

	if (!pIRP)
	{
//		sputs ("usb_irp_enqueue(): Invalid IRP!\n");
		return -1;
	}

//	printf("cy_addr: %4.4X, Next: %4.4X, Addr: %4.4X, Size: %4.4X, CB: %4.4X\n", pIRP->cy_irp_addr, pIRP->cy_req.next, pIRP->cy_req.addr, pIRP->cy_req.size, pIRP->cy_req.callback);

	if (pIRP->is_out)
	{
		hpi_write_ram(pIRP->cy_irp_addr, (uint16_t*)&pIRP->cy_req, sizeof(cy_req_t));
		if (pIRP->port == 0)
			result = hpi_comm_exec_int(SUSB1_RECEIVE_INT, 2, R1, pIRP->ept, R8, pIRP->cy_irp_addr);

		if (pIRP->port == 1)
			result = hpi_comm_exec_int(SUSB2_RECEIVE_INT, 2, R1, pIRP->ept, R8, pIRP->cy_irp_addr);
	}
	if (pIRP->is_in)
	{
		pIRP->tx_in_progress = 1;
		hpi_write_ram(pIRP->cy_irp_addr, (uint16_t*)&pIRP->cy_req, sizeof(cy_req_t));
		if (pIRP->port == 0)
			result = hpi_comm_exec_int(SUSB1_SEND_INT, 2, R1, pIRP->ept, R8, pIRP->cy_irp_addr);

		if (pIRP->port == 1)
			result = hpi_comm_exec_int(SUSB2_SEND_INT, 2, R1, pIRP->ept, R8, pIRP->cy_irp_addr);
	}

//	if (result)
//		printf ("usb_irp_enqueue(): Result = %8.8X\n", result);

	return result;

}

uint32_t fifo_alloc(fifo_t *pObj, uint32_t size)
{
	pObj->pBuf = (uint8_t*)malloc(size);
	if (!pObj->pBuf)
	{
//		printf ("fifo_alloc(): Error allocating memory!\n");
		return -1;
	}
//	printf ("fifo_alloc(): pBuf at %8.8X\n", (uint32_t)pObj->pBuf);
	pObj->size = size;
	pObj->pPtr_wr = pObj->pBuf;
	pObj->pPtr_rd = pObj->pBuf;

	return 0;
}

uint32_t fifo_free(fifo_t *pObj)
{
	if (pObj->pBuf)
		free(pObj->pBuf);

	return 0;
}

uint32_t fifo_flush(fifo_t *pObj)
{
	pObj->pPtr_wr = pObj->pBuf;
	pObj->pPtr_rd = pObj->pBuf;

	return 0;
}

uint32_t fifo_is_empty(fifo_t *pObj)
{
	return (pObj->pPtr_wr == pObj->pPtr_rd);
}

uint32_t fifo_is_full(fifo_t *pObj)
{
	uint8_t *pNext, *pEnd;

	pEnd = pObj->pBuf + pObj->size;
	pNext = pObj->pPtr_wr + 1;
	if (pNext == pEnd)
		pNext = pObj->pBuf;

	return (pNext == pObj->pPtr_rd);
}

uint32_t fifo_read(fifo_t *pObj, uint8_t *pData, uint32_t size, uint32_t timeout, uint32_t do_block)
{
	uint32_t i;
	uint32_t wait_4ever, timeout_cnt, cnt;
	uint8_t *pEnd;

	pEnd = pObj->pBuf + pObj->size;
	wait_4ever = (timeout == 0);

	cnt = 0;
	while(size)
	{
		timeout_cnt = timeout;

		while(fifo_is_empty(pObj))
		{
			if (!do_block)
				return cnt;

			if (!wait_4ever)
			{
				if (timeout_cnt)
				{
					timeout_cnt--;
					sleep(1);
				}
				else
				{
					return cnt;
				}
			}
		}

		if (pData)
			pData[cnt] = *(pObj->pPtr_rd);

		cnt++;
		pObj->pPtr_rd++;
		if (pObj->pPtr_rd == pEnd)
			pObj->pPtr_rd = pObj->pBuf;

		size--;
	}

	return cnt;
}

uint32_t fifo_write(fifo_t *pObj, uint8_t *pData, uint32_t size, uint32_t timeout, uint32_t do_block)
{
	uint32_t wait_4ever, timeout_cnt, cnt;
	uint8_t *pEnd;

	pEnd = pObj->pBuf + pObj->size;
	wait_4ever = (timeout == 0);

	cnt = 0;
	while(size)
	{
		timeout_cnt = timeout;

		while(fifo_is_full(pObj))
		{
			if (!do_block)
				return cnt;

			if (!wait_4ever)
			{
				if (timeout_cnt)
				{
					timeout_cnt--;
					sleep(1);
				}
				else
				{
					return cnt;
				}
			}
		}

		if (pData)
			*(pObj->pPtr_wr) = pData[cnt];

		cnt++;
		pObj->pPtr_wr++;
		if (pObj->pPtr_wr == pEnd)
			pObj->pPtr_wr = pObj->pBuf;

		size--;
	}

	return cnt;
}

// ---------------------------------------------------------