|
@@ -0,0 +1,640 @@
|
|
|
+#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);
|
|
|
+}
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|