src/fsfw/action/ActionHelper.cpp

@@ -32,6 +32,17 @@ ReturnValue_t ActionHelper::initialize(MessageQueueIF* queueToUse_) {
         setQueueToUse(queueToUse_);
     }
 
+    if(queueToUse == nullptr) {
+#if FSFW_VERBOSE_LEVEL >= 1
+#if FSFW_CPP_OSTREAM_ENABLED == 1
+        sif::warning << "ActionHelper::initialize: No queue set" << std::endl;
+#else
+        sif::printWarning("ActionHelper::initialize: No queue set\n");
+#endif
+#endif /* FSFW_VERBOSE_LEVEL >= 1 */
+        return HasReturnvaluesIF::RETURN_FAILED;
+    }
+
     return HasReturnvaluesIF::RETURN_OK;
 }
 

src/fsfw/globalfunctions/DleEncoder.cpp

@@ -1,27 +1,41 @@
 #include "fsfw/globalfunctions/DleEncoder.h"
 
-DleEncoder::DleEncoder() {}
+DleEncoder::DleEncoder(bool escapeStxEtx, bool escapeCr):
+        escapeStxEtx(escapeStxEtx), escapeCr(escapeCr) {}
 
 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;
+    if(escapeStxEtx) {
+        return encodeStreamEscaped(sourceStream, sourceLen,
+                destStream, maxDestLen, encodedLen, addStxEtx);
     }
