diff --git a/src/conn.c b/src/conn.c
index 5bfcec6..66b5029 100644
--- a/src/conn.c
+++ b/src/conn.c
@@ -48,9 +48,7 @@ int max_sd; /* major descriptor in the select() sets */
 void conn_tty_start(ttydata_t *tty, conn_t *conn);
 ssize_t conn_read(int d, void *buf, size_t nbytes);
 ssize_t conn_write(int d, void *buf, size_t nbytes, int istty);
-int conn_select(int nfds,
-                fd_set *readfds, fd_set *writefds, fd_set *exceptfds,
-                struct timeval *timeout);
+void conn_fix_request_header_len(conn_t *conn, unsigned char len);
 
 #define FD_MSET(d, s) do { FD_SET(d, s); max_sd = MAX(d, max_sd); } while (0);
 
@@ -207,9 +205,9 @@ conn_tty_start(ttydata_t *tty, conn_t *conn)
 {
   (void)memcpy((void *)tty->txbuf,
                (void *)(conn->buf + HDRSIZE),
-               MB_HDR(conn->buf, MB_LENGTH_L));
-  modbus_crc_write(tty->txbuf, MB_HDR(conn->buf, MB_LENGTH_L));
-  tty->txlen = MB_HDR(conn->buf, MB_LENGTH_L) + CRCSIZE;
+               MB_FRAME(conn->buf, MB_LENGTH_L));
+  modbus_crc_write(tty->txbuf, MB_FRAME(conn->buf, MB_LENGTH_L));
+  tty->txlen = MB_FRAME(conn->buf, MB_LENGTH_L) + CRCSIZE;
   state_tty_set(tty, TTY_RQST);
   actconn = conn;
 }
