/**
 * @file comms_uhf_csp.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_endian.h"
#include "csp/interfaces/csp_if.h"
#include "csp/arch/csp_time.h"
#include "comms_uhf_main.h"
#include "comms_uhf_ipc.h"
#include "comms_uhf_csp.h"
#include "comms_uhf_sim.h"
#include "comms_uhf_init.h"
#include "errno.h"
#include "exo_common.h"
#include "csp_if_uhf.h"
#include <csp/csp.h>
#include "exo_io_al_sos_timer.h"
#include "exo_tctm_ipc.h"
#include "logger.h"
#include "comms_uhf_intf.h"
#include "err_handler.h"
#include "sdr_app_main.h"


#define DEFAULT_TIMEOUT 1000///< Default timeout value
uint8_t PLACE_IN_EXT_RAM_MEM tx_buff[UHF_MAX_PLD] = {0};///< TX buffer
uint8_t PLACE_IN_EXT_RAM_MEM uhf_rx_cmd_buff[UHF_MAX_PAYLOAD+1] = {0};///< UHF RX command buffer
uint32_t PLACE_IN_EXT_RAM_MEM csp_uhf_data_count = 0;///< CSP UHF data count
extern uint8_t PLACE_IN_EXT_RAM_MEM uhf_tc_cmd_id;///< UHF TC command ID
uint8_t PLACE_IN_EXT_RAM_MEM uhf_tc_len = 0;///< UHF TC length
uint32_t PLACE_IN_EXT_RAM_MEM uhf_fw_page = 0;///< UHF firmware page
uint8_t PLACE_IN_EXT_RAM_MEM uhf_fw_page_num = 0;///< UHF firmware page number
extern uint8_t PLACE_IN_EXT_RAM_MEM beacon_op;///< Beacon operation
extern uint8_t PLACE_IN_EXT_RAM_MEM uhf_fw_upd_sts;
sys_time_s PLACE_IN_EXT_RAM_MEM get_t_uhf;///< UHF time

/*** struct for UHF Timer Interval ***/
extern s_uhf_comms_timer PLACE_IN_EXT_RAM_MEM uhf_trans_timer;///< UHF transmission timer

extern os_timer_handle_ptr uhf_cmd_timeout;///< UHF command timeout

extern s_uhf_rf_tx_cfg PLACE_IN_EXT_RAM_MEM uhf_nvm_tx_cfg;
extern s_uhf_rf_rx_cfg PLACE_IN_EXT_RAM_MEM uhf_nvm_rx_cfg;
extern s_uhf_rf_tx_pkt_cfg PLACE_IN_EXT_RAM_MEM uhf_nvm_tx_pkt_cfg;
extern s_uhf_rf_rx_pkt_cfg PLACE_IN_EXT_RAM_MEM uhf_nvm_rx_pkt_cfg;
extern s_uhf_rf_pwr_cfg PLACE_IN_EXT_RAM_MEM uhf_nvm_pwr_cfg;

/** UHF TC-TM buffer */
extern uint8_t PLACE_IN_EXT_RAM_MEM comms_uhf_csw_rx_buf[512];///< UHF CSW RX buffer

extern s_comms_uhf_get_tm_prop PLACE_IN_EXT_RAM_MEM comms_uhf_tm_read;///< UHF TM read properties

extern s_uhf_cc1110_reg_map PLACE_IN_EXT_RAM_MEM uhf_reg_map;///< UHF register map
extern s_uhf_cc1110_reg_map PLACE_IN_EXT_RAM_MEM uhf_reg_rb;///< UHF register RB
extern s_uhf_pa_cfg PLACE_IN_EXT_RAM_MEM uhf_pa_cfg;///< UHF PA config
extern s_uhf_pkt_cfg PLACE_IN_EXT_RAM_MEM uhf_pkt_cfg;///< UHF packet config
extern s_uhf_rf_cfg PLACE_IN_EXT_RAM_MEM uhf_rf_cfg;///< UHF RF config
extern s_uhf_pa_cfg PLACE_IN_EXT_RAM_MEM uhf_pa_cfg_rb;///< UHF PA config RB
extern s_uhf_pkt_cfg PLACE_IN_EXT_RAM_MEM uhf_pkt_cfg_rb;///< UHF packet config RB
extern s_uhf_rf_cfg PLACE_IN_EXT_RAM_MEM uhf_rf_cfg_rb;///< UHF RF config RB
extern s_comms_uhf_beacon_data PLACE_IN_EXT_RAM_MEM uhf_tx_beacon_data;///< UHF TX beacon data
extern s_seri_pack_send_t PLACE_IN_EXT_RAM_MEM snd_pack;///< Serial pack send
extern uint8_t  uhf_hk_upd_id;///< UHF HK update ID
extern uint8_t  is_uhf_hk_upd_fill;///< Is UHF HK update filled
extern uint8_t  uhf_siz_hk_id ;///< UHF HK ID size
extern int8 PLACE_IN_EXT_RAM_MEM outbuff[mem_size];///< Output buffer

extern uint16_t PLACE_IN_EXT_RAM_MEM uhf_tc_tm_id;///< UHF TC TM ID
extern s_comms_uhf_tm_prop PLACE_IN_EXT_RAM_MEM comms_uhf_tm;///< UHF TM properties

extern s_sdr_beacon_pld uhf_beacon_data;

uint16_t PLACE_IN_EXT_RAM_MEM uhf_seq_num =0;///< UHF sequence number
uint16_t PLACE_IN_EXT_RAM_MEM uhf_data_seq_num =0;///< UHF data sequence number

/**
 * @brief This function is used for updating the CSP address of UHF radio
 * @brief Update the CSP port and address used for UHF communication.
 * 
 * This function updates the CSP port and address that will be used when
 * sending/receiving UHF packets over CSP.
 * 
 * @param[in,out] csp_tx_port Pointer to CSP port variable. Will be updated.
 * @param[in,out] csp_tx_addr Pointer to CSP address variable. Will be updated.
 */
void comms_uhf_upd_port_addr(uint8_t *csp_tx_port,uint8_t *csp_tx_addr)
{
    *csp_tx_port = UHF_CSP_TX_DATA_PORT ;
    *csp_tx_addr = UHF_UART_TX_ADDRESS ;
}

/**
 * @brief This function is used for updating the CSP address of UHF radio simulation
 * @brief Update simulated CSP port and address for UHF.
 * 
 * In simulation mode, update the port and address used for UHF communication.
 * 
 * @param[in,out] csp_tx_port Simulated CSP port. 
 * @param[in,out] csp_tx_addr Simulated CSP address.
 */
