/**
 * @file tc_tm_mngr.c
 *
 * @brief This file contains functions that provides demonstration of
 *        usage of TC TM Library APIs
 *
 * The functions provided in the file provides demonstration on
 * usage of TC TM library APIs
 *
 * @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.
 *        usage of TC TM Library APIs
 */
/*********************************************************************/

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include "tc_common.h"
#include "tc_tm_lib_common.h"
#include "tm_common.h"
#include "exo_osal.h"
#include "exo_common.h"
#include "exo_ral_cmn.h"
#include "tc_tm_mngr_crypt.h"
#include "adcs_csw.h"
#include "tc_tm_mngr.h"
#include "tc_tm_mngr_buffer.h"
#include "exo_osal_itc.h"
#include "logger.h"
#include "exo_tctm_ipc.h"
#include "comms_uhf_main.h"

#include "obc_tc_tm.h"
#ifndef LINUX_TEMP_PORT
#include "obc_main.h"
#endif
#include "exo_io_al_sos_timer.h"
#include "comms_csw.h"
#include "nvm.h"
#include "obc_main_fsm_hdlr.h"
#include "schedular.h"
#include "sdr_app_main.h"
#include "obc_ipc.h"

///TM storage constants
#define TM_HEADER_OVERHEAD         (128)
#define MAX_TM_CONT_CNT            (MAX_TM_STORAGE_DEPTH-1)
#define MAX_TM_DROP_CNT_FOR_NEW_TM (5)
#define TM_MAX_BUFF_SIZE           (2048)

/**
 * @brief Global variables for thread and IPC operations
 */
os_itc_msg_handle_t tc_trans_msg;
os_itc_msg_handle_t tc_recv_msg;
os_itc_msg_handle_t tm_recv_msg;
os_itc_msg_handle_t tm_trans_msg;
e_tc_tm_mngr_sts tc_tm_mngr_init = 0;
os_queue_handle_ptr tm_queue;
uint32_t tm_mem_fail_drop_cnt=0;
uint32_t tm_sent_cnt=0;
volatile uint8* residue_pac=NULL;
s_radio_module radio_mod[MAX_RADIO_MOD];
os_itc_msg_handle_t tm_store_local_msg;
os_itc_msg_handle_t tm_store_local_q_send_msg;
/**
 * @brief Global variables for debugging control block
 */

extern exo_sw_dbg_cb exo_sw_dbg;

static s_tctm_rm_overrule radio_id_overrule;
/**
 * @brief Global variables for TC parser expiry timer
 */
os_timer_handle_ptr tc_rx_dec_timeout_tmr;
os_sem_handle_ptr rx_buff_sem;
#define  MAX_RX_DEC_WAIT_TIME_MS (30*1000)
os_timer_handle_ptr tc_rx_dec_timeout_tmr_per;

/**
 * @brief Global arrays for storing tc/tm data and cmac_hmac
 */
uint8 cmac_hmac_tc[SHA256_HASH_SIZE] = {0};
uint8 cmac_hmac_tm[SHA256_HASH_SIZE] = {0};
#define TC_TM_RX_TX_BUFF_MAX_SIZE (2*1024)
uint8 PLACE_IN_EXT_RAM_MEM tc_rx_data[TC_TM_RX_TX_BUFF_MAX_SIZE] = {0};
uint8 PLACE_IN_EXT_RAM_MEM tm_tx_data[TC_TM_RX_TX_BUFF_MAX_SIZE] = {0};
static uint16 tc_tm_seq_no = 1;


typedef struct _icm_tc_pld_info
{
    uint16 tc_id;
    uint8 src_id;
    uint8 dst_id;
    uint8 *tc_pld_buff;
    uint16 tc_pld_len;
}icm_tc_pld_info_s;

//icm_tc_pld_info_s tc_pld_info;

uint8 *ps_icd_encoded_tx_buffer;
uint16 ps_icd_encoded_tx_buffer_len;
icm_tc_pld_info_s icd_tc_pld_info;



/**
 * @brief Extern variables for security and authentication status
 */

extern volatile uint8_t security_sts;
extern volatile uint8_t authentication_sts;
extern volatile uint8_t def_radio_id;
extern uint8 tc_storage[TC_DEFERRED_STORAGE_LENGTH]PLACE_IN_EXT_RAM_MEM;

extern int8 obc_ps_ftp_msg_cmd_enc (uint8 **data, uint16* len, icm_tc_pld_info_s *tc_pld_info);
extern int8 obc_ps_tc_tm_msg_cmd_enc (uint8 **data, uint16* len, \
        icm_tc_pld_info_s *tc_pld_info);
extern void obc_icd_socket_send (uint8 *data, uint16 data_len);

/**
 * @brief Thread that handles the received IPC and transmits the TC
 * to the destination application through IPC interface.
 *
 * @param[in] ptr   : Unused pointer.
 *
 * @return Doesn't return
 * @retval Doesn't return.
 */
static void tc_recv_thread_ipc(void* ptr);

/**
 * @brief This API packs the tm data and computes the tm size
 *
 * @param[in]  rx_data  : Pointer to received data.
 * @param[out] tx_data  : Pointer to data packed with tc tm headers
 *
 * @return status of the pack api
 * @retval greater than zero -> Success / less than zero -> failure.
 */
int32 handle_tm_data(os_itc_msg_handle_t* rx_data, uint8* tx_data);

/**
 * @brief This API packs the tm data and computes the tm size
 *
 * @param[in] encrp_data   : Pointer to encrypted data.
 * @param[out] decrp_data  : Pointer to decrypted data.
 * @param[in] size         : Size of encrypted data.
 *
 * @return status of the decryption
 * @retval zero -> Success / less than zero -> failure.
 */
static e_tc_tm_mngr_sts handle_decryption(uint8* encrp_data, uint8* decrp_data
        ,uint16 size);

/**
 * @brief This API configures the encryption parameters
 *
 * @param[in] tm_frame   : Pointer to s_tm_frame.
 *
 * @return status of the configuration of encryption parameters
 * @retval zero -> Success / less than zero -> failure.
 */
static e_tc_tm_mngr_sts handle_encryption(s_tm_frame* tm_frame);

/**
 * @brief This API compares the actual sequence number with the expected
 *        sequence number and throws a warning if both doesn't match
 *
 * @param[in] seq_no_in   : Actual sequence number.
 *
 * @return status of the comparison
 * @retval TC_TM_MNGR_OK (0) -> Success / TC_TM_MNGR_ERR (-1) -> failure.
 */
static e_tc_tm_mngr_sts validate_seq_no(uint16 seq_no_in);

/**
 * @brief This function handle the telemetry storage thread
 *
 * @param[in] ptr : Unused pointer.
 *
 * @return Doesn't return
 */
static void tm_storage_thread(void* ptr);

/**
 * @brief This API sets the authetication and encoding status of telemetry
 *        based on the global flag.
 *
 * @param[in] tm_ctrl : telemetry control
 *
 * @return Modified tm_ctrl based on the global flag
 * @retval Modified tm_ctrl based on the global flag
 */
static uint8 check_auth_enc(uint8 tm_ctrl);


/**
 * @brief This API is used to process the tm message
 * 1.Send TCTM directly if GS contact is available
 * 2.If there is no GS contact store TM message in TM storage buffer
 */
void tc_tm_process_tm(os_itc_msg_handle_t tm_storage_msg_s);


/**
 * @brief This API is used to send the stored TM message on the following conditions
 * 1.Ground pass is available
 * 2.Control flag for downlink TM is set
 */
void tc_tm_send_tm_storage_buffer();


/**
 * @brief This API invokes two threads to handle transmit and
 * receive IPC messages
 */
os_status_t tc_tm_mngr_main(void)
{
    os_status_t status=ral_success;
    nvm_parameter_read(NVM_DATA,NVM_TC_TM_ENCRYP_KEY,(uint8_t*)tc_tm_mngr_key);

    get_security_sts();
    get_authentication_sts();
    get_radio_id();

    set_radio_module(COMMS_S_BAND,COMMS_SBAND_TX,is_sband_gs_contact_valid);
    //set_radio_module(COMMS_UHF,COMMS_UHF_TX,is_sband_gs_contact_valid);
    set_radio_module(COMMS_UHF,COMMS_UHF_TX,is_uhf_gs_contact_valid);
    set_radio_module(COMMS_SBAND_IQS,COMMS_SBAND_TX,is_sband_gs_contact_valid);
    set_radio_module(GS2_BACK_DOOR_SOC,COMMS_SBAND_TX,is_sband_gs_contact_valid); // used only for development and validation

    status &= os_thread_create(TC_TM_MANAGER_IPC_RECEIVE,
            T_TCTM_IPC_RECIEVE_STACK_SIZE,
            P_TC_TM_MANAGER_IPC_RECEIVE,
            "TC TM manager TC recv thread - IPC",
            tc_recv_thread_ipc,
            NULL,
            NULL,
            NULL);

    status &= os_thread_create(TM_STORAGE,
            T_TCTM_STORAGE_STACK_SIZE,
            P_TM_STORAGE,
            "TM storage thread - IPC",
            tm_storage_thread,
            NULL,
            NULL,
            NULL);

    if(ral_success == status)
    {
        tc_tm_mngr_init = TC_TM_MNGR_OK;
    }
    else
    {
        tc_tm_mngr_init = TC_TM_MNGR_ERR;
    }

    return status;
}

