/*
 * Copyright (C) 2021 Satlab A/S (https://www.satlab.com)
 *
 * This file is part of CSP. See COPYING for details.
 */

#include <csp/interfaces/csp_if_sltcp.h>

#include <stdint.h>
#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include <unistd.h>
#include <errno.h>
#include <inttypes.h>

#ifdef LINUX_TEMP_PORT
#include <sys/socket.h>
#include <sys/ioctl.h>
#include <netinet/in.h>
#include <arpa/inet.h>
#include <net/if.h>
#else
#include <sockets.h>
#endif

#include <pthread.h>

#include <csp/csp.h>
#include <csp/csp_endian.h>
#include <csp/csp_interface.h>
#include <csp/arch/csp_malloc.h>
#include <csp/arch/csp_thread.h>

#ifdef S_BAND_SIM_CB
#include <comms_csw.h>
#endif

#include <exo_common.h>
#include"comms_uhf_csp.h"
#include"csp_if_uhf.h"
#include "csp_comms.h"

/* MTU of the sltcp interface */
#define CSP_IF_SLTCP_MTU   1400

uint32_t sl_tcp_port =0;
uint32_t sl_tcp_addr =0;
FILE *sl_tcp_inf;

#ifdef LINUX_TEMP_PORT
s_obc_sock_info obc_gs1_soc_inf;
s_obc_sock_info obc_gs2_soc_inf;
s_obc_sock_info obc_uhf_soc_inf;

#endif

struct csp_sltcp_ifdata {
    char ifname[CSP_IFLIST_NAME_MAX + 1];
    csp_iface_t iface;

    int dest_socket, server;
    struct sockaddr_in dest_addr;
    pthread_t rx_thread;
};

struct sockaddr_in si_other;
int slen = sizeof(struct sockaddr_in);

#ifdef S_BAND_SIM_CB
extern s_comms_rx_cfg rx_cfg_sim;
#endif

