Merge branch 'mueller/dle-improvements' into mueller/master

src/fsfw/globalfunctions/DleEncoder.cpp

@@ -14,6 +14,10 @@ ReturnValue_t DleEncoder::encode(const uint8_t* sourceStream,
 	size_t encodedIndex = 0, sourceIndex = 0;
 	uint8_t nextByte;
 	if (addStxEtx) {
+	    if(not escapeStxEtx) {
+	        destStream[0] = DLE_CHAR;
+	        ++encodedIndex;
+	    }
 		destStream[0] = STX_CHAR;
 		++encodedIndex;
 	}
@@ -61,6 +65,10 @@ ReturnValue_t DleEncoder::encode(const uint8_t* sourceStream,
 
 	if (sourceIndex == sourceLen and encodedIndex < maxDestLen) {
 		if (addStxEtx) {
+	        if(not escapeStxEtx) {
+	            destStream[encodedIndex] = DLE_CHAR;
+	            ++encodedIndex;
+	        }
 			destStream[encodedIndex] = ETX_CHAR;
 			++encodedIndex;
 		}
@@ -76,58 +84,108 @@ ReturnValue_t DleEncoder::decode(const uint8_t *sourceStream,
 		size_t sourceStreamLen, size_t *readLen, uint8_t *destStream,
 		size_t maxDestStreamlen, size_t *decodedLen) {
 	size_t encodedIndex = 0, decodedIndex = 0;
-	uint8_t nextByte;
+	if(not escapeStxEtx) {
-	if (*sourceStream != STX_CHAR) {
+	    if (*sourceStream != DLE_CHAR) {
-		return DECODING_ERROR;
+	        return DECODING_ERROR;
+	    }
+	    ++encodedIndex;
 	}
+	if (sourceStream[encodedIndex] != STX_CHAR) {
+	    return DECODING_ERROR;
+	}
+
 	++encodedIndex;
 
-	while ((encodedIndex < sourceStreamLen) && (decodedIndex < maxDestStreamlen)
+	if(escapeStxEtx) {
-			&& (sourceStream[encodedIndex] != ETX_CHAR)
+	    return decodeStreamEscaped(encodedIndex, decodedIndex, sourceStream, sourceStreamLen,
-			&& (sourceStream[encodedIndex] != STX_CHAR)) {
+	            readLen, destStream, maxDestStreamlen, decodedLen);
-		if (sourceStream[encodedIndex] == DLE_CHAR) {
-			nextByte = sourceStream[encodedIndex + 1];
-			// The next byte is a DLE character that was escaped by another
-			// DLE character, so we can write it to the destination stream.
-			if (nextByte == DLE_CHAR) {
-				destStream[decodedIndex] = nextByte;
-			}
-			else {
-			    if(this->escapeStxEtx) {
-	                /* The next byte is a STX, DTX or 0x0D character which
-	                 * was escaped by a DLE character. The actual byte was
-	                 * also encoded by adding + 0x40 to prevent having control chars,
-	                 * in the stream at all, so we convert it back. */
-	                if ((nextByte == STX_CHAR + 0x40 or nextByte == ETX_CHAR + 0x40) or
-	                        (this->escapeCr and nextByte == CARRIAGE_RETURN + 0x40)) {
-	                    destStream[decodedIndex] = nextByte - 0x40;
-	                }
-	                else {
-	                    return DECODING_ERROR;
-	                }
-			    }
-			    else {
-	                return DECODING_ERROR;
-			    }
-			}
-			++encodedIndex;
-		}
-		else {
-			destStream[decodedIndex] = sourceStream[encodedIndex];
-		}
-
-		++encodedIndex;
-		++decodedIndex;
-	}
-
-	if (sourceStream[encodedIndex] != ETX_CHAR) {
-		*readLen = ++encodedIndex;
-		return DECODING_ERROR;
 	}
 	else {
-		*readLen = ++encodedIndex;
+        return decodeStreamNonEscaped(encodedIndex, decodedIndex, sourceStream, sourceStreamLen,
-		*decodedLen = decodedIndex;
+                readLen, destStream, maxDestStreamlen, decodedLen);
-		return RETURN_OK;
 	}
 }
 
+ReturnValue_t DleEncoder::decodeStreamEscaped(size_t encodedIndex, size_t decodedIndex,
+        const uint8_t *sourceStream, size_t sourceStreamLen, size_t *readLen, uint8_t *destStream,
+        size_t maxDestStreamlen, size_t *decodedLen) {
+    uint8_t nextByte;
+    while ((encodedIndex < sourceStreamLen) && (decodedIndex < maxDestStreamlen)
+            && (sourceStream[encodedIndex] != ETX_CHAR)
+            && (sourceStream[encodedIndex] != STX_CHAR)) {
+        if (sourceStream[encodedIndex] == DLE_CHAR) {
+            nextByte = sourceStream[encodedIndex + 1];
+            // The next byte is a DLE character that was escaped by another
+            // DLE character, so we can write it to the destination stream.
+            if (nextByte == DLE_CHAR) {
+                destStream[decodedIndex] = nextByte;
+            }
+            else {
+                if(this->escapeStxEtx) {
+                    /* The next byte is a STX, DTX or 0x0D character which
+                     * was escaped by a DLE character. The actual byte was
+                     * also encoded by adding + 0x40 to prevent having control chars,
+                     * in the stream at all, so we convert it back. */
+                    if ((nextByte == STX_CHAR + 0x40 or nextByte == ETX_CHAR + 0x40) or
+                            (this->escapeCr and nextByte == CARRIAGE_RETURN + 0x40)) {
+                        destStream[decodedIndex] = nextByte - 0x40;
+                    }
+                    else {
+                        return DECODING_ERROR;
+                    }
+                }
+                else {
+                    return DECODING_ERROR;
+                }
+            }
+            ++encodedIndex;
+        }
+        else {
+            destStream[decodedIndex] = sourceStream[encodedIndex];
+        }
+
+        ++encodedIndex;
+        ++decodedIndex;
+    }
+    if (sourceStream[encodedIndex] != ETX_CHAR) {
+        *readLen = ++encodedIndex;
+        return DECODING_ERROR;
+    }
+    else {
+        *readLen = ++encodedIndex;
+        *decodedLen = decodedIndex;
+        return RETURN_OK;
+    }
+}
+
+ReturnValue_t DleEncoder::decodeStreamNonEscaped(size_t encodedIndex, size_t decodedIndex,
+        const uint8_t *sourceStream, size_t sourceStreamLen, size_t *readLen, uint8_t *destStream,
+        size_t maxDestStreamlen, size_t *decodedLen) {
+    uint8_t nextByte;
+    while ((encodedIndex < sourceStreamLen) && (decodedIndex < maxDestStreamlen)) {
+        if (sourceStream[encodedIndex] == DLE_CHAR) {
+            nextByte = sourceStream[encodedIndex + 1];
+            if(nextByte == STX_CHAR) {
+                *readLen = ++encodedIndex;
+                return DECODING_ERROR;
+            }
+            else if(nextByte == DLE_CHAR) {
+                // The next byte is a DLE character that was escaped by another
+                // DLE character, so we can write it to the destination stream.
+                destStream[decodedIndex] = nextByte;
+                ++encodedIndex;
+            }
+            else if(nextByte == ETX_CHAR) {
+                // End of stream reached
+                return RETURN_OK;
+            }
+        }
+        else {
+            destStream[decodedIndex] = sourceStream[encodedIndex];
+        }
+        ++encodedIndex;
+        ++decodedIndex;
+    }
+    return DECODING_ERROR;
+}
+

src/fsfw/globalfunctions/DleEncoder.h

@@ -90,6 +90,14 @@ public:
             size_t sourceStreamLen, size_t *readLen, uint8_t *destStream,
             size_t maxDestStreamlen, size_t *decodedLen);
 
+    ReturnValue_t decodeStreamEscaped(size_t encodedIndex, size_t decodedIndex,
+            const uint8_t *sourceStream, size_t sourceStreamLen, size_t *readLen,
+            uint8_t *destStream, size_t maxDestStreamlen, size_t *decodedLen);
+
+    ReturnValue_t decodeStreamNonEscaped(size_t encodedIndex, size_t decodedIndex,
+            const uint8_t *sourceStream, size_t sourceStreamLen, size_t *readLen,
+            uint8_t *destStream, size_t maxDestStreamlen, size_t *decodedLen);
+
 private:
 
     bool escapeStxEtx;