/**
 * @brief This function deletes the tc_tm_mngr queue and
 * release its memory
 */
os_status_t tc_tm_mngr_suspend(void)
{
    os_status_t status;
    status =  os_thread_suspend(TC_TM_MANAGER_IPC_RECEIVE);
    status &=  os_thread_suspend(TC_TM_MANAGER_IPC_SEND);
    return status;
}

/**
 * @brief This functionalities resumes the tc_tm_mngr thread
 */
os_status_t tc_tm_mngr_resume(void)
{
    os_status_t status;
    status = os_thread_resume(TC_TM_MANAGER_IPC_RECEIVE);
    status &= os_thread_resume(TC_TM_MANAGER_IPC_SEND);
    return status;
}

/**
 * @brief call back function for expiry timer
 */
void rx_dec_resudial_clean_tmr_exp(void)
{
    os_sem_take(rx_buff_sem,os_wait_forever);
    if(residue_pac)
    {
        os_free((void*)residue_pac);
        residue_pac = NULL;
    }
    os_sem_give(rx_buff_sem);
}

/**
 * @brief Thread that handles the received IPC and transmits the TC
 * to the destination application through IPC interface.
 */
static void tc_recv_thread_ipc(void* ptr)
{
    uint16 tc_size = 0;
    int32 handle_status;
    uint8* tc_frame_data;
    uint8* tc_data = NULL;
    uint32 offset = 0;
    uint32 residue_size = 0;
    uint32 rem_size = 0;

    os_sem_create_bin(&rx_buff_sem, "TCTM_RX_BUFF_SEM");
    os_sem_give(rx_buff_sem);

    // Create timer
    os_timer_create(&tc_rx_dec_timeout_tmr,
            "TC_RX_Dec_TO",
            ONESHOT,
            (ral_timer_cbfunc_t)rx_dec_resudial_clean_tmr_exp,
            NULL);
    while(1)
    {
        os_itc_msg_rcv(TC_TM_MANAGER_IPC_RECEIVE,
                &tc_recv_msg,
                os_wait_forever);

            DEBUG_CPRINT(("TC_TM_MNGR: tc_process_buff_fw_ipc IPC received  \n"));
            if(OBC_MAIN == tc_recv_msg.src_entity)
            {

                if(OBC_TC_TM_INIT_REQ == tc_recv_msg.Msg_id)
                {
                    //Request from OBC

                    tc_trans_msg.Msg_id = TC_TM_OBC_INIT_RSP;
                    tc_trans_msg.pld.pld_ptr = NULL;
                }
                else if(OBC_TC_TM_DEINIT_REQ == tc_recv_msg.Msg_id)
                {
                    tc_trans_msg.Msg_id = TC_TM_OBC_DEINIT_RSP;
                    tc_trans_msg.pld.pld_ptr = NULL;
                }
                else
                {
                }
            }
            /** The message is from scheduler */
            else if(TC_SCH_LUT_TC_TRIG == tc_recv_msg.Msg_id)
            {
                /** -  This is special handling for scheduler based TC trigger */
              //  if(TC_SCH_LUT_TC_TRIG == tc_recv_msg.Msg_id)
                {
                    if(tc_recv_msg.pld.pld_ptr != NULL)
                    {
                        /**  - Process the data received from scheduler */
                        tc_process_sched_msg(tc_recv_msg.pld.pld_ptr);
                    }
                }
            }
            else
            {

                if(NULL != tc_recv_msg.pld.pld_ptr && tc_recv_msg.Msg_len < (TC_MIN_LEN + TC_MAX_LEN))
                {
                    /*Control variable - Increments on every IPC receive*/
                    //exo_sw_dbg.tc_tm_dbg.tm_manager_ipc_rcvd++;
                    tc_frame_data = (uint8*)tc_recv_msg.pld.pld_ptr;
                    tc_size = tc_recv_msg.Msg_len;
                    rem_size = tc_size;
                    offset = 0;
                    os_sem_take(rx_buff_sem,os_wait_forever);

                    //Check if the previous transmission had any residual packets
                    if(residue_pac)
                    {
                        os_timer_stop(tc_rx_dec_timeout_tmr);
                        if((tc_frame_data[0] != TC_TM_LIB_SOF1) && (tc_frame_data[1] != TC_TM_LIB_SOF2))
                        {
                            rem_size +=residue_size;
                            tc_data = os_malloc(rem_size);
                            if(NULL != tc_data)
                            {
                                os_memcpy(tc_data, (void*)residue_pac,residue_size);
                                os_free((void*)residue_pac);
                                residue_pac = NULL;

                                os_memcpy(&tc_data[residue_size],tc_frame_data,tc_size);
                                residue_size = 0;
                            }
                            else
                            {
                                /**
                                 * Error handling incase garbage received from COMMS.
                                 * Setting SOF parameters as ZERO for packet drop
                                 */
                                os_free((void*)residue_pac);
                                residue_pac = NULL;
                                tc_data = os_malloc(TC_MIN_LEN + 1);
                                if(NULL == tc_data)
                                {
                                    tc_data = tc_frame_data;
                                }
                                else
                                {
                                }
                                os_memset(tc_data, 0, TC_MIN_LEN);
                                rem_size = TC_MIN_LEN + 1;
                            }
                        }
                        else
                        {
                            tc_data = tc_frame_data;
                            os_free((void*)residue_pac);
                            residue_pac = NULL;
                        }
                        os_sem_give(rx_buff_sem);
                    }
                    else
                    {
                        os_sem_give(rx_buff_sem);
                        tc_data = tc_frame_data;
                    }
                    do
                    {
                        /*
                         * This API extracts the tc_data and forwards it to the
                         * destination application based on the da_id
                         */
                        handle_status = handle_tc_data(&tc_data[offset],
                                rem_size);
                        if((handle_status>0) && (rem_size>handle_status))
                        {
                            /*
                             * Data has more than one TC frame. Parse the next TC
                             * by changing the offset. handle_status will have the
                             * size of previous TC frame
                             */
                            offset = offset + handle_status;
                            rem_size = rem_size - handle_status;
                        }
                        else if(TC_TM_FULL_PLD_NOT_PRSNT == handle_status ||
                                TC_TM_MIN_LEN_ERROR == handle_status)
                        {
                            /*
                             * Probably a segmented packet. Copy the remaining
                             * bytes and keep for next iteration or this can be a
                             * disintegrated packet too. Need to handle this logic
                             */
                            residue_size = rem_size;

                            os_sem_take(rx_buff_sem,os_wait_forever);
                            residue_pac = os_malloc(residue_size);
                            if(NULL != residue_pac)
                            {
                                os_memcpy((void*)residue_pac,
                                        &tc_data[offset],
                                        residue_size);

                                os_timer_start(tc_rx_dec_timeout_tmr,
                                        MAX_RX_DEC_WAIT_TIME_MS);
                            }
                            else
                            {
                                residue_size = 0;
                            }
                            os_sem_give(rx_buff_sem);
                            break;
                        }
                        else
                        {
                            //exo_sw_dbg.tc_tm_dbg.tc_extract_succ++;
                            break;
                        }
                    }while(rem_size != 0);

                    if((tc_frame_data != tc_data) && (tc_data))
                    {
                        os_free(tc_data);
                        tc_data = NULL;
                    }

                }
                else
                {
                    DEBUG_CPRINT(("TC_TM_MNGR : NULL TC data from COMMS or Msg length exceed maximum length :- %d \n",tc_recv_msg.Msg_len));
                    log_id(LOG_INFO_L1,LOG_TCTM,NULL_TC_DATA_FROM_COMMS);
                }
            }

        if(tc_recv_msg.pld.pld_ptr)
        {
            os_free(tc_recv_msg.pld.pld_ptr);
            tc_recv_msg.pld.pld_ptr = NULL;
        }
    }
}

/**
 * @brief This API will process the LUT structured TC and routes to the proper destination
 * based on da_id
 */
