245 lines
6.8 KiB
C++
245 lines
6.8 KiB
C++
#include "UdpTmTcBridge.h"
|
|
|
|
extern "C" {
|
|
#include <arpa/inet.h>
|
|
}
|
|
#include <errno.h>
|
|
#include <fsfw/action.h>
|
|
#include <fsfw/ipc/QueueFactory.h>
|
|
#include <fsfw/objectmanager.h>
|
|
#include <fsfw/serviceinterface/ServiceInterface.h>
|
|
#include <fsfw/tmtc/TmManager.h>
|
|
#include <sys/socket.h>
|
|
#include <sys/types.h> // POSIX.1-2001 does not require the inclusion of <sys/types.h>, and this header file is not required on Linux. However, some historical (BSD) implementations required this header file, and portable applications are probably wise to include it.
|
|
|
|
#include "FsfwProtocolHeader.h"
|
|
#include "TmMessage.h"
|
|
|
|
UdpTmTcBridgeNew::UdpTmTcBridgeNew(object_id_t objectId, uint16_t listeningPort,
|
|
const sockaddr *defaultDestination_in,
|
|
socklen_t defaultDestinationLen)
|
|
: SystemObject(objectId),
|
|
listeningPort(listeningPort),
|
|
defaultDestinationLen(defaultDestinationLen) {
|
|
messageQueue =
|
|
QueueFactory::instance()->createMessageQueue(50, MessageQueueMessage::MAX_MESSAGE_SIZE);
|
|
memcpy(&this->defaultDestination, defaultDestination_in, defaultDestinationLen);
|
|
}
|
|
|
|
UdpTmTcBridgeNew::~UdpTmTcBridgeNew() {}
|
|
|
|
ReturnValue_t UdpTmTcBridgeNew::initialize() {
|
|
ReturnValue_t result = SystemObject::initialize();
|
|
if (result != returnvalue::OK) {
|
|
return result;
|
|
}
|
|
|
|
IPCStore = ObjectManager::instance()->get<StorageManagerIF>(objects::IPC_STORE);
|
|
if (IPCStore == nullptr) {
|
|
return returnvalue::FAILED;
|
|
}
|
|
|
|
TmManager *tmManager = ObjectManager::instance()->get<TmManager>(objects::TM_MANAGER);
|
|
if (tmManager == nullptr) {
|
|
return returnvalue::FAILED;
|
|
}
|
|
|
|
// we do not need any space reserved for the header, as we can send with detached header
|
|
// information
|
|
tmManager->registerNetworkProtocolInterface(this, IP6, 0);
|
|
|
|
int retval;
|
|
|
|
serverSocket = socket(AF_INET6, SOCK_DGRAM, 0);
|
|
if (serverSocket == -1) {
|
|
// TODO resolve errno
|
|
#if FSFW_CPP_OSTREAM_ENABLED == 1
|
|
sif::error << "UdpTmTcBridge::initialize: Socket initialization failed!" << std::endl;
|
|
#endif
|
|
return returnvalue::FAILED;
|
|
}
|
|
|
|
//TODO verify that lwip interprets this correctly
|
|
int flags = fcntl(serverSocket, F_GETFL, 0);
|
|
flags |= O_NONBLOCK;
|
|
fcntl(serverSocket, F_SETFL, flags);
|
|
|
|
sockaddr_in6 serverAddr;
|
|
|
|
memset(&serverAddr, 0, sizeof(serverAddr));
|
|
|
|
serverAddr.sin6_family = AF_INET6;
|
|
serverAddr.sin6_port = htons(listeningPort);
|
|
serverAddr.sin6_addr = IN6ADDR_ANY_INIT;
|
|
|
|
retval = bind(serverSocket, (struct sockaddr *)&serverAddr, sizeof(serverAddr));
|
|
if (retval == -1) {
|
|
#if FSFW_CPP_OSTREAM_ENABLED == 1
|
|
sif::error << "UdpTmTcBridge::initialize: bind failed with " << errno << std::endl;
|
|
#endif
|
|
return returnvalue::FAILED;
|
|
}
|
|
|
|
return returnvalue::OK;
|
|
}
|
|
|
|
ReturnValue_t UdpTmTcBridgeNew::performOperation(uint8_t operationCode) {
|
|
switch (operationCode) {
|
|
case BOTH:
|
|
handleTC();
|
|
handleTM();
|
|
break;
|
|
case TM:
|
|
handleTM();
|
|
break;
|
|
case TC:
|
|
handleTC();
|
|
break;
|
|
default:
|
|
break;
|
|
}
|
|
return returnvalue::OK;
|
|
}
|
|
|
|
MessageQueueId_t UdpTmTcBridgeNew::getReportReceptionQueue() const { return messageQueue->getId(); }
|
|
|
|
void UdpTmTcBridgeNew::handleTC() {
|
|
sockaddr_storage sender;
|
|
socklen_t senderlen = sizeof(sender);
|
|
|
|
ssize_t peekLen =
|
|
recvfrom(serverSocket, NULL, 0, MSG_PEEK | MSG_TRUNC, (struct sockaddr *)&sender, &senderlen);
|
|
|
|
if (peekLen <= 0) {
|
|
return;
|
|
}
|
|
|
|
if (peekLen < FsfwProtocolHeader::HEADER_SIZE) {
|
|
recv(serverSocket, NULL, 0, MSG_TRUNC);
|
|
return;
|
|
}
|
|
|
|
if (sender.ss_family != AF_INET6) {
|
|
// TODO handle v4 if we allow setting a listening address
|
|
// for now, we listen as AF_INET6 and this should not happen
|
|
return;
|
|
}
|
|
|
|
store_address_t storageId;
|
|
uint8_t *bufferPointer;
|
|
ReturnValue_t result =
|
|
IPCStore->getFreeElement(&storageId, peekLen + 1 + sizeof(sockaddr_in6), &bufferPointer);
|
|
|
|
if (result != returnvalue::OK) {
|
|
return;
|
|
}
|
|
|
|
*bufferPointer = IP6;
|
|
|
|
uint8_t *applicationData = bufferPointer + 1 + sizeof(sockaddr_in6);
|
|
senderlen = sizeof(sockaddr_in6);
|
|
|
|
ssize_t receivedLen = recvfrom(serverSocket, applicationData, peekLen, 0,
|
|
(struct sockaddr *)(bufferPointer + 1), &senderlen);
|
|
|
|
if (receivedLen == -1) {
|
|
return;
|
|
}
|
|
|
|
if (receivedLen != peekLen) {
|
|
// should not happen, if it does throw away
|
|
return;
|
|
}
|
|
|
|
|
|
size_t bufferLen = receivedLen;
|
|
const uint8_t *constApplicationData = applicationData;
|
|
|
|
FsfwProtocolHeader header;
|
|
|
|
result = header.deSerialize(&constApplicationData, &bufferLen, SerializeIF::Endianness::NETWORK);
|
|
|
|
if (result != returnvalue::OK) {
|
|
return;
|
|
}
|
|
|
|
sif::debug << "Received msg for 0x" << std::hex << header.getObjectId() << std::dec
|
|
<< " interface " << (int)header.getInterface() << std::endl;
|
|
|
|
CommandMessage message;
|
|
|
|
switch (header.getInterface()) {
|
|
case HasActionsIF::INTERFACE_ID: {
|
|
HasActionsIF *object = ObjectManager::instance()->get<HasActionsIF>(header.getObjectId());
|
|
if (object == nullptr) {
|
|
return;
|
|
}
|
|
|
|
ActionMessage::setCommand(&message, 1 + sizeof(sockaddr_in6), storageId);
|
|
result = messageQueue->sendMessage(object->getCommandQueue(), &message);
|
|
// sif::debug << "UdpTmTcBridge::performOperation: sent " << (int)storageId.raw << std::endl;
|
|
} break;
|
|
default:
|
|
#if FSFW_CPP_OSTREAM_ENABLED == 1
|
|
// sif::debug << "UdpTmTcBridge::performOperation: illegal interface"
|
|
// << (int) header.getInterface() << std::endl;
|
|
#endif
|
|
break;
|
|
}
|
|
}
|
|
|
|
void UdpTmTcBridgeNew::handleTM() {
|
|
CommandMessage message;
|
|
ReturnValue_t result = messageQueue->receiveMessage(&message);
|
|
|
|
if (result != returnvalue::OK) {
|
|
return;
|
|
}
|
|
|
|
if (message.getCommand() != TmMessage::SEND_TM) {
|
|
return;
|
|
}
|
|
|
|
store_address_t tc = TmMessage::getTc(&message);
|
|
store_address_t tm = TmMessage::getTm(&message);
|
|
|
|
const sockaddr *receiver;
|
|
socklen_t receiverLen;
|
|
|
|
const uint8_t *tcData;
|
|
size_t tcDataSize;
|
|
result = IPCStore->getData(tc, &tcData, &tcDataSize);
|
|
if (result != returnvalue::OK) {
|
|
receiver = (const sockaddr *)&defaultDestination;
|
|
receiverLen = defaultDestinationLen;
|
|
} else {
|
|
if (*tcData != IP6) {
|
|
// this should not have been routed here
|
|
return;
|
|
}
|
|
receiver = (const sockaddr *)(tcData + 1);
|
|
receiverLen = sizeof(sockaddr_in6);
|
|
}
|
|
|
|
const uint8_t *tmData;
|
|
size_t tmDataSize;
|
|
result = IPCStore->getData(tm, &tmData, &tmDataSize);
|
|
if (result != returnvalue::OK) {
|
|
// nothing to send
|
|
if (IPCStore->hasDataAtId(tc)) {
|
|
IPCStore->deleteData(tc);
|
|
}
|
|
return;
|
|
}
|
|
|
|
int res = sendto(serverSocket, tmData, tmDataSize, 0, receiver, receiverLen);
|
|
|
|
if (res == -1) {
|
|
sif::error << "UdpTmTcBridge::handleTM: sendto failed with " << errno << std::endl;
|
|
}
|
|
IPCStore->deleteData(tm);
|
|
if (IPCStore->hasDataAtId(tc)) {
|
|
IPCStore->deleteData(tc);
|
|
}
|
|
}
|