void comms_uhf_sim_upd_port_addr(uint8_t *csp_tx_port,uint8_t *csp_tx_addr)
{
    *csp_tx_port = UHF_CSP_TX_DATA_PORT ;
    *csp_tx_addr = UHF_UART_TX_ADDRESS ;
}

/**
 * @brief This function updates the command ID.
 * @brief Send UHF radio command over UART.
 * 
 * Sends a UHF radio command to the UHF radio module over the UART interface.
 * 
 * @param[in] cmd_id The UHF radio command ID to send.
 */
void uhf_upd_send_uart_cmd(uint8_t cmd_id)
{
    uint16_t len = 0;
    uhf_tc_cmd_id = cmd_id;

    DEBUG_CPRINT(("\n\r [UHF] TC cmd: %x",cmd_id));
    log_id_with_one_arg(LOG_INFO_L1,LOG_COMMS,UHF_TC_CMD__hex_,cmd_id);
    if(PWR_ON == uhf_get_state())
    {
        len = sizeof(uhf_tc_cmd_id);
        os_memcpy(tx_buff,&uhf_tc_cmd_id,len);

        switch(cmd_id)
        {
            case UHF_BOOTLOADER_MSG_PING:
                {
                }
                break;

            case UHF_BOOTLOADER_MSG_WRITE_PAGE:
                {
                    /*	    s_comms_uhf_data *uhf_fw_data;

                            uhf_fw_data->header.sync1 = UHF_START_BYTE_0;
                            uhf_fw_data->header.sync2 = UHF_START_BYTE_1;
                            uhf_fw_data->header.length = snd_pack.lngth;
                            uhf_fw_data->header.hwid = snd_pack.hwid;
                            uhf_fw_data->header.seqnum = snd_pack.seq;
                            uhf_fw_data->header.system = snd_pack.dst;
                            uhf_fw_data->header.command = uhf_tc_cmd_id;
                            os_memcpy(&tx_buff[len],&snd_pack.hx_data,(sizeof(snd_pack.hx_data)));
                            len += sizeof(snd_pack.hx_data);*/
                }
                break;

            case UHF_BOOTLOADER_MSG_ERASE:
                {
                }
                break;

            case UHF_RADIO_MSG_REBOOT:
                {
                    /*		s_comms_uhf_data *uhf_fw_data;

                                uhf_fw_data->header.sync1 = UHF_START_BYTE_0;
                                uhf_fw_data->header.sync2 = UHF_START_BYTE_1;
                                uhf_fw_data->header.length = snd_pack.lngth;
                                uhf_fw_data->header.hwid = snd_pack.hwid;
                                uhf_fw_data->header.seqnum = snd_pack.seq;
                                uhf_fw_data->header.system = snd_pack.dst;
                                uhf_fw_data->header.command = uhf_tc_cmd_id;
                                */
                }
                break;

            case UHF_RADIO_MSG_GET_TIME:
                {
                    /**Payload TODO */
                }
                break;

            case UHF_RADIO_MSG_SET_TIME:
                {
                }
                break;

            case UHF_RADIO_MSG_RANGING:
                {
                }
                break;
            case UHF_RADIO_MSG_TELEM:
                {

                }
                break;
            case UHF_RADIO_MSG_GET_TELEM:
                {
                }
                break;

            case UHF_RADIO_MSG_SET_CALLSIGN:
                {
                    /**Payload TODO */
                }
                break;

            case UHF_RADIO_MSG_SET_CFG:
                {
                    os_memcpy(&tx_buff[len],&uhf_beacon_data,(sizeof(s_sdr_beacon_pld)));
                    len += sizeof(s_sdr_beacon_pld);
                }
                break;
            case UHF_SET_ALL_RADIO_CFG_REQ:
                {
                    os_memcpy(&tx_buff[len],&uhf_reg_map,sizeof(s_uhf_cc1110_reg_map));
                    len += sizeof(s_uhf_cc1110_reg_map);
                }
                break;

            case UHF_SET_BCON_TIMER:
                {
                    os_memcpy(&tx_buff[len],&uhf_trans_timer,sizeof(s_uhf_comms_timer));
                    len += sizeof(s_uhf_comms_timer);
                }
                break;
            case UHF_GET_ALL_RADIO_CFG_REQ:
                {
                }
                break;

            case UHF_SET_TX_RADIO_CFG_REQ :
                {
                    os_memcpy(&tx_buff[len],&uhf_rf_cfg,(sizeof(s_uhf_rf_cfg)));
                    len += sizeof(s_uhf_rf_cfg);
                }
                break;

            case UHF_GET_TX_RADIO_CFG_REQ :
                {
                }
                break;

            case UHF_SET_RX_RADIO_CFG_REQ :
                {
                    os_memcpy(&tx_buff[len],&uhf_rf_cfg,(sizeof(s_uhf_rf_cfg)));
                    len += sizeof(s_uhf_rf_cfg);
                }
                break;

            case UHF_GET_RX_RADIO_CFG_REQ :
                {

                }
                break;

            case UHF_SET_TX_PKT_CFG_REQ   :
                {
                    os_memcpy(&tx_buff[len],&uhf_pkt_cfg,(sizeof(s_uhf_pkt_cfg)));
                    len += sizeof(s_uhf_pkt_cfg);
                }
                break;

            case UHF_GET_TX_PKT_CFG_REQ   :
                {
                }
                break;

            case UHF_SET_RX_PKT_CFG_REQ   :
                {
                    os_memcpy(&tx_buff[len],&uhf_pkt_cfg,(sizeof(s_uhf_pkt_cfg)));
                    len += sizeof(s_uhf_pkt_cfg);
                }
                break;

            case UHF_GET_RX_PKT_CFG_REQ   :
                {
                }
                break;

            case UHF_SET_PWR_CFG_REQ      :
                {
                    os_memcpy(&tx_buff[len],&uhf_pa_cfg,(sizeof(s_uhf_pa_cfg)));
                    len += sizeof(s_uhf_pa_cfg);
                }
                break;

            case UHF_GET_PWR_CFG_REQ      :
                {
                }
                break;

            case UHF_SET_BCON_CFG_STRT     :
                {

                }
                break;

            case UHF_SET_BCON_CFG_STP     :
                {

                }
                break;

            case UHF_GET_ADDR_CFG_REQ     :
                {
                }
                break;

            case UHF_UART_DEBUG_OFF:
                {
                    DEBUG_CPRINT(("\n\r [UHF] UART Debug OFF \n "));
                }
                break;


            case UHF_UART_DEBUG_ON:
                {
                    DEBUG_CPRINT(("\n\r [UHF] UART Debug ON \n "));
                }
                break;
        }

        DEBUG_CPRINT(("[UHF] TX length: %d",len));
        log_id_with_one_arg(LOG_INFO_L1,LOG_COMMS,UHF_TX_LENGTH__u16_,len);

#ifndef LINUX_TEMP_PORT
        uhf_csp_uart_cmd_tx(tx_buff, len);
        os_timer_start(uhf_cmd_timeout, UHF_CMD_TIM_OUT_TIMER);
#else
        uhf_sim_pack_cmd_rsp(cmd_id);
#endif

    }
    else
    {
        comms_uhf_cfg_nvm_write();
    }


}

