1031 lines
31 KiB
C++
1031 lines
31 KiB
C++
#include "sub_modbus_tcp.h"
|
|
|
|
#define MB_TCP_TID 0
|
|
#define MB_TCP_PID 2
|
|
#define MB_TCP_LEN 4
|
|
#define MB_TCP_UID 6
|
|
#define MB_TCP_FUNC 7
|
|
|
|
#define MB_TCP_BUF_SIZE ( 256 + 7 ) /* Must hold a complete Modbus TCP frame. */
|
|
|
|
///////////////////////////////////////////////////////////////////////////////////
|
|
CSubModbusTcpProcessItem::CSubModbusTcpProcessItem()
|
|
{
|
|
ied_addr = 255;
|
|
m_nNum = 0;
|
|
|
|
m_CoilRegCount;
|
|
m_DiscreteRegCount = 0;
|
|
m_HoldingRegCount = 0;
|
|
m_InputregCount = 0;
|
|
|
|
m_pCoilRegTable = NULL;
|
|
m_pDiscreteRegTable = NULL;
|
|
m_pHoldingRegTable = NULL;
|
|
m_pInputRegTable = NULL;
|
|
}
|
|
|
|
CSubModbusTcpProcessItem::~CSubModbusTcpProcessItem()
|
|
{
|
|
ied_addr = 255;
|
|
m_nNum = 0;
|
|
|
|
if (m_pCoilRegTable)
|
|
{
|
|
delete[] m_pCoilRegTable;
|
|
m_pCoilRegTable = NULL;
|
|
}
|
|
|
|
if (m_pDiscreteRegTable)
|
|
{
|
|
delete[] m_pDiscreteRegTable;
|
|
m_pDiscreteRegTable = NULL;
|
|
}
|
|
|
|
if (m_pHoldingRegTable)
|
|
{
|
|
delete[] m_pHoldingRegTable;
|
|
m_pHoldingRegTable = NULL;
|
|
}
|
|
|
|
if (m_pInputRegTable)
|
|
{
|
|
delete[] m_pInputRegTable;
|
|
m_pInputRegTable = NULL;
|
|
}
|
|
}
|
|
|
|
void CSubModbusTcpProcessItem::Attach(int uid, int sock, DWORD peer_addr, WORD peer_port)
|
|
{
|
|
CNetProcessItem::Attach(uid, sock, peer_addr, peer_port);
|
|
|
|
m_nNum = 0;
|
|
}
|
|
|
|
void CSubModbusTcpProcessItem::Release(void)
|
|
{
|
|
m_nNum = 0;
|
|
|
|
CNetProcessItem::Release();
|
|
}
|
|
|
|
CSubModbusTcpProcess::CSubModbusTcpProcess()
|
|
{
|
|
|
|
}
|
|
|
|
CSubModbusTcpProcess::~CSubModbusTcpProcess()
|
|
{
|
|
}
|
|
|
|
CNetProcessItem *CSubModbusTcpProcess::CreateItem(int ord)
|
|
{
|
|
return dynamic_cast<CNetProcessItem *>(new CSubModbusTcpProcessItem);
|
|
}
|
|
|
|
void CSubModbusTcpProcess::DestroyItem(int ord, BOOLEAN bDeleted /* = FALSE */)
|
|
{
|
|
CSubModbusTcpProcessItem *pItem = (CSubModbusTcpProcessItem *)GetItem(ord);
|
|
if (pItem != NULL && !bDeleted)
|
|
{
|
|
delete pItem;
|
|
return CNetProcess::DestroyItem(ord, TRUE);
|
|
}
|
|
|
|
return CNetProcess::DestroyItem(ord, bDeleted);
|
|
}
|
|
|
|
BOOLEAN CSubModbusTcpProcess::OnPreCreate(int id)
|
|
{
|
|
int i, j;
|
|
int uid;
|
|
BYTE addr[6];
|
|
CSubModbusTcpProcessItem *pItem;
|
|
if (!CNetProcess::OnPreCreate(id)) return FALSE;
|
|
|
|
struSubModbusTCPOption m_option;
|
|
if (!GetOption(&m_option, sizeof(m_option))) return FALSE;
|
|
|
|
m_YC_Type = m_option.defaultYCType;
|
|
for (i = 0; i < PROCESS_UNIT_NUM; i++)
|
|
{
|
|
pItem = (CSubModbusTcpProcessItem *)GetItem(i);
|
|
if (NULL == pItem) continue;
|
|
uid = GetUnitID(i);
|
|
|
|
if (uid < 0 || uid >= UNIT_NUM) continue;
|
|
if (GetUnitAddr(uid, addr, 6))
|
|
{ //获取单元地址成功，则该段原地址采用配置地址，否则该单元为无效地址。
|
|
pItem->ied_addr = addr[4]; //配置地址
|
|
}
|
|
//此处增加判断
|
|
m_CoilRegTable.clear();
|
|
m_DiscreteTable.clear();
|
|
m_HoldingRegTable.clear();
|
|
m_InputRegTable.clear();
|
|
//获取遥信数量
|
|
int yxcount = GetUnitYXCount(uid);
|
|
//获取coil和discrete寄存器
|
|
for (j = 0; j < yxcount; j++) {
|
|
BYTE* param = GetUnitYXParamByPoint(uid, j);
|
|
if (param) {
|
|
if (param[0] == 0 || param[0] == 1) {
|
|
m_DiscreteTable.push_back(j);
|
|
} else {
|
|
m_CoilRegTable.push_back(j);
|
|
}
|
|
}
|
|
}
|
|
|
|
int yccount = GetUnitYCCount(uid);
|
|
for (j = 0; j < yccount; j++) {
|
|
BYTE* param = GetUnitYCParamByPoint(uid, j);
|
|
if (param) {
|
|
if (param[0] == 0 || param[0] == 1) {
|
|
m_InputRegTable.push_back(j);
|
|
} else {
|
|
m_HoldingRegTable.push_back(j);
|
|
}
|
|
}
|
|
}
|
|
|
|
if (m_CoilRegTable.size() > 0)
|
|
{
|
|
pItem->m_CoilRegCount = m_CoilRegTable.size();
|
|
pItem->m_pCoilRegTable = new BYTE[pItem->m_CoilRegCount];
|
|
memset(pItem->m_pCoilRegTable, 0, sizeof(BYTE) * pItem->m_CoilRegCount);
|
|
}
|
|
if (m_DiscreteTable.size() > 0)
|
|
{
|
|
pItem->m_DiscreteRegCount = m_DiscreteTable.size();
|
|
pItem->m_pDiscreteRegTable = new BYTE[pItem->m_DiscreteRegCount];
|
|
memset(pItem->m_pDiscreteRegTable, 0, sizeof(BYTE) * pItem->m_DiscreteRegCount);
|
|
}
|
|
if (m_InputRegTable.size() > 0)
|
|
{
|
|
int count = 0;
|
|
if (m_YC_Type == M_ME_NC) count = (m_InputRegTable.size() << 1);
|
|
else count = m_InputRegTable.size();
|
|
pItem->m_InputregCount = count;
|
|
pItem->m_pInputRegTable = new WORD[count];
|
|
memset(pItem->m_pInputRegTable, 0, sizeof(WORD) * count);
|
|
}
|
|
int ymcount = GetUnitYMCount(uid);
|
|
if (m_HoldingRegTable.size() > 0 || ymcount > 0)
|
|
{
|
|
int count = m_HoldingRegTable.size();
|
|
if (m_YC_Type == M_ME_NC) count = (count << 1);
|
|
count += (ymcount << 1);
|
|
pItem->m_HoldingRegCount = count;
|
|
pItem->m_pHoldingRegTable = new WORD[count];
|
|
memset(pItem->m_pHoldingRegTable, 0, sizeof(WORD) * count);
|
|
}
|
|
}
|
|
return TRUE;
|
|
}
|
|
|
|
BOOLEAN CSubModbusTcpProcess::Run(void)
|
|
{
|
|
if (!CNetProcess::Run()) return FALSE;
|
|
|
|
int i, j, uid;
|
|
|
|
FeedDog();
|
|
for (i = 0; i < PROCESS_UNIT_NUM; i++)
|
|
{
|
|
CSubModbusTcpProcessItem *pItem = (CSubModbusTcpProcessItem *)GetItem(i);
|
|
if (NULL == pItem) continue;
|
|
uid = pItem->GetUnitID();
|
|
if (uid < 0) continue;
|
|
//刷新寄存器
|
|
WORD *wd_input = NULL;
|
|
WORD *wd_hold = NULL;
|
|
BYTE *bt_coil = NULL;
|
|
BYTE *bt_discrete = NULL;
|
|
|
|
if (pItem->m_pInputRegTable) {
|
|
wd_input = (WORD *)pItem->m_pInputRegTable;
|
|
}
|
|
if (pItem->m_pHoldingRegTable) {
|
|
wd_hold = (WORD *)pItem->m_pHoldingRegTable;
|
|
}
|
|
for (j = 0; j < m_InputRegTable.size(); j++)
|
|
{
|
|
if (m_YC_Type == M_ME_NC)
|
|
{
|
|
union
|
|
{
|
|
float fv;
|
|
DWORD dv;
|
|
} val;
|
|
|
|
val.fv = GetUnitYCReal(uid, m_InputRegTable[j]);
|
|
*wd_input = ((val.dv >> 16) & 0xffff); wd_input++;
|
|
*wd_input = (val.dv & 0xffff); wd_input++;
|
|
}
|
|
else
|
|
{
|
|
*wd_input = (WORD)GetUnitYC(uid, m_InputRegTable[j]); wd_input++;
|
|
}
|
|
}
|
|
for (j = 0; j < m_HoldingRegTable.size(); j++)
|
|
{
|
|
if (m_YC_Type == M_ME_NC)
|
|
{
|
|
union
|
|
{
|
|
float fv;
|
|
DWORD dv;
|
|
} val;
|
|
|
|
val.fv = GetUnitYCReal(uid, m_HoldingRegTable[j]);
|
|
*wd_hold = ((val.dv >> 16) & 0xffff); wd_hold++;
|
|
*wd_hold = (val.dv & 0xffff); wd_hold++;
|
|
}
|
|
else
|
|
{
|
|
*wd_hold = (WORD)GetUnitYC(uid, m_HoldingRegTable[j]); wd_hold++;
|
|
}
|
|
}
|
|
for (j = 0; j < GetUnitYMCount(uid); j++)
|
|
{
|
|
DWORD val = (DWORD)GetUnitYM(uid, j);
|
|
*wd_hold = ((val >> 16) & 0xffff); wd_hold++;
|
|
*wd_hold = (val & 0xffff); wd_hold++;
|
|
}
|
|
|
|
if (pItem->m_pCoilRegTable) {
|
|
bt_coil = (BYTE *)pItem->m_pCoilRegTable;
|
|
for (j = 0; j < m_CoilRegTable.size(); j++)
|
|
{
|
|
*bt_coil = (BYTE)GetUnitYX(uid, m_CoilRegTable[j]); bt_coil++;
|
|
}
|
|
}
|
|
if (pItem->m_pDiscreteRegTable) {
|
|
bt_discrete = (BYTE *)pItem->m_pDiscreteRegTable;
|
|
for (j = 0; j < m_DiscreteTable.size(); j++)
|
|
{
|
|
*bt_discrete = (BYTE)GetUnitYX(uid, m_DiscreteTable[j]); bt_discrete++;
|
|
}
|
|
}
|
|
}
|
|
return TRUE;
|
|
}
|
|
|
|
BOOLEAN CSubModbusTcpProcess::OnTimer(void)
|
|
{
|
|
if (!CNetProcess::OnTimer()) return FALSE;
|
|
|
|
int i, uid;
|
|
for (i = 0; i < PROCESS_UNIT_NUM; i++)
|
|
{
|
|
CSubModbusTcpProcessItem *pItem = (CSubModbusTcpProcessItem *)GetItem(i);
|
|
if (NULL == pItem) continue;
|
|
uid = pItem->GetUnitID();
|
|
if (uid < 0) continue;
|
|
|
|
int order;
|
|
BYTE action, result;
|
|
DWORD value;
|
|
BYTE by_value;
|
|
if (GetUnitYT(uid, order, value, action, result))
|
|
{
|
|
vLog(LOG_DEBUG, "Unit(%d) set point(%d) %d state is %s result is %s\n", uid, order, value, val_to_str(action, yt_state, "STATE=%d"), val_to_str(result, yt_result, "RESULT=%d"));
|
|
if (YTS_SELED == action && YTR_FAIL == result)
|
|
{
|
|
action = YTS_ABRED;
|
|
}
|
|
else if (YTS_SELREQ == action && YTR_OVER == result)
|
|
{
|
|
action = YTS_ABRED;
|
|
}
|
|
else if (YTS_SELING == action && YTR_OVER == result)
|
|
{
|
|
action = YTS_ABRED;
|
|
}
|
|
|
|
if (YTS_SELED == action)
|
|
{
|
|
SetUnitYT(uid, order, value, YTS_EXEREQ, YTR_IDLE);
|
|
vLog(LOG_DEBUG, "Unit(%d) set point(%d) %d state is YTS_EXEREQ result is YTR_IDLE.\n", uid, order, value);
|
|
}
|
|
else if (YTS_ABRED == action)
|
|
{
|
|
SetUnitYT(uid, order, value, YTS_ABRREQ, YTR_IDLE);
|
|
vLog(LOG_DEBUG, "Unit(%d) set point(%d) %d state is YTS_ABRREQ result is YTR_IDLE.\n", uid, order, value);
|
|
}
|
|
}
|
|
if (GetUnitYK(uid, order, by_value, action, result))
|
|
{
|
|
vLog(LOG_WARN, "Unit(%d) yk(%d) %s state is %s result is %s\n", uid, order, (by_value ? "CLOSE" : "TRIP"), val_to_str(action, yk_state, "STATE=%d"), val_to_str(result, yk_result, "RESULT=%d"));
|
|
if (YKS_SELED == action && YKR_FAIL == result)
|
|
{
|
|
action = YKS_ABRED;
|
|
}
|
|
else if (YKS_SELREQ == action && YKR_OVER == result)
|
|
{
|
|
action = YKS_ABRED;
|
|
}
|
|
else if (YKS_SELING == action && YKR_OVER == result)
|
|
{
|
|
action = YKS_ABRED;
|
|
}
|
|
if (YKS_ABRED == action)
|
|
{
|
|
SetUnitYK(uid, order, by_value, YKS_ABRREQ, YKR_IDLE);
|
|
vLog(LOG_WARN, "Unit(%d) yk(%d) %s state is YKS_ABRREQ result is YKR_IDLE.\n", uid, order, (by_value ? "CLOSE" : "TRIP"));
|
|
}
|
|
else if (YKS_SELED == action)
|
|
{
|
|
SetUnitYK(uid, order, by_value, YKS_EXEREQ, YKR_IDLE);
|
|
vLog(LOG_WARN, "Unit(%d) yk(%d) %s state is YKS_EXEREQ result is YKR_IDLE.\n", uid, order, (by_value ? "CLOSE" : "TRIP"));
|
|
}
|
|
}
|
|
}
|
|
return TRUE;
|
|
}
|
|
|
|
int CSubModbusTcpProcess::OnPackageReceived(BYTE* pBuf, int count, int ord)
|
|
{
|
|
int uid;
|
|
CSubModbusTcpProcessItem* pItem;
|
|
pItem = (CSubModbusTcpProcessItem *)GetItem(ord);
|
|
if (NULL == pItem) return -1;
|
|
uid = pItem->GetUnitID();
|
|
if (uid < 0 || uid >= UNIT_NUM) return -1;
|
|
//头尾判断。
|
|
WORD usLength;
|
|
WORD usTID;
|
|
WORD usPID;
|
|
|
|
if (count < MB_TCP_FUNC)
|
|
{ //数据长度不足
|
|
return 0;
|
|
}
|
|
usLength = pBuf[MB_TCP_LEN] << 8U;
|
|
usLength |= pBuf[MB_TCP_LEN + 1];
|
|
if (count < (MB_TCP_UID + usLength))
|
|
{ //数据接收未完
|
|
vLog(LOG_WARN, "数据没有接收完成.\n");
|
|
return 0;
|
|
}
|
|
usPID = pBuf[MB_TCP_PID] << 8U;
|
|
usPID |= pBuf[MB_TCP_PID + 1];
|
|
if (usPID != MB_TCP_PROTOCOL_ID)
|
|
{ //未知协议
|
|
vLog(LOG_ERROR, "未知协议\n");
|
|
return -1;
|
|
}
|
|
usTID = pBuf[MB_TCP_TID] << 8U;
|
|
usTID |= pBuf[MB_TCP_TID + 1];
|
|
usLength = count - MB_TCP_FUNC;
|
|
|
|
BYTE *ppucFrame = &pBuf[MB_TCP_FUNC];
|
|
BYTE ucRcvAddress = pBuf[MB_TCP_UID];
|
|
if (ucRcvAddress != pItem->ied_addr && ucRcvAddress != MB_ADDRESS_BROADCAST)
|
|
{
|
|
vLog(LOG_DEBUG, "数据不是发送给我的.\n");
|
|
return count;
|
|
}
|
|
BYTE ucFunctionCode = ppucFrame[MB_PDU_FUNC_OFF];
|
|
eMBException eException;
|
|
eMBErrorCode eStatus = MB_ENOERR;
|
|
|
|
DisplayRxData(pBuf, (MB_TCP_UID + usLength + 1), TRUE, uid);
|
|
UnitFeedDog(uid);
|
|
eException = MB_EX_ILLEGAL_FUNCTION;
|
|
switch (ucFunctionCode)
|
|
{
|
|
case MB_FUNC_READ_INPUT_REGISTER:
|
|
eException = eMBFuncReadInputRegister(pItem, ppucFrame, &usLength);
|
|
break;
|
|
case MB_FUNC_READ_HOLDING_REGISTER:
|
|
eException = eMBFuncReadHoldingRegister(pItem, ppucFrame, &usLength);
|
|
break;
|
|
case MB_FUNC_WRITE_MULTIPLE_REGISTERS:
|
|
eException = eMBFuncWriteMultipleHoldingRegister(pItem, ppucFrame, &usLength);
|
|
break;
|
|
case MB_FUNC_WRITE_REGISTER:
|
|
eException = eMBFuncWriteHoldingRegister(pItem, ppucFrame, &usLength);
|
|
break;
|
|
case MB_FUNC_READ_COILS:
|
|
eException = eMBFuncReadCoils(pItem, ppucFrame, &usLength);
|
|
break;
|
|
case MB_FUNC_WRITE_SINGLE_COIL:
|
|
eException = eMBFuncWriteCoil(pItem, ppucFrame, &usLength);
|
|
break;
|
|
case MB_FUNC_WRITE_MULTIPLE_COILS:
|
|
eException = eMBFuncWriteMultipleCoils(pItem, ppucFrame, &usLength);
|
|
break;
|
|
case MB_FUNC_READ_DISCRETE_INPUTS:
|
|
eException = eMBFuncReadDiscreteInputs(pItem, ppucFrame, &usLength);
|
|
break;
|
|
default:
|
|
break;
|
|
|
|
}
|
|
if (ucRcvAddress != MB_ADDRESS_BROADCAST)
|
|
{
|
|
if (eException != MB_EX_NONE)
|
|
{
|
|
usLength = 0;
|
|
ppucFrame[usLength++] = (BYTE)(ucFunctionCode | MB_FUNC_ERROR);
|
|
ppucFrame[usLength++] = eException;
|
|
}
|
|
|
|
eStatus = eMBTCPSend(ucRcvAddress, ppucFrame, usLength);
|
|
if (eStatus != MB_ENOERR)
|
|
{
|
|
vLog(LOG_ERROR, "modbus tcp send error(%d)\n", eStatus);
|
|
return -1;
|
|
}
|
|
}
|
|
return (count);
|
|
}
|
|
|
|
eMBErrorCode CSubModbusTcpProcess::eMBTCPSend(BYTE ucSlaveAddress, const BYTE *pucFrame, WORD usLength)
|
|
{
|
|
eMBErrorCode eStatus = MB_ENOERR;
|
|
BYTE *pucMBTCPFrame = (BYTE *) pucFrame - MB_TCP_FUNC;
|
|
WORD usTCPLength = usLength + MB_TCP_FUNC;
|
|
|
|
pucMBTCPFrame[MB_TCP_LEN] = (usLength + 1) >> 8U;
|
|
pucMBTCPFrame[MB_TCP_LEN + 1] = (usLength + 1) & 0xFF;
|
|
if (WriteData(pucMBTCPFrame, usTCPLength, GetCurOrder()))
|
|
{
|
|
DisplayTxData(pucMBTCPFrame, usTCPLength, TRUE);
|
|
}
|
|
else
|
|
{
|
|
eStatus = MB_EIO;
|
|
}
|
|
return eStatus;
|
|
}
|
|
|
|
eMBException CSubModbusTcpProcess::prveMBError2Exception(eMBErrorCode eErrorCode)
|
|
{
|
|
eMBException eStatus;
|
|
|
|
switch (eErrorCode)
|
|
{
|
|
case MB_ENOERR:
|
|
eStatus = MB_EX_NONE;
|
|
break;
|
|
case MB_ENOREG:
|
|
eStatus = MB_EX_ILLEGAL_DATA_ADDRESS;
|
|
break;
|
|
case MB_ETIMEDOUT:
|
|
eStatus = MB_EX_SLAVE_BUSY;
|
|
break;
|
|
default:
|
|
eStatus = MB_EX_SLAVE_DEVICE_FAILURE;
|
|
break;
|
|
}
|
|
return eStatus;
|
|
}
|
|
|
|
eMBException CSubModbusTcpProcess::eMBFuncReadInputRegister(CSubModbusTcpProcessItem *pItem, BYTE* pucFrame, WORD* usLen)
|
|
{
|
|
WORD usRegAddress;
|
|
WORD usRegCount;
|
|
BYTE *pucFrameCur;
|
|
|
|
eMBException eStatus = MB_EX_NONE;
|
|
eMBErrorCode eRegStatus;
|
|
|
|
if (*usLen == (MB_PDU_FUNC_READ_SIZE + MB_PDU_SIZE_MIN))
|
|
{
|
|
usRegAddress = (WORD)(pucFrame[MB_PDU_FUNC_READ_ADDR_OFF] << 8);
|
|
usRegAddress |= (WORD)(pucFrame[MB_PDU_FUNC_READ_ADDR_OFF + 1]);
|
|
|
|
usRegCount = (WORD)(pucFrame[MB_PDU_FUNC_READ_REGCNT_OFF] << 8);
|
|
usRegCount |= (WORD)(pucFrame[MB_PDU_FUNC_READ_REGCNT_OFF + 1]);
|
|
|
|
if ((usRegCount >= 1) && (usRegCount < MB_PDU_FUNC_READ_REGCNT_MAX))
|
|
{
|
|
pucFrameCur = &pucFrame[MB_PDU_FUNC_OFF];
|
|
*usLen = MB_PDU_FUNC_OFF;
|
|
|
|
*pucFrameCur++ = MB_FUNC_READ_INPUT_REGISTER;
|
|
*usLen += 1;
|
|
|
|
*pucFrameCur++ = (BYTE)(usRegCount << 1);
|
|
*usLen += 1;
|
|
eRegStatus = eMBRegInputCB(pItem, pucFrameCur, usRegAddress, usRegCount);
|
|
|
|
if (eRegStatus != MB_ENOERR)
|
|
{
|
|
eStatus = prveMBError2Exception(eRegStatus);
|
|
}
|
|
else
|
|
{
|
|
*usLen += usRegCount << 1;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
eStatus = MB_EX_ILLEGAL_DATA_VALUE;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
eStatus = MB_EX_ILLEGAL_DATA_VALUE;
|
|
}
|
|
return eStatus;
|
|
}
|
|
|
|
eMBException CSubModbusTcpProcess::eMBFuncWriteHoldingRegister(CSubModbusTcpProcessItem *pItem, BYTE* pucFrame, WORD* usLen)
|
|
{
|
|
WORD usRegAddress;
|
|
eMBException eStatus = MB_EX_NONE;
|
|
eMBErrorCode eRegStatus;
|
|
|
|
if (*usLen == (MB_PDU_FUNC_WRITE_SIZE + MB_PDU_SIZE_MIN))
|
|
{
|
|
usRegAddress = (WORD)(pucFrame[MB_PDU_FUNC_WRITE_ADDR_OFF] << 8);
|
|
usRegAddress |= (WORD)(pucFrame[MB_PDU_FUNC_WRITE_ADDR_OFF + 1]);
|
|
|
|
eRegStatus = eMBRegHoldingCB(pItem, &pucFrame[MB_PDU_FUNC_WRITE_VALUE_OFF], usRegAddress, 1, MB_REG_WRITE);
|
|
|
|
if (eRegStatus != MB_ENOERR)
|
|
{
|
|
eStatus = prveMBError2Exception(eRegStatus);
|
|
}
|
|
}
|
|
else
|
|
{
|
|
eStatus = MB_EX_ILLEGAL_DATA_VALUE;
|
|
}
|
|
return eStatus;
|
|
}
|
|
|
|
eMBException CSubModbusTcpProcess::eMBFuncWriteMultipleHoldingRegister(CSubModbusTcpProcessItem *pItem, BYTE* pucFrame, WORD* usLen)
|
|
{
|
|
WORD usRegAddress;
|
|
WORD usRegCount;
|
|
BYTE ucRegByteCount;
|
|
|
|
eMBException eStatus = MB_EX_NONE;
|
|
eMBErrorCode eRegStatus;
|
|
|
|
if (*usLen >= (MB_PDU_FUNC_WRITE_MUL_SIZE_MIN + MB_PDU_SIZE_MIN))
|
|
{
|
|
usRegAddress = (WORD)(pucFrame[MB_PDU_FUNC_WRITE_MUL_ADDR_OFF] << 8);
|
|
usRegAddress |= (WORD)(pucFrame[MB_PDU_FUNC_WRITE_MUL_ADDR_OFF + 1]);
|
|
|
|
usRegCount = (WORD)(pucFrame[MB_PDU_FUNC_WRITE_MUL_REGCNT_OFF] << 8);
|
|
usRegCount |= (WORD)(pucFrame[MB_PDU_FUNC_WRITE_MUL_REGCNT_OFF + 1]);
|
|
|
|
ucRegByteCount = pucFrame[MB_PDU_FUNC_WRITE_MUL_BYTECNT_OFF];
|
|
|
|
if ((usRegCount >= 1) && (usRegCount <= MB_PDU_FUNC_WRITE_MUL_REGCNT_MAX) && (ucRegByteCount == (BYTE) (usRegCount << 1)))
|
|
{
|
|
eRegStatus = eMBRegHoldingCB(pItem, &pucFrame[MB_PDU_FUNC_WRITE_MUL_VALUES_OFF], usRegAddress, usRegCount, MB_REG_WRITE);
|
|
|
|
if (eRegStatus != MB_ENOERR)
|
|
{
|
|
eStatus = prveMBError2Exception(eRegStatus);
|
|
}
|
|
else
|
|
{
|
|
*usLen = MB_PDU_FUNC_WRITE_MUL_BYTECNT_OFF;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
eStatus = MB_EX_ILLEGAL_DATA_VALUE;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
eStatus = MB_EX_ILLEGAL_DATA_VALUE;
|
|
}
|
|
return eStatus;
|
|
}
|
|
|
|
eMBException CSubModbusTcpProcess::eMBFuncReadHoldingRegister(CSubModbusTcpProcessItem *pItem, BYTE* pucFrame, WORD* usLen)
|
|
{
|
|
WORD usRegAddress;
|
|
WORD usRegCount;
|
|
BYTE *pucFrameCur;
|
|
|
|
eMBException eStatus = MB_EX_NONE;
|
|
eMBErrorCode eRegStatus;
|
|
|
|
if (*usLen == (MB_PDU_FUNC_READ_SIZE + MB_PDU_SIZE_MIN))
|
|
{
|
|
usRegAddress = (WORD)(pucFrame[MB_PDU_FUNC_READ_ADDR_OFF] << 8);
|
|
usRegAddress |= (WORD)(pucFrame[MB_PDU_FUNC_READ_ADDR_OFF + 1]);
|
|
|
|
usRegCount = (WORD)(pucFrame[MB_PDU_FUNC_READ_REGCNT_OFF] << 8);
|
|
usRegCount = (WORD)(pucFrame[MB_PDU_FUNC_READ_REGCNT_OFF + 1]);
|
|
|
|
if ((usRegCount >= 1) && (usRegCount <= MB_PDU_FUNC_READ_REGCNT_MAX))
|
|
{
|
|
pucFrameCur = &pucFrame[MB_PDU_FUNC_OFF];
|
|
*usLen = MB_PDU_FUNC_OFF;
|
|
|
|
*pucFrameCur++ = MB_FUNC_READ_HOLDING_REGISTER;
|
|
*usLen += 1;
|
|
|
|
*pucFrameCur++ = (BYTE) (usRegCount << 1);
|
|
*usLen += 1;
|
|
|
|
eRegStatus = eMBRegHoldingCB(pItem, pucFrameCur, usRegAddress, usRegCount, MB_REG_READ);
|
|
|
|
if (eRegStatus != MB_ENOERR)
|
|
{
|
|
eStatus = prveMBError2Exception(eRegStatus);
|
|
}
|
|
else
|
|
{
|
|
*usLen += usRegCount << 1;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
eStatus = MB_EX_ILLEGAL_DATA_VALUE;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
eStatus = MB_EX_ILLEGAL_DATA_VALUE;
|
|
}
|
|
return eStatus;
|
|
}
|
|
|
|
eMBException CSubModbusTcpProcess::eMBFuncReadCoils(CSubModbusTcpProcessItem *pItem, BYTE* pucFrame, WORD* usLen)
|
|
{
|
|
WORD usRegAddress;
|
|
WORD usCoilCount;
|
|
BYTE ucNBytes;
|
|
BYTE *pucFrameCur;
|
|
|
|
eMBException eStatus = MB_EX_NONE;
|
|
eMBErrorCode eRegStatus;
|
|
|
|
if (*usLen == (MB_PDU_FUNC_READ_SIZE + MB_PDU_SIZE_MIN))
|
|
{
|
|
usRegAddress = (WORD)(pucFrame[MB_PDU_FUNC_READ_ADDR_OFF] << 8);
|
|
usRegAddress |= (WORD)(pucFrame[MB_PDU_FUNC_READ_ADDR_OFF + 1]);
|
|
|
|
usCoilCount = (WORD)(pucFrame[MB_PDU_FUNC_READ_COILCNT_OFF] << 8);
|
|
usCoilCount |= (WORD)(pucFrame[MB_PDU_FUNC_READ_COILCNT_OFF + 1]);
|
|
|
|
if ((usCoilCount >= 1) && (usCoilCount < MB_PDU_FUNC_READ_COILCNT_MAX))
|
|
{
|
|
pucFrameCur = &pucFrame[MB_PDU_FUNC_OFF];
|
|
*usLen = MB_PDU_FUNC_OFF;
|
|
|
|
*pucFrameCur++ = MB_FUNC_READ_COILS;
|
|
*usLen += 1;
|
|
|
|
if ((usCoilCount & 0x0007) != 0)
|
|
{
|
|
ucNBytes = (BYTE)((usCoilCount >> 3) + 1);
|
|
}
|
|
else
|
|
{
|
|
ucNBytes = (BYTE)(usCoilCount >> 3);
|
|
}
|
|
*pucFrameCur++ = ucNBytes;
|
|
*usLen += 1;
|
|
|
|
eRegStatus = eMBRegCoilsCB(pItem, pucFrameCur, usRegAddress, usCoilCount, MB_REG_READ);
|
|
|
|
if (eRegStatus != MB_ENOERR)
|
|
{
|
|
eStatus = prveMBError2Exception(eRegStatus);
|
|
}
|
|
else
|
|
{
|
|
|
|
*usLen += ucNBytes;;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
eStatus = MB_EX_ILLEGAL_DATA_VALUE;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
|
|
eStatus = MB_EX_ILLEGAL_DATA_VALUE;
|
|
}
|
|
return eStatus;
|
|
}
|
|
|
|
eMBException CSubModbusTcpProcess::eMBFuncWriteCoil(CSubModbusTcpProcessItem *pItem, BYTE* pucFrame, WORD* usLen)
|
|
{
|
|
WORD usRegAddress;
|
|
BYTE ucBuf[2];
|
|
|
|
eMBException eStatus = MB_EX_NONE;
|
|
eMBErrorCode eRegStatus;
|
|
|
|
if (*usLen == (MB_PDU_FUNC_WRITE_SIZE + MB_PDU_SIZE_MIN))
|
|
{
|
|
usRegAddress = (WORD)(pucFrame[MB_PDU_FUNC_WRITE_ADDR_OFF] << 8);
|
|
usRegAddress |= (WORD)(pucFrame[MB_PDU_FUNC_WRITE_ADDR_OFF + 1]);
|
|
|
|
if ((pucFrame[MB_PDU_FUNC_WRITE_VALUE_OFF + 1] == 0x00) && ((pucFrame[MB_PDU_FUNC_WRITE_VALUE_OFF] == 0xFF) || (pucFrame[MB_PDU_FUNC_WRITE_VALUE_OFF] == 0x00)))
|
|
{
|
|
ucBuf[1] = 0;
|
|
if (pucFrame[MB_PDU_FUNC_WRITE_VALUE_OFF] == 0xFF)
|
|
{
|
|
ucBuf[0] = 1;
|
|
}
|
|
else
|
|
{
|
|
ucBuf[0] = 0;
|
|
}
|
|
eRegStatus = eMBRegCoilsCB(pItem, &ucBuf[0], usRegAddress, 1, MB_REG_WRITE);
|
|
|
|
if (eRegStatus != MB_ENOERR)
|
|
{
|
|
eStatus = prveMBError2Exception(eRegStatus);
|
|
}
|
|
}
|
|
else
|
|
{
|
|
eStatus = MB_EX_ILLEGAL_DATA_VALUE;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
|
|
eStatus = MB_EX_ILLEGAL_DATA_VALUE;
|
|
}
|
|
return eStatus;
|
|
}
|
|
|
|
eMBException CSubModbusTcpProcess::eMBFuncWriteMultipleCoils(CSubModbusTcpProcessItem *pItem, BYTE* pucFrame, WORD* usLen)
|
|
{
|
|
WORD usRegAddress;
|
|
WORD usCoilCnt;
|
|
BYTE ucByteCount;
|
|
BYTE ucByteCountVerify;
|
|
|
|
eMBException eStatus = MB_EX_NONE;
|
|
eMBErrorCode eRegStatus;
|
|
|
|
if (*usLen > (MB_PDU_FUNC_WRITE_SIZE + MB_PDU_SIZE_MIN))
|
|
{
|
|
usRegAddress = (WORD)(pucFrame[MB_PDU_FUNC_WRITE_MUL_ADDR_OFF] << 8);
|
|
usRegAddress |= (WORD)(pucFrame[MB_PDU_FUNC_WRITE_MUL_ADDR_OFF + 1]);
|
|
|
|
usCoilCnt = (WORD)(pucFrame[MB_PDU_FUNC_WRITE_MUL_COILCNT_OFF] << 8);
|
|
usCoilCnt |= (WORD)(pucFrame[MB_PDU_FUNC_WRITE_MUL_COILCNT_OFF + 1]);
|
|
|
|
ucByteCount = pucFrame[MB_PDU_FUNC_WRITE_MUL_BYTECNT_OFF];
|
|
|
|
if ((usCoilCnt & 0x0007) != 0)
|
|
{
|
|
ucByteCountVerify = (BYTE)(usCoilCnt / 8 + 1);
|
|
}
|
|
else
|
|
{
|
|
ucByteCountVerify = (BYTE)(usCoilCnt / 8);
|
|
}
|
|
|
|
if ((usCoilCnt >= 1) &&
|
|
(usCoilCnt <= MB_PDU_FUNC_WRITE_MUL_COILCNT_MAX) &&
|
|
(ucByteCountVerify == ucByteCount))
|
|
{
|
|
eRegStatus = eMBRegCoilsCB(pItem, &pucFrame[MB_PDU_FUNC_WRITE_MUL_VALUES_OFF], usRegAddress, usCoilCnt, MB_REG_WRITE);
|
|
|
|
if (eRegStatus != MB_ENOERR)
|
|
{
|
|
eStatus = prveMBError2Exception(eRegStatus);
|
|
}
|
|
else
|
|
{
|
|
*usLen = MB_PDU_FUNC_WRITE_MUL_BYTECNT_OFF;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
eStatus = MB_EX_ILLEGAL_DATA_VALUE;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
eStatus = MB_EX_ILLEGAL_DATA_VALUE;
|
|
}
|
|
return eStatus;
|
|
}
|
|
|
|
eMBException CSubModbusTcpProcess::eMBFuncReadDiscreteInputs(CSubModbusTcpProcessItem *pItem, BYTE* pucFrame, WORD* usLen)
|
|
{
|
|
WORD usRegAddress;
|
|
WORD usDiscreteCnt;
|
|
BYTE ucNBytes;
|
|
BYTE *pucFrameCur;
|
|
|
|
eMBException eStatus = MB_EX_NONE;
|
|
eMBErrorCode eRegStatus;
|
|
|
|
if (*usLen == (MB_PDU_FUNC_READ_SIZE + MB_PDU_SIZE_MIN))
|
|
{
|
|
usRegAddress = (WORD)(pucFrame[MB_PDU_FUNC_READ_ADDR_OFF] << 8);
|
|
usRegAddress |= (WORD)(pucFrame[MB_PDU_FUNC_READ_ADDR_OFF + 1]);
|
|
|
|
usDiscreteCnt = (WORD)(pucFrame[MB_PDU_FUNC_READ_DISCCNT_OFF] << 8);
|
|
usDiscreteCnt |= (WORD)(pucFrame[MB_PDU_FUNC_READ_DISCCNT_OFF + 1]);
|
|
|
|
if ((usDiscreteCnt >= 1) && (usDiscreteCnt < MB_PDU_FUNC_READ_DISCCNT_MAX))
|
|
{
|
|
pucFrameCur = &pucFrame[MB_PDU_FUNC_OFF];
|
|
*usLen = MB_PDU_FUNC_OFF;
|
|
|
|
*pucFrameCur++ = MB_FUNC_READ_DISCRETE_INPUTS;
|
|
*usLen += 1;
|
|
|
|
if ((usDiscreteCnt & 0x0007) != 0)
|
|
{
|
|
ucNBytes = (BYTE) ((usDiscreteCnt >> 3) + 1);
|
|
}
|
|
else
|
|
{
|
|
ucNBytes = (BYTE) (usDiscreteCnt >> 3);
|
|
}
|
|
*pucFrameCur++ = ucNBytes;
|
|
*usLen += 1;
|
|
|
|
eRegStatus = eMBRegDiscreteCB(pItem, pucFrameCur, usRegAddress, usDiscreteCnt);
|
|
|
|
if (eRegStatus != MB_ENOERR)
|
|
{
|
|
eStatus = prveMBError2Exception(eRegStatus);
|
|
}
|
|
else
|
|
{
|
|
*usLen += ucNBytes;;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
eStatus = MB_EX_ILLEGAL_DATA_VALUE;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
eStatus = MB_EX_ILLEGAL_DATA_VALUE;
|
|
}
|
|
return eStatus;
|
|
}
|
|
|
|
eMBErrorCode CSubModbusTcpProcess::eMBRegInputCB(CSubModbusTcpProcessItem *pItem, BYTE* pucRegBuffer, WORD usAddress, WORD usNRegs)
|
|
{
|
|
eMBErrorCode eStatus = MB_ENOERR;
|
|
int iRegIndex;
|
|
|
|
if ((usAddress >= 0) && (usAddress + usNRegs <= 0 + pItem->m_InputregCount))
|
|
{
|
|
iRegIndex = (int)(usAddress - 0);
|
|
while (usNRegs > 0)
|
|
{
|
|
*pucRegBuffer++ = (BYTE)(pItem->m_pInputRegTable[iRegIndex] >> 8);
|
|
*pucRegBuffer++ = (BYTE)(pItem->m_pInputRegTable[iRegIndex] & 0xFF);
|
|
iRegIndex++;
|
|
usNRegs--;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
eStatus = MB_ENOREG;
|
|
}
|
|
|
|
return eStatus;
|
|
}
|
|
|
|
eMBErrorCode CSubModbusTcpProcess::eMBRegHoldingCB(CSubModbusTcpProcessItem *pItem, BYTE* pucRegBuffer, WORD usAddress, WORD usNRegs, eMBRegisterMode eMode)
|
|
{
|
|
eMBErrorCode eStatus = MB_ENOERR;
|
|
WORD usRegHoldingValue;
|
|
int iRegIndex;
|
|
int uid;
|
|
|
|
uid = pItem->GetUnitID();
|
|
|
|
if ((usAddress >= 0) && (usAddress + usNRegs <= 0 + pItem->m_HoldingRegCount))
|
|
{
|
|
iRegIndex = (int)(usAddress - 0);
|
|
switch (eMode)
|
|
{
|
|
case MB_REG_READ:
|
|
while (usNRegs > 0)
|
|
{
|
|
*pucRegBuffer++ = (BYTE)(pItem->m_pHoldingRegTable[iRegIndex] >> 8);
|
|
*pucRegBuffer++ = (BYTE)(pItem->m_pHoldingRegTable[iRegIndex] & 0xFF);
|
|
iRegIndex++;
|
|
usNRegs--;
|
|
}
|
|
break;
|
|
case MB_REG_WRITE:
|
|
while (usNRegs > 0)
|
|
{
|
|
usRegHoldingValue = *pucRegBuffer++ << 8;
|
|
usRegHoldingValue |= *pucRegBuffer++;
|
|
//写寄存器
|
|
SetUnitYT(uid, iRegIndex, usRegHoldingValue, YTS_EXEREQ, YTR_IDLE);
|
|
vLog(LOG_DEBUG, "Unit(%d) set point(%d) %d state is YTS_EXEREQ result is YTR_IDLE.\n", uid, iRegIndex, usRegHoldingValue);
|
|
iRegIndex++;
|
|
usNRegs--;
|
|
}
|
|
}
|
|
}
|
|
else
|
|
{
|
|
eStatus = MB_ENOREG;
|
|
}
|
|
return eStatus;
|
|
}
|
|
|
|
|
|
eMBErrorCode CSubModbusTcpProcess::eMBRegCoilsCB(CSubModbusTcpProcessItem *pItem, BYTE* pucRegBuffer, WORD usAddress, WORD usNCoils, eMBRegisterMode eMode)
|
|
{
|
|
eMBErrorCode eStatus = MB_ENOERR;
|
|
WORD usCoilGroups = ((usNCoils - 1) / 8 + 1);
|
|
BYTE ucStatus = 0;
|
|
BYTE usRegCoilsValue;
|
|
BYTE ucBits = 0;
|
|
BYTE ucDisp = 0;
|
|
int iRegIndex = usAddress - 0;
|
|
int uid;
|
|
|
|
uid = pItem->GetUnitID();
|
|
if ((usAddress >= 0) && (usAddress + usNCoils <= 0 + pItem->m_CoilRegCount))
|
|
{
|
|
iRegIndex = (int)(usAddress - 0);
|
|
switch (eMode)
|
|
{ /* Pass current register values to the protocol stack. */
|
|
case MB_REG_READ:
|
|
while (usCoilGroups--)
|
|
{
|
|
ucDisp = 0;
|
|
ucBits = 8;
|
|
ucStatus = 0;
|
|
while((usNCoils--) != 0 && (ucBits--) != 0)
|
|
{
|
|
ucStatus |= (pItem->m_pCoilRegTable[iRegIndex++] << (ucDisp++));
|
|
}
|
|
*pucRegBuffer++ = ucStatus;
|
|
}
|
|
break;
|
|
case MB_REG_WRITE:
|
|
while (usCoilGroups--)
|
|
{
|
|
ucStatus = *pucRegBuffer++;
|
|
ucBits = 8;
|
|
while ((usNCoils--) != 0 && (ucBits--) != 0)
|
|
{
|
|
usRegCoilsValue = ucStatus & 0X01;
|
|
//写线圈
|
|
SetUnitYK(uid, iRegIndex, usRegCoilsValue, YKS_SELREQ, YKR_IDLE);
|
|
vLog(LOG_WARN, "Unit(%d) yk(%d) %s state is YKS_EXEREQ result is YKR_IDLE.\n", uid, iRegIndex, (usRegCoilsValue ? "CLOSE" : "TRIP"));
|
|
iRegIndex++;
|
|
ucStatus >>= 1;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
else
|
|
{
|
|
eStatus = MB_ENOREG;
|
|
}
|
|
return eStatus;
|
|
}
|
|
|
|
eMBErrorCode CSubModbusTcpProcess::eMBRegDiscreteCB(CSubModbusTcpProcessItem *pItem, BYTE* pucRegBuffer, WORD usAddress, WORD usNDiscrete)
|
|
{
|
|
eMBErrorCode eStatus = MB_ENOERR;
|
|
WORD usDiscreteGroups = ((usNDiscrete - 1) / 8 + 1);
|
|
BYTE ucStatus = 0;
|
|
BYTE ucBits = 0;
|
|
BYTE ucDisp = 0;
|
|
int iRegIndex = usAddress - 0;
|
|
|
|
if ((usAddress >= 0) && (usAddress + usNDiscrete <= 0 + pItem->m_DiscreteRegCount))
|
|
{
|
|
iRegIndex = (int)(usAddress - 0);
|
|
while (usDiscreteGroups--)
|
|
{
|
|
ucDisp = 0;
|
|
ucBits = 8;
|
|
ucStatus = 0;
|
|
while((usNDiscrete--) != 0 && (ucBits--) != 0)
|
|
{
|
|
ucStatus |= (pItem->m_pDiscreteRegTable[iRegIndex++] << (ucDisp++));
|
|
}
|
|
*pucRegBuffer++ = ucStatus;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
eStatus = MB_ENOREG;
|
|
}
|
|
return eStatus;
|
|
}
|