2021-07-14 10:50:44 +02:00
|
|
|
#include "fsfw/osal/host/MessageQueue.h"
|
2020-09-05 21:19:53 +02:00
|
|
|
|
2022-02-02 10:29:30 +01:00
|
|
|
#include <cstring>
|
|
|
|
|
2021-07-14 10:50:44 +02:00
|
|
|
#include "fsfw/ipc/MutexFactory.h"
|
|
|
|
#include "fsfw/ipc/MutexGuard.h"
|
2022-02-02 10:29:30 +01:00
|
|
|
#include "fsfw/objectmanager/ObjectManager.h"
|
|
|
|
#include "fsfw/osal/host/QueueMapManager.h"
|
|
|
|
#include "fsfw/serviceinterface/ServiceInterface.h"
|
2020-09-05 20:18:52 +02:00
|
|
|
|
2022-02-02 10:29:30 +01:00
|
|
|
MessageQueue::MessageQueue(size_t messageDepth, size_t maxMessageSize)
|
|
|
|
: messageSize(maxMessageSize), messageDepth(messageDepth) {
|
|
|
|
queueLock = MutexFactory::instance()->createMutex();
|
|
|
|
auto result = QueueMapManager::instance()->addMessageQueue(this, &mqId);
|
|
|
|
if (result != HasReturnvaluesIF::RETURN_OK) {
|
2021-01-03 14:16:52 +01:00
|
|
|
#if FSFW_CPP_OSTREAM_ENABLED == 1
|
2022-02-02 10:29:30 +01:00
|
|
|
sif::error << "MessageQueue::MessageQueue: Could not be created" << std::endl;
|
2021-06-03 12:29:06 +02:00
|
|
|
#else
|
2022-02-02 10:29:30 +01:00
|
|
|
sif::printError("MessageQueue::MessageQueue: Could not be created\n");
|
2021-01-03 13:58:18 +01:00
|
|
|
#endif
|
2022-02-02 10:29:30 +01:00
|
|
|
}
|
2020-09-05 20:18:52 +02:00
|
|
|
}
|
|
|
|
|
2022-02-02 10:29:30 +01:00
|
|
|
MessageQueue::~MessageQueue() { MutexFactory::instance()->deleteMutex(queueLock); }
|
2020-09-05 20:18:52 +02:00
|
|
|
|
2022-02-02 10:29:30 +01:00
|
|
|
ReturnValue_t MessageQueue::sendMessage(MessageQueueId_t sendTo, MessageQueueMessageIF* message,
|
|
|
|
bool ignoreFault) {
|
|
|
|
return sendMessageFrom(sendTo, message, this->getId(), ignoreFault);
|
2020-09-05 20:18:52 +02:00
|
|
|
}
|
|
|
|
|
|
|
|
ReturnValue_t MessageQueue::sendToDefault(MessageQueueMessageIF* message) {
|
2022-02-02 10:29:30 +01:00
|
|
|
return sendToDefaultFrom(message, this->getId());
|
2020-09-05 20:18:52 +02:00
|
|
|
}
|
|
|
|
|
|
|
|
ReturnValue_t MessageQueue::sendToDefaultFrom(MessageQueueMessageIF* message,
|
2022-02-02 10:29:30 +01:00
|
|
|
MessageQueueId_t sentFrom, bool ignoreFault) {
|
|
|
|
return sendMessageFrom(defaultDestination, message, sentFrom, ignoreFault);
|
2020-09-05 20:18:52 +02:00
|
|
|
}
|
|
|
|
|
|
|
|
ReturnValue_t MessageQueue::reply(MessageQueueMessageIF* message) {
|
2022-02-02 10:29:30 +01:00
|
|
|
if (this->lastPartner != MessageQueueIF::NO_QUEUE) {
|
|
|
|
return sendMessageFrom(this->lastPartner, message, this->getId());
|
|
|
|
} else {
|
|
|
|
return MessageQueueIF::NO_REPLY_PARTNER;
|
|
|
|
}
|
2020-09-05 20:18:52 +02:00
|
|
|
}
|
|
|
|
|
2022-02-02 10:29:30 +01:00
|
|
|
ReturnValue_t MessageQueue::sendMessageFrom(MessageQueueId_t sendTo, MessageQueueMessageIF* message,
|
|
|
|
MessageQueueId_t sentFrom, bool ignoreFault) {
|
|
|
|
return sendMessageFromMessageQueue(sendTo, message, sentFrom, ignoreFault);
|
2020-09-05 20:18:52 +02:00
|
|
|
}
|
|
|
|
|
|
|
|
ReturnValue_t MessageQueue::receiveMessage(MessageQueueMessageIF* message,
|
2022-02-02 10:29:30 +01:00
|
|
|
MessageQueueId_t* receivedFrom) {
|
|
|
|
ReturnValue_t status = this->receiveMessage(message);
|
|
|
|
if (status == HasReturnvaluesIF::RETURN_OK) {
|
|
|
|
*receivedFrom = this->lastPartner;
|
|
|
|
}
|
|
|
|
return status;
|
2020-09-05 20:18:52 +02:00
|
|
|
}
|
|
|
|
|
|
|
|
ReturnValue_t MessageQueue::receiveMessage(MessageQueueMessageIF* message) {
|
2022-02-02 10:29:30 +01:00
|
|
|
if (messageQueue.empty()) {
|
|
|
|
return MessageQueueIF::EMPTY;
|
|
|
|
}
|
|
|
|
MutexGuard mutexLock(queueLock, MutexIF::TimeoutType::WAITING, 20);
|
|
|
|
std::copy(messageQueue.front().data(), messageQueue.front().data() + messageSize,
|
|
|
|
message->getBuffer());
|
|
|
|
messageQueue.pop();
|
|
|
|
// The last partner is the first uint32_t field in the message
|
|
|
|
this->lastPartner = message->getSender();
|
|
|
|
return HasReturnvaluesIF::RETURN_OK;
|
2020-09-05 20:18:52 +02:00
|
|
|
}
|
|
|
|
|
2022-02-02 10:29:30 +01:00
|
|
|
MessageQueueId_t MessageQueue::getLastPartner() const { return lastPartner; }
|
2020-09-05 20:18:52 +02:00
|
|
|
|
|
|
|
ReturnValue_t MessageQueue::flush(uint32_t* count) {
|
2022-02-02 10:29:30 +01:00
|
|
|
*count = messageQueue.size();
|
|
|
|
// Clears the queue.
|
|
|
|
messageQueue = std::queue<std::vector<uint8_t>>();
|
|
|
|
return HasReturnvaluesIF::RETURN_OK;
|
2020-09-05 20:18:52 +02:00
|
|
|
}
|
|
|
|
|
2022-02-02 10:29:30 +01:00
|
|
|
MessageQueueId_t MessageQueue::getId() const { return mqId; }
|
2020-09-05 20:18:52 +02:00
|
|
|
|
|
|
|
void MessageQueue::setDefaultDestination(MessageQueueId_t defaultDestination) {
|
2022-02-02 10:29:30 +01:00
|
|
|
defaultDestinationSet = true;
|
|
|
|
this->defaultDestination = defaultDestination;
|
2020-09-05 20:18:52 +02:00
|
|
|
}
|
|
|
|
|
2022-02-02 10:29:30 +01:00
|
|
|
MessageQueueId_t MessageQueue::getDefaultDestination() const { return defaultDestination; }
|
2020-09-05 20:18:52 +02:00
|
|
|
|
2022-02-02 10:29:30 +01:00
|
|
|
bool MessageQueue::isDefaultDestinationSet() const { return defaultDestinationSet; }
|
2020-09-05 20:18:52 +02:00
|
|
|
|
|
|
|
// static core function to send messages.
|
|
|
|
ReturnValue_t MessageQueue::sendMessageFromMessageQueue(MessageQueueId_t sendTo,
|
2022-02-02 10:29:30 +01:00
|
|
|
MessageQueueMessageIF* message,
|
|
|
|
MessageQueueId_t sentFrom,
|
|
|
|
bool ignoreFault) {
|
|
|
|
if (message == nullptr) {
|
|
|
|
return HasReturnvaluesIF::RETURN_FAILED;
|
|
|
|
}
|
|
|
|
message->setSender(sentFrom);
|
|
|
|
if (message->getMessageSize() > message->getMaximumMessageSize()) {
|
|
|
|
// Actually, this should never happen or an error will be emitted
|
|
|
|
// in MessageQueueMessage.
|
|
|
|
// But I will still return a failure here.
|
|
|
|
return HasReturnvaluesIF::RETURN_FAILED;
|
|
|
|
}
|
|
|
|
MessageQueue* targetQueue =
|
|
|
|
dynamic_cast<MessageQueue*>(QueueMapManager::instance()->getMessageQueue(sendTo));
|
|
|
|
if (targetQueue == nullptr) {
|
|
|
|
if (not ignoreFault) {
|
|
|
|
InternalErrorReporterIF* internalErrorReporter =
|
|
|
|
ObjectManager::instance()->get<InternalErrorReporterIF>(objects::INTERNAL_ERROR_REPORTER);
|
|
|
|
if (internalErrorReporter != nullptr) {
|
|
|
|
internalErrorReporter->queueMessageNotSent();
|
|
|
|
}
|
2021-04-12 12:38:56 +02:00
|
|
|
}
|
2022-02-02 10:29:30 +01:00
|
|
|
return MessageQueueIF::DESTINATION_INVALID;
|
|
|
|
}
|
|
|
|
if (targetQueue->messageQueue.size() < targetQueue->messageDepth) {
|
|
|
|
MutexGuard mutexLock(targetQueue->queueLock, MutexIF::TimeoutType::WAITING, 20);
|
|
|
|
targetQueue->messageQueue.push(std::vector<uint8_t>(message->getMaximumMessageSize()));
|
|
|
|
memcpy(targetQueue->messageQueue.back().data(), message->getBuffer(),
|
|
|
|
message->getMaximumMessageSize());
|
|
|
|
} else {
|
|
|
|
return MessageQueueIF::FULL;
|
|
|
|
}
|
|
|
|
return HasReturnvaluesIF::RETURN_OK;
|
2020-09-05 20:18:52 +02:00
|
|
|
}
|
|
|
|
|
2022-02-02 10:29:30 +01:00
|
|
|
ReturnValue_t MessageQueue::lockQueue(MutexIF::TimeoutType timeoutType, dur_millis_t lockTimeout) {
|
|
|
|
return queueLock->lockMutex(timeoutType, lockTimeout);
|
2020-09-05 20:18:52 +02:00
|
|
|
}
|
|
|
|
|
2022-02-02 10:29:30 +01:00
|
|
|
ReturnValue_t MessageQueue::unlockQueue() { return queueLock->unlockMutex(); }
|