/**
 * @brief Send UHF received packet over UART. 
 * 
 * Sends a received UHF packet from the radio module to the host MCU over UART.
 * 
 * @param[in] frame Pointer to the received packet data. 
 * @param[in] uhf_packet_length Length of the received packet.
 */
void uhf_send_rx_uart_cmd(uint8_t *frame,uint8_t uhf_packet_length)
{
    uhf_send_ipc_pld(UHF_UART_CMD_RSP,COMMS_UHF_CTLR,uhf_packet_length,frame);
}


/**
 * @brief Process UHF command response from UART.
 * 
 * Parses a UHF radio command response received over UART.
 * 
 * @param[in] pld Pointer to the UART response payload data.
 * @return UHF command response status.
 */
uint8_t uhf_proc_uart_cmd_rsp(uint8_t *pld)
{
    s_comms_uhf_uart_data *uhf_rx_data = (s_comms_uhf_uart_data *)pld;


    DEBUG_CPRINT(("\n\r [UHF] Command response : %x, length: %d",uhf_rx_data->header.command,uhf_rx_data->length));
    log_id_with_two_arg(LOG_INFO_L1,LOG_COMMS,UHF_COMMAND_RESPONSE__hex__LENGTH__u16_,uhf_rx_data->header.command,uhf_rx_data->length);

    switch(uhf_rx_data->header.command)
    {
        case UHF_GET_TX_RADIO_CFG_RSP :
            {
                os_memcpy(&uhf_rf_cfg_rb,uhf_rx_data->data,uhf_rx_data->length);
                sdr_uhf_get_rf_tx_cfg();
                uhf_rx_data->length = sizeof(s_uhf_rf_tx_cfg);

                if(TC_COMMS_UHF_GET_TX_CFG == uhf_tc_tm_id)
                {
                    comms_uhf_tc_tm_rsp_hdlr(TM_COMMS_UHF_GET_TX_CFG, uhf_rx_data->length);
                }
                else
                {
                    //error or warning log
                }
            }
            break;

        case UHF_GET_RX_RADIO_CFG_RSP :
            {
                os_memcpy(&uhf_rf_cfg_rb,uhf_rx_data->data,uhf_rx_data->length);
                sdr_uhf_get_rf_rx_cfg();
                uhf_rx_data->length = sizeof(s_uhf_rf_rx_cfg);

                if(TC_COMMS_UHF_GET_RX_CFG == uhf_tc_tm_id)
                {
                    comms_uhf_tc_tm_rsp_hdlr(TM_COMMS_UHF_GET_RX_CFG, uhf_rx_data->length);
                }
                else
                {
                    //error or warning log
                }
            }
            break;

        case UHF_GET_TX_PKT_CFG_RSP   :
            {
                os_memcpy(&uhf_pkt_cfg_rb,uhf_rx_data->data,uhf_rx_data->length);
                sdr_uhf_get_tx_pkt_cfg();
                uhf_rx_data->length = sizeof(s_uhf_rf_tx_pkt_cfg);

                if(TC_COMMS_UHF_GET_TX_PKT_CFG == uhf_tc_tm_id)
                {
                    comms_uhf_tc_tm_rsp_hdlr(TM_COMMS_UHF_GET_TX_PKT_CFG, uhf_rx_data->length);
                }
                else
                {
                    //error or warning log
                }
            }
            break;

        case UHF_GET_RX_PKT_CFG_RSP   :
            {
                os_memcpy(&uhf_pkt_cfg_rb,uhf_rx_data->data,uhf_rx_data->length);
                sdr_uhf_get_rx_pkt_cfg();
                uhf_rx_data->length = sizeof(s_uhf_rf_rx_pkt_cfg);

                if(TC_COMMS_UHF_GET_RX_PKT_CFG == uhf_tc_tm_id)
                {
                    comms_uhf_tc_tm_rsp_hdlr(TM_COMMS_UHF_GET_RX_PKT_CFG, uhf_rx_data->length);
                }
                else
                {
                    //error or warning log
                }
            }
            break;

        case UHF_GET_PWR_CFG_RSP      :
            {
                os_memcpy(&uhf_pa_cfg_rb,uhf_rx_data->data,uhf_rx_data->length);
                sdr_uhf_get_pwr_cfg();
                uhf_rx_data->length = sizeof(s_uhf_rf_pwr_cfg);

                if(TC_COMMS_UHF_GET_PWR_CFG == uhf_tc_tm_id)
                {
                    comms_uhf_tc_tm_rsp_hdlr(TM_COMMS_UHF_GET_PWR_CFG, uhf_rx_data->length);
                }
                else
                {
                    //error or warning log
                }
            }
            break;

        case UHF_SET_TX_RADIO_CFG_ACK :
            {
                if(TC_COMMS_UHF_SET_TX_CFG == uhf_tc_tm_id)
                {
                    comms_uhf_csw_rx_buf[TM_PLD_IDX] = 0;
                    comms_uhf_tc_tm_rsp_hdlr(TM_COMMS_UHF_SET_TX_CFG, uhf_rx_data->length);
                }
                else
                {
                    //error or warning log
                }
            }
            break;

        case UHF_SET_RX_RADIO_CFG_ACK :
            {
                if(TC_COMMS_UHF_SET_RX_CFG == uhf_tc_tm_id)
                {
                    comms_uhf_csw_rx_buf[TM_PLD_IDX] = 0;
                    comms_uhf_tc_tm_rsp_hdlr(TM_COMMS_UHF_SET_RX_CFG, uhf_rx_data->length);
                }
                else
                {
                    //error or warning log
                }
            }
            break;

        case UHF_SET_TX_PKT_CFG_ACK   :
            {
                if(TC_COMMS_UHF_SET_TX_PKT_CFG == uhf_tc_tm_id)
                {
                    comms_uhf_csw_rx_buf[TM_PLD_IDX] = 0;
                    comms_uhf_tc_tm_rsp_hdlr(TM_COMMS_UHF_SET_TX_PKT_CFG, uhf_rx_data->length);
                }
                else
                {
                    //error or warning log
                }
            }
            break;

        case UHF_SET_RX_PKT_CFG_ACK   :
            {
                if(TC_COMMS_UHF_SET_RX_PKT_CFG == uhf_tc_tm_id)
                {
                    comms_uhf_csw_rx_buf[TM_PLD_IDX] = 0;
                    comms_uhf_tc_tm_rsp_hdlr(TM_COMMS_UHF_SET_RX_PKT_CFG, uhf_rx_data->length);
                }
                else
                {
                    //error or warning log
                }
            }
            break;

        case UHF_SET_PWR_CFG_ACK      :
            {
                if(TC_COMMS_UHF_SET_PWR_CFG == uhf_tc_tm_id)
                {
                    comms_uhf_csw_rx_buf[TM_PLD_IDX] = 0;
                    comms_uhf_tc_tm_rsp_hdlr(TM_COMMS_UHF_SET_PWR_CFG, uhf_rx_data->length);
                }
                else
                {
                    //error or warning log
                }
            }
            break;

        case UHF_SET_BCON_CFG_STRT: //UHF_SET_ADDR_CFG_ACK     :
            {

            }
            break;

        case UHF_SET_BCON_CFG_STP: //UHF_SET_ADDR_CFG_REQ     :
            {

            }
            break;

            /*   case UHF_RADIO_MSG_CALLSIGN:
                 {
                 os_memcpy(&comms_uhf_csw_rx_buf[TM_PLD_IDX],uhf_rx_data->data,uhf_rx_data->length);

                 if(TC_COMMS_UHF_GET_CALLSIGN == uhf_tc_tm_id)
                 {
                 comms_uhf_tc_tm_rsp_hdlr(uhf_tc_tm_id, uhf_rx_data->length);
                 }
                 else
                 {
            //error or warning log
            }
            }
            break;*/

        case UHF_CMN_MSG_ASCII:
            {
                //os_memcpy(&comms_uhf_csw_rx_buf[TM_PLD_IDX],uhf_rx_data->data,uhf_rx_data->length);
                int8 res;

                uint8 beacon_init_str[] = {"2269170101000001405265626f6f7420436f6d6d656e63656421"};
                res = memcmp(beacon_init_str, uhf_rx_data->data,18);
                if (!res)
                {

                    DEBUG_CPRINT(("The UHF Board Reset the value is %c",beacon_init_str[16]));
                }
                DEBUG_CPRINT(("\n\r [UHF] ASCII message is received == %s\n ",uhf_rx_data->data));

            }
            break;

        case UHF_BOOTLOADER_MSG_ACK:
            {
                comms_uhf_csw_rx_buf[TM_PLD_IDX] = 0;
                os_memcpy(&comms_uhf_csw_rx_buf[TM_PLD_IDX],uhf_rx_data->data,uhf_rx_data->length);
            }
            break;

        case UHF_BOOTLOADER_MSG_NACK: //case won't hit here
            {
                comms_uhf_csw_rx_buf[TM_PLD_IDX] = 1;
                os_memcpy(&comms_uhf_csw_rx_buf[TM_PLD_IDX],uhf_rx_data->data,uhf_rx_data->length);
            }
            break;

        case UHF_CMN_MSG_ACK:
            {
                comms_uhf_csw_rx_buf[TM_PLD_IDX] = 0;
                os_memcpy(&comms_uhf_csw_rx_buf[TM_PLD_IDX],uhf_rx_data->data,uhf_rx_data->length);
            }
            break;

        case UHF_RADIO_MSG_RANGING_ACK:
            {
                comms_uhf_csw_rx_buf[TM_PLD_IDX] = 0;
                comms_uhf_tc_tm_rsp_hdlr(uhf_tc_tm_id, uhf_rx_data->length);
            }
            break;


        case UHF_RADIO_MSG_TELEM:
            {
                s_comms_uhf_tm_prop *tm_info = (s_comms_uhf_tm_prop *)uhf_rx_data->data;

                /** TODO prints */
                tm_info = tm_info;

                if(uhf_tc_tm_id == TC_COMMS_UHF_GET_TM_INFO)
                {
                    os_memcpy(&comms_uhf_csw_rx_buf[TM_PLD_IDX],uhf_rx_data->data,uhf_rx_data->length);
                    comms_uhf_tc_tm_rsp_hdlr(TM_COMMS_UHF_GET_TM_INFO, uhf_rx_data->length);
                }
                else
                {
                    os_memcpy(&comms_uhf_tm_read.comms_uhf_tm_stor_time[uhf_hk_upd_id].comms_uhf_tm_stor,uhf_rx_data->data,uhf_rx_data->length);

                    comms_uhf_tm_read.comms_uhf_tm_stor_time[uhf_hk_upd_id].time_stamp=get_crnt_time_date_in_epoch();

                    uint8_t *uhf_buff_fw_data = os_malloc(sizeof(s_comms_uhf_get_tm_prop_time));
                        
                    if(uhf_buff_fw_data)
                    {
                        os_memcpy(uhf_buff_fw_data,&comms_uhf_tm_read.comms_uhf_tm_stor_time[uhf_hk_upd_id],sizeof(s_comms_uhf_get_tm_prop_time));
                        tc_uhf_buff_fw_enqueue_req(uhf_buff_fw_data,UHF_TM_PROP_QUEUE_ID);
                    }
                    else
                    {
                        //Malloc error
                    }
                }
            }
            break;

        case UHF_RADIO_MSG_GET_CALLSIGN:
            {
                os_memcpy(&comms_uhf_csw_rx_buf[TM_PLD_IDX],uhf_rx_data->data,uhf_rx_data->length);
                comms_uhf_tc_tm_rsp_hdlr(uhf_tc_tm_id, uhf_rx_data->length);
            }
            break;

        case UHF_RADIO_MSG_GET_CFG:
            {
                os_memcpy(&comms_uhf_csw_rx_buf[TM_PLD_IDX],uhf_rx_data->data,uhf_rx_data->length);
                comms_uhf_tc_tm_rsp_hdlr(uhf_tc_tm_id, uhf_rx_data->length);
            }
            break;

        case UHF_REBOOT_COMMENCED:
            {
                DEBUG_CPRINT(("\n\r [UHF] Reboot Commenced \n "));
            }
            break;

        case UHF_BEACON_MODE_STOPPED_OBC:
            {
                DEBUG_CPRINT(("\n\r [UHF] Beacon mode Stopped OBC \n "));
            }
            break;

        case UHF_BEACON_MODE_STOPPED_TIMEOUT:
            {
                DEBUG_CPRINT(("\n\r [UHF] Beacon mode Stopped Timeout \n "));
            }
            break;

        case UHF_TX_CPLT:
            {
                DEBUG_CPRINT(("\n\r [UHF] TX cplt \n "));
            }
            break;

        case UHF_INVALID_CMD:
            {
                DEBUG_CPRINT(("\n\r [UHF] Invalid command \n "));
            }
            break;

        case UHF_MSG_LENGTH_SMALL:
            {
                DEBUG_CPRINT(("\n\r [UHF] Msg length small \n "));
            }
            break;

        case UHF_BEACON_MODE_RUNNING:
            {
                DEBUG_CPRINT(("\n\r [UHF] Beacon mode running \n "));
                if(!beacon_op)
                {
                    uhf_upd_send_uart_cmd(UHF_SET_BCON_CFG_STP);
                }
            }
            break;

        case UHF_DEBUG_RX_FRAME_DETECTED:
            {
                DEBUG_CPRINT(("\n\r [UHF] Debug RX frame detected \n "));
                uhf_upd_send_uart_cmd(UHF_UART_DEBUG_OFF);
            }
            break;

        case UHF_DEBUG_RX_CPLT:
            {
                DEBUG_CPRINT(("\n\r [UHF] RX cplt\n "));
            }
            break;

        case UHF_TX_UNDERFLOW:
            {
                DEBUG_CPRINT(("\n\r [UHF] TX underflow \n "));
            }
            break;

        case UHF_RX_OVERFLOW:
            {
                DEBUG_CPRINT(("\n\r [UHF] RX overflow \n "));
            }
            break;

        case UHF_RX_CRC_ERROR:
            {
                DEBUG_CPRINT(("\n\r [UHF] RX CRC error \n "));
            }
            break;

        case UHF_CMN_MSG_NACK:
            {
                err_detected(ERR_GROUP_COMMS, ERR_COMMS_NACK_RCV_UHF);
                os_timer_start(uhf_cmd_timeout, UHF_CMD_RESEND_TIMER);

                comms_uhf_csw_rx_buf[TM_PLD_IDX] = 1;
                os_memcpy(&comms_uhf_csw_rx_buf[TM_PLD_IDX],uhf_rx_data->data,uhf_rx_data->length);
            }
            break;

    }
    DEBUG_CPRINT(("\n\r[UHF] TM length: %d",uhf_rx_data->length));
    log_id_with_one_arg(LOG_INFO_L1,LOG_COMMS,UHF_TM_LENGTH__u16_,uhf_rx_data->length);

    return uhf_rx_data->header.command;
}