-	size_t encodedIndex = 0, sourceIndex = 0;
-	uint8_t nextByte;
-	if (addStxEtx) {
-		destStream[0] = STX_CHAR;
-		++encodedIndex;
+    else {
+        return encodeStreamNonEscaped(sourceStream, sourceLen,
+                destStream, maxDestLen, encodedLen, addStxEtx);
     }
 
-	while (encodedIndex < maxDestLen and sourceIndex < sourceLen)
-	{
+}
+
+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 = 0;
+    size_t sourceIndex = 0;
+    uint8_t nextByte = 0;
+    if(addStxEtx) {
+        if(maxDestLen < 1) {
+            return STREAM_TOO_SHORT;
+        }
+        destStream[encodedIndex++] = STX_CHAR;
+    }
+    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 nextByte == CARRIAGE_RETURN) {
+        if ((nextByte == STX_CHAR or nextByte == ETX_CHAR) or
+                (this->escapeCr and nextByte == CARRIAGE_RETURN)) {
             if (encodedIndex + 1 >= maxDestLen) {
                 return STREAM_TOO_SHORT;
             }
@@ -56,8 +70,11 @@ ReturnValue_t DleEncoder::encode(const uint8_t* sourceStream,
         ++sourceIndex;
     }
 
-	if (sourceIndex == sourceLen and encodedIndex < maxDestLen) {
+    if (sourceIndex == sourceLen) {
         if (addStxEtx) {
+            if(encodedIndex + 1 >= maxDestLen) {
+                return STREAM_TOO_SHORT;
+            }
             destStream[encodedIndex] = ETX_CHAR;
             ++encodedIndex;
         }
@@ -69,20 +86,95 @@ ReturnValue_t DleEncoder::encode(const uint8_t* sourceStream,
     }
 }
 
+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 = 0;
+    size_t sourceIndex = 0;
+    uint8_t nextByte = 0;
+    if(addStxEtx) {
+        if(maxDestLen < 2) {
+            return STREAM_TOO_SHORT;
+        }
+        destStream[encodedIndex++] = DLE_CHAR;
+        destStream[encodedIndex++] = STX_CHAR;
+    }
+    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) {
+        if (addStxEtx) {
+            if(encodedIndex + 2 >= maxDestLen) {
+                return STREAM_TOO_SHORT;
+            }
+            destStream[encodedIndex++] = DLE_CHAR;
+            destStream[encodedIndex++] = ETX_CHAR;
+        }
+        *encodedLen = encodedIndex;
+        return RETURN_OK;
+    }
+    else {
+        return STREAM_TOO_SHORT;
+    }
+}
+
 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;
+    if(escapeStxEtx) {
+        return decodeStreamEscaped(sourceStream, sourceStreamLen,
+                readLen, destStream, maxDestStreamlen, decodedLen);
+    }
+    else {
+        return decodeStreamNonEscaped(sourceStream, sourceStreamLen,
+                readLen, destStream, maxDestStreamlen, decodedLen);
+    }
+}
+
+ReturnValue_t DleEncoder::decodeStreamEscaped(const uint8_t *sourceStream, size_t sourceStreamLen,
+        size_t *readLen, uint8_t *destStream,
+        size_t maxDestStreamlen, size_t *decodedLen) {
+    size_t encodedIndex = 0;
+    size_t decodedIndex = 0;
     uint8_t nextByte;
-	if (*sourceStream != STX_CHAR) {
+
+    //init to 0 so that we can just return in the first checks (which do not consume anything from
+    //the source stream)
+    *readLen = 0;
+
+    if(maxDestStreamlen < 1) {
+        return STREAM_TOO_SHORT;
+    }
+    if (sourceStream[encodedIndex++] != STX_CHAR) {
         return DECODING_ERROR;
     }
-	++encodedIndex;
-
-	while ((encodedIndex < sourceStreamLen) && (decodedIndex < maxDestStreamlen)
-			&& (sourceStream[encodedIndex] != ETX_CHAR)
-			&& (sourceStream[encodedIndex] != STX_CHAR)) {
+    while ((encodedIndex < sourceStreamLen)
+            and (decodedIndex < maxDestStreamlen)
+            and (sourceStream[encodedIndex] != ETX_CHAR)
+            and (sourceStream[encodedIndex] != STX_CHAR)) {
         if (sourceStream[encodedIndex] == DLE_CHAR) {
+            if(encodedIndex + 1 >= sourceStreamLen) {
+                //reached the end of the sourceStream
+                *readLen = sourceStreamLen;
+                return DECODING_ERROR;
+            }
             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.
@@ -94,10 +186,13 @@ ReturnValue_t DleEncoder::decode(const uint8_t *sourceStream,
                  * 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 == 0x42 or nextByte == 0x43 or nextByte == 0x4D) {
+                if ((nextByte == STX_CHAR + 0x40 or nextByte == ETX_CHAR + 0x40) or
+                        (this->escapeCr and nextByte == CARRIAGE_RETURN + 0x40)) {
                     destStream[decodedIndex] = nextByte - 0x40;
                 }
                 else {
+                    // Set readLen so user can resume parsing after incorrect data
+                    *readLen = encodedIndex + 2;
                     return DECODING_ERROR;
                 }
             }
@@ -110,11 +205,17 @@ ReturnValue_t DleEncoder::decode(const uint8_t *sourceStream,
         ++encodedIndex;
         ++decodedIndex;
     }
-
     if (sourceStream[encodedIndex] != ETX_CHAR) {
+        if(decodedIndex == maxDestStreamlen) {
+            //so far we did not find anything wrong here, so let user try again
+            *readLen = 0;
+            return STREAM_TOO_SHORT;
+        }
+        else {
             *readLen = ++encodedIndex;
             return DECODING_ERROR;
         }
+    }
     else {
         *readLen = ++encodedIndex;
         *decodedLen = decodedIndex;
@@ -122,3 +223,74 @@ ReturnValue_t DleEncoder::decode(const uint8_t *sourceStream,
     }
 }
 
+ReturnValue_t DleEncoder::decodeStreamNonEscaped(const uint8_t *sourceStream,
+        size_t sourceStreamLen, size_t *readLen, uint8_t *destStream,
+        size_t maxDestStreamlen, size_t *decodedLen) {
+    size_t encodedIndex = 0;
+    size_t decodedIndex = 0;
+    uint8_t nextByte;
+
+    //init to 0 so that we can just return in the first checks (which do not consume anything from
+    //the source stream)
+    *readLen = 0;
+    
+    if(maxDestStreamlen < 2) {
+        return STREAM_TOO_SHORT;
+    }
+    if (sourceStream[encodedIndex++] != DLE_CHAR) {
+        return DECODING_ERROR;
+    }
+    if (sourceStream[encodedIndex++] != STX_CHAR) {
+        *readLen = 1;
+        return DECODING_ERROR;
+    }
+    while ((encodedIndex < sourceStreamLen) && (decodedIndex < maxDestStreamlen)) {
+        if (sourceStream[encodedIndex] == DLE_CHAR) {
+            if(encodedIndex + 1 >= sourceStreamLen) {
+                *readLen = encodedIndex;
+                return DECODING_ERROR;
+            }
+            nextByte = sourceStream[encodedIndex + 1];
+            if(nextByte == STX_CHAR) {
+                // Set readLen so the DLE/STX char combination is preserved. Could be start of
+                // another frame
+                *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
+                *readLen = encodedIndex + 2;
+                *decodedLen = decodedIndex;
+                return RETURN_OK;
+            }
+            else {
+                *readLen = encodedIndex;
+                return DECODING_ERROR;
+            }
+        }
+        else {
+            destStream[decodedIndex] = sourceStream[encodedIndex];
+        }
+        ++encodedIndex;
+        ++decodedIndex;
+    }
+
+    if(decodedIndex == maxDestStreamlen) {
+        //so far we did not find anything wrong here, so let user try again
+        *readLen = 0;
+        return STREAM_TOO_SHORT;
+    } else {
+        *readLen = encodedIndex;
+        return DECODING_ERROR;
+    }
+}
+
+void DleEncoder::setEscapeMode(bool escapeStxEtx) {
+    this->escapeStxEtx = escapeStxEtx;
+}

src/fsfw/globalfunctions/DleEncoder.h

@@ -1,7 +1,7 @@
 #ifndef FRAMEWORK_GLOBALFUNCTIONS_DLEENCODER_H_
 #define FRAMEWORK_GLOBALFUNCTIONS_DLEENCODER_H_
 
-#include "../returnvalues/HasReturnvaluesIF.h"
+#include "fsfw/returnvalues/HasReturnvaluesIF.h"
 #include <cstddef>
 
 /**
@@ -12,24 +12,36 @@
  * https://en.wikipedia.org/wiki/C0_and_C1_control_codes
  *
  * This encoder can be used to achieve a basic transport layer when using
- * char based transmission systems.
- * The passed source strean is converted into a encoded stream by adding
- * a STX marker at the start of the stream and an ETX marker at the end of
- * the stream. Any STX, ETX, DLE and CR occurrences in the source stream are
- * escaped by a DLE character. The encoder also replaces escaped control chars
- * by another char, so STX, ETX and CR should not appear anywhere in the actual
- * encoded data stream.
+ * char based transmission systems. There are two implemented variants:
  *
- * When using a strictly char based reception of packets encoded with DLE,
+ * 1. Escaped variant
+ *
+ * The encoded stream starts with a STX marker and ends with an ETX marker.
+ * STX and ETX occurrences in the stream are escaped and internally encoded as well so the
+ * receiver side can simply check for STX and ETX markers as frame delimiters. When using a
+ * strictly char based reception of packets encoded with DLE,
  * STX can be used to notify a reader that actual data will start to arrive
  * while ETX can be used to notify the reader that the data has ended.
+ *
+ * 2. Non-escaped variant
+ *
+ * The encoded stream starts with DLE STX and ends with DLE ETX. All DLE occurrences in the stream
+ * are escaped with DLE. If the receiver detects a DLE char, it needs to read the next char
+ * to determine whether a start (STX) or end (ETX) of a frame has been detected.
  */
 class DleEncoder: public HasReturnvaluesIF {
-private:
-	DleEncoder();
+public:
+    /**
+     * Create an encoder instance with the given configuration.
+     * @param escapeStxEtx  Determines whether the algorithm works in escaped or non-escaped mode
+     * @param escapeCr      In escaped mode, escape all CR occurrences as well
+     */
+    DleEncoder(bool escapeStxEtx = true, bool escapeCr = false);
+
+    void setEscapeMode(bool escapeStxEtx);
+
     virtual ~DleEncoder();
 
-public:
     static constexpr uint8_t INTERFACE_ID = CLASS_ID::DLE_ENCODER;
     static constexpr ReturnValue_t STREAM_TOO_SHORT = MAKE_RETURN_CODE(0x01);
     static constexpr ReturnValue_t DECODING_ERROR = MAKE_RETURN_CODE(0x02);
@@ -45,19 +57,24 @@ public:
 
     /**
      * Encodes the give data stream by preceding it with the STX marker
-     * and ending it with an ETX marker. STX, ETX and DLE characters inside
-     * the stream are escaped by DLE characters and also replaced by adding
-     * 0x40 (which is reverted in the decoding process).
+     * and ending it with an ETX marker. DLE characters inside
+     * the stream are escaped by DLE characters. STX, ETX and CR characters can be escaped with a
+     * DLE character as well. The escaped characters are also encoded by adding
+     * 0x40 (which is reverted in the decoding process). This is performed so the source stream
+     * does not have STX/ETX/CR occurrences anymore, so the receiving side can simply parse for
+     * start and end markers
      * @param sourceStream
      * @param sourceLen
      * @param destStream
      * @param maxDestLen
      * @param encodedLen
-     * @param addStxEtx
-     * Adding STX and ETX can be omitted, if they are added manually.
+     * @param addStxEtx     Adding STX start marker and ETX end marker can be omitted,
+     *                      if they are added manually
      * @return
+     *  - RETURN_OK for successful encoding operation
+     *  - STREAM_TOO_SHORT if the destination stream is too short
      */
-    static ReturnValue_t encode(const uint8_t *sourceStream, size_t sourceLen,
+    ReturnValue_t encode(const uint8_t *sourceStream, size_t sourceLen,
             uint8_t *destStream, size_t maxDestLen, size_t *encodedLen,
             bool addStxEtx = true);
 
@@ -70,10 +87,32 @@ public:
      * @param maxDestStreamlen
      * @param decodedLen
      * @return
+     *  - RETURN_OK for successful decode operation
+     *  - DECODE_ERROR if the source stream is invalid
+     *  - STREAM_TOO_SHORT if the destination stream is too short
      */
-	static ReturnValue_t decode(const uint8_t *sourceStream,
+    ReturnValue_t decode(const uint8_t *sourceStream,
             size_t sourceStreamLen, size_t *readLen, uint8_t *destStream,
             size_t maxDestStreamlen, size_t *decodedLen);
+
+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);
+
+    ReturnValue_t decodeStreamNonEscaped(const uint8_t *sourceStream, size_t sourceStreamLen,
+            size_t *readLen, uint8_t *destStream, size_t maxDestStreamlen, size_t *decodedLen);
+
+    bool escapeStxEtx;
+    bool escapeCr;
 };
 
 #endif /* FRAMEWORK_GLOBALFUNCTIONS_DLEENCODER_H_ */

tests/src/fsfw_tests/unit/CatchFactory.h

@@ -1,7 +1,7 @@
 #ifndef FSFW_CATCHFACTORY_H_
 #define FSFW_CATCHFACTORY_H_
 
-#include "TestConfig.h"
+#include "TestsConfig.h"
 #include "fsfw/objectmanager/SystemObjectIF.h"
 #include "fsfw/objectmanager/ObjectManager.h"
 

tests/src/fsfw_tests/unit/globalfunctions/CMakeLists.txt

@@ -1,2 +1,3 @@
 target_sources(${TARGET_NAME} PRIVATE
+    testDleEncoder.cpp
 )

tests/src/fsfw_tests/unit/globalfunctions/testDleEncoder.cpp

@@ -0,0 +1,222 @@
+#include "fsfw/globalfunctions/DleEncoder.h"
+#include "fsfw_tests/unit/CatchDefinitions.h"
+#include "catch2/catch_test_macros.hpp"
+
+#include <array>
+
+const std::vector<uint8_t> TEST_ARRAY_0 = { 0, 0, 0, 0, 0 };
+const std::vector<uint8_t> TEST_ARRAY_1 = { 0, DleEncoder::DLE_CHAR, 5};
+const std::vector<uint8_t> TEST_ARRAY_2 = { 0, DleEncoder::STX_CHAR, 5};
+const std::vector<uint8_t> TEST_ARRAY_3 = { 0, DleEncoder::CARRIAGE_RETURN, DleEncoder::ETX_CHAR};
+const std::vector<uint8_t> TEST_ARRAY_4 = { DleEncoder::DLE_CHAR, DleEncoder::ETX_CHAR,
+        DleEncoder::STX_CHAR };
+
+const std::vector<uint8_t> TEST_ARRAY_0_ENCODED_ESCAPED = {
+        DleEncoder::STX_CHAR, 0, 0, 0, 0, 0, DleEncoder::ETX_CHAR
+};
+const std::vector<uint8_t> TEST_ARRAY_0_ENCODED_NON_ESCAPED = {
+        DleEncoder::DLE_CHAR, DleEncoder::STX_CHAR, 0, 0, 0, 0, 0,
+        DleEncoder::DLE_CHAR, DleEncoder::ETX_CHAR
+};
+
+const std::vector<uint8_t> TEST_ARRAY_1_ENCODED_ESCAPED = {
+        DleEncoder::STX_CHAR, 0, DleEncoder::DLE_CHAR, DleEncoder::DLE_CHAR, 5, DleEncoder::ETX_CHAR
+};
+const std::vector<uint8_t> TEST_ARRAY_1_ENCODED_NON_ESCAPED = {
+        DleEncoder::DLE_CHAR, DleEncoder::STX_CHAR, 0, DleEncoder::DLE_CHAR, DleEncoder::DLE_CHAR,
+        5, DleEncoder::DLE_CHAR, DleEncoder::ETX_CHAR
+};
+
+const std::vector<uint8_t> TEST_ARRAY_2_ENCODED_ESCAPED = {
+        DleEncoder::STX_CHAR, 0, DleEncoder::DLE_CHAR, DleEncoder::STX_CHAR + 0x40,
+        5, DleEncoder::ETX_CHAR
+};
+const std::vector<uint8_t> TEST_ARRAY_2_ENCODED_NON_ESCAPED = {
+        DleEncoder::DLE_CHAR, DleEncoder::STX_CHAR, 0,
+        DleEncoder::STX_CHAR, 5, DleEncoder::DLE_CHAR, DleEncoder::ETX_CHAR
+};
+
+const std::vector<uint8_t> TEST_ARRAY_3_ENCODED_ESCAPED = {
+        DleEncoder::STX_CHAR, 0, DleEncoder::CARRIAGE_RETURN,
+        DleEncoder::DLE_CHAR,  DleEncoder::ETX_CHAR + 0x40, DleEncoder::ETX_CHAR
+};
+const std::vector<uint8_t> TEST_ARRAY_3_ENCODED_NON_ESCAPED = {
+        DleEncoder::DLE_CHAR, DleEncoder::STX_CHAR, 0,
+        DleEncoder::CARRIAGE_RETURN, DleEncoder::ETX_CHAR, DleEncoder::DLE_CHAR,
+        DleEncoder::ETX_CHAR
+};
+
+const std::vector<uint8_t> TEST_ARRAY_4_ENCODED_ESCAPED = {
+        DleEncoder::STX_CHAR, DleEncoder::DLE_CHAR, DleEncoder::DLE_CHAR,
+        DleEncoder::DLE_CHAR, DleEncoder::ETX_CHAR + 0x40, DleEncoder::DLE_CHAR,
+        DleEncoder::STX_CHAR + 0x40, DleEncoder::ETX_CHAR
+};
+const std::vector<uint8_t> TEST_ARRAY_4_ENCODED_NON_ESCAPED = {
+        DleEncoder::DLE_CHAR, DleEncoder::STX_CHAR, DleEncoder::DLE_CHAR, DleEncoder::DLE_CHAR,
+        DleEncoder::ETX_CHAR, DleEncoder::STX_CHAR, DleEncoder::DLE_CHAR, DleEncoder::ETX_CHAR
+};
+
+
+TEST_CASE("DleEncoder" , "[DleEncoder]") {
+    DleEncoder dleEncoder;
+    ReturnValue_t result = HasReturnvaluesIF::RETURN_OK;
+    std::array<uint8_t, 32> buffer;
+
+    size_t encodedLen = 0;
+    size_t readLen = 0;
+    size_t decodedLen = 0;
+
+    auto testLambdaEncode = [&](DleEncoder& encoder, const std::vector<uint8_t>& vecToEncode,
+            const std::vector<uint8_t>& expectedVec) {
+        result = encoder.encode(vecToEncode.data(), vecToEncode.size(),
+                buffer.data(), buffer.size(), &encodedLen);
+        REQUIRE(result == retval::CATCH_OK);
+        for(size_t idx = 0; idx < expectedVec.size(); idx++) {
+            REQUIRE(buffer[idx] == expectedVec[idx]);
+        }
+        REQUIRE(encodedLen == expectedVec.size());
+    };
+
+    auto testLambdaDecode = [&](DleEncoder& encoder, const std::vector<uint8_t>& testVecEncoded,
+            const std::vector<uint8_t>& expectedVec) {
+        result = encoder.decode(testVecEncoded.data(),
+                testVecEncoded.size(),
+                &readLen, buffer.data(), buffer.size(), &decodedLen);
+        REQUIRE(result == retval::CATCH_OK);
+        REQUIRE(readLen == testVecEncoded.size());
+        REQUIRE(decodedLen == expectedVec.size());
+        for(size_t idx = 0; idx < decodedLen; idx++) {
+            REQUIRE(buffer[idx] == expectedVec[idx]);
+        }
+    };
+
+    SECTION("Encoding") {
+        testLambdaEncode(dleEncoder, TEST_ARRAY_0, TEST_ARRAY_0_ENCODED_ESCAPED);
+        testLambdaEncode(dleEncoder, TEST_ARRAY_1, TEST_ARRAY_1_ENCODED_ESCAPED);
+        testLambdaEncode(dleEncoder, TEST_ARRAY_2, TEST_ARRAY_2_ENCODED_ESCAPED);
+        testLambdaEncode(dleEncoder, TEST_ARRAY_3, TEST_ARRAY_3_ENCODED_ESCAPED);
+        testLambdaEncode(dleEncoder, TEST_ARRAY_4, TEST_ARRAY_4_ENCODED_ESCAPED);
+
+        auto testFaultyEncoding = [&](const std::vector<uint8_t>& vecToEncode,
+                const std::vector<uint8_t>& expectedVec) {
+
+            for(size_t faultyDestSize = 0; faultyDestSize < expectedVec.size(); faultyDestSize ++) {
+                result = dleEncoder.encode(vecToEncode.data(), vecToEncode.size(),
+                        buffer.data(), faultyDestSize, &encodedLen);
+                REQUIRE(result == DleEncoder::STREAM_TOO_SHORT);
+            }
+        };
+
+        testFaultyEncoding(TEST_ARRAY_0, TEST_ARRAY_0_ENCODED_ESCAPED);
+        testFaultyEncoding(TEST_ARRAY_1, TEST_ARRAY_1_ENCODED_ESCAPED);
+        testFaultyEncoding(TEST_ARRAY_2, TEST_ARRAY_2_ENCODED_ESCAPED);
+        testFaultyEncoding(TEST_ARRAY_3, TEST_ARRAY_3_ENCODED_ESCAPED);
+        testFaultyEncoding(TEST_ARRAY_4, TEST_ARRAY_4_ENCODED_ESCAPED);
+
+        dleEncoder.setEscapeMode(false);
+        testLambdaEncode(dleEncoder, TEST_ARRAY_0, TEST_ARRAY_0_ENCODED_NON_ESCAPED);
+        testLambdaEncode(dleEncoder, TEST_ARRAY_1, TEST_ARRAY_1_ENCODED_NON_ESCAPED);
+        testLambdaEncode(dleEncoder, TEST_ARRAY_2, TEST_ARRAY_2_ENCODED_NON_ESCAPED);
+        testLambdaEncode(dleEncoder, TEST_ARRAY_3, TEST_ARRAY_3_ENCODED_NON_ESCAPED);
+        testLambdaEncode(dleEncoder, TEST_ARRAY_4, TEST_ARRAY_4_ENCODED_NON_ESCAPED);
+
+        testFaultyEncoding(TEST_ARRAY_0, TEST_ARRAY_0_ENCODED_NON_ESCAPED);
+        testFaultyEncoding(TEST_ARRAY_1, TEST_ARRAY_1_ENCODED_NON_ESCAPED);
+        testFaultyEncoding(TEST_ARRAY_2, TEST_ARRAY_2_ENCODED_NON_ESCAPED);
+        testFaultyEncoding(TEST_ARRAY_3, TEST_ARRAY_3_ENCODED_NON_ESCAPED);
+        testFaultyEncoding(TEST_ARRAY_4, TEST_ARRAY_4_ENCODED_NON_ESCAPED);
+        dleEncoder.setEscapeMode(true);
+    }
+
+    SECTION("Decoding") {
+        testLambdaDecode(dleEncoder, TEST_ARRAY_0_ENCODED_ESCAPED, TEST_ARRAY_0);
+        testLambdaDecode(dleEncoder, TEST_ARRAY_1_ENCODED_ESCAPED, TEST_ARRAY_1);
+        testLambdaDecode(dleEncoder, TEST_ARRAY_2_ENCODED_ESCAPED, TEST_ARRAY_2);
+        testLambdaDecode(dleEncoder, TEST_ARRAY_3_ENCODED_ESCAPED, TEST_ARRAY_3);
+        testLambdaDecode(dleEncoder, TEST_ARRAY_4_ENCODED_ESCAPED, TEST_ARRAY_4);
+
+        // Faulty source data
+        auto testArray1EncodedFaulty = TEST_ARRAY_1_ENCODED_ESCAPED;
+        testArray1EncodedFaulty[3] = 0;
+        result = dleEncoder.decode(testArray1EncodedFaulty.data(), testArray1EncodedFaulty.size(),
+                &readLen, buffer.data(), buffer.size(), &encodedLen);
+        REQUIRE(result == static_cast<int>(DleEncoder::DECODING_ERROR));
+        auto testArray2EncodedFaulty = TEST_ARRAY_2_ENCODED_ESCAPED;
+        testArray2EncodedFaulty[5] = 0;
+        result = dleEncoder.decode(testArray2EncodedFaulty.data(), testArray2EncodedFaulty.size(),
+                &readLen, buffer.data(), buffer.size(), &encodedLen);
+        REQUIRE(result == static_cast<int>(DleEncoder::DECODING_ERROR));
+        auto testArray4EncodedFaulty = TEST_ARRAY_4_ENCODED_ESCAPED;
+        testArray4EncodedFaulty[2] = 0;
+        result = dleEncoder.decode(testArray4EncodedFaulty.data(), testArray4EncodedFaulty.size(),
+                &readLen, buffer.data(), buffer.size(), &encodedLen);
+        REQUIRE(result == static_cast<int>(DleEncoder::DECODING_ERROR));
+        auto testArray4EncodedFaulty2 = TEST_ARRAY_4_ENCODED_ESCAPED;
+        testArray4EncodedFaulty2[4] = 0;
+        result = dleEncoder.decode(testArray4EncodedFaulty2.data(), testArray4EncodedFaulty2.size(),
+                &readLen, buffer.data(), buffer.size(), &encodedLen);
+        REQUIRE(result == static_cast<int>(DleEncoder::DECODING_ERROR));
+
+        auto testFaultyDecoding = [&](const std::vector<uint8_t>& vecToDecode,
+                const std::vector<uint8_t>& expectedVec) {
+            for(size_t faultyDestSizes = 0;
+                    faultyDestSizes < expectedVec.size();
+                    faultyDestSizes ++) {
+                result = dleEncoder.decode(vecToDecode.data(),
+                        vecToDecode.size(), &readLen,
+                        buffer.data(), faultyDestSizes, &decodedLen);
+                REQUIRE(result == static_cast<int>(DleEncoder::STREAM_TOO_SHORT));
+            }
+        };
+
+        testFaultyDecoding(TEST_ARRAY_0_ENCODED_ESCAPED, TEST_ARRAY_0);
+        testFaultyDecoding(TEST_ARRAY_1_ENCODED_ESCAPED, TEST_ARRAY_1);
+        testFaultyDecoding(TEST_ARRAY_2_ENCODED_ESCAPED, TEST_ARRAY_2);
+        testFaultyDecoding(TEST_ARRAY_3_ENCODED_ESCAPED, TEST_ARRAY_3);
+        testFaultyDecoding(TEST_ARRAY_4_ENCODED_ESCAPED, TEST_ARRAY_4);
+
+        dleEncoder.setEscapeMode(false);
+        testLambdaDecode(dleEncoder, TEST_ARRAY_0_ENCODED_NON_ESCAPED, TEST_ARRAY_0);
+        testLambdaDecode(dleEncoder, TEST_ARRAY_1_ENCODED_NON_ESCAPED, TEST_ARRAY_1);
+        testLambdaDecode(dleEncoder, TEST_ARRAY_2_ENCODED_NON_ESCAPED, TEST_ARRAY_2);
+        testLambdaDecode(dleEncoder, TEST_ARRAY_3_ENCODED_NON_ESCAPED, TEST_ARRAY_3);
+        testLambdaDecode(dleEncoder, TEST_ARRAY_4_ENCODED_NON_ESCAPED, TEST_ARRAY_4);
+
+        testFaultyDecoding(TEST_ARRAY_0_ENCODED_NON_ESCAPED, TEST_ARRAY_0);
+        testFaultyDecoding(TEST_ARRAY_1_ENCODED_NON_ESCAPED, TEST_ARRAY_1);
+        testFaultyDecoding(TEST_ARRAY_2_ENCODED_NON_ESCAPED, TEST_ARRAY_2);
+        testFaultyDecoding(TEST_ARRAY_3_ENCODED_NON_ESCAPED, TEST_ARRAY_3);
+        testFaultyDecoding(TEST_ARRAY_4_ENCODED_NON_ESCAPED, TEST_ARRAY_4);
+
+        // Faulty source data
+        testArray1EncodedFaulty = TEST_ARRAY_1_ENCODED_NON_ESCAPED;
+        auto prevVal = testArray1EncodedFaulty[0];
+        testArray1EncodedFaulty[0] = 0;
+        result = dleEncoder.decode(testArray1EncodedFaulty.data(), testArray1EncodedFaulty.size(),
+                &readLen, buffer.data(), buffer.size(), &encodedLen);
+        REQUIRE(result == static_cast<int>(DleEncoder::DECODING_ERROR));
+        testArray1EncodedFaulty[0] = prevVal;
+        testArray1EncodedFaulty[1] = 0;
+        result = dleEncoder.decode(testArray1EncodedFaulty.data(), testArray1EncodedFaulty.size(),
+                &readLen, buffer.data(), buffer.size(), &encodedLen);
+        REQUIRE(result == static_cast<int>(DleEncoder::DECODING_ERROR));
+
+        testArray1EncodedFaulty = TEST_ARRAY_1_ENCODED_NON_ESCAPED;
+        testArray1EncodedFaulty[6] = 0;
+        result = dleEncoder.decode(testArray1EncodedFaulty.data(), testArray1EncodedFaulty.size(),
+                &readLen, buffer.data(), buffer.size(), &encodedLen);
+        REQUIRE(result == static_cast<int>(DleEncoder::DECODING_ERROR));
+        testArray1EncodedFaulty = TEST_ARRAY_1_ENCODED_NON_ESCAPED;
+        testArray1EncodedFaulty[7] = 0;
+        result = dleEncoder.decode(testArray1EncodedFaulty.data(), testArray1EncodedFaulty.size(),
+                &readLen, buffer.data(), buffer.size(), &encodedLen);
+        REQUIRE(result == static_cast<int>(DleEncoder::DECODING_ERROR));
+        testArray4EncodedFaulty = TEST_ARRAY_4_ENCODED_NON_ESCAPED;
+        testArray4EncodedFaulty[3] = 0;
+        result = dleEncoder.decode(testArray4EncodedFaulty.data(), testArray4EncodedFaulty.size(),
+                &readLen, buffer.data(), buffer.size(), &encodedLen);
+        REQUIRE(result == static_cast<int>(DleEncoder::DECODING_ERROR));
+
+        dleEncoder.setEscapeMode(true);
+    }
+}