diff --git a/rtuclient.go b/rtuclient.go
index ad6a31a..8465124 100644
--- a/rtuclient.go
+++ b/rtuclient.go
@@ -90,6 +90,22 @@ func (mb *rtuPackager) Verify(aduRequest []byte, aduResponse []byte) (err error)
 // Decode extracts PDU from RTU frame and verify CRC.
 func (mb *rtuPackager) Decode(adu []byte) (pdu *ProtocolDataUnit, err error) {
 	length := len(adu)
+
+	if length > 1 && adu[1] < 5 {
+		// adjust real length
+		if length < 3 {
+			err = fmt.Errorf("modbus: response length less than min '%v'", length)
+			return
+		} else {
+			real_len := int(adu[2]) + 5
+			if real_len > length {
+				err = fmt.Errorf("modbus: response length '%v' less than real length '%v'", length, real_len)
+				return
+			} else {
+				length = real_len
+			}
+		}
+	}
 	// Calculate checksum
 	var crc crc
 	crc.reset().pushBytes(adu[0 : length-2])
diff --git a/rtuclient_test.go b/rtuclient_test.go
index 5a0bcc2..c6cc10b 100644
--- a/rtuclient_test.go
+++ b/rtuclient_test.go
@@ -29,17 +29,17 @@ func TestRTUEncoding(t *testing.T) {
 
 func TestRTUDecoding(t *testing.T) {
 	decoder := rtuPackager{}
-	adu := []byte{0x01, 0x10, 0x8A, 0x00, 0x00, 0x03, 0xAA, 0x10}
+	adu := []byte{0x05, 0x03, 0x02, 0x00, 0xAA, 0xC9, 0xFB, 0xFF, 0xFF, 0xFF, 0xFF}
 
 	pdu, err := decoder.Decode(adu)
 	if err != nil {
 		t.Fatal(err)
 	}
 
-	if 16 != pdu.FunctionCode {
-		t.Fatalf("Function code: expected %v, actual %v", 16, pdu.FunctionCode)
+	if 3 != pdu.FunctionCode {
+		t.Fatalf("Function code: expected %v, actual %v", 3, pdu.FunctionCode)
 	}
-	expected := []byte{0x8A, 0x00, 0x00, 0x03}
+	expected := []byte{0x02, 0x00, 0xAA}
 	if !bytes.Equal(expected, pdu.Data) {
 		t.Fatalf("Data: expected %v, actual %v", expected, pdu.Data)
 	}
@@ -88,7 +88,8 @@ func BenchmarkRTUDecoder(b *testing.B) {
 	decoder := rtuPackager{
 		SlaveId: 10,
 	}
-	adu := []byte{0x01, 0x10, 0x8A, 0x00, 0x00, 0x03, 0xAA, 0x10}
+	//	adu := []byte{0x01, 0x10, 0x8A, 0x00, 0x00, 0x03, 0xAA, 0x10}
+	adu := []byte{0x05, 0x03, 0x02, 0x00, 0xAA, 0xC9, 0xFB, 0xFF, 0xFF, 0xFF}
 	for i := 0; i < b.N; i++ {
 		_, err := decoder.Decode(adu)
 		if err != nil {