/**
 * @file csp_comms.c
 *
 * @brief #TODO
 *
 * @copyright Copyright 2023 Antaris, Inc.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
 */
/*********************************************************************/
#include "csp/interfaces/csp_if.h"
#include "csp/arch/csp_time.h"
#include "errno.h"
#include "exo_common.h"
#include <csp/csp.h>
#include <csp/csp_comms.h>
#include <csp/csp_endian.h>
#include "comms_uhf_csp.h"
#include "comms_uhf_main.h"
#include "comms_sband_csp.h"
#include "comms_sband_sim.h"
#include "sdr_app_main.h"
#include "comms_sband_xlink_sock.h"
#include "exo_io_al_sos_timer.h"
#include "comms_sband_xlink_api.h"
#include "exo_tctm_ipc.h"
#include "comms_ipc.h"

#define SDR_CSP_DBG 1
#define DEFAULT_TIMEOUT 1000

os_sem_handle_ptr tx_win_on_sem;

/** Timer used for transmit ON duration */
extern os_timer_handle_ptr uhf_tx_tm_on;

extern os_timer_handle_ptr sband_tx_tm_on;

/** Timer used for enable and periodic transmission of Beacon signal */
extern os_timer_handle_ptr sband_beacon_enb_timer;

/** Timer used for enable and periodic transmission of Beacon signal */
extern os_timer_handle_ptr uhf_beacon_enb_timer;

extern volatile uint8_t PLACE_IN_EXT_RAM_MEM sband_sch_gs_contact ;
extern uint8_t PLACE_IN_EXT_RAM_MEM uhf_sch_gs_contact ;
extern s_sdr_tmr_cfg PLACE_IN_EXT_RAM_MEM sband_tmr_cfg;
extern s_sdr_tmr_cfg PLACE_IN_EXT_RAM_MEM uhf_tmr_cfg;

#ifdef S_BAND_SIM_CB
extern s_comms_rx_cfg PLACE_IN_EXT_RAM_MEM rx_cfg_sim;
#endif

#ifdef COREBOARD
extern ioal_can_hdle ioal_hcan2;
#else
extern ioal_can_hdle ioal_hcan3;
#endif
extern ioal_uart_hdle ioal_huart6;
extern ioal_uart_hdle ioal_huart4;
extern s_comms_xlinks_tx_cfg PLACE_IN_EXT_RAM_MEM iqs_tx_cfg_global;
extern os_timer_handle_ptr sband_tx_win_on;
extern sys_time_s PLACE_IN_EXT_RAM_MEM get_t_iqs;

volatile uint8_t csp_setup_done =0;

uint8_t comms_radio_id = 0;
uint8_t sband_iqs_tx_ON = 0;

extern exo_sw_dbg_cb exo_sw_dbg;
/**
 * @brief Thread that handles the received IPC and transmits the TM
 * to the TC originator through socket interface.
 *
 * @param[in] ptr   : Unused pointer.
 *
 * @return Doesn't return
 * @retval Doesn't return.
 */

static void comms_uhf_tx_thread(void* ptr);
static void comms_sband_tx_thread(void* ptr);

static void comms_rx_thread(void* ptr);
uint8_t db_csp_comms = 0;

/**
 * @brief This function is used for configuring CSP
 */
