719 lines
No EOL
25 KiB
C++
719 lines
No EOL
25 KiB
C++
/*-------------------------------------------------------------------
|
|
ModbusProtocol.cpp
|
|
-------------------------------------------------------------------
|
|
|
|
(c) Vector Informatik GmbH. All rights reserved.
|
|
|
|
------------------------------------------------------------------- */
|
|
|
|
#include "StdAfx.h"
|
|
#include "ModbusProtocol.h"
|
|
#include <fstream>
|
|
#include <iomanip>
|
|
#include <cstdarg>
|
|
|
|
////////////////////////////////////////////////////////////////////////
|
|
//
|
|
// Implementation of VExampleSignalProtocol
|
|
//
|
|
////////////////////////////////////////////////////////////////////////
|
|
|
|
|
|
//----------------------------------------------------------------------
|
|
//! Constructor
|
|
/**
|
|
*/
|
|
ModbusProtocol::ModbusProtocol() :
|
|
mProtocolId( NIPB::kNil ),
|
|
mTxId( NIPB::kNil ),
|
|
mProtType( NIPB::kNil ),
|
|
mLength( NIPB::kNil ),
|
|
mUnitId( NIPB::kNil ),
|
|
mFuncCode( NIPB::kNil )
|
|
{
|
|
}
|
|
|
|
|
|
////////////////////////////////////////////////////////////////////////
|
|
// IProtocol
|
|
////////////////////////////////////////////////////////////////////////
|
|
|
|
/*--------------------------------------------------------------------*/
|
|
//! Initialize the protocol
|
|
/*!
|
|
Register the protocol and its tokens at the IPB Packet Lib.
|
|
|
|
\param protocolManager The protocol manager for this protocol. The pointer can be stored and is valid
|
|
during the whole lifetime of the IProtocol object.
|
|
\param networkModel The network model for this protocol. The pointer can be stored and is valid
|
|
during the whole lifetime of the IProtocol object.
|
|
\param modelCreator Define network model entities with this object. Pointer is only valid during this call.
|
|
*/
|
|
VDEF ModbusProtocol::Initialize( NIPB::IProtocolManager /*in*/ *protocolManager, NIPB::INetworkModel /*in*/ *networkModel, NIPB::INetworkModelCreator /*in*/ *modelCreator )
|
|
{
|
|
if (!log.is_open())
|
|
log.open("log.txt", std::ofstream::out | std::ofstream::app);
|
|
log << "Initializing";
|
|
|
|
if (protocolManager == 0) return NIPB::kInvalidArg;
|
|
if (networkModel == 0) return NIPB::kInvalidArg;
|
|
if (modelCreator == 0) return NIPB::kInvalidArg;
|
|
|
|
if (mProtocolId == NIPB::kNil)
|
|
{
|
|
mProtocolManager = protocolManager;
|
|
|
|
NIPB::ITokenDefinitionCreator *protocol = 0;
|
|
if (modelCreator->DefineProtocol( "modbus", &mProtocolId, &protocol ) == NIPB::kOK)
|
|
{
|
|
log << " Defining fields";
|
|
modelCreator->DefineProtocolField( protocol, "TxId", &mTxId, 0 );
|
|
modelCreator->DefineProtocolField( protocol, "Protocol", &mProtType, 0 );
|
|
modelCreator->DefineProtocolField( protocol, "Length", &mLength, 0 );
|
|
modelCreator->DefineProtocolField( protocol, "UnitId", &mUnitId, 0 );
|
|
modelCreator->DefineProtocolField( protocol, "FuncCode", &mFuncCode, 0 );
|
|
}
|
|
log << " MyProtocolId = " << int(mProtocolId);
|
|
}
|
|
else
|
|
{
|
|
log << " not :) ";
|
|
}
|
|
|
|
log << ".\n";
|
|
log.flush();
|
|
return NIPB::kOK;
|
|
}
|
|
|
|
/*--------------------------------------------------------------------*/
|
|
//! Denitialize the protocol
|
|
/*!
|
|
\param protocolManager The protocol manager for this protocol. The pointer is invalid after Deinitialize.
|
|
\param networkModel The network model for this protocol. The pointer is invalid after Deinitialize.
|
|
*/
|
|
VDEF ModbusProtocol::Deinitialize( NIPB::IProtocolManager /*in*/ * /*protocolManager*/, NIPB::INetworkModel /*in*/ * /*networkModel*/ )
|
|
{
|
|
return NIPB::kOK;
|
|
}
|
|
|
|
/*--------------------------------------------------------------------*/
|
|
//! Returns the textual designator of the protocol
|
|
/*!
|
|
The designator is used to access the protocol in CAPL (via Nodelayer) or the GUI of CANoe/CANalyzer.
|
|
|
|
\param bufferSize Length of 'buffer'. Must not > 0
|
|
\param buffer The designator is copied to this buffer. Must not be 0
|
|
*/
|
|
VDEF ModbusProtocol::GetDesignator( unsigned long /*in*/ bufferSize, char /*out**/ *buffer )
|
|
{
|
|
log << "GetDesignator";
|
|
if (bufferSize == 0) return NIPB::kInvalidArg;
|
|
if (buffer == 0) return NIPB::kInvalidArg;
|
|
if (bufferSize < strlen("modbus")) return NIPB::kInvalidArg;
|
|
|
|
strcpy_s( buffer, bufferSize, "modbus" );
|
|
|
|
log << ".\n";
|
|
log.flush();
|
|
return NIPB::kOK;
|
|
}
|
|
|
|
/*--------------------------------------------------------------------*/
|
|
//! Unique token identifier of the protocol
|
|
/*!
|
|
The VTokenId of the protocol is registerd on startup during defintion of the protocol token.
|
|
|
|
\param identifier A pointer to a VTokenId, which retrieves the identifier. Must not be 0
|
|
*/
|
|
VDEF ModbusProtocol::GetIdentifier( NIPB::VTokenId /*out*/ *identifier )
|
|
{
|
|
if (identifier == 0) return NIPB::kInvalidArg;
|
|
|
|
*identifier = mProtocolId;
|
|
|
|
return NIPB::kOK;
|
|
}
|
|
|
|
/*--------------------------------------------------------------------*/
|
|
//! Returns specific encoder for a token ID
|
|
/*!
|
|
The IEncoder is used to access a token within a IPacket.
|
|
|
|
\param tokenId Token identifier for which the encoder is requested.
|
|
\param encoder Returns a pointer to the encoder. Must not be 0.
|
|
*/
|
|
VDEF ModbusProtocol::GetEncoder( NIPB::VTokenId /*in*/ tokenId, NIPB::IEncoder /*out*/ **encoder )
|
|
{
|
|
if (encoder == 0) return NIPB::kInvalidArg;
|
|
|
|
if ( tokenId == mTxId
|
|
|| tokenId == mProtType
|
|
|| tokenId == mLength
|
|
|| tokenId == mUnitId
|
|
|| tokenId == mFuncCode)
|
|
{
|
|
return mProtocolManager->GetEncoder( NIPB::kEncoderUnsignedBE, encoder );
|
|
}
|
|
else
|
|
{
|
|
return NIPB::kEncodingNotSupported;
|
|
}
|
|
}
|
|
|
|
/*--------------------------------------------------------------------*/
|
|
//! Initialize a specific protocol during creation of a new packet
|
|
/*!
|
|
Use this method to setup a protocol within a packet.
|
|
|
|
\param packet The packet where the protocol should be setup. Must not be 0.
|
|
\param protocolTypeId Type ID for a specific type of the protocol, i.e ARP request or ARP response
|
|
\param topProtocolToken Points to the top most protocol within the packet.
|
|
\param protocolToken This strucuture must be filled in InitProtocol.
|
|
*/
|
|
VDEF ModbusProtocol::InitProtocol( NIPB::IPacket /*in*/ *packet, NIPB::VTokenId /*in*/ /*protocolTypeId*/, const NIPB::VProtocolToken /*in*/ * /*topProtocolToken*/, NIPB::VProtocolToken /*out*/ *modbus )
|
|
{
|
|
log << "InitProtocol";
|
|
if (mProtocolManager == 0) return NIPB::kInternalError;
|
|
if (packet == 0) return NIPB::kInvalidArg;
|
|
|
|
NIPB::VResult result = NIPB::kError;
|
|
BYTE broadcastMacId[6] = { 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF };
|
|
|
|
//
|
|
// Init Ethernet protocol
|
|
//
|
|
NIPB::VProtocolToken eth;
|
|
if ((result = mProtocolManager->GetProtocolByIndex( packet, 0, ð )) != NIPB::kOK)
|
|
{
|
|
return result;
|
|
}
|
|
this->SetTokenData( eth, NIPB::kEthDestination, 6, broadcastMacId ); // Set destination MAC ID to Broadcast
|
|
|
|
//
|
|
// Init IPv4 protocol
|
|
//
|
|
NIPB::VProtocolToken ipv4;
|
|
if ((result = mProtocolManager->InitProtocol( packet, NIPB::kIPv4, 0, &ipv4 )) != NIPB::kOK)
|
|
{
|
|
return result;
|
|
}
|
|
this->SetTokenUnsigned( ipv4, NIPB::kIPv4Destination, 4, 0xFFFFFFFF ); // Set destination IP address to Broadcast
|
|
|
|
//
|
|
// Init UDP protocol
|
|
//
|
|
NIPB::VProtocolToken udp;
|
|
if ((result = mProtocolManager->InitProtocol( packet, NIPB::kUDP, 0, &udp )) != NIPB::kOK)
|
|
{
|
|
return result;
|
|
}
|
|
|
|
this->SetTokenUnsigned( udp, NIPB::kUDPSource, 2, kClientPort ); // Set source port
|
|
this->SetTokenUnsigned( udp, NIPB::kUDPDestination, 2, kServerPort ); // Set destination port
|
|
|
|
//
|
|
// Init Modbus protocol
|
|
//
|
|
if (result == (mProtocolManager->ResizeToken( &udp, kHeaderBitLength, 0 )))
|
|
{
|
|
modbus->tokenId = mProtocolId;
|
|
modbus->protocol = this;
|
|
modbus->packet = packet;
|
|
modbus->headerBitOffset = udp.bitOffset;
|
|
modbus->headerBitLength = kHeaderBitLength;
|
|
modbus->bitOffset = udp.bitOffset + kHeaderBitLength;
|
|
modbus->bitLength = 0;
|
|
|
|
this->SetTokenUnsigned( *modbus, mTxId, 2, 0x0000 ); // Set some TxID
|
|
this->SetTokenUnsigned( *modbus, mProtType, 2, 0x0000 ); // Set ProtocolID = Modbus
|
|
this->SetTokenUnsigned( *modbus, mLength, 2, 0x0000 ); // Set some invalid length
|
|
this->SetTokenUnsigned( *modbus, mUnitId, 1, 0xFF ); // Set some UnitID (unused)
|
|
this->SetTokenUnsigned( *modbus, mFuncCode, 1, 0x00 ); // Set some invalid FuncCode
|
|
}
|
|
|
|
log << ".\n";
|
|
log.flush();
|
|
return result;
|
|
}
|
|
|
|
/*--------------------------------------------------------------------*/
|
|
//! Complete the protocol at the end of creation/modification of a packet.
|
|
/*!
|
|
In this function the checksum or length field of a protocol can be calculated.
|
|
|
|
\param packet The packet to complete
|
|
\param protocol Points to the protocol token for this protocol
|
|
*/
|
|
VDEF ModbusProtocol::CompleteProtocol( NIPB::IPacket /*in*/ *packet, const NIPB::VProtocolToken /*in*/ *modbus )
|
|
{
|
|
log << "CompleteProtocol";
|
|
if (packet == 0) return NIPB::kInvalidArg;
|
|
if (modbus == 0) return NIPB::kInvalidArg;
|
|
if (modbus->tokenId != mProtocolId) return NIPB::kError;
|
|
|
|
log << ": Modbus->bitLength = " << std::dec << modbus->bitLength << ", ";
|
|
WORD lengthBy = (WORD)BI2BY(modbus->bitLength); // this is the length of the payload
|
|
lengthBy += 2; // these are the UnitID and FuncCode fields
|
|
this->SetTokenUnsigned( *(NIPB::VProtocolToken*)modbus, mLength, 2, lengthBy ); // Set the correct length
|
|
|
|
log << " --> Length = " << std::dec << int(lengthBy) << ".\n";
|
|
log.flush();
|
|
return NIPB::kOK;
|
|
}
|
|
|
|
/*--------------------------------------------------------------------*/
|
|
//! Parse a received packet.
|
|
/*!
|
|
The method is called, when a packet is parsed. During parsing the VProtocols structure is initialzed
|
|
with the protocol information, whiche are returned with 'protocolToken'.
|
|
|
|
\param packet The packet to parse.
|
|
\param topProtocolToken Points to the top most, already parsed protocol within the packet.
|
|
\param errorWriter Use this interface to return parsing errors, i.e wrong checksum
|
|
\param nextProcotolId Return the VTokenId of the next protocol.
|
|
\param protocolToken Fill this structure to setup the VProtocols structure.
|
|
*/
|
|
VDEF ModbusProtocol::ParsePacket( NIPB::IPacket /*in*/ *packet, const NIPB::VProtocolToken /*in*/ *topProtocolToken, NIPB::IFormatter /*in*/ * /*errorWriter*/, NIPB::VTokenId /*out*/ *nextProcotolId, NIPB::VProtocolToken /*out*/ *protocolToken )
|
|
{
|
|
log << "ParsePacket topProtocolToken->tokenId = " << topProtocolToken->tokenId;
|
|
if (packet == 0 ) return NIPB::kInvalidArg;
|
|
if (nextProcotolId == 0 ) return NIPB::kInvalidArg;
|
|
if (protocolToken == 0 ) return NIPB::kInvalidArg;
|
|
if (topProtocolToken->tokenId != NIPB::kUDP ) return NIPB::kInvalidArg;
|
|
|
|
NIPB::VResult result = NIPB::kError;
|
|
const BYTE *packetData = 0;
|
|
ULONG packetLength = 0;
|
|
NIPB::VProtocols *protocols = 0;
|
|
|
|
if ( ((result = packet->GetDataSize( &packetLength )) != NIPB::kOK) // get packet
|
|
|| ((result = packet->GetDataPtr ( (void**)&packetData )) != NIPB::kOK)
|
|
|| ((result = packet->GetProtocols( &protocols )) != NIPB::kOK) )
|
|
return result;
|
|
|
|
if (topProtocolToken->bitLength <= kHeaderBitLength) // packet is too short
|
|
return NIPB::kPacketParseError;
|
|
|
|
if ((WORD)packetData[BI2BY(topProtocolToken->bitOffset) + 2] != 0x0000) // no Modbus protocol. Caution! BE/LE
|
|
return NIPB::kPacketParseError;
|
|
|
|
|
|
log << " byteOffset = " << BI2BY(topProtocolToken->bitOffset) << ", byteLength = " << BI2BY(topProtocolToken->bitLength) << std::endl;
|
|
log << " Data = ";
|
|
for (ULONG i = BI2BY(topProtocolToken->bitOffset); i < BI2BY(topProtocolToken->bitOffset+topProtocolToken->bitLength); i++)
|
|
log << std::hex << std::setw(2) << std::setfill('0') << int(packetData[i]) << " ";
|
|
log << std::dec << std::endl;
|
|
log.flush();
|
|
|
|
protocolToken->tokenId = mProtocolId;
|
|
protocolToken->protocol = this;
|
|
protocolToken->packet = packet;
|
|
protocolToken->headerBitOffset = topProtocolToken->bitOffset;
|
|
protocolToken->headerBitLength = kHeaderBitLength;
|
|
protocolToken->bitOffset = topProtocolToken->bitOffset + kHeaderBitLength;
|
|
protocolToken->bitLength = topProtocolToken->bitLength - kHeaderBitLength; // don't add UnitID and FuncCode here
|
|
|
|
protocols->contentId = this->GetContentId( packetData, packetLength, *protocolToken );
|
|
log << " --> byteOffset = " << BI2BY(protocolToken->bitOffset) << ", byteLength = " << BI2BY(protocolToken->bitLength) << std::endl;
|
|
log << " --> ContentId = " << std::hex << protocols->contentId << std::dec;
|
|
|
|
*nextProcotolId = 0;
|
|
|
|
log << ".\n\n";
|
|
log.flush();
|
|
return NIPB::kOK;
|
|
}
|
|
|
|
/*--------------------------------------------------------------------*/
|
|
//! Return a token of a protocol.
|
|
/*!
|
|
The function must fill the 'token' with the information of the request 'tokenId'.
|
|
|
|
\param protocolToken Points the the VProtocolToken for the protocol.
|
|
\param tokenId The identifier of the requested token.
|
|
\param token Points to a VToken, which must be filled by this function.
|
|
*/
|
|
VDEF ModbusProtocol::GetToken( const NIPB::VProtocolToken /*in*/ *protocolToken, NIPB::VTokenId /*in*/ tokenId, NIPB::VToken /*out*/ *token )
|
|
{
|
|
if (token == 0) return NIPB::kInvalidArg;
|
|
if (protocolToken == 0) return NIPB::kInvalidArg;
|
|
if (protocolToken->tokenId != mProtocolId) return NIPB::kTokenNotFound;
|
|
|
|
if (tokenId == mTxId)
|
|
return MakeHeaderToken( protocolToken, mTxId, 0, 16, token );
|
|
else if (tokenId == mProtType)
|
|
return MakeHeaderToken( protocolToken, mProtType, 16, 16, token );
|
|
else if (tokenId == mLength)
|
|
return MakeHeaderToken( protocolToken, mLength, 32, 16, token );
|
|
else if (tokenId == mUnitId)
|
|
return MakeHeaderToken( protocolToken, mUnitId, 48, 8, token );
|
|
else if (tokenId == mFuncCode)
|
|
return MakeHeaderToken( protocolToken, mFuncCode, 56, 8, token );
|
|
else if (tokenId == NIPB::kData)
|
|
return this->MakePayloadToken( protocolToken, NIPB::kData, this->kHeaderBitLength, protocolToken->bitLength, token );
|
|
else if (tokenId == NIPB::kHeader)
|
|
return this->MakeHeaderToken( protocolToken, NIPB::kHeader, 0, this->kHeaderBitLength, token );
|
|
else
|
|
return NIPB::kTokenNotFound;
|
|
}
|
|
|
|
/*--------------------------------------------------------------------*/
|
|
//! Build a tree with information of the protocol.
|
|
/*!
|
|
The function is used in CANoe/CANalyzer in the detail view of the trace window to build
|
|
a tree with protocols and fields.
|
|
|
|
\param protocolToken Protocol token of this protocol
|
|
\param type Inspection type
|
|
\param inspector IPacketInspectore to create the output.
|
|
*/
|
|
VDEF ModbusProtocol::InspectProtocol ( const NIPB::VProtocolToken /*in*/ *protocolToken, NIPB::VInspectionType /*in*/ type, NIPB::IPacketInspector /*in*/ *inspector )
|
|
{
|
|
if (protocolToken == 0) return NIPB::kInvalidArg;
|
|
if (protocolToken->packet == 0) return NIPB::kInvalidArg;
|
|
if (inspector == 0) return NIPB::kInvalidArg;
|
|
|
|
NIPB::VResult result = NIPB::kError;
|
|
const BYTE *packetData = 0;
|
|
ULONG packetLength = 0;
|
|
|
|
if ( ((result = protocolToken->packet->GetDataSize( &packetLength )) != NIPB::kOK)
|
|
|| ((result = protocolToken->packet->GetDataPtr ( (void**)&packetData )) != NIPB::kOK) )
|
|
return result;
|
|
|
|
if (inspector->BeginToken( mProtocolId, 0, 0) != NIPB::kOK)
|
|
return NIPB::kError;
|
|
|
|
NIPB::IFormatter *formatter = 0;
|
|
ULONG val;
|
|
|
|
// TODO: Make InspectProtocol nice and smooth
|
|
switch(type)
|
|
{
|
|
// Protocol Info column
|
|
case NIPB::kInspectionInfoColumn:
|
|
if (inspector->SelectField( NIPB::kFieldLabel, &formatter ) == NIPB::kOK)
|
|
{
|
|
formatter->FormatString( "MB: [" );
|
|
|
|
val = GetTxId( packetData, packetLength, *protocolToken );
|
|
formatter->FormatUnsigned(val, 2, 10);
|
|
|
|
formatter->FormatString( "] " );
|
|
|
|
val = GetFuncCode( packetData, packetLength, *protocolToken );
|
|
if (val > 0x80)
|
|
{
|
|
formatter->FormatString( "(0x" );
|
|
formatter->FormatUnsigned( val & 0x0F, 1, 16 );
|
|
|
|
val = GetExCode( packetData, packetLength, *protocolToken );
|
|
formatter->FormatString( ") Ex 0x" );
|
|
formatter->FormatUnsigned( val, 1, 16);
|
|
switch (val) // exCode
|
|
{
|
|
case 0x01:
|
|
formatter->FormatString( ": Illegal func code!" );
|
|
formatter->FormatUnsigned( GetFuncCode( packetData, packetLength, *protocolToken ), 1, 16 );
|
|
break;
|
|
case 0x02:
|
|
formatter->FormatString( ": Illegal data address!" );
|
|
break;
|
|
case 0x03:
|
|
formatter->FormatString( ": Illegal data value!" );
|
|
break;
|
|
case 0x04:
|
|
formatter->FormatString( ": Server failure!" );
|
|
break;
|
|
case 0x05:
|
|
formatter->FormatString( ": Acknowledge!" );
|
|
break;
|
|
case 0x06:
|
|
formatter->FormatString( ": Server busy!" );
|
|
break;
|
|
case 0x0A:
|
|
case 0x0B:
|
|
formatter->FormatString( ": Gateway problem!" );
|
|
break;
|
|
default:
|
|
formatter->FormatString( "!" );
|
|
break;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
switch (val) // funcCode
|
|
{
|
|
case 0x01:
|
|
case 0x02:
|
|
formatter->FormatString( "Read bits" );
|
|
break;
|
|
case 0x03:
|
|
case 0x04:
|
|
formatter->FormatString( "Read regs" );
|
|
break;
|
|
case 0x05:
|
|
formatter->FormatString( "Write bit" );
|
|
break;
|
|
case 0x06:
|
|
formatter->FormatString( "Write reg" );
|
|
break;
|
|
case 0x0F:
|
|
formatter->FormatString( "Write bits" );
|
|
break;
|
|
case 0x10:
|
|
formatter->FormatString( "Write regs" );
|
|
break;
|
|
case 0x16:
|
|
formatter->FormatString( "Write masks" );
|
|
break;
|
|
case 0x17:
|
|
formatter->FormatString( "Read&Write regs" );
|
|
break;
|
|
default:
|
|
formatter->FormatString( "[0x" );
|
|
formatter->FormatUnsigned(val, 1, 16);
|
|
formatter->FormatString( "]" );
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
break;
|
|
//
|
|
// Detail View
|
|
//
|
|
case NIPB::kInspectionDetailTree:
|
|
if (inspector->SelectField( NIPB::kFieldLabel, &formatter ) == NIPB::kOK)
|
|
formatter->FormatString( "ModbusDetail" );
|
|
|
|
val = GetContentId( packetData, packetLength, *protocolToken );
|
|
FormatUnsigned( inspector, "TxIDDetail", mTxId, val, 2 );
|
|
break;
|
|
}
|
|
|
|
inspector->EndToken();
|
|
|
|
return result;
|
|
}
|
|
|
|
////////////////////////////////////////////////////////////////////////
|
|
// Methods
|
|
////////////////////////////////////////////////////////////////////////
|
|
|
|
/*--------------------------------------------------------------------*/
|
|
//! Returns the content ID
|
|
/*!
|
|
*/
|
|
ULONG ModbusProtocol::GetContentId( const BYTE *packetData, ULONG packetLength, const NIPB::VProtocolToken &protocol )
|
|
{
|
|
ULONG ip = GetIp( packetData, packetLength, protocol );
|
|
BYTE funcCode = GetFuncCode( packetData, packetLength, protocol );
|
|
|
|
return ((ip << 16) & 0xFFFF0000) | funcCode;
|
|
}
|
|
|
|
/*--------------------------------------------------------------------*/
|
|
//! Returns the TxID
|
|
/*!
|
|
*/
|
|
WORD ModbusProtocol::GetTxId( const BYTE *packetData, ULONG packetLength, const NIPB::VProtocolToken &protocol )
|
|
{
|
|
ULONG offset = BI2BY(protocol.headerBitOffset);
|
|
|
|
if (offset+1 < packetLength)
|
|
return packetData[offset] << 8 | packetData[offset+1]; // BE/LE
|
|
else
|
|
return 0;
|
|
}
|
|
|
|
/*--------------------------------------------------------------------*/
|
|
//! Returns the FuncCode
|
|
/*!
|
|
*/
|
|
BYTE ModbusProtocol::GetFuncCode( const BYTE *packetData, ULONG packetLength, const NIPB::VProtocolToken &protocol )
|
|
{
|
|
ULONG offset = BI2BY(protocol.headerBitOffset) + 7;
|
|
|
|
if (offset+1 < packetLength)
|
|
{
|
|
return packetData[offset];
|
|
}
|
|
else
|
|
{
|
|
return 0;
|
|
}
|
|
}
|
|
|
|
/*--------------------------------------------------------------------*/
|
|
//! Returns the DestinationIP
|
|
/*!
|
|
*/
|
|
ULONG ModbusProtocol::GetIp( const BYTE * /*packetData*/, ULONG /*packetLength*/, const NIPB::VProtocolToken &/*protocol*/ )
|
|
{
|
|
// TODO: GetDestIp
|
|
/*NIPB::VToken *ipv4DestIp;
|
|
|
|
if (NIPB::GetToken(NIPB::kIPv4, NIPB::kIPv4Destination, ipv4DestIp) != NIPB::kOK)
|
|
return 0;
|
|
|
|
return ipv4DestIp->bitOffset
|
|
|
|
if (offset+2 < packetLength)
|
|
{
|
|
return *((WORD*)&packetData[offset]);
|
|
}
|
|
else
|
|
{
|
|
return 0;
|
|
}*/
|
|
return 0xC0A80102;
|
|
}
|
|
|
|
BYTE ModbusProtocol::GetExCode( const BYTE *packetData, ULONG packetLength, const NIPB::VProtocolToken &protocol )
|
|
{
|
|
ULONG offset = BI2BY(protocol.bitOffset);
|
|
|
|
if (offset+1 < packetLength)
|
|
{
|
|
return packetData[offset];
|
|
}
|
|
else
|
|
{
|
|
return 0;
|
|
}
|
|
}
|
|
|
|
/*--------------------------------------------------------------------*/
|
|
//! Fill a VToken structure.
|
|
/*!
|
|
*/
|
|
NIPB::VResult ModbusProtocol::MakeHeaderToken( const NIPB::VProtocolToken /*in*/ *protocolToken, NIPB::VTokenId /*in*/ tokenId, ULONG /*in*/ offset, ULONG /*in*/ length, NIPB::VToken /*out*/ *token )
|
|
{
|
|
if (token == 0) return NIPB::kInvalidArg;
|
|
if (protocolToken == 0) return NIPB::kInvalidArg;
|
|
|
|
token->protocol = protocolToken->protocol;
|
|
token->packet = protocolToken->packet;
|
|
token->tokenId = tokenId;
|
|
token->bitOffset = offset + protocolToken->headerBitOffset;
|
|
token->bitLength = length;
|
|
|
|
return NIPB::kOK;
|
|
}
|
|
|
|
/*--------------------------------------------------------------------*/
|
|
//! Fill a VToken structure.
|
|
/*!
|
|
*/
|
|
NIPB::VResult ModbusProtocol::MakePayloadToken( const NIPB::VProtocolToken /*in*/ *protocolToken, NIPB::VTokenId /*in*/ tokenId, ULONG /*in*/ offset, ULONG /*in*/ length, NIPB::VToken /*out*/ *token )
|
|
{
|
|
if (token == 0) return NIPB::kInvalidArg;
|
|
if (protocolToken == 0) return NIPB::kInvalidArg;
|
|
|
|
token->protocol = protocolToken->protocol;
|
|
token->packet = protocolToken->packet;
|
|
token->tokenId = tokenId;
|
|
token->bitOffset = offset + protocolToken->bitOffset;
|
|
token->bitLength = length;
|
|
|
|
return NIPB::kOK;
|
|
}
|
|
|
|
/*--------------------------------------------------------------------*/
|
|
//! Format a unsigned token.
|
|
/*!
|
|
*/
|
|
void ModbusProtocol::FormatUnsigned( NIPB::IPacketInspector *inspector, LPCTSTR labelPre, NIPB::VTokenId tokenId, LPCTSTR labelPost, ULONG value, ULONG valueLength )
|
|
{
|
|
if ((inspector) && (inspector->BeginToken( tokenId, 0, 0) == NIPB::kOK))
|
|
{
|
|
NIPB::IFormatter *formatter = 0;
|
|
|
|
if (inspector->SelectField( NIPB::kFieldLabel, &formatter ) == NIPB::kOK)
|
|
formatter->FormatString( labelPre );
|
|
|
|
if (inspector->SelectField( NIPB::kFieldValue, &formatter ) == NIPB::kOK)
|
|
formatter->FormatUnsigned( value, valueLength, 0 );
|
|
|
|
if (inspector->SelectField( NIPB::kFieldLabel, &formatter ) == NIPB::kOK)
|
|
formatter->FormatString( labelPost );
|
|
|
|
inspector->EndToken();
|
|
}
|
|
}
|
|
/*--------------------------------------------------------------------*/
|
|
//! Format a unsigned token.
|
|
/*!
|
|
*/
|
|
void ModbusProtocol::FormatUnsigned( NIPB::IPacketInspector *inspector, LPCTSTR label, NIPB::VTokenId tokenId, ULONG value, ULONG valueLength )
|
|
{
|
|
if ((inspector) && (inspector->BeginToken( tokenId, 0, 0) == NIPB::kOK))
|
|
{
|
|
NIPB::IFormatter *formatter = 0;
|
|
|
|
if (inspector->SelectField( NIPB::kFieldLabel, &formatter ) == NIPB::kOK)
|
|
formatter->FormatString( label );
|
|
|
|
if (inspector->SelectField( NIPB::kFieldValue, &formatter ) == NIPB::kOK)
|
|
formatter->FormatUnsigned( value, valueLength, 0 );
|
|
|
|
inspector->EndToken();
|
|
}
|
|
}
|
|
|
|
/*--------------------------------------------------------------------*/
|
|
//! Format a string.
|
|
/*!
|
|
*/
|
|
void ModbusProtocol::FormatString( NIPB::IPacketInspector *inspector, LPCTSTR text )
|
|
{
|
|
if (inspector)
|
|
{
|
|
NIPB::IFormatter *formatter = 0;
|
|
|
|
if (inspector->SelectField( NIPB::kFieldLabel, &formatter ) == NIPB::kOK)
|
|
formatter->FormatString( text );
|
|
}
|
|
}
|
|
|
|
/*--------------------------------------------------------------------*/
|
|
//! Set the value of a unsigned token.
|
|
/*!
|
|
*/
|
|
NIPB::VResult ModbusProtocol::SetTokenUnsigned( NIPB::VProtocolToken &protocolToken, NIPB::VTokenId tokenId, ULONG valueLength, ULONG value )
|
|
{
|
|
if (protocolToken.protocol == 0) return NIPB::kInvalidArg;
|
|
|
|
NIPB::VResult result = NIPB::kError;
|
|
NIPB::VToken token;
|
|
if ((result = protocolToken.protocol->GetToken( &protocolToken, tokenId, &token)) == NIPB::kOK)
|
|
{
|
|
NIPB::IEncoder *encoder = 0;
|
|
if (((result = token.protocol->GetEncoder( tokenId, &encoder )) == NIPB::kOK) && (encoder))
|
|
result = encoder->Encode( &token, 0, NIPB::kEncodingUnsigned, valueLength, &value );
|
|
}
|
|
|
|
return result;
|
|
}
|
|
|
|
/*--------------------------------------------------------------------*/
|
|
//! Set the data of a data token.
|
|
/*!
|
|
*/
|
|
NIPB::VResult ModbusProtocol::SetTokenData( NIPB::VProtocolToken &protocolToken, NIPB::VTokenId tokenId, ULONG dataLength, const BYTE *data )
|
|
{
|
|
if (protocolToken.protocol == 0) return NIPB::kInvalidArg;
|
|
|
|
NIPB::VResult result = NIPB::kError;
|
|
NIPB::VToken token;
|
|
if ((result = protocolToken.protocol->GetToken( &protocolToken, tokenId, &token)) == NIPB::kOK)
|
|
{
|
|
NIPB::IEncoder *encoder = 0;
|
|
if (((result = token.protocol->GetEncoder( tokenId, &encoder )) == NIPB::kOK) && (encoder))
|
|
{
|
|
result = encoder->Encode( &token, 0, NIPB::kEncodingData, dataLength, data );
|
|
}
|
|
}
|
|
|
|
return result;
|
|
} |