Modbus_RTU_for_Tiva/gfamodbusrtumst/mbrtumast.c

#include <stdlib.h>
#include <string.h>
#include <gfambrtumst.h>

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

#define MODBUS_MASTER_RX_TIMEOUT_US							500000	// slave to master response timeout

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

typedef struct _MODBUS_RTU_MASTER
{
	HFIFO hFifoRX;
	HFIFO hFifoTX;
	uint8_t curSlaveID;
	uint8_t curFunc;
	uint32_t nTID;
	int nRxErr;
	GFA_MODBUS_RTU_MST_STATES state;
	GFA_MODBUS_RTU_MST_STATES oldstate;
	MODBUS_RTU_ADU adu;
	PFN_GFA_RAW_REQUEST_COMPLETE pfnRawReqComplete;
	void *pParam;
	size_t nCbDataExpected;
	size_t nCbRx;
	uint64_t nRxTimeoutUs;
	uint64_t nTimerUs;
	GFA_MODBUS_MASTER_APP_INTERFACE appItf;
	GFA_MODBUS_PROTOCOL_TIMEOUTS mpt;
}MODBUS_RTU_MASTER, *LPMODBUS_RTU_MASTER;
typedef const MODBUS_RTU_MASTER *LPCMODBUS_RTU_MASTER;

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

static MODBUS_RTU_MASTER		g_mbMast = {0};

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

static bool _ValidateADU(LPMODBUS_RTU_MASTER pMst)
{
	if(pMst->nRxErr)
	{
		if(pMst->nRxErr != GFA_MB_MST_ERROR_SLAVE_ERROR)
			return false;
	}

	if(!GfaBufVerifyCRC(&pMst->adu, pMst->nCbDataExpected + 2, &pMst->adu.pdu.b[pMst->nCbDataExpected]))
	{
		pMst->nRxErr = GFA_MB_MST_ERROR_SLAVE_INVALID_CRC;
		return false;
	}
	return true;
}

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

static bool _TimerElapsed(LPMODBUS_RTU_MASTER pMst)
{
	struct timeval tv;
	return pMst->nTimerUs < GfaSystickTimeval2Us(GfaSystickGetUsClock(&tv));
}

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

static void _RespTimerTrigger(LPMODBUS_RTU_MASTER pMst)
{
	struct timeval tv;
	pMst->nTimerUs = GfaSystickTimeval2Us(GfaSystickGetUsClock(&tv)) + pMst->nRxTimeoutUs;
}

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

static void _CharTimerTrigger(LPMODBUS_RTU_MASTER pMst)
{
	struct timeval tv;
	pMst->nTimerUs = GfaSystickTimeval2Us(GfaSystickGetUsClock(&tv)) + pMst->mpt.nCharTimeoutUs;
}

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

static void _FrameTimerTrigger(LPMODBUS_RTU_MASTER pMst)
{
	struct timeval tv;
	pMst->nTimerUs = GfaSystickTimeval2Us(GfaSystickGetUsClock(&tv)) + pMst->mpt.nFrameTimeoutUs;
}

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

static bool _PeekStateChange(LPMODBUS_RTU_MASTER pMst)
{
	if(pMst->oldstate != pMst->state)
	{
		if(pMst->appItf.pfnStateChanged)
			(*pMst->appItf.pfnStateChanged)(pMst->state, pMst->oldstate);
		pMst->oldstate = pMst->state;
		return true;
	}
	return false;
}

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