static const uint32_t crc32c_table[256] = {
	0x00000000l, 0xf26b8303l, 0xe13b70f7l, 0x1350f3f4l,
	0xc79a971fl, 0x35f1141cl, 0x26a1e7e8l, 0xd4ca64ebl,
	0x8ad958cfl, 0x78b2dbccl, 0x6be22838l, 0x9989ab3bl,
	0x4d43cfd0l, 0xbf284cd3l, 0xac78bf27l, 0x5e133c24l,
	0x105ec76fl, 0xe235446cl, 0xf165b798l, 0x030e349bl,
	0xd7c45070l, 0x25afd373l, 0x36ff2087l, 0xc494a384l,
	0x9a879fa0l, 0x68ec1ca3l, 0x7bbcef57l, 0x89d76c54l,
	0x5d1d08bfl, 0xaf768bbcl, 0xbc267848l, 0x4e4dfb4bl,
	0x20bd8edel, 0xd2d60dddl, 0xc186fe29l, 0x33ed7d2al,
	0xe72719c1l, 0x154c9ac2l, 0x061c6936l, 0xf477ea35l,
	0xaa64d611l, 0x580f5512l, 0x4b5fa6e6l, 0xb93425e5l,
	0x6dfe410el, 0x9f95c20dl, 0x8cc531f9l, 0x7eaeb2fal,
	0x30e349b1l, 0xc288cab2l, 0xd1d83946l, 0x23b3ba45l,
	0xf779deael, 0x05125dadl, 0x1642ae59l, 0xe4292d5al,
	0xba3a117el, 0x4851927dl, 0x5b016189l, 0xa96ae28al,
	0x7da08661l, 0x8fcb0562l, 0x9c9bf696l, 0x6ef07595l,
	0x417b1dbcl, 0xb3109ebfl, 0xa0406d4bl, 0x522bee48l,
	0x86e18aa3l, 0x748a09a0l, 0x67dafa54l, 0x95b17957l,
	0xcba24573l, 0x39c9c670l, 0x2a993584l, 0xd8f2b687l,
	0x0c38d26cl, 0xfe53516fl, 0xed03a29bl, 0x1f682198l,
	0x5125dad3l, 0xa34e59d0l, 0xb01eaa24l, 0x42752927l,
	0x96bf4dccl, 0x64d4cecfl, 0x77843d3bl, 0x85efbe38l,
	0xdbfc821cl, 0x2997011fl, 0x3ac7f2ebl, 0xc8ac71e8l,
	0x1c661503l, 0xee0d9600l, 0xfd5d65f4l, 0x0f36e6f7l,
	0x61c69362l, 0x93ad1061l, 0x80fde395l, 0x72966096l,
	0xa65c047dl, 0x5437877el, 0x4767748al, 0xb50cf789l,
	0xeb1fcbadl, 0x197448ael, 0x0a24bb5al, 0xf84f3859l,
	0x2c855cb2l, 0xdeeedfb1l, 0xcdbe2c45l, 0x3fd5af46l,
	0x7198540dl, 0x83f3d70el, 0x90a324fal, 0x62c8a7f9l,
	0xb602c312l, 0x44694011l, 0x5739b3e5l, 0xa55230e6l,
	0xfb410cc2l, 0x092a8fc1l, 0x1a7a7c35l, 0xe811ff36l,
	0x3cdb9bddl, 0xceb018del, 0xdde0eb2al, 0x2f8b6829l,
	0x82f63b78l, 0x709db87bl, 0x63cd4b8fl, 0x91a6c88cl,
	0x456cac67l, 0xb7072f64l, 0xa457dc90l, 0x563c5f93l,
	0x082f63b7l, 0xfa44e0b4l, 0xe9141340l, 0x1b7f9043l,
	0xcfb5f4a8l, 0x3dde77abl, 0x2e8e845fl, 0xdce5075cl,
	0x92a8fc17l, 0x60c37f14l, 0x73938ce0l, 0x81f80fe3l,
	0x55326b08l, 0xa759e80bl, 0xb4091bffl, 0x466298fcl,
	0x1871a4d8l, 0xea1a27dbl, 0xf94ad42fl, 0x0b21572cl,
	0xdfeb33c7l, 0x2d80b0c4l, 0x3ed04330l, 0xccbbc033l,
	0xa24bb5a6l, 0x502036a5l, 0x4370c551l, 0xb11b4652l,
	0x65d122b9l, 0x97baa1bal, 0x84ea524el, 0x7681d14dl,
	0x2892ed69l, 0xdaf96e6al, 0xc9a99d9el, 0x3bc21e9dl,
	0xef087a76l, 0x1d63f975l, 0x0e330a81l, 0xfc588982l,
	0xb21572c9l, 0x407ef1cal, 0x532e023el, 0xa145813dl,
	0x758fe5d6l, 0x87e466d5l, 0x94b49521l, 0x66df1622l,
	0x38cc2a06l, 0xcaa7a905l, 0xd9f75af1l, 0x2b9cd9f2l,
	0xff56bd19l, 0x0d3d3e1al, 0x1e6dcdeel, 0xec064eedl,
	0xc38d26c4l, 0x31e6a5c7l, 0x22b65633l, 0xd0ddd530l,
	0x0417b1dbl, 0xf67c32d8l, 0xe52cc12cl, 0x1747422fl,
	0x49547e0bl, 0xbb3ffd08l, 0xa86f0efcl, 0x5a048dffl,
	0x8ecee914l, 0x7ca56a17l, 0x6ff599e3l, 0x9d9e1ae0l,
	0xd3d3e1abl, 0x21b862a8l, 0x32e8915cl, 0xc083125fl,
	0x144976b4l, 0xe622f5b7l, 0xf5720643l, 0x07198540l,
	0x590ab964l, 0xab613a67l, 0xb831c993l, 0x4a5a4a90l,
	0x9e902e7bl, 0x6cfbad78l, 0x7fab5e8cl, 0x8dc0dd8fl,
	0xe330a81al, 0x115b2b19l, 0x020bd8edl, 0xf0605beel,
	0x24aa3f05l, 0xd6c1bc06l, 0xc5914ff2l, 0x37faccf1l,
	0x69e9f0d5l, 0x9b8273d6l, 0x88d28022l, 0x7ab90321l,
	0xae7367cal, 0x5c18e4c9l, 0x4f48173dl, 0xbd23943el,
	0xf36e6f75l, 0x0105ec76l, 0x12551f82l, 0xe03e9c81l,
	0x34f4f86al, 0xc69f7b69l, 0xd5cf889dl, 0x27a40b9el,
	0x79b737bal, 0x8bdcb4b9l, 0x988c474dl, 0x6ae7c44el,
	0xbe2da0a5l, 0x4c4623a6l, 0x5f16d052l, 0xad7d5351l
};

uint32_t crc32c_update(uint32_t crc, const uint8_t *input, size_t bytes)
{
	while (bytes--)
		crc = crc32c_table[(crc ^ *input++) & 0xff] ^ (crc >> 8);

	return crc;
}

uint32_t crc32c(const uint8_t *input, size_t bytes)
{
	uint32_t crc = ~(uint32_t)0;

	crc = crc32c_update(crc, input, bytes);

	return crc ^ ~0UL;
}

#ifdef SDR_HWIL
struct csp_sltcp_ifdata sltcp_data;
static void csp_sltcp_rx_gs1(void *arg);
extern exo_sw_dbg_cb exo_sw_dbg;