int tc_process_sched_msg(uint8 *payload)
{
    /** Variable to store TC related informations */
    s_tc_frame tc_orig;

    /** return value */
    int32 tc_tm_status = -1;

    /** IPC status */
    os_status_t ipc_status = ral_error;

    /** Typecast it to LUT structure from scheduluer */
    conops_sch_evnt_s *sched_pld = (conops_sch_evnt_s*)payload;

    /** Fill the structure to be sent to destination layer */
    tc_orig.sa_id = sched_pld->src_id;
    tc_orig.tc_id = sched_pld->id;

    /** Additional 3 Bytes for sequence number and lenght */
    tc_orig.tc_len = sched_pld->arg2 + 3;

    uint8 * tc_data = (uint8*)os_malloc_eram(tc_orig.tc_len);

    if(tc_data)
    {
        tc_data[0] = 0xFF;
        tc_data[1] = 0xFF;
#ifndef LINUX_TEMP_PORT
        if(TC_DEFERRED_STORAGE_LENGTH >= (sched_pld->arg1+sched_pld->arg2))
        {
            /*
             * Refer Conops Scheduler SW Architecture Document for
             * deferred storage frame format information
             */
            // Assign Radio Id from deferred storage
            tc_data[2] = tc_storage[sched_pld->arg1];
            // Copy Payload data
            os_memcpy(&tc_data[3], &tc_storage[sched_pld->arg1 + 1], sched_pld->arg2);
        }
#else
        // File operation variable declerations
        int itr = 0;
        FILE *fp;
        const unsigned MAX_LENGTH = 512;
        char buffer[MAX_LENGTH];
        char *token;
        uint32 file_offset = 0;
        /*
         * Refer Conops Scheduler SW Architecture Document for
         * deferred storage frame format information
         */
        // Read along with radio id
        uint8* tc_tm_data = &tc_data[2];

        // Open file for both reading and writing
        fp = fopen("tc_deffered_strng.txt", "r+");
        if(fp != NULL)
        {
            file_offset = sched_pld->arg1 * 5;
            fseek(fp, file_offset, SEEK_SET);
            fgets(buffer, MAX_LENGTH, fp);
            // get the first token
            token = strtok(buffer, ",");

            // walk through other tokens
            // 1 byte extra added for radio id
            while( (token != NULL) &&
                    (itr < sched_pld->arg2 + 1))
            {
                tc_tm_data[itr] =
                    (uint8_t)strtol(token, NULL, 0);
                token = strtok(NULL, ",");
                itr++;
            }
            // Close the file
            fclose(fp);
        }
        else
        {
            /*
             * File open failes stop further process 
             * for current schedule
             */
            DEBUG_CPRINT(("File Open failed "
                        "tc_deffered_strng.txt\n"));
        }
#endif
    }

    /** Route it to destination based on destination id */
    switch (sched_pld->dest_id)
    {
        case TC_DST_ADCS:
            tc_trans_msg.src_entity= tc_orig.sa_id;
            tc_trans_msg.Msg_id = tc_orig.tc_id;
            tc_trans_msg.Msg_len = sched_pld->arg2;
            tc_trans_msg.pld.pld_ptr = tc_data;
            ipc_status =  os_itc_msg_send(&tc_trans_msg,
                    ADCS_CONTROLLER,
                    os_send_itc_wait);

            if(ral_success != ipc_status)
            {
                tc_tm_status = ipc_status;

            }
            else
            {
                /* IPC success. Proceed and return the number
                 * of unpacked bytes
                 */
            }
            break;

        case TC_DST_COMMS_S:
            tc_trans_msg.src_entity= tc_orig.sa_id;
            tc_trans_msg.Msg_id = tc_orig.tc_id;
            tc_trans_msg.Msg_len = sched_pld->arg2;
            tc_trans_msg.pld.pld_ptr = tc_data;
            ipc_status =  os_itc_msg_send(&tc_trans_msg,
                    COMMS_SBAND_CTLR,
                    os_send_itc_wait);
            if(ral_success != ipc_status)
            {

                tc_tm_status = ipc_status;
            }
            else
            {
                /* IPC success. Proceed and return the number
                 * of unpacked bytes
                 */
            }
            break;

#ifdef SDR_XBAND_XLINK
        case TC_DST_COMMS_X:
            tc_trans_msg.src_entity= tc_orig.sa_id;
            tc_trans_msg.Msg_id = tc_orig.tc_id;
            tc_trans_msg.Msg_len = sched_pld->arg2;
            tc_trans_msg.pld.pld_ptr = tc_data;
            ipc_status =  os_itc_msg_send(&tc_trans_msg,
                    COMMS_XBAND_CTLR,
                    os_send_itc_wait);
            if(ral_success != ipc_status)
            {

                tc_tm_status = ipc_status;
            }
            else
            {
                /* IPC success. Proceed and return the number
                 * of unpacked bytes
                 */
            }
            break;
#endif

        case TC_DST_COMMS_U:
            tc_trans_msg.src_entity= tc_orig.sa_id;
            tc_trans_msg.Msg_id = tc_orig.tc_id;
            tc_trans_msg.Msg_len = sched_pld->arg2;
            tc_trans_msg.pld.pld_ptr = tc_data;
            ipc_status =  os_itc_msg_send(&tc_trans_msg,
                    COMMS_UHF_CTLR,
                    os_send_itc_wait);
            if(ral_success != ipc_status)
            {

                tc_tm_status = ipc_status;
            }
            else
            {
                /* IPC success. Proceed and return the number
                 * of unpacked bytes
                 */
            }
            break;

        case TC_DST_SDR_APP:
            tc_trans_msg.src_entity= tc_orig.sa_id;
            tc_trans_msg.Msg_id = tc_orig.tc_id;
            tc_trans_msg.Msg_len = sched_pld->arg2;
            tc_trans_msg.pld.pld_ptr = tc_data;
            ipc_status =  os_itc_msg_send(&tc_trans_msg,SDR_APP,
                    os_send_itc_wait);
            if(ral_success != ipc_status)
            {
                tc_tm_status = ipc_status;
            }
            else
            {
                /* IPC success. Proceed and return the number
                 * of unpacked bytes
                 */
            }
            break;

        case TC_DST_EPS:
            tc_trans_msg.src_entity= TC_TM_MANAGER_IPC_RECEIVE;
            tc_trans_msg.Msg_id = tc_orig.tc_id;
            tc_trans_msg.Msg_len = sched_pld->arg2;
            tc_trans_msg.pld.pld_ptr = tc_data;
            ipc_status =  os_itc_msg_send(&tc_trans_msg,
                                          EPS_TASK,
                                          os_send_itc_wait);
            if(ral_success != ipc_status)
            {
              tc_tm_status = ipc_status;

            }
            else
            {
              /* IPC success. Proceed and return the number
                                 * of unpacked bytes
                                 */
            }
            break;

        case TC_DST_PLD_SRVR:
        case TC_DST_PLD_TC:
        case TC_DST_PLD_SW:
        case TC_DST_PLD_NET:
        case TC_DST_PLD_ZES:
        case TC_DST_PLD_SL:
        case TC_DST_OBC:
        case TC_DST_FTM_OTSU:
        case TC_DST_FTM_DBGLOG:
        case TC_DST_FTM_OBC_COMMS:
        case TC_DST_FTM_OBC_EPS:
        case TC_DST_FTM_OBC_ADCS:
            {
                tc_trans_msg.src_entity=TC_TM_MANAGER_IPC_RECEIVE;
                tc_trans_msg.pld_info = sched_pld->dest_id;
                tc_trans_msg.Msg_id = tc_orig.tc_id;
                tc_trans_msg.Msg_len = sched_pld->arg2;
                tc_trans_msg.pld.pld_ptr = tc_data;
                ipc_status =  os_itc_msg_send(&tc_trans_msg,
                        OBC_MAIN,
                        os_send_itc_wait);
                if(ral_success != ipc_status)
                {

                    tc_tm_status = ipc_status;
                }
                else
                {
                    /* IPC success. Proceed and return the number
                     * of unpacked bytes
                     */
                }
            }
            break;


        case TC_DST_EDGE:
            /*TBD: Yet to implement */
            break;

        default:
            tc_tm_status = -1;
            break;
    }
    return tc_tm_status;
}

/**
 * @brief This API unpacks the tc data and routes to the proper destination
 * based on da_id
 */
