Read exactly one modbus message when reading from the fd

webserver/core/ladder.h

@@ -143,6 +143,7 @@ extern time_t start_time;
 extern time_t end_time;
 
 //modbus.cpp
+int readModbusMessage(int fd, unsigned char *buffer, size_t bufferSize);
 int processModbusMessage(unsigned char *buffer, int bufferSize);
 void mapUnusedIO();
 

webserver/core/modbus.cpp

@@ -1052,6 +1052,49 @@ void debugGetMd5(unsigned char *mb_frame, void *endianness)
     MessageLength = md5_len + 9;
 }
 
+//-----------------------------------------------------------------------------
+// Read one complete modbus message from the blocking file descriptor `fd`
+// into the `buffer`, which has a size of `bufferSize`.
+// If the message is too large only `bufferSize` bytes will be read.
+// Returns the number of bytes of the message that was read.
+//-----------------------------------------------------------------------------
+int readModbusMessage(int fd, unsigned char *buffer, size_t bufferSize)
+{
+    int messageSize = 0;
+    // Read the modbus TCP/IP ADU frame header up to the length field.
+#define MODBUS_HEADER_SIZE 6
+    if (bufferSize < MODBUS_HEADER_SIZE)
+    {
+        return -1;
+    }
+    do
+    {
+        int bytesRead = read(fd, buffer + messageSize, MODBUS_HEADER_SIZE - messageSize);
+        if (bytesRead <= 0)
+        {
+            return bytesRead;
+        }
+        messageSize += bytesRead;
+    } while (messageSize < MODBUS_HEADER_SIZE);
+
+    // Read the length (byte 5 & 6).
+    uint16_t length = le16toh(((u_int16_t *)buffer)[2]);
+    size_t totalMessageSize = MODBUS_HEADER_SIZE + length;
+
+    // Read the rest of the message.
+    while (messageSize < MODBUS_HEADER_SIZE + length)
+    {
+        int bytesRead = read(fd, buffer + messageSize, (totalMessageSize > bufferSize ? bufferSize : totalMessageSize) - messageSize);
+        if (bytesRead <= 0)
+        {
+            return bytesRead;
+        }
+        messageSize += bytesRead;
+    }
+
+    return messageSize;
+}
+
 //-----------------------------------------------------------------------------
 // This function must parse and process the client request and write back the
 // response for it. The return value is the size of the response message in

webserver/core/server.cpp

@@ -218,7 +218,14 @@ void *handleConnections(void *arguments)
         //unsigned char buffer[NET_BUFFER_SIZE];
         //int messageSize;
 
-        messageSize = listenToClient(client_fd, buffer);
+        if (protocol_type == MODBUS_PROTOCOL)
+        {
+            messageSize = readModbusMessage(client_fd, buffer, sizeof(buffer) / sizeof(buffer[0]));
+        }
+        else
+        {
+            messageSize = listenToClient(client_fd, buffer);
+        }
         if (messageSize <= 0 || messageSize > NET_BUFFER_SIZE)
         {
             // something has  gone wrong or the client has closed connection