static void csp_sltcp_rx_gs1(void *arg)
{
    int32_t maxsd=0,sd = 0;
    int32_t i, socketcount ;
    int dest_addr_len ;
    struct csp_sltcp_ifdata *data = &sltcp_data;
    int32_t clientsocket[10] = {0};

    fd_set readfds;
    FD_ZERO(&readfds);
    int nbytes;
    uint32_t id;
    uint16_t length;
    csp_packet_t *packet;

#ifdef S_BAND_SIM_CB
    uint32_t crc_val = 0, rcv_crc_val = 0;
    uint8_t frame[sizeof(length) + sizeof(id) + CSP_IF_SLTCP_MTU + sizeof(crc_val)];
#else
    uint8_t frame[sizeof(id) + CSP_IF_SLTCP_MTU];
#endif

    // Accept a new client connection
    dest_addr_len = sizeof(data->dest_addr);

    while(1)
    {
        memset(frame, 0,sizeof(frame));
        FD_ZERO(&readfds);
        FD_SET( data->server, &readfds);

        maxsd = data->server;

        for(i=0;i<10;i++)
        {
            sd = clientsocket[i];
            if(sd>0)
                FD_SET(clientsocket[i],&readfds);
            if(sd>maxsd)
            {
                maxsd = sd;
            }
        }

        socketcount = select( maxsd+1, &readfds, NULL, NULL, NULL );

        if(socketcount<1)
        {
#ifdef COMMS_SBAND_SIM_DGB_PRNT
            DEBUG_CPRINT(("COMMS_SBAND_SIM: [-]Error in select"));
#endif
        }

        if(FD_ISSET(data->server, &readfds))
        {
            if((data->dest_socket = accept(data->server, (struct sockaddr *)&data->dest_addr, (socklen_t *)&dest_addr_len)) < 0) {
                DEBUG_CPRINT(("[SLTCP] Error in accepting TCTM Socket.\n"));
            }
            else
            {
                DEBUG_CPRINT(("[SLTCP] Accepted new connection from %s:%d\n", inet_ntoa(data->dest_addr.sin_addr), (data->dest_addr.sin_port)));
            }

            for(i=0;i<10;i++)
            {
                if(clientsocket[i]==0)
                {
                	clientsocket[i]=data->dest_socket;
                	break;
                }
            }
        }
        else
        {
            for(i=0;i<10;i++)
            {
                sd = clientsocket[i];
                if(FD_ISSET(sd, &readfds))
                {
                    nbytes = recv(sd,frame,sizeof(frame),MSG_DONTWAIT);
                    if(nbytes==0 )
                    {
                        close(sd);
                        clientsocket[i]=0;
                    }
                    else if(nbytes>=0)
                    {

#ifdef S_BAND_SIM_CB
                        /*Control variable - Increments on every packet received in csp-sltcp*/
                        exo_sw_dbg.comms_dbg.csp_sltcp_rcvd++;
                        if (nbytes < 0 || (unsigned int)nbytes < sizeof(frame))
                            continue;

                        crc_val = crc32c(frame,(sizeof(frame) - 4));
                        memcpy(&rcv_crc_val,&frame[sizeof(frame)-4],4);

#if (CSP_DEBUG)
                        DEBUG_CPRINT(("\n[SBAND_SIM] Received bytes %d\n",nbytes));
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wformat"
                        DEBUG_CPRINT(("\n[SBAND_SIM] Received CRC %x\n",rcv_crc_val));
#pragma GCC diagnostic pop
#endif

                        if(rcv_crc_val == crc_val)
                        {
                            /** CRC matched */
                            rx_cfg_sim.rx_cfg_detected++;
                            nbytes = nbytes - 4;
                        }
                        else
                        {
#if (CSP_DEBUG)
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wformat"
                            DEBUG_CPRINT(("\n[SBAND_SIM] CRC %x\n",crc_val));
#pragma GCC diagnostic pop
#endif
                            DEBUG_CPRINT(("\n[SBAND_SIM] CRC mismatched \n"));
                            continue;
                        }

                        /* Extract CSP length */
                        memcpy(&length, &frame[0], sizeof(length));

                        /* Allocate buffer for frame */
                        packet = csp_buffer_get(length);
                        if (!packet)
                            continue;

                        packet->length = length;

                        /* Set packet ID */
                        memcpy(&id, &frame[sizeof(uint16_t)], sizeof(id));
                        packet->id.ext = csp_ntoh32(id);

                        /* Copy received data into CSP buffer */
                        memcpy(packet->data, &frame[sizeof(id)+sizeof(uint16_t)], packet->length);
#else

                        if (nbytes < 0 || (unsigned int)nbytes < sizeof(id))
                            continue;

                        /* Extract CSP length */
                        length = nbytes - sizeof(id);

                        /* Allocate buffer for frame */
                        packet = csp_buffer_get(length);
                        if (!packet)
                            continue;
                        packet->length = length;

                        /* Set packet ID */
                        memcpy(&id, &frame[0], sizeof(id));
                        packet->id.ext = csp_ntoh32(id);

                        /* Copy received data into CSP buffer */
                        memcpy(packet->data, &frame[sizeof(id)], packet->length);

#endif

                        /* Pass frame to CSP stack */
                        csp_qfifo_write(packet, &data->iface, NULL);
                    }
                }
            }
        }
    }
    close(data->server);

}

