/*<html><pre>*/
#include <../source/TAL/objbase.h>

/*
 *	TODO: fifo->size is power of 2; convert to use different warp scheme:
 *	from:
 *		if(++fifo->head == fifo->size) fifo->head=0;
 *	to:
 *		fifo->head = (fifo->head+1) & (fifo->size - 1);
 */
 
/*
 *	Implementation of u8_fifo_t
 */
void	V_GLOBAL
u8fInit(u8_fifo_t *fifo)
{
#ifdef	CHECKED_BUILD
	if(!fifo)
	{
		conlPuts(LOG_ERROR,"u8fInit(NULL): incorrect parameter.\n");
		return;
	}
#endif	/* CHECKED_BUILD */
	memZero(fifo,sizeof(u8_fifo_t));
}

void	V_GLOBAL
u8fClear(u8_fifo_t *fifo)
{
#ifdef	CHECKED_BUILD
	if(!fifo)
	{
		conlPuts(LOG_ERROR,"u8fClear(NULL): incorrect parameter.\n");
		return;
	}
#endif	/* CHECKED_BUILD */
	if(fifo->buf)
		memFree(fifo->buf);
	memZero(fifo,sizeof(u8_fifo_t));
}

/*
 *	not_todo: don't convert to use memRealloc().
 */
void	V_LOCAL
u8fGrowBuffer(u8_fifo_t *fifo,u32_t next_minsize)
{
	u8_t *nextbuf;
	u32_t nextlen,len,headlen,taillen;
	/* How many elements in fifo */
	len = fifo->size-fifo->avail;
	/* How many elements between tail end end of buffer */
	headlen = fifo->size-fifo->tail;
	/* How many elements must be copied from tail (first block) */
	if(headlen>len)
		headlen=len;
	/* How many elements must be copied from start (second block) */
	taillen=len-headlen;
	NOT_LESS_POWER_TWO(nextlen,next_minsize,u32_t);
	nextbuf=(u8_t*)memAlloc(nextlen);
	/* Copy head bytes */
	if(headlen)
		memCopy(nextbuf,fifo->buf+fifo->tail,headlen);
	/* Copy tail bytes */
	if(taillen)
		memCopy(nextbuf+headlen,fifo->buf,taillen);
	fifo->head=headlen+taillen;
	fifo->tail=0;
	if(fifo->buf)
		memFree(fifo->buf);
	fifo->buf=nextbuf;
	fifo->size=nextlen;
	fifo->avail=fifo->size-fifo->head;
}

void	V_GLOBAL
u8fPurge(u8_fifo_t *fifo)
{
#ifdef	CHECKED_BUILD
	if(!fifo)
	{
		conlPuts(LOG_ERROR,"u8fPurge(NULL): incorrect parameter.\n");
		return;
	}
#endif	/* CHECKED_BUILD */
	fifo->head=fifo->tail=0;
	fifo->avail=fifo->size;
}

void	V_GLOBAL
u8fPush(u8_fifo_t *fifo,u8_t element)
{
#ifdef	CHECKED_BUILD
	if(!fifo)
	{
		conlPuts(LOG_ERROR,"u8fPush(NULL): incorrect parameter.\n");
		return;
	}
#endif	/* CHECKED_BUILD */
	if(fifo->avail == 0)
		/* Allocate new storage */
		u8fGrowBuffer(fifo,fifo->size+1);
	*(fifo->buf + fifo->head) = element;
	fifo->head++;
	if(fifo->head == fifo->size)
		fifo->head = 0;
	fifo->avail--;
}

