modbustcp.cpp

一个通讯管理机的源代码。比较好用。推荐

#include "allmgr.h"
#include "modbustcp.h"
#include "modbusregset.h"

int CModbusTCPSlave::processMsg(unsigned char b[],   // message buffer, starting with prefix
								unsigned len)        // length of incoming messsage
								// returns length of response
{

	// if you wish to make your processing dependent upon unit identifier
	// use b[6]. Many PLC devices will ignore this field, and most clients
	// default value to zero. However gateways or specialized programs can
	// use the unit number to indicate what type of precessing is desired

	//    unsigned i;

	// handle the function codes 3 and 16

	switch(b[7])
	{
	case 3: // <10000 ?4xxxx >>8xxxx read n input word
		// read registers
		// request   03 rr rr nn nn
		// response  03 bb da ta ......
		{
			BYTE* pbyTemp = (BYTE *)&b[8];
			unsigned regNo = CW(pbyTemp);
			unsigned regCount = CW(pbyTemp);
			if (len != 12)
			{
				// exception 3 - bad structure to message
				b[7] |= 0x80;
				b[8] = 3;
				b[5] = 3; // length
				break;
			}
			if (regCount == 0) regCount=2;
			if (regCount >126) regCount = 126;

			struReg reg;
			reg.wArea = 8;

			WORD w1=0;
			for (WORD w=0;w<regCount;w++,regNo++)
			{
				struLocal local;
				reg.wRegAddr = regNo/2;
				BYTE * pbyDWORD = &b[9+w*2];
				map<struReg, struLocal>::const_iterator it = m_mapReg.find(reg);
				if (it != m_mapReg.end())
				{
					local = it->second;
					CAnalog* pAnalog = m_pAllMgr->GetAnalog(local.wDevNo*1000+local.wLocalNo);
					if (pAnalog)
					{
						float fValue = pAnalog->GetValue();
						long lValue = (long) fValue;
						if (regNo%2 == 1)
						{
							lValue &= 0xffff;
							SW(lValue,pbyDWORD);
							w1+=2;
						}
						else 
						{
							lValue >>= 16;
							SW(lValue,pbyDWORD);
							w1+=2;
						}
					}
					else SW(0,pbyDWORD);
				}
				else SW(0,pbyDWORD);
			}//end for
			// OK so prepare the 'OK response'
			if (w1 == 0)
			{
				b[7] |= 0x80;
				b[8] = 4;
				b[5] = 3; // length
			}
			else
			{
				b[8] = regCount*2;
				b[5] = b[8] + 3;
			}
			break;
		}
	case 4: // <10000 ?3xxxx :8xxxx read n input word
		// read registers
		// request   03 rr rr nn nn
		// response  03 bb da ta ......
		{
			BYTE* pbyTemp = (BYTE *)&b[8];
			unsigned regNo = CW(pbyTemp);
			unsigned regCount = CW(pbyTemp);
			if (len != 12)
			{
				// exception 3 - bad structure to message
				b[7] |= 0x80;
				b[8] = 3;
				b[5] = 3; // length
				break;
			}
			if (regCount == 0) regCount=1;
			if (regCount >126) regCount = 126;
			if ((regNo < 10000) && (regCount + regNo > 10000))
				regCount = 10000-regNo;

			struReg reg;
			reg.wArea = 3;
			if (regNo>=10000)
			{
				regNo = regNo-10000;
				reg.wArea = 8;
				if (regCount > 63)
					regCount = 63;
			}

			WORD w1=0;
			for (WORD w=0;w<regCount;w++,regNo++)
			{
				struLocal local;
				reg.wRegAddr = regNo;
				map<struReg, struLocal>::const_iterator it = m_mapReg.find(reg);
				if (it != m_mapReg.end())
				{
					local = it->second;
					CAnalog* pAnalog = m_pAllMgr->GetAnalog(local.wDevNo*1000+local.wLocalNo);
					if (pAnalog)
					{
						float fValue = pAnalog->GetValue();
						if (reg.wArea == 8)
						{
							long lValue = (long) fValue;
							BYTE * pbyDWORD = &b[9+w*4];
//							 = CDW(*(DWORD *)&lValue);
							SDW(lValue,pbyDWORD);
							w1+=4;
						}
						else
						{
							short sValue = (short) fValue;
							BYTE * pbyWORD = &b[9+w*2];
//							*((WORD*)&b[9+w*2])   = CW(*(WORD *)&sValue);
							SW(sValue,pbyWORD);
							w1+=2;
						}
					}
					else
					{
						if (reg.wArea == 8)
						{
							BYTE * pbyDWORD = &b[9+w*4];
//							*((DWORD*)&b[9+w*4]) = 0;
							SDW(0,pbyDWORD);
						}
						else
						{
							BYTE * pbyWORD = &b[9+w*2];
//							*((WORD*)&b[9+w*2])   = 0;
							SW(0,pbyWORD);
						}
					}
				}
				else 
				{
					if (reg.wArea == 8)
					{
						BYTE * pbyDWORD = &b[9+w*4];
//							*((DWORD*)&b[9+w*4]) = 0;
						SDW(0,pbyDWORD);
					}
					else
					{
						BYTE * pbyWORD = &b[9+w*2];
//							*((WORD*)&b[9+w*2])   = 0;
						SW(0,pbyWORD);
					}
				}
			}//end for
			// OK so prepare the 'OK response'
			if (w1 == 0)
			{
				b[7] |= 0x80;
				b[8] = 4;
				b[5] = 3; // length
			}
			else
			{
				if (reg.wArea == 8)
					b[8] = regCount*4;
				else b[8] = regCount*2;
				b[5] = b[8] + 3;
			}
			break;
		}
	case 5: //0xxxx, YK, write 1 bit
		{
			if (len != 12)
			{
				// exception 3 - bad structure to message
				b[7] |= 0x80;
				b[8] = 3;
				b[5] = 3; // length
				break;
			}
			BYTE* pbyTemp = &b[8];
			unsigned bitregNo = CW(pbyTemp);
			unsigned w = CW(pbyTemp);
			bool bOn = (w==0)?false:true;

			struLocal local;
			struReg reg;
			reg.wArea = 0;
			reg.wRegAddr = bitregNo;
			map<struReg, struLocal>::const_iterator it = m_mapReg.find(reg);
			if (it != m_mapReg.end())
			{
				local = it->second;

				if (!m_pAllMgr->YK(local.wDevNo,local.wLocalNo,bOn))
				{
					b[7] |= 0x80;
					b[8] = 3;
					b[5] = 3; // length
				}
			}
			else
			{
				b[7] |= 0x80;
				b[8] = 4;
				b[5] = 3; // length
			}
			break;
		}	
	case 2://1xxxx read n input bit
		{
			if (len != 12)
			{
				// exception 3 - bad structure to message
				b[7] |= 0x80;
				b[8] = 3;
				b[5] = 3; // length
				break;
			}
			BYTE* pbyTemp = (BYTE *)&b[8];
			unsigned regNo = CW(pbyTemp);
			unsigned regCount = CW(pbyTemp);
			if (regCount > 252*8) regCount = 252*8;
			
//			unsigned regNo = CW(*((WORD*) &b[8]));
//			unsigned regCount = CW(*((WORD*) &b[10]));
			struReg reg;
			reg.wArea = 1;
			WORD w1=0;	
			for (WORD w=0;w<regCount;w++)
			{
				if (w%8 == 0) b[9+w/8] = 0;
			}
			for (WORD w=0;w<regCount;w++,regNo++)
			{
				struLocal local;
				reg.wRegAddr = regNo;
				map<struReg, struLocal>::const_iterator it = m_mapReg.find(reg);
				if (it != m_mapReg.end())
				{
					local = it->second;
					CDigital* pDigital = m_pAllMgr->GetDigital(local.wDevNo*1000+local.wLocalNo);
					if (pDigital)
					{
						w1++;
						bool bValue = pDigital->GetValue();
						BYTE* pTemp = (BYTE *)&b[9+w/8];
						BYTE byValue = *pTemp;
						BYTE bbb=1<<(w%8);
						if (bValue)
							*pTemp = byValue | bbb;
						else
						{
							bbb = ~bbb;
							*pTemp  = byValue & bbb;
						}	
					}
				}
			}
			// OK so prepare the 'OK response'

			if (w1 == 0)
			{
				b[7] |= 0x80;
				b[8] = 4;
				b[5] = 3; // length
			}
			else
			{
				b[8] = (regCount+7)/8;
				b[5] = b[8] + 3;
			}
			break;
		}

	default:
		// generate exception 1 - unknown function code
		b[7] |= 0x80;
		b[8] = 1;
		b[5] = 3; // length
		break;
	}

	// return the total size of the MB/TCP response
	// notice that bytes 0-4 and 6 will be identical to those of the request

	return 6+b[5];
}