int32 handle_tc_data(uint8* data, uint16 size)
{
    int32 tc_tm_status = -1;
    uint16 tc_seq_no = 0;
    s_tc_frame tc_orig;
    e_tc_dst_id da_id;
    e_tc_tm_mngr_sts sts = TC_TM_MNGR_OK;
    os_status_t ipc_status = ral_error;
    s_tc_ctrl ctrl_param;
    tc_orig.cmac_hmac = cmac_hmac_tc;
    tc_orig.tc_data = tc_rx_data;

    if( NULL != data )
    {
        //exo_sw_dbg.tc_tm_dbg.tc_data_not_null++;
        tc_tm_status = tc_unpack(data, size, &tc_orig);
        DEBUG_CPRINT(("TC Received Time:- %u\n",tc_orig.time_stamp));
        if(tc_tm_status>0)
        {
             //exo_sw_dbg.tc_tm_dbg.tc_unpack_succ++;
            /*Extract the sequence number and prepend it to
             *the data buffer. The sequence number needs to
             *be unpacked by the receving entity
             */
            uint8 radio_id;
            tc_term_extract_seq_no(&tc_orig, &tc_seq_no);
            validate_seq_no(tc_seq_no);
            //2 byte sequence number, 1 byte radio id
            uint8* tc_data = os_malloc_eram(tc_orig.tc_len + 3);
            if(tc_data!= NULL)
            {
            os_memcpy(tc_data,&tc_seq_no, sizeof(tc_seq_no));
            tc_term_extract_tc_ctrl(&tc_orig, &ctrl_param);
            tc_term_extract_rm_id(&tc_orig,&radio_id);
            tc_data[2] = radio_id;
            /** This is introduced since we need to DL the HM in radio id received in TC
             */
            set_radio_module_id(radio_id);
            if(ctrl_param.encrypt)
            {
                sts = handle_decryption(tc_orig.tc_data,
                        &tc_data[2+1],
                        tc_orig.tc_len);
            }
            else
            {
                os_memcpy(&tc_data[2+1], tc_orig.tc_data, tc_orig.tc_len);
            }

            if(TC_TM_MNGR_OK == sts)
            {
                if(0 == tc_orig.tc_id)
                {
                    /*
                     * This case is for handling multiple TC/TLM request
                     * in one transaction, which is yet to be implemented
                     */
                    os_memcpy(&da_id, &tc_data[4+1], 2);
                    os_memcpy(&tc_orig.tc_id, &tc_data[6+1], 2);
                }
                else
                {
                    da_id = tc_orig.da_id;
                }
                store_rcvd_tc_info(tc_orig.time_stamp, da_id, tc_orig.tc_id);
                switch (da_id)
                {
                    case TC_DST_ADCS:
                        tc_trans_msg.src_entity= tc_orig.sa_id;
                        tc_trans_msg.Msg_id = tc_orig.tc_id;
                        tc_trans_msg.Msg_len = tc_orig.tc_len;
                        tc_trans_msg.pld.pld_ptr = tc_data;
                        ipc_status =  os_itc_msg_send(&tc_trans_msg,
                                ADCS_CONTROLLER,
                                os_send_itc_wait);
                        if(ral_success != ipc_status)
                        {
                            tc_tm_status = ipc_status;

                        }
                        else
                        {
                            /* IPC success. Proceed and return the number
                             * of unpacked bytes
                             */
                        }
                        break;

                    case TC_DST_COMMS_S:
                        tc_trans_msg.src_entity= tc_orig.sa_id;
                        tc_trans_msg.Msg_id = tc_orig.tc_id;
                        tc_trans_msg.Msg_len = tc_orig.tc_len;
                        tc_trans_msg.pld.pld_ptr = tc_data;
                        ipc_status =  os_itc_msg_send(&tc_trans_msg,
                                COMMS_SBAND_CTLR,
                                os_send_itc_wait);
                        if(ral_success != ipc_status)
                        {

                            tc_tm_status = ipc_status;
                        }
                        else
                        {
                            /* IPC success. Proceed and return the number
                             * of unpacked bytes
                             */
                        }
                        break;

                    case TC_DST_COMMS_X:
                        tc_trans_msg.src_entity= tc_orig.sa_id;
                        tc_trans_msg.Msg_id = tc_orig.tc_id;
                        tc_trans_msg.Msg_len = tc_orig.tc_len;
                        tc_trans_msg.pld.pld_ptr = tc_data;
                        ipc_status =  os_itc_msg_send(&tc_trans_msg,
                                COMMS_XBAND_CTLR,
                                os_send_itc_wait);
                        if(ral_success != ipc_status)
                        {

                            tc_tm_status = ipc_status;
                        }
                        else
                        {
                            /* IPC success. Proceed and return the number
                             * of unpacked bytes
                             */
                        }
                        break;

                    case TC_DST_COMMS_U:
                        tc_trans_msg.src_entity= tc_orig.sa_id;
                        tc_trans_msg.Msg_id = tc_orig.tc_id;
                        tc_trans_msg.Msg_len = tc_orig.tc_len;
                        tc_trans_msg.pld.pld_ptr = tc_data;
                        ipc_status =  os_itc_msg_send(&tc_trans_msg,
                                                      COMMS_UHF_CTLR,
                                                      os_send_itc_wait);
                        if(ral_success != ipc_status)
                        {

                            tc_tm_status = ipc_status;
                        }
                        else
                        {
                            /* IPC success. Proceed and return the number
                             * of unpacked bytes
                             */
                        }
                         break;

                    case TC_DST_SDR_APP:
                         tc_trans_msg.src_entity= tc_orig.sa_id;
                         tc_trans_msg.Msg_id = tc_orig.tc_id;
                         tc_trans_msg.Msg_len = tc_orig.tc_len;
                         tc_trans_msg.pld.pld_ptr = tc_data;
                         ipc_status =  os_itc_msg_send(&tc_trans_msg,SDR_APP,
                                  		                      os_send_itc_wait);
                         if(ral_success != ipc_status)
                            {
                                tc_tm_status = ipc_status;
                            }
                         else
                            {
                              /* IPC success. Proceed and return the number
                               * of unpacked bytes
                               */
                             }
                         break;

                    case TC_DST_EPS:
                        /*TBD: Yet to implement */
                    	tc_trans_msg.src_entity= TC_TM_MANAGER_IPC_RECEIVE;
						tc_trans_msg.Msg_id = tc_orig.tc_id;
						tc_trans_msg.Msg_len = tc_orig.tc_len;
                            tc_trans_msg.pld.pld_ptr = tc_data;
						ipc_status =  os_itc_msg_send(&tc_trans_msg,
								EPS_TASK,
								os_send_itc_wait);
						if(ral_success != ipc_status)
						{
							tc_tm_status = ipc_status;

						}
						else
						{
							/* IPC success. Proceed and return the number
							 * of unpacked bytes
							 */
						}
                        break;



                    case TC_DST_OBC:
                        {
                            tc_trans_msg.src_entity=TC_TM_MANAGER_IPC_RECEIVE;
                            tc_trans_msg.pld_info = da_id;
                            tc_trans_msg.Msg_id = tc_orig.tc_id;
                            tc_trans_msg.Msg_len = tc_orig.tc_len;
                                tc_trans_msg.pld.pld_ptr = tc_data;
                            ipc_status =  os_itc_msg_send(&tc_trans_msg,
                                    OBC_MAIN,
                                    os_send_itc_wait);
                            if(ral_success != ipc_status)
                            {

                                tc_tm_status = ipc_status;
                                /*Control variable - Increments on failure of TC sent to OBC*/
                                exo_sw_dbg.tc_tm_dbg.tc_sent_to_obc_fail++;
                            }
                            else
                            {
                                /* IPC success. Proceed and return the number
                                 * of unpacked bytes
                                 *
                                 */
                            /*Control variable - Increments on successful TC sent to OBC*/
                            exo_sw_dbg.tc_tm_dbg.tc_sent_to_obc_succ++;                            
                            }
                        }
                        break;
                    case TC_DST_PLD_TC:
                    case TC_DST_PLD_SW:
                    case TC_DST_PLD_NET:
                    case TC_DST_PLD_ZES:
                    case TC_DST_PLD_SL:
                    case TC_DST_FTM_PS_COMMS:
                    case TC_DST_PLD_SRVR:
                    {
                        DEBUG_CPRINT(("\nMessage recived in PLD_SERVER \n"));
                        icd_tc_pld_info.tc_id = tc_orig.tc_id;
                        icd_tc_pld_info.src_id = TC_TM_MANAGER_IPC_RECEIVE;
                        icd_tc_pld_info.dst_id = da_id;
                        icd_tc_pld_info.tc_pld_len = tc_orig.tc_len;
                        icd_tc_pld_info.tc_pld_buff = os_malloc(icd_tc_pld_info.tc_pld_len);

                        //Sequence number field is skipped for now and TC data is passed to TC decoder
                        //TODO Use Sequence number for correlation ID in future
                        os_memcpy(icd_tc_pld_info.tc_pld_buff, (&tc_data[3]), icd_tc_pld_info.tc_pld_len);

                        //For FTP related TC TM messages, messages are sent with unique FTP ICD Message ID
                        //to enable PS CTRL SW to prioritize FTP ICD messages over other ICD messages
                        if((icd_tc_pld_info.tc_id == TC_FTCI) || \
                                (icd_tc_pld_info.tc_id == TC_FTFCI) || \
                                (icd_tc_pld_info.tc_id == TC_FTSR) || \
                                (icd_tc_pld_info.tc_id == TC_FTDS) || \
                        	    (icd_tc_pld_info.tc_id == TC_FTDB))
                        {
                            obc_ps_ftp_msg_cmd_enc (&ps_icd_encoded_tx_buffer,&ps_icd_encoded_tx_buffer_len,&icd_tc_pld_info);
                        }
                        else
                        {
                            obc_ps_tc_tm_msg_cmd_enc (&ps_icd_encoded_tx_buffer,&ps_icd_encoded_tx_buffer_len,&icd_tc_pld_info);
                        }
                        obc_icd_socket_send(ps_icd_encoded_tx_buffer,ps_icd_encoded_tx_buffer_len);

                        os_free(ps_icd_encoded_tx_buffer);
                        ps_icd_encoded_tx_buffer = NULL;

            /*Functional flow need to be reviewed consumer flow is not clear*/
                        os_free(icd_tc_pld_info.tc_pld_buff);
              //          icd_tc_pld_info.tc_pld_buff = NULL;
                    }

                    os_free(tc_data);
                    tc_data=NULL;
                    break;





                    case TC_DST_FTM_OTSU:
                    case TC_DST_FTM_DBGLOG:
                    case TC_DST_FTM_OBC_COMMS:
                    case TC_DST_FTM_OBC_EPS:
                    case TC_DST_FTM_OBC_ADCS:
                    	 ft_send_tc_to_ftm(tc_orig.tc_id,&tc_data[3],tc_orig.tc_len, da_id);
                         os_free(tc_data);
                         tc_data=NULL;

                    break;


                    case TC_DST_EDGE:
                        /*TBD: Yet to implement */
                        break;
                    case TC_DST_TEST_LAYER:
                        {
#if defined(TEST_LAYER)
                            tc_trans_msg.src_entity= tc_orig.sa_id;
                            tc_trans_msg.Msg_id = tc_orig.tc_id;
                            tc_trans_msg.Msg_len = tc_orig.tc_len;
                            tc_trans_msg.pld.pld_ptr = tc_data;
                            ipc_status =  os_itc_msg_send(&tc_trans_msg,
                                    TEST_TASK,os_send_itc_wait);
                            if(ral_success != ipc_status)
                            {
                                tc_tm_status = ipc_status;

                            }
                            else
                            {
                                /* IPC success. Proceed and return the number
                                 * of unpacked bytes
                                 */
                                DEBUG_CPRINT(("IPC to TEST Layer sent\n"));
                            }
#else
                                DEBUG_CPRINT(("Test layer disabled\n"));
#endif
                        }
                        break;
                    default:
                        tc_tm_status = -1;
                        break;
                }
            }
            else
            {
                /**
                 * Error in decryption. Report the status to the caller. 
                 */
                }
            }
            else
            {
               // exo_sw_dbg.tc_tm_dbg.tc_tm_sts_fail++;
                /*Malloc failure*/
                tc_tm_status = -1;

            }
        }
        else
        {
            /**
             * Error in parsing TC. Report the status to the caller. 
             */
        }
    }
    else
    {
        tc_tm_status = TC_TM_LIB_NULL_PTR;
    }

    return tc_tm_status;
}

