Modbus_RTU_for_Tiva/gfamodbusrtucom/fifo.c

#include <string.h>
#include <gfambrtucom.h>
#include "driverlib/sw_crc.h"

/////////////////////////////////////////////////////////////////////////////

#define FIFO_COUNT								2
#define FIFO_BUFFER_SIZE						256
#define FIFO_BUFFER_MASK						(FIFO_BUFFER_SIZE - 1)

/////////////////////////////////////////////////////////////////////////////

typedef struct _FIFOHDR
{
	volatile size_t nReadPtr;
	volatile size_t nWritePtr;
	volatile size_t nCbData;
	size_t nCbSize;
	size_t nBufferMask;
	volatile uint32_t nFlags;
	GFA_FIFO_BACKEND backend;
}FIFOHDR, *LPFIFOHDR;
typedef const FIFOHDR *LPCFIFOHDR;

/////////////////////////////////////////////////////////////////////////////

typedef struct _FIFO
{
	FIFOHDR hdr;
	uint8_t fifo[FIFO_BUFFER_SIZE];
}FIFO, *LPFIFO;
typedef const FIFO *LPCFIFO;

/////////////////////////////////////////////////////////////////////////////

static FIFO g_fifo[FIFO_COUNT];

/////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////

static int _IsPowerOf2(size_t x)
{
	return x && !(x & (x - 1));
}

/////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////

HFIFO GfaMbFifoCreate(int nFifoIndex, LPCGFA_FIFO_BACKEND pBackend)
{
	LPFIFO p;
	if((nFifoIndex <  0) || (nFifoIndex >= FIFO_COUNT))
		return NULL;
	if(!_IsPowerOf2(FIFO_BUFFER_SIZE))
		return NULL;
	p = &g_fifo[nFifoIndex];
	memset(p, 0, sizeof(FIFO));
	p->hdr.nCbSize		= FIFO_BUFFER_SIZE;
	p->hdr.nBufferMask	= FIFO_BUFFER_MASK;

	memcpy(&p->hdr.backend, pBackend, sizeof(GFA_FIFO_BACKEND));
	return (HFIFO)p;
}

/////////////////////////////////////////////////////////////////////////////

void GfaMbFifoRelease(HFIFO hFifo)
{
}

/////////////////////////////////////////////////////////////////////////////

void GfaMbFifoReset(HFIFO hFifo, bool bLock)
{
	bool bPrevLocked;
	register LPFIFO pf = (LPFIFO)hFifo;
	if(bLock && pf->hdr.backend.pfnLockBackend)
		bPrevLocked = (*pf->hdr.backend.pfnLockBackend)(false);
	pf->hdr.nReadPtr	= 0;
	pf->hdr.nWritePtr	= 0;
	pf->hdr.nCbData		= 0;
	if(bLock && pf->hdr.backend.pfnLockBackend)
		(*pf->hdr.backend.pfnLockBackend)(bPrevLocked);
}

/////////////////////////////////////////////////////////////////////////////

size_t GfaMbFifoRead(HFIFO hFifo, void *pData, size_t nCbData, bool bLock)
{
	size_t nToRead;
	uint8_t *pByte = (uint8_t*)pData;
	register LPFIFO pf = (LPFIFO)hFifo;
	if(bLock && pf->hdr.backend.pfnLockBackend)
		(*pf->hdr.backend.pfnLockBackend)(false);

	for(nToRead = nCbData; nCbData; --nCbData)
	{
		if((pf->hdr.nWritePtr == pf->hdr.nReadPtr) && !pf->hdr.nCbData)
			break;
		*pByte++ = pf->fifo[pf->hdr.nReadPtr++];
		pf->hdr.nReadPtr &= pf->hdr.nBufferMask;
		pf->hdr.nCbData--;
	}

	if(bLock && pf->hdr.backend.pfnLockBackend)
		(*pf->hdr.backend.pfnLockBackend)(true);

	return nToRead - nCbData;
}

/////////////////////////////////////////////////////////////////////////////