void CSP_Setup(void)
{
    if(csp_setup_done != 1)
    {
        os_status_t status;
        uint8_t ioal_eth_csp = 0 ;
        ioal_eth_csp = ioal_eth_csp;
        csp_conf_t *csp_conf1 = os_malloc(sizeof(csp_conf_t));

        /* Add interface(s) */
#ifdef BACKDOOR_SOCK_ENB
        csp_iface_t *eth_iface_gs2 = os_malloc(sizeof(csp_iface_t));
#endif
        csp_iface_t *can_iface = os_malloc(sizeof(csp_iface_t));
#if !defined(STM32F769xx) || defined(COMMS_UART6_ENB)
        csp_iface_t *uart_iface = os_malloc(sizeof(csp_iface_t));
#endif

        csp_conf_get_defaults(csp_conf1);

        /* Init CSP */
        csp_conf1->buffer_data_size = 1450*8;
        csp_init(csp_conf1);

        /* Start router */
        int res = csp_route_start_task(384, P_CSP_ROUTE);

#if SDR_CSP_DBG
        if (res != CSP_ERR_NONE) {
            DEBUG_CPRINT(("csp_route_start_task() %d", res));
        }
#endif

#ifdef COREBOARD
        res = csp_intf_init(CAN,&ioal_hcan2, &can_iface);
        res = csp_intf_init(UART,&ioal_huart4, &uart_iface);
#else
        res = csp_intf_init(CAN,&ioal_hcan3, &can_iface);
#if !defined(STM32F769xx) || defined(COMMS_UART6_ENB)
        res = csp_intf_init(UART,&ioal_huart6, &uart_iface);
#endif
#endif
        /** UHF UART interface initialisation */
        DEBUG_CPRINT(("\nUART Init :%d",res));

#ifdef BACKDOOR_SOCK_ENB
        ioal_eth_csp = S2_DEFAULT_SBAND_GS2 ;
        res = csp_intf_init(ETHERNET,&ioal_eth_csp, &eth_iface_gs2);
        DEBUG_CPRINT(("\nGS2 Init :%d",res));


        csp_rtable_set(csp_conf1->address, 0, eth_iface_gs2, CSP_NO_VIA_ADDRESS);
        csp_route_set(CSP_DEFAULT_ROUTE,eth_iface_gs2 , CSP_NO_VIA_ADDRESS);
#endif

#ifdef LINUX_TEMP_PORT

#ifndef S_BAND_SIM_CB

#ifdef SDR_HWIL
        csp_iface_t *eth_iface_gs1 = os_malloc(sizeof(csp_iface_t));

        ioal_eth_csp = S1_SBAND_HWIL_GS1 ;
        res = csp_intf_init(ETHERNET,&ioal_eth_csp, &eth_iface_gs1);
        DEBUG_CPRINT(("\nGS1 Init :%d",res));

        csp_rtable_set(SRS4_CAN_TX_ADDRESS, 5, eth_iface_gs1, CSP_NO_VIA_ADDRESS);
        csp_route_set(SRS4_CAN_TX_ADDRESS, eth_iface_gs1 , CSP_NO_VIA_ADDRESS);

        csp_rtable_set(SRS4_CTRL_ADDRESS, 5, eth_iface_gs1, CSP_NO_VIA_ADDRESS);
        csp_route_set(SRS4_CTRL_ADDRESS , eth_iface_gs1 , CSP_NO_VIA_ADDRESS);
#endif

#else
        csp_rtable_set(SRS4_CAN_TX_ADDRESS, 5, eth_iface_gs2, CSP_NO_VIA_ADDRESS);
        csp_route_set(SRS4_CAN_TX_ADDRESS, eth_iface_gs2 , CSP_NO_VIA_ADDRESS);

        csp_rtable_set(SRS4_CTRL_ADDRESS, 5, eth_iface_gs2, CSP_NO_VIA_ADDRESS);
        csp_route_set(SRS4_CTRL_ADDRESS, eth_iface_gs2 , CSP_NO_VIA_ADDRESS);
#endif
#ifdef BACKDOOR_SOCK_ENB
        csp_rtable_set(SRS4_ETH_TX_ADDRESS, 5, eth_iface_gs2, CSP_NO_VIA_ADDRESS);
        csp_route_set(SRS4_ETH_TX_ADDRESS, eth_iface_gs2 , CSP_NO_VIA_ADDRESS);
#endif
        csp_iface_t *eth_iface_uhf = os_malloc(sizeof(csp_iface_t));

        ioal_eth_csp = S3_UHF_HWIL_GS3 ;
        res = csp_intf_init(ETHERNET,&ioal_eth_csp, &eth_iface_uhf);
        DEBUG_CPRINT(("\nUHF Init :%d",res));
        csp_rtable_set(UHF_UART_TX_ADDRESS, 5, eth_iface_uhf, CSP_NO_VIA_ADDRESS);
        csp_route_set(UHF_UART_TX_ADDRESS, eth_iface_uhf , CSP_NO_VIA_ADDRESS);

        csp_rtable_set(UHF_CMD_TX_ADDRESS , 5, eth_iface_uhf, CSP_NO_VIA_ADDRESS);
        csp_route_set(UHF_CMD_TX_ADDRESS, eth_iface_uhf , CSP_NO_VIA_ADDRESS);
#else
#ifdef BACKDOOR_SOCK_ENB
        csp_rtable_set(SRS4_ETH_TX_ADDRESS, 5, eth_iface_gs2, CSP_NO_VIA_ADDRESS);
        csp_route_set(SRS4_ETH_TX_ADDRESS, eth_iface_gs2 , CSP_NO_VIA_ADDRESS);
#endif
        csp_rtable_set(SRS4_CAN_TX_ADDRESS, 5, can_iface, CSP_NO_VIA_ADDRESS);
        csp_route_set(SRS4_CAN_TX_ADDRESS, can_iface , CSP_NO_VIA_ADDRESS);

        csp_rtable_set(SRS4_CTRL_ADDRESS, 5, can_iface, CSP_NO_VIA_ADDRESS);
        csp_route_set(SRS4_CTRL_ADDRESS, can_iface , CSP_NO_VIA_ADDRESS);
#if !defined(STM32F769xx) || defined(COMMS_UART6_ENB)
        csp_rtable_set(UHF_UART_TX_ADDRESS, 5, uart_iface, CSP_NO_VIA_ADDRESS);
        csp_route_set(UHF_UART_TX_ADDRESS, uart_iface , CSP_NO_VIA_ADDRESS);

        csp_rtable_set(UHF_CMD_TX_ADDRESS , 5, uart_iface, CSP_NO_VIA_ADDRESS);
        csp_route_set(UHF_CMD_TX_ADDRESS, uart_iface , CSP_NO_VIA_ADDRESS);
#endif
#endif

        /** Thread creation of driver TX Thread */
        status = os_thread_create(COMMS_SBAND_TX,
        		T_COMMS_SBAND_TX_STACK_SIZE,
                P_COMMS_SBAND_TX,
                "COMMS SBAND transmit thread",
                comms_sband_tx_thread,
                NULL,
                NULL,
                NULL);

        /** Thread creation of driver TX Thread */
        status = os_thread_create(COMMS_UHF_TX,
        		T_COMMS_UHF_TX_STACK_SIZE,
                P_COMMS_UHF_TX,
                "COMMS UHF transmit thread",
                comms_uhf_tx_thread,
                NULL,
                NULL,
                NULL);

        /** Thread creation of driver RX thread */
        status &= os_thread_create(COMMS_RX,
                T_COMMS_RX_STACK_SIZE,
                P_COMMS_RX,
                "COMMS recv thread-socket",
                comms_rx_thread,
                NULL,
                NULL,
                NULL);

#if SDR_CSP_DBG
        DEBUG_CPRINT(("Connection table\r\n"));
        csp_conn_print_table();

        DEBUG_CPRINT(("Interfaces\r\n"));
        csp_iflist_print();

        DEBUG_CPRINT(("Route table\r\n"));
        csp_rtable_print();
#endif
        os_free(csp_conf1);
        csp_conf1 = NULL;
        csp_setup_done = 1;
    }
}

