#include <string.h>
#include "gfambrtuslv.h"
#include "gfambrtuslv_priv.h"
#include "driverlib/sw_crc.h"

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

size_t GfaModbusRequestGetDataInfoLength(LPCGFA_MODBUS_RTU_SLAVE pSlv)
{
	size_t nRet;

	switch(pSlv->adu.pdu.func)
	{
	case MB_FUNC_READ_HOLDING_REGISTERS:
	case MB_FUNC_READ_INPUT_REGISTERS:
		nRet = 4;
		break;

	case MB_FUNC_PRESET_SINGLE_REGISTER:
		nRet = 2;
		break;

	case MB_FUNC_WRITE_MULTIPLE_REGISTERS:
		nRet = 5;
		break;

	case MB_FUNC_DIAGNOSTIC:
		nRet = 2;
		break;

	default:
		nRet = 0;
		break;
	}

	return nRet;
}

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

size_t GfaModbusRequestGetDataPayloadLength(LPCGFA_MODBUS_RTU_SLAVE pSlv)
{
	size_t nRet;

	switch(pSlv->adu.pdu.func)
	{
	case MB_FUNC_READ_HOLDING_REGISTERS:
	case MB_FUNC_READ_INPUT_REGISTERS:
		nRet = 0;
		break;

	case MB_FUNC_PRESET_SINGLE_REGISTER:
		nRet = 2;
		break;

	case MB_FUNC_WRITE_MULTIPLE_REGISTERS:
		nRet = (size_t)pSlv->adu.pdu.b[4];
		break;

	case MB_FUNC_DIAGNOSTIC:
		nRet = 2;
		break;

	default:
		nRet = 0;
		break;
	}

	return nRet;
}

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

bool GfaModbusRequestFunctionKnown(uint8_t func)
{
	switch(func)
	{
	case MB_FUNC_READ_HOLDING_REGISTERS:
	case MB_FUNC_READ_INPUT_REGISTERS:
	case MB_FUNC_PRESET_SINGLE_REGISTER:
	case MB_FUNC_WRITE_MULTIPLE_REGISTERS:
	case MB_FUNC_DIAGNOSTIC:
		return true;
	default:
		return false;
	}
}

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

bool GfaModbusRequestSubFunctionKnown(uint16_t subfunc)
{
	switch(subfunc)
	{
	case MB_SUBFUNC_RETURN_QUERY_DATA:
	case MB_SUBFUNC_CLEAR_CTRS_AND_DIAGNOSTIC_REG:
	case MB_SUBFUNC_RETURN_BUS_MESSAGE_COUNT:
	case MB_SUBFUNC_RETURN_BUS_COMM_ERROR_COUNT:
	case MB_SUBFUNC_RETURN_BUS_EXCEPTION_ERROR_COUNT:
	case MB_SUBFUNC_RETURN_SLAVE_MESSAGE_COUNT:
		return true;
	default:
		return false;
	}
}

bool GfaModbusRequestVerifyCRC(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 GfaModbusRequestSetCRC(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);
}

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

size_t GfaModbusRequestReadRegisters(uint8_t func, LPGFA_MODBUS_REGISTER pRegMap, LPGFA_MODBUS_SLAVE_APP_INTERFACE pAppItf, void *pBuf, uint8_t *pnCbWritten)
{
	uint8_t *p = (uint8_t*)pBuf;
	uint16_t nRegsRead = 0, nPhyStart;
	uint16_t nRegStart	= GfaBufGetUnaligned_uint16(&p[0]);
	uint16_t nRegCount	= GfaBufGetUnaligned_uint16(&p[2]);

	if(pAppItf->pfnMapRegAddr)
	{
		if((nPhyStart = (*pAppItf->pfnMapRegAddr)(nRegStart)) == (uint16_t)-1)
		{
			*p				= MB_ERROR_ILLEGAL_DATA_ADDRESS;
			*pnCbWritten	= 1;
			return 0;
		}
	}
	else
	{
		nPhyStart = nRegStart;
	}

	if(nRegCount > MODBUS_MAX_READ_REGISTERS)
	{
		*p				= MB_ERROR_ILLEGAL_DATA_VALUE;
		*pnCbWritten	= 1;
		return 0;
	}

	if((nPhyStart + nRegCount) > pRegMap->nCountRegs)
		nRegCount = pRegMap->nCountRegs - nPhyStart;

	*p++ = nRegCount * sizeof(uint16_t);

	if(pAppItf->pfnPreRead)
		(*pAppItf->pfnPreRead)(func, nPhyStart, nRegCount);

	for(nRegsRead = 0; nRegsRead < nRegCount; nRegsRead++, nPhyStart++)
	{
		GfaBufSetUnaligned_uint16(p, pRegMap->pRegs[nPhyStart]);
		p += sizeof(uint16_t);
	}

	if(pAppItf->pfnPostRead)
		(*pAppItf->pfnPostRead)(func, nPhyStart, nRegsRead);

	*pnCbWritten = 1 + nRegsRead * sizeof(uint16_t);
	return nRegsRead;
}

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

