diff --git a/DMRTrellis.cpp b/DMRTrellis.cpp
index 60eb33d..f0a73cb 100644
--- a/DMRTrellis.cpp
+++ b/DMRTrellis.cpp
@@ -58,39 +58,27 @@ bool CDMRTrellis::decode(const unsigned char* data, unsigned char* payload)
 	unsigned char points[49U];
 	dibitsToPoints(dibits, points);
 
-	unsigned char tribits[49U];
-
 	// Check the original code
+	unsigned char tribits[49U];
 	unsigned int failPos = checkCode(points, tribits);
 	if (failPos == 999U) {
 		tribitsToBits(tribits, payload);
 		return true;
 	}
 
-	for (unsigned j = 0U; j < 20U; j++) {
-		unsigned int bestPos = 0U;
-		unsigned int bestVal = 0U;
+	unsigned char savePoints[49U];
+	for (unsigned int i = 0U; i < 49U; i++)
+		savePoints[i] = points[i];
 
-		for (unsigned int i = 0U; i < 16U; i++) {
-			points[failPos] = i;
+	bool ret = fixCode(points, failPos, payload);
+	if (ret)
+		return true;
 
-			unsigned int pos = checkCode(points, tribits);
-			if (pos == 999U) {
-				tribitsToBits(tribits, payload);
-				return true;
-			}
+	if (failPos == 0U)
+		return false;
 
-			if (pos > bestPos) {
-				bestPos = pos;
-				bestVal = i;
-			}
-		}
-
-		points[failPos] = bestVal;
-		failPos = bestPos;
-	}
-
-	return false;
+	// Backtrack one place for a last go
+	return fixCode(savePoints, failPos - 1U, payload);
 }
 
 void CDMRTrellis::encode(const unsigned char* payload, unsigned char* data)
@@ -330,6 +318,35 @@ void CDMRTrellis::tribitsToBits(const unsigned char* tribits, unsigned char* pay
 	}
 }
 
+bool CDMRTrellis::fixCode(unsigned char* points, unsigned int failPos, unsigned char* payload) const
+{
+	for (unsigned j = 0U; j < 20U; j++) {
+		unsigned int bestPos = 0U;
+		unsigned int bestVal = 0U;
+
+		for (unsigned int i = 0U; i < 16U; i++) {
+			points[failPos] = i;
+
+			unsigned char tribits[49U];
+			unsigned int pos = checkCode(points, tribits);
+			if (pos == 999U) {
+				tribitsToBits(tribits, payload);
+				return true;
+			}
+
+			if (pos > bestPos) {
+				bestPos = pos;
+				bestVal = i;
+			}
+		}
+
+		points[failPos] = bestVal;
+		failPos = bestPos;
+	}
+
+	return false;
+}
+
 unsigned int CDMRTrellis::checkCode(const unsigned char* points, unsigned char* tribits) const
 {
 	unsigned char state = 0U;
diff --git a/DMRTrellis.h b/DMRTrellis.h
index a63043b..55ed0c0 100644
--- a/DMRTrellis.h
+++ b/DMRTrellis.h
@@ -29,6 +29,7 @@ private:
 	void pointsToDibits(const unsigned char* points, signed char* dibits) const;
 	void bitsToTribits(const unsigned char* payload, unsigned char* tribits) const;
 	void tribitsToBits(const unsigned char* tribits, unsigned char* payload) const;
+	bool fixCode(unsigned char* points, unsigned int failPos, unsigned char* payload) const;
 	unsigned int checkCode(const unsigned char* points, unsigned char* tribits) const;
 };