1 files changed, 13 insertions, 16 deletions
diff --git a/software/communication/data_link.go b/software/communication/data_link.go
index d6e752d..82375bf 100644
--- a/software/communication/data_link.go
+++ b/software/communication/data_link.go
@@ -1,5 +1,9 @@
 package communication
 
+import (
+	"log"
+)
+
 const (
 	SLIP_END         = 0xC0
 	SLIP_ESC         = 0xDB
@@ -32,32 +36,25 @@ func (dl *dataLink) receive(source chan byte) {
 }
 
 func unslip(source chan byte) []byte {
-	escaped := false
 	buffer := make([]byte, 0)
 
 	for {
 		octet := <-source
-		if escaped {
-			switch octet {
+		switch octet {
+		case SLIP_ESC:
+			escaped := <-source
+			switch escaped {
 			case SLIP_ESC_END:
 				buffer = append(buffer, SLIP_END)
 			case SLIP_ESC_ESC:
 				buffer = append(buffer, SLIP_ESC)
-			}
-		} else {
-			switch octet {
-			case SLIP_END:
-				break
-			case SLIP_ESC:
-				escaped = true
-				continue
 			default:
-				buffer = append(buffer, octet)
-				continue
+				log.Printf("SLIP: Invalid 0x%x followed an escape byte", escaped)
 			}
-			break
+		case SLIP_END:
+			return buffer
+		default:
+			buffer = append(buffer, octet)
 		}
 	}
-
-	return buffer
 }