HMBRTUMST GfaModbusRTUMstCreate(LPCGFA_MODBUS_RTU_MASTER_PARAMETERS pmap)
{
	if(!pmap || !pmap->hFifoRX || !pmap->hFifoTX)
		return NULL;

	LPMODBUS_RTU_MASTER pMst = &g_mbMast;
	memset(pMst, 0, sizeof(MODBUS_RTU_MASTER));
	memcpy(&pMst->mpt, &pmap->mpt, sizeof(GFA_MODBUS_PROTOCOL_TIMEOUTS));
	memcpy(&pMst->appItf, &pmap->appItf, sizeof(GFA_MODBUS_MASTER_APP_INTERFACE));

	pMst->oldstate		= MB_RTU_MST_Void;
	pMst->hFifoRX		= pmap->hFifoRX;
	pMst->hFifoTX		= pmap->hFifoTX;
	pMst->nRxTimeoutUs	= pmap->nRxTimeoutUs;

	return (HMBRTUMST)pMst;
}

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

void GfaModbusRTUMstRelease(HMBRTUMST hMbMst)
{
}

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

bool GfaModbusRTUMstStateMachine(HMBRTUMST hMbMst)
{
	uint8_t b;
	LPMODBUS_RTU_MASTER pMst = (LPMODBUS_RTU_MASTER)hMbMst;

	switch(pMst->state)
	{
	case MB_RTU_MST_Idle:
		_PeekStateChange(pMst);
		break;

	case MB_RTU_MST_TxStart:
		_PeekStateChange(pMst);
		if(	_TimerElapsed(pMst) &&
			GfaMbFifoTxPrepare(pMst->hFifoTX, false))
		{
			if(pMst->appItf.pfnPreTx)
				(*pMst->appItf.pfnPreTx)(pMst->nTID, pMst->curSlaveID);
			GfaMbFifoClearFlags(pMst->hFifoRX, MB_RTU_FLAG_FRAME_GAP_DETECT, true);
			GfaMbFifoSetFlags(pMst->hFifoTX, MB_RTU_FLAG_TRANSMIT_IN_PROGRESS, false);
			GfaMbFifoTxStart(pMst->hFifoTX, true);
			pMst->state = MB_RTU_MST_TxWaitEnd;
			// fall through to next state
		}
		else
			break;

	case MB_RTU_MST_TxWaitEnd:
		_PeekStateChange(pMst);
		if(!GfaMbFifoMatchFlags(pMst->hFifoTX, MB_RTU_FLAG_TRANSMIT_IN_PROGRESS, true))
		{
			pMst->state = MB_RTU_MST_TxDone;
			// fall through to next state
		}
		else
			break;

	case MB_RTU_MST_TxDone:
		_PeekStateChange(pMst);
		pMst->state = MB_RTU_MST_WaitRxFifo;
		_RespTimerTrigger(pMst);
		if(pMst->appItf.pfnPostTx)
			(*pMst->appItf.pfnPostTx)(pMst->nTID, pMst->curSlaveID);
		// fall through to next state

	case MB_RTU_MST_WaitRxFifo:
		_PeekStateChange(pMst);
		if(GfaMbFifoPeek(pMst->hFifoRX, true))
		{
			GfaMbFifoClearFlags(pMst->hFifoRX, MB_RTU_FLAG_FRAME_GAP_DETECT, true);
			pMst->state = MB_RTU_MST_RxSlvID;
			if(pMst->appItf.pfnRxStart)
				(*pMst->appItf.pfnRxStart)(pMst->nTID);
			// fall through to next state
		}
		else
		{
			if(_TimerElapsed(pMst))
			{
				pMst->nRxErr	= GFA_MB_MST_ERROR_SLAVE_RESPONSE_TIMEOUT;
				pMst->state 	= MB_RTU_MST_ReportError;
			}
			break;
		}

	case MB_RTU_MST_RxSlvID:
		_PeekStateChange(pMst);
		if(!GfaMbFifoPop(pMst->hFifoRX, &b, true))
		{
			pMst->nRxErr	= GFA_MB_MST_ERROR_SLAVE_RESPONSE_TIMEOUT;
			pMst->state 	= MB_RTU_MST_ReportError;
			break;
		}
		else if(pMst->curSlaveID != b)
		{
			GfaMbFifoSetFlags(pMst->hFifoRX, MB_RTU_FLAG_IGNORE_FRAME, true);
			GfaMbFifoReset(pMst->hFifoRX, true);
			pMst->nRxErr	= GFA_MB_MST_ERROR_INVALID_SLAVE_ID;
			pMst->state		= MB_RTU_MST_ReportError;
			break;
		}
		pMst->adu.slaveID	= b;
		pMst->state			= MB_RTU_MST_RxFunc;
		_CharTimerTrigger(pMst);
		// fall through to next state

	case MB_RTU_MST_RxFunc:
		_PeekStateChange(pMst);
		if(!GfaMbFifoPop(pMst->hFifoRX, &b, true))
		{
			if(	GfaMbFifoMatchFlags(pMst->hFifoRX, MB_RTU_FLAG_FRAME_GAP_DETECT, true) ||
				_TimerElapsed(pMst))
			{
				pMst->nRxErr	= GFA_MB_MST_ERROR_SLAVE_CHAR_TIMEOUT;
				pMst->state		= MB_RTU_MST_ReportError;
			}
			break;
		}
		else if(b & MB_FUNC_ERROR_FLAG)
		{
			pMst->nRxErr		= GFA_MB_MST_ERROR_SLAVE_ERROR;
			pMst->adu.pdu.func	= b;
			pMst->state			= MB_RTU_MST_RxError;
			break;
		}
		else if(b != pMst->curFunc)
		{
			pMst->nRxErr		= GFA_MB_MST_ERROR_INVALID_FUNCTION;
			pMst->state			= MB_RTU_MST_ReportError;
			break;
		}
		else
		{
			pMst->adu.pdu.func	= b;
			pMst->nCbRx			= 0;
			pMst->state			= MB_RTU_MST_RxData;
			_CharTimerTrigger(pMst);
			// fall through to next state
		}

	case MB_RTU_MST_RxData:
		_PeekStateChange(pMst);
		while(pMst->nCbRx < pMst->nCbDataExpected)
		{
			if(GfaMbFifoPop(pMst->hFifoRX, &b, true))
			{
				pMst->adu.pdu.b[pMst->nCbRx++] = b;
				_CharTimerTrigger(pMst);
			}
			else
			{
				if(	GfaMbFifoMatchFlags(pMst->hFifoRX, MB_RTU_FLAG_FRAME_GAP_DETECT, true) ||
					_TimerElapsed(pMst))
				{
					pMst->nRxErr	= GFA_MB_MST_ERROR_SLAVE_CHAR_TIMEOUT;
					pMst->state		= MB_RTU_MST_ReportError;
				}
				break;
			}
		}

		if(pMst->nCbRx == pMst->nCbDataExpected)
		{
			pMst->nCbRx = 0;
			pMst->state = MB_RTU_MST_RxCRC;
			_CharTimerTrigger(pMst);
			// fall through to next state
		}
		else
			break;

	case MB_RTU_MST_RxCRC:
		_PeekStateChange(pMst);
		while(pMst->nCbRx < sizeof(uint16_t))
		{
			if(GfaMbFifoPop(pMst->hFifoRX, &b, true))
			{
				pMst->adu.pdu.b[pMst->nCbDataExpected + pMst->nCbRx++] = b;
				_CharTimerTrigger(pMst);
			}
			else
			{
				if(	GfaMbFifoMatchFlags(pMst->hFifoRX, MB_RTU_FLAG_FRAME_GAP_DETECT, true) ||
					_TimerElapsed(pMst))
				{
					pMst->nRxErr	= GFA_MB_MST_ERROR_SLAVE_CHAR_TIMEOUT;
					pMst->state		= MB_RTU_MST_ReportError;
				}
				break;
			}
		}

		if(pMst->nCbRx == sizeof(uint16_t))
		{
			pMst->nCbRx = 0;
			pMst->state = MB_RTU_MST_RxFinalize;
			// fall through to next state
		}
		else
			break;

	case MB_RTU_MST_RxFinalize:
		_PeekStateChange(pMst);
		_FrameTimerTrigger(pMst);
		if(_ValidateADU(pMst))
		{
			if(pMst->appItf.pfnRxEnd)
				(*pMst->appItf.pfnRxEnd)(pMst->nTID);
			(*pMst->pfnRawReqComplete)(pMst->nTID, pMst->curSlaveID, pMst->nRxErr, &pMst->adu, pMst->nCbDataExpected, pMst->pParam);
			pMst->state = MB_RTU_MST_Idle;
			break;
		}
		pMst->state = MB_RTU_MST_ReportError;
		break;

	case MB_RTU_MST_RxError:
		_PeekStateChange(pMst);
		if(!GfaMbFifoPop(pMst->hFifoRX, &b, true))
		{
			if(	GfaMbFifoMatchFlags(pMst->hFifoRX, MB_RTU_FLAG_FRAME_GAP_DETECT, true) ||
				_TimerElapsed(pMst))
			{
				pMst->nRxErr	= GFA_MB_MST_ERROR_SLAVE_CHAR_TIMEOUT;
				pMst->state		= MB_RTU_MST_ReportError;
			}
			break;
		}
		pMst->adu.pdu.b[0]		= b;
		pMst->nCbDataExpected	= 1;
		pMst->state				= MB_RTU_MST_RxCRC;
		_CharTimerTrigger(pMst);
		break;

	case MB_RTU_MST_ReportError:
		_PeekStateChange(pMst);
		_FrameTimerTrigger(pMst);
		if(pMst->appItf.pfnRxEnd)
			(*pMst->appItf.pfnRxEnd)(pMst->nTID);
		(*pMst->pfnRawReqComplete)(pMst->nTID, pMst->curSlaveID, pMst->nRxErr, NULL, 0, pMst->pParam);
		pMst->state = MB_RTU_MST_Idle;
		break;
	}

	return true;
}

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