/**
 * @brief This API packs the tm data and computes the tm size
 */

int32 handle_tm_data(os_itc_msg_handle_t* rx_data, uint8* tx_data)
{
    int32 tm_size = -1;
    uint8* tm_data;
    s_tm_frame tm_orig;
    uint16 co_id = 0;
    e_tc_tm_mngr_sts status;
    uint8 tm_ctrl = 0;

    tm_orig_init(&tm_orig);
    tm_orig.cmac_hmac = cmac_hmac_tm;
    if((NULL != rx_data) && (NULL !=rx_data->pld.pld_ptr) && (NULL != tx_data))
    {
        tm_data = (uint8*)rx_data->pld.pld_ptr;
        tm_ctrl = check_auth_enc(tm_data[2]);
        tm_orig_cfg_tm_ctrl(&tm_orig, tm_ctrl);
        status = handle_encryption(&tm_orig);
        if(TC_TM_MNGR_OK == status)
        {
            tm_orig_cfg_sof(&tm_orig, TC_TM_LIB_SOF1, TC_TM_LIB_SOF2);
            tm_orig_cfg_tm_id(&tm_orig, rx_data->Msg_id);

            // Select SA ID based on thread Id  
            switch(rx_data->src_entity)
            {
            case ADCS_CONTROLLER:
            {
                tm_orig_cfg_sa_id(&tm_orig, TM_SRC_ADCS);
            }
            break;
            case COMMS_SBAND_CTLR:
            {
                tm_orig_cfg_sa_id(&tm_orig, TM_SRC_COMMS_S);
            }
            break;
            case COMMS_XBAND_CTLR:
            {
                tm_orig_cfg_sa_id(&tm_orig, TM_SRC_COMMS_X);
            }
            break;
            case COMMS_UHF_CTLR:
            {
                tm_orig_cfg_sa_id(&tm_orig, TM_SRC_COMMS_U);
            }
            break;
            case SDR_APP:
            {
                tm_orig_cfg_sa_id(&tm_orig, TM_SRC_SDR_APP);
            }
            break;
            case EPS_TASK:
            {
                tm_orig_cfg_sa_id(&tm_orig, TM_SRC_EPS);
            }
            break;
            default:
            {
                tm_orig_cfg_sa_id(&tm_orig, rx_data->src_entity);
            }
            break;
            }
            tm_orig_cfg_rm_id(&tm_orig,tm_data[3]);
            tm_orig_cfg_eof(&tm_orig, TC_TM_LIB_EOF);
            /* Extract the sequence number and store it in corelation id*/
            os_memcpy(&co_id, tm_data, sizeof(co_id));
            tm_orig_cfg_co_id(&tm_orig, co_id);

            tm_orig_cfg_tm_len(&tm_orig, rx_data->Msg_len);
            tm_orig.tm_data = &tm_data[4];
            tm_size = tm_pack(tx_data, &tm_orig);
            DEBUG_CPRINT(("TM Sent Time:- %u\n",tm_orig.time_stamp));
            store_snt_tm_info(tm_orig.time_stamp, tm_orig.sa_id, tm_orig.tm_id);
        }
    }

    return tm_size;
}

/**
 * @brief This API packs the tm data and computes the tm size
 */
static e_tc_tm_mngr_sts handle_decryption(uint8* encrp_data, uint8* decrp_data,
        uint16 size)
{
    e_tc_tm_mngr_sts status;
    s_crypt_params param;

    /* Configure decryption parameters */
    os_memcpy(param.iv, tc_tm_mngr_iv, 16);
    param.key_size = 256;
    param.key = tc_tm_mngr_key;
    param.encrypt_mode = CRYPT_AES_CFB8;

    /* Decrypt Telemetry data */
    status = tc_orig_decrypt_data(&param,
            (const unsigned char *)encrp_data,size,
            (unsigned char *)decrp_data);

    if(0 == status)
    {
        status = TC_TM_MNGR_OK;
    }
    else
    {
        status = TC_TM_MNGR_ERR;
    }

    return status;
}

/**
 * @brief This API configures the encryption parameters
 */
static e_tc_tm_mngr_sts handle_encryption(s_tm_frame* tm_frame)
{
    e_tc_tm_mngr_sts status;
    s_crypt_params param;

    if(((tm_frame->tm_ctrl & TM_CTRL_CRYPT_BIT) >> TM_CTRL_CRYPT_POS) & 1)
    {
        /* Configure encryption parameters */
        os_memcpy(param.iv, tc_tm_mngr_iv, 16);
        param.key_size = 256;
        param.key = tc_tm_mngr_key;
        param.encrypt_mode = CRYPT_AES_CFB8;

        status = tm_orig_select_encryption(&param);

        if(0 == status)
        {
            status = TC_TM_MNGR_OK;
        }
        else
        {
            status = TC_TM_MNGR_ERR;
        }
    }
    else
    {
        status = TC_TM_MNGR_OK;
    }

    return status;
}

/**
 * @brief This API compares the actual sequence number with the expected
 *        sequence number and throws a warning if both doesn't match
 */
static e_tc_tm_mngr_sts validate_seq_no(uint16 seq_no_in)
{
    e_tc_tm_mngr_sts sts;
    if(seq_no_in != tc_tm_seq_no)
    {
        DEBUG_CPRINT(("*TM_MNGR:Seq mismat WAR\r\n"));
        log_id(LOG_INFO_L3,LOG_TCTM,SEQ_MISMAT_WAR);
        DEBUG_CPRINT(("E-Seq:%d\r\n", tc_tm_seq_no));
        log_id_with_one_arg(LOG_INFO_L3,LOG_TCTM,E_SEQ__u16_,tc_tm_seq_no);
        DEBUG_CPRINT(("A-Seq:%d\r\n", seq_no_in));
        log_id_with_one_arg(LOG_INFO_L3,LOG_TCTM,A_SEQ__u16_,seq_no_in);
        sts = TC_TM_MNGR_ERR;
    }
    else
    {
        if(65535 == tc_tm_seq_no)
        {
            tc_tm_seq_no = 0;
        }
        tc_tm_seq_no++;
        sts = TC_TM_MNGR_OK;
    }

    return sts;
}

