205 lines
6.8 KiB
C++
205 lines
6.8 KiB
C++
|
#include "TmTcLwIpUdpBridge.h"
|
||
|
|
#include "udp_config.h"
|
||
|
|
#include "app_ethernet.h"
|
||
|
|
#include "ethernetif.h"
|
||
|
|
#include <OBSWConfig.h>
|
||
|
|
|
||
|
|
#include <fsfw/ipc/MutexGuard.h>
|
||
|
|
#include <fsfw/serviceinterface/ServiceInterface.h>
|
||
|
|
#include <fsfw/serialize/EndianConverter.h>
|
||
|
|
|
||
|
|
|
||
|
|
TmTcLwIpUdpBridge::TmTcLwIpUdpBridge(object_id_t objectId,
|
||
|
|
object_id_t ccsdsPacketDistributor, object_id_t tmStoreId,
|
||
|
|
object_id_t tcStoreId):
|
||
|
|
TmTcBridge(objectId, ccsdsPacketDistributor, tmStoreId, tcStoreId) {
|
||
|
|
TmTcLwIpUdpBridge::lastAdd.addr = IPADDR_TYPE_ANY;
|
||
|
|
}
|
||
|
|
|
||
|
|
TmTcLwIpUdpBridge::~TmTcLwIpUdpBridge() {}
|
||
|
|
|
||
|
|
ReturnValue_t TmTcLwIpUdpBridge::initialize() {
|
||
|
|
TmTcBridge::initialize();
|
||
|
|
bridgeLock = MutexFactory::instance()->createMutex();
|
||
|
|
if(bridgeLock == nullptr) {
|
||
|
|
return ObjectManagerIF::CHILD_INIT_FAILED;
|
||
|
|
}
|
||
|
|
ReturnValue_t result = udp_server_init();
|
||
|
|
return result;
|
||
|
|
}
|
||
|
|
|
||
|
|
ReturnValue_t TmTcLwIpUdpBridge::udp_server_init(void) {
|
||
|
|
err_t err;
|
||
|
|
/* Create a new UDP control block */
|
||
|
|
TmTcLwIpUdpBridge::upcb = udp_new();
|
||
|
|
if (TmTcLwIpUdpBridge::upcb)
|
||
|
|
{
|
||
|
|
/* Bind the upcb to the UDP_PORT port */
|
||
|
|
/* Using IP_ADDR_ANY allow the upcb to be used by any local interface */
|
||
|
|
err = udp_bind(TmTcLwIpUdpBridge::upcb, IP_ADDR_ANY, UDP_SERVER_PORT);
|
||
|
|
|
||
|
|
if(err == ERR_OK)
|
||
|
|
{
|
||
|
|
/* Set a receive callback for the upcb */
|
||
|
|
udp_recv(TmTcLwIpUdpBridge::upcb, &udp_server_receive_callback,
|
||
|
|
(void*) this);
|
||
|
|
return RETURN_OK;
|
||
|
|
}
|
||
|
|
else
|
||
|
|
{
|
||
|
|
udp_remove(TmTcLwIpUdpBridge::upcb);
|
||
|
|
return RETURN_FAILED;
|
||
|
|
}
|
||
|
|
} else {
|
||
|
|
return RETURN_FAILED;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
ReturnValue_t TmTcLwIpUdpBridge::performOperation(uint8_t operationCode) {
|
||
|
|
TmTcBridge::performOperation();
|
||
|
|
|
||
|
|
#if TCPIP_RECV_WIRETAPPING == 1
|
||
|
|
if(connectFlag) {
|
||
|
|
uint32_t ipAddress = ((ip4_addr*) &lastAdd)->addr;
|
||
|
|
int ipAddress1 = (ipAddress & 0xFF000000) >> 24;
|
||
|
|
int ipAddress2 = (ipAddress & 0xFF0000) >> 16;
|
||
|
|
int ipAddress3 = (ipAddress & 0xFF00) >> 8;
|
||
|
|
int ipAddress4 = ipAddress & 0xFF;
|
||
|
|
#if OBSW_VERBOSE_LEVEL == 1
|
||
|
|
#if FSFW_CPP_OSTREAM_ENABLED == 1
|
||
|
|
sif::info << "TmTcLwIpUdpBridge: Client IP Address " << std::dec
|
||
|
|
<< ipAddress4 << "." << ipAddress3 << "." << ipAddress2 << "."
|
||
|
|
<< ipAddress1 << std::endl;
|
||
|
|
uint16_t portSwapped = EndianConverter::convertBigEndian(lastPort);
|
||
|
|
sif::info << "TmTcLwIpUdpBridge: Client IP Port "
|
||
|
|
<< (int)portSwapped << std::endl;
|
||
|
|
#else
|
||
|
|
sif::printInfo("TmTcLwIpUdpBridge: Client IP Address %d.%d.%d.%d\n",
|
||
|
|
ipAddress4, ipAddress3, ipAddress2, ipAddress1);
|
||
|
|
uint16_t portSwapped = EndianConverter::convertBigEndian(lastPort);
|
||
|
|
sif::printInfo("TmTcLwIpUdpBridge: Client IP Port: %d\n", portSwapped);
|
||
|
|
#endif
|
||
|
|
#endif
|
||
|
|
connectFlag = false;
|
||
|
|
}
|
||
|
|
#endif
|
||
|
|
|
||
|
|
return RETURN_OK;
|
||
|
|
}
|
||
|
|
|
||
|
|
ReturnValue_t TmTcLwIpUdpBridge::sendTm(const uint8_t * data, size_t dataLen) {
|
||
|
|
struct pbuf *p_tx = pbuf_alloc(PBUF_TRANSPORT, dataLen, PBUF_RAM);
|
||
|
|
if ((p_tx != nullptr) && (lastAdd.addr != IPADDR_TYPE_ANY) && (upcb != nullptr)) {
|
||
|
|
/* copy data to pbuf */
|
||
|
|
err_t err = pbuf_take(p_tx, (char*) data, dataLen);
|
||
|
|
if(err!=ERR_OK){
|
||
|
|
pbuf_free(p_tx);
|
||
|
|
return err;
|
||
|
|
}
|
||
|
|
/* Connect to the remote client */
|
||
|
|
err = udp_connect(TmTcLwIpUdpBridge::upcb, &lastAdd , lastPort);
|
||
|
|
if(err != ERR_OK){
|
||
|
|
pbuf_free(p_tx);
|
||
|
|
return err;
|
||
|
|
}
|
||
|
|
/* Tell the client that we have accepted it */
|
||
|
|
err = udp_send(TmTcLwIpUdpBridge::upcb, p_tx);
|
||
|
|
pbuf_free(p_tx);
|
||
|
|
if(err!=ERR_OK){
|
||
|
|
return err;
|
||
|
|
}
|
||
|
|
|
||
|
|
/* free the UDP connection, so we can accept new clients */
|
||
|
|
udp_disconnect (TmTcLwIpUdpBridge::upcb);
|
||
|
|
}
|
||
|
|
else{
|
||
|
|
return RETURN_FAILED;
|
||
|
|
}
|
||
|
|
return RETURN_OK;
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
void TmTcLwIpUdpBridge::udp_server_receive_callback(void* arg,
|
||
|
|
struct udp_pcb* upcb_, struct pbuf* p, const ip_addr_t* addr,
|
||
|
|
u16_t port) {
|
||
|
|
struct pbuf *p_tx = nullptr;
|
||
|
|
auto udpBridge = reinterpret_cast<TmTcLwIpUdpBridge*>(arg);
|
||
|
|
if(udpBridge == nullptr) {
|
||
|
|
#if FSFW_CPP_OSTREAM_ENABLED == 1
|
||
|
|
sif::warning << "TmTcLwIpUdpBridge::udp_server_receive_callback: Invalid UDP bridge!" <<
|
||
|
|
std::endl;
|
||
|
|
#else
|
||
|
|
sif::printWarning("TmTcLwIpUdpBridge::udp_server_receive_callback: Invalid UDP bridge!\n");
|
||
|
|
#endif
|
||
|
|
}
|
||
|
|
/* allocate pbuf from RAM*/
|
||
|
|
p_tx = pbuf_alloc(PBUF_TRANSPORT,p->len, PBUF_RAM);
|
||
|
|
|
||
|
|
if(p_tx != NULL)
|
||
|
|
{
|
||
|
|
if(udpBridge != nullptr) {
|
||
|
|
MutexGuard lg(udpBridge->bridgeLock);
|
||
|
|
udpBridge->upcb = upcb_;
|
||
|
|
udpBridge->lastAdd = *addr;
|
||
|
|
udpBridge->lastPort = port;
|
||
|
|
if(not udpBridge->comLinkUp()) {
|
||
|
|
udpBridge->registerCommConnect();
|
||
|
|
#if TCPIP_RECV_WIRETAPPING == 1
|
||
|
|
udpBridge->connectFlag = true;
|
||
|
|
#endif
|
||
|
|
/* This should have already been done, but we will still do it */
|
||
|
|
udpBridge->physicalConnectStatusChange(true);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
pbuf_take(p_tx, (char*)p->payload, p->len);
|
||
|
|
/* send the received data to the uart port */
|
||
|
|
char* data = reinterpret_cast<char*>(p_tx->payload);
|
||
|
|
*(data+p_tx->len) = '\0';
|
||
|
|
|
||
|
|
#if TCPIP_RECV_WIRETAPPING == 1
|
||
|
|
udpBridge->printData(p,data);
|
||
|
|
#endif
|
||
|
|
|
||
|
|
store_address_t storeId;
|
||
|
|
ReturnValue_t returnValue = udpBridge->tcStore->addData(&storeId,
|
||
|
|
reinterpret_cast<uint8_t*>(p->payload), p->len);
|
||
|
|
if (returnValue != RETURN_OK) {
|
||
|
|
#if FSFW_CPP_OSTREAM_ENABLED == 1
|
||
|
|
sif::warning << "UDP Server: Data storage failed" << std::endl;
|
||
|
|
#endif
|
||
|
|
pbuf_free(p_tx);
|
||
|
|
return;
|
||
|
|
}
|
||
|
|
TmTcMessage message(storeId);
|
||
|
|
if (udpBridge->tmTcReceptionQueue->sendToDefault(&message)
|
||
|
|
!= RETURN_OK) {
|
||
|
|
#if FSFW_CPP_OSTREAM_ENABLED == 1
|
||
|
|
sif::warning << "TmTcLwIpUdpBridgw::udp_server_receive_callback:"
|
||
|
|
<< " Sending message to queue failed" << std::endl;
|
||
|
|
#endif
|
||
|
|
udpBridge->tcStore->deleteData(storeId);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
/* Free the p_tx buffer */
|
||
|
|
pbuf_free(p_tx);
|
||
|
|
}
|
||
|
|
|
||
|
|
/* Caller must ensure thread-safety */
|
||
|
|
bool TmTcLwIpUdpBridge::comLinkUp() const {
|
||
|
|
return communicationLinkUp;
|
||
|
|
}
|
||
|
|
|
||
|
|
/* Caller must ensure thread-safety */
|
||
|
|
void TmTcLwIpUdpBridge::physicalConnectStatusChange(bool connect) {
|
||
|
|
if(connect) {
|
||
|
|
/* Physical connection does not mean there is a recipient to send packets too.
|
||
|
|
This will be done by the receive callback! */
|
||
|
|
physicalConnection = true;
|
||
|
|
}
|
||
|
|
else {
|
||
|
|
physicalConnection = false;
|
||
|
|
/* If there is no physical connection, we can't send anything back */
|
||
|
|
registerCommDisconnect();
|
||
|
|
}
|
||
|
|
}
|