static int csp_sltcp_tx_gs1(const csp_route_t *route, csp_packet_t *packet)
{
    struct csp_sltcp_ifdata *data = (struct csp_sltcp_ifdata*)route->iface->driver_data;

    int nbytes;
    uint32_t id;
    uint16_t length = 0, size = 0;
    size=size;
    struct sockaddr_in dest = data->dest_addr;

    uint8_t frame[sizeof(id) + CSP_IF_SLTCP_MTU];
    id = csp_hton32(packet->id.ext);

    memcpy(&frame[length], &id, sizeof(id));
    length += sizeof(id);
    memcpy(&frame[length], packet->data, packet->length);
    length += packet->length;

    /* Send message */
    nbytes = sendto(data->dest_socket, frame, length, 0,(struct sockaddr *) &dest, sizeof(dest));

    if (nbytes != (int)length)
        return CSP_ERR_TIMEDOUT;

    csp_buffer_free(packet);

    return CSP_ERR_NONE;
}

int csp_sltcp_init_gs1(const char *device, const char *ifname, csp_iface_t **ifc)
{
    int ret ;
    struct sockaddr_in sa;

#ifndef LINUX_TEMP_PORT
    ip4_addr_t eth_ipaddr;
#endif

    /* Allocate interface data */
    struct csp_sltcp_ifdata *data = &sltcp_data;
    if (!data)
        return CSP_ERR_NOMEM;

    if (!ifname)
        ifname = CSP_IF_SLTCP_DEFAULT_NAME;

    strncpy(data->ifname, ifname, sizeof(data->ifname) - 1);
    data->iface.name = data->ifname;
    data->iface.driver_data = data;
    data->iface.nexthop = csp_sltcp_tx_gs1;
    data->iface.mtu = CSP_IF_SLTCP_MTU + 20;

    /* Create socket */
    data->server = socket(AF_INET, SOCK_STREAM, 0);
    if (data->server < 0) {
        csp_log_error("failed to allocate TCP socket\n");
        csp_tm_global.csp_err_driver ++ ;
        return CSP_ERR_DRIVER;
    }

#ifdef LINUX_TEMP_PORT

    DEBUG_CPRINT(("\n\r[SLTCP] OBC TC TM socket address is %s %u \n",obc_gs1_soc_inf.ip_addr,(obc_gs1_soc_inf.port)));
    //sl_tcp_port = atoi(obc_gs1_soc_inf.port_num);
    sl_tcp_port = (obc_gs1_soc_inf.port);
    sl_tcp_addr = inet_addr(obc_gs1_soc_inf.ip_addr);
#else

#ifdef GSOBC_USB_EN
    IP4_ADDR(&eth_ipaddr, USB_IP_ADDR0, USB_IP_ADDR1, USB_IP_ADDR2, USB_IP_ADDR3);
#else
    IP4_ADDR(&eth_ipaddr, ETH_IP_ADDR0, ETH_IP_ADDR1, ETH_IP_ADDR2, ETH_IP_ADDR3);
#endif
    sl_tcp_port  = GS_SRVR_PORT;
    sl_tcp_addr  = eth_ipaddr.addr;
#endif

    /* Bind to incoming port on interface */
    memset(&sa, 0, sizeof(struct sockaddr_in));
    sa.sin_family = AF_INET;
    sa.sin_addr.s_addr = sl_tcp_addr;
    sa.sin_port = htons(sl_tcp_port);

    ret = bind(data->server, (const struct sockaddr *)&sa, sizeof(struct sockaddr_in));
    if (ret < 0) {
        csp_log_error("[SLTCP] failed to bind receive socket\n");
        csp_tm_global.csp_err_driver ++ ;
        return CSP_ERR_DRIVER;
    }

    // Listen for incoming connections
    if (listen(data->server, 10) == -1) {
        perror("listen");
    }

    DEBUG_CPRINT(("[SLTCP] Server listening on port %d\n", sa.sin_port));

    /* Create destination address */
    memset(&data->dest_addr, 0, sizeof(data->dest_addr));
    data->dest_addr.sin_family = AF_INET;
    data->dest_addr.sin_addr.s_addr = sl_tcp_addr;
    data->dest_addr.sin_port = sl_tcp_port;

    /* Start receiver thread */
    ret = os_thread_create(CSP_ETH_RX, T_STACK_5K, P_CSP_INTF_ETH_RX,"CSP Ethernet RX",csp_sltcp_rx_gs1, data,NULL,NULL);

    if (ret < 0) {
        csp_log_error("failed to start receive thread\n");
        csp_tm_global.csp_err_driver ++ ;
        return CSP_ERR_DRIVER;
    }

    /* Register interface */
    csp_iflist_add(&data->iface);

    if (ifc)
        *ifc = &data->iface;

    return CSP_ERR_NONE;
}
#endif

#ifdef BACKDOOR_SOCK_ENB
struct csp_sltcp_ifdata sltcp_data_gs2;
static void csp_sltcp_rx_gs2(void *arg);