static void _OnRawReadRegistersComplete(uint32_t nTID, uint8_t nSlvID, uint16_t err, LPCMODBUS_RTU_ADU padu, size_t nCbData, void *pParam)
{
	if(pParam)
	{
		PFN_GFA_READ_REGISTERS_COMPLETE pfnReadRegsComplete = (PFN_GFA_READ_REGISTERS_COMPLETE)pParam;
		uint16_t regs[MODBUS_MAX_READ_REGISTERS];
		const void *pRegs = NULL;
		size_t nCntRegs = 0;

		if(err)
		{
			if(err == GFA_MB_MST_ERROR_SLAVE_ERROR && padu)
				err |= ((uint16_t)padu->pdu.b[0] << 8);
		}
		else
		{
			if((nCntRegs = (size_t)padu->pdu.b[0] / sizeof(uint16_t)) <= MODBUS_MAX_READ_REGISTERS)
			{
				GfaBufCpyUnaligned_uint16(regs, &padu->pdu.b[1], nCntRegs);
				pRegs = regs;
			}
			else
			{
				pRegs = NULL;
				nCntRegs = 0;
				err = GFA_MB_MST_ERROR_SLAVE_INVALID_DATA;
			}
		}

		(*pfnReadRegsComplete)(nTID, nSlvID, err, pRegs, nCntRegs);
	}
}

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