/**
 * @brief This function is used for sending Telemetry to SBAND module
 */
int32_t comms_send_tm_sband_bc(uint8_t* tm_data, uint16_t len)
{
    csp_conn_t *conn;
    csp_packet_t *packet=NULL;
    size_t sent = 0 , datasize;
    size_t remain = 0;
    remain = remain;
    uint16_t mtu_size = 0;

    DEBUG_CPRINT(("The COMMS TM_LEN %d", len));

    uint8_t csp_tx_port = SRS4_ETH_TX_DATA_PORT;
    uint8_t csp_tx_addr = SRS4_ETH_TX_ADDRESS;

    comms_radio_id = GS2_BACK_DOOR_SOC ;
    switch(comms_radio_id)
    {
#ifdef SDR_SBAND_SRS4
        case COMMS_S_BAND:
            {
#ifdef LINUX_TEMP_PORT 
                comms_sband_sim_upd_port_addr(&csp_tx_port,&csp_tx_addr);
                mtu_size = SRS4_CAN_MTU;
#else
                comms_sband_upd_port_addr(&csp_tx_port,&csp_tx_addr);
                mtu_size = SRS4_CAN_MTU;
#endif
            }
            break;
#endif

#ifdef BACKDOOR_SOCK_ENB
        case GS2_BACK_DOOR_SOC:
            {
                comms_sband_sim_upd_port_addr(&csp_tx_port,&csp_tx_addr);
                mtu_size = SRS4_CAN_MTU;

            }
            break;
#endif
    }

    comms_sband_sim_upd_port_addr(&csp_tx_port,&csp_tx_addr);
    mtu_size = SRS4_CAN_MTU;

    DEBUG_CPRINT(("\n csp_tx_addr: %d, csp_tx_port: %d, radio_id: %d\n",csp_tx_addr,csp_tx_port,comms_radio_id));

    conn = csp_connect(CSP_PRIO_NORM, csp_tx_addr, csp_tx_port , DEFAULT_TIMEOUT, CSP_O_NONE);
    if (!conn)
        return -ECONNREFUSED;

    remain = (len + mtu_size - 1) / mtu_size;

    if(len!=0)
        while (sent < len) {
            if (len - sent > mtu_size)
                datasize = mtu_size;
            else
                datasize = len - sent;
            packet = csp_buffer_get(datasize);
            if (!packet) {
                csp_close(conn);
                return -ENOMEM;
            }

            memcpy(packet->data, &tm_data[sent], datasize);
            packet->length = datasize;

#if SDR_CSP_DBG
          //  int i=0,j = 0;
            DEBUG_CPRINT(("data size is %d\n tm_data: ", (uint16_t)datasize));

           // for(i = sent; i<datasize; i++)
           // {
           //     DEBUG_CPRINT(("%x ",tm_data[i]));
          //  }
            DEBUG_CPRINT(("\n value of TX packet->length is %d\r\n", packet->length));
            //DEBUG_CPRINT(("\n value of TX packet->id is %x\r\n", packet->id.ext));

            //for(j = 0; j<packet->length; j++)
            //{
            //    DEBUG_CPRINT(("%x ",packet->data[j]));
           // }
    /*
                packet[0] = 0;
                packet[1] = 0xc80101;
                packet[2] = 0;
                packet[3] = 0;
                packet[4] = 0;
                packet[5] = 0;
                packet[6] = 0;*/
#endif

            if (!csp_send(conn, packet, DEFAULT_TIMEOUT)) {
                csp_buffer_free(packet);
                csp_close(conn);
                DEBUG_CPRINT(("SBAND BK packet sent fail\r\n"));
                return -EIO;
            }
#ifdef S_BAND_SIM_CB
            sdr_sim_upd_srs4_tm_prop_with_tx();
#endif
            DEBUG_CPRINT(("SBAND BK packet successfully sent \r\n"));
            sent += datasize;
        }

    csp_buffer_free(packet);
    csp_close(conn);
    return 0;
}
/**
 * @brief This function is used for sending Telemetry to SBAND module
 */
