-
Notifications
You must be signed in to change notification settings - Fork 3
/
Tpkt.cs
131 lines (122 loc) · 5.33 KB
/
Tpkt.cs
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
/*
* Copyright (C) 2013 Pavel Charvat
*
* This file is part of IEDExplorer.
*
* IEDExplorer is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* IEDExplorer 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
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with IEDExplorer. If not, see <http://www.gnu.org/licenses/>.
*/
using System;
using System.Collections.Generic;
using System.Net;
namespace IEDExplorer
{
/// <summary>
/// TPKT Header parsing according to RFC1006 / OSI (COTP) mapping to TCP/IP
/// </summary>
class Tpkt
{
public const byte TPKT_START = 0x03;
public const byte TPKT_RES = 0x00;
public const int TPKT_MAXLEN = 2048;
public const int TPKT_IDX_START = 0;
public const int TPKT_IDX_RES = 1;
public const int TPKT_IDX_LEN = 2;
public const int TPKT_SIZEOF = 4;
/// <summary>
/// Parsing of data from socket into TPKT datagrams
/// </summary>
/// <param name="iecs">Global protocol state structure</param>
public static void Parse(TcpState tcps)
{
Iec61850State iecs = (Iec61850State)tcps;
for (int i = 0; i < iecs.recvBytes; i++)
{
if (iecs.kstate == TpktState.TPKT_RECEIVE_ERROR)
{
iecs.kstate = TpktState.TPKT_RECEIVE_START;
tcps.logger.LogError("iec61850tpktState.IEC61850_RECEIVE_ERROR\n");
break;
}
switch (iecs.kstate)
{
case TpktState.TPKT_RECEIVE_START:
if (iecs.recvBuffer[i] == TPKT_START)
{
iecs.kstate = TpktState.TPKT_RECEIVE_RES;
iecs.dataBufferIndex = 0;
}
else
{
tcps.logger.LogError("Synchronization lost: TPKT START / VERSION!\n");
iecs.kstate = TpktState.TPKT_RECEIVE_ERROR;
}
break;
case TpktState.TPKT_RECEIVE_RES:
if (iecs.recvBuffer[i] == TPKT_RES)
{
iecs.kstate = TpktState.TPKT_RECEIVE_LEN1;
}
else
{
tcps.logger.LogError("Synchronization lost: TPKT RES!\n");
iecs.kstate = TpktState.TPKT_RECEIVE_ERROR;
}
break;
case TpktState.TPKT_RECEIVE_LEN1:
iecs.TpktLen = iecs.recvBuffer[i] << 8;
iecs.kstate = TpktState.TPKT_RECEIVE_LEN2;
break;
case TpktState.TPKT_RECEIVE_LEN2:
iecs.TpktLen |= iecs.recvBuffer[i];
if (iecs.TpktLen <= TPKT_MAXLEN)
{
iecs.kstate = TpktState.TPKT_RECEIVE_DATA_COPY;
}
else
{
tcps.logger.LogError("Synchronization lost: TPKT TPDU too long!\n");
iecs.kstate = TpktState.TPKT_RECEIVE_ERROR;
}
break;
case TpktState.TPKT_RECEIVE_DATA_COPY:
// OPTIMIZE!!!
//iecs.dataBuffer[iecs.dataBufferIndex++] = iecs.recvBuffer[i];
int copylen = Math.Min(/*available*/iecs.recvBytes - i, /*wanted*/iecs.TpktLen - TPKT_SIZEOF - iecs.dataBufferIndex);
Array.Copy(iecs.recvBuffer, i, iecs.dataBuffer, iecs.dataBufferIndex, copylen);
i += copylen - 1; // i will be incremented in 'for' cycle, so we must decrement here
iecs.dataBufferIndex += copylen;
if (iecs.dataBufferIndex == iecs.TpktLen - TPKT_SIZEOF)
{
iecs.kstate = TpktState.TPKT_RECEIVE_START;
// Call OSI Layer
tcps.logger.LogDebug("TPKT sent to OSI");
iecs.osi.Receive(iecs);
}
break;
default:
tcps.logger.LogError("iecs.tstate: neznamy stav!\n");
break;
} // switch
} // for
}
public static void Send(TcpState tcps)
{
// TPKT
tcps.sendBuffer[Tpkt.TPKT_IDX_START] = Tpkt.TPKT_START;
tcps.sendBuffer[Tpkt.TPKT_IDX_RES] = Tpkt.TPKT_RES;
Array.Copy(BitConverter.GetBytes(IPAddress.HostToNetworkOrder((short)(tcps.sendBytes))), 0, tcps.sendBuffer, Tpkt.TPKT_IDX_LEN, 2);
TcpRw.Send(tcps);
}
}
}