146 lines
2.3 KiB
C
146 lines
2.3 KiB
C
#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;
|
|
}
|