/**
 * @brief This function is used for packing the UART data to send via UHF module
 * @brief Pack CSP packet into UHF UART frame format.
 * 
 * Takes a CSP packet and packs it into the UHF UART frame format.
 * 
 * @param[in] packet Pointer to CSP packet to pack. 
 * @param[out] frame_buff Buffer to store packed UHF UART frame.
 */
void uhf_pack_uart_csp_frame(csp_packet_t *packet, uint8_t *frame_buff)
{
    uint16_t length = 0;
    uint32_t id;
    s_comms_uhf_data *uhf_data = (s_comms_uhf_data *)frame_buff;

    uhf_data->header.sync1 = UHF_START_BYTE_0;
    uhf_data->header.sync2 = UHF_START_BYTE_1;
    uhf_data->header.length = packet->length + UHF_HEADER_SIZE + CSP_HEADER_LENGTH;
    uhf_data->header.hwid = 15;
    uhf_data->header.seqnum = uhf_data_seq_num;
    uhf_data->header.system = 0;
    uhf_data->header.command = 0;//uhf_tc_cmd_id ;

    uhf_data_seq_num = (uhf_data_seq_num + 1) & 0xFFFF;

    id = csp_hton32(packet->id.ext);

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

    memcpy(&uhf_data->data[length], packet->data, packet->length);
    length += packet->length;

    //    uhf_tc_cmd_id = 0;

    int i = 0;
    DEBUG_CPRINT(("\nUHF CSP pack: packet_len:%d, p_packet: ",uhf_data->header.length));
    log_id_with_one_arg(LOG_INFO_L1,LOG_COMMS,CSP_PACKET_LEN__u16__P_PACKET_,uhf_data->header.length);
    for(i=0;i<uhf_data->header.length+3;i++)
    {
        DEBUG_CPRINT(("%x ",frame_buff[i]));
    }

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

}