bool CModbusTCPSlave::OnRead(CTcpStream& stTcpStream)
{
	int nSocket = stTcpStream.SocketFd();
	fragMsg *thisFrag = GetFragMsg(nSocket);
	
    unsigned char *ibuf = thisFrag->fragBuf;

	int iReadAvail = stTcpStream.rdbuf()->in_avail();
	int iToRead = 0;
	if (thisFrag->fragLen < 6)
	{
		int iToRead = (iReadAvail<(6 - thisFrag->fragLen)) ? iReadAvail : (6 - thisFrag->fragLen);
		
		stTcpStream.read((char *)&thisFrag->fragBuf[thisFrag->fragLen], iToRead);
		if (iToRead < 0) return false;
		thisFrag->fragLen += iToRead;
		iReadAvail -= iToRead;
	}
	
	if ((thisFrag->fragLen >= 6) && (iReadAvail > 0))
	{
		if (ibuf[2] != 0 || ibuf[3] != 0 || ibuf[4] != 0 || ibuf[5] < 2)
		{
		// this is not legitimate Modbus/TCP
		// possibly your client is very confused
		// close down the connection
		// remove session from active list
			return false;
		}

	// the real length is in ibuf[5]

		if (thisFrag->fragLen < 6+ibuf[5])
		{
			iToRead = (iReadAvail<(6 + ibuf[5] - thisFrag->fragLen)) ? iReadAvail : (6 + ibuf[5] - thisFrag->fragLen);
			stTcpStream.read((char *)&thisFrag->fragBuf[thisFrag->fragLen], iToRead);
			if (iToRead < 0) return false;
			thisFrag->fragLen += iToRead;
		}
		if (thisFrag->fragLen == 6+ibuf[5])
		{
			int nLen = processMsg(ibuf, thisFrag->fragLen);
			if (nLen > 0)
			{		
				stTcpStream.write((char *)ibuf,nLen);
			}
			
            thisFrag->fragLen = 0;
            
		}
	}

	stTcpStream<<flush;

	return true;
}

