159 lines
6.6 KiB
C++
159 lines
6.6 KiB
C++
#include "fsfw/pus/Service2DeviceAccess.h"
|
|
|
|
#include "fsfw/action/ActionMessage.h"
|
|
#include "fsfw/devicehandlers/DeviceHandlerIF.h"
|
|
#include "fsfw/devicehandlers/DeviceHandlerMessage.h"
|
|
#include "fsfw/objectmanager/ObjectManager.h"
|
|
#include "fsfw/pus/servicepackets/Service2Packets.h"
|
|
#include "fsfw/serialize/EndianConverter.h"
|
|
#include "fsfw/serialize/SerialLinkedListAdapter.h"
|
|
#include "fsfw/serialize/SerializeAdapter.h"
|
|
#include "fsfw/serviceinterface/ServiceInterface.h"
|
|
#include "fsfw/storagemanager/StorageManagerIF.h"
|
|
|
|
Service2DeviceAccess::Service2DeviceAccess(object_id_t objectId, uint16_t apid, uint8_t serviceId,
|
|
uint8_t numberOfParallelCommands,
|
|
uint16_t commandTimeoutSeconds)
|
|
: CommandingServiceBase(objectId, apid, "PUS 2 Raw Commanding", serviceId,
|
|
numberOfParallelCommands, commandTimeoutSeconds) {}
|
|
|
|
Service2DeviceAccess::~Service2DeviceAccess() {}
|
|
|
|
ReturnValue_t Service2DeviceAccess::isValidSubservice(uint8_t subservice) {
|
|
switch (static_cast<Subservice>(subservice)) {
|
|
case Subservice::COMMAND_RAW_COMMANDING:
|
|
case Subservice::COMMAND_TOGGLE_WIRETAPPING:
|
|
return returnvalue::OK;
|
|
default:
|
|
#if FSFW_CPP_OSTREAM_ENABLED == 1
|
|
sif::error << "Invalid Subservice" << std::endl;
|
|
#endif
|
|
return AcceptsTelecommandsIF::INVALID_SUBSERVICE;
|
|
}
|
|
}
|
|
|
|
ReturnValue_t Service2DeviceAccess::getMessageQueueAndObject(uint8_t subservice,
|
|
const uint8_t* tcData,
|
|
size_t tcDataLen, MessageQueueId_t* id,
|
|
object_id_t* objectId) {
|
|
if (tcDataLen < sizeof(object_id_t)) {
|
|
return CommandingServiceBase::INVALID_TC;
|
|
}
|
|
SerializeAdapter::deSerialize(objectId, &tcData, &tcDataLen, SerializeIF::Endianness::BIG);
|
|
|
|
return checkInterfaceAndAcquireMessageQueue(id, objectId);
|
|
}
|
|
|
|
ReturnValue_t Service2DeviceAccess::checkInterfaceAndAcquireMessageQueue(
|
|
MessageQueueId_t* messageQueueToSet, object_id_t* objectId) {
|
|
DeviceHandlerIF* possibleTarget = ObjectManager::instance()->get<DeviceHandlerIF>(*objectId);
|
|
if (possibleTarget == nullptr) {
|
|
return CommandingServiceBase::INVALID_OBJECT;
|
|
}
|
|
*messageQueueToSet = possibleTarget->getCommandQueue();
|
|
return returnvalue::OK;
|
|
}
|
|
|
|
ReturnValue_t Service2DeviceAccess::prepareCommand(CommandMessage* message, uint8_t subservice,
|
|
const uint8_t* tcData, size_t tcDataLen,
|
|
uint32_t* state, object_id_t objectId) {
|
|
switch (static_cast<Subservice>(subservice)) {
|
|
case Subservice::COMMAND_RAW_COMMANDING: {
|
|
return prepareRawCommand(message, tcData, tcDataLen);
|
|
} break;
|
|
case Subservice::COMMAND_TOGGLE_WIRETAPPING: {
|
|
return prepareWiretappingCommand(message, tcData, tcDataLen);
|
|
} break;
|
|
default:
|
|
return returnvalue::FAILED;
|
|
}
|
|
}
|
|
|
|
ReturnValue_t Service2DeviceAccess::prepareRawCommand(CommandMessage* messageToSet,
|
|
const uint8_t* tcData, size_t tcDataLen) {
|
|
RawCommand RawCommand(tcData, tcDataLen);
|
|
// store command into the Inter Process Communication Store
|
|
store_address_t storeAddress;
|
|
ReturnValue_t result =
|
|
ipcStore->addData(&storeAddress, RawCommand.getCommand(), RawCommand.getCommandSize());
|
|
DeviceHandlerMessage::setDeviceHandlerRawCommandMessage(messageToSet, storeAddress);
|
|
return result;
|
|
}
|
|
|
|
ReturnValue_t Service2DeviceAccess::prepareWiretappingCommand(CommandMessage* messageToSet,
|
|
const uint8_t* tcData,
|
|
size_t tcDataLen) {
|
|
if (tcDataLen != WiretappingToggle::WIRETAPPING_COMMAND_SIZE) {
|
|
return CommandingServiceBase::INVALID_TC;
|
|
}
|
|
WiretappingToggle command;
|
|
ReturnValue_t result = command.deSerialize(&tcData, &tcDataLen, SerializeIF::Endianness::BIG);
|
|
DeviceHandlerMessage::setDeviceHandlerWiretappingMessage(messageToSet,
|
|
command.getWiretappingMode());
|
|
return result;
|
|
}
|
|
|
|
ReturnValue_t Service2DeviceAccess::handleReply(const CommandMessage* reply,
|
|
Command_t previousCommand, uint32_t* state,
|
|
CommandMessage* optionalNextCommand,
|
|
object_id_t objectId, bool* isStep) {
|
|
switch (reply->getCommand()) {
|
|
case CommandMessage::REPLY_COMMAND_OK:
|
|
return returnvalue::OK;
|
|
case CommandMessage::REPLY_REJECTED:
|
|
return reply->getReplyRejectedReason();
|
|
default:
|
|
return CommandingServiceBase::INVALID_REPLY;
|
|
}
|
|
}
|
|
|
|
// All device handlers set service 2 as default raw receiver for wiretapping
|
|
// so we have to handle those unrequested messages.
|
|
void Service2DeviceAccess::handleUnrequestedReply(CommandMessage* reply) {
|
|
switch (reply->getCommand()) {
|
|
case DeviceHandlerMessage::REPLY_RAW_COMMAND:
|
|
sendWiretappingTm(reply, static_cast<uint8_t>(Subservice::REPLY_WIRETAPPING_RAW_TC));
|
|
break;
|
|
case DeviceHandlerMessage::REPLY_RAW_REPLY:
|
|
sendWiretappingTm(reply, static_cast<uint8_t>(Subservice::REPLY_RAW));
|
|
break;
|
|
default:
|
|
#if FSFW_CPP_OSTREAM_ENABLED == 1
|
|
sif::error << "Unknown message in Service2DeviceAccess::"
|
|
"handleUnrequestedReply with command ID "
|
|
<< reply->getCommand() << std::endl;
|
|
#endif
|
|
break;
|
|
}
|
|
// Must be reached by all cases to clear message
|
|
reply->clear();
|
|
}
|
|
|
|
void Service2DeviceAccess::sendWiretappingTm(CommandMessage* reply, uint8_t subservice) {
|
|
// Raw Wiretapping
|
|
// Get Address of Data from Message
|
|
store_address_t storeAddress = DeviceHandlerMessage::getStoreAddress(reply);
|
|
const uint8_t* data = nullptr;
|
|
size_t size = 0;
|
|
ReturnValue_t result = ipcStore->getData(storeAddress, &data, &size);
|
|
if (result != returnvalue::OK) {
|
|
#if FSFW_CPP_OSTREAM_ENABLED == 1
|
|
sif::error << "Service2DeviceAccess::sendWiretappingTm: Data Lost in "
|
|
"handleUnrequestedReply with failure ID "
|
|
<< result << std::endl;
|
|
#endif
|
|
return;
|
|
}
|
|
|
|
// Init our dummy packet and correct endianness of object ID before
|
|
// sending it back.
|
|
WiretappingPacket tmPacket(DeviceHandlerMessage::getDeviceObjectId(reply), data);
|
|
result = sendTmPacket(subservice, tmPacket.objectId, tmPacket.data, size);
|
|
if (result != returnvalue::OK) {
|
|
// TODO: Warning
|
|
return;
|
|
}
|
|
}
|
|
|
|
MessageQueueId_t Service2DeviceAccess::getDeviceQueue() { return commandQueue->getId(); }
|