size_t GfaModbusRequestWriteRegisters(uint8_t func, LPGFA_MODBUS_REGISTER pRegMap, LPGFA_MODBUS_SLAVE_APP_INTERFACE pAppItf, void *pBuf, uint8_t *pnCbWritten)
{
	uint8_t *p = (uint8_t*)pBuf;
	uint16_t nRegsWritten = 0, nPhyStart;
	uint16_t nRegStart	= GfaBufGetUnaligned_uint16(&p[0]);
	uint16_t nRegCount	= GfaBufGetUnaligned_uint16(&p[2]);
	uint16_t nCbToWrite	= (uint16_t)p[4];

	if(pAppItf->pfnMapRegAddr)
	{
		if((nPhyStart = (*pAppItf->pfnMapRegAddr)(nRegStart)) == (uint16_t)-1)
		{
			*p				= MB_ERROR_ILLEGAL_DATA_ADDRESS;
			*pnCbWritten	= 1;
			return 0;
		}
	}
	else
	{
		nPhyStart = nRegStart;
	}

	if(nRegCount > MODBUS_MAX_READ_REGISTERS)
	{
		*p				= MB_ERROR_ILLEGAL_DATA_VALUE;
		*pnCbWritten	= 1;
		return 0;
	}
	else if(nRegCount != nCbToWrite / sizeof(uint16_t))
	{
		*p				= MB_ERROR_ILLEGAL_DATA_VALUE;
		*pnCbWritten	= 1;
		return 0;
	}

	p += 1 + 2 * sizeof(uint16_t);

	if((nPhyStart + nRegCount) > pRegMap->nCountRegs)
		nRegCount = pRegMap->nCountRegs - nPhyStart;

	if(pAppItf->pfnPreWrite)
		(*pAppItf->pfnPreWrite)(func, nPhyStart, nRegCount);

	for(nRegsWritten = 0; nRegsWritten < nRegCount; nRegsWritten++, nPhyStart++)
	{
		pRegMap->pRegs[nPhyStart] = GfaBufGetUnaligned_uint16(p);
		p += sizeof(uint16_t);
	}

	if(pAppItf->pfnPostWrite)
		(*pAppItf->pfnPostWrite)(func, nPhyStart, nRegsWritten);

	p = (uint8_t*)pBuf;
	GfaBufSetUnaligned_uint16(p, nRegStart);
	p += sizeof(uint16_t);
	GfaBufSetUnaligned_uint16(p, nRegCount);
	p += sizeof(uint16_t);

	*pnCbWritten = 2 * sizeof(uint16_t);
	return nRegsWritten;
}

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

size_t GfaModbusRequestWriteSingleRegister(uint8_t func, LPGFA_MODBUS_REGISTER pRegMap, LPGFA_MODBUS_SLAVE_APP_INTERFACE pAppItf, void *pBuf, uint8_t *pnCbWritten)
{
	uint8_t *p = (uint8_t*)pBuf;
	uint16_t nPhyStart, nRegAddr = GfaBufGetUnaligned_uint16(&p[0]);

	if(pAppItf->pfnMapRegAddr)
	{
		if((nPhyStart = (*pAppItf->pfnMapRegAddr)(nRegAddr)) == (uint16_t)-1)
		{
			*p				= MB_ERROR_ILLEGAL_DATA_ADDRESS;
			*pnCbWritten	= 1;
			return 0;
		}
	}
	else
	{
		nPhyStart = nRegAddr;
	}

	if(nPhyStart >= pRegMap->nCountRegs)
	{
		*p				= MB_ERROR_ILLEGAL_DATA_ADDRESS;
		*pnCbWritten	= 1;
		return 0;
	}

	p += sizeof(uint16_t);
	if(pAppItf->pfnPreWrite)
		(*pAppItf->pfnPreWrite)(func, nPhyStart, 1);
	pRegMap->pRegs[nPhyStart] = GfaBufGetUnaligned_uint16(p);
	if(pAppItf->pfnPostWrite)
		(*pAppItf->pfnPostWrite)(func, nPhyStart, 1);
	*pnCbWritten = 2 * sizeof(uint16_t);
	return 1;
}

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