static void csp_sltcp_rx_gs2(void *arg)
{
    int32_t maxsd=0,sd = 0;
    int32_t i, socketcount ;
    int dest_addr_len ;
    struct csp_sltcp_ifdata *data = &sltcp_data_gs2;
    int32_t clientsocket[10] = {0};

    fd_set readfds;
    FD_ZERO(&readfds);
    int nbytes;
    uint32_t id;
    uint16_t length;
    csp_packet_t *packet;

#ifdef S_BAND_SIM_CB
    uint32_t crc_val = 0, rcv_crc_val = 0;

    uint8_t frame[sizeof(length) + sizeof(id) + CSP_IF_SLTCP_MTU + sizeof(crc_val)];
#else
    uint8_t frame[sizeof(id) + CSP_IF_SLTCP_MTU];
#endif

    // Accept a new client connection
    dest_addr_len = sizeof(data->dest_addr);

    while(1)
    {
        memset(frame, 0,sizeof(frame));
        FD_ZERO(&readfds);
        FD_SET( data->server, &readfds);

        maxsd = data->server;

        for(i=0;i<10;i++)
        {
            sd = clientsocket[i];
            if(sd>0)
                FD_SET(clientsocket[i],&readfds);
            if(sd>maxsd)
            {
                maxsd = sd;
            }
        }

        socketcount = select( maxsd+1, &readfds, NULL, NULL, NULL );

        if(socketcount<1)
        {
#ifdef COMMS_SBAND_SIM_DGB_PRNT
            DEBUG_CPRINT(("COMMS_SBAND_SIM: [-]Error in select"));
#endif
        }

        if(FD_ISSET(data->server, &readfds))
        {
            if((data->dest_socket = accept(data->server, (struct sockaddr *)&data->dest_addr, (socklen_t *)&dest_addr_len)) < 0) {
                DEBUG_CPRINT(("[SLTCP] Error in accepting TCTM Socket.\n"));
            }
            else
            {
                DEBUG_CPRINT(("[SLTCP] Accepted new connection from %s:%d\n", inet_ntoa(data->dest_addr.sin_addr), (data->dest_addr.sin_port)));
            }

            for(i=0;i<10;i++)
            {
                if(clientsocket[i]==0)
                {
                	clientsocket[i]=data->dest_socket;
                	break;
                }
            }
        }
        else
        {
            for(i=0;i<10;i++)
            {
                sd = clientsocket[i];
                if(FD_ISSET(sd, &readfds))
                {
                    nbytes = recv(sd,frame,sizeof(frame),MSG_DONTWAIT);
                    if(nbytes==0 )
                    {
                        close(sd);
                        clientsocket[i]=0;
                    }
                    else if(nbytes>=0)
                    {
#ifdef S_BAND_SIM_CB
                    	if (nbytes < 0 || (unsigned int)nbytes < sizeof(frame))
                    		continue;

                    	crc_val = crc32c(frame,(sizeof(frame) - 4));
                    	memcpy(&rcv_crc_val,&frame[sizeof(frame)-4],4);

#if (CSP_DEBUG)
                    	DEBUG_CPRINT(("\n[SBAND_SIM] Received bytes %d\n",nbytes));
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wformat"
                    	DEBUG_CPRINT(("\n[SBAND_SIM] Received CRC %x\n",rcv_crc_val));
#pragma GCC diagnostic pop
#endif

                    	if(rcv_crc_val == crc_val)
                    	{
                    		/** CRC matched */
                    		rx_cfg_sim.rx_cfg_detected++;
                    		nbytes = nbytes - 4;
                    	}
                    	else
                    	{
#if (CSP_DEBUG)
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wformat"
                    		DEBUG_CPRINT(("\n[SBAND_SIM] CRC %x\n",crc_val));
#pragma GCC diagnostic pop
#endif
                    		DEBUG_CPRINT(("\n[SBAND_SIM] CRC mismatched \n"));
                    		continue;
                    	}

                    	/* Extract CSP length */
                    	memcpy(&length, &frame[0], sizeof(length));

                    	/* Allocate buffer for frame */
                    	packet = csp_buffer_get(length);
                    	if (!packet)
                    		continue;

                    	packet->length = length;

                    	/* Set packet ID */
                    	memcpy(&id, &frame[sizeof(uint16_t)], sizeof(id));
                    	packet->id.ext = csp_ntoh32(id);

                    	/* Copy received data into CSP buffer */
                    	memcpy(packet->data, &frame[sizeof(id)+sizeof(uint16_t)], packet->length);
#else
                    	//DEBUG_CPRINT(("\n\r[SLUDP] BACKDOOR Received packet %d from %s:%d\n",nbytes, inet_ntoa(si_udp.sin_addr), ntohs(si_udp.sin_port)));
                    	DEBUG_CPRINT(("\n\r[SLTCP] BACKDOOR Received packet %d \n",nbytes));

                    	if (nbytes < 0 || (unsigned int)nbytes < sizeof(id))
                    		continue;

                    	/* Extract CSP length */
                    	length = nbytes - sizeof(id);

                    	/* Allocate buffer for frame */
                    	packet = csp_buffer_get(length);
                    	if (!packet)
                    		continue;
                    	packet->length = length;

                    	/* Set packet ID */
                    	memcpy(&id, &frame[0], sizeof(id));
                    	packet->id.ext = csp_ntoh32(id);

                    	/* Copy received data into CSP buffer */
                    	memcpy(packet->data, &frame[sizeof(id)], packet->length);

#endif
                    	/* Pass frame to CSP stack */
                    	//csp_qfifo_write(packet, &data->iface, NULL);
                    	/*Control variable - Increments on every packer received in csp-sludp*/
                    	//exo_sw_dbg_cb.comms_dbg.csp_sludp_rcvd++;
                    	comms_back_door_data_receive(packet->data, packet->length);
                    	csp_buffer_free(packet);
                    }
                }
            }
        }
    }
    close(data->server);

}

