#include <stdlib.h>
#include "libsys.h"
#include "fifo.h"

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