int32_t comms_send_tm_uhf(uint8_t* tm_data, uint16_t len)
{
    csp_conn_t *conn;
    csp_packet_t *packet=NULL;
    size_t sent = 0 , datasize;
    size_t remain = 0;
    remain = remain;
    uint16_t mtu_size = 0;

    uint8_t csp_tx_port = UHF_CSP_TX_DATA_PORT;
    uint8_t csp_tx_addr = UHF_UART_TX_ADDRESS;

    DEBUG_CPRINT(("The COMMS TM_LEN %d", len));

    comms_uhf_upd_port_addr(&csp_tx_port,&csp_tx_addr);
    mtu_size = UHF_UART_MTU ;

    comms_radio_id = COMMS_UHF;
    DEBUG_CPRINT(("\n csp_tx_addr: %d, csp_tx_port: %d, radio_id: %d\n",csp_tx_addr,csp_tx_port,comms_radio_id));

    conn = csp_connect(CSP_PRIO_NORM, csp_tx_addr, csp_tx_port , DEFAULT_TIMEOUT, CSP_O_NONE);
    if (!conn)
        return -ECONNREFUSED;

    remain = (len + mtu_size - 1) / mtu_size;

    if(len!=0)
        while (sent < len) {
            if (len - sent > mtu_size)
                datasize = mtu_size;
            else
                datasize = len - sent;
            packet = csp_buffer_get(datasize);
            if (!packet) {
                csp_close(conn);
                return -ENOMEM;
            }

            memcpy(packet->data, &tm_data[sent], datasize);
            packet->length = datasize;

#if SDR_CSP_DBG
//            int i=0,j = 0;
            DEBUG_CPRINT(("data size is %d\n tm_data: ", (uint16_t)datasize));

//            for(i = sent; i<datasize; i++)
//            {
//                DEBUG_CPRINT(("%x ",tm_data[i]));
//            }
            DEBUG_CPRINT(("\n value of TX packet->length is %d\r\n", packet->length));
            //DEBUG_CPRINT(("\n value of TX packet->id is %x\r\n", packet->id.ext));

//            for(j = 0; j<packet->length; j++)
//            {
//                DEBUG_CPRINT(("%x ",packet->data[j]));
//            }
#endif

            if (!csp_send(conn, packet, DEFAULT_TIMEOUT)) {
                csp_buffer_free(packet);
                csp_close(conn);
                return -EIO;
            }
            DEBUG_CPRINT(("packet successfully sent \r\n"));
            sent += datasize;
        }

    csp_buffer_free(packet);
    csp_close(conn);
    return 0;
}

