eive-obsw/mission/devices/devicedefinitions/rwHelpers.cpp

#ifndef MISSION_DEVICES_DEVICEDEFINITIONS_RWHELPERS_CPP_
#define MISSION_DEVICES_DEVICEDEFINITIONS_RWHELPERS_CPP_

#include "rwHelpers.h"

void rws::encodeHdlc(const uint8_t* sourceBuf, size_t sourceLen, uint8_t* encodedBuffer,
                     size_t& encodedLen) {
  encodedBuffer[0] = rws::FRAME_DELIMITER;
  encodedLen = 1;
  for (size_t sourceIdx = 0; sourceIdx < sourceLen; sourceIdx++) {
    if (sourceBuf[sourceIdx] == 0x7E) {
      encodedBuffer[encodedLen++] = 0x7D;
      encodedBuffer[encodedLen++] = 0x5E;
    } else if (sourceBuf[sourceIdx] == 0x7D) {
      encodedBuffer[encodedLen++] = 0x7D;
      encodedBuffer[encodedLen++] = 0x5D;
    } else {
      encodedBuffer[encodedLen++] = sourceBuf[sourceIdx];
    }
  }
  encodedBuffer[encodedLen++] = rws::FRAME_DELIMITER;
}

#endif /* MISSION_DEVICES_DEVICEDEFINITIONS_RWHELPERS_CPP_ */