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

#include "fsfw/osal/rtems/MessageQueue.h"

#include <cstring>

#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)
    : id(0), lastPartner(0), defaultDestination(NO_QUEUE), internalErrorReporter(nullptr) {
  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) {
#if FSFW_CPP_OSTREAM_ENABLED == 1
    sif::error << "MessageQueue::MessageQueue: Creating Queue " << std::hex << name << std::dec
               << " failed with status:" << (uint32_t)status << std::endl;
#endif
    this->id = 0;
  }
}

MessageQueue::~MessageQueue() { rtems_message_queue_delete(id); }

ReturnValue_t MessageQueue::sendMessage(MessageQueueId_t sendTo, MessageQueueMessageIF* message,
                                        bool ignoreFault) {
  return sendMessageFrom(sendTo, message, this->getId(), ignoreFault);
}

ReturnValue_t MessageQueue::sendToDefault(MessageQueueMessageIF* message) {
  return sendToDefaultFrom(message, this->getId());
}

ReturnValue_t MessageQueue::reply(MessageQueueMessageIF* message) {
  if (this->lastPartner != 0) {
    return sendMessage(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) {
  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->lastPartner = message->getSender();
    // 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);
}

MessageQueueId_t MessageQueue::getLastPartner() const { return this->lastPartner; }

ReturnValue_t MessageQueue::flush(uint32_t* count) {
  rtems_status_code status = rtems_message_queue_flush(id, count);
  return convertReturnCode(status);
}

MessageQueueId_t MessageQueue::getId() const { return this->id; }

void MessageQueue::setDefaultDestination(MessageQueueId_t defaultDestination) {
  this->defaultDestination = defaultDestination;
}

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());

  // 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();
    }
  }

  ReturnValue_t returnCode = convertReturnCode(result);
  if (result == MessageQueueIF::EMPTY) {
    return HasReturnvaluesIF::RETURN_FAILED;
  }

  return returnCode;
}

ReturnValue_t MessageQueue::sendToDefaultFrom(MessageQueueMessageIF* message,
                                              MessageQueueId_t sentFrom, bool ignoreFault) {
  return sendMessageFrom(defaultDestination, message, sentFrom, ignoreFault);
}

MessageQueueId_t MessageQueue::getDefaultDestination() const { return this->defaultDestination; }

bool MessageQueue::isDefaultDestinationSet() const { return (defaultDestination != NO_QUEUE); }

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;
  }
}

uint16_t MessageQueue::queueCounter = 0;