static int csp_sltcp_tx_gs2(const csp_route_t *route, csp_packet_t *packet)
{
    struct csp_sltcp_ifdata *data = (struct csp_sltcp_ifdata*)route->iface->driver_data;

    int nbytes;
    uint32_t id;
    uint16_t length = 0, size = 0;
    size = size;
    length = length;
    struct sockaddr_in dest = data->dest_addr;
#ifdef S_BAND_SIM_CB
    uint32_t crc_val = 0;
    uint8_t frame[sizeof(length) + sizeof(id) + CSP_IF_SLTCP_MTU + sizeof(crc_val)];

    memset(frame,0,sizeof(frame));

    size = packet->length;
    id = csp_hton32(packet->id.ext);
    memcpy(&frame[length], &size, sizeof(size));
    length += sizeof(size);

    memcpy(&frame[length], &id, sizeof(id));
    length += sizeof(id);
    memcpy(&frame[length], packet->data, packet->length);
    length += packet->length;

    crc_val = crc32c(frame,((sizeof(frame)) - sizeof(crc_val)));
    memcpy(&frame[sizeof(frame)-sizeof(crc_val)], &crc_val, sizeof(crc_val));

#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wformat"
    DEBUG_CPRINT(("GS2 CRC VAL : %x ", crc_val));
#pragma GCC diagnostic pop

    /* Send message */
    nbytes = sendto(data->dest_socket, frame, sizeof(frame), 0,(struct sockaddr *) &dest, sizeof(dest));

    if (nbytes != (int)sizeof(frame))
    {
        csp_tm_global.csp_err_timedout++;
        return CSP_ERR_TIMEDOUT;
    }
#else
    uint8_t frame[sizeof(id) + CSP_IF_SLTCP_MTU];
    id = csp_hton32(packet->id.ext);

    memcpy(&frame[length], &id, sizeof(id));
    length += sizeof(id);
    memcpy(&frame[length], packet->data, packet->length);
    length += packet->length;

    /* Send message */
    nbytes = sendto(data->dest_socket, frame, length, 0,(struct sockaddr *) &dest, sizeof(dest));

    if (nbytes != (int)length)
        return CSP_ERR_TIMEDOUT;

#endif

    csp_buffer_free(packet);

    return CSP_ERR_NONE;
}

