diff --git a/src/fsfw/globalfunctions/DleEncoder.cpp b/src/fsfw/globalfunctions/DleEncoder.cpp
index e30eee22b..a043cbf7f 100644
--- a/src/fsfw/globalfunctions/DleEncoder.cpp
+++ b/src/fsfw/globalfunctions/DleEncoder.cpp
@@ -8,76 +8,131 @@ DleEncoder::~DleEncoder() {}
 ReturnValue_t DleEncoder::encode(const uint8_t* sourceStream,
 		size_t sourceLen, uint8_t* destStream, size_t maxDestLen,
 		size_t* encodedLen, bool addStxEtx) {
-	if (maxDestLen < 2) {
-		return STREAM_TOO_SHORT;
-	}
-	size_t encodedIndex = 0, sourceIndex = 0;
-	uint8_t nextByte;
+    size_t minAllowedLen = 0;
+    if(escapeStxEtx) {
+        minAllowedLen = 2;
+
+    }
+    else {
+        minAllowedLen = 1;
+
+    }
+    if(maxDestLen < minAllowedLen) {
+        return STREAM_TOO_SHORT;
+    }
 	if (addStxEtx) {
 	    if(not escapeStxEtx) {
 	        destStream[0] = DLE_CHAR;
-	        ++encodedIndex;
 	    }
 		destStream[0] = STX_CHAR;
-		++encodedIndex;
 	}
 
-	while (encodedIndex < maxDestLen and sourceIndex < sourceLen) {
-		nextByte = sourceStream[sourceIndex];
-		// STX, ETX and CR characters in the stream need to be escaped with DLE
-		if ((nextByte == STX_CHAR or nextByte == ETX_CHAR) or
-		        (this->escapeCr and nextByte == CARRIAGE_RETURN)) {
-		    if(this->escapeStxEtx) {
-	            if (encodedIndex + 1 >= maxDestLen) {
-	                return STREAM_TOO_SHORT;
-	            }
-	            else {
-	                destStream[encodedIndex] = DLE_CHAR;
-	                ++encodedIndex;
-	                /* Escaped byte will be actual byte + 0x40. This prevents
-	                 * STX, ETX, and carriage return characters from appearing
-	                 * in the encoded data stream at all, so when polling an
-	                 * encoded stream, the transmission can be stopped at ETX.
-	                 * 0x40 was chosen at random with special requirements:
-	                 *  - Prevent going from one control char to another
-	                 *  - Prevent overflow for common characters */
-	                destStream[encodedIndex] = nextByte + 0x40;
-	            }
-		    }
-		}
-		// DLE characters are simply escaped with DLE.
-		else if (nextByte == DLE_CHAR) {
-			if (encodedIndex + 1 >= maxDestLen) {
-			    return STREAM_TOO_SHORT;
-			}
-			else {
-				destStream[encodedIndex] = DLE_CHAR;
-				++encodedIndex;
-				destStream[encodedIndex] = DLE_CHAR;
-			}
-		}
-		else {
-			destStream[encodedIndex] = nextByte;
-		}
-		++encodedIndex;
-		++sourceIndex;
-	}
+    if(escapeStxEtx) {
+        return encodeStreamEscaped(sourceStream, sourceLen,
+                destStream, maxDestLen, encodedLen, addStxEtx);
+    }
+    else {
+        return encodeStreamNonEscaped(sourceStream, sourceLen,
+                destStream, maxDestLen, encodedLen, addStxEtx);
+    }
 
-	if (sourceIndex == sourceLen and encodedIndex < maxDestLen) {
-		if (addStxEtx) {
-	        if(not escapeStxEtx) {
-	            destStream[encodedIndex] = DLE_CHAR;
-	            ++encodedIndex;
-	        }
-			destStream[encodedIndex] = ETX_CHAR;
-			++encodedIndex;
-		}
-		*encodedLen = encodedIndex;
-		return RETURN_OK;
-	}
-	else {
-		return STREAM_TOO_SHORT;
-	}
+}
+
+ReturnValue_t DleEncoder::encodeStreamEscaped(const uint8_t *sourceStream, size_t sourceLen,
+        uint8_t *destStream, size_t maxDestLen, size_t *encodedLen,
+        bool addStxEtx) {
+    size_t encodedIndex = 2;
+    size_t sourceIndex = 0;
+    uint8_t nextByte = 0;
+    while (encodedIndex < maxDestLen and sourceIndex < sourceLen) {
+        nextByte = sourceStream[sourceIndex];
+        // STX, ETX and CR characters in the stream need to be escaped with DLE
+        if ((nextByte == STX_CHAR or nextByte == ETX_CHAR) or
+                (this->escapeCr and nextByte == CARRIAGE_RETURN)) {
+            if (encodedIndex + 1 >= maxDestLen) {
+                return STREAM_TOO_SHORT;
+            }
+            else {
+                destStream[encodedIndex] = DLE_CHAR;
+                ++encodedIndex;
+                /* Escaped byte will be actual byte + 0x40. This prevents
+                 * STX, ETX, and carriage return characters from appearing
+                 * in the encoded data stream at all, so when polling an
+                 * encoded stream, the transmission can be stopped at ETX.
+                 * 0x40 was chosen at random with special requirements:
+                 *  - Prevent going from one control char to another
+                 *  - Prevent overflow for common characters */
+                destStream[encodedIndex] = nextByte + 0x40;
+            }
+        }
+        // DLE characters are simply escaped with DLE.
+        else if (nextByte == DLE_CHAR) {
+            if (encodedIndex + 1 >= maxDestLen) {
+                return STREAM_TOO_SHORT;
+            }
+            else {
+                destStream[encodedIndex] = DLE_CHAR;
+                ++encodedIndex;
+                destStream[encodedIndex] = DLE_CHAR;
+            }
+        }
+        else {
+            destStream[encodedIndex] = nextByte;
+        }
+        ++encodedIndex;
+        ++sourceIndex;
+    }
+
+    if (sourceIndex == sourceLen and encodedIndex < maxDestLen) {
+        if (addStxEtx) {
+            destStream[encodedIndex] = ETX_CHAR;
+            ++encodedIndex;
+        }
+        *encodedLen = encodedIndex;
+        return RETURN_OK;
+    }
+    else {
+        return STREAM_TOO_SHORT;
+    }
+}
+
+ReturnValue_t DleEncoder::encodeStreamNonEscaped(const uint8_t *sourceStream, size_t sourceLen,
+        uint8_t *destStream, size_t maxDestLen, size_t *encodedLen,
+        bool addStxEtx) {
+    size_t encodedIndex = 1;
+    size_t sourceIndex = 0;
+    uint8_t nextByte = 0;
+    while (encodedIndex < maxDestLen and sourceIndex < sourceLen) {
+        nextByte = sourceStream[sourceIndex];
+        // DLE characters are simply escaped with DLE.
+        if (nextByte == DLE_CHAR) {
+            if (encodedIndex + 1 >= maxDestLen) {
+                return STREAM_TOO_SHORT;
+            }
+            else {
+                destStream[encodedIndex] = DLE_CHAR;
+                ++encodedIndex;
+                destStream[encodedIndex] = DLE_CHAR;
+            }
+        }
+        else {
+            destStream[encodedIndex] = nextByte;
+        }
+        ++encodedIndex;
+        ++sourceIndex;
+    }
+
+    if (sourceIndex == sourceLen and encodedIndex < maxDestLen) {
+        if (addStxEtx) {
+            destStream[encodedIndex] = ETX_CHAR;
+            ++encodedIndex;
+        }
+        *encodedLen = encodedIndex;
+        return RETURN_OK;
+    }
+    else {
+        return STREAM_TOO_SHORT;
+    }
 }
 
 ReturnValue_t DleEncoder::decode(const uint8_t *sourceStream,
diff --git a/src/fsfw/globalfunctions/DleEncoder.h b/src/fsfw/globalfunctions/DleEncoder.h
index 292e00f8c..dc178a0e9 100644
--- a/src/fsfw/globalfunctions/DleEncoder.h
+++ b/src/fsfw/globalfunctions/DleEncoder.h
@@ -89,6 +89,14 @@ public:
 
 private:
 
+    ReturnValue_t encodeStreamEscaped(const uint8_t *sourceStream, size_t sourceLen,
+            uint8_t *destStream, size_t maxDestLen, size_t *encodedLen,
+            bool addStxEtx = true);
+
+    ReturnValue_t encodeStreamNonEscaped(const uint8_t *sourceStream, size_t sourceLen,
+            uint8_t *destStream, size_t maxDestLen, size_t *encodedLen,
+            bool addStxEtx = true);
+
     ReturnValue_t decodeStreamEscaped(const uint8_t *sourceStream, size_t sourceStreamLen,
             size_t *readLen, uint8_t *destStream, size_t maxDestStreamlen, size_t *decodedLen);