2023-07-05 13:49:03 +02:00
# include "UdpTmTcBridge.h"
2023-10-27 13:40:29 +02:00
extern " C " {
2023-07-05 13:49:03 +02:00
# include <arpa/inet.h>
2023-10-27 13:40:29 +02:00
}
2023-07-05 13:49:03 +02:00
# include <errno.h>
# include <fsfw/action.h>
# include <fsfw/ipc/QueueFactory.h>
# include <fsfw/objectmanager.h>
# include <fsfw/serviceinterface/ServiceInterface.h>
2023-07-05 23:47:07 +02:00
# include <fsfw/tmtc/TmManager.h>
2023-07-05 13:49:03 +02:00
# 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.
2023-07-05 23:47:07 +02:00
# include "FsfwProtocolHeader.h"
# include "TmMessage.h"
2023-07-10 16:17:05 +02:00
UdpTmTcBridgeNew : : UdpTmTcBridgeNew ( object_id_t objectId , uint16_t listeningPort ,
const sockaddr * defaultDestination_in ,
socklen_t defaultDestinationLen )
: SystemObject ( objectId ) ,
listeningPort ( listeningPort ) ,
defaultDestinationLen ( defaultDestinationLen ) {
2023-07-05 13:49:03 +02:00
messageQueue =
QueueFactory : : instance ( ) - > createMessageQueue ( 50 , MessageQueueMessage : : MAX_MESSAGE_SIZE ) ;
2023-07-10 16:17:05 +02:00
memcpy ( & this - > defaultDestination , defaultDestination_in , defaultDestinationLen ) ;
2023-07-05 13:49:03 +02:00
}
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 ;
}
2023-07-05 23:47:07 +02:00
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 ) ;
2023-07-05 13:49:03 +02:00
int retval ;
2023-10-27 13:40:29 +02:00
serverSocket = socket ( AF_INET6 , SOCK_DGRAM , 0 ) ;
2023-07-05 13:49:03 +02:00
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 ;
}
2023-10-27 13:40:29 +02:00
//TODO verify that lwip interprets this correctly
int flags = fcntl ( serverSocket , F_GETFL , 0 ) ;
flags | = O_NONBLOCK ;
fcntl ( serverSocket , F_SETFL , flags ) ;
2023-07-05 23:47:07 +02:00
sockaddr_in6 serverAddr ;
2023-07-05 13:49:03 +02:00
2023-07-05 23:47:07 +02:00
memset ( & serverAddr , 0 , sizeof ( serverAddr ) ) ;
serverAddr . sin6_family = AF_INET6 ;
2023-07-10 16:17:05 +02:00
serverAddr . sin6_port = htons ( listeningPort ) ;
2023-07-05 23:47:07 +02:00
serverAddr . sin6_addr = IN6ADDR_ANY_INIT ;
2023-07-05 13:49:03 +02:00
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 ) {
2023-07-05 23:47:07 +02:00
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 ) ;
2023-07-05 13:49:03 +02:00
if ( peekLen < = 0 ) {
2023-07-05 23:47:07 +02:00
return ;
2023-07-05 13:49:03 +02:00
}
if ( peekLen < FsfwProtocolHeader : : HEADER_SIZE ) {
recv ( serverSocket , NULL , 0 , MSG_TRUNC ) ;
2023-07-05 23:47:07 +02:00
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 ;
2023-07-05 13:49:03 +02:00
}
2023-07-05 23:47:07 +02:00
2023-07-05 13:49:03 +02:00
store_address_t storageId ;
uint8_t * bufferPointer ;
2023-07-05 23:47:07 +02:00
ReturnValue_t result =
IPCStore - > getFreeElement ( & storageId , peekLen + 1 + sizeof ( sockaddr_in6 ) , & bufferPointer ) ;
2023-07-05 13:49:03 +02:00
if ( result ! = returnvalue : : OK ) {
2023-07-05 23:47:07 +02:00
return ;
2023-07-05 13:49:03 +02:00
}
2023-07-05 23:47:07 +02:00
* bufferPointer = IP6 ;
uint8_t * applicationData = bufferPointer + 1 + sizeof ( sockaddr_in6 ) ;
senderlen = sizeof ( sockaddr_in6 ) ;
2023-07-05 13:49:03 +02:00
2023-07-05 23:47:07 +02:00
ssize_t receivedLen = recvfrom ( serverSocket , applicationData , peekLen , 0 ,
( struct sockaddr * ) ( bufferPointer + 1 ) , & senderlen ) ;
2023-07-05 13:49:03 +02:00
if ( receivedLen = = - 1 ) {
2023-07-05 23:47:07 +02:00
return ;
2023-07-05 13:49:03 +02:00
}
if ( receivedLen ! = peekLen ) {
// should not happen, if it does throw away
2023-07-05 23:47:07 +02:00
return ;
2023-07-05 13:49:03 +02:00
}
2023-10-27 13:40:29 +02:00
2023-07-05 13:49:03 +02:00
size_t bufferLen = receivedLen ;
2023-07-05 23:47:07 +02:00
const uint8_t * constApplicationData = applicationData ;
2023-07-05 13:49:03 +02:00
FsfwProtocolHeader header ;
2023-07-05 23:47:07 +02:00
result = header . deSerialize ( & constApplicationData , & bufferLen , SerializeIF : : Endianness : : NETWORK ) ;
2023-07-05 13:49:03 +02:00
if ( result ! = returnvalue : : OK ) {
2023-07-05 23:47:07 +02:00
return ;
2023-07-05 13:49:03 +02:00
}
2023-07-05 23:47:07 +02:00
sif : : debug < < " Received msg for 0x " < < std : : hex < < header . getObjectId ( ) < < std : : dec
< < " interface " < < ( int ) header . getInterface ( ) < < std : : endl ;
2023-07-05 13:49:03 +02:00
CommandMessage message ;
switch ( header . getInterface ( ) ) {
case HasActionsIF : : INTERFACE_ID : {
HasActionsIF * object = ObjectManager : : instance ( ) - > get < HasActionsIF > ( header . getObjectId ( ) ) ;
if ( object = = nullptr ) {
2023-07-05 23:47:07 +02:00
return ;
2023-07-05 13:49:03 +02:00
}
2023-07-05 23:47:07 +02:00
2023-07-14 14:11:22 +02:00
ActionMessage : : setCommand ( & message , 1 + sizeof ( sockaddr_in6 ) , storageId ) ;
2023-07-05 13:49:03 +02:00
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 ;
}
2023-07-05 23:47:07 +02:00
}
2023-07-05 13:49:03 +02:00
2023-07-05 23:47:07 +02:00
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 ) {
2023-07-10 16:17:05 +02:00
receiver = ( const sockaddr * ) & defaultDestination ;
2023-07-05 23:47:07 +02:00
receiverLen = defaultDestinationLen ;
} else {
if ( * tcData ! = IP6 ) {
// this should not have been routed here
return ;
}
2023-07-06 13:33:59 +02:00
receiver = ( const sockaddr * ) ( tcData + 1 ) ;
2023-07-05 23:47:07 +02:00
receiverLen = sizeof ( sockaddr_in6 ) ;
}
const uint8_t * tmData ;
size_t tmDataSize ;
result = IPCStore - > getData ( tm , & tmData , & tmDataSize ) ;
if ( result ! = returnvalue : : OK ) {
// nothing to send
2023-07-11 14:57:17 +02:00
if ( IPCStore - > hasDataAtId ( tc ) ) {
2023-07-10 16:17:05 +02:00
IPCStore - > deleteData ( tc ) ;
}
2023-07-05 23:47:07 +02:00
return ;
}
int res = sendto ( serverSocket , tmData , tmDataSize , 0 , receiver , receiverLen ) ;
if ( res = = - 1 ) {
2023-07-06 13:33:59 +02:00
sif : : error < < " UdpTmTcBridge::handleTM: sendto failed with " < < errno < < std : : endl ;
2023-07-05 23:47:07 +02:00
}
2023-07-10 16:17:05 +02:00
IPCStore - > deleteData ( tm ) ;
2023-07-11 14:57:17 +02:00
if ( IPCStore - > hasDataAtId ( tc ) ) {
2023-07-10 16:17:05 +02:00
IPCStore - > deleteData ( tc ) ;
}
2023-07-05 13:49:03 +02:00
}