size_t GfaMbFifoWrite(HFIFO hFifo, const void *pData, size_t nCbData, bool bLock)
{
	size_t nToWrite;
	const uint8_t *pByte = (const uint8_t*)pData;
	register LPFIFO pf = (LPFIFO)hFifo;
	if(bLock && pf->hdr.backend.pfnLockBackend)
		(*pf->hdr.backend.pfnLockBackend)(false);

	for(nToWrite = nCbData; nCbData; --nCbData)
	{
		if((pf->hdr.nCbData == pf->hdr.nCbSize))
			break;
		pf->fifo[pf->hdr.nWritePtr++] = *pByte++;
		pf->hdr.nWritePtr &= pf->hdr.nBufferMask;
		pf->hdr.nCbData++;
	}

	if(bLock && pf->hdr.backend.pfnLockBackend)
		(*pf->hdr.backend.pfnLockBackend)(true);

	return nToWrite - nCbData;
}

/////////////////////////////////////////////////////////////////////////////

bool GfaMbFifoPop(HFIFO hFifo, uint8_t *b, bool bLock)
{
	bool bRet;
	register LPFIFO pf = (LPFIFO)hFifo;
	if(bLock && pf->hdr.backend.pfnLockBackend)
	{
		(*pf->hdr.backend.pfnLockBackend)(false);

		if((bRet = !!pf->hdr.nCbData))
		{
			*b = pf->fifo[pf->hdr.nReadPtr++];
			pf->hdr.nReadPtr &= pf->hdr.nBufferMask;
			pf->hdr.nCbData--;
		}

		(*pf->hdr.backend.pfnLockBackend)(true);
	}
	else
	{
		if((bRet = !!pf->hdr.nCbData))
		{
			*b = pf->fifo[pf->hdr.nReadPtr++];
			pf->hdr.nReadPtr &= pf->hdr.nBufferMask;
			pf->hdr.nCbData--;
		}
	}

	return bRet;
}

/////////////////////////////////////////////////////////////////////////////

bool GfaMbFifoPush(HFIFO hFifo, uint8_t b, bool bLock)
{
	bool bRet;
	register LPFIFO pf = (LPFIFO)hFifo;
	if(bLock && pf->hdr.backend.pfnLockBackend)
	{
		(*pf->hdr.backend.pfnLockBackend)(false);

		if((bRet = (pf->hdr.nCbData < pf->hdr.nCbSize)))
		{
			pf->fifo[pf->hdr.nWritePtr++] = b;
			pf->hdr.nWritePtr &= pf->hdr.nBufferMask;
			pf->hdr.nCbData++;
		}

		(*pf->hdr.backend.pfnLockBackend)(true);
	}
	else
	{
		if((bRet = (pf->hdr.nCbData < pf->hdr.nCbSize)))
		{
			pf->fifo[pf->hdr.nWritePtr++] = b;
			pf->hdr.nWritePtr &= pf->hdr.nBufferMask;
			pf->hdr.nCbData++;
		}
	}

	return bRet;
}

/////////////////////////////////////////////////////////////////////////////

size_t GfaMbFifoPeek(HFIFO hFifo, bool bLock)
{
	bool bPrevLocked;
	size_t nAvail;
	register LPFIFO pf = (LPFIFO)hFifo;
	if(bLock && pf->hdr.backend.pfnLockBackend)
	{
		bPrevLocked = (*pf->hdr.backend.pfnLockBackend)(false);
		nAvail = pf->hdr.nCbData;
		if(bPrevLocked)
			(*pf->hdr.backend.pfnLockBackend)(true);
		return nAvail;
	}
	else
		return pf->hdr.nCbData;
}

/////////////////////////////////////////////////////////////////////////////

bool GfaMbFifoCanWrite(HFIFO hFifo, bool bLock)
{
	bool bPrevLocked, bCanWrite;
	register LPFIFO pf = (LPFIFO)hFifo;
	if(bLock && pf->hdr.backend.pfnLockBackend)
	{
		bPrevLocked = (*pf->hdr.backend.pfnLockBackend)(false);
		bCanWrite = (pf->hdr.nWritePtr != pf->hdr.nReadPtr) || !pf->hdr.nCbData;
		if(bPrevLocked)
			(*pf->hdr.backend.pfnLockBackend)(true);
		return bCanWrite;
	}
	else
		return (pf->hdr.nWritePtr != pf->hdr.nReadPtr) || !pf->hdr.nCbData;
}

/////////////////////////////////////////////////////////////////////////////

