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

static volatile UINT32 _g_int_active;
static volatile UINT32 _g_mbx_return;
static volatile UINT32 _g_mbx_in_flag;

// ---------------------------------------------------------
// 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(void)
{
	INT32 i;
	UINT16 sie_msg, hpi_status;
	UINT32 port, siemsg_handled, int_handled;
	_g_int_active = 1;
//	sputs("--------------------------------------------------------\n");
//	sputs("USB-Interrupt(4)\n");
//	sputs("HPI Status   : ");
	hpi_status = cy67k3_read_HPI_STATUS();
//	print_word(hpi_status);
//	sputs("\n");

//	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;
//		sputs("MAILBOX (HPI): ");
		_g_mbx_return = cy67k3_read_HPI_MAILBOX();
		_g_mbx_in_flag = 1;
//		print_word(_g_mbx_return);
//		sputs("\n");
	}

	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);
//		sputs("RESET 1\n");
	}

	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);
//		sputs("RESET 2\n");
	}

	if (hpi_status & HPI_STATUS_DONE1)
	{
		int_handled = 1;
//		sputs("DONE 1\n");
	}

	if (hpi_status & HPI_STATUS_DONE2)
	{
		int_handled = 1;
//		sputs("DONE 2\n");
	}

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

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

	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)
		{
			sputs("Unhandled SIE1 message ");
			print_word(sie_msg);
			sputs("!\n");
		}

	}
	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)
		{
			sputs("Unhandled SIE2 message ");
			print_word(sie_msg);
			sputs("!\n");
		}

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

//	sputs("--------------------------------------------------------\n");
	_g_int_active = 0;
}

// ---------------------------------------------------------
// CY7C67300 low-level routines
// ---------------------------------------------------------
void cy67k3_reset(void)
{
	int i;
	volatile UINT32 *pHpi = (UINT32*)sys_usb_ctrl;

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

	*pHpi = 0;

}

UINT16 cy67k3_read_HPI_DATA(void)
{
	volatile UINT32 *pHpi = (UINT32*)sys_usb_data;

	return (UINT16)*pHpi;
}

UINT16 cy67k3_read_HPI_MAILBOX(void)
{
	volatile UINT32 *pHpi = (UINT32*)sys_usb_mbx;

	return (UINT16)*pHpi;
}

UINT16 cy67k3_read_HPI_ADDRESS(void)
{
	volatile UINT32 *pHpi = (UINT32*)sys_usb_addr;

	return (UINT16)*pHpi;
}

UINT16 cy67k3_read_HPI_STATUS(void)
{
	volatile UINT32 *pHpi = (UINT32*)sys_usb_status;

	return (UINT16)*pHpi;
}

void cy67k3_write_HPI_DATA(UINT16 data)
{
	volatile UINT32 *pHpi = (UINT32*)sys_usb_data;

	*pHpi = (UINT32)data;
}

void cy67k3_write_HPI_MAILBOX(UINT16 data)
{
	volatile UINT32 *pHpi = (UINT32*)sys_usb_mbx;

	*pHpi = (UINT32)data;
}

void cy67k3_write_HPI_ADDRESS(UINT16 data)
{
	volatile UINT32 *pHpi = (UINT32*)sys_usb_addr;

	*pHpi = (UINT32)data;
}

void cy67k3_write_HPI_STATUS(UINT16 data)
{
	volatile UINT32 *pHpi = (UINT32*)sys_usb_status;

	*pHpi = (UINT32)data;
}

// ---------------------------------------------------------
// HPI API
// ---------------------------------------------------------
UINT32 hpi_init(void)
{
	int i;
	UINT16 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);

	return HPI_NOERROR;
}

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

UINT32 hpi_mbx_read(void)
{
	UINT32 err;
	UINT16 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 hpi_check_addr(UINT16 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 hpi_read_reg(UINT16 addr)
{
	UINT32 result;

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

	cy67k3_write_HPI_ADDRESS(addr);
	return cy67k3_read_HPI_DATA();
}

UINT32 hpi_write_reg(UINT16 addr, UINT16 data)
{
	UINT32 result;

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

	cy67k3_write_HPI_ADDRESS(addr);
	cy67k3_write_HPI_DATA(data);
}

UINT32 hpi_write_ram(UINT16 addr, UINT16 *pData, UINT32 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 hpi_read_ram(UINT16 addr, UINT16 *pData, UINT32 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 hpi_comm_reset(void)
{
	hpi_mbx_write(COMM_RESET);

	return hpi_mbx_read();
}

UINT32 hpi_comm_jump2code(UINT16 addr)
{
	cy67k3_write_HPI_ADDRESS(addr);
	hpi_mbx_write(COMM_JUMP2CODE);

	return hpi_mbx_read();
}

UINT32 hpi_comm_callcode(UINT16 addr)
{
	hpi_write_reg(COMM_CODE_ADDR, addr);

	hpi_mbx_write(COMM_CALL_CODE);

	return hpi_mbx_read();
}

UINT32 hpi_comm_write_ctrl_reg(UINT16 addr, UINT16 data, UINT16 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 hpi_comm_read_ctrl_reg(UINT16 addr)
{
	UINT32 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)cy67k3_read_HPI_DATA();

}

UINT32 hpi_comm_write_xmem(UINT16 addr, UINT8 *pData, UINT16 len)
{
	return HPI_ERR_NOTIMPL;

}

UINT32 hpi_comm_read_xmem(UINT16 addr, UINT8 *pData, UINT16 len)
{
	return HPI_ERR_NOTIMPL;

}

typedef struct _sreg_param_t
{
	UINT32 id;
	UINT16 val;

} reg_param_t;

UINT32 hpi_comm_exec_int(UINT16 intnum, UINT32 nargs, ...)
{
	int i;
	UINT32 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);
		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)va_arg(args, UINT32);
	}
	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)hpi_read_reg(COMM_R0);
}

UINT32 usb_init(void)
{
	INT32 i;

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

UINT32 usb_callback_register(UINT32 port, UINT32 type, fp_t pFunc, void *pArg)
{
	UINT32 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 usb_irp_register(usb_irp_t *pIRP, UINT32 port, UINT32 ept, UINT32 size, UINT32 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 usb_irp_enqueue(usb_irp_t *pIRP)
{
	UINT32 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*)&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*)&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 fifo_alloc(fifo_t *pObj, UINT32 size)
{
	pObj->pBuf = (UINT8*)malloc(size);
	if (!pObj->pBuf)
	{
//		printf ("fifo_alloc(): Error allocating memory!\n");
		return -1;
	}
//	printf ("fifo_alloc(): pBuf at %8.8X\n", (UINT32)pObj->pBuf);
	pObj->size = size;
	pObj->pPtr_wr = pObj->pBuf;
	pObj->pPtr_rd = pObj->pBuf;

	return 0;
}

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

	return 0;
}

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

	return 0;
}

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

UINT32 fifo_is_full(fifo_t *pObj)
{
	UINT8 *pNext, *pEnd;

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

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

UINT32 fifo_read(fifo_t *pObj, UINT8 *pData, UINT32 size, UINT32 timeout, UINT32 do_block)
{
	UINT32 i;
	UINT32 wait_4ever, timeout_cnt, cnt;
	UINT8 *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 fifo_write(fifo_t *pObj, UINT8 *pData, UINT32 size, UINT32 timeout, UINT32 do_block)
{
	UINT32 wait_4ever, timeout_cnt, cnt;
	UINT8 *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;
}

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