496 lines
No EOL
18 KiB
C++
496 lines
No EOL
18 KiB
C++
/*-------------------------------------------------------------------
|
|
ModbusProtocol.cpp
|
|
-------------------------------------------------------------------
|
|
|
|
(c) Vector Informatik GmbH. All rights reserved.
|
|
|
|
------------------------------------------------------------------- */
|
|
|
|
#include "StdAfx.h"
|
|
#include "ModbusProtocol.h"
|
|
|
|
////////////////////////////////////////////////////////////////////////
|
|
//
|
|
// Implementation of VExampleSignalProtocol
|
|
//
|
|
////////////////////////////////////////////////////////////////////////
|
|
|
|
|
|
//----------------------------------------------------------------------
|
|
//! Constructor
|
|
/**
|
|
*/
|
|
ModbusProtocol::ModbusProtocol() :
|
|
mProtocolId( NIPB::kNil ),
|
|
mTxId(0),
|
|
mProtType(0),
|
|
mLength(0),
|
|
mUnitId(0),
|
|
mFuncCode(0)
|
|
{
|
|
}
|
|
|
|
|
|
////////////////////////////////////////////////////////////////////////
|
|
// 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 (protocolManager == 0) return NIPB::kInvalidArg;
|
|
if (networkModel == 0) return NIPB::kInvalidArg;
|
|
if (modelCreator == 0) return NIPB::kInvalidArg;
|
|
|
|
mProtocolManager = protocolManager;
|
|
|
|
NIPB::ITokenDefinitionCreator *protocol = 0;
|
|
if (modelCreator->DefineProtocol( "modbus", &mProtocolId, &protocol ) == NIPB::kOK)
|
|
{
|
|
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 );
|
|
}
|
|
|
|
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*/ )
|
|
{
|
|
mProtocolId = 0;
|
|
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 )
|
|
{
|
|
if (bufferSize == 0) return NIPB::kInvalidArg;
|
|
if (buffer == 0) return NIPB::kInvalidArg;
|
|
if (bufferSize < strlen("modbus")) return NIPB::kInvalidArg;
|
|
|
|
strcpy_s( buffer, bufferSize, "modbus" );
|
|
|
|
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)
|
|
{
|
|
return mProtocolManager->GetEncoder( NIPB::kEncoderUnsignedLE, encoder );
|
|
}
|
|
else if ( 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 )
|
|
{
|
|
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, 40002 ); // Set source port
|
|
this->SetTokenUnsigned( udp, NIPB::kUDPDestination, 2, 502 ); // 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
|
|
}
|
|
|
|
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 )
|
|
{
|
|
if (packet == 0) return NIPB::kInvalidArg;
|
|
if (modbus == 0) return NIPB::kInvalidArg;
|
|
if (modbus->tokenId != mProtocolId) return NIPB::kError;
|
|
|
|
WORD lengthByte;
|
|
lengthByte = (WORD)BI2BY(modbus->bitLength - kHeaderBitLength + 16); // add UnitID and FuncCode to length although they are header
|
|
this->SetTokenUnsigned( *(NIPB::VProtocolToken*)modbus, mLength, 2, lengthByte ); // Set the correct length
|
|
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 )
|
|
{
|
|
if (packet == 0 ) return NIPB::kInvalidArg;
|
|
if (nextProcotolId == 0 ) return NIPB::kInvalidArg;
|
|
if (protocolToken == 0 ) return NIPB::kInvalidArg;
|
|
if (topProtocolToken == 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)
|
|
|| ((result = packet->GetDataPtr ( (void**)&packetData )) != NIPB::kOK)
|
|
|| ((result = packet->GetProtocols( &protocols )) != NIPB::kOK) )
|
|
return result;
|
|
|
|
if (topProtocolToken->bitLength <= kHeaderBitLength)
|
|
return NIPB::kPacketParseError;
|
|
|
|
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 );
|
|
|
|
*nextProcotolId = 0;
|
|
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 fille 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( "ModbusInfo" );
|
|
|
|
// Source port
|
|
val = GetContentId( packetData, packetLength, *protocolToken );
|
|
FormatUnsigned( inspector, "TxIDInfo", mTxId, val, 2 );
|
|
break;
|
|
//
|
|
// Detail View
|
|
//
|
|
case NIPB::kInspectionDetailTree:
|
|
if (inspector->SelectField( NIPB::kFieldLabel, &formatter ) == NIPB::kOK)
|
|
formatter->FormatString( "ModbusDetail" );
|
|
|
|
// Source port
|
|
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 offset = BI2BY(protocol.headerBitOffset);
|
|
|
|
if (offset+4 < packetLength)
|
|
{
|
|
return *((ULONG*)&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 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();
|
|
}
|
|
}
|
|
|
|
/*--------------------------------------------------------------------*/
|
|
//! 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;
|
|
} |