103 lines
4.1 KiB
C++
103 lines
4.1 KiB
C++
#include "fsfw/osal/host/MessageQueue.h"
|
|
|
|
#include <cstring>
|
|
|
|
#include "fsfw/ipc/MutexFactory.h"
|
|
#include "fsfw/ipc/MutexGuard.h"
|
|
#include "fsfw/objectmanager/ObjectManager.h"
|
|
#include "fsfw/osal/host/QueueMapManager.h"
|
|
#include "fsfw/serviceinterface/ServiceInterface.h"
|
|
|
|
MessageQueue::MessageQueue(size_t messageDepth, size_t maxMessageSize, MqArgs* args)
|
|
: MessageQueueBase(MessageQueueIF::NO_QUEUE, MessageQueueIF::NO_QUEUE, args),
|
|
messageSize(maxMessageSize),
|
|
messageDepth(messageDepth) {
|
|
queueLock = MutexFactory::instance()->createMutex();
|
|
auto result = QueueMapManager::instance()->addMessageQueue(this, &id);
|
|
if (result != HasReturnvaluesIF::RETURN_OK) {
|
|
#if FSFW_CPP_OSTREAM_ENABLED == 1
|
|
sif::error << "MessageQueue::MessageQueue: Could not be created" << std::endl;
|
|
#else
|
|
sif::printError("MessageQueue::MessageQueue: Could not be created\n");
|
|
#endif
|
|
}
|
|
}
|
|
|
|
MessageQueue::~MessageQueue() { MutexFactory::instance()->deleteMutex(queueLock); }
|
|
|
|
ReturnValue_t MessageQueue::sendMessageFrom(MessageQueueId_t sendTo, MessageQueueMessageIF* message,
|
|
MessageQueueId_t sentFrom, bool ignoreFault) {
|
|
return sendMessageFromMessageQueue(sendTo, message, sentFrom, ignoreFault);
|
|
}
|
|
|
|
ReturnValue_t MessageQueue::receiveMessage(MessageQueueMessageIF* message) {
|
|
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->last = message->getSender();
|
|
return HasReturnvaluesIF::RETURN_OK;
|
|
}
|
|
|
|
ReturnValue_t MessageQueue::flush(uint32_t* count) {
|
|
*count = messageQueue.size();
|
|
// Clears the queue.
|
|
messageQueue = std::queue<std::vector<uint8_t>>();
|
|
return HasReturnvaluesIF::RETURN_OK;
|
|
}
|
|
|
|
// static core function to send messages.
|
|
ReturnValue_t MessageQueue::sendMessageFromMessageQueue(MessageQueueId_t sendTo,
|
|
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;
|
|
}
|
|
auto* targetQueue =
|
|
dynamic_cast<MessageQueue*>(QueueMapManager::instance()->getMessageQueue(sendTo));
|
|
if (targetQueue == nullptr) {
|
|
if (not ignoreFault) {
|
|
auto* internalErrorReporter =
|
|
ObjectManager::instance()->get<InternalErrorReporterIF>(objects::INTERNAL_ERROR_REPORTER);
|
|
if (internalErrorReporter != nullptr) {
|
|
internalErrorReporter->queueMessageNotSent();
|
|
}
|
|
}
|
|
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 {
|
|
if (not ignoreFault) {
|
|
auto* internalErrorReporter =
|
|
ObjectManager::instance()->get<InternalErrorReporterIF>(objects::INTERNAL_ERROR_REPORTER);
|
|
if (internalErrorReporter != nullptr) {
|
|
internalErrorReporter->queueMessageNotSent();
|
|
}
|
|
}
|
|
return MessageQueueIF::FULL;
|
|
}
|
|
return HasReturnvaluesIF::RETURN_OK;
|
|
}
|
|
|
|
ReturnValue_t MessageQueue::lockQueue(MutexIF::TimeoutType timeoutType, dur_millis_t lockTimeout) {
|
|
return queueLock->lockMutex(timeoutType, lockTimeout);
|
|
}
|
|
|
|
ReturnValue_t MessageQueue::unlockQueue() { return queueLock->unlockMutex(); }
|