size_t GfaModbusRequestDiagnostic(LPGFA_MODBUS_RTU_SLAVE pSlv, uint8_t *pnCbWritten)
{
	uint8_t *p = pSlv->adu.pdu.b;
	uint16_t nVal, nSubFunc = GfaBufGetUnaligned_uint16(p);
	p += sizeof(uint16_t);

	switch(nSubFunc)
	{
	case MB_SUBFUNC_RETURN_QUERY_DATA:
		*pnCbWritten = 2 * sizeof(uint16_t);	// Echo Request Data
		break;

	case MB_SUBFUNC_CLEAR_CTRS_AND_DIAGNOSTIC_REG:
		memset(&pSlv->diag, 0, sizeof(pSlv->diag));
		*pnCbWritten = 2 * sizeof(uint16_t);	// Echo Request Data
		break;

	case MB_SUBFUNC_RETURN_BUS_MESSAGE_COUNT:
		nVal = (uint16_t)(pSlv->diag.nBusMsgCount & 0xFFFF);
		GfaBufSetUnaligned_uint16(p, nVal);
		*pnCbWritten = 2 * sizeof(uint16_t);
		break;

	case MB_SUBFUNC_RETURN_BUS_COMM_ERROR_COUNT:
		nVal = (uint16_t)(pSlv->diag.nCrcErrCount & 0xFFFF);
		GfaBufSetUnaligned_uint16(p, nVal);
		*pnCbWritten = 2 * sizeof(uint16_t);
		break;

	case MB_SUBFUNC_RETURN_BUS_EXCEPTION_ERROR_COUNT:
		nVal = (uint16_t)(pSlv->diag.nExcErrCount & 0xFFFF);
		GfaBufSetUnaligned_uint16(p, nVal);
		*pnCbWritten = 2 * sizeof(uint16_t);
		break;

	case MB_SUBFUNC_RETURN_SLAVE_MESSAGE_COUNT:
		nVal = (uint16_t)(pSlv->diag.nSlvMsgCount & 0xFFFF);
		GfaBufSetUnaligned_uint16(p, nVal);
		*pnCbWritten = 2 * sizeof(uint16_t);
		break;

	default:
		pSlv->adu.pdu.b[0]	= MB_ERROR_ILLEGAL_FUNCTION;
		*pnCbWritten		= 1;
		break;
	}

	return *pnCbWritten;
}

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

size_t GfaModbusRequestCreateExceptionResponse(LPMODBUS_RTU_PDU pPdu, uint8_t nExceptionCode)
{
	pPdu->func |= MB_FUNC_ERROR_FLAG;
	pPdu->b[0] = nExceptionCode;
	return 1;
}

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

bool GfaModbusRequestHandler(LPGFA_MODBUS_RTU_SLAVE pSlv, size_t *pnCbData, bool *pbIsException)
{
	uint8_t nCbWritten;
	bool bSendResponse = true;
	*pnCbData = 0;
	*pbIsException = false;

	switch(pSlv->adu.pdu.func)
	{
	case MB_FUNC_READ_HOLDING_REGISTERS:
		if(!GfaModbusRequestReadRegisters(pSlv->adu.pdu.func, &pSlv->regMap, &pSlv->appItf, pSlv->adu.pdu.b, &nCbWritten))
		{
			pSlv->adu.pdu.func |= MB_FUNC_ERROR_FLAG;
			*pbIsException = true;
		}
		*pnCbData = nCbWritten;
		break;

	case MB_FUNC_PRESET_SINGLE_REGISTER:
		if(!GfaModbusRequestWriteSingleRegister(pSlv->adu.pdu.func, &pSlv->regMap, &pSlv->appItf, pSlv->adu.pdu.b, &nCbWritten))
		{
			pSlv->adu.pdu.func |= MB_FUNC_ERROR_FLAG;
			*pbIsException = true;
		}
		*pnCbData = nCbWritten;
		break;

	case MB_FUNC_WRITE_MULTIPLE_REGISTERS:
		if(!GfaModbusRequestWriteRegisters(pSlv->adu.pdu.func, &pSlv->regMap, &pSlv->appItf, pSlv->adu.pdu.b, &nCbWritten))
		{
			pSlv->adu.pdu.func |= MB_FUNC_ERROR_FLAG;
			*pbIsException = true;
		}
		*pnCbData = nCbWritten;
		break;

	case MB_FUNC_DIAGNOSTIC:
		if(!GfaModbusRequestDiagnostic(pSlv, &nCbWritten))
		{
			pSlv->adu.pdu.func |= MB_FUNC_ERROR_FLAG;
			*pbIsException = true;
		}
		*pnCbData = nCbWritten;
		break;

	default:
		*pnCbData = GfaModbusRequestCreateExceptionResponse(&pSlv->adu.pdu, MB_ERROR_ILLEGAL_FUNCTION);
		*pbIsException = true;
		break;
	}

	return bSendResponse;
}