/*-------------------------------------------------------------------
	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, &eth )) != 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, 0, 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 valA, valB;

	// 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: [" );

			valA = GetTxId( packetData, packetLength, *protocolToken );
			formatter->FormatUnsigned(valA, 2, 10);

			formatter->FormatString( "] " );
			
			valA = GetFuncCode( packetData, packetLength, *protocolToken );
			valB = GetFirstDataByte( packetData, packetLength, *protocolToken );		// Exception Code OR number of bytes
			if (valA > 0x80)
			{
				formatter->FormatString( "(0x" );
				formatter->FormatUnsigned( valA & 0x0F, 1, 16 );

				formatter->FormatString( ") Ex" );
				formatter->FormatUnsigned( valB, 1, 10);
				switch (valB)	// 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 (valA)	// funcCode
				{
				case 0x01:
				case 0x02:
					if (protocolToken->bitLength == BY2BI(valB+1))	// if the first byte gives the count of following bytes --> response
					{
						formatter->FormatString( "Read bits response" );
					}
					else
					{
						formatter->FormatString( "Read " );
						valB = GetAddrNCount( packetData, packetLength, *protocolToken );
						formatter->FormatUnsigned( valB & 0xFFFF, 2, 10 );
						formatter->FormatString( " bits from 0x" );
						formatter->FormatUnsigned( valB >> 16, 2, 16 );
					}
					break;
				case 0x03:
				case 0x04:
					if (protocolToken->bitLength == BY2BI(valB+1))	// if the first byte gives the count of following bytes --> response
					{
						formatter->FormatString( "Read regs response" );
					}
					else
					{
						formatter->FormatString( "Read " );
						valB = GetAddrNCount( packetData, packetLength, *protocolToken );
						formatter->FormatUnsigned( valB & 0xFFFF, 2, 10 );
						formatter->FormatString( " regs from 0x" );
						formatter->FormatUnsigned( valB >> 16, 2, 16 );
					}
					break;
				case 0x05:
					formatter->FormatString( "Set bit 0x" );
					formatter->FormatUnsigned( valB >> 16, 2, 16 );

					valB = GetAddrNCount( packetData, packetLength, *protocolToken );
					formatter->FormatString( " to " );
					if (valB & 0xFFFF > 0xFF00)
						formatter->FormatString( "1" );
					else
						formatter->FormatString( "0" );
					break;
				case 0x06:
					formatter->FormatString( "Set reg 0x" );
					formatter->FormatUnsigned( valB >> 16, 2, 16 );

					valB = GetAddrNCount( packetData, packetLength, *protocolToken );
					formatter->FormatString( " to " );
					formatter->FormatUnsigned( valB & 0xFFFF, 2, 10 );
					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(valA, 1, 16);
					formatter->FormatString( "]" );
					break;
				}
			}
		}
		break;
	//
	// Detail View
	//
	case NIPB::kInspectionDetailTree:
		if (inspector->SelectField( NIPB::kFieldLabel, &formatter ) == NIPB::kOK)
			formatter->FormatString( "ModbusDetail" );

		valB = GetContentId( packetData, packetLength, *protocolToken );
		FormatUnsigned( inspector, "TxIDDetail", mTxId, valB, 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 < 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;
}

/*--------------------------------------------------------------------*/
//! Returns the first byte of the payload
/*!
*/
BYTE ModbusProtocol::GetFirstDataByte(	const BYTE *packetData, ULONG packetLength, const NIPB::VProtocolToken &protocol )
{
	ULONG offset = BI2BY(protocol.bitOffset);

	if (offset < packetLength)
	{
		return packetData[offset];
	}
	else
	{
		return 0;
	}
}

/*--------------------------------------------------------------------*/
//! Returns the first two words of the payload
/*!
*/
ULONG ModbusProtocol::GetAddrNCount(		const BYTE *packetData, ULONG packetLength, const NIPB::VProtocolToken &protocol )
{
	ULONG offset = BI2BY(protocol.bitOffset);
	ULONG value;

	if (offset+3 < packetLength)
	{
		value = packetData[offset] << 8 | packetData[offset+1];			// LE/BE
		value <<= 16;
		offset += 2;
		value |= packetData[offset] << 8 | packetData[offset+1];		// LE/BE
		return value;
	}
	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;
}