/**
 * @brief This API flushes the TM queue
 */
void flush_tm_queue(uint16_t* pld_data)
{
    uint16_t flush_cnt = 0;
    os_memcpy(&flush_cnt,pld_data,2);
    os_itc_msg_handle_t tm_store_flush_q_msg;
    int queue_cnt=os_queue_get_count(radio_mod[COMMS_S_BAND].tm_storage_queue);
    queue_cnt = queue_cnt - flush_cnt;
    if(queue_cnt < 0)
    {
        queue_cnt = 0;
    }
    // dequeue
    for(int i=0;i<queue_cnt;i++)
    {
        if(ral_success == os_queue_receive(
                    radio_mod[COMMS_S_BAND].tm_storage_queue,
                    &tm_store_flush_q_msg,0))
        {
            // free pld
            os_free(tm_store_flush_q_msg.pld.pld_ptr);
            tm_store_flush_q_msg.pld.pld_ptr = NULL;
        }
        else
        {
            DEBUG_CPRINT((" Q was full, but dequeue failed \n"));
            log_id(LOG_WARNING ,LOG_TCTM,Q_WAS_FULL_cm__BUT_DEQUEUE_FAILED);
        }
    }
}

/*
 * @brief This function handle the telemetry storage thread
 */
static void tm_storage_thread(void* ptr)
{
    os_itc_msg_handle_t tm_storage_msg;
	tm_storage_msg.pld.pld_ptr = NULL;
    os_status_t q_sts;
    uint8_t dwn_ctrl_flag = 1;

    uint8_t max_free_cnt =  MAX_TM_DROP_CNT_FOR_NEW_TM;
    max_free_cnt=max_free_cnt;
    void *cb_mem = NULL;
    void *mq_mem = NULL;
    for(int i =0;i<MAX_RADIO_MOD;i++)
    {
        if(radio_mod[i].radio_set_flag)
        {
            mq_mem = os_malloc_eram(MAX_TM_STORAGE_DEPTH * sizeof(os_itc_msg_handle_t));

            if(mq_mem)
            {
                //health_buff_mb = os_malloc_eram(sizeof(StaticQueue_t));
                cb_mem = os_malloc_eram(2000);// this has to be changed
                if(cb_mem)
                {
                    q_sts = os_queue_create_static(&radio_mod[i].tm_storage_queue,
                            MAX_TM_STORAGE_DEPTH,sizeof( os_itc_msg_handle_t ), cb_mem, mq_mem, "RADIO_STORAGE");

                    if(ral_success != q_sts)
                    {
                        DEBUG_CPRINT(("TC_TM_MNGR: os_queue_create failure in tm_storage\n"));
                        log_id(LOG_WARNING ,LOG_TCTM,OS_QUEUE_CREATE_FAILURE_IN_TM_STORAGE);
                    }
                }
                else
                {
                    os_free(mq_mem);
                    DEBUG_CPRINT(("TC_TM_MNGR: os_queue_create failure in tm_storage\n"));
                    q_sts = ral_err_invld_ptr;
                }
            }
        }
    }

    // Dummy allocation
    os_free(os_malloc_eram(TM_HEADER_OVERHEAD));

    while(1)
    {
        /*
         * 100ms timeout. if timeout, check the gs_contact flag, dequeue if
         * available (not empty) send to s-band check if this is COMMS UP IPC
         * from Comms. Force sband tx by adding os_delay in sband task of
         * os_delay(1000). Reuduce q depth, test q - full ps-gs file download
         * reduce eram - pool size hit full scenraio (ps-gs) file download
         * add metrics dropped packets, sent packet in obc, received packets
         * in gs MAX continous Qued TM send count
         */

#ifndef LINUX_TEMP_PORT
        if(ral_success == os_itc_msg_rcv(TM_STORAGE,&tm_storage_msg,os_wait_forever))
#else
            if(ral_success == os_itc_msg_rcv(TM_STORAGE,&tm_storage_msg,os_wait_forever))
#endif
            {
                switch(tm_storage_msg.src_entity)
                {
                    case SDR_APP:
                    case COMMS_SBAND_CTLR:
                    case COMMS_UHF_CTLR:
                        {
                            switch(tm_storage_msg.Msg_id)
                            {
                                case TC_TM_STORAGE_TM_DLREQ:
                                case UHF_TC_TM_ST_REQ:
                                case SBAND_TC_TM_ST_REQ:
                                    {
                                        /*Control variable - Increments on every ground contact*/ 
                                        exo_sw_dbg.tc_tm_dbg.grd_contact_strt_ind++;
                                    	dwn_ctrl_flag = check_tm_buffer_cntrl_flag();

                                    	if(dwn_ctrl_flag == 1)
                                        {
                                    	tc_tm_send_tm_storage_buffer();
                                        }

                                        /**  - Get the radio ID */
                                        uint8 radio_id = ((uint8*)tm_storage_msg.pld.pld_ptr)[0];

                                        /**   - Check if overrule is enabled */
                                        if(radio_id_overrule.overrule_enb ==1)
                                        {
                                            /**    - Battery is in safe level */
                                            if(sat_btry_info.status == HW_BTRY_SAFE)
                                            {
                                                /**     - Override with safe radio_id */
                                                radio_id = radio_id_overrule.safe_radio_id;
                                            }
                                            /**    - Battery is in safe level */
                                            else if(sat_btry_info.status == HW_BTRY_NORMAL)
                                            {
                                                /**     - Override with normal radio_id */
                                                radio_id = radio_id_overrule.norm_radio_id;
                                            }
                                            else
                                            {
                                                /**     - Not applicable for overide */
                                            }
                                        }
                                        /**  - Check for valid radio id */
                                        if(DEFAULT_COMMS_RADIO_ID != radio_id)
                                        {
                                            /**  - When GS Contact is received start downlinking the HM 
                                             * via radio module id received from comms
                                             * */
                                            tc_tm_hm_dl_req(radio_id);
                                        }
                                        /**  - Invalid radio id from comms indicates, HM has to take radio id 
                                         *     from TC received
                                         * */
                                        /** Note: get_radio_module_id() will return the most recent TC's Radio id */
                                        else
                                        {
                                            /**  - When GS Contact is received start downlinking the HM data 
                                             * via radio module id received in TC
                                             * */
                                            tc_tm_hm_dl_req(get_radio_module_id());
                                        }
                                    }
                                    break;

                                case SBAND_TC_TM_STOP_REQ:
                                case UHF_TC_TM_STOP_REQ:
                                    {
                                        /*
                                         * On stop notification do not queue
                                         * more packets,may be keep only last
                                         * 10% packet of the queue
                                         */
                                        /*Control variable - Increments on every ground contact loss*/
                                        exo_sw_dbg.tc_tm_dbg.grd_contact_stop_ind++;
                                    }
                                    break;

                                case SDR_RADIO_ID_OVR_RULE_CFG:
                                    {
                                        os_memcpy(&radio_id_overrule,tm_storage_msg.pld.pld_ptr,sizeof(radio_id_overrule));
                                    }
                                    break;

                                default:
                                    {
                                    	 tc_tm_process_tm(tm_storage_msg);
                                    }
                            }
                        }
                        break;
                    case OBC_MAIN:
                        {
                            switch(tm_storage_msg.Msg_id)
                            {
                                case TM_STORAGE_FLUSH_REQ:
                                    {
                                        flush_tm_queue(
                                                tm_storage_msg.pld.pld_ptr);
                                        os_free(tm_storage_msg.pld.pld_ptr);
                                        tm_storage_msg.pld.pld_ptr = NULL;
                                    }
                                    break;
                                default:
                                    {
                                    	 tc_tm_process_tm(tm_storage_msg);
                                    }
                            }
                        }
                        break;

                    default:
                        {
                        	 tc_tm_process_tm(tm_storage_msg);
                        }
                }
            }

   	     //** FREE_TBD same pointer was used, need to discuss */
		 if(tm_storage_msg.pld.pld_ptr)
		 {
			 os_free(tm_storage_msg.pld.pld_ptr);
			 tm_storage_msg.pld.pld_ptr = NULL;
		 }
     }
}


/**
 * @brief This API is used to process the tm message
 * 1.Send TCTM directly if GS contact is available
 * 2.If there is no GS contact store TM message in TM storage buffer
 */

