From 8a43d8e8a48b83ed7899def9a880f4b77bf2e036 Mon Sep 17 00:00:00 2001
From: Damanox <apodvalny@gmail.com>
Date: Sat, 15 Dec 2018 14:47:18 +0200
Subject: [PATCH] fix: handle 0xAA in response

---
 sl500.go | 32 +++++++++++++++++++++++++++++++-
 1 file changed, 31 insertions(+), 1 deletion(-)

diff --git a/sl500.go b/sl500.go
index c380b2f..70b2891 100644
--- a/sl500.go
+++ b/sl500.go
@@ -344,7 +344,7 @@ func readResponse(s *Sl500) ([]byte, error) {
 		if totalRead < 3 {
 			continue
 		}
-		if int(buf[2]) != len(buf)-4 {
+		if int(buf[2]) != getBufLen(buf)-4 {
 			continue
 		}
 
@@ -370,6 +370,7 @@ func readResponse(s *Sl500) ([]byte, error) {
 		return nil, fmt.Errorf("Response verification failed")
 	}
 
+	buf = filterBuf(buf)
 	buf = buf[5 : len(buf)-1]
 
 	if s.logging {
@@ -379,6 +380,35 @@ func readResponse(s *Sl500) ([]byte, error) {
 	return buf, nil
 }
 
+func getBufLen(buf []byte) int {
+	l := len(buf)
+	for i, b := range buf {
+		if i < 2 {
+			continue
+		}
+
+		if b == 0xAA {
+			l--
+		}
+	}
+	return l
+}
+
+func filterBuf(buf []byte) []byte {
+	result := make([]byte, getBufLen(buf))
+
+	ind := 0
+	for i, b := range buf {
+		if i > 0 && buf[i - 1] == 0xAA && b == 0x00 {
+			continue
+		}
+
+		result[ind] = b
+		ind++
+	}
+	return result
+}
+
 func sendRequest(s *Sl500, commandCode int16, bytesData ...[]byte) error {
 	if !s.open {
 		return errors.New("serial port is closed")