void	V_GLOBAL
u8fPushBuffer(u8_fifo_t *fifo,u8_t *src,u32_t num_bytes)
{
#ifdef	CHECKED_BUILD
	if(!fifo || !src || !num_bytes)
	{
		conlPrintf(LOG_ERROR,
			"u8fPushBuffer(%X,%X,%u): incorrect parameter(s).\n",
			fifo,src,num_bytes);
		return;
	}
#endif	/* CHECKED_BUILD */
	if(fifo->avail<num_bytes)
	{
		/* Allocate new storage */
		u8fGrowBuffer(fifo,fifo->size+num_bytes);
		/* fifo->tail granted to be 0, so we can issue only one copy */
		memCopy(fifo->buf+fifo->head,src,num_bytes);
		fifo->head+=num_bytes;
	}
	else
	{
		/* In common case we must perform two copy operations */
		u32_t headlen, taillen;
		/* How many elements feet between head and end of buffer */
		headlen = fifo->size-fifo->head;
		/* How many elements must be copied at head (first block) */
		if(headlen>num_bytes)
			headlen=num_bytes;
		/* How many elements must be copied at start (second block) */
		taillen=num_bytes-headlen;
		if(headlen)
			memCopy(fifo->buf+fifo->head,src,headlen);
		if(taillen)
		{
			memCopy(fifo->buf,src+headlen,taillen);
			fifo->head=taillen;
		}
		else
		{
			fifo->head+=num_bytes;
			if(fifo->head==fifo->size)
				fifo->head=0;
		}
	}
	fifo->avail-=num_bytes;
}

vboolean	V_GLOBAL
u8fPop(u8_fifo_t *fifo,u8_t *element)
{
#ifdef	CHECKED_BUILD
	if(!fifo || !element)
	{
		conlPrintf(LOG_ERROR,
			"u8fPop(%X,%X): incorrect parameter(s).\n",
			fifo,element);
		return false;
	}
#endif	/* CHECKED_BUILD */
	if(fifo->avail == fifo->size)
		return 0;
	*element = *(fifo->buf + fifo->tail);
	fifo->tail++;
	if(fifo->tail == fifo->size)
		fifo->tail = 0;
	fifo->avail++;
	return 1;
}

/*	Returns number of copied bytes */
u32_t	V_GLOBAL
u8fPopBuffer(u8_fifo_t *fifo,u8_t *dst,u32_t num_bytes)
{
	u32_t len,headlen,taillen;
#ifdef	CHECKED_BUILD
	if(!fifo || !dst || !num_bytes)
	{
		conlPrintf(LOG_ERROR,
			"u8fPopBuffer(%X,%X,%u): incorrect parameter(s).\n",
			fifo,dst,num_bytes);
		return 0;
	}
#endif	/* CHECKED_BUILD */
	len=fifo->size-fifo->avail;
	if(num_bytes>len)
		num_bytes=len;
	/* How many elements between tail end end of buffer */
	headlen = fifo->size-fifo->tail;
	/* How many elements must be copied from tail (first block) */
	if(headlen>=num_bytes)
	{
		headlen=num_bytes;	/* taillen = 0 */
		memCopy(dst,fifo->buf+fifo->tail,headlen);
		fifo->tail+=num_bytes;
		if(fifo->tail==fifo->size)
			fifo->tail=0;
	}
	else
	{
		taillen=num_bytes-headlen;
		memCopy(dst,fifo->buf+fifo->tail,headlen);
		memCopy(dst+headlen,fifo->buf,taillen);
		fifo->tail=taillen;
	}
	fifo->avail+=num_bytes;
	return num_bytes;
}

#ifdef	FIFO_NO_MACRO

u32_t V_GLOBAL
u8fCount(u8_fifo_t *fifo)
{
#ifdef	CHECKED_BUILD
	if(!fifo)
	{
		conlPuts(LOG_ERROR,"u8fCount(NULL): incorrect parameter.\n");
		return 0;
	}
#endif	/* CHECKED_BUILD */
	return fifo->size-fifo->avail;
}

#endif	/* FIFO_NO_MACRO */

/*	Implementation of fifo_t */

void	V_GLOBAL
fifoInit(fifo_t *fifo,u32_t element_size)
{
#ifdef	CHECKED_BUILD
	if(!fifo || !element_size)
	{
		conlPrintf(LOG_ERROR,
			"fifoInit(%X,%u): incorrect parameter(s).\n",
			fifo,element_size);
		return;
	}
#endif	/* CHECKED_BUILD */
	memZero(fifo,sizeof(fifo_t));
	fifo->element_size=element_size;
}