void tc_tm_process_tm(os_itc_msg_handle_t tm_storage_msg_s)
{
	int32 tm_size = -1;
        uint8_t sts;
	uint8 *tm_storage_data;
	uint8_t *storag_data = NULL;
	os_itc_msg_handle_t tm_store_local_q_msg;
	tm_store_local_msg.pld.pld_ptr = NULL;
	uint8_t storage_radio_id = 0;
#ifndef LINUX_TEMP_PORT
    uint8_t max_free_cnt =  MAX_TM_DROP_CNT_FOR_NEW_TM;
#endif

	tm_store_local_msg.Msg_id = tm_storage_msg_s.Msg_id;
	tm_store_local_msg.pld_info = tm_storage_msg_s.pld_info;
	tm_store_local_msg.src_entity = TM_STORAGE;

	/*
	 * Check enough memory is avialable before allocation.
	 * Also 10kb reserved atleast
	 */

	storag_data = tm_storage_msg_s.pld.pld_ptr;
	UNUSED(storag_data);
	storage_radio_id = storag_data[3];
	//storage_radio_id = COMMS_S_BAND;

#ifndef LINUX_TEMP_PORT
        if(radio_mod[storage_radio_id].radio_set_flag)
        {
            while(((tm_storage_msg_s.Msg_len + TM_HEADER_OVERHEAD +
                       (10*1024)) >= xPortGetMinimumEverFreeHeapSize_eram())
                    && max_free_cnt)
            {
                // dequeue
                if(ral_success == os_queue_receive(
                            radio_mod[storage_radio_id].tm_storage_queue,
                            &tm_store_local_q_msg,0))
                {
                    /*
                     * TM was not able to fit due to low RAM memory,
                     * removing old TMs.
                     */
                    os_free(tm_store_local_q_msg.pld.pld_ptr);
                    tm_store_local_q_msg.pld.pld_ptr = NULL;
                    tm_mem_fail_drop_cnt++;
                }
                else
                {
                    /*
                     * No TM avail in Q, Maybe memory was full due to
                     *  other apps
                     */
                    max_free_cnt=0;
                    break;
                }

                max_free_cnt--;
            }
        }
#endif
        if(radio_mod[storage_radio_id].radio_set_flag)
        {
            /*
             * extra allocation were done, need to check, TM Headers might
             * be around 40 bytes
             */
            //tm_storage_data = (uint8*)os_malloc_eram(tm_storage_msg_s.Msg_len
                    //+TM_HEADER_OVERHEAD);
            /* Using ERAM allocation caused crash while 1KB data is pushed from PS to OBC via TTY DBG TC
             * Need to debug further */
            tm_storage_data = (uint8*)os_malloc(tm_storage_msg_s.Msg_len
                    +TM_HEADER_OVERHEAD);

            if(tm_storage_data)
            {
                //Add tc/tm headers with the telemetry data
                tm_size = handle_tm_data(&tm_storage_msg_s,
                        tm_storage_data);

                if (tm_size>0)
                {
                    //Fill the new message in the local variable
                	tm_store_local_msg.pld_info = storage_radio_id;
                    tm_store_local_msg.pld.pld_ptr = tm_storage_data;
                    tm_store_local_msg.Msg_len = tm_size;

                    if(radio_id_overrule.overrule_enb ==1)
                    {
                        if(sat_btry_info.status == HW_BTRY_SAFE)
                        {
                            storage_radio_id = radio_id_overrule.safe_radio_id;
                        }
                        else if(sat_btry_info.status == HW_BTRY_NORMAL)
                        {
                            storage_radio_id = radio_id_overrule.norm_radio_id;
                        }
                    }

                    if((radio_mod[storage_radio_id].gs_contact_valid != NULL)&&
                            				  (radio_mod[storage_radio_id].radio_set_flag))
                      	     {
                      		  if(radio_mod[storage_radio_id].gs_contact_valid())
                      		  {
                      			  sts = os_itc_msg_send(&tm_store_local_msg,
                      			 						radio_mod[storage_radio_id].thread_id,
                      			 						   os_send_itc_wait);
                                          if(ral_success == sts)
                                          {
                                              /*Control variable - Increments on successful TM sent to comms*/
                                              exo_sw_dbg.tc_tm_dbg.tm_sent_to_comms_succ++;
                                          }
                                          else
                                          {
                                              /*Control variable - Increments on failure of TM sent to comms*/
                                              exo_sw_dbg.tc_tm_dbg.tm_sent_to_comms_fail++;
                                          }

                      			}
                      	     else
                      	     {
                      		//  for(uint8_t id = start_id;id<stop_id;id++)
                      		//   {
                      				  if(os_queue_get_remain_count(
                      								radio_mod[storage_radio_id].tm_storage_queue) > 0)
                      					{
                      						/** En-queue */
                      						if(ral_success != os_queue_send(
                      									radio_mod[storage_radio_id].tm_storage_queue,
                      									&tm_store_local_msg, 0))
                      						{
                      							/* TM storage Q depth was full, remove oldest TM and
                      							 * add new TM again
                      							 */
                      							os_free(tm_store_local_msg.pld.pld_ptr);
                      							tm_store_local_msg.pld.pld_ptr = NULL;
                      							tm_mem_fail_drop_cnt++;
                      						}

                      					}
                      					else
                      					{
                      						// dequeue
                      						if(ral_success == os_queue_receive(
                      									radio_mod[storage_radio_id].tm_storage_queue,
                      									&tm_store_local_q_msg,0))
                      						{
                      							// free it
                      							os_free(tm_store_local_q_msg.pld.pld_ptr);
                      							tm_store_local_q_msg.pld.pld_ptr = NULL;
                      							tm_mem_fail_drop_cnt++;
                      						}
                      						else
                      						{
                      							DEBUG_CPRINT((" Q was full, but dequeue failed \n"));
                      							log_id(LOG_WARNING ,LOG_TCTM,Q_WAS_FULL_cm__BUT_DEQUEUE_FAILED);
                      						}

                      						// retry new one
                      						if(ral_success != os_queue_send(
                      									radio_mod[storage_radio_id].tm_storage_queue,
                      									&tm_store_local_msg, 0))
                      						{
                      							/** drop the new one */
                      							os_free(tm_store_local_msg.pld.pld_ptr);
                      							tm_store_local_msg.pld.pld_ptr = NULL;
                      							tm_mem_fail_drop_cnt++;
                      						}
                      					 }
                      			  }
                        	}
                }
                else
                {
                    DEBUG_CPRINT(("TM Packing failed \n"));
                    log_id(LOG_WARNING ,LOG_TCTM,TMPACKING_FAILED);
                    if(tm_storage_data)
                    {
                        os_free(tm_storage_data);
                        tm_storage_data = NULL;
                    }
                }
            }
            else
            {
                /*
                 * memory not available, just drop it as already we tried
                 * to free many old ones
                 */
                tm_mem_fail_drop_cnt++;
            }
        }
}


/**
 * @brief This API is used to send the stored TM message on the following conditions
 * 1.Ground pass is available
 * 2.Control flag for downlink TM is set
 */
