diff --git a/mission/devices/PlocHandler.cpp b/mission/devices/PlocHandler.cpp index 30f3859d..d8ed395e 100644 --- a/mission/devices/PlocHandler.cpp +++ b/mission/devices/PlocHandler.cpp @@ -92,11 +92,16 @@ ReturnValue_t PlocHandler::scanForReply(const uint8_t *start, default: { sif::debug << "PlocHandler::scanForReply: Reply has invalid apid" << std::endl; *foundLen = remainingSize; - result = INVALID_APID; - break; + return INVALID_APID; } } + /** + * This should normally never fail. However, this function is also responsible for incrementing + * the packet sequence count why it is called here. + */ + result = checkPacketSequenceCount(start); + return result; } @@ -151,7 +156,7 @@ ReturnValue_t PlocHandler::prepareTcMemWriteCommand(const uint8_t * commandData, | *(commandData + 2) << 8 | *(commandData + 3); const uint32_t memoryData = *(commandData + 4) << 24 | *(commandData + 5) << 16 | *(commandData + 6) << 8 | *(commandData + 7); - PLOC::TcMemWrite tcMemWrite(memoryAddress, memoryData); + PLOC::TcMemWrite tcMemWrite(memoryAddress, memoryData, packetSequenceCount); if (tcMemWrite.getFullSize() > PLOC::MAX_COMMAND_SIZE) { sif::debug << "PlocHandler::prepareTcMemWriteCommand: Command too big" << std::endl; return RETURN_FAILED; @@ -167,7 +172,7 @@ ReturnValue_t PlocHandler::prepareTcMemReadCommand(const uint8_t * commandData, size_t commandDataLen) { const uint32_t memoryAddress = *(commandData) << 24 | *(commandData + 1) << 16 | *(commandData + 2) << 8 | *(commandData + 3); - PLOC::TcMemRead tcMemRead(memoryAddress); + PLOC::TcMemRead tcMemRead(memoryAddress, packetSequenceCount); if (tcMemRead.getFullSize() > PLOC::MAX_COMMAND_SIZE) { sif::debug << "PlocHandler::prepareTcMemReadCommand: Command too big" << std::endl; return RETURN_FAILED; @@ -176,6 +181,7 @@ ReturnValue_t PlocHandler::prepareTcMemReadCommand(const uint8_t * commandData, rawPacket = commandBuffer; rawPacketLen = tcMemRead.getFullSize(); nextReplyId = PLOC::ACK_REPORT; + return RETURN_OK; } @@ -183,7 +189,7 @@ ReturnValue_t PlocHandler::verifyPacket(const uint8_t* start, size_t foundLen) { uint16_t receivedCrc = *(start + foundLen - 2) << 8 | *(start + foundLen - 1); - uint16_t recalculatedCrc = CRC::crc16ccitt(start, foundLen - 2, 0); + uint16_t recalculatedCrc = CRC::crc16ccitt(start, foundLen - 2); if (receivedCrc != recalculatedCrc) { return CRC_FAILURE; @@ -474,3 +480,16 @@ void PlocHandler::disableExeReportReply() { /* Expected replies is set to one here. The value will set to 0 in replyToReply() */ info->command->second.expectedReplies = 0; } + +ReturnValue_t PlocHandler::checkPacketSequenceCount(const uint8_t* data) { + uint16_t receivedSequenceCount = *(data + 2) << 8 | *(data + 3) & PACKET_SEQUENCE_COUNT_MASK; + if (receivedSequenceCount != ((packetSequenceCount + 1) & PACKET_SEQUENCE_COUNT_MASK)) { + sif::debug << "PlocHandler::checkPacketSequenceCount: Packet sequence count mismatch" + << std::endl; + /** The packet sequence count is corrected here to match the sequence count of the PLOC */ + packetSequenceCount = (receivedSequenceCount + 1) & PACKET_SEQUENCE_COUNT_MASK; + return SUBSEQUENCE_COUNT_FAILURE; + } + packetSequenceCount = (packetSequenceCount + 1) & PACKET_SEQUENCE_COUNT_MASK; + return RETURN_OK; +} diff --git a/mission/devices/PlocHandler.h b/mission/devices/PlocHandler.h index b241c838..1b529484 100644 --- a/mission/devices/PlocHandler.h +++ b/mission/devices/PlocHandler.h @@ -53,6 +53,7 @@ private: static const ReturnValue_t RECEIVED_ACK_FAILURE = MAKE_RETURN_CODE(0xA1); //!> Received ACK failure reply from PLOC static const ReturnValue_t RECEIVED_EXE_FAILURE = MAKE_RETURN_CODE(0xA2); //!> Received execution failure reply from PLOC static const ReturnValue_t INVALID_APID = MAKE_RETURN_CODE(0xA3); //!> Received space packet with invalid APID from PLOC + static const ReturnValue_t SUBSEQUENCE_COUNT_FAILURE = MAKE_RETURN_CODE(0xA4); //!> The packet sequence count does not match the expected packet sequence count. There may be one packet lost. static const uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::PLOC_HANDLER; @@ -62,9 +63,22 @@ private: static const Event CRC_FAILURE_EVENT = MAKE_EVENT(4, severity::LOW); //!> PLOC reply has invalid crc static const uint16_t APID_MASK = 0x7FF; + static const unit16_t PACKET_SEQUENCE_COUNT_MASK = 0x3FFF; uint8_t commandBuffer[PLOC::MAX_COMMAND_SIZE]; + /** + * @brief This object is incremented each time a packet is sent or received. By checking the + * packet sequence count of a received packet, no packets can be lost without noticing + * it. Only the least significant 14 bits represent the packet sequence count in a + * space packet. Thus the maximum value amounts to 16383 (0x3FFF). + * @note Normally this should never happen because the PLOC replies are always sent in a + * fixed order. However, the PLOC software checks this value and will return an ACK + * failure report in case the sequence count is not incremented with each transferred + * space packet. + */ + uint16_t packetSequenceCount = 0x3FFF; + /** * This variable is used to store the id of the next reply to receive. This is necessary * because the PLOC sends as reply to each command at least one acknowledgment and execution @@ -168,6 +182,16 @@ private: * the variable expectedReplies of an active command will be set to 0. */ void disableExeReportReply(); + + /** + * @brief This function checks the subsequence count of a received space packet to detect + * lost packets. + * + * @param data Pointer to a space packet. + * + * @return RETURN_OK if successful, else SUBSEQUENCE_COUNT_FAILURE + */ + ReturnValue_t checkSubsequenceCount(const uint8_t* data); }; #endif /* MISSION_DEVICES_PLOCHANDLER_H_ */ diff --git a/mission/devices/devicedefinitions/PlocDefinitions.h b/mission/devices/devicedefinitions/PlocDefinitions.h index b7378a8a..f5add451 100644 --- a/mission/devices/devicedefinitions/PlocDefinitions.h +++ b/mission/devices/devicedefinitions/PlocDefinitions.h @@ -63,10 +63,10 @@ namespace PLOC { * * @param memAddr The memory address to read from. */ - TcMemRead(const uint32_t memAddr) : - SpacePacket(LENGTH_TC_MEM_READ - 1, true, APID_TC_MEM_READ) { - fillPacketDataField(&memAddr); - } + TcMemRead(const uint32_t memAddr, uint16_t sequenceCount) : + SpacePacket(LENGTH_TC_MEM_READ - 1, true, APID_TC_MEM_READ, sequenceCount) { + fillPacketDataField(&memAddr); + } private: @@ -80,22 +80,21 @@ namespace PLOC { size_t serializedSize = 0; uint8_t* memoryAddressPos = this->localData.fields.buffer; SerializeAdapter::serialize(memAddrPtr, &memoryAddressPos, &serializedSize, - sizeof(*memAddrPtr), SerializeIF::Endianness::BIG); + sizeof(*memAddrPtr), SerializeIF::Endianness::LITTLE); /* Add memLen to packet data field */ - this->localData.fields.buffer[OFFSET_MEM_LEN_FIELD] = 0; - this->localData.fields.buffer[OFFSET_MEM_LEN_FIELD + 1] = 1; + this->localData.fields.buffer[OFFSET_MEM_LEN_FIELD] = 1; + this->localData.fields.buffer[OFFSET_MEM_LEN_FIELD + 1] = 0; /* Calculate crc */ uint16_t crc = CRC::crc16ccitt(this->localData.byteStream, - sizeof(CCSDSPrimaryHeader) + LENGTH_TC_MEM_READ - CRC_SIZE, 0); + sizeof(CCSDSPrimaryHeader) + LENGTH_TC_MEM_READ - CRC_SIZE); /* Add crc to packet data field of space packet */ serializedSize = 0; uint8_t* crcPos = this->localData.fields.buffer + CRC_OFFSET; SerializeAdapter::serialize(&crc, &crcPos, &serializedSize, sizeof(crc), SerializeIF::Endianness::BIG); - memcpy(this->localData.fields.buffer + CRC_OFFSET, &crc, sizeof(crc)); } static const uint8_t OFFSET_MEM_LEN_FIELD = 4; @@ -117,9 +116,10 @@ namespace PLOC { * * @param memAddr The PLOC memory address where to write to. * @param memoryData The data to write to the specified memory address. + * @param sequenceCount The subsequence count. Must be incremented with each new packet. */ - TcMemWrite(const uint32_t memAddr, const uint32_t memoryData) : - SpacePacket(LENGTH_TC_MEM_WRITE - 1, true, APID_TC_MEM_WRITE) { + TcMemWrite(const uint32_t memAddr, const uint32_t memoryData, uint16_t sequenceCount) : + SpacePacket(LENGTH_TC_MEM_WRITE - 1, true, APID_TC_MEM_WRITE, sequenceCount) { fillPacketDataField(&memAddr, &memoryData); } @@ -140,8 +140,8 @@ namespace PLOC { sizeof(*memAddrPtr), SerializeIF::Endianness::BIG); /* Add memLen to packet data field */ - this->localData.fields.buffer[OFFSET_MEM_LEN_FIELD] = 0; - this->localData.fields.buffer[OFFSET_MEM_LEN_FIELD + 1] = 1; + this->localData.fields.buffer[OFFSET_MEM_LEN_FIELD] = 1; + this->localData.fields.buffer[OFFSET_MEM_LEN_FIELD + 1] = 0; /* Add memData to packet data field */ serializedSize = 0; @@ -151,14 +151,13 @@ namespace PLOC { /* Calculate crc */ uint16_t crc = CRC::crc16ccitt(this->localData.byteStream, - sizeof(CCSDSPrimaryHeader) + LENGTH_TC_MEM_WRITE - CRC_SIZE, 0); + sizeof(CCSDSPrimaryHeader) + LENGTH_TC_MEM_WRITE - CRC_SIZE); serializedSize = 0; uint8_t* crcPos = this->localData.fields.buffer + CRC_OFFSET; /* Add crc to packet data field of space packet */ SerializeAdapter::serialize(&crc, &crcPos, &serializedSize, sizeof(crc), SerializeIF::Endianness::BIG); - memcpy(this->localData.fields.buffer + CRC_OFFSET, &crc, sizeof(crc)); } /** Offsets from base address of packet data field */