forked from RoanBrand/MQTT-Siemens-S7-300
-
Notifications
You must be signed in to change notification settings - Fork 0
/
MQTT_Receiving.SCL
152 lines (147 loc) · 4.16 KB
/
MQTT_Receiving.SCL
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
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
FUNCTION mqttAvailable : BOOL
BEGIN
mqttAvailable := (mqttData.pRead <> mqttData.pRec) OR mqttData.RecBufFull;
END_FUNCTION
(**************************************************************)
FUNCTION mqttReadByte : BYTE
BEGIN
mqttReadByte := mqttData.tcpRecBuf[mqttData.pRead];
mqttData.pRead := mqttData.pRead + 1;
IF (mqttData.pRead >= mqttGlobals.TCP_RECVBUFFERSIZE) THEN
mqttData.pRead := 0;
END_IF;
mqttData.RecBufFull := false;
END_FUNCTION
(**************************************************************)
FUNCTION mqttReadByteToBuf : INT
VAR_INPUT
pos : INT;
END_VAR
VAR_TEMP
cur_index : INT;
readed : BYTE;
END_VAR
BEGIN
cur_index := pos;
readed := mqttReadByte();
mqttData.buffer[cur_index] := readed;
cur_index := cur_index + 1;
mqttReadByteToBuf := cur_index;
END_FUNCTION
(**************************************************************)
FUNCTION_BLOCK mqttPacketReader
VAR_IN_OUT
iBegin : BOOL;
END_VAR
VAR
_len : INT;
isPublish : BOOL;
multiplier : DINT;
length : DINT;
digit : BYTE;
start : INT;
State : INT;
oldState : INT;
previousTime : TIME;
END_VAR
VAR_OUTPUT
xDone : BOOL;
result : INT;
lengthLength : INT;
END_VAR
BEGIN
CASE (State) OF
-1: // Alternative to original blocking call
IF ((mqttData.runTime - previousTime) >= mqttGlobals.MQTT_SOCKET_TIMEOUT) THEN
xDone := true;
result := 0;
State := 0;
ELSIF mqttAvailable() THEN
State := oldState;
END_IF;
0 : // Wait for Start
oldState := 0;
IF (iBegin) THEN
iBegin := false;
xDone := false;
_len := 0;
State := 1;
END_IF;
1 : // Getting first byte
oldState := 1;
IF mqttAvailable() THEN
_len := mqttReadByteToBuf(pos := _len);
isPublish := BYTE_TO_INT(mqttData.buffer[0] AND B#16#F0) = mqttGlobals.MQTTPUBLISH;
multiplier := 1;
length := 0;
digit := B#16#00;
start := 0;
State := 2;
ELSE
previousTime := mqttData.runTime;
State := -1;
END_IF;
2 : // do..while((digit & 128) != 0) loop equivalent
oldState := 2;
IF mqttAvailable() THEN
digit := mqttReadByte();
mqttData.buffer[_len] := digit;
_len := _len + 1;
length := length + (BYTE_TO_DINT(digit AND B#16#7F) * multiplier);
multiplier := multiplier * 128;
IF (digit AND B#16#80) = B#16#00 THEN
lengthLength := _len - 1;
IF (isPublish) THEN
State := 3;
ELSE
State := 5;
END_IF;
END_IF;
ELSE
previousTime := mqttData.runTime;
State := -1;
END_IF;
3 : // First Publish Byte
oldState := 3;
IF mqttAvailable() THEN
_len := mqttReadByteToBuf(pos := _len);
State := 4;
ELSE
previousTime := mqttData.runTime;
State := -1;
END_IF;
4 : // Second Publish Byte
oldState := 4;
IF mqttAvailable() THEN
_len := mqttReadByteToBuf(pos := _len);
start := 2;
State := 5;
ELSE
previousTime := mqttData.runTime;
State := -1;
END_IF;
5 : // Read rest of Packet (for loop replacement)
oldState := 5;
//loop exit
IF (start = length) THEN
IF (_len > mqttGlobals.TCP_MAXRECVSIZE) THEN
_len := 0; // just drop packet if too long
END_IF;
xDone := true;
result := _len;
State := 0;
ELSIF mqttAvailable() THEN
digit := mqttReadByte();
IF (_len < mqttGlobals.TCP_MAXRECVSIZE) THEN
mqttData.buffer[_len] := digit;
END_IF;
_len := _len + 1;
//loop
start := start + 1;
ELSE
previousTime := mqttData.runTime;
State := -1;
END_IF;
END_CASE;
END_FUNCTION_BLOCK
(**************************************************************)