diff --git a/src/conn.c b/src/conn.c
index 9c880ae..b605624 100644
--- a/src/conn.c
+++ b/src/conn.c
@@ -767,7 +767,7 @@ conn_loop(void)
           {
             rc = conn_read(curconn->sd,
                            curconn->buf + curconn->ctr,
-                           RQSTSIZE - curconn->ctr);
+                           curconn->read_len - curconn->ctr);
             if (rc <= 0)
             { /* error - drop this connection and go to next queue element */
               curconn = conn_close(curconn);
diff --git a/src/conn.h b/src/conn.h
index c8d042a..9b7c121 100644
--- a/src/conn.h
+++ b/src/conn.h
@@ -105,6 +105,7 @@ typedef struct conn_t
   int timeout;          /* timeout value, secs */
   struct sockaddr_in sockaddr; /* connection structure */
   int ctr;              /* counter of data in the buffer */
+  int read_len;         /* length of modbus frame to read */
   unsigned char buf[HDRSIZE + BUFSIZE];    /* data buffer */
 } conn_t;
 
diff --git a/src/modbus.c b/src/modbus.c
index 22b2cf5..4042fce 100644
--- a/src/modbus.c
+++ b/src/modbus.c
@@ -32,6 +32,7 @@
  */
 
 #include "modbus.h"
+#include "conn.h"
 
 /*
  * Check CRC of MODBUS frame
@@ -83,5 +84,6 @@ modbus_check_header(unsigned char *packet)
   return (MB_FRAME(packet, MB_PROTO_ID_H) == 0 &&
           MB_FRAME(packet, MB_PROTO_ID_L) == 0 &&
           MB_FRAME(packet, MB_LENGTH_H) == 0   &&
-          MB_FRAME(packet, MB_LENGTH_L) > 0) ? RC_OK : RC_ERR;
+          MB_FRAME(packet, MB_LENGTH_L) > 0    &&
+          MB_FRAME(packet, MB_LENGTH_L) < BUFSIZE - CRCSIZE) ? RC_OK : RC_ERR;
 }
diff --git a/src/state.c b/src/state.c
index 29e6e15..6698fdb 100644
--- a/src/state.c
+++ b/src/state.c
@@ -74,12 +74,14 @@ state_conn_set(conn_t *conn, int state)
   {
     case CONN_HEADER:
       conn->ctr = 0;
+      conn->read_len = HDRSIZE;
 #ifdef DEBUG
       logw(5, "conn[%s]: state now is CONN_HEADER",
              inet_ntoa(conn->sockaddr.sin_addr));
 #endif
       break;
     case CONN_RQST_FUNC:
+      conn->read_len = HDRSIZE + MB_FRAME(conn->buf, MB_LENGTH_L);
 #ifdef DEBUG
       logw(5, "conn[%s]: state now is CONN_RQST_FUNC",
              inet_ntoa(conn->sockaddr.sin_addr));