static void _OnRawWriteRegistersComplete(uint32_t nTID, uint8_t nSlvID, uint16_t err, LPCMODBUS_RTU_ADU padu, size_t nCbData, void *pParam)
{
	if(pParam)
	{
		PFN_GFA_WRITE_REGISTERS_COMPLETE pfnWriteRegsComplete = (PFN_GFA_WRITE_REGISTERS_COMPLETE)pParam;
		uint16_t nStart = 0, nWritten = 0;

		if(err)
		{
			if(err == GFA_MB_MST_ERROR_SLAVE_ERROR && padu)
				err |= ((uint16_t)padu->pdu.b[0] << 8);
		}
		else
		{
			nStart		= GfaBufGetUnaligned_uint16(padu->pdu.b);
			nWritten	= GfaBufGetUnaligned_uint16(&padu->pdu.b[2]);
		}

		(*pfnWriteRegsComplete)(nTID, nSlvID, err, nStart, nWritten);
	}
}

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

static void _OnRawPresetSingleRegisterComplete(uint32_t nTID, uint8_t nSlvID, uint16_t err, LPCMODBUS_RTU_ADU padu, size_t nCbData, void *pParam)
{
	if(pParam)
	{
		PFN_GFA_PRESET_SINGLE_REGISTER_COMPLETE pfnPresetRegComplete = (PFN_GFA_PRESET_SINGLE_REGISTER_COMPLETE)pParam;

		if(err)
		{
			if(err == GFA_MB_MST_ERROR_SLAVE_ERROR && padu)
				err |= ((uint16_t)padu->pdu.b[0] << 8);
		}

		(*pfnPresetRegComplete)(nTID, nSlvID, err);
	}
}

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

