fsfw_example_public/common/stm32_nucleo/networking/TmTcLwIpUdpBridge.cpp

#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();
    }
}