Robin Mueller
0ac1d1c1ca
This PR adds the packet stack for the CCSDS File Delivery Protocol. It also refactors the existing TMTC infastructure to allow sending of CFDP packets to the CCSDS handlers. This includes the whole PDU (Protocol Data Unit) stack: - File Data PDUs and all file directive PDUs - ACK PDU - NAK PDU - Metadata PDU - Finished PDU - Prompt PDU - Keep Alive PDU - EOF PDU The PR includes a full set of unittests for the packet stack with a coverage of 90+ %. The refactoring of the existing TMTC infastructure includes non-ideal solutions like diamond inheritance. Avoiding this solution would require refactoring the packet stack. This would be a good idea anyway because the existing stack is tightly coupled to the FSFW, making reuse more difficult if only the stack is planned to be used without the store functionalities etc. The PDU implementation provided here is only weakly coupled to the FSFW, only using components like returnvalues or the Serialization modules. There are dedicated serializers and deserializers, which also helps in creating small focused modules which are easy to test. Some of the modules here were provied by Matthias Tompert.
393 lines
14 KiB
C++
393 lines
14 KiB
C++
#include "fsfw/osal/linux/MessageQueue.h"
|
|
#include "fsfw/osal/linux/unixUtility.h"
|
|
|
|
#include "fsfw/serviceinterface/ServiceInterface.h"
|
|
#include "fsfw/objectmanager/ObjectManager.h"
|
|
|
|
#include <fstream>
|
|
|
|
#include <fcntl.h> /* For O_* constants */
|
|
#include <sys/stat.h> /* For mode constants */
|
|
#include <cstring>
|
|
#include <errno.h>
|
|
|
|
|
|
MessageQueue::MessageQueue(uint32_t messageDepth, size_t maxMessageSize):
|
|
id(MessageQueueIF::NO_QUEUE),lastPartner(MessageQueueIF::NO_QUEUE),
|
|
defaultDestination(MessageQueueIF::NO_QUEUE), maxMessageSize(maxMessageSize) {
|
|
mq_attr attributes;
|
|
this->id = 0;
|
|
//Set attributes
|
|
attributes.mq_curmsgs = 0;
|
|
attributes.mq_maxmsg = messageDepth;
|
|
attributes.mq_msgsize = maxMessageSize;
|
|
attributes.mq_flags = 0; //Flags are ignored on Linux during mq_open
|
|
//Set the name of the queue. The slash is mandatory!
|
|
sprintf(name, "/FSFW_MQ%u\n", queueCounter++);
|
|
|
|
// Create a nonblocking queue if the name is available (the queue is read
|
|
// and writable for the owner as well as the group)
|
|
int oflag = O_NONBLOCK | O_RDWR | O_CREAT | O_EXCL;
|
|
mode_t mode = S_IWUSR | S_IREAD | S_IWGRP | S_IRGRP | S_IROTH | S_IWOTH;
|
|
mqd_t tempId = mq_open(name, oflag, mode, &attributes);
|
|
if (tempId == -1) {
|
|
handleOpenError(&attributes, messageDepth);
|
|
}
|
|
else {
|
|
//Successful mq_open call
|
|
this->id = tempId;
|
|
}
|
|
}
|
|
|
|
MessageQueue::~MessageQueue() {
|
|
int status = mq_close(this->id);
|
|
if(status != 0){
|
|
utility::printUnixErrorGeneric(CLASS_NAME, "~MessageQueue", "close");
|
|
}
|
|
status = mq_unlink(name);
|
|
if(status != 0){
|
|
utility::printUnixErrorGeneric(CLASS_NAME, "~MessageQueue", "unlink");
|
|
}
|
|
}
|
|
|
|
ReturnValue_t MessageQueue::sendMessage(MessageQueueId_t sendTo,
|
|
MessageQueueMessageIF* message, bool ignoreFault) {
|
|
return sendMessageFrom(sendTo, message, this->getId(), false);
|
|
}
|
|
|
|
ReturnValue_t MessageQueue::sendToDefault(MessageQueueMessageIF* message) {
|
|
return sendToDefaultFrom(message, this->getId());
|
|
}
|
|
|
|
ReturnValue_t MessageQueue::reply(MessageQueueMessageIF* message) {
|
|
if (this->lastPartner != 0) {
|
|
return sendMessageFrom(this->lastPartner, message, this->getId());
|
|
} else {
|
|
return NO_REPLY_PARTNER;
|
|
}
|
|
}
|
|
|
|
ReturnValue_t MessageQueue::receiveMessage(MessageQueueMessageIF* message,
|
|
MessageQueueId_t* receivedFrom) {
|
|
ReturnValue_t status = this->receiveMessage(message);
|
|
*receivedFrom = this->lastPartner;
|
|
return status;
|
|
}
|
|
|
|
ReturnValue_t MessageQueue::receiveMessage(MessageQueueMessageIF* message) {
|
|
if(message == nullptr) {
|
|
#if FSFW_CPP_OSTREAM_ENABLED == 1
|
|
sif::error << "MessageQueue::receiveMessage: Message is "
|
|
"nullptr!" << std::endl;
|
|
#endif
|
|
return HasReturnvaluesIF::RETURN_FAILED;
|
|
}
|
|
|
|
if(message->getMaximumMessageSize() < maxMessageSize) {
|
|
#if FSFW_CPP_OSTREAM_ENABLED == 1
|
|
sif::error << "MessageQueue::receiveMessage: Message size "
|
|
<< message->getMaximumMessageSize()
|
|
<< " too small to receive data!" << std::endl;
|
|
#endif
|
|
return HasReturnvaluesIF::RETURN_FAILED;
|
|
}
|
|
|
|
unsigned int messagePriority = 0;
|
|
int status = mq_receive(id,reinterpret_cast<char*>(message->getBuffer()),
|
|
message->getMaximumMessageSize(),&messagePriority);
|
|
if (status > 0) {
|
|
this->lastPartner = message->getSender();
|
|
//Check size of incoming message.
|
|
if (message->getMessageSize() < message->getMinimumMessageSize()) {
|
|
return HasReturnvaluesIF::RETURN_FAILED;
|
|
}
|
|
return HasReturnvaluesIF::RETURN_OK;
|
|
}
|
|
else if (status==0) {
|
|
//Success but no message received
|
|
return MessageQueueIF::EMPTY;
|
|
}
|
|
else {
|
|
//No message was received. Keep lastPartner anyway, I might send
|
|
//something later. But still, delete packet content.
|
|
memset(message->getData(), 0, message->getMaximumDataSize());
|
|
switch(errno){
|
|
case EAGAIN:
|
|
//O_NONBLOCK or MQ_NONBLOCK was set and there are no messages
|
|
//currently on the specified queue.
|
|
return MessageQueueIF::EMPTY;
|
|
case EBADF: {
|
|
//mqdes doesn't represent a valid queue open for reading.
|
|
utility::printUnixErrorGeneric(CLASS_NAME, "receiveMessage", "EBADF");
|
|
break;
|
|
}
|
|
case EINVAL: {
|
|
/*
|
|
* This value indicates one of the following:
|
|
* - The pointer to the buffer for storing the received message,
|
|
* msg_ptr, is NULL.
|
|
* - The number of bytes requested, msg_len is less than zero.
|
|
* - msg_len is anything other than the mq_msgsize of the specified
|
|
* queue, and the QNX extended option MQ_READBUF_DYNAMIC hasn't
|
|
* been set in the queue's mq_flags.
|
|
*/
|
|
utility::printUnixErrorGeneric(CLASS_NAME, "receiveMessage", "EINVAL");
|
|
break;
|
|
}
|
|
case EMSGSIZE: {
|
|
/*
|
|
* This value indicates one of the following:
|
|
* - the QNX extended option MQ_READBUF_DYNAMIC hasn't been set,
|
|
* and the given msg_len is shorter than the mq_msgsize for
|
|
* the given queue.
|
|
* - the extended option MQ_READBUF_DYNAMIC has been set, but the
|
|
* given msg_len is too short for the message that would have
|
|
* been received.
|
|
*/
|
|
utility::printUnixErrorGeneric(CLASS_NAME, "receiveMessage", "EMSGSIZE");
|
|
break;
|
|
}
|
|
|
|
case EINTR: {
|
|
//The operation was interrupted by a signal.
|
|
utility::printUnixErrorGeneric(CLASS_NAME, "receiveMessage", "EINTR");
|
|
break;
|
|
}
|
|
case ETIMEDOUT: {
|
|
//The operation was interrupted by a signal.
|
|
utility::printUnixErrorGeneric(CLASS_NAME, "receiveMessage", "ETIMEDOUT");
|
|
break;
|
|
}
|
|
|
|
default:
|
|
return HasReturnvaluesIF::RETURN_FAILED;
|
|
}
|
|
return HasReturnvaluesIF::RETURN_FAILED;
|
|
}
|
|
}
|
|
|
|
MessageQueueId_t MessageQueue::getLastPartner() const {
|
|
return this->lastPartner;
|
|
}
|
|
|
|
ReturnValue_t MessageQueue::flush(uint32_t* count) {
|
|
mq_attr attrib;
|
|
int status = mq_getattr(id,&attrib);
|
|
if(status != 0){
|
|
switch(errno){
|
|
case EBADF:
|
|
//mqdes doesn't represent a valid message queue.
|
|
utility::printUnixErrorGeneric(CLASS_NAME, "flush", "EBADF");
|
|
break;
|
|
/*NO BREAK*/
|
|
case EINVAL:
|
|
//mq_attr is NULL
|
|
utility::printUnixErrorGeneric(CLASS_NAME, "flush", "EINVAL");
|
|
break;
|
|
default:
|
|
return HasReturnvaluesIF::RETURN_FAILED;
|
|
}
|
|
return HasReturnvaluesIF::RETURN_FAILED;
|
|
}
|
|
*count = attrib.mq_curmsgs;
|
|
attrib.mq_curmsgs = 0;
|
|
status = mq_setattr(id,&attrib,NULL);
|
|
if(status != 0){
|
|
switch(errno) {
|
|
case EBADF:
|
|
//mqdes doesn't represent a valid message queue.
|
|
utility::printUnixErrorGeneric(CLASS_NAME, "flush", "EBADF");
|
|
break;
|
|
case EINVAL:
|
|
/*
|
|
* This value indicates one of the following:
|
|
* - mq_attr is NULL.
|
|
* - MQ_MULT_NOTIFY had been set for this queue, and the given
|
|
* mq_flags includes a 0 in the MQ_MULT_NOTIFY bit. Once
|
|
* MQ_MULT_NOTIFY has been turned on, it may never be turned off.
|
|
*/
|
|
utility::printUnixErrorGeneric(CLASS_NAME, "flush", "EINVAL");
|
|
break;
|
|
default:
|
|
return HasReturnvaluesIF::RETURN_FAILED;
|
|
}
|
|
return HasReturnvaluesIF::RETURN_FAILED;
|
|
}
|
|
return HasReturnvaluesIF::RETURN_OK;
|
|
}
|
|
|
|
MessageQueueId_t MessageQueue::getId() const {
|
|
return this->id;
|
|
}
|
|
|
|
void MessageQueue::setDefaultDestination(MessageQueueId_t defaultDestination) {
|
|
this->defaultDestination = defaultDestination;
|
|
}
|
|
|
|
ReturnValue_t MessageQueue::sendToDefaultFrom(MessageQueueMessageIF* message,
|
|
MessageQueueId_t sentFrom, bool ignoreFault) {
|
|
return sendMessageFrom(defaultDestination, message, sentFrom, ignoreFault);
|
|
}
|
|
|
|
|
|
ReturnValue_t MessageQueue::sendMessageFrom(MessageQueueId_t sendTo,
|
|
MessageQueueMessageIF* message, MessageQueueId_t sentFrom,
|
|
bool ignoreFault) {
|
|
return sendMessageFromMessageQueue(sendTo,message, sentFrom,ignoreFault);
|
|
|
|
}
|
|
|
|
MessageQueueId_t MessageQueue::getDefaultDestination() const {
|
|
return this->defaultDestination;
|
|
}
|
|
|
|
bool MessageQueue::isDefaultDestinationSet() const {
|
|
return (defaultDestination != NO_QUEUE);
|
|
}
|
|
|
|
uint16_t MessageQueue::queueCounter = 0;
|
|
|
|
ReturnValue_t MessageQueue::sendMessageFromMessageQueue(MessageQueueId_t sendTo,
|
|
MessageQueueMessageIF *message, MessageQueueId_t sentFrom,
|
|
bool ignoreFault) {
|
|
if(message == nullptr) {
|
|
#if FSFW_CPP_OSTREAM_ENABLED == 1
|
|
sif::error << "MessageQueue::sendMessageFromMessageQueue: Message is nullptr!" << std::endl;
|
|
#else
|
|
sif::printError("MessageQueue::sendMessageFromMessageQueue: Message is nullptr!\n");
|
|
#endif
|
|
return HasReturnvaluesIF::RETURN_FAILED;
|
|
}
|
|
|
|
message->setSender(sentFrom);
|
|
int result = mq_send(sendTo,
|
|
reinterpret_cast<const char*>(message->getBuffer()),
|
|
message->getMessageSize(),0);
|
|
|
|
//TODO: Check if we're in ISR.
|
|
if (result != 0) {
|
|
if(!ignoreFault){
|
|
InternalErrorReporterIF* internalErrorReporter = ObjectManager::instance()->
|
|
get<InternalErrorReporterIF>(objects::INTERNAL_ERROR_REPORTER);
|
|
if (internalErrorReporter != NULL) {
|
|
internalErrorReporter->queueMessageNotSent();
|
|
}
|
|
}
|
|
switch(errno){
|
|
case EAGAIN:
|
|
//The O_NONBLOCK flag was set when opening the queue, or the
|
|
//MQ_NONBLOCK flag was set in its attributes, and the
|
|
//specified queue is full.
|
|
return MessageQueueIF::FULL;
|
|
case EBADF: {
|
|
//mq_des doesn't represent a valid message queue descriptor,
|
|
//or mq_des wasn't opened for writing.
|
|
|
|
utility::printUnixErrorGeneric(CLASS_NAME, "sendMessageFromMessageQueue", "EBADF");
|
|
#if FSFW_CPP_OSTREAM_ENABLED == 1
|
|
sif::warning << "mq_send to " << sendTo << " sent from "
|
|
<< sentFrom << " failed" << std::endl;
|
|
#else
|
|
sif::printWarning("mq_send to %d sent from %d failed\n", sendTo, sentFrom);
|
|
#endif
|
|
return DESTINATION_INVALID;
|
|
}
|
|
case EINTR:
|
|
//The call was interrupted by a signal.
|
|
utility::printUnixErrorGeneric(CLASS_NAME, "sendMessageFromMessageQueue", "EINTR");
|
|
break;
|
|
case EINVAL:
|
|
/*
|
|
* This value indicates one of the following:
|
|
* - msg_ptr is NULL.
|
|
* - msg_len is negative.
|
|
* - msg_prio is greater than MQ_PRIO_MAX.
|
|
* - msg_prio is less than 0.
|
|
* - MQ_PRIO_RESTRICT is set in the mq_attr of mq_des, and
|
|
* msg_prio is greater than the priority of the calling process.
|
|
*/
|
|
utility::printUnixErrorGeneric(CLASS_NAME, "sendMessageFromMessageQueue", "EINVAL");
|
|
break;
|
|
case EMSGSIZE:
|
|
// The msg_len is greater than the msgsize associated with
|
|
//the specified queue.
|
|
utility::printUnixErrorGeneric(CLASS_NAME, "sendMessageFromMessageQueue", "EMSGSIZE");
|
|
break;
|
|
default:
|
|
return HasReturnvaluesIF::RETURN_FAILED;
|
|
}
|
|
return HasReturnvaluesIF::RETURN_FAILED;
|
|
}
|
|
return HasReturnvaluesIF::RETURN_OK;
|
|
}
|
|
|
|
ReturnValue_t MessageQueue::handleOpenError(mq_attr* attributes,
|
|
uint32_t messageDepth) {
|
|
switch(errno) {
|
|
case(EINVAL): {
|
|
utility::printUnixErrorGeneric(CLASS_NAME, "MessageQueue", "EINVAL");
|
|
size_t defaultMqMaxMsg = 0;
|
|
// Not POSIX conformant, but should work for all UNIX systems.
|
|
// Just an additional helpful printout :-)
|
|
if(std::ifstream("/proc/sys/fs/mqueue/msg_max",std::ios::in) >>
|
|
defaultMqMaxMsg and defaultMqMaxMsg < messageDepth) {
|
|
/*
|
|
See: https://www.man7.org/linux/man-pages/man3/mq_open.3.html
|
|
This happens if the msg_max value is not large enough
|
|
It is ignored if the executable is run in privileged mode.
|
|
Run the unlockRealtime script or grant the mode manually by using:
|
|
sudo setcap 'CAP_SYS_RESOURCE=+ep' <pathToBinary>
|
|
|
|
Persistent solution for session:
|
|
echo <newMsgMax> | sudo tee /proc/sys/fs/mqueue/msg_max
|
|
|
|
Permanent solution:
|
|
sudo nano /etc/sysctl.conf
|
|
Append at end: fs/mqueue/msg_max = <newMsgMaxLen>
|
|
Apply changes with: sudo sysctl -p
|
|
*/
|
|
#if FSFW_CPP_OSTREAM_ENABLED == 1
|
|
sif::error << "MessageQueue::MessageQueue: Default MQ size " << defaultMqMaxMsg <<
|
|
" is too small for requested size " << messageDepth << std::endl;
|
|
sif::error << "This error can be fixed by setting the maximum "
|
|
"allowed message size higher!" << std::endl;
|
|
#else
|
|
sif::printError("MessageQueue::MessageQueue: Default MQ size %d is too small for"
|
|
"requested size %d\n");
|
|
sif::printError("This error can be fixes by setting the maximum allowed"
|
|
"message size higher!\n");
|
|
#endif
|
|
}
|
|
break;
|
|
}
|
|
case(EEXIST): {
|
|
// An error occured during open.
|
|
// We need to distinguish if it is caused by an already created queue
|
|
// There's another queue with the same name
|
|
// We unlink the other queue
|
|
int status = mq_unlink(name);
|
|
if (status != 0) {
|
|
utility::printUnixErrorGeneric(CLASS_NAME, "MessageQueue", "EEXIST");
|
|
}
|
|
else {
|
|
// Successful unlinking, try to open again
|
|
mqd_t tempId = mq_open(name,
|
|
O_NONBLOCK | O_RDWR | O_CREAT | O_EXCL,
|
|
S_IWUSR | S_IREAD | S_IWGRP | S_IRGRP, attributes);
|
|
if (tempId != -1) {
|
|
//Successful mq_open
|
|
this->id = tempId;
|
|
return HasReturnvaluesIF::RETURN_OK;
|
|
}
|
|
}
|
|
break;
|
|
}
|
|
|
|
default: {
|
|
// Failed either the first time or the second time
|
|
utility::printUnixErrorGeneric(CLASS_NAME, "MessageQueue", "Unknown");
|
|
}
|
|
}
|
|
return HasReturnvaluesIF::RETURN_FAILED;
|
|
}
|