void tc_tm_send_tm_storage_buffer()
{

	uint8_t start_id = 0,stop_id = 0;
	uint8_t loc_radio_id = 0;
	os_status_t queue_sts = ral_error;
	os_itc_msg_handle_t tm_store_local_q_msg;
	uint16_t max_pkt_cnt_cont = MAX_TM_CONT_CNT;
	uint16 itc_queue_avail_depth_cnt = 0;


    if(def_radio_id == DEFAULT_RADIO_ID)
	   {
		   start_id = COMMS_S_BAND;
		   stop_id = MAX_RADIO_MOD;
	   }
	   else
	   {
		   if(def_radio_id < MAX_RADIO_MOD)
		   {
			   start_id = def_radio_id;
			   stop_id = def_radio_id+1;
			   loc_radio_id = COMMS_S_BAND;
		   }
		   else
		   {
			   set_radio_id(DEFAULT_RADIO_ID);
			   start_id = COMMS_S_BAND;
			   stop_id = DEFAULT_RADIO_ID;
		   }
	   }

          uint8_t radio_id;

				  for(uint8_t id = start_id;id<stop_id;id++)
				  {
                                      if(radio_id_overrule.overrule_enb ==1)
                                      {
                                          if(sat_btry_info.status == HW_BTRY_SAFE)
                                          {
                                              radio_id = radio_id_overrule.safe_radio_id;
                                          }
                                          else if(sat_btry_info.status == HW_BTRY_NORMAL)
                                          {
                                              radio_id = radio_id_overrule.norm_radio_id;
                                          }
                                          else
                                          {
                                              radio_id = id;
                                          }
                                      }
                                      else
                                      {
                                          radio_id = id;
                                      }
				   if((radio_mod[radio_id].gs_contact_valid != NULL)&&
										  (radio_mod[radio_id].radio_set_flag))
							 {
							  if(radio_mod[radio_id].gs_contact_valid())
							  {
					uint8_t *tm_intf_buff = os_malloc_eram(TM_MAX_BUFF_SIZE);
					uint8_t *tm_pld_buff = NULL;
					uint16_t tm_intf_buff_len=0, tm_cpy_buff_len = 0;

					 max_pkt_cnt_cont = MAX_TM_CONT_CNT;

					 itc_queue_avail_depth_cnt = 0;
					 os_itc_msg_get_free_count(radio_mod[radio_id].thread_id,
							 &itc_queue_avail_depth_cnt);

					 /*
					  * if s-band queue is full do enqueue at last, break the
					  * loop here, Do not send more packets continuously more
					  * than MAX_TM_CONT_CNT
					  */
					 while(max_pkt_cnt_cont && itc_queue_avail_depth_cnt
							 && tm_intf_buff)
					 {
						 //do dequeue and send it to s-band for max
						 //Continuous counts
						 if(def_radio_id == DEFAULT_RADIO_ID)
						 {
							 queue_sts = os_queue_receive(
									 radio_mod[id].tm_storage_queue,
									 &tm_store_local_q_msg,0);
						 }
						 else
						 {
							 if(radio_mod[loc_radio_id].radio_set_flag)
							 {
								 queue_sts = os_queue_receive(
									  radio_mod[loc_radio_id].tm_storage_queue,
									  &tm_store_local_q_msg,0);
							 }
							 else
							 {
								 queue_sts = ral_error;
							 }
						 }

						 if(ral_success == queue_sts)
						 {
							   if (tm_intf_buff_len + tm_store_local_q_msg.Msg_len
																<= TM_MAX_BUFF_SIZE)
								{
									os_memcpy(&tm_intf_buff[tm_intf_buff_len],
											tm_store_local_q_msg.pld.pld_ptr,
											tm_store_local_q_msg.Msg_len);
									tm_intf_buff_len +=
										tm_store_local_q_msg.Msg_len;
									os_free(tm_store_local_q_msg.pld.pld_ptr);
									tm_store_local_q_msg.pld.pld_ptr = NULL;
								}
								else
								{
									tm_cpy_buff_len = TM_MAX_BUFF_SIZE - tm_intf_buff_len;
									os_memcpy(&tm_intf_buff[tm_intf_buff_len],
											tm_store_local_q_msg.pld.pld_ptr,
											tm_cpy_buff_len);
									tm_intf_buff_len = tm_intf_buff_len + tm_cpy_buff_len;
									tm_store_local_q_send_msg.Msg_len =
											tm_intf_buff_len;
									tm_store_local_q_send_msg.pld.pld_ptr =
											tm_intf_buff;
									tm_store_local_q_send_msg.pld_info = radio_id;
									os_itc_msg_send(&tm_store_local_q_send_msg,
											radio_mod[radio_id].thread_id,
											os_send_itc_wait);
                                    /** Delay for UHF is needed since flow control is not enabled 
                                     *  As per the UHF data rate 1 sec delay is required for
                                     *  every 250 Bytes, so N/250 = delay, where N is Byte size 
                                     */
                                    /** TBD: This has to be removed after flow control enable */
                                    if(COMMS_UHF == id)
                                    {
                                        os_delay(((tm_intf_buff_len/250) ? ((tm_intf_buff_len/250)*1000) : 1000));
                                    }
									tm_intf_buff_len =
											tm_store_local_q_msg.Msg_len - tm_intf_buff_len;
									tm_pld_buff = tm_store_local_q_msg.pld.pld_ptr;
									tm_intf_buff = os_malloc_eram(tm_intf_buff_len);
									os_memcpy(tm_intf_buff,
											tm_pld_buff + tm_cpy_buff_len,
											tm_intf_buff_len);
									tm_store_local_q_send_msg.Msg_len =
											tm_intf_buff_len;
									tm_store_local_q_send_msg.pld.pld_ptr =
											tm_intf_buff;
									tm_store_local_q_send_msg.pld_info = radio_id;
									os_itc_msg_send(&tm_store_local_q_send_msg,
											radio_mod[radio_id].thread_id,
											os_send_itc_wait);
                                    /** Delay for UHF is needed since flow control is not enabled 
                                     *  As per the UHF data rate 1 sec delay is required for
                                     *  every 250 Bytes, so N/250 = delay, where N is Byte size 
                                     */
                                    /** TBD: This has to be removed after flow control enable */
                                    if(COMMS_UHF == id)
                                    {
                                        os_delay(((tm_intf_buff_len/250) ? ((tm_intf_buff_len/250)*1000) : 1000));
                                    }
									os_free(tm_store_local_q_msg.pld.pld_ptr);
									tm_store_local_q_msg.pld.pld_ptr = NULL;
									tm_intf_buff_len = 0;
									tm_intf_buff=os_malloc_eram(TM_MAX_BUFF_SIZE);
								}


							 if(!radio_mod[radio_id].gs_contact_valid())
							 {
								 break;
							 }
						 }
						 else
						 {
							 if(def_radio_id == DEFAULT_RADIO_ID)
							 {
								 break;
							 }
							 else if(loc_radio_id < MAX_RADIO_MOD-1)
							 {
								 loc_radio_id++;
							 }
							 else
							 {
								 break;
							 }
						 }
						 max_pkt_cnt_cont--;
						 itc_queue_avail_depth_cnt = 0;
						 os_itc_msg_get_free_count(radio_mod[id].thread_id ,
								 &itc_queue_avail_depth_cnt);
					 }

					 // if partial tm buffer availble, send to sband
					 if(tm_intf_buff_len)
					 {
						 tm_store_local_q_msg.pld.pld_ptr = tm_intf_buff;
						 tm_store_local_q_msg.Msg_len = tm_intf_buff_len;
						 tm_store_local_q_msg.pld_info = id;
						 // sent last partial buffer to sband tx
						 os_itc_msg_send(&tm_store_local_q_msg,
								 radio_mod[id].thread_id,
								 os_send_itc_wait);
						 tm_store_local_q_msg.pld.pld_ptr = NULL;
					 }
					 else
					 {
						 os_free(tm_intf_buff);
						 tm_intf_buff=NULL;
					 }

				 }
			 }
	     }
}

/**
 * @brief This API sets the authetication and encoding status of telemetry
 *        based on the global flag.
 */
static uint8 check_auth_enc(uint8 tm_ctrl)
{
    uint8 tm_ctrl_local = tm_ctrl;

    if(1 == security_sts)
    {
        tm_ctrl_local = (tm_ctrl_local | ((1 & ((tm_ctrl_local
                          >>TM_CTRL_CRYPT_POS)&1)) << TM_CTRL_CRYPT_POS));
    }
    else
    {
        tm_ctrl_local = (tm_ctrl_local & (~(1 << TM_CTRL_CRYPT_POS)));
    }

    if(1 == authentication_sts)
    {
        tm_ctrl_local = (tm_ctrl_local | ((1 & ((tm_ctrl_local
                                >>TM_CTRL_AUTH_POS)&1)) << TM_CTRL_AUTH_POS));
    }
    else
    {
        tm_ctrl_local = (tm_ctrl_local & (~(1 << TM_CTRL_AUTH_POS)));
    }

    return tm_ctrl_local;
}

/**
 * @brief This API used to set radio module parameters for thread id and
 * ground contact valid
 */
void set_radio_module(uint8_t radio_id,uint8_t thread_id,
        gs_contact_valid_fptr_t ground_contact_api)
{
    if(radio_id < MAX_RADIO_MOD)
    {
        radio_mod[radio_id].gs_contact_valid = ground_contact_api;
        radio_mod[radio_id].thread_id = thread_id;
        radio_mod[radio_id].radio_set_flag = 1;
    }
}

/**
 * @brief This API used to send DL request to HM buffer FW
 */
void tc_tm_hm_dl_req(uint8 radio_id)
{
    os_itc_msg_handle_t obc_snd_hlt_ipc_msg;

    /** Allocate memory for dl req structure */
    s_tc_tm_dl_q_req *dl_req = os_malloc(sizeof(s_tc_tm_dl_q_req));

    /** Memory allocation success */
    if(dl_req)
    {
        /** - Keep submodule and queue as invalid so that all modules queue with 0 priority will be downloaded */
        dl_req->submod_id = 0xFF;
        dl_req->queue_id = 0xFF;
        dl_req->priority = 0;

        obc_snd_hlt_ipc_msg.pld.pld_ptr = (void*)dl_req;
        obc_snd_hlt_ipc_msg.pld.pld_data = radio_id; 
        obc_snd_hlt_ipc_msg.src_entity = TM_STORAGE;
        obc_snd_hlt_ipc_msg.Msg_id = OBC_TC_TM_DL_BUFFER_REQ;
        os_itc_msg_send(&obc_snd_hlt_ipc_msg, HM_MANAGER_IPC_RECEIVE, os_send_itc_wait);
    }
}