int csp_sltcp_init_gs2(const char *device, const char *ifname, csp_iface_t **ifc)
{
    int ret ;
    struct sockaddr_in sa;

#ifndef LINUX_TEMP_PORT
    ip4_addr_t eth_ipaddr;
#endif

    /* Allocate interface data */
    struct csp_sltcp_ifdata *data = &sltcp_data_gs2;
    if (!data)
        return CSP_ERR_NOMEM;

    if (!ifname)
        ifname = CSP_IF_SLTCP_DEFAULT_NAME_GS2;

    strncpy(data->ifname, ifname, sizeof(data->ifname) - 1);
    data->iface.name = data->ifname;
    data->iface.driver_data = data;
    data->iface.nexthop = csp_sltcp_tx_gs2;
    data->iface.mtu = CSP_IF_SLTCP_MTU + 20;

    /* Create socket */
    data->server = socket(AF_INET, SOCK_STREAM, 0);
    if (data->server < 0) {
        csp_log_error("failed to allocate TCP socket\n");
        csp_tm_global.csp_err_driver ++ ;
        return CSP_ERR_DRIVER;
    }

#ifdef LINUX_TEMP_PORT
    DEBUG_CPRINT(("\n\r[SLTCP - GS2] OBC TC TM socket address is %s %u \n",obc_gs2_soc_inf.ip_addr,(obc_gs2_soc_inf.port)));
    sl_tcp_port = (obc_gs2_soc_inf.port);
    sl_tcp_addr = inet_addr(obc_gs2_soc_inf.ip_addr);
#else

#ifdef GSOBC_USB_EN
    IP4_ADDR(&eth_ipaddr, USB_IP_ADDR0, USB_IP_ADDR1, USB_IP_ADDR2, USB_IP_ADDR3);
#else
    IP4_ADDR(&eth_ipaddr, ETH_IP_ADDR0, ETH_IP_ADDR1, ETH_IP_ADDR2, ETH_IP_ADDR3);
#endif
    sl_tcp_port  = GS_SRVR_PORT;
    sl_tcp_addr  = eth_ipaddr.addr;
#endif

    /* Bind to incoming port on interface */
    memset(&sa, 0, sizeof(struct sockaddr_in));
    sa.sin_family = AF_INET;
    sa.sin_addr.s_addr = sl_tcp_addr;
    sa.sin_port = htons(sl_tcp_port);

    ret = bind(data->server, (const struct sockaddr *)&sa, sizeof(struct sockaddr_in));
    if (ret < 0) {
        csp_log_error("[SLTCP - GS2] failed to bind receive socket\n");
        csp_tm_global.csp_err_driver ++ ;
        return CSP_ERR_DRIVER;
    }

    // Listen for incoming connections
    if (listen(data->server, 10) == -1) {
        perror("listen");
    }

    DEBUG_CPRINT(("[SLTCP - GS2] Server listening on port %d\n", sa.sin_port));

    /* Create destination address */
    memset(&data->dest_addr, 0, sizeof(data->dest_addr));
    data->dest_addr.sin_family = AF_INET;
    data->dest_addr.sin_addr.s_addr = sl_tcp_addr;
    data->dest_addr.sin_port = sl_tcp_port;

    /* Start receiver thread */
    ret = os_thread_create(CSP_ETH_RX, T_STACK_5K, P_CSP_INTF_ETH_RX,"CSP Ethernet RX",csp_sltcp_rx_gs2, data,NULL,NULL);

    if (ret < 0) {
        csp_log_error("[SLTCP - GS2] failed to start receive thread\n");
        csp_tm_global.csp_err_driver ++ ;
        return CSP_ERR_DRIVER;
    }

    /* Register interface */
    csp_iflist_add(&data->iface);

    if (ifc)
        *ifc = &data->iface;

    return CSP_ERR_NONE;
}
#endif

#ifdef LINUX_TEMP_PORT
struct csp_sltcp_ifdata sltcp_data_uhf;
static void csp_sltcp_rx_uhf(void *arg);

static void csp_sltcp_rx_uhf(void *arg)
{
    int32_t maxsd=0,sd = 0;
    int32_t i, socketcount ;
    int dest_addr_len ;
    struct csp_sltcp_ifdata *data = &sltcp_data_uhf;
    int32_t clientsocket[10] = {0};

    fd_set readfds;
    FD_ZERO(&readfds);
    int nbytes;
    csp_packet_t *packet;

    uint8_t frame[CSP_IF_SLTCP_UHF_MTU];

    // Accept a new client connection
    dest_addr_len = sizeof(data->dest_addr);

    while(1)
    {
        memset(frame, 0,sizeof(frame));
        FD_ZERO(&readfds);
        FD_SET( data->server, &readfds);

        maxsd = data->server;

        for(i=0;i<10;i++)
        {
            sd = clientsocket[i];
            if(sd>0)
                FD_SET(clientsocket[i],&readfds);
            if(sd>maxsd)
            {
                maxsd = sd;
            }
        }

        socketcount = select( maxsd+1, &readfds, NULL, NULL, NULL );

        if(socketcount<1)
        {
#ifdef COMMS_SBAND_SIM_DGB_PRNT
            DEBUG_CPRINT(("UHF_SIM: [-]Error in select"));
#endif
        }

        if(FD_ISSET(data->server, &readfds))
        {
            if((data->dest_socket = accept(data->server, (struct sockaddr *)&data->dest_addr, (socklen_t *)&dest_addr_len)) < 0) {
                DEBUG_CPRINT(("[SLTCP - UHF] Error in accepting TCTM Socket.\n"));
            }
            else
            {
                DEBUG_CPRINT(("[SLTCP - UHF] Accepted new connection from %s:%d\n", inet_ntoa(data->dest_addr.sin_addr), (data->dest_addr.sin_port)));
            }

            for(i=0;i<10;i++)
            {
                if(clientsocket[i]==0)
                {
                	clientsocket[i]=data->dest_socket;
                	break;
                }
            }
        }
        else
        {
            for(i=0;i<10;i++)
            {
                sd = clientsocket[i];
                if(FD_ISSET(sd, &readfds))
                {
                    nbytes = recv(sd,frame,sizeof(frame),MSG_DONTWAIT);
                    if(nbytes==0 )
                    {
                        close(sd);
                        clientsocket[i]=0;
                    }
                    else if(nbytes>=0)
                    {
                        if (nbytes < 0 || (unsigned int)nbytes < sizeof(frame))
                            continue;

                        /* Allocate buffer for frame */
                        packet = csp_buffer_get(CSP_IF_SLTCP_UHF_MTU);

                        uhf_unpack_uart_csp_frame(packet, frame);

                        /* Pass frame to CSP stack */
                        csp_qfifo_write(packet, &data->iface, NULL);
                    }
                }
            }
        }
    }
    close(data->server);
}

