fsfw/src/fsfw/osal/rtems/MessageQueue.cpp

99 lines
3.4 KiB
C++
Raw Normal View History

#include "fsfw/osal/rtems/MessageQueue.h"
2021-06-05 19:52:38 +02:00
2018-07-13 18:28:26 +02:00
#include <cstring>
2021-06-05 19:52:38 +02:00
2022-02-02 10:29:30 +01:00
#include "fsfw/objectmanager/ObjectManager.h"
#include "fsfw/osal/rtems/RtemsBasic.h"
#include "fsfw/serviceinterface/ServiceInterface.h"
MessageQueue::MessageQueue(size_t message_depth, size_t max_message_size, MqArgs* args)
: MessageQueueBase(MessageQueueIF::NO_QUEUE, MessageQueueIF::NO_QUEUE, args),
internalErrorReporter(nullptr) {
2022-02-02 10:29:30 +01:00
rtems_name name = ('Q' << 24) + (queueCounter++ << 8);
rtems_status_code status =
rtems_message_queue_create(name, message_depth, max_message_size, 0, &(this->id));
if (status != RTEMS_SUCCESSFUL) {
2021-01-04 15:34:08 +01:00
#if FSFW_CPP_OSTREAM_ENABLED == 1
2022-02-02 10:29:30 +01:00
sif::error << "MessageQueue::MessageQueue: Creating Queue " << std::hex << name << std::dec
<< " failed with status:" << (uint32_t)status << std::endl;
2021-01-04 15:34:08 +01:00
#endif
this->id = MessageQueueIF::NO_QUEUE;
2022-02-02 10:29:30 +01:00
}
}
2022-02-02 10:29:30 +01:00
MessageQueue::~MessageQueue() { rtems_message_queue_delete(id); }
2020-11-17 19:25:57 +01:00
ReturnValue_t MessageQueue::receiveMessage(MessageQueueMessageIF* message) {
2022-02-02 10:29:30 +01:00
size_t size = 0;
rtems_status_code status =
rtems_message_queue_receive(id, message->getBuffer(), &size, RTEMS_NO_WAIT, 1);
if (status == RTEMS_SUCCESSFUL) {
message->setMessageSize(size);
this->last = message->getSender();
2022-02-02 10:29:30 +01:00
// Check size of incoming message.
if (message->getMessageSize() < message->getMinimumMessageSize()) {
return HasReturnvaluesIF::RETURN_FAILED;
}
} else {
// No message was received. Keep lastPartner anyway, I might send something later.
// But still, delete packet content.
memset(message->getData(), 0, message->getMaximumDataSize());
}
return convertReturnCode(status);
}
ReturnValue_t MessageQueue::flush(uint32_t* count) {
2022-02-02 10:29:30 +01:00
rtems_status_code status = rtems_message_queue_flush(id, count);
return convertReturnCode(status);
}
2022-02-02 10:29:30 +01:00
ReturnValue_t MessageQueue::sendMessageFrom(MessageQueueId_t sendTo, MessageQueueMessageIF* message,
MessageQueueId_t sentFrom, bool ignoreFault) {
message->setSender(sentFrom);
rtems_status_code result =
rtems_message_queue_send(sendTo, message->getBuffer(), message->getMessageSize());
2022-02-02 10:29:30 +01:00
// TODO: Check if we're in ISR.
if (result != RTEMS_SUCCESSFUL && !ignoreFault) {
if (internalErrorReporter == nullptr) {
internalErrorReporter =
ObjectManager::instance()->get<InternalErrorReporterIF>(objects::INTERNAL_ERROR_REPORTER);
}
if (internalErrorReporter != nullptr) {
internalErrorReporter->queueMessageNotSent();
}
}
2022-02-02 10:29:30 +01:00
ReturnValue_t returnCode = convertReturnCode(result);
if (result == MessageQueueIF::EMPTY) {
return HasReturnvaluesIF::RETURN_FAILED;
}
2022-02-02 10:29:30 +01:00
return returnCode;
}
2022-02-02 10:29:30 +01:00
ReturnValue_t MessageQueue::convertReturnCode(rtems_status_code inValue) {
switch (inValue) {
case RTEMS_SUCCESSFUL:
return HasReturnvaluesIF::RETURN_OK;
case RTEMS_INVALID_ID:
return HasReturnvaluesIF::RETURN_FAILED;
case RTEMS_TIMEOUT:
return HasReturnvaluesIF::RETURN_FAILED;
case RTEMS_OBJECT_WAS_DELETED:
return HasReturnvaluesIF::RETURN_FAILED;
case RTEMS_INVALID_ADDRESS:
return HasReturnvaluesIF::RETURN_FAILED;
case RTEMS_INVALID_SIZE:
return HasReturnvaluesIF::RETURN_FAILED;
case RTEMS_TOO_MANY:
return MessageQueueIF::FULL;
case RTEMS_UNSATISFIED:
return MessageQueueIF::EMPTY;
default:
return HasReturnvaluesIF::RETURN_FAILED;
}
2018-07-13 18:28:26 +02:00
}
uint16_t MessageQueue::queueCounter = 0;