143 lines
4.2 KiB
C++
143 lines
4.2 KiB
C++
#include "UdpTmTcBridge.h"
|
|
|
|
#include "FsfwProtocolHeader.h"
|
|
|
|
#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 <netinet/in.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.
|
|
|
|
UdpTmTcBridgeNew::UdpTmTcBridgeNew(object_id_t objectId, object_id_t tmStoreId,
|
|
object_id_t tcStoreId)
|
|
: SystemObject(objectId) {
|
|
messageQueue =
|
|
QueueFactory::instance()->createMessageQueue(50, MessageQueueMessage::MAX_MESSAGE_SIZE);
|
|
}
|
|
|
|
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;
|
|
}
|
|
|
|
int retval;
|
|
|
|
serverSocket = socket(AF_INET, SOCK_DGRAM | SOCK_NONBLOCK, 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;
|
|
}
|
|
|
|
sockaddr_in serverAddr;
|
|
serverAddr.sin_family = AF_INET;
|
|
serverAddr.sin_port = htons(6667);
|
|
|
|
retval = inet_aton("10.13.90.2", &serverAddr.sin_addr);
|
|
if (retval == 0) {
|
|
#if FSFW_CPP_OSTREAM_ENABLED == 1
|
|
sif::error << "UdpTmTcBridge::initialize: Invalid IP!" << std::endl;
|
|
#endif
|
|
return returnvalue::FAILED;
|
|
}
|
|
|
|
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) {
|
|
ssize_t peekLen = recv(serverSocket, NULL, 0, MSG_PEEK | MSG_TRUNC);
|
|
|
|
if (peekLen <= 0) {
|
|
return returnvalue::OK;
|
|
}
|
|
|
|
if (peekLen < FsfwProtocolHeader::HEADER_SIZE) {
|
|
recv(serverSocket, NULL, 0, MSG_TRUNC);
|
|
return returnvalue::OK;
|
|
}
|
|
store_address_t storageId;
|
|
uint8_t *bufferPointer;
|
|
ReturnValue_t result = IPCStore->getFreeElement(&storageId, peekLen, &bufferPointer);
|
|
|
|
if (result != returnvalue::OK) {
|
|
return returnvalue::OK;
|
|
}
|
|
|
|
sockaddr_storage sender;
|
|
socklen_t senderlen = sizeof(sender);
|
|
|
|
ssize_t receivedLen =
|
|
recvfrom(serverSocket, bufferPointer, peekLen, 0, (struct sockaddr *)&sender, &senderlen);
|
|
|
|
if (receivedLen == -1) {
|
|
return returnvalue::OK;
|
|
}
|
|
|
|
if (receivedLen != peekLen) {
|
|
// should not happen, if it does throw away
|
|
return returnvalue::OK;
|
|
}
|
|
|
|
|
|
|
|
size_t bufferLen = receivedLen;
|
|
const uint8_t *constBufferPointer = bufferPointer;
|
|
|
|
FsfwProtocolHeader header;
|
|
|
|
result = header.deSerialize(&constBufferPointer, &bufferLen,
|
|
SerializeIF::Endianness::NETWORK);
|
|
|
|
if (result != returnvalue::OK) {
|
|
return returnvalue::OK;
|
|
}
|
|
|
|
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 returnvalue::OK;
|
|
}
|
|
|
|
ActionMessage::setCommand(&message, header.HEADER_SIZE, 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;
|
|
}
|
|
|
|
return returnvalue::OK;
|
|
}
|