/**
 * @brief This function is used for sending Telemetry to SBAND module
 */
int32_t comms_send_tm_sband_xlink(uint8_t* tm_data, uint16_t len)
{
    if(NULL != tm_data)
    {
        DEBUG_CPRINT(("The COMMS TM_LEN %d", len));

        if(PWR_ON == sband_get_state())
        {
            if(!sband_iqs_tx_ON)
            {
                get_rtc_time(&get_t_iqs);
                DEBUG_CPRINT(("\nIQS RTC Time-%d:%d:%d",get_t_iqs.hour, get_t_iqs.min, get_t_iqs.sec));
                sband_send_ipc(SBAND_TX_WIN_ON_REQ, COMMS_SBAND_CTLR);
                DEBUG_CPRINT(("\n\rIQS TX_WIN_ON: %d",iqs_tx_cfg_global.tx_idletime));
                os_timer_start(sband_tx_win_on,(iqs_tx_cfg_global.tx_idletime*1000)); //(iqs_tx_cfg_global.tx_idletime)
                os_sem_take(tx_win_on_sem,os_wait_forever);
                os_delay(100);
            }
            else
            {
                os_timer_start(sband_tx_win_on,(iqs_tx_cfg_global.tx_idletime*1000));
            }

            sband_xlink_tx(tm_data,len,1);
        }
        else
        {
            DEBUG_CPRINT(("\nS-Band module is turned OFF \n"));
        }
    }
    else
    {
        DEBUG_CPRINT(("COMMS TX malloc failed\n"));
    }
    return 0;
}