/**
 * @brief This function is used for unpacking the UART data to send via UHF module
 * @brief Unpack UHF UART frame into CSP packet.
 * 
 * Takes a UHF UART frame and unpacks it into a CSP packet structure.
 * 
 * @param[out] packet CSP packet structure to unpack frame into.
 * @param[in] frame_buff Buffer containing UHF UART frame to unpack.
 */
void uhf_unpack_uart_csp_frame(csp_packet_t *packet, uint8_t *frame_buff)
{
    uint16_t length = 0;
    uint32_t id;
    s_comms_uhf_data *uhf_data = (s_comms_uhf_data *)frame_buff;

    length = uhf_data->header.length - UHF_HEADER_SIZE - CSP_HEADER_LENGTH;

    packet->length = length;

    length = 0;

    //if (!packet)
    //continue;

    /* Set packet ID */
    memcpy(&id, &uhf_data->data[length], sizeof(id));
    length += sizeof(id);
    packet->id.ext = csp_ntoh32(id);

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

    int i = 0;
    DEBUG_CPRINT(("\nUHF CSP unpack: packet_len:%d, u_packet: ",uhf_data->header.length));
    log_id_with_one_arg(LOG_INFO_L1,LOG_COMMS,CSP_UNPACK_PACKET_LEN__u16__U_PACKET_,uhf_data->header.length);
    for(i=0;i<uhf_data->header.length;i++)
    {
        DEBUG_CPRINT(("%x ",frame_buff[i]));
    }
    DEBUG_CPRINT(("\n\r"));
    DEBUG_CPRINT(("\n\r"));
}


