diff --git a/linux/devices/ploc/PlocSupvUartMan.cpp b/linux/devices/ploc/PlocSupvUartMan.cpp index 4fe9a033..ff57222d 100644 --- a/linux/devices/ploc/PlocSupvUartMan.cpp +++ b/linux/devices/ploc/PlocSupvUartMan.cpp @@ -970,7 +970,7 @@ ReturnValue_t PlocSupvUartManager::handleRunningLongerRequest() { ReturnValue_t PlocSupvUartManager::encodeAndSendPacket(const uint8_t* sendData, size_t sendLen) { size_t encodedLen = 0; - hdlc_add_framing(sendData, sendLen, encodedSendBuf.data(), &encodedLen); + addHdlcFraming(sendData, sendLen, encodedSendBuf.data(), &encodedLen, encodedSendBuf.size()); sif::debug << "Sending TC" << std::endl; arrayprinter::print(encodedSendBuf.data(), encodedLen); size_t bytesWritten = write(serialPort, encodedSendBuf.data(), encodedLen); @@ -1051,8 +1051,8 @@ ReturnValue_t PlocSupvUartManager::parseRecRingBufForHdlc(size_t& readSize, size if (encodedBuf[idx] == HDLC_END_MARKER) { if (startMarkerFound) { // Probably a packet, so decode it - int retval = hdlc_remove_framing_with_crc_check( - encodedBuf.data() + startIdx, idx + 1 - startIdx, decodedBuf.data(), &decodedLen); + int retval = removeHdlcFramingWithCrcCheck(encodedBuf.data() + startIdx, idx + 1 - startIdx, + decodedBuf.data(), &decodedLen); readSize = idx + 1; if (retval == -1 or retval == -2) { triggerEvent(HDLC_FRAME_REMOVAL_ERROR, retval); @@ -1097,3 +1097,58 @@ void PlocSupvUartManager::performUartShutdown() { } state = InternalState::GO_TO_SLEEP; } + +void PlocSupvUartManager::addHdlcFraming(const uint8_t* src, size_t slen, uint8_t* dst, + size_t* dlen, size_t maxDest) { + size_t tlen = 0; + uint16_t ii; + uint8_t bt; + + // calc crc16 + uint16_t crc16 = calc_crc16_buff_reflected(src, slen); + + dst[tlen++] = 0x7E; + for (ii = 0; ii < slen; ii++) { + bt = *src++; + hdlc_add_byte(bt, dst, &tlen); + } + + size_t dummy = 0; + // hdlc crc16 is in little endian format + SerializeAdapter::serialize(&crc16, dst + tlen, &dummy, maxDest, SerializeIF::Endianness::LITTLE); + tlen += dummy; + + dst[tlen++] = 0x7C; + *dlen = tlen; +} + +int PlocSupvUartManager::removeHdlcFramingWithCrcCheck(const uint8_t* src, size_t slen, + uint8_t* dst, size_t* dlen) { + uint16_t tlen = 0; + uint16_t ii; + uint8_t bt; + + *dlen = 0; + if (slen < 4) return -1; + if ((src[tlen] != 0x7E) && (src[slen - 1] != 0x7C)) return -2; + src++; + for (ii = 1; ii < slen - 1; ii++) { + bt = *src++; + + if (bt == 0x7D) { + bt = *src++ ^ 0x20; + ii++; + } + dst[tlen++] = bt; + } + // calc crc16 + uint16_t calcCrc = calc_crc16_buff_reflected(dst, tlen - 2); + uint16_t crc; + size_t dummy; + SerializeAdapter::deSerialize(&crc, dst + tlen - 2, &dummy, SerializeIF::Endianness::LITTLE); + if (calcCrc != crc) { + return 1; + } + *dlen = tlen - 2; + return 0; +} diff --git a/linux/devices/ploc/PlocSupvUartMan.h b/linux/devices/ploc/PlocSupvUartMan.h index 58abf68b..bc39a477 100644 --- a/linux/devices/ploc/PlocSupvUartMan.h +++ b/linux/devices/ploc/PlocSupvUartMan.h @@ -15,6 +15,7 @@ #include "fsfw/tasks/ExecutableObjectIF.h" #include "fsfw_hal/linux/serial/SerialComIF.h" #include "linux/devices/devicedefinitions/PlocSupervisorDefinitions.h" +#include "tas/crc.h" #ifdef XIPHOS_Q7S #include "bsp_q7s/fs/SdCardManager.h" @@ -259,6 +260,8 @@ class PlocSupvUartManager : public DeviceCommunicationIF, ReturnValue_t handleRunningLongerRequest(); bool handleUartReception(); + void addHdlcFraming(const uint8_t* src, size_t slen, uint8_t* dst, size_t* dlen, size_t maxDest); + int removeHdlcFramingWithCrcCheck(const uint8_t* src, size_t slen, uint8_t* dst, size_t* dlen); ReturnValue_t encodeAndSendPacket(const uint8_t* sendData, size_t sendLen); void executeFullCheckMemoryCommand(); diff --git a/thirdparty/tas/hdlc.c b/thirdparty/tas/hdlc.c index 428a28da..296c8988 100644 --- a/thirdparty/tas/hdlc.c +++ b/thirdparty/tas/hdlc.c @@ -14,9 +14,9 @@ #include -static void hdlc_add_byte(uint8_t ch, uint8_t *buff, uint16_t *pos) +void hdlc_add_byte(uint8_t ch, uint8_t *buff, size_t *pos) { - uint16_t templen = *pos; + size_t templen = *pos; if ((ch == 0x7E) || (ch == 0x7D) || @@ -32,7 +32,7 @@ static void hdlc_add_byte(uint8_t ch, uint8_t *buff, uint16_t *pos) void hdlc_add_framing(const uint8_t *src, size_t slen, uint8_t *dst, size_t *dlen) { - uint16_t tlen = 0; + size_t tlen = 0; uint16_t ii; uint16_t crc16; uint8_t bt; @@ -48,6 +48,9 @@ void hdlc_add_framing(const uint8_t *src, size_t slen, uint8_t *dst, size_t *dle } // hdlc crc16 is in little endian format + // WARNING: This is not portable code! Bytes need to be swapped on a big + // endian system + // TODO: Fix hdlc_add_byte((uint8_t) (crc16 & 0xFF), dst, &tlen); hdlc_add_byte((uint8_t) ((crc16 >> 8) & 0xFF), dst, &tlen); @@ -75,6 +78,7 @@ int hdlc_remove_framing_with_crc_check(const uint8_t *src, size_t slen, uint8_t dst[tlen++] = bt; } // calc crc16 + // TODO: Warning: This does not work because the CRC16 is little endian if(calc_crc16_buff_reflected( dst, tlen ) != 0) { return 1; } diff --git a/thirdparty/tas/tas/crc.h b/thirdparty/tas/tas/crc.h index a9735402..0a45653c 100644 --- a/thirdparty/tas/tas/crc.h +++ b/thirdparty/tas/tas/crc.h @@ -10,6 +10,10 @@ #ifndef TAS_D_C_CRC_H #define TAS_D_C_CRC_H +#ifdef __cplusplus +extern "C" { +#endif + #include #include @@ -104,4 +108,9 @@ void calc_crc16_byte_reflected(uint16_t *crc16, uint8_t bt); * \return CRC result */ uint16_t calc_crc16_buff_reflected(const uint8_t *data, uint16_t len); + +#ifdef __cplusplus +} +#endif + #endif diff --git a/thirdparty/tas/tas/hdlc.h b/thirdparty/tas/tas/hdlc.h index c4179d11..b137fc4c 100644 --- a/thirdparty/tas/tas/hdlc.h +++ b/thirdparty/tas/tas/hdlc.h @@ -26,6 +26,8 @@ extern "C" { #define HDLC_END_BYTE (0x7Cu) #define HDLC_ESCAPE_CHAR (0x20u) +void hdlc_add_byte(uint8_t ch, uint8_t *buff, size_t *pos); + void hdlc_add_framing(const uint8_t *src, size_t slen, uint8_t *dst, size_t *dlen); /**