diff --git a/src/codec/rtu.rs b/src/codec/rtu.rs
index 5e33b67a..900e57fb 100644
--- a/src/codec/rtu.rs
+++ b/src/codec/rtu.rs
@@ -11,6 +11,7 @@ use log::{debug, error, warn};
 use smallvec::SmallVec;
 use std::io::{Cursor, Error, ErrorKind, Result};
 use tokio_util::codec::{Decoder, Encoder};
+use std::time::{Instant, Duration};
 
 // [MODBUS over Serial Line Specification and Implementation Guide V1.02](http://modbus.org/docs/Modbus_over_serial_line_V1_02.pdf), page 13
 // "The maximum size of a MODBUS RTU frame is 256 bytes."
@@ -105,6 +106,8 @@ pub(crate) struct RequestDecoder {
 #[derive(Debug, Default, Eq, PartialEq)]
 pub(crate) struct ResponseDecoder {
     frame_decoder: FrameDecoder,
+    last_time: Option<Instant>,
+    last_size: usize,
 }
 
 #[derive(Debug, Default, Eq, PartialEq)]
@@ -165,7 +168,14 @@ fn get_response_pdu_len(adu_buf: &BytesMut) -> Result<Option<usize>> {
                     // incomplete frame
                     return Ok(None);
                 }
-            }
+            },
+            // For custom functions, use all the buffer
+            65..=75 | 100..=110 => if adu_buf.len() > 3 {
+                adu_buf.len() - 3
+            } else {
+                return Ok(None);
+            },
+            // Above this value it means an exception code as the first bit is 1
             0x81..=0xAB => 2,
             _ => {
                 return Err(Error::new(
@@ -223,12 +233,36 @@ impl Decoder for ResponseDecoder {
     type Error = Error;
 
     fn decode(&mut self, buf: &mut BytesMut) -> Result<Option<(SlaveId, Bytes)>> {
-        decode(
+        if let Some(last_time) = self.last_time {
+            // Check if 20 ms passed since the last burst of data
+            if last_time.elapsed() > Duration::from_millis(20) {
+                // If it was, clear the previous bytes received
+                buf.advance(self.last_size);
+                self.last_size = 0;
+            }
+        }
+        let resp = decode(
             "response",
             &mut self.frame_decoder,
             get_response_pdu_len,
             buf,
-        )
+        );
+
+        if let Ok(Some(_)) = resp {
+            // Restart if the frame was parsed
+            self.last_time = None;
+            // Ensure last size is cleared
+            self.last_size = 0;
+        } else {
+            if let Ok(_) = resp {
+                // Only update the last_time if the frame was not parsed
+                self.last_time = Some(Instant::now());
+            }
+            // In any case, update the last size
+            self.last_size = buf.len();
+        }
+
+        resp
     }
 }
 
@@ -242,28 +276,20 @@ where
     F: Fn(&BytesMut) -> Result<Option<usize>>,
 {
     // TODO: Transform this loop into idiomatic code
-    loop {
-        let mut retry = false;
-        let res = get_pdu_len(buf)
-            .and_then(|pdu_len| {
-                debug_assert!(!retry);
-                if let Some(pdu_len) = pdu_len {
-                    frame_decoder.decode(buf, pdu_len)
-                } else {
-                    // Incomplete frame
-                    Ok(None)
-                }
-            })
-            .or_else(|err| {
-                warn!("Failed to decode {} frame: {}", pdu_type, err);
-                frame_decoder.recover_on_error(buf);
-                retry = true;
-                Ok(None)
-            });
-        if !retry {
-            return res;
-        }
-    }
+    get_pdu_len(buf)
+        .and_then(|pdu_len| {
+            if let Some(pdu_len) = pdu_len {
+                frame_decoder.decode(buf, pdu_len)
+            } else {
+                // Incomplete frame
+                 Ok(None)
+            }
+        })
+        .or_else(|err| {
+            warn!("Failed to decode {} frame: {}", pdu_type, err);
+            //frame_decoder.recover_on_error(buf);
+            Ok(None)
+        })
 }
 
 impl Decoder for ClientCodec {