void* thrdModbusTCPSlave(void *pVoid)
{
	CModbusTCPSlave *pSlave = (CModbusTCPSlave *)pVoid;
	pthread_setcancelstate(PTHREAD_CANCEL_ENABLE,NULL);
	pthread_setcanceltype(PTHREAD_CANCEL_ASYNCHRONOUS,NULL);
	pSlave->RunSlave();
	return 0;
}

void CModbusTCPSlave::RunSlave()
{
	for (BYTE b=0; b<10; b++)
	{
		m_dwPrev++;
		sleep(1);
	}
	while (true)
	{
		m_dwPrev++;
		try
		{
			if (m_bShutdown)
				break;
				
			CheckConnTimeout();
			CheckEvent();
			OnCheck();
		}
		catch (CException& e)
		{
//			if (!OnException(e))
	//			break;
		}
	}
}


bool CModbusTCPSlave::Init()
{
	bool bRet = false;

	xbXBase	x;
	CModbusRegDefSet	set(&x);
	if (set.Open())
	{
		if (set.GetFirst())
		{
			do {
				struLocal local;
				struReg reg;
				local.wDevNo = set.m_wDevID;
				local.wLocalNo = set.m_wRegAddr;
				reg.wArea = set.m_wArea;
				reg.wRegAddr = set.m_wLocalAddr;
				pair <struReg, struLocal> temp(reg,local);
				m_mapReg.insert(temp);
			} while (set.GetNext());
		}
		set.Close();
	}

	if (pthread_create(&m_hThread,&attr,thrdModbusTCPSlave,(void *)this) == 0)
		bRet = true;
	else	m_hThread = 0;
	return bRet;
}