/**
 * @brief This function is used for receiving Telecommand from S-BAND module
 */
int32_t comms_recv_tc(void)
{
    os_itc_msg_handle_t trans_msg;
    csp_conn_t *conn;
    csp_packet_t *packet;

    csp_socket_t *sock;

    sock = csp_socket(CSP_SO_NONE);

    /* Create socket with no specific socket options, e.g. accepts CRC32, HMAC, etc. if enabled during compilation */

    /* Bind socket to all ports, e.g. all incoming connections will be handled here */
    csp_bind(sock, CSP_ANY);

    /* Create a backlog of 10 connections, i.e. up to 10 new connections can be queued */
    csp_listen(sock, 10);

    while (1)
    {

        if ((conn = csp_accept(sock, 10)) == NULL) {
            /* timeout */
            os_delay(10);
        }

        while ((packet = csp_read(conn, 1)) != NULL)
        {
            /*Control variable - Increments on every packet received in csp comms*/
            exo_sw_dbg.comms_dbg.csp_comms_rcvd++;
            int j = 0;
            switch (csp_conn_dport(conn))
            {
#ifdef SDR_SBAND_SRS4
                case SRS4_CAN_RX_DATA_PORT :
                    DEBUG_CPRINT(("Packet received in port: %d",csp_conn_dport(conn)));

#ifdef S_BAND_SIM_CB
                    rx_cfg_sim.rx_cfg_frames++;
                    rx_cfg_sim.rx_cfg_carrier_lock       = 1;
                    rx_cfg_sim.rx_frame_lock             = 1;
#endif

                    trans_msg.src_entity= COMMS_RX;
                    trans_msg.pld.pld_ptr = (uint8_t *)os_malloc(packet->length);
                    os_memcpy(trans_msg.pld.pld_ptr,packet->data, packet->length);
                    trans_msg.Msg_len = packet->length;

                    os_itc_msg_send(&trans_msg,
                            TC_TM_MANAGER_IPC_RECEIVE,
                            os_send_itc_wait);

                    if(!sband_sch_gs_contact)
                    {
                        os_timer_start(sband_tx_tm_on, sband_tmr_cfg.sdr_tm_tx_tmr);
                        sband_gs_contact_status_upd_itc(SBAND_TC_TM_ST_REQ,DEFAULT_COMMS_RADIO_ID);
                    }
                    csp_buffer_free(packet);
                    break;
#endif

                case SRS4_ETH_RX_DATA_PORT:
                    DEBUG_CPRINT(("Packet received in port: %d",csp_conn_dport(conn)));

#ifdef S_BAND_SIM_CB
                    rx_cfg_sim.rx_cfg_frames++;
                    rx_cfg_sim.rx_cfg_carrier_lock       = 1;
                    rx_cfg_sim.rx_frame_lock             = 1;
#endif

                    trans_msg.src_entity= COMMS_RX;
                    trans_msg.pld.pld_ptr = (uint8_t *)os_malloc(packet->length);
                    os_memcpy(trans_msg.pld.pld_ptr,packet->data, packet->length);
                    trans_msg.Msg_len = packet->length;

#if SDR_CSP_DBG
                    DEBUG_CPRINT(("\n value of RX packet->length is %d\r\n", packet->length));
                    //DEBUG_CPRINT(("\n value of RX packet->id is %x\r\n", packet->id.ext));

                    for(j = 0; j<packet->length; j++)
                    {
                        DEBUG_CPRINT(("%x ",packet->data[j]));
                    }
#endif
                    os_itc_msg_send(&trans_msg,
                            TC_TM_MANAGER_IPC_RECEIVE,
                            os_send_itc_wait);

                    DEBUG_CPRINT(("\nsband_sch_gs_contact %d", sband_sch_gs_contact));

                    if((!sband_sch_gs_contact)&&(!uhf_sch_gs_contact))
                    {
                        sdr_gs_contact_status_upd_itc();
                        os_timer_start(uhf_tx_tm_on,uhf_tmr_cfg.sdr_tm_tx_tmr);
                        os_timer_start(sband_tx_tm_on,sband_tmr_cfg.sdr_tm_tx_tmr);
                        DEBUG_CPRINT(("\nsband_tx_tm_on timer at csp comms %d", sband_tmr_cfg.sdr_tm_tx_tmr));
                    }

                    os_timer_start(sband_beacon_enb_timer,sband_tmr_cfg.beacon_enb_tmr);

                    csp_buffer_free(packet);

                    break;

                case UHF_CSP_RX_DATA_PORT:
                    DEBUG_CPRINT(("Packet received in port: %d",csp_conn_dport(conn)));

                    trans_msg.src_entity= COMMS_RX;
                    trans_msg.pld.pld_ptr = (uint8_t *)os_malloc(packet->length);
                    os_memcpy(trans_msg.pld.pld_ptr,packet->data, packet->length);
                    trans_msg.Msg_len = packet->length;

#if SDR_CSP_DBG
                    DEBUG_CPRINT(("\n value of RX packet->length is %d\r\n", packet->length));
                    //DEBUG_CPRINT(("\n value of RX packet->id is %x\r\n", packet->id.ext));

                    for(j = 0; j<packet->length; j++)
                    {
                        DEBUG_CPRINT(("%x ",packet->data[j]));
                    }/*
                        packet[0] = 0;
                        packet[1] = 0xc80101;
                        packet[2] = 0;
                        packet[3] = 0;
                        packet[4] = 0;
                        packet[5] = 0;
                        packet[6] = 0;*/
#endif
                    os_itc_msg_send(&trans_msg,
                            TC_TM_MANAGER_IPC_RECEIVE,
                            os_send_itc_wait);

                    if((!sband_sch_gs_contact)&&(!uhf_sch_gs_contact))
                    {
                        sdr_gs_contact_status_upd_itc();
                        os_timer_start(uhf_tx_tm_on,uhf_tmr_cfg.sdr_tm_tx_tmr);
                        os_timer_start(sband_tx_tm_on,sband_tmr_cfg.sdr_tm_tx_tmr);
                        DEBUG_CPRINT(("UHF_tx_tm_on timer at csp comms %d", uhf_tmr_cfg.sdr_tm_tx_tmr));
                    }

                    os_timer_start(uhf_beacon_enb_timer,uhf_tmr_cfg.beacon_enb_tmr);

                    csp_buffer_free(packet);

                    break;

                default:
                    /* Call the default CSP service handler, handle pings, buffer use, etc. */
                    csp_service_handler(conn,packet);
                    break;
            }
        }

        /* Close current connection */
        csp_close(conn);
    }
    return 0;
}

