76 lines
2.9 KiB
C++
76 lines
2.9 KiB
C++
|
#include "MpsocCommunication.h"
|
||
|
|
|
||
|
|
#include "fsfw/globalfunctions/CRC.h"
|
||
|
|
#include "fsfw/returnvalues/returnvalue.h"
|
||
|
|
#include "fsfw/tmtcpacket/ccsds/SpacePacketReader.h"
|
||
|
|
#include "fsfw/tmtcpacket/ccsds/header.h"
|
||
|
|
#include "linux/payload/plocMpsocHelpers.h"
|
||
|
|
#include "unistd.h"
|
||
|
|
|
||
|
|
MpsocCommunication::MpsocCommunication(object_id_t objectId, SerialConfig cfg)
|
||
|
|
: SystemObject(objectId), readRingBuf(4096, true), helper(cfg) {}
|
||
|
|
|
||
|
|
ReturnValue_t MpsocCommunication::initialize() { return helper.initialize(); }
|
||
|
|
|
||
|
|
ReturnValue_t MpsocCommunication::send(const uint8_t* data, size_t dataLen) {
|
||
|
|
if (MPSOC_LOW_LEVEL_TX_WIRETAPPING) {
|
||
|
|
sif::debug << "SEND MPSOC packet with size " << dataLen << std::endl;
|
||
|
|
}
|
||
|
|
return helper.send(data, dataLen);
|
||
|
|
}
|
||
|
|
|
||
|
|
ReturnValue_t MpsocCommunication::parseAndRetrieveNextPacket() {
|
||
|
|
// We do not have a data link layer, so this whole thing is a mess in any case..
|
||
|
|
// But basically, we try to parse space packets from the internal ring buffer and trasnfer
|
||
|
|
// them to the higher level device handler. The CRC check is performed here as well, with
|
||
|
|
// few other ways to detect if we even have a valid packet.
|
||
|
|
size_t availableReadData = readRingBuf.getAvailableReadData();
|
||
|
|
// Minimum valid size for a space packet header.
|
||
|
|
if (availableReadData < ccsds::HEADER_LEN + 1) {
|
||
|
|
return returnvalue::OK;
|
||
|
|
}
|
||
|
|
readRingBuf.readData(readBuf, availableReadData);
|
||
|
|
spReader.setReadOnlyData(readBuf, sizeof(readBuf));
|
||
|
|
auto res = spReader.checkSize();
|
||
|
|
if (res != returnvalue::OK) {
|
||
|
|
return res;
|
||
|
|
}
|
||
|
|
// The packet might be garbage, with no way to recover without a data link layer.
|
||
|
|
if (spReader.getFullPacketLen() > 4096) {
|
||
|
|
readRingBuf.clear();
|
||
|
|
// TODO: Maybe we should also clear the serial input buffer in Linux?
|
||
|
|
return FAULTY_PACKET_SIZE;
|
||
|
|
}
|
||
|
|
if (availableReadData < spReader.getFullPacketLen()) {
|
||
|
|
// Might be split packet where the rest still has to be read.
|
||
|
|
return returnvalue::OK;
|
||
|
|
}
|
||
|
|
if (CRC::crc16ccitt(readBuf, spReader.getFullPacketLen()) != 0) {
|
||
|
|
// Possibly invalid packet. We can not even trust the detected packet length.
|
||
|
|
// Just clear the whole read buffer as well.
|
||
|
|
readRingBuf.clear();
|
||
|
|
triggerEvent(mpsoc::CRC_FAILURE);
|
||
|
|
return CRC_CHECK_FAILED;
|
||
|
|
}
|
||
|
|
readRingBuf.deleteData(spReader.getFullPacketLen());
|
||
|
|
return PACKET_RECEIVED;
|
||
|
|
}
|
||
|
|
|
||
|
|
ReturnValue_t MpsocCommunication::readSerialInterface() {
|
||
|
|
int bytesRead = read(helper.rawFd(), readBuf, sizeof(readBuf));
|
||
|
|
if (bytesRead < 0) {
|
||
|
|
return returnvalue::FAILED;
|
||
|
|
}
|
||
|
|
if (bytesRead > 0) {
|
||
|
|
if (MPSOC_LOW_LEVEL_RX_WIRETAPPING) {
|
||
|
|
sif::debug << "Read " << bytesRead << " bytes on the MPSoC interface" << std::endl;
|
||
|
|
}
|
||
|
|
return readRingBuf.writeData(readBuf, bytesRead);
|
||
|
|
}
|
||
|
|
return returnvalue::OK;
|
||
|
|
}
|
||
|
|
|
||
|
|
const SpacePacketReader& MpsocCommunication::getSpReader() const { return spReader; }
|
||
|
|
|
||
|
|
SerialCommunicationHelper& MpsocCommunication::getComHelper() { return helper; }
|