Modbus TCP receive bugfix

otherio/lib/rt/src/os_linux/rt_io_m_mb_tcp_slave.c

@@ -65,7 +65,7 @@
 
 #include "rt_io_mb_locals.h"
 
-char rcv_buffer[65536];
+//char rcv_buffer[65536];
 
 /* Check if channel should be fetched from diagnostic area, 
    i.e. channel name starts with "Diag_" */
@@ -110,6 +110,7 @@ static int connect_slave( io_sRackLocal *local, io_sRack *rp)
     errh_Error( "Address error for IO modbus tcp slave %s %s", rp->Name, op->Address);
     return sts;
   }
+  local->rem_addr.sin_addr.s_addr = ntohl(local->rem_addr.sin_addr.s_addr);
   
   /* Connect to remote address */
 
@@ -131,7 +132,7 @@ static int connect_slave( io_sRackLocal *local, io_sRack *rp)
       tv.tv_usec = 200000;
     }
 
-    sts = select(32, &fdr, &fdw, NULL, &tv);
+    sts = select((int)local->s+1, &fdr, &fdw, NULL, &tv);
     
     if (sts <= 0) {
       close(local->s);
@@ -154,12 +155,11 @@ pwr_tStatus mb_recv_data(io_sRackLocal *local,
   pwr_sClass_Modbus_ModuleMsg *mp;
   pwr_tStatus sts;
   fd_set fdr;				/* For select call */
-  fd_set fde;				/* For select call */
-  fd_set fdw;				/* For select call */
   struct timeval tv;
   pwr_tBoolean found;
   int data_size;
   rec_buf *rb;
+  char rcv_buffer[260];
   pwr_tCid cid;
   unsigned char fc;
   short int trans_id;
@@ -167,37 +167,29 @@ pwr_tStatus mb_recv_data(io_sRackLocal *local,
   int modules;
   int i;
   
-  /* Receive answer */
+	short int remaining_data; // Data we have to get from the socket.
+	short int received_data;  // Data that has been received.
 
 	sts = 1;
+	rb = (rec_buf *) rcv_buffer;
 
-  while (sts > 0) {
-    FD_ZERO(&fdr);
-    FD_ZERO(&fdw);
-    FD_ZERO(&fde);
-    FD_SET(local->s, &fdr);
-    FD_SET(local->s, &fdw);
-    FD_SET(local->s, &fde);
+	while (sts > 0) { /* Receive answer */
 		
 		size_of_msg = 0;
-    tv.tv_sec = 0;
-    tv.tv_usec = 0;
-
-    sts = select(32, &fdr, &fdw, &fde, &tv);
+		remaining_data = sizeof(mbap_header);
+		received_data = 0;
+		
+		/*
+		First read at least the MBAP header, and no more.
+		Then, read the remaining bytes indicaterd in the header, but no more.
+		We control the amount of data because could be more than one message in the socket buffer or
+		only the first bytes of a packet.
+		*/
 		
-    if (sts < 0) {
-      sp->Status = MB__CONNLOST;
-      close(local->s);
-      errh_Error( "Connection lost to modbus slave, %s", rp->Name);
-      return IO__SUCCESS;
-    }
+		while ( (remaining_data > 0) && (sts > 0) ) { // if there is data to read and everything is ok, receive.
 			
-    if (!(FD_ISSET(local->s, &fdw))) {
-      sp->Status = MB__CONNDOWN;
-      close(local->s);
-      errh_Error( "Connection down to modbus slave, %s", rp->Name);
-      return IO__SUCCESS;
-    }
+			FD_ZERO(&fdr);
+			FD_SET(local->s, &fdr);
 			
 			if (local->expected_msgs > 0) {
 			  tv.tv_sec = 0;
@@ -207,65 +199,71 @@ pwr_tStatus mb_recv_data(io_sRackLocal *local,
 			  tv.tv_usec = 0;
 			}
 		
-    FD_ZERO(&fdr);
-    FD_ZERO(&fde);
-    FD_SET(local->s, &fdr);
-    FD_SET(local->s, &fde);
-
-    sts = select(32, &fdr, NULL, &fde, &tv);
+			sts = select((int)local->s+1, &fdr, NULL, NULL, &tv);
 			
-    if (sts < 0) {
-      sp->Status = MB__CONNLOST;
-      close(local->s);
-      errh_Error( "Connection lost to modbus slave, %s", rp->Name);
-      return IO__SUCCESS;
-    }
-    
-    if ((sts == 0) && (local->expected_msgs > 0)) {
+			if (sts<=0) { // Timeout or error.
+				if ((sts == 0) && (local->expected_msgs > 0)) { // Timeout but there are messages pending
 					local->msgs_lost++;
 					if (local->msgs_lost > MAX_MSGS_LOST) {
 						sp->Status = MB__CONNDOWN;
 						close(local->s);
-        errh_Error( "Connection down to modbus slave, %s", rp->Name);
+						errh_Error( "Data expected but timeout. Connection down to modbus slave, %s", rp->Name);
+					}
+					return IO__SUCCESS;
 				}
+				
+				if (sts < 0) { // Error in the socket
+					sp->Status = MB__CONNLOST;
+					close(local->s);
+					errh_Error( "Socket Error. Connection lost to modbus slave, %s", rp->Name);
 					return IO__SUCCESS;
 				}
 				
-    if (sts > 0 && FD_ISSET(local->s, &fdr)) {
-      data_size = recv(local->s, rcv_buffer, sizeof(rec_buf), 0);
+			} else { // There are something to read (no timeout and no error). Could be a closed socket too, so we have to check later anyway.
+				data_size = recv(local->s, &rcv_buffer[received_data], remaining_data, 0);
 
 				if (data_size < 0) {
 					sp->Status = MB__CONNLOST;
 					close(local->s);
-	errh_Error( "Connection lost to modbus slave, %s", rp->Name);
+					errh_Error( "Error reading data. Connection lost to modbus slave, %s", rp->Name);
 					return IO__SUCCESS;
 				}
 				
 				if (data_size == 0) {
 					sp->Status = MB__CONNDOWN;
 					close(local->s);
-	errh_Error( "Connection down to modbus slave, %s", rp->Name);
+					errh_Error( "Error reading data. Connection down to modbus slave, %s", rp->Name);
 					return IO__SUCCESS;
 				}
 
-      while (data_size > 0) {
-
-        local->msgs_lost = 0;
+				remaining_data = remaining_data - data_size;
+				received_data = received_data + data_size;
 				
-  	sp->RX_packets++;
-	
-	local->expected_msgs--;
+				if ( (received_data >= sizeof(mbap_header)) && (size_of_msg == 0 )) {
+					// Compute the complete header
+					trans_id = ntohs(rb->head.trans_id);
+					size_of_msg = ntohs(rb->head.length) + 6;
 					
-	cardp = rp->cardlist;
+					// Check header data
+					if ((ntohs(rb->head.proto_id)!=0) || (size_of_msg>260)) { // Invalid modbus packet
+						sp->Status = MB__CONNDOWN;
+						close(local->s);
+						errh_Error( "Invalid Modbus packet. Connection down to modbus slave, %s", rp->Name);
+						return IO__SUCCESS;
+					}
 					
-	if (data_size < sizeof(mbap_header))
-	  break;
+					// Update remaining data
+					remaining_data = size_of_msg - received_data;
+				}
+			}
+		} // while
 
-	rb = (rec_buf *) &rcv_buffer[size_of_msg];
+		if (sts > 0) { // processing packet...
 		
-	trans_id = ntohs(rb->head.trans_id);
-	size_of_msg += ntohs(rb->head.length) + 6;
-	data_size -= ntohs(rb->head.length) + 6;
+			local->msgs_lost = 0;
+			sp->RX_packets++;
+			local->expected_msgs--;
+			cardp = rp->cardlist;
 			
 			while(cardp) {
 				/* From v4.1.3 we can have subclasses, find the super class */
@@ -355,14 +353,13 @@ pwr_tStatus mb_recv_data(io_sRackLocal *local,
 					}
 					mp++;
 					local_card++;
-	  }
+				} // for ( i = 0; i < modules; i++)
 				if (found)
 				break;
 				cardp = cardp->next;
 			} /* End - while(cardp) ... */
-      } /* End - data_size > 0 ... */
-    } /* End - if received message ... */
-  } /* End - receive messages ... */
+		} // if (sts > 0) processing packet...
+	} // while (sts > 0)  Receive answer
 
   return IO__SUCCESS;
 }