/**
 * @brief Thread that handles the received IPC and transmits the TM
 * to the TC originator through socket interface.
 */

static void comms_uhf_tx_thread(void* ptr)
{
    int32_t tm_size = 0;

    os_itc_msg_handle_t recv_msg;

    while(1)
    {
        /** IPC receive of sending TM to S-Band module */
        os_itc_msg_rcv(COMMS_UHF_TX, &recv_msg, os_wait_forever);

        DEBUG_CPRINT(("\n COMMS UHF TX THRD IQS RTC Time-%d:%d:%d",get_t_iqs.hour, get_t_iqs.min, get_t_iqs.sec));

        tm_size = recv_msg.Msg_len;

        comms_radio_id = recv_msg.pld_info;

        DEBUG_CPRINT(("\n Radio_ID IPC: Radio_id:%d ,tm_len:%d ,src_entity:%d \n",comms_radio_id, recv_msg.Msg_len, recv_msg.src_entity));

        /** Sending the TM S-Band module */
        if (tm_size>0)
        {
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wformat"
            DEBUG_CPRINT(("comms_tm_len: %ld",tm_size));
#pragma GCC diagnostic pop
#ifndef LINUX_TEMP_PORT
            comms_send_tm_uhf(recv_msg.pld.pld_ptr, tm_size);
#endif

#ifdef BACKDOOR_SOCK_ENB
            comms_radio_id = GS2_BACK_DOOR_SOC;
            comms_send_tm_sband_bc(recv_msg.pld.pld_ptr, tm_size);
#endif
        }

        /** Freeing of payload pointer */
        if(recv_msg.pld.pld_ptr)
        {
            os_free(recv_msg.pld.pld_ptr);
            recv_msg.pld.pld_ptr = NULL;
        }
    }
}

