eive-obsw/mission/acs/str/ArcsecDatalinkLayer.cpp

#include "ArcsecDatalinkLayer.h"

extern "C" {
#include <wire/common/SLIP.h>
#include <wire/common/misc.h>
}

ArcsecDatalinkLayer::ArcsecDatalinkLayer() : decodeRingBuf(BUFFER_LENGTHS, true) { slipInit(); }

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;
    // TODO: Find SLIP ID
    slip_error_t slipError = slip_decode_frame(
        decodedRxFrame, &rxFrameSize, rxAnalysisBuffer + startIdx,
        sizeof(rxAnalysisBuffer) - startIdx, &encodedDataSize, ARC_DEF_SAGITTA_SLIP_ID);
    decodeRingBuf.deleteData(idx + 1);
    switch (slipError) {
      case (SLIP_OK): {
        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;
      }
    }
  }
  //    switch (decResult) {
  //      case ARC_DEC_INPROGRESS: {
  //        break;
  //      }
  //      case ARC_DEC_ERROR: {
  //        decodeRingBuf.deleteData(idx);
  //        return returnvalue::FAILED;
  //      }
  //      case ARC_DEC_ASYNC:
  //      case ARC_DEC_SYNC: {
  //        // Reset length of SLIP struct for next frame
  //        slipInfo.length = 0;
  //        if (decodedFrame != nullptr) {
  //          *decodedFrame = decodedRxFrame;
  //        }
  //        frameLen = rxFrameSize;
  //        decodeRingBuf.deleteData(idx);
  //        return returnvalue::OK;
  //      }
  //      default:
  //        sif::debug << "ArcsecDatalinkLayer::decodeFrame: Unknown result code" << std::endl;
  //        break;
  //        return returnvalue::FAILED;
  //    }
  //  }
  //  decodeRingBuf.deleteData(currentLen);
  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() {
  // slipInit();
  decodeRingBuf.clear();
}

// void ArcsecDatalinkLayer::slipInit() {
//   slip_decode_init(rxBufferArc, sizeof(rxBufferArc), &slipInfo);
// }

void ArcsecDatalinkLayer::encodeFrame(const uint8_t* data, size_t length, const uint8_t** txFrame,
                                      size_t& size) {
  slip_encode_frame(data, length, txEncoded, &size, ARC_DEF_SAGITTA_SLIP_ID);
  if (txFrame != nullptr) {
    *txFrame = txEncoded;
  }
}