/**
 * @brief This function is used for packing the UART data to send via UHF module
 * @brief Pack CSP packet into generic UHF UART frame.
 * 
 * Packs a CSP packet into a generic UHF UART frame format.
 * 
 * @param[in] packet Pointer to CSP packet to pack.
 * @param[out] frame_buff Buffer to store packed UHF UART frame. 
 */
void uhf_pack_uart_frame(csp_packet_t *packet, uint8_t *frame_buff)
{
    s_comms_uhf_data *uhf_data = (s_comms_uhf_data *)frame_buff;

    packet->length = packet->length - 1;

    uhf_data->header.sync1 = UHF_START_BYTE_0;
    uhf_data->header.sync2 = UHF_START_BYTE_1;
    uhf_data->header.length = packet->length + UHF_HEADER_SIZE ;
    uhf_data->header.seqnum = uhf_seq_num;

    uhf_seq_num = (uhf_seq_num + 1) &0xFFFF;

    os_memcpy(&uhf_data->header.command,packet->data,sizeof(uhf_data->header.command)) ;

    uhf_data->header.hwid = 1;//0xFFFF;
    uhf_data->header.system = 1;

    os_memcpy(&uhf_data->data[0], &packet->data[1], packet->length);

    int i = 0;
    get_rtc_time(&get_t_uhf);
    DEBUG_CPRINT(("\nUHF RTC Time-%d:%d:%d",get_t_uhf.hour, get_t_uhf.min, get_t_uhf.sec));
    log_id_with_three_arg(LOG_INFO_L2,LOG_COMMS,UHF_RTC_TIME__u8__u8__u8_,get_t_uhf.hour, get_t_uhf.min, get_t_uhf.sec);
    DEBUG_CPRINT(("\n\rUHF HWID: %d",uhf_data->header.hwid));
    log_id_with_one_arg(LOG_INFO_L1,LOG_COMMS,UHF_HWID__u8_,uhf_data->header.hwid);
    DEBUG_CPRINT(("\n\rUHF pack: packet_len:%d, p_packet: ",uhf_data->header.length));
    log_id_with_one_arg(LOG_INFO_L1,LOG_COMMS,UHF_PACK_PACKET_LEN__u16__P_PACKET_,uhf_data->header.length);

    for(i=0;i<uhf_data->header.length+3;i++)
    {
        DEBUG_CPRINT(("%x ",frame_buff[i]));
    }

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


    DEBUG_CPRINT(("\n"));

}

/**
 * @brief This function is used for unpacking the UART data to send via UHF module
 * @brief Unpack generic UHF UART frame into CSP packet.
 * 
 * Unpacks a generic UHF UART frame into a CSP packet structure.
 * 
 * @param[out] packet CSP packet structure to unpack frame into.
 * @param[in] frame_buff Buffer containing UHF UART frame to unpack.
 */
void uhf_unpack_uart_frame(csp_packet_t *packet, uint8_t *frame_buff)
{
    uint16_t length = 0;
    uint32_t id;
    s_comms_uhf_uart_data *uhf_data = (s_comms_uhf_uart_data *)frame_buff;

    length = uhf_data->length - UHF_HEADER_SIZE - CSP_HEADER_LENGTH;

    packet->length = length;

    length = 0;

    //if (!packet)
    //continue;

    /* Set packet ID */
    memcpy(&id, &uhf_data->data[length], sizeof(id));
    length += sizeof(id);
    packet->id.ext = csp_ntoh32(id);

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

    int i = 0;

    get_rtc_time(&get_t_uhf);
    DEBUG_CPRINT(("\nUHF RTC Time-%d:%d:%d\n",get_t_uhf.hour, get_t_uhf.min, get_t_uhf.sec));
    log_id_with_three_arg(LOG_INFO_L2,LOG_COMMS,UHF_RTC_TIME__u8__u8__u8_,get_t_uhf.hour, get_t_uhf.min, get_t_uhf.sec);
    DEBUG_CPRINT(("\nUHF unpack: packet_len:%d, u_packet: ",uhf_data->length));
    log_id_with_one_arg(LOG_INFO_L1,LOG_COMMS,UHF_UNPACK_PACKET_LEN__u16__U_PACKET_,uhf_data->length);
    for(i=0;i<uhf_data->length;i++)
    {
        DEBUG_CPRINT(("%x ",frame_buff[i]));
    }

    DEBUG_CPRINT(("\n\npacket->length:%d\r",packet->length));
    DEBUG_CPRINT(("\n\r"));
    DEBUG_CPRINT(("\n\r"));

    DEBUG_CPRINT(("\n"));
}

/**
 * @brief This function is used for sending CSP ping for S-Band Module
 * @brief Send UHF ping command.
 * 
 * Sends a ping command to the UHF radio module.
 * 
 * @param[in] tc_cmd Pointer to ping command buffer.
 */
void uhf_csp_ping(uint8_t *tc_cmd)
{
    uint8_t node;
    uint32_t size;
    uint32_t ping_count;
    uint32_t ping_time_out;
    uint32_t csp_ping_count = 0;
    int32_t result = 0;

    os_memcpy(&node,tc_cmd,1);
    os_memcpy(&size,&tc_cmd[1],4);
    os_memcpy(&ping_count,&tc_cmd[5],4);
    os_memcpy(&ping_time_out,&tc_cmd[9],4);

    /* Send ping to server, timeout 1000 mS, ping size 100 bytes */
    DEBUG_CPRINT(("\t\r\n Ping Start Time= "));
csp_ping:
    result = csp_ping(node, ping_time_out, size, CSP_O_NONE);
    //DEBUG_CPRINT(("\t CSP PING address: %u, result %d [mS]", node, result));

    if(result > 0)
    {
        DEBUG_CPRINT(("\n\r CSP PING SUCCESS\n"));
    }
    else
    {
        DEBUG_CPRINT(("\n\r CSP PING FAILURE\n"));
    }

    csp_ping_count++;
    if(csp_ping_count < ping_count)
    {
        goto csp_ping;
    }
    log_id(LOG_INFO_L2,LOG_COMMS,CSP_PING_TEST_COMPLETE);
    DEBUG_CPRINT(("\n\r CSP PING TEST Complete\n"));
}

/**
 * @brief This function is used for sending CSP transmission data from the S-Band
 * @brief Send CSP data over UHF.
 * 
 * Takes a CSP packet and transmits it over UHF.
 */
