#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 = 0;
    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();
#if 1
        //获取遥信数量
        int yxcount = GetUnitYXCount(uid);
        for (j = 0; j < yxcount; j++) {
            m_DiscreteTable.push_back(j);
        }

        int ykcount = GetUnitYKCount(uid);
        for (j = 0; j < ykcount; j++) {
            m_CoilRegTable.push_back(j);
        }
        
        int yccount = GetUnitYCCount(uid);
        for (j = 0; j < yccount; j++) {
            m_InputRegTable.push_back(j);
        }
        int ymcount = GetUnitYMCount(uid);
        for (j = 0; j < ymcount; j++) {
            m_InputRegTable.push_back(j);
        }

        int ytcount = GetUnitYTCount(uid);
        for (j = 0; j < ytcount; j++) {
            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);
            pItem->m_ykcount = ykcount;
        }
        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 = yccount + (ymcount << 1);
            }
            pItem->m_InputregCount = count;
            pItem->m_pInputRegTable = new WORD[count];
            memset(pItem->m_pInputRegTable, 0, sizeof(WORD) * count);
            pItem->m_yccount = yccount;
            pItem->m_ymcount = ymcount;
        }
        if (m_HoldingRegTable.size() > 0)
        {
            int count = m_HoldingRegTable.size();
            pItem->m_HoldingRegCount = count;
            pItem->m_pHoldingRegTable = new WORD[count];
            memset(pItem->m_pHoldingRegTable, 0, sizeof(WORD) * count);
            pItem->m_ytcount = ytcount;
        }
#else
        pItem->m_yxcount = GetUnitYXCount(uid);
        pItem->m_yccount = GetUnitYCCount(uid);
        pItem->m_ymcount = GetUnitYMCount(uid);
        pItem->m_ykcount = GetUnitYKCount(uid);
        pItem->m_ytcount = GetUnitYTCount(uid);
        
        pItem->m_CoilRegCount = UNIT_YK_MAX;
        pItem->m_pCoilRegTable = new BYTE[pItem->m_CoilRegCount];
        memset(pItem->m_pCoilRegTable, 0, sizeof(BYTE) * pItem->m_CoilRegCount);

        pItem->m_DiscreteRegCount = UNIT_YX_MAX;
        pItem->m_pDiscreteRegTable = new BYTE[pItem->m_DiscreteRegCount];
        memset(pItem->m_pDiscreteRegTable, 0, sizeof(BYTE) * pItem->m_DiscreteRegCount);

        pItem->m_HoldingRegCount = UNIT_YT_MAX;
        pItem->m_pHoldingRegTable = new WORD[pItem->m_HoldingRegCount];
        memset(pItem->m_pHoldingRegTable, 0, sizeof(WORD) * pItem->m_HoldingRegCount);

        pItem->m_InputregCount = 0x5700;
        pItem->m_pHoldingRegTable = new WORD[pItem->m_InputregCount];
        memset(pItem->m_pHoldingRegTable, 0, sizeof(WORD) * pItem->m_InputregCount);
#endif
    }
    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;
        BYTE *bt_discrete = NULL;

        if (pItem->m_pInputRegTable)
        {
            wd_input = (WORD *)pItem->m_pInputRegTable;
            for (j = 0; j < pItem->m_yccount; j++)
            {
                if (m_YC_Type == M_ME_NC)
                {
                    union
                    {
                        float fv;
                        DWORD dv;
                    } val;
                    val.fv = GetUnitYCReal(uid, j);
                    *wd_input =  ((val.dv >> 16) & 0xffff); wd_input++;
                    *wd_input =  (val.dv & 0xffff); wd_input++;
                }
                else
                {
                    *wd_input = (WORD)GetUnitYC(uid, j); wd_input++;
                }
            }
            for (j = 0; j < pItem->m_ymcount; j++)
            {
                if (m_YC_Type == M_ME_NC)
                {
                    union
                    {
                        float fv;
                        DWORD dv;
                    } val;
                    val.fv = GetUnitYMReal(uid, j);
                    *wd_input =  ((val.dv >> 16) & 0xffff); wd_input++;
                    *wd_input =  (val.dv & 0xffff); wd_input++;
                }
                else
                {
                    DWORD value = GetUnitYM(uid, j);
                    *wd_input =  ((value >> 16) & 0xffff); wd_input++;
                    *wd_input =  (value & 0xffff); wd_input++;
                }
            }
        }
        
        if (pItem->m_pDiscreteRegTable) {
            bt_discrete = (BYTE *)pItem->m_pDiscreteRegTable;
            for (j = 0; j < pItem->m_yxcount; 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 0
    if (ucRcvAddress != pItem->ied_addr && ucRcvAddress != MB_ADDRESS_BROADCAST)
    {
        vLog(LOG_DEBUG, "数据不是发送给我的.\n");
        return count;
    }
#endif
    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, ord);
        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, int ord)
{
    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, ord))
    {
        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;
}