void	V_GLOBAL
fifoClear(fifo_t *fifo)
{
#ifdef	CHECKED_BUILD
	if(!fifo)
	{
		conlPuts(LOG_ERROR,"fifoClear(NULL): incorrect parameter.\n");
		return;
	}
#endif	/* CHECKED_BUILD */
	if(fifo->buf)
		memFree(fifo->buf);
	fifoInit(fifo,fifo->element_size);
}

void	V_GLOBAL
fifoPush(fifo_t *fifo,void *element)
{
#ifdef	CHECKED_BUILD
	if(!fifo || !element)
	{
		conlPrintf(LOG_ERROR,
			"fifoPush(%X,%X): incorrect parameter(s).\n",
			fifo,element);
		return;
	}
#endif	/* CHECKED_BUILD */
	if(fifo->avail == 0)
	{
		/* Allocate new storage */
		u8_t *nextbuf;
		u32_t nextlen,len,headlen,taillen;
		len = fifo->size-fifo->avail;
		headlen = fifo->size - (fifo->tail-fifo->buf) / fifo->element_size;
		if(headlen>len)
			headlen=len;
		taillen=len-headlen;
		GREATER_POWER_TWO(nextlen,fifo->size,u32_t);
		nextbuf=(u8_t*)memAlloc(nextlen*fifo->element_size);
		/* Copy head bytes */
		if(headlen)
			memCopy(nextbuf,fifo->tail,headlen*fifo->element_size);
		/* Copy tail bytes */
		if(taillen)
			memCopy(nextbuf+headlen*fifo->element_size,fifo->buf,
				taillen*fifo->element_size);
		if(fifo->buf)
			memFree(fifo->buf);
		fifo->buf=nextbuf;
		fifo->limit=fifo->buf + nextlen * fifo->element_size;
		fifo->head=fifo->buf + len * fifo->element_size;
		fifo->tail=fifo->buf;
		fifo->size=nextlen;
		fifo->avail=fifo->size-len;
	}
	memCopy(fifo->head,element,fifo->element_size);
	fifo->head+=fifo->element_size;
	if(fifo->head == fifo->limit)
		fifo->head = fifo->buf;
	fifo->avail--;
}

void	V_GLOBAL
fifoPurge(fifo_t *fifo)
{
#ifdef	CHECKED_BUILD
	if(!fifo)
	{
		conlPuts(LOG_ERROR,"fifoPurge(NULL): incorrect parameter.\n");
		return;
	}
#endif	/* CHECKED_BUILD */
	fifo->head=fifo->tail=fifo->buf;
	fifo->avail=fifo->size;
}

vboolean	V_GLOBAL
fifoPop(fifo_t *fifo,void *element)
{
#ifdef	CHECKED_BUILD
	if(!fifo || !element)
	{
		conlPrintf(LOG_ERROR,
			"fifoPop(%X,%X): incorrect parameter(s).\n",
			fifo,element);
		return 0;
	}
#endif	/* CHECKED_BUILD */
	if(fifo->avail == fifo->size)
		return false;
	memCopy(element,fifo->tail,fifo->element_size);
	fifo->tail+=fifo->element_size;
	if(fifo->tail == fifo->limit)
		fifo->tail = fifo->buf;
	fifo->avail++;
	return true;
}

#ifdef	FIFO_NO_MACRO

u32_t	V_GLOBAL
fifoCount(fifo_t *fifo)
{
#ifdef	CHECKED_BUILD
	if(!fifo)
	{
		conlPuts(LOG_ERROR,"fifoCount(NULL): incorrect parameter.\n");
		return 0;
	}
#endif	/* CHECKED_BUILD */
	return fifo->size-fifo->avail;
}

#endif /* FIFO_NO_MACRO */

/*</pre></html>*/
/*EOF*/
