Skip to content

Commit 015d751

Browse files
budulinekbudulinek
committed
Optimize TCP/UDP recieve
1 parent c654d9c commit 015d751

File tree

1 file changed

+39
-50
lines changed

1 file changed

+39
-50
lines changed

‎arduino-modbus-rtu-tcp-gateway/02-modbus-tcp.ino

Lines changed: 39 additions & 50 deletions
Original file line numberDiff line numberDiff line change
@@ -4,12 +4,10 @@
44
recvUdp
55
- receives Modbus UDP (or Modbus RTU over UDP) messages
66
- calls checkRequest
7-
- stores requests in queue or replies with error
87
98
recvTcp
109
- receives Modbus TCP (or Modbus RTU over TCP) messages
1110
- calls checkRequest
12-
- stores requests in queue or replies with error
1311
1412
processRequests
1513
- inserts scan request into queue
@@ -18,6 +16,7 @@
1816
checkRequest
1917
- checks Modbus TCP/UDP requests (correct MBAP header, CRC in case of Modbus RTU over TCP/UDP)
2018
- checks availability of queue
19+
- stores requests in queue or returns an error
2120
2221
deleteRequest
2322
- deletes requests from queue
@@ -44,44 +43,37 @@ typedef struct {
4443
CircularBuffer<header, reqQueueCount> queueHeaders; // queue of requests' headers and metadata (MBAP transaction ID, MBAP unit ID, PDU length, remIP, remPort, TCP client)
4544
CircularBuffer<byte, reqQueueSize> queuePDUs; // queue of PDU data (function code, data)
4645
CircularBuffer<byte, reqQueueCount> queueRetries; // queue of retry counters
46+
byte pduStart; // first byte of Protocol Data Unit (i.e. Function code)
4747

4848
void recvUdp() {
4949
unsigned int packetSize = Udp.parsePacket();
5050
if (packetSize) {
5151
#ifdef ENABLE_EXTRA_DIAG
5252
ethRxCount += packetSize;
5353
#endif /* ENABLE_EXTRA_DIAG */
54-
byte udpInBuffer[modbusSize + 4]; // Modbus TCP frame is 4 bytes longer than Modbus RTU frame
55-
// Modbus TCP/UDP frame: [0][1] transaction ID, [2][3] protocol ID, [4][5] length and [6] unit ID (address).....
56-
// Modbus RTU frame: [0] address.....
57-
Udp.read(udpInBuffer, sizeof(udpInBuffer));
54+
byte inBuffer[modbusSize + 4]; // Modbus TCP frame is 4 bytes longer than Modbus RTU frame
55+
// Modbus TCP/UDP frame: [0][1] transaction ID, [2][3] protocol ID, [4][5] length and [6] unit ID (address)..... no CRC
56+
// Modbus RTU frame: [0] address.....[n-1][n] CRC
57+
Udp.read(inBuffer, sizeof(inBuffer));
5858
Udp.flush();
59-
60-
byte errorCode = checkRequest(udpInBuffer, packetSize);
61-
byte pduStart; // first byte of Protocol Data Unit (i.e. Function code)
6259
if (localConfig.enableRtuOverTcp) pduStart = 1; // In Modbus RTU, Function code is second byte (after address)
6360
else pduStart = 7; // In Modbus TCP/UDP, Function code is 8th byte (after address)
64-
if (errorCode == 0) {
65-
// Store in request queue: 2 bytes MBAP Transaction ID (ignored in Modbus RTU over TCP); MBAP Unit ID (address); PDUlen (func + data);remote IP; remote port; TCP client Number (socket) - 0xFF for UDP
66-
queueHeaders.push(header{ { udpInBuffer[0], udpInBuffer[1] }, udpInBuffer[pduStart - 1], (byte)(packetSize - pduStart), Udp.remoteIP(), Udp.remotePort(), UDP_REQUEST });
67-
queueRetries.push(0);
68-
for (byte i = 0; i < (byte)(packetSize - pduStart); i++) {
69-
queuePDUs.push(udpInBuffer[i + pduStart]);
70-
}
71-
} else if (errorCode != 0xFF) {
61+
62+
byte errorCode = checkRequest(inBuffer, packetSize, (IPAddress)Udp.remoteIP(), Udp.remotePort(), UDP_REQUEST);
63+
if (errorCode) {
7264
// send back message with error code
7365
Udp.beginPacket(Udp.remoteIP(), Udp.remotePort());
7466
if (!localConfig.enableRtuOverTcp) {
75-
Udp.write(udpInBuffer, 5);
67+
Udp.write(inBuffer, 5);
7668
Udp.write(0x03);
7769
}
78-
Udp.write(udpInBuffer[pduStart - 1]); // address
79-
Udp.write(udpInBuffer[pduStart] + 0x80); // function + 0x80
70+
Udp.write(inBuffer[pduStart - 1]); // address
71+
Udp.write(inBuffer[pduStart] + 0x80); // function + 0x80
8072
Udp.write(errorCode);
8173
if (localConfig.enableRtuOverTcp) {
8274
crc = 0xFFFF;
83-
calculateCRC(udpInBuffer[pduStart - 1]);
84-
calculateCRC(udpInBuffer[pduStart] + 0x80);
75+
calculateCRC(inBuffer[pduStart - 1]);
76+
calculateCRC(inBuffer[pduStart] + 0x80);
8577
calculateCRC(errorCode);
8678
Udp.write(lowByte(crc)); // send CRC, low byte first
8779
Udp.write(highByte(crc));
@@ -95,43 +87,36 @@ void recvUdp() {
9587
}
9688
}
9789

98-
9990
void recvTcp() {
10091
EthernetClient client = modbusServer.available();
10192
if (client) {
10293
unsigned int packetSize = client.available();
10394
#ifdef ENABLE_EXTRA_DIAG
10495
ethRxCount += packetSize;
10596
#endif /* ENABLE_EXTRA_DIAG */
106-
byte tcpInBuffer[modbusSize + 4]; // Modbus TCP frame is 4 bytes longer than Modbus RTU frame
97+
byte inBuffer[modbusSize + 4]; // Modbus TCP frame is 4 bytes longer than Modbus RTU frame
10798
// Modbus TCP/UDP frame: [0][1] transaction ID, [2][3] protocol ID, [4][5] length and [6] unit ID (address).....
10899
// Modbus RTU frame: [0] address.....
109-
client.read(tcpInBuffer, sizeof(tcpInBuffer));
100+
client.read(inBuffer, sizeof(inBuffer));
110101
client.flush();
111-
byte errorCode = checkRequest(tcpInBuffer, packetSize);
112-
byte pduStart; // first byte of Protocol Data Unit (i.e. Function code)
102+
113103
if (localConfig.enableRtuOverTcp) pduStart = 1; // In Modbus RTU, Function code is second byte (after address)
114104
else pduStart = 7; // In Modbus TCP/UDP, Function code is 8th byte (after address)
115-
if (errorCode == 0) {
116-
// Store in request queue: 2 bytes MBAP Transaction ID (ignored in Modbus RTU over TCP); MBAP Unit ID (address); PDUlen (func + data);remote IP; remote port; TCP client Number (socket) - 0xFF for UDP
117-
queueHeaders.push(header{ { tcpInBuffer[0], tcpInBuffer[1] }, tcpInBuffer[pduStart - 1], (byte)(packetSize - pduStart), {}, 0, client.getSocketNumber() });
118-
queueRetries.push(0);
119-
for (byte i = 0; i < packetSize - pduStart; i++) {
120-
queuePDUs.push(tcpInBuffer[i + pduStart]);
121-
}
122-
} else if (errorCode != 0xFF) {
105+
106+
byte errorCode = checkRequest(inBuffer, packetSize, (IPAddress){}, 0, client.getSocketNumber());
107+
if (errorCode) {
123108
// send back message with error code
124109
if (!localConfig.enableRtuOverTcp) {
125-
client.write(tcpInBuffer, 5);
110+
client.write(inBuffer, 5);
126111
client.write(0x03);
127112
}
128-
client.write(tcpInBuffer[pduStart - 1]); // address
129-
client.write(tcpInBuffer[pduStart] + 0x80); // function + 0x80
113+
client.write(inBuffer[pduStart - 1]); // address
114+
client.write(inBuffer[pduStart] + 0x80); // function + 0x80
130115
client.write(errorCode);
131116
if (localConfig.enableRtuOverTcp) {
132117
crc = 0xFFFF;
133-
calculateCRC(tcpInBuffer[pduStart - 1]);
134-
calculateCRC(tcpInBuffer[pduStart] + 0x80);
118+
calculateCRC(inBuffer[pduStart - 1]);
119+
calculateCRC(inBuffer[pduStart] + 0x80);
135120
calculateCRC(errorCode);
136121
client.write(lowByte(crc)); // send CRC, low byte first
137122
client.write(highByte(crc));
@@ -156,7 +141,6 @@ void processRequests() {
156141
scanCounter++;
157142
if (scanCounter == maxSlaves + 1) scanCounter = 0;
158143
}
159-
160144
// Optimize queue (prioritize requests from responding slaves) and trigger sending via serial
161145
if (serialState == IDLE) { // send new data over serial only if we are not waiting for response
162146
if (!queueHeaders.isEmpty()) {
@@ -174,25 +158,30 @@ void processRequests() {
174158
}
175159
}
176160

177-
byte checkRequest(byte buffer[], unsigned int bufferSize) {
161+
byte checkRequest(byte inBuffer[], unsigned int packetSize, IPAddress remoteIP, unsigned int remotePort, byte clientNum) {
178162
byte address;
179-
if (localConfig.enableRtuOverTcp) address = buffer[0];
180-
else address = buffer[6];
181-
163+
if (localConfig.enableRtuOverTcp) address = inBuffer[0];
164+
else address = inBuffer[6];
182165
if (localConfig.enableRtuOverTcp) { // check CRC for Modbus RTU over TCP/UDP
183-
if (checkCRC(buffer, bufferSize) == false) {
184-
return 0xFF; // reject: do nothing and return no error code
166+
if (checkCRC(inBuffer, packetSize) == false) {
167+
return 0; // reject: do nothing and return no error code
185168
}
186169
} else { // check MBAP header structure for Modbus TCP/UDP
187-
if (buffer[2] != 0x00 || buffer[3] != 0x00 || buffer[4] != 0x00 || buffer[5] != bufferSize - 6) {
188-
return 0xFF; // reject: do nothing and return no error code
170+
if (inBuffer[2] != 0x00 || inBuffer[3] != 0x00 || inBuffer[4] != 0x00 || inBuffer[5] != packetSize - 6) {
171+
return 0; // reject: do nothing and return no error code
189172
}
190173
}
191174
// check if we have space in request queue
192-
if (queueHeaders.available() < 1 || (localConfig.enableRtuOverTcp && queuePDUs.available() < bufferSize - 1) || (!localConfig.enableRtuOverTcp && queuePDUs.available() < bufferSize - 7)) {
175+
if (queueHeaders.available() < 1 || (localConfig.enableRtuOverTcp && queuePDUs.available() < packetSize - 1) || (!localConfig.enableRtuOverTcp && queuePDUs.available() < packetSize - 7)) {
193176
return 0x06; // return modbus error 6 (Slave Device Busy) - try again later
194177
}
195178
// al checkes passed OK, we can store the incoming data in request queue
179+
// Store in request queue: 2 bytes MBAP Transaction ID (ignored in Modbus RTU over TCP); MBAP Unit ID (address); PDUlen (func + data);remote IP; remote port; TCP client Number (socket) - 0xFF for UDP
180+
queueHeaders.push(header{ { inBuffer[0], inBuffer[1] }, inBuffer[pduStart - 1], (byte)(packetSize - pduStart), (IPAddress)remoteIP, (unsigned int)remotePort, (byte)clientNum });
181+
queueRetries.push(0);
182+
for (byte i = 0; i < packetSize - pduStart; i++) {
183+
queuePDUs.push(inBuffer[i + pduStart]);
184+
}
196185
return 0;
197186
}
198187

0 commit comments

Comments
 (0)