static int csp_sltcp_tx_uhf(const csp_route_t *route, csp_packet_t *packet)
{
    int nbytes;
    uint8_t frame[CSP_IF_SLTCP_UHF_MTU];
    struct csp_sltcp_ifdata *data = (struct csp_sltcp_ifdata*)route->iface->driver_data;
    struct sockaddr_in dest = data->dest_addr;

    memset(frame,0,sizeof(frame));

    uhf_pack_uart_csp_frame(packet, frame);

    /* Send message */
    nbytes = sendto(data->dest_socket, frame, sizeof(frame), 0,(struct sockaddr *) &dest, sizeof(dest));

    if (nbytes != (int)sizeof(frame))
    {
        csp_tm_global.csp_err_timedout++;
        return CSP_ERR_TIMEDOUT;
    }

    csp_buffer_free(packet);

    return CSP_ERR_NONE;
}

int csp_sltcp_init_uhf(const char *device, const char *ifname, csp_iface_t **ifc)
{
    int ret ;
    struct sockaddr_in sa;

#ifndef LINUX_TEMP_PORT
    ip4_addr_t eth_ipaddr;
#endif

    /* Allocate interface data */
    struct csp_sltcp_ifdata *data = &sltcp_data_uhf;
    if (!data)
        return CSP_ERR_NOMEM;

    if (!ifname)
        ifname = CSP_IF_SLTCP_DEFAULT_NAME_UHF;

    strncpy(data->ifname, ifname, sizeof(data->ifname) - 1);
    data->iface.name = data->ifname;
    data->iface.driver_data = data;
    data->iface.nexthop = csp_sltcp_tx_uhf;
    data->iface.mtu = CSP_IF_SLTCP_UHF_MTU + 20;

    /* Create socket */
    data->server = socket(AF_INET, SOCK_STREAM, 0);
    if (data->server < 0) {
        csp_log_error("failed to allocate TCP socket\n");
        csp_tm_global.csp_err_driver ++ ;
        return CSP_ERR_DRIVER;
    }

#ifdef LINUX_TEMP_PORT
    DEBUG_CPRINT(("\n\r[SLTCP - UHF] OBC TC TM socket address is %s %u \n",obc_uhf_soc_inf.ip_addr,(obc_uhf_soc_inf.port)));
    sl_tcp_port = (obc_uhf_soc_inf.port);
    sl_tcp_addr = inet_addr(obc_uhf_soc_inf.ip_addr);
#else

#ifdef GSOBC_USB_EN
    IP4_ADDR(&eth_ipaddr, USB_IP_ADDR0, USB_IP_ADDR1, USB_IP_ADDR2, USB_IP_ADDR3);
#else
    IP4_ADDR(&eth_ipaddr, ETH_IP_ADDR0, ETH_IP_ADDR1, ETH_IP_ADDR2, ETH_IP_ADDR3);
#endif
    sl_tcp_port  = GS_SRVR_PORT_UHF;
    sl_tcp_addr  = eth_ipaddr.addr;
#endif

    /* Bind to incoming port on interface */
    memset(&sa, 0, sizeof(struct sockaddr_in));
    sa.sin_family = AF_INET;
    sa.sin_addr.s_addr = sl_tcp_addr;
    sa.sin_port = htons(sl_tcp_port);

    ret = bind(data->server, (const struct sockaddr *)&sa, sizeof(struct sockaddr_in));
    if (ret < 0) {
        csp_log_error("[SLTCP-UHF] failed to bind receive socket\n");
        csp_tm_global.csp_err_driver ++ ;
        return CSP_ERR_DRIVER;
    }

    // Listen for incoming connections
    if (listen(data->server, 10) == -1) {
        perror("listen");
    }

    DEBUG_CPRINT(("[SLTCP-UHF] Server listening on port %d\n", sa.sin_port));

    /* Create destination address */
    memset(&data->dest_addr, 0, sizeof(data->dest_addr));
    data->dest_addr.sin_family = AF_INET;
    data->dest_addr.sin_addr.s_addr = sl_tcp_addr;
    data->dest_addr.sin_port = sl_tcp_port;

    /* Start receiver thread */
    ret = os_thread_create(CSP_ETH_RX, T_STACK_5K, P_CSP_INTF_ETH_RX,"CSP UHF Socket RX",csp_sltcp_rx_uhf, data,NULL,NULL);

    if (ret < 0) {
        csp_log_error("[SLTCP-UHF] failed to start receive thread\n");
        csp_tm_global.csp_err_driver ++ ;
        return CSP_ERR_DRIVER;
    }

    /* Register interface */
    csp_iflist_add(&data->iface);

    if (ifc)
        *ifc = &data->iface;

    return CSP_ERR_NONE;
}
#endif