/**
 * @brief Thread that handles the received IPC and transmits the TM
 * to the TC originator through socket interface.
 */

static void comms_sband_tx_thread(void* ptr)
{
    int32_t tm_size = 0;

    os_itc_msg_handle_t recv_msg;
    os_sem_create_bin(&tx_win_on_sem, "COMMS_TX_SEM");
    os_sem_give(tx_win_on_sem);


    while(1)
    {
        /** IPC receive of sending TM to S-Band module */
        os_itc_msg_rcv(COMMS_SBAND_TX, &recv_msg, os_wait_forever);

        tm_size = recv_msg.Msg_len;

        comms_radio_id = recv_msg.pld_info;

        DEBUG_CPRINT(("\n Radio_ID IPC: Radio_id:%d ,tm_len:%d ,src_entity:%d \n",comms_radio_id, recv_msg.Msg_len, recv_msg.src_entity));

        /** Sending the TM S-Band module */
        if (tm_size>0)
        {
            /*Control variable - Increments on every packet sent in csp comms*/
            exo_sw_dbg.comms_dbg.csp_comms_sent++;
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wformat"
            DEBUG_CPRINT(("comms_tm_len: %ld",tm_size));
#pragma GCC diagnostic pop
            if(comms_radio_id == COMMS_SBAND_IQS)
            { 
                comms_send_tm_sband_xlink(recv_msg.pld.pld_ptr, tm_size);
            }

#ifdef BACKDOOR_SOCK_ENB
            comms_radio_id = GS2_BACK_DOOR_SOC;
            comms_send_tm_sband_bc(recv_msg.pld.pld_ptr, tm_size);
#endif
        }
        /** Freeing of payload pointer */
        if(recv_msg.pld.pld_ptr)
        {
            os_free(recv_msg.pld.pld_ptr);
            recv_msg.pld.pld_ptr = NULL;
        }
    }
}

/**
 * @brief This thread receives the tc data through the socket interface
 * and handles the received data through handle_tc_data()
 */
static void comms_rx_thread(void* ptr)
{
    comms_recv_tc();
}



void comms_back_door_data_receive(uint8_t *data, uint16_t length)
{
    os_itc_msg_handle_t trans_msg;
    trans_msg.src_entity= COMMS_RX;
    trans_msg.pld.pld_ptr = (uint8_t *)os_malloc(length);
    os_memcpy(trans_msg.pld.pld_ptr,data, length);
    trans_msg.Msg_len = length;

    os_itc_msg_send(&trans_msg,
            TC_TM_MANAGER_IPC_RECEIVE,
            os_send_itc_wait);
#ifdef LINUX_TEMP_PORT
    sleep(0.3);
#endif

    if((!sband_sch_gs_contact)&&(!uhf_sch_gs_contact))
    {
        sdr_gs_contact_status_upd_itc();
        //backdoor_soc_gs_contact_status_upd_itc(SBAND_TC_TM_ST_REQ);
        os_timer_start(uhf_tx_tm_on,uhf_tmr_cfg.sdr_tm_tx_tmr);
        os_timer_start(sband_tx_tm_on,sband_tmr_cfg.sdr_tm_tx_tmr);
        DEBUG_CPRINT(("SDR_tx_tm_on timer at backdoor %d", uhf_tmr_cfg.sdr_tm_tx_tmr));
    }
                        
}