bool GfaMbFifoEmpty(HFIFO hFifo, bool bLock)
{
	bool bPrevLocked, bEmpty;
	register LPFIFO pf = (LPFIFO)hFifo;
	if(bLock && pf->hdr.backend.pfnLockBackend)
	{
		bPrevLocked = (*pf->hdr.backend.pfnLockBackend)(false);
		bEmpty = !pf->hdr.nCbData;
		if(bPrevLocked)
			(*pf->hdr.backend.pfnLockBackend)(true);
		return bEmpty;
	}
	else
		return !pf->hdr.nCbData;
}

/////////////////////////////////////////////////////////////////////////////

bool GfaMbFifoRxPrepare(HFIFO hFifo, bool bLock)
{
	bool bPrevLocked, bRet = false;
	register LPFIFO pf = (LPFIFO)hFifo;
	if(pf->hdr.backend.pfnRxPrepare)
	{
		if(bLock && pf->hdr.backend.pfnLockBackend)
		{
			bPrevLocked = (*pf->hdr.backend.pfnLockBackend)(false);
			bRet = (*pf->hdr.backend.pfnRxPrepare)();
			if(bPrevLocked)
				(*pf->hdr.backend.pfnLockBackend)(true);
		}
		else
			bRet = (*pf->hdr.backend.pfnRxPrepare)();
	}

	return bRet;
}

/////////////////////////////////////////////////////////////////////////////

void GfaMbFifoRxStart(HFIFO hFifo, bool bLock)
{
	bool bPrevLocked;
	register LPFIFO pf = (LPFIFO)hFifo;
	if(pf->hdr.backend.pfnRxStart)
	{
		if(bLock && pf->hdr.backend.pfnLockBackend)
		{
			bPrevLocked = (*pf->hdr.backend.pfnLockBackend)(false);
			(*pf->hdr.backend.pfnRxStart)();
			if(bPrevLocked)
				(*pf->hdr.backend.pfnLockBackend)(true);
		}
		else
			(*pf->hdr.backend.pfnRxStart)();
	}
}

/////////////////////////////////////////////////////////////////////////////

bool GfaMbFifoRxFinalize(HFIFO hFifo, bool bLock)
{
	bool bPrevLocked, bRet = false;
	register LPFIFO pf = (LPFIFO)hFifo;
	if(pf->hdr.backend.pfnRxFinalize)
	{
		if(bLock && pf->hdr.backend.pfnLockBackend)
		{
			bPrevLocked = (*pf->hdr.backend.pfnLockBackend)(false);
			bRet = (*pf->hdr.backend.pfnRxFinalize)();
			if(bPrevLocked)
				(*pf->hdr.backend.pfnLockBackend)(true);
		}
		else
			bRet = (*pf->hdr.backend.pfnRxFinalize)();
	}

	return bRet;
}

/////////////////////////////////////////////////////////////////////////////

bool GfaMbFifoTxPrepare(HFIFO hFifo, bool bLock)
{
	bool bPrevLocked, bRet = false;
	register LPFIFO pf = (LPFIFO)hFifo;
	if(pf->hdr.backend.pfnTxPrepare)
	{
		if(bLock && pf->hdr.backend.pfnLockBackend)
		{
			bPrevLocked = (*pf->hdr.backend.pfnLockBackend)(false);
			bRet = (*pf->hdr.backend.pfnTxPrepare)();
			if(bPrevLocked)
				(*pf->hdr.backend.pfnLockBackend)(true);
		}
		else
			bRet = (*pf->hdr.backend.pfnTxPrepare)();
	}

	return bRet;
}

/////////////////////////////////////////////////////////////////////////////

void GfaMbFifoTxStart(HFIFO hFifo, bool bLock)
{
	bool bPrevLocked;
	register LPFIFO pf = (LPFIFO)hFifo;
	if(pf->hdr.backend.pfnTxStart)
	{
		if(bLock && pf->hdr.backend.pfnLockBackend)
		{
			bPrevLocked = (*pf->hdr.backend.pfnLockBackend)(false);
			(*pf->hdr.backend.pfnTxStart)();
			if(bPrevLocked)
				(*pf->hdr.backend.pfnLockBackend)(true);
		}
		else
			(*pf->hdr.backend.pfnTxStart)();
	}
}

/////////////////////////////////////////////////////////////////////////////

bool GfaMbFifoTxFinalize(HFIFO hFifo, bool bLock)
{
	bool bPrevLocked, bRet = false;
	register LPFIFO pf = (LPFIFO)hFifo;
	if(pf->hdr.backend.pfnTxFinalize)
	{
		if(bLock && pf->hdr.backend.pfnLockBackend)
		{
			bPrevLocked = (*pf->hdr.backend.pfnLockBackend)(false);
			bRet = (*pf->hdr.backend.pfnTxFinalize)();
			if(bPrevLocked)
				(*pf->hdr.backend.pfnLockBackend)(true);
		}
		else
			bRet = (*pf->hdr.backend.pfnTxFinalize)();
	}

	return bRet;
}

/////////////////////////////////////////////////////////////////////////////

void GfaMbFifoSetFlags(HFIFO hFifo, uint32_t nFlags, bool bLock)
{
	bool bPrevLocked;
	register LPFIFO pf = (LPFIFO)hFifo;
	if(bLock && pf->hdr.backend.pfnLockBackend)
	{
		bPrevLocked = (*pf->hdr.backend.pfnLockBackend)(false);
		pf->hdr.nFlags |= nFlags;
		if(bPrevLocked)
			(*pf->hdr.backend.pfnLockBackend)(true);
	}
	else
		pf->hdr.nFlags |= nFlags;
}

/////////////////////////////////////////////////////////////////////////////

void GfaMbFifoClearFlags(HFIFO hFifo, uint32_t nFlags, bool bLock)
{
	bool bPrevLocked;
	register LPFIFO pf = (LPFIFO)hFifo;
	if(bLock && pf->hdr.backend.pfnLockBackend)
	{
		bPrevLocked = (*pf->hdr.backend.pfnLockBackend)(false);
		pf->hdr.nFlags &= ~nFlags;
		if(bPrevLocked)
			(*pf->hdr.backend.pfnLockBackend)(true);
	}
	else
		pf->hdr.nFlags &= ~nFlags;
}

/////////////////////////////////////////////////////////////////////////////

uint32_t GfaMbFifoGetFlags(HFIFO hFifo, bool bLock)
{
	uint32_t nFlags;
	bool bPrevLocked;
	register LPFIFO pf = (LPFIFO)hFifo;
	if(bLock && pf->hdr.backend.pfnLockBackend)
	{
		bPrevLocked = (*pf->hdr.backend.pfnLockBackend)(false);
		nFlags = pf->hdr.nFlags;
		if(bPrevLocked)
			(*pf->hdr.backend.pfnLockBackend)(true);
		return nFlags;
	}
	else
		return pf->hdr.nFlags;
}

bool GfaMbFifoMatchFlags(HFIFO hFifo, uint32_t nFlags, bool bLock)
{
	bool bMatch;
	bool bPrevLocked;
	register LPFIFO pf = (LPFIFO)hFifo;
	if(bLock && pf->hdr.backend.pfnLockBackend)
	{
		bPrevLocked = (*pf->hdr.backend.pfnLockBackend)(false);
		bMatch = (pf->hdr.nFlags & nFlags) == nFlags;
		if(bPrevLocked)
			(*pf->hdr.backend.pfnLockBackend)(true);
		return bMatch;
	}
	else
		return (pf->hdr.nFlags & nFlags) == nFlags;
}

uint16_t GfaMbFifoCalcCRC(HFIFO hFifo, bool bLock)
{
	bool bPrevLocked = false;
	register LPFIFO pf = (LPFIFO)hFifo;
	if(bLock && pf->hdr.backend.pfnLockBackend)
		bPrevLocked = (*pf->hdr.backend.pfnLockBackend)(false);

	if(!pf->hdr.nCbData)
	{
		if(bLock && pf->hdr.backend.pfnLockBackend)
			(*pf->hdr.backend.pfnLockBackend)(bPrevLocked);
		return 0;
	}

	size_t nCbData	= pf->hdr.nCbData;
	size_t nReadPtr	= pf->hdr.nReadPtr;
	uint8_t b;
	uint16_t nCrcSw = 0xFFFF;

	while(nCbData--)			// calculate the CRC
	{
		b = pf->fifo[nReadPtr++];
		nReadPtr &= pf->hdr.nBufferMask;
		nCrcSw = Crc16(nCrcSw, &b, 1);
	}

	if(bLock && pf->hdr.backend.pfnLockBackend)
		(*pf->hdr.backend.pfnLockBackend)(bPrevLocked);

	return nCrcSw;
}