static void _OnRawDiagnosisComplete(uint32_t nTID, uint8_t nSlvID, uint16_t err, LPCMODBUS_RTU_ADU padu, size_t nCbData, void *pParam)
{
	if(pParam)
	{
		PFN_GFA_DIAGNOSIS_COMPLETE pfnDiagComplete = (PFN_GFA_DIAGNOSIS_COMPLETE)pParam;
		uint16_t nSubFunc = 0, nData = 0;

		if(err)
		{
			if(err == GFA_MB_MST_ERROR_SLAVE_ERROR && padu)
				err |= ((uint16_t)padu->pdu.b[0] << 8);
		}
		else
		{
			nSubFunc	= GfaBufGetUnaligned_uint16(padu->pdu.b);
			nData		= GfaBufGetUnaligned_uint16(&padu->pdu.b[2]);
		}

		(*pfnDiagComplete)(nTID, nSlvID, err, nSubFunc, nData);
	}
}

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

static void _OnRawReportSlaveIDComplete(uint32_t nTID, uint8_t nSlvID, uint16_t err, LPCMODBUS_RTU_ADU padu, size_t nCbData, void *pParam)
{
	if(pParam)
	{
		PFN_GFA_REPORT_SLAVE_ID_COMPLETE pfnRepSlvIDComplete = (PFN_GFA_REPORT_SLAVE_ID_COMPLETE)pParam;
		const void *pData = NULL;
		size_t nData = 0;

		if(err)
		{
			if(err == GFA_MB_MST_ERROR_SLAVE_ERROR && padu)
				err |= ((uint16_t)padu->pdu.b[0] << 8);
		}
		else
		{
			pData = &padu->pdu.b[1];
			nData = padu->pdu.b[0];
		}

		(*pfnRepSlvIDComplete)(nTID, nSlvID, err, pData, nData);
	}
}

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

static size_t _SubFunctionResponseLength(uint16_t nSubFunc)
{
	size_t nRet = 0;

	switch(nSubFunc)
	{
	case MB_SUBFUNC_RETURN_QUERY_DATA:
	case MB_SUBFUNC_RESTART_COMM_OPTION:
	case MB_SUBFUNC_RETURN_DIAGNOSTIC_REGISTER:
	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:
	case MB_SUBFUNC_RETURN_SLAVE_NO_RESPONSE_COUNT:
	case MB_SUBFUNC_RETURN_SLAVE_NAK_COUNT:
	case MB_SUBFUNC_RETURN_SLAVE_BUSY_COUNT:
	case MB_SUBFUNC_RETURN_BUS_CHAR_OVERRUN_COUNT:
	case MB_SUBFUNC_RETURN_OVERRUN_ERROR_COUNT:
	case MB_SUBFUNC_CLEAR_OVERRUN_COUNTER_AND_FLAG:
		nRet = 4;
		break;

//	case MB_SUBFUNC_FORCE_LISTEN_ONLY_MODE:
	default:
		nRet = (size_t)-1;
		break;
	}

	return nRet;
}

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