void uhf_csp_tx_data(void)
{
    /* Send data packet (string) to server */

    /* 1. Connect to host on 'SRS4_SPACE_ADDRESS', port MY_SERVER_PORT with regular UDP-like protocol and 1000 ms timeout */
    csp_conn_t * conn = csp_connect(CSP_PRIO_NORM, UHF_UART_TX_ADDRESS , UHF_CSP_TX_DATA_PORT ,1000, CSP_O_NONE);

    /** Connection used for Space data transfer from S-Band module */
    //csp_conn_t * conn = csp_connect(CSP_PRIO_NORM, 10, 11, 1000, CSP_O_NONE);
    if (conn == NULL) {
        /* Connect failed */
        DEBUG_CPRINT(("\r\n\rConnection failed\n"));
        log_id(LOG_ERROR,LOG_COMMS,CONNECTION_FAILED);
        return;
    }

    /* 2. Get packet buffer for message/data */
    csp_packet_t * packet = csp_buffer_get(100);
    if (packet == NULL) {
        /* Could not get buffer element */
        DEBUG_CPRINT(("\r\n\rFailed to get CSP buffer\n"));
        log_id(LOG_ERROR,LOG_COMMS,FAILD_TO_GET_CSP_BUFFER);
        return;
    }

    /* 3. Copy data to packet */
    memcpy(packet->data, "Hello world ", 12);
    memcpy(packet->data + 12, &csp_uhf_data_count, 1);
    memset(packet->data + 13, 0, 1);
    csp_uhf_data_count++;

    /* 4. Set packet length */
    packet->length = (strlen((char *) packet->data) + 1); /* include the 0 termination */

    /* 5. Send packet */
    csp_send(conn, packet,0xFFFF);
    DEBUG_CPRINT(("\tPacket Sent - %s",packet->data));
    log_string(LOG_INFO_L1,LOG_COMMS,"PACKET_SENT__s8_");
    log_string(LOG_INFO_L1,LOG_COMMS,(char *)packet->data);
    /* 6. Close connection */

    csp_close(conn);
}

/**
 * @brief Send UHF command over UART.
 * 
 * Sends a UHF radio command to the module over UART.
 * 
 * @param[in] data Pointer to command data buffer. 
 * @param[in] len Length of command data in bytes.
 * @return 0 on success, negative error code on failure.
 */ 
int32_t uhf_csp_uart_cmd_tx(uint8_t* 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 = UHF_MAX_PLD;

    conn = csp_connect(CSP_PRIO_NORM, UHF_CMD_TX_ADDRESS, UHF_CSP_TX_DATA_PORT , DEFAULT_TIMEOUT, CSP_O_NONE);
    if (!conn)
        return -ECONNREFUSED;

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

    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, &data[sent], datasize);
        packet->length = datasize;

        int i=0,j = 0;
        DEBUG_CPRINT(("UHF command data size is %d\n data: ", (uint16_t)datasize));
        log_id_with_one_arg(LOG_INFO_L1,LOG_COMMS,UHF_COMMAND_DATA_SIZE__u16_,(uint16_t)datasize);

        for(i = sent; i<=datasize; i++)
        {
            DEBUG_CPRINT(("%x ",data[i]));
        }
        DEBUG_CPRINT(("\n UHF command packet->length is %d\r\n", packet->length));
        log_id_with_one_arg(LOG_INFO_L1,LOG_COMMS,UHF_COMMAND_PACKET_LENGTH__u16_,packet->length);

        //DEBUG_CPRINT(("\n UHF command 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;*/

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

    csp_buffer_free(packet);

    csp_close(conn);

    return 0;
}

/**
 * @brief Process UHF firmware update sequence.
 * 
 * Handles the UHF firmware update sequence by processing 
 * responses and sending next commands.
 * 
 * @param[in] pld Pointer to firmware update response payload.
 * @return Next firmware update sequence step.
 */