@@ -341,7 +339,9 @@ conn_loop(void)
       switch (curconn->state)
       {
         case CONN_HEADER:
-        case CONN_RQST:
+        case CONN_RQST_FUNC:
+        case CONN_RQST_NVAL:
+        case CONN_RQST_TAIL:
           FD_MSET(curconn->sd, &sdsetrd);
           break;
         case CONN_RESP:
@@ -607,7 +607,7 @@ conn_loop(void)
           /* we received more than 3 bytes from header - address, request id and bytes count */
           if (!tty.rxoffset) {
             /* offset is unknown */
-        	unsigned char i;
+            unsigned char i;
             for (i = 0; i < tty.ptrbuf - tty.rxoffset + rc - 1; i++) {
               if (tty.rxbuf[i] == tty.txbuf[0] && tty.rxbuf[i+1] == tty.txbuf[1]) {
 #ifdef DEBUG
@@ -625,8 +625,8 @@ conn_loop(void)
                 i = 5 + tty.rxbuf[tty.rxoffset + 2];
                 break;
               default:
-        	i = tty.rxlen;
-        	break;
+                i = tty.rxlen;
+                break;
             }
             if (i + tty.rxoffset > TTY_BUFSIZE)
               i = TTY_BUFSIZE - tty.rxoffset;
@@ -713,7 +713,9 @@ conn_loop(void)
       switch (curconn->state)
       {
         case CONN_HEADER:
-        case CONN_RQST:
+        case CONN_RQST_FUNC:
+        case CONN_RQST_NVAL:
+        case CONN_RQST_TAIL:
           if (FD_ISSET(curconn->sd, &sdsetrd))
           {
             rc = conn_read(curconn->sd,
@@ -726,19 +728,75 @@ conn_loop(void)
             }
             curconn->ctr += rc;
             if (curconn->state == CONN_HEADER)
-              if (curconn->ctr >= HDRSIZE)
+              if (curconn->ctr >= MB_UNIT_ID)
               { /* header received completely */
                 if (modbus_check_header(curconn->buf) != RC_OK)
                 { /* header is damaged, drop connection */
                   curconn = conn_close(curconn);
                   break;
                 }
-                state_conn_set(curconn, CONN_RQST);
+                state_conn_set(curconn, CONN_RQST_FUNC);
               }
-            if (curconn->state == CONN_RQST)
-              if (curconn->ctr >=
-                    HDRSIZE + MB_HDR(curconn->buf, MB_LENGTH_L))
-              { /* ### packet received completely ### */
+            if (curconn->state == CONN_RQST_FUNC)
+              if (curconn->ctr >= MB_DATA)
+              {
+                /* check request function code */
+                unsigned char fc = MB_FRAME(curconn->buf, MB_FCODE);
+#ifdef DEBUG
+                logw(7, "conn[%s]: read request fc %d",
+                     inet_ntoa(curconn->sockaddr.sin_addr), fc);
+#endif
+                switch (fc)
+                {
+                  case 1: /* Read Coil Status */
+                  case 2: /* Read Input Status */
+                  case 3: /* Read Holding Registers */
+                  case 4: /* Read Input Registers */
+                  case 5: /* Force Single Coil */
+                  case 6: /* Preset Single Register */
+                  {
+                    /* set data length for requests with fixed length */
+                    conn_fix_request_header_len(curconn, 6);
+                    state_conn_set(curconn, CONN_RQST_TAIL);
+                  }
+                    break;
+                  case 15: /* Force Multiple Coils */
+                  case 16: /* Preset Multiple Registers */
+                    /* will read number of registers/coils to compute request data length */
+                    state_conn_set(curconn, CONN_RQST_NVAL);
+                    break;
+                  default:
+                    /* unknown function code, will rely on data length from header */
+                    state_conn_set(curconn, CONN_RQST_TAIL);
+                    break;
+                }
+              }
+            if (curconn->state == CONN_RQST_NVAL)
+              if (curconn->ctr >= MB_DATA_NBYTES)
+              {
+                /* compute request data length for fc 15/16 */
+                unsigned int len;
+                switch (MB_FRAME(curconn->buf, MB_FCODE))
+                {
+                  case 15: /* Force Multiple Coils */
+                    len = 7 + (MB_FRAME(curconn->buf, MB_DATA_NVAL_H) * 256 +
+                        MB_FRAME(curconn->buf, MB_DATA_NVAL_L) + 7) / 8;
+                    break;
+                  case 16: /* Preset Multiple Registers */
+                    len = 7 + MB_FRAME(curconn->buf, MB_DATA_NVAL_L) * 2;
+                    break;
+                }
+                if (len == 0 || len > BUFSIZE - 2)
+                { /* invalid request data length, drop connection */
+                  curconn = conn_close(curconn);
+                  break;
+                }
+                conn_fix_request_header_len(curconn, len);
+                state_conn_set(curconn, CONN_RQST_TAIL);
+              }
+            if (curconn->state == CONN_RQST_TAIL)
+              if (curconn->ctr >= HDRSIZE + MB_FRAME(curconn->buf, MB_LENGTH_L))
+              { /* ### frame received completely ### */
                 state_conn_set(curconn, CONN_TTY);
                 if (tty.state == TTY_READY)
                   conn_tty_start(&tty, curconn);
@@ -751,7 +809,7 @@ conn_loop(void)
           {
             rc = conn_write(curconn->sd,
                             curconn->buf + curconn->ctr,
-                            MB_HDR(curconn->buf, MB_LENGTH_L) +
+                            MB_FRAME(curconn->buf, MB_LENGTH_L) +
                             HDRSIZE - curconn->ctr, 0);
             if (rc <= 0)
             { /* error - drop this connection and go to next queue element */
@@ -759,7 +817,7 @@ conn_loop(void)
               break;
             }
             curconn->ctr += rc;
-            if (curconn->ctr == (MB_HDR(curconn->buf, MB_LENGTH_L) + HDRSIZE))
+            if (curconn->ctr == (MB_FRAME(curconn->buf, MB_LENGTH_L) + HDRSIZE))
               state_conn_set(curconn, CONN_HEADER);
           }
           curconn = queue_next_elem(&queue, curconn);
@@ -770,3 +828,23 @@ conn_loop(void)
 
   /* XXX some cleanup must be here */
 }
+
+/*
+ * Fix request header length field, if needed
+ * Parameters: CONN - ptr to connection
+ *             LEN - expected request data length
+ * Return: none
+ */
+void
+conn_fix_request_header_len(conn_t *conn, unsigned char len)
+{
+  if (MB_FRAME(conn->buf, MB_LENGTH_L) != len)
+  {
+#ifdef DEBUG
+    logw(5, "conn[%s]: request data len changed from %d to %d",
+         MB_FRAME(conn->buf, MB_LENGTH_L), len);
+#endif
+    MB_FRAME(conn->buf, MB_LENGTH_L) = len;
+  }
+}
+
diff --git a/src/conn.h b/src/conn.h
index 2a5d343..c8d042a 100644
--- a/src/conn.h
+++ b/src/conn.h
@@ -86,10 +86,12 @@
 /*
  * Client connection FSM states
  */
-#define CONN_HEADER 0
-#define CONN_RQST   1
-#define CONN_TTY    2
-#define CONN_RESP   3
+#define CONN_HEADER    0  /* reading frame header */
+#define CONN_RQST_FUNC 1  /* reading request function code */
+#define CONN_RQST_NVAL 2  /* reading request number of values (registers/coils) */
+#define CONN_RQST_TAIL 3  /* reading request tail */
+#define CONN_TTY       4  /* writing request to TTY */
+#define CONN_RESP      5  /* reading response from TTY */
 
 /*
  * Client connection related data storage structure
diff --git a/src/modbus.c b/src/modbus.c
index 9b5aef7..22b2cf5 100644
--- a/src/modbus.c
+++ b/src/modbus.c
@@ -4,18 +4,18 @@
  * modbus.c - MODBUS protocol related procedures
  *
  * Copyright (c) 2002-2003, 2013, Victor Antonovich (v.antonovich@gmail.com)
- * 
+ *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
  * are met:
- * 
+ *
  * - Redistributions of source code must retain the above copyright
  * notice, this list of conditions and the following disclaimer.
- * 
+ *
  * - Redistributions in binary form must reproduce the above copyright
  * notice, this list of conditions and the following disclaimer in the
  * documentation and/or other materials provided with the distribution.
- * 
+ *
  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
  * ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
@@ -67,21 +67,21 @@ modbus_crc_write(unsigned char *frame, unsigned int len)
 void
 modbus_ex_write(unsigned char *packet, unsigned char code)
 {
-  MB_HDR(packet, MB_FCODE) |= 0x80;
-  MB_HDR(packet, MB_DATA) = code;
+  MB_FRAME(packet, MB_FCODE) |= 0x80;
+  MB_FRAME(packet, MB_DATA) = code;
   WORD_WR_BE(packet + MB_LENGTH_H, MB_EX_LEN);
 }
 
 /*
  * Check MODBUS packet header consistency
- * Parameters: HEADER - address of the header
+ * Parameters: PACKET - address of the request packet,
  * Return: RC_OK if (mostly) all is right, RC_ERR otherwise
  */
 int
 modbus_check_header(unsigned char *packet)
 {
-  return (MB_HDR(packet, MB_PROTO_ID_H) == 0 &&
-          MB_HDR(packet, MB_PROTO_ID_L) == 0 &&   
-          MB_HDR(packet, MB_LENGTH_H) == 0   &&
-          MB_HDR(packet, MB_LENGTH_L) > 0) ? RC_OK : RC_ERR;
+  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;
 }
diff --git a/src/modbus.h b/src/modbus.h
index fee354b..e0ed2a1 100644
--- a/src/modbus.h
+++ b/src/modbus.h
@@ -4,18 +4,18 @@
  * modbus.h - MODBUS protocol related procedures
  *
  * Copyright (c) 2002-2003, 2013, Victor Antonovich (v.antonovich@gmail.com)
- * 
+ *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
  * are met:
- * 
+ *
  * - Redistributions of source code must retain the above copyright
  * notice, this list of conditions and the following disclaimer.
- * 
+ *
  * - Redistributions in binary form must reproduce the above copyright
  * notice, this list of conditions and the following disclaimer in the
  * documentation and/or other materials provided with the distribution.
- * 
+ *
  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
  * ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
@@ -38,9 +38,9 @@
 #include "crc16.h"
 
 /*
- * Macros for invoking data from MODBUS packet header
+ * Macro for accessing data in MODBUS frame
  */
-#define MB_HDR(p, d) ( *(p + d) )
+#define MB_FRAME(p, d) ( *(p + d) )
 
 /*
  * MODBUS frame lengths
@@ -50,7 +50,7 @@
 #define	MB_MAX_LEN 256
 
 /*
- * Macroses for word operations
+ * Macros for word operations
  */
 #define HIGH(w) ( (unsigned char)(((w) >> 8) & 0xff) )
 #define LOW(w) ( (unsigned char)((w) & 0xff) )
@@ -79,7 +79,12 @@
 #define MB_UNIT_ID    6    /* unit identifier */
 #define MB_FCODE      7    /* function code */
 #define MB_DATA       8    /* MODBUS data */
-                      
+#define MB_DATA_ADDR_H MB_DATA    /* MODBUS data: address high byte */
+#define MB_DATA_ADDR_L       9    /* MODBUS data: address low byte */
+#define MB_DATA_NVAL_H      10    /* MODBUS data: number of values high byte */
+#define MB_DATA_NVAL_L      11    /* MODBUS data: number of values low byte */
+#define MB_DATA_NBYTES      12    /* MODBUS data: number of bytes to follow (fc 15,16) */
+
 /*
  * Exception codes
  */
diff --git a/src/state.c b/src/state.c
index c4d5afa..29e6e15 100644
--- a/src/state.c
+++ b/src/state.c
@@ -79,16 +79,30 @@ state_conn_set(conn_t *conn, int state)
              inet_ntoa(conn->sockaddr.sin_addr));
 #endif
       break;
+    case CONN_RQST_FUNC:
 #ifdef DEBUG
-    case CONN_RQST:
-      logw(5, "conn[%s]: state now is CONN_RQST",
+      logw(5, "conn[%s]: state now is CONN_RQST_FUNC",
              inet_ntoa(conn->sockaddr.sin_addr));
+#endif
+    break;
+    case CONN_RQST_NVAL:
+#ifdef DEBUG
+      logw(5, "conn[%s]: state now is CONN_RQST_NVAL",
+             inet_ntoa(conn->sockaddr.sin_addr));
+#endif
+    break;
+    case CONN_RQST_TAIL:
+#ifdef DEBUG
+      logw(5, "conn[%s]: state now is CONN_RQST_TAIL",
+             inet_ntoa(conn->sockaddr.sin_addr));
+#endif
     break;
     case CONN_TTY:
+#ifdef DEBUG
       logw(5, "conn[%s]: state now is CONN_TTY",
              inet_ntoa(conn->sockaddr.sin_addr));
-      break;
 #endif
+      break;
     case CONN_RESP:
       conn->ctr = 0;
 #ifdef DEBUG