bool GfaModbusRTUMstSendRawRequest(HMBRTUMST hMbMst, uint32_t nTID, uint8_t nSlvID, uint8_t nFunc, const void *pData, size_t nCbData, size_t nCbDataExpected, PFN_GFA_RAW_REQUEST_COMPLETE pfnRawReqComplete, void *pParam)
{
	uint16_t nCRC;
	LPMODBUS_RTU_MASTER pMst = (LPMODBUS_RTU_MASTER)hMbMst;

	if(!pMst || nCbData > MODBUS_MAX_DATA_PAYLOAD || nCbDataExpected > MODBUS_MAX_DATA_PAYLOAD || !pfnRawReqComplete)
		return false;

	if(!nSlvID || nSlvID > MODBUS_MAX_SLAVE_ID)
		return false;

	if(pMst->state != MB_RTU_MST_Idle)
		return false;

	pMst->nTID				= nTID;
	pMst->curSlaveID		= nSlvID;
	pMst->curFunc			= nFunc;
	pMst->nCbDataExpected	= nCbDataExpected;
	pMst->pfnRawReqComplete	= pfnRawReqComplete;
	pMst->pParam			= pParam;
	pMst->nRxErr			= 0;

	GfaMbFifoReset(pMst->hFifoTX, false);
	GfaMbFifoReset(pMst->hFifoRX, false);

	GfaMbFifoPush(pMst->hFifoTX, nSlvID, false);
	GfaMbFifoPush(pMst->hFifoTX, nFunc, false);
	if(pData && nCbData)
		GfaMbFifoWrite(pMst->hFifoTX, pData, nCbData, false);
	nCRC = GfaMbFifoCalcCRC(pMst->hFifoTX, false);
	GfaMbFifoWrite(pMst->hFifoTX, &nCRC, sizeof(nCRC), false);

	pMst->state = MB_RTU_MST_TxStart;
	return true;
}

bool GfaModbusRTUMstReadHoldingRegisters(HMBRTUMST hMbMst, uint32_t nTID, uint8_t nSlvID, uint16_t nRegStart, uint16_t nRegCount, PFN_GFA_READ_REGISTERS_COMPLETE pfnReadRegsComplete)
{
	uint8_t buf[4];
	if(!pfnReadRegsComplete)
		return false;
	GfaBufSetUnaligned_uint16(buf, nRegStart);
	GfaBufSetUnaligned_uint16(&buf[2], nRegCount);
	return GfaModbusRTUMstSendRawRequest(hMbMst, nTID, nSlvID, MB_FUNC_READ_HOLDING_REGISTERS, buf, sizeof(buf), 1 + nRegCount * sizeof(uint16_t), _OnRawReadRegistersComplete, pfnReadRegsComplete);
}

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

bool GfaModbusRTUMstReadInputRegisters(HMBRTUMST hMbMst, uint32_t nTID, uint8_t nSlvID, uint16_t nRegStart, uint16_t nRegCount, PFN_GFA_READ_REGISTERS_COMPLETE pfnReadRegsComplete)
{
	uint8_t buf[4];
	if(!pfnReadRegsComplete)
		return false;
	GfaBufSetUnaligned_uint16(buf, nRegStart);
	GfaBufSetUnaligned_uint16(&buf[2], nRegCount);
	return GfaModbusRTUMstSendRawRequest(hMbMst, nTID, nSlvID, MB_FUNC_READ_INPUT_REGISTERS, buf, sizeof(buf), 1 + nRegCount * sizeof(uint16_t), _OnRawReadRegistersComplete, pfnReadRegsComplete);
}

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

