542 lines
16 KiB
C++
542 lines
16 KiB
C++
/*=============================================================================|
|
|
| PROJECT SNAP7 1.3.0 |
|
|
|==============================================================================|
|
|
| Copyright (C) 2013, 2015 Davide Nardella |
|
|
| All rights reserved. |
|
|
|==============================================================================|
|
|
| SNAP7 is free software: you can redistribute it and/or modify |
|
|
| it under the terms of the Lesser GNU General Public License as published by |
|
|
| the Free Software Foundation, either version 3 of the License, or |
|
|
| (at your option) any later version. |
|
|
| |
|
|
| It means that you can distribute your commercial software linked with |
|
|
| SNAP7 without the requirement to distribute the source code of your |
|
|
| application and without the requirement that your application be itself |
|
|
| distributed under LGPL. |
|
|
| |
|
|
| SNAP7 is distributed in the hope that it will be useful, |
|
|
| but WITHOUT ANY WARRANTY; without even the implied warranty of |
|
|
| MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
|
|
| Lesser GNU General Public License for more details. |
|
|
| |
|
|
| You should have received a copy of the GNU General Public License and a |
|
|
| copy of Lesser GNU General Public License along with Snap7. |
|
|
| If not, see http://www.gnu.org/licenses/ |
|
|
|=============================================================================*/
|
|
#include "s7_isotcp.h"
|
|
//---------------------------------------------------------------------------
|
|
TIsoTcpSocket::TIsoTcpSocket()
|
|
{
|
|
RecvTimeout = 3000; // Some old equipments are a bit slow to answer....
|
|
RemotePort = isoTcpPort;
|
|
// These fields should be $0000 and in any case RFC says that they are not considered.
|
|
// But some equipment...need a non zero value for the source reference.
|
|
DstRef = 0x0000;
|
|
SrcRef = 0x0100;
|
|
// PDU size requested
|
|
IsoPDUSize =1024;
|
|
IsoMaxFragments=MaxIsoFragments;
|
|
LastIsoError=0;
|
|
}
|
|
//---------------------------------------------------------------------------
|
|
TIsoTcpSocket::~TIsoTcpSocket()
|
|
{
|
|
}
|
|
//---------------------------------------------------------------------------
|
|
int TIsoTcpSocket::CheckPDU(void *pPDU, u_char PduTypeExpected)
|
|
{
|
|
PIsoHeaderInfo Info;
|
|
int Size;
|
|
ClrIsoError();
|
|
if (pPDU!=0)
|
|
{
|
|
Info = PIsoHeaderInfo(pPDU);
|
|
Size = PDUSize(pPDU);
|
|
// Performs check
|
|
if (( Size<7 ) || ( Size>IsoPayload_Size ) || // Checks RFC 1006 header length
|
|
( Info->HLength<sizeof( TCOTP_DT )-1 ) || // Checks ISO 8073 header length
|
|
( Info->PDUType!=PduTypeExpected)) // Checks PDU Type
|
|
return SetIsoError(errIsoInvalidPDU);
|
|
else
|
|
return noError;
|
|
}
|
|
else
|
|
return SetIsoError(errIsoNullPointer);
|
|
}
|
|
//---------------------------------------------------------------------------
|
|
int TIsoTcpSocket::SetIsoError(int Error)
|
|
{
|
|
LastIsoError = Error | LastTcpError;
|
|
return LastIsoError;
|
|
}
|
|
//---------------------------------------------------------------------------
|
|
void TIsoTcpSocket::ClrIsoError()
|
|
{
|
|
LastIsoError=0;
|
|
LastTcpError=0;
|
|
}
|
|
//---------------------------------------------------------------------------
|
|
int TIsoTcpSocket::BuildControlPDU()
|
|
{
|
|
int ParLen, IsoLen;
|
|
|
|
ClrIsoError();
|
|
FControlPDU.COTP.Params.PduSizeCode=0xC0; // code that identifies TPDU size
|
|
FControlPDU.COTP.Params.PduSizeLen =0x01; // 1 byte this field
|
|
switch(IsoPDUSize)
|
|
{
|
|
case 128:
|
|
FControlPDU.COTP.Params.PduSizeVal =0x07;
|
|
break;
|
|
case 256:
|
|
FControlPDU.COTP.Params.PduSizeVal =0x08;
|
|
break;
|
|
case 512:
|
|
FControlPDU.COTP.Params.PduSizeVal =0x09;
|
|
break;
|
|
case 1024:
|
|
FControlPDU.COTP.Params.PduSizeVal =0x0A;
|
|
break;
|
|
case 2048:
|
|
FControlPDU.COTP.Params.PduSizeVal =0x0B;
|
|
break;
|
|
case 4096:
|
|
FControlPDU.COTP.Params.PduSizeVal =0x0C;
|
|
break;
|
|
case 8192:
|
|
FControlPDU.COTP.Params.PduSizeVal =0x0D;
|
|
break;
|
|
default:
|
|
FControlPDU.COTP.Params.PduSizeVal =0x0B; // Our Default
|
|
};
|
|
// Build TSAPs
|
|
FControlPDU.COTP.Params.TSAP[0]=0xC1; // code that identifies source TSAP
|
|
FControlPDU.COTP.Params.TSAP[1]=2; // source TSAP Len
|
|
FControlPDU.COTP.Params.TSAP[2]=(SrcTSap>>8) & 0xFF; // HI part
|
|
FControlPDU.COTP.Params.TSAP[3]=SrcTSap & 0xFF; // LO part
|
|
|
|
FControlPDU.COTP.Params.TSAP[4]=0xC2; // code that identifies dest TSAP
|
|
FControlPDU.COTP.Params.TSAP[5]=2; // dest TSAP Len
|
|
FControlPDU.COTP.Params.TSAP[6]=(DstTSap>>8) & 0xFF; // HI part
|
|
FControlPDU.COTP.Params.TSAP[7]=DstTSap & 0xFF; // LO part
|
|
|
|
// Params length
|
|
ParLen=11; // 2 Src TSAP (Code+field Len) +
|
|
// 2 Src TSAP len +
|
|
// 2 Dst TSAP (Code+field Len) +
|
|
// 2 Src TSAP len +
|
|
// 3 PDU size (Code+field Len+Val) = 11
|
|
// Telegram length
|
|
IsoLen=sizeof(TTPKT)+ // TPKT Header
|
|
7 + // COTP Header Size without params
|
|
ParLen; // COTP params
|
|
|
|
FControlPDU.TPKT.Version =isoTcpVersion;
|
|
FControlPDU.TPKT.Reserved =0;
|
|
FControlPDU.TPKT.HI_Lenght=0; // Connection Telegram size cannot exced 255 bytes, so
|
|
// this field is always 0
|
|
FControlPDU.TPKT.LO_Lenght=IsoLen;
|
|
|
|
FControlPDU.COTP.HLength =ParLen + 6; // <-- 6 = 7 - 1 (COTP Header size - 1)
|
|
FControlPDU.COTP.PDUType =pdu_type_CR; // Connection Request
|
|
FControlPDU.COTP.DstRef =DstRef; // Destination reference
|
|
FControlPDU.COTP.SrcRef =SrcRef; // Source reference
|
|
FControlPDU.COTP.CO_R =0x00; // Class + Option : RFC0983 states that it must be always 0x40
|
|
// but for some equipment (S7) must be 0 in disaccord of specifications !!!
|
|
return noError;
|
|
}
|
|
//---------------------------------------------------------------------------
|
|
int TIsoTcpSocket::PDUSize(void *pPDU)
|
|
{
|
|
return PIsoHeaderInfo(pPDU)->TPKT.HI_Lenght*256+PIsoHeaderInfo( pPDU )->TPKT.LO_Lenght;
|
|
}
|
|
//---------------------------------------------------------------------------
|
|
void TIsoTcpSocket::IsoParsePDU(TIsoControlPDU pdu)
|
|
{
|
|
// Currently we accept a connection with any kind of src/dst tsap
|
|
// Override to implement special filters.
|
|
}
|
|
//---------------------------------------------------------------------------
|
|
int TIsoTcpSocket::IsoConfirmConnection(u_char PDUType)
|
|
{
|
|
PIsoControlPDU CPDU = PIsoControlPDU(&PDU);
|
|
u_short TempRef;
|
|
|
|
ClrIsoError();
|
|
PDU.COTP.PDUType=PDUType;
|
|
// Exchange SrcRef<->DstRef, not strictly needed by COTP 8073 but S7PLC as client needs it.
|
|
TempRef=CPDU->COTP.DstRef;
|
|
CPDU->COTP.DstRef=CPDU->COTP.SrcRef;
|
|
CPDU->COTP.SrcRef=0x0100;//TempRef;
|
|
|
|
return SendPacket(&PDU,PDUSize(&PDU));
|
|
}
|
|
//---------------------------------------------------------------------------
|
|
void TIsoTcpSocket::FragmentSkipped(int Size)
|
|
{
|
|
// override for log purpose
|
|
}
|
|
//---------------------------------------------------------------------------
|
|
int TIsoTcpSocket::isoConnect()
|
|
{
|
|
pbyte TmpControlPDU;
|
|
PIsoControlPDU ControlPDU;
|
|
u_int Length;
|
|
int Result;
|
|
|
|
// Build the default connection telegram
|
|
BuildControlPDU();
|
|
ControlPDU =&FControlPDU;
|
|
|
|
// Checks the format
|
|
Result =CheckPDU(ControlPDU, pdu_type_CR);
|
|
if (Result!=0)
|
|
return Result;
|
|
|
|
Result =SckConnect();
|
|
if (Result==noError)
|
|
{
|
|
// Calcs the length
|
|
Length =PDUSize(ControlPDU);
|
|
// Send connection telegram
|
|
SendPacket(ControlPDU, Length);
|
|
if (LastTcpError==0)
|
|
{
|
|
TmpControlPDU = pbyte(ControlPDU);
|
|
// Receives TPKT header (4 bytes)
|
|
RecvPacket(TmpControlPDU, sizeof(TTPKT));
|
|
if (LastTcpError==0)
|
|
{
|
|
// Calc the packet length
|
|
Length =PDUSize(TmpControlPDU);
|
|
// Check if it fits in the buffer and if it's greater then TTPKT size
|
|
if ((Length<=sizeof(TIsoControlPDU)) && (Length>sizeof(TTPKT)))
|
|
{
|
|
// Points to COTP
|
|
TmpControlPDU+=sizeof(TTPKT);
|
|
Length -= sizeof(TTPKT);
|
|
// Receives remainin bytes 4 bytes after
|
|
RecvPacket(TmpControlPDU, Length);
|
|
if (LastTcpError==0)
|
|
{
|
|
// Finally checks the Connection Confirm telegram
|
|
Result =CheckPDU(ControlPDU, pdu_type_CC);
|
|
if (Result!=0)
|
|
LastIsoError=Result;
|
|
}
|
|
else
|
|
Result =SetIsoError(errIsoRecvPacket);
|
|
}
|
|
else
|
|
Result =SetIsoError(errIsoInvalidPDU);
|
|
}
|
|
else
|
|
Result =SetIsoError(errIsoRecvPacket);
|
|
// Flush buffer
|
|
if (Result!=0)
|
|
Purge();
|
|
}
|
|
else
|
|
Result =SetIsoError(errIsoSendPacket);
|
|
|
|
if (Result!=0)
|
|
SckDisconnect();
|
|
}
|
|
return Result;
|
|
}
|
|
//---------------------------------------------------------------------------
|
|
int TIsoTcpSocket::isoSendBuffer(void *Data, int Size)
|
|
{
|
|
int Result;
|
|
u_int IsoSize;
|
|
|
|
ClrIsoError();
|
|
// Total Size = Size + Header Size
|
|
IsoSize =Size+DataHeaderSize;
|
|
// Checks the length
|
|
if ((IsoSize>0) && (IsoSize<=IsoFrameSize))
|
|
{
|
|
// Builds the header
|
|
Result =0;
|
|
// TPKT
|
|
PDU.TPKT.Version = isoTcpVersion;
|
|
PDU.TPKT.Reserved = 0;
|
|
PDU.TPKT.HI_Lenght= (u_short(IsoSize)>> 8) & 0xFF;
|
|
PDU.TPKT.LO_Lenght= u_short(IsoSize) & 0xFF;
|
|
// COPT
|
|
PDU.COTP.HLength =sizeof(TCOTP_DT)-1;
|
|
PDU.COTP.PDUType =pdu_type_DT;
|
|
PDU.COTP.EoT_Num =pdu_EoT;
|
|
// Fill payload
|
|
if (Data!=0) // Data=null ==> use internal buffer PDU.Payload
|
|
memcpy(&PDU.Payload, Data, Size);
|
|
// Send over TCP/IP
|
|
SendPacket(&PDU, IsoSize);
|
|
|
|
if (LastTcpError!=0)
|
|
Result =SetIsoError(errIsoSendPacket);
|
|
}
|
|
else
|
|
Result =SetIsoError(errIsoInvalidDataSize );
|
|
return Result;
|
|
}
|
|
//---------------------------------------------------------------------------
|
|
int TIsoTcpSocket::isoRecvBuffer(void *Data, int & Size)
|
|
{
|
|
int Result;
|
|
|
|
ClrIsoError();
|
|
Size =0;
|
|
Result =isoRecvPDU(&PDU);
|
|
if (Result==0)
|
|
{
|
|
Size =PDUSize( &PDU )-DataHeaderSize;
|
|
if (Data!=0) // Data=NULL ==> a child will consume directly PDY.Payload
|
|
memcpy(Data, &PDU.Payload, Size);
|
|
}
|
|
return Result;
|
|
}
|
|
//---------------------------------------------------------------------------
|
|
int TIsoTcpSocket::isoExchangeBuffer(void *Data, int &Size)
|
|
{
|
|
int Result;
|
|
|
|
ClrIsoError();
|
|
Result =isoSendBuffer(Data, Size);
|
|
if (Result==0)
|
|
Result =isoRecvBuffer(Data, Size);
|
|
return Result;
|
|
}
|
|
//---------------------------------------------------------------------------
|
|
bool TIsoTcpSocket::IsoPDUReady()
|
|
{
|
|
ClrIsoError();
|
|
return PacketReady(sizeof(TCOTP_DT));
|
|
}
|
|
//---------------------------------------------------------------------------
|
|
int TIsoTcpSocket::isoDisconnect(bool OnlyTCP)
|
|
{
|
|
int Result;
|
|
|
|
ClrIsoError();
|
|
if (Connected)
|
|
Purge(); // Flush pending
|
|
LastIsoError=0;
|
|
// OnlyTCP true -> Disconnect Request telegram is not required : only TCP disconnection
|
|
if (!OnlyTCP)
|
|
{
|
|
// if we are connected -> we have a valid connection telegram
|
|
if (Connected)
|
|
FControlPDU.COTP.PDUType =pdu_type_DR;
|
|
// Checks the format
|
|
Result =CheckPDU(&FControlPDU, pdu_type_DR);
|
|
if (Result!=0)
|
|
return Result;
|
|
// Sends Disconnect request
|
|
SendPacket(&FControlPDU, PDUSize(&FControlPDU));
|
|
if (LastTcpError!=0)
|
|
{
|
|
Result =SetIsoError(errIsoSendPacket);
|
|
return Result;
|
|
}
|
|
}
|
|
// TCP disconnect
|
|
SckDisconnect();
|
|
if (LastTcpError!=0)
|
|
Result =SetIsoError(errIsoDisconnect);
|
|
else
|
|
Result =0;
|
|
|
|
return Result;
|
|
}
|
|
//---------------------------------------------------------------------------
|
|
int TIsoTcpSocket::isoSendPDU(PIsoDataPDU Data)
|
|
{
|
|
int Result;
|
|
|
|
ClrIsoError();
|
|
Result=CheckPDU(Data,pdu_type_DT);
|
|
if (Result==0)
|
|
{
|
|
SendPacket(Data,PDUSize(Data));
|
|
if (LastTcpError!=0)
|
|
Result=SetIsoError(errIsoSendPacket);
|
|
}
|
|
return Result;
|
|
}
|
|
//------------------------------------------------------------------------------
|
|
int TIsoTcpSocket::isoRecvFragment(void *From, int Max, int &Size, bool &EoT)
|
|
{
|
|
int DataLength;
|
|
|
|
Size =0;
|
|
EoT =false;
|
|
byte PDUType;
|
|
ClrIsoError();
|
|
// header is received always from beginning
|
|
RecvPacket(&PDU, DataHeaderSize); // TPKT + COPT_DT
|
|
if (LastTcpError==0)
|
|
{
|
|
PDUType=PDU.COTP.PDUType;
|
|
switch (PDUType)
|
|
{
|
|
case pdu_type_CR:
|
|
case pdu_type_DR:
|
|
EoT=true;
|
|
break;
|
|
case pdu_type_DT:
|
|
EoT = (PDU.COTP.EoT_Num & 0x80) == 0x80; // EoT flag
|
|
break;
|
|
default:
|
|
return SetIsoError(errIsoInvalidPDU);
|
|
}
|
|
|
|
DataLength = PDUSize(&PDU) - DataHeaderSize;
|
|
if (CheckPDU(&PDU, PDUType)!=0)
|
|
return LastIsoError;
|
|
// Checks for data presence
|
|
if (DataLength>0) // payload present
|
|
{
|
|
// Check if the data fits in the buffer
|
|
if(DataLength<=Max)
|
|
{
|
|
RecvPacket(From, DataLength);
|
|
if (LastTcpError!=0)
|
|
return SetIsoError(errIsoRecvPacket);
|
|
else
|
|
Size =DataLength;
|
|
}
|
|
else
|
|
return SetIsoError(errIsoPduOverflow);
|
|
}
|
|
}
|
|
else
|
|
return SetIsoError(errIsoRecvPacket);
|
|
|
|
return LastIsoError;
|
|
}
|
|
//---------------------------------------------------------------------------
|
|
// Fragments Recv schema
|
|
//------------------------------------------------------------------------------
|
|
//
|
|
// packet 1 packet 2 packet 3
|
|
// +--------+------------+ +--------+------------+ +--------+------------+
|
|
// | HEADER | FRAGMENT 1 | | HEADER | FRAGMENT 2 | | HEADER | FRAGMENT 3 |
|
|
// +--------+------------+ +--------+------------+ +--------+------------+
|
|
// | | |
|
|
// | +-----------+ |
|
|
// | | |
|
|
// | | +------------------------+
|
|
// | | | (Packet 3 has EoT Flag set)
|
|
// V V V
|
|
// +--------+------------+------------+------------+
|
|
// | HEADER | FRAGMENT 1 : FRAGMENT 2 : FRAGMENT 3 |
|
|
// +--------+------------+------------+------------+
|
|
// ^
|
|
// |
|
|
// +-- A new header is built with updated info
|
|
//
|
|
//------------------------------------------------------------------------------
|
|
int TIsoTcpSocket::isoRecvPDU(PIsoDataPDU Data)
|
|
{
|
|
int Result;
|
|
int Size;
|
|
pbyte pData;
|
|
int max;
|
|
int Offset;
|
|
int Received;
|
|
int NumParts;
|
|
bool Complete;
|
|
|
|
NumParts =1;
|
|
Offset =0;
|
|
Complete =false;
|
|
ClrIsoError();
|
|
pData = pbyte(&PDU.Payload);
|
|
do {
|
|
pData=pData+Offset;
|
|
max =IsoPayload_Size-Offset; // Maximum packet allowed
|
|
if (max>0)
|
|
{
|
|
Result =isoRecvFragment(pData, max, Received, Complete);
|
|
if((Result==0) && !Complete)
|
|
{
|
|
++NumParts;
|
|
Offset += Received;
|
|
if (NumParts>IsoMaxFragments)
|
|
Result =SetIsoError(errIsoTooManyFragments);
|
|
}
|
|
}
|
|
else
|
|
Result =SetIsoError(errIsoTooManyFragments);
|
|
} while ((!Complete) && (Result==0));
|
|
|
|
|
|
if (Result==0)
|
|
{
|
|
// Add to offset the header size
|
|
Size =Offset+Received+DataHeaderSize;
|
|
// Adjust header
|
|
PDU.TPKT.HI_Lenght =(u_short(Size)>>8) & 0xFF;
|
|
PDU.TPKT.LO_Lenght =u_short(Size) & 0xFF;
|
|
// Copies data if target is not the local PDU
|
|
if (Data!=&PDU)
|
|
memcpy(Data, &PDU, Size);
|
|
}
|
|
else
|
|
if (LastTcpError!=WSAECONNRESET)
|
|
Purge();
|
|
return Result;
|
|
}
|
|
//---------------------------------------------------------------------------
|
|
int TIsoTcpSocket::isoExchangePDU(PIsoDataPDU Data)
|
|
{
|
|
int Result;
|
|
ClrIsoError();
|
|
Result=isoSendPDU(Data);
|
|
if (Result==0)
|
|
Result=isoRecvPDU(Data);
|
|
return Result;
|
|
}
|
|
//---------------------------------------------------------------------------
|
|
void TIsoTcpSocket::IsoPeek(void *pPDU, TPDUKind &PduKind)
|
|
{
|
|
PIsoHeaderInfo Info;
|
|
u_int IsoLen;
|
|
|
|
Info=PIsoHeaderInfo(pPDU);
|
|
IsoLen=PDUSize(Info);
|
|
|
|
// Check for empty fragment : size of PDU = size of header and nothing else
|
|
if (IsoLen==DataHeaderSize )
|
|
{
|
|
// We don't need to check the EoT flag since the PDU is empty....
|
|
PduKind=pkEmptyFragment;
|
|
return;
|
|
};
|
|
// Check for invalid packet : size of PDU < size of header
|
|
if (IsoLen<DataHeaderSize )
|
|
{
|
|
PduKind=pkInvalidPDU;
|
|
return;
|
|
};
|
|
// Here IsoLen>DataHeaderSize : check the PDUType
|
|
switch (Info->PDUType)
|
|
{
|
|
case pdu_type_CR:
|
|
PduKind=pkConnectionRequest;
|
|
break;
|
|
case pdu_type_DR:
|
|
PduKind=pkDisconnectRequest;
|
|
break;
|
|
case pdu_type_DT:
|
|
PduKind=pkValidData;
|
|
break;
|
|
default:
|
|
PduKind=pkUnrecognizedType;
|
|
};
|
|
}
|
|
|
|
|
|
|