uint8_t uhf_fw_update_seq(uint8_t *pld)
{
    uint16_t len = 0;
    uint8_t success = 0;
    s_comms_uhf_uart_data *uhf_rx_data = (s_comms_uhf_uart_data *)pld;

    DEBUG_CPRINT(("\n\r [UHF] Command response in fw update : %x, length: %d, CMD_ID : %x \n",uhf_rx_data->header.command,uhf_rx_data->length, uhf_tc_cmd_id));

    len = sizeof(uhf_tc_cmd_id);

    switch(uhf_rx_data->header.command)
    {

        case UHF_TX_CPLT:
        case UHF_BEACON_MODE_RUNNING:
        case UHF_CMN_MSG_ACK:
        case UHF_CMN_MSG_ASCII:
        case UHF_REBOOT_COMMENCED:
            {

                if(UHF_RADIO_MSG_REBOOT == uhf_tc_cmd_id)
                {
                    uhf_tc_cmd_id = UHF_SET_BCON_CFG_STP;  //temp change
                    DEBUG_CPRINT(("\n\r [UHF] ASCII message -> REBOOT, %d", uhf_tc_cmd_id));
                }

                else if(UHF_SET_BCON_CFG_STP  == uhf_tc_cmd_id)
                {
                    uhf_tc_cmd_id = UHF_BOOTLOADER_MSG_ERASE;  //temp change
                    DEBUG_CPRINT(("\n\r [UHF] ASCII message -> BCON STP, %d", uhf_tc_cmd_id));
                }

                else if (UHF_BOOTLOADER_MSG_ERASE == uhf_tc_cmd_id)
                {
                    uhf_tc_cmd_id = UHF_RADIO_MSG_REBOOT;
                    DEBUG_CPRINT(("\n\r [UHF] ASCII message -> ERASE, %d", uhf_tc_cmd_id));
                }

                /*
                   else if(UHF_BOOTLOADER_MSG_PING == uhf_tc_cmd_id)
                   {
                   cmd_id = UHF_RADIO_MSG_REBOOT;
                   }*/
            }
            break;

        case UHF_CMN_MSG_NACK:
            {
                if(UHF_RADIO_MSG_REBOOT == uhf_tc_cmd_id)
                {
                    DEBUG_CPRINT(("\n\r [UHF] BTL NACK message ---- ERASE"));
                    uhf_tc_cmd_id = UHF_BOOTLOADER_MSG_ERASE;
                }
                else if (UHF_BOOTLOADER_MSG_ERASE == uhf_tc_cmd_id)
                {
                    DEBUG_CPRINT(("\n\r [UHF] BTL NACK message ---- ERASE"));
                    uhf_tc_cmd_id = UHF_BOOTLOADER_MSG_ERASE;
                }
            }
            break;

        case UHF_BOOTLOADER_MSG_ACK:
            {
                os_timer_stop(uhf_cmd_timeout);
                if(UHF_BOOTLOADER_MSG_ERASE == uhf_tc_cmd_id)
                {
                    uhf_tc_cmd_id = UHF_BOOTLOADER_MSG_WRITE_PAGE;

                    DEBUG_CPRINT(("\n\r [UHF] BTL ACK message -> WRITE, %d, PAGE = %d", uhf_tc_cmd_id, uhf_fw_page_num));
                    uhf_fw_page = FLASH_APP_START;
                    uhf_fw_page_num = uhf_fw_page/FLASH_PAGE_SIZE;

                    os_memcpy(&tx_buff[1], &uhf_fw_page_num, sizeof(uint8_t));

                    os_memcpy(&tx_buff[2], &outbuff[uhf_fw_page], FLASH_PAGE_SIZE);

                    len += FLASH_PAGE_SIZE + sizeof(uint8_t);
                }
                else if(UHF_BOOTLOADER_MSG_WRITE_PAGE == uhf_tc_cmd_id)
                {
DO_SKIP:

                    uhf_fw_page += FLASH_PAGE_SIZE;
                    uhf_fw_page_num = uhf_fw_page/FLASH_PAGE_SIZE;

                    if(uhf_fw_page < FLASH_APP_END)
                    {
                        uhf_tc_cmd_id = UHF_BOOTLOADER_MSG_WRITE_PAGE;
                        DEBUG_CPRINT(("\n [UHF] BTL WRITE message -> WRITE, %d, PAGE = %d", uhf_tc_cmd_id, uhf_fw_page_num ));

                        os_memcpy(&tx_buff[1], &uhf_fw_page_num, sizeof(uint8_t));
                        os_memcpy(&tx_buff[2], &outbuff[uhf_fw_page], FLASH_PAGE_SIZE);

                        if(is_page_empty())
                        {
                            uhf_seq_num ++;
                            goto DO_SKIP;
                        }
                        len += FLASH_PAGE_SIZE + sizeof(uint8_t);
                    }
                    else if((uhf_fw_page_num > 0xD7) && (uhf_fw_page_num != 0xFF))
                    {
                        DEBUG_CPRINT(("\n\r [UHF] BTL WRITE message -> SUCCESS, %d, PAGE = %d", uhf_tc_cmd_id, uhf_fw_page_num ));
                        uhf_tc_cmd_id = UHF_BOOTLOADER_MSG_WRITE_PAGE;
                        uhf_fw_page_num = 0xFF;
                        os_memcpy(&tx_buff[1], &uhf_fw_page_num, sizeof(uint8_t));
                        len += sizeof(uint8_t);
                        DEBUG_CPRINT(("lst bootloader_write_page 255 \n"));

                    }
                    else
                    {
                        DEBUG_CPRINT(("\n[UHF] uhf_fw_page_num: %d, uhf_tc_cmd_id:%d\n",uhf_fw_page_num,uhf_tc_cmd_id));
                        len = 0;
                    }
                    os_delay(10);
                }
            }
            break;

        case UHF_BOOTLOADER_MSG_NACK:
            {
                if(UHF_BOOTLOADER_MSG_ERASE == uhf_tc_cmd_id)
                {
                    //uhf_tc_cmd_id = uhf_tc_cmd_id;
                    len = uhf_tc_len;
                }
                else if(UHF_BOOTLOADER_MSG_WRITE_PAGE == uhf_tc_cmd_id)
                {
                    if((uhf_fw_page_num > 0xD7) && (uhf_fw_page_num == 0xFF))
                    {
                        DEBUG_CPRINT(("\n\r [UHF] BTL FW UPDATE -> SUCCESS, %d, PAGE = %d", uhf_tc_cmd_id, uhf_fw_page_num ));
                        uhf_fw_upd_sts = 1;

                        return success;
                        //uhf_tc_cmd_id = uhf_tc_cmd_id;
                    }
                    else if((uhf_fw_page_num <= 0xD7) && (uhf_fw_page_num != 0xFF))
                    {
                        uhf_tc_cmd_id = UHF_BOOTLOADER_MSG_WRITE_PAGE;
                        os_memcpy(&tx_buff[1], &uhf_fw_page_num, sizeof(uint8_t));
                        os_memcpy(&tx_buff[2], &outbuff[uhf_fw_page], FLASH_PAGE_SIZE);
                        len += FLASH_PAGE_SIZE + sizeof(uint8_t);

                        DEBUG_CPRINT(("\n\r [UHF] BTL NACK message -> WRITE, %d, PAGE = %d", uhf_tc_cmd_id, uhf_fw_page_num ));

                    }
                    else
                    {
                        DEBUG_CPRINT(("\n[UHF] uhf_fw_page_num: %d, uhf_tc_cmd_id:%d\n",uhf_fw_page_num,uhf_tc_cmd_id));
                        len = 0;
                    }
                }
            }
            break;

        default:
            DEBUG_CPRINT(("\n[UHF] Unhandled message received in boot state : %d\n",uhf_rx_data->header.command));
            break;
    }

    if(len)
    {
        uhf_tc_len = len;
        DEBUG_CPRINT(("\n\r [UHF] Command response --- IN BOOT CMD_ID : %x LEN %d\n",uhf_tc_cmd_id, len ));

        os_memcpy(tx_buff,&uhf_tc_cmd_id,sizeof(uint8_t));

#ifndef LINUX_TEMP_PORT
        uhf_csp_uart_cmd_tx(tx_buff, len);
#else
        uhf_sim_pack_cmd_rsp(uhf_tc_cmd_id);
#endif
    }

    return 1;
}

/**
 * @brief Check if UHF page buffer is empty.
 * 
 * Checks if the UHF firmware page buffer is empty.
 * 
 * @return 1 if empty, 0 if not empty.
 */
int8_t is_page_empty(void)
{
    uint8_t null_cnt = 0;
    uint8_t i = 0;
    uint8_t is_empty = 0;

    if (tx_buff[2] == 0xff) {
        for (i = 0; i < FLASH_PAGE_SIZE; i++) {
            if (tx_buff[i + 2] == 0xff){
                null_cnt = i;
            }
            else {
                is_empty = false;
                return is_empty;
            }
        }
        if(null_cnt == FLASH_PAGE_SIZE - 1)
        {
            is_empty = true;
            //DEBUG_CPRINT(("Page %d is empty, skipping \n", uhf_fw_page_num));
            uhf_seq_num++;
            tx_buff[1]++;
            return is_empty;
        }
    }
    return is_empty;
}