bool GfaModbusRTUMstWriteMultipleRegisters(HMBRTUMST hMbMst, uint32_t nTID, uint8_t nSlvID, uint16_t nRegStart, uint16_t nRegCount, const void *pRegData, PFN_GFA_WRITE_REGISTERS_COMPLETE pfnWriteRegsComplete)
{
	uint8_t buf[MODBUS_MAX_WRITE_REGISTERS * sizeof(uint16_t)];
	const uint8_t *pData = (const uint8_t*)pRegData;
	size_t nCbData = 0;
	if(!pfnWriteRegsComplete)
		return false;
	if(nRegCount > MODBUS_MAX_WRITE_REGISTERS)
		return false;
	GfaBufSetUnaligned_uint16(&buf[nCbData], nRegStart);
	nCbData += 2;
	GfaBufSetUnaligned_uint16(&buf[nCbData], nRegCount);
	nCbData += 2;
	buf[nCbData++] = (uint8_t)(nRegCount * sizeof(uint16_t));
	GfaBufCpyUnaligned_uint16(&buf[nCbData], pData, nRegCount);
	nCbData += nRegCount * sizeof(uint16_t);
	return GfaModbusRTUMstSendRawRequest(hMbMst, nTID, nSlvID, MB_FUNC_WRITE_MULTIPLE_REGISTERS, buf, nCbData, 2 * sizeof(uint16_t), _OnRawWriteRegistersComplete, pfnWriteRegsComplete);
}

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

bool GfaModbusRTUMstPresetSingleRegister(HMBRTUMST hMbMst, uint32_t nTID, uint8_t nSlvID, uint16_t nRegAddr, uint16_t nRegVal, PFN_GFA_PRESET_SINGLE_REGISTER_COMPLETE pfnPresetRegComplete)
{
	uint8_t buf[4];
	if(!pfnPresetRegComplete)
		return false;
	GfaBufSetUnaligned_uint16(buf, nRegAddr);
	GfaBufSetUnaligned_uint16(&buf[2], nRegVal);
	return GfaModbusRTUMstSendRawRequest(hMbMst, nTID, nSlvID, MB_FUNC_PRESET_SINGLE_REGISTER, buf, sizeof(buf), 4, _OnRawPresetSingleRegisterComplete, pfnPresetRegComplete);
}

bool GfaModbusRTUMstDiagnosis(HMBRTUMST hMbMst, uint32_t nTID, uint8_t nSlvID, uint16_t nSubFunc, uint16_t nData, PFN_GFA_DIAGNOSIS_COMPLETE pfnDiagComplete)
{
	uint8_t buf[4];
	if(!pfnDiagComplete)
		return false;
	GfaBufSetUnaligned_uint16(buf, nSubFunc);
	GfaBufSetUnaligned_uint16(&buf[2], nData);
	size_t nResponseLength = _SubFunctionResponseLength(nSubFunc);
	if(nResponseLength == (size_t)-1)
		return false;
	return GfaModbusRTUMstSendRawRequest(hMbMst, nTID, nSlvID, MB_FUNC_DIAGNOSTIC, buf, sizeof(buf), nResponseLength, _OnRawDiagnosisComplete, pfnDiagComplete);
}

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

bool GfaModbusRTUMstReportSlaveID(HMBRTUMST hMbMst, uint32_t nTID, uint8_t nSlvID, size_t nCbDataExpected, PFN_GFA_REPORT_SLAVE_ID_COMPLETE pfnRepSlvIDComplete)
{
	if(!pfnRepSlvIDComplete)
		return false;
	return GfaModbusRTUMstSendRawRequest(hMbMst, nTID, nSlvID, MB_FUNC_REPORT_SLAVE_ID, NULL, 0, nCbDataExpected, _OnRawReportSlaveIDComplete, pfnRepSlvIDComplete);
}