KSat/fsfw
15
4
Fork 0
forked from fsfw/fsfw
Code Issues Pull Requests 1 Releases Wiki Activity

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

Browse Source
This commit is contained in:
Robin Müller 2021-08-17 15:41:01 +02:00
commit eb6c2ce8c5
2 changed files with 113 additions and 47 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

152
src/fsfw/globalfunctions/DleEncoder.cpp
View File

@ -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 (*sourceStream != STX_CHAR) {
return DECODING_ERROR;
if(not escapeStxEtx) {
if (*sourceStream != DLE_CHAR) {
return DECODING_ERROR;
}
++encodedIndex;
}
if (sourceStream[encodedIndex] != STX_CHAR) {
return DECODING_ERROR;
}
++encodedIndex;
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;
if(escapeStxEtx) {
return decodeStreamEscaped(encodedIndex, decodedIndex, sourceStream, sourceStreamLen,
readLen, destStream, maxDestStreamlen, decodedLen);
}
else {
*readLen = ++encodedIndex;
*decodedLen = decodedIndex;
return RETURN_OK;
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)) {
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;
}

8
src/fsfw/globalfunctions/DleEncoder.h
View File

@ -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;