Modbus_RTU_for_Tiva/gfamodbusrtucom/buffer.c

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

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

void GfaBufCpyUnaligned_uint16_LE(void *pTo, const void *pFrom, size_t nCntWords)
{
	size_t i, j;
	uint8_t *p1 = (uint8_t*)pTo;
	const uint8_t *p2 = (const uint8_t*)pFrom;
	for(i = 0; i < nCntWords; i++)
	{
		p2 += sizeof(uint16_t);
		for(j = 0; j < sizeof(uint16_t); j++)
		{
			*p1++ = *--p2;
		}
		p2 += sizeof(uint16_t);
	}
}

void GfaBufCpyUnaligned_uint16_BE(void *pTo, const void *pFrom, size_t nCntWords)
{
	size_t i, j;
	uint8_t *p1 = (uint8_t*)pTo;
	const uint8_t *p2 = (const uint8_t*)pFrom;
	for(i = 0; i < nCntWords; i++)
	{
		for(j = 0; j < sizeof(uint16_t); j++)
		{
			*p1++ = *p2++;
		}
	}
}

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

void GfaBufCpyUnaligned_uint32_LE(void *pTo, const void *pFrom, size_t nCntDWords)
{
	size_t i, j;
	uint8_t *p1 = (uint8_t*)pTo;
	const uint8_t *p2 = (const uint8_t*)pFrom;
	for(i = 0; i < nCntDWords; i++)
	{
		p2 += sizeof(uint32_t);
		for(j = 0; j < sizeof(uint32_t); j++)
		{
			*p1++ = *--p2;
		}
		p2 += sizeof(uint32_t);
	}
}

void GfaBufCpyUnaligned_uint32_BE(void *pTo, const void *pFrom, size_t nCntDWords)
{
	size_t i, j;
	uint8_t *p1 = (uint8_t*)pTo;
	const uint8_t *p2 = (const uint8_t*)pFrom;
	for(i = 0; i < nCntDWords; i++)
	{
		for(j = 0; j < sizeof(uint32_t); j++)
		{
			*p1++ = *p2++;
		}
	}
}

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

uint16_t GfaBufGetUnaligned_uint16_LE(const void *pData)
{
	uint16_t nRet;
	GfaBufCpyUnaligned_uint16_LE(&nRet, pData, 1);
	return nRet;
}

uint16_t GfaBufGetUnaligned_uint16_BE(const void *pData)
{
	uint16_t nRet;
	GfaBufCpyUnaligned_uint16_BE(&nRet, pData, 1);
	return nRet;
}

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

void GfaBufSetUnaligned_uint16_LE(void *pData, uint16_t nVal)
{
	GfaBufCpyUnaligned_uint16_LE(pData, &nVal, 1);
}

void GfaBufSetUnaligned_uint16_BE(void *pData, uint16_t nVal)
{
	GfaBufCpyUnaligned_uint16_BE(pData, &nVal, 1);
}

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

uint32_t GfaBufGetUnaligned_uint32_LE(const void *pData)
{
	uint32_t nRet;
	GfaBufCpyUnaligned_uint32_LE(&nRet, pData, 1);
	return nRet;
}

uint32_t GfaBufGetUnaligned_uint32_BE(const void *pData)
{
	uint32_t nRet;
	GfaBufCpyUnaligned_uint32_BE(&nRet, pData, 1);
	return nRet;
}

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

void GfaBufSetUnaligned_uint32_LE(void *pData, uint16_t nVal)
{
	GfaBufCpyUnaligned_uint32_LE(pData, &nVal, 1);
}

void GfaBufSetUnaligned_uint32_BE(void *pData, uint16_t nVal)
{
	GfaBufCpyUnaligned_uint32_BE(pData, &nVal, 1);
}

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

float GfaBufGetUnaligned_float(const void *pData)
{
	float fVal;
	uint8_t *p1 = (uint8_t*)pData;
	uint8_t *p2 = (uint8_t*)&fVal;
	memcpy(p2, p1, sizeof(float));
	return fVal;
}

/////////////////////////////////////////////////////////////////////////////
void GfaBufSetUnaligned_float(void *pData, float fVal)
{
	uint8_t *p1 = (uint8_t*)pData;
	uint8_t *p2 = (uint8_t*)&fVal;
	memcpy(p1, p2, sizeof(float));
}

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

double GfaBufGetUnaligned_double(const void *pData)
{
	double fVal;
	uint8_t *p1 = (uint8_t*)pData;
	uint8_t *p2 = (uint8_t*)&fVal;
	memcpy(p2, p1, sizeof(double));
	return fVal;
}

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

void GfaBufSetUnaligned_double(void *pData, double fVal)
{
	uint8_t *p1 = (uint8_t*)pData;
	uint8_t *p2 = (uint8_t*)&fVal;
	memcpy(p1, p2, sizeof(double));
}

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

bool GfaBufVerifyCRC(const void *pData, size_t nCbData, const void *pCRC)
{
	uint16_t nCrc = Crc16(0xFFFF, (const uint8_t*)pData, nCbData);
	const uint8_t *pCrc	= (const uint8_t*)pCRC;
	return ((pCrc[0]	== (uint8_t)(nCrc &  0xff)) &&
			(pCrc[1]	== (uint8_t)(nCrc >> 8)));
}

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

void GfaBufSetCRC(const void *pData, size_t nCbData, void *pCRC)
{
	uint16_t nCrc = Crc16(0xFFFF, (const uint8_t*)pData, nCbData);
	uint8_t *pCrc	= (uint8_t*)pCRC;
	pCrc[0]			= (uint8_t)(nCrc &  0xff);
	pCrc[1]			= (uint8_t)(nCrc >> 8);
}