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,12 +84,32 @@ 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;
 	    }
 	    ++encodedIndex;
+	}
+	if (sourceStream[encodedIndex] != STX_CHAR) {
+	    return DECODING_ERROR;
+	}
 
+	++encodedIndex;
+
+	if(escapeStxEtx) {
+	    return decodeStreamEscaped(encodedIndex, decodedIndex, sourceStream, sourceStreamLen,
+	            readLen, destStream, maxDestStreamlen, decodedLen);
+	}
+	else {
+        return decodeStreamNonEscaped(encodedIndex, decodedIndex, sourceStream, sourceStreamLen,
+                readLen, destStream, maxDestStreamlen, decodedLen);
+	}
+}
+
+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)) {
@@ -119,7 +147,6 @@ ReturnValue_t DleEncoder::decode(const uint8_t *sourceStream,
         ++encodedIndex;
         ++decodedIndex;
     }
-
     if (sourceStream[encodedIndex] != ETX_CHAR) {
         *readLen = ++encodedIndex;
         return DECODING_ERROR;
@@ -131,3 +158,34 @@ ReturnValue_t DleEncoder::decode(const uint8_t *sourceStream,
     }
 }
 
+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;