84 lines
2.5 KiB
C++
84 lines
2.5 KiB
C++
#include "ArcsecDatalinkLayer.h"
|
|
|
|
extern "C" {
|
|
#include <wire/common/SLIP.h>
|
|
#include <wire/common/misc.h>
|
|
}
|
|
|
|
ArcsecDatalinkLayer::ArcsecDatalinkLayer() : decodeRingBuf(BUFFER_LENGTHS, true) {}
|
|
|
|
ArcsecDatalinkLayer::~ArcsecDatalinkLayer() {}
|
|
|
|
ReturnValue_t ArcsecDatalinkLayer::checkRingBufForFrame(const uint8_t** decodedFrame,
|
|
size_t& frameLen) {
|
|
size_t currentLen = decodeRingBuf.getAvailableReadData();
|
|
if (currentLen == 0) {
|
|
return DEC_IN_PROGRESS;
|
|
}
|
|
decodeRingBuf.readData(rxAnalysisBuffer, currentLen);
|
|
|
|
bool startFound = false;
|
|
size_t startIdx = 0;
|
|
for (size_t idx = 0; idx < currentLen; idx++) {
|
|
if (rxAnalysisBuffer[idx] != SLIP_START_AND_END) {
|
|
continue;
|
|
}
|
|
if (not startFound) {
|
|
startFound = true;
|
|
startIdx = idx;
|
|
continue;
|
|
}
|
|
// Now we can try decoding the whole frame.
|
|
size_t encodedDataSize = 0;
|
|
slip_error_t slipError =
|
|
slip_decode_frame(decodedRxFrame, &rxFrameSize, rxAnalysisBuffer + startIdx,
|
|
idx - startIdx + 1, &encodedDataSize, ARC_DEF_SAGITTA_SLIP_ID, 0);
|
|
decodeRingBuf.deleteData(idx + 1);
|
|
switch (slipError) {
|
|
case (SLIP_OK): {
|
|
if (decodedFrame != nullptr) {
|
|
*decodedFrame = decodedRxFrame;
|
|
}
|
|
frameLen = rxFrameSize;
|
|
return returnvalue::OK;
|
|
}
|
|
case (SLIP_BAD_CRC): {
|
|
return CRC_FAILURE;
|
|
}
|
|
case (SLIP_OVERFLOW): {
|
|
return SLIP_OVERFLOW_RETVAL;
|
|
}
|
|
// Should not happen, we searched for start and end marker..
|
|
case (SLIP_NO_END): {
|
|
return returnvalue::FAILED;
|
|
}
|
|
case (SLIP_ID_MISMATCH): {
|
|
return SLIP_ID_MISSMATCH_RETVAL;
|
|
}
|
|
default: {
|
|
return returnvalue::FAILED;
|
|
}
|
|
}
|
|
}
|
|
return DEC_IN_PROGRESS;
|
|
}
|
|
|
|
ReturnValue_t ArcsecDatalinkLayer::feedData(const uint8_t* rawData, size_t rawDataLen) {
|
|
if (rawDataLen > 4096) {
|
|
sif::error << "ArcsecDatalinklayer: Can not write more than 4096 bytes to ring buffer"
|
|
<< std::endl;
|
|
return returnvalue::FAILED;
|
|
}
|
|
return decodeRingBuf.writeData(rawData, rawDataLen);
|
|
}
|
|
|
|
void ArcsecDatalinkLayer::reset() { decodeRingBuf.clear(); }
|
|
|
|
void ArcsecDatalinkLayer::encodeFrame(const uint8_t* data, size_t length, const uint8_t** txFrame,
|
|
size_t& size) {
|
|
slip_encode_frame(data, length, txEncoded, sizeof(txEncoded), &size, ARC_DEF_SAGITTA_SLIP_ID);
|
|
if (txFrame != nullptr) {
|
|
*txFrame = txEncoded;
|
|
}
|
|
}
|