mbtk/mbtk_gnssd/gnss_hd8122.c

/*
*    gnss_hd8122.c
*
*    HD8122 GNSS source.
*
*/
/******************************************************************************

                          EDIT HISTORY FOR FILE

  WHEN        WHO       WHAT,WHERE,WHY
--------    --------    -------------------------------------------------------
2024/6/14     LiuBin      Initial version

******************************************************************************/
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <errno.h>
#include <fcntl.h>
#include <pthread.h>

#include "mbtk_log.h"
#include "mbtk_type.h"
#include "mbtk_gpio.h"
#include "mbtk_utils.h"
#include "gnss_utils.h"
#include "gnss_hd8122.h"
#include "hd8122_dl/hd8040_upgrade.h"

#define UART_BITRATE_NMEA_DEF_FW    115200   // Default bitrate.
#define GNSS_POWER_GPIO 43
#define GNSS_SET_TIMEOUT           3000     // 3s
#define GNSS_PACK_BUFF_SIZE           1024
#define GNSS_MSG_NUM_MAX 30
#define READ_LEN_MAX 1024 //BOOT_UPGRADE_BUFF_MAX_1
#define GNSS_FW_GQALS_PATH "/lib/firmware/HD8122.YIKE.GN3.115200.0035.720e5.53ef0.GQALS.ANT.EPH.CFG.PPS13.240115R1.bin"
#define GNSS_FW_GAQBS_PATH "/lib/firmware/HD8122.YIKE.GN3.115200.0037.dbd12.53ef0.GAQBS.B1C.ANT.EPH.CFG.PPS13.240416R1.bin"

static pthread_cond_t read_cond;
static pthread_mutex_t read_mutex;
static bool setting_waitting = FALSE;
static bool setting_busy = FALSE;
static void *gnss_set_rsp_ptr = NULL;
static gnss_err_enum gnss_set_result = GNSS_ERR_OK;
static hd8122_msg_id_t msg_array[GNSS_MSG_NUM_MAX];
static char gnss_ctrl_path[128] = "/sys/devices/platform/mbtk-gnss/ctrl";


int gnss_write(int fd, const void *data, int data_len);
int OpenUart(char* UART_DEV);
int uart_close(int fd);
int fw_update_boot(int uart_fd, uint8_t *data, uint32_t len);

static uint16 fletcher16(const uint8_t* data, int data_len)
{
    uint32_t sum1 = 0;
    uint32_t sum2 = 0;
    int index;

    for (index = 0; index < data_len; ++index )
    {
        sum1 += data[index];
        sum2 += sum1;
    }

    return ((0xFF & sum2) << 8) | (0xFF & sum1);
}

static void gnss_set_timer_cb(int signo)
{
    if(setting_busy)
    {
        pthread_mutex_lock(&read_mutex);
        pthread_cond_signal(&read_cond);
        pthread_mutex_unlock(&read_mutex);
        gnss_set_result = GNSS_ERR_TIMEOUT;
    }
    return;
}

static void msg_init()
{
    unsigned int i = 0;
    while(i < ARRAY_SIZE(msg_array))
    {
        msg_array[i].enable = FALSE;
        i++;
    }
}

static int msg_insert(uint8 gid, uint8 sid)
{
    unsigned int i = 0;
    while(i < ARRAY_SIZE(msg_array))
    {
        if(!msg_array[i].enable)
            break;
        i++;
    }

    if(i == ARRAY_SIZE(msg_array))
    {
        LOGE("Msg full : %d", i);
        return -1;
    }
    else
    {
        msg_array[i].enable = TRUE;
        msg_array[i].gid = gid;
        msg_array[i].sid = sid;
        return 0;
    }
}

static int msg_find(uint8 gid, uint8 sid)
{
    unsigned int i = 0;
    while(i < ARRAY_SIZE(msg_array))
    {
        if(msg_array[i].enable && gid == msg_array[i].gid && sid == msg_array[i].sid)
            break;
        i++;
    }

    if(i == ARRAY_SIZE(msg_array))
    {
        LOGE("No found %d - %d", gid, sid);
        return -1;
    }
    else
    {
        return i;
    }
}

static int msg_remove(uint8 gid, uint8 sid)
{
    int i = msg_find(gid, sid);
    if(i >= 0)
    {
        msg_array[i].enable = FALSE;
        msg_array[i].gid = 0;
        msg_array[i].sid = 0;
        return 0;
    }
    else
    {
        return -1;
    }
}

static int msg_count()
{
    unsigned int i = 0;
    int count = 0;
    while(i < ARRAY_SIZE(msg_array))
    {
        if(msg_array[i].enable)
            count++;
        i++;
    }
    return count;
}

static int gnss_ctrl_write(char* str)
{
    int fd = -1;
    int ret = -1;
    fd = open((const char*)gnss_ctrl_path, O_WRONLY);
    if (fd < 0)
    {
        LOGE("open %s fail", gnss_ctrl_path);
        return -1;
    }
    else
    {
        ret = write(fd, str, strlen(str));
        close(fd);
        if(ret < 0)
        {
            LOGE("write %s fail", str);
            return -1;
        }
    }
    return 0;
}

static int gnss_fwdl_enable()
{
    int ret = -1;
	ret = gnss_ctrl_write("fdl");
    if(ret < 0)
    {
        LOGE("gnss_ctrl_write fail");
        return -1;
    }
    return 0;
}

static uint32_t read_bin_file(uint8_t *path, uint8_t *buff)
{
    int fp = -1;
    int ret = 0;
    int i = 0;
    int size = 0;

    if (NULL == path || NULL == buff)
    {
        LOGE("ARG error");
        return 0;
    }

    fp = open((char *)path, O_RDONLY);
    if(fp < 0)
    {
        LOGE( "open file failed ! errno is %d", errno);
        return 0;
    }

    size = lseek(fp, 0x00, SEEK_END);
    if(size <= 0)
    {
        LOGE( "file is empty.");
        return 0;
    }

    LOGD( "file size is:%d", size);
    lseek(fp, 0x00, SEEK_SET);
    while(1)
    {
        ret = read(fp, buff, READ_LEN_MAX);
        i += ret;
        if(ret == READ_LEN_MAX)
        {
            buff += READ_LEN_MAX;
        }
        else
        {
            break;
        }
    }

    LOGD("file size is:%d,i:%d", size, i);
    close(fp);
    if(size != i)
    {
        return 0;
    }
    return size;
}


static int pack_create(hd8122_id_type_enum id_type, uint8 id, uint16 data_len, const uint8 *data, uint8 *pack, int pack_len)
{
    if(pack == NULL || pack_len < HD8122_PACK_LEN_MIN)
    {
        return -1;
    }
    memset(pack, 0, pack_len);
    uint8 *data_ptr = pack;
    data_ptr += uint16_2_byte(HD8122_PACK_HEAD, data_ptr, false);
    *data_ptr++ = (uint8)id_type;
    *data_ptr++ = id;
    data_ptr += uint16_2_byte(data_len, data_ptr, false);
    if(data_len > 0)
    {
        memcpy(data_ptr, data, data_len);
        data_ptr += data_len;
    }
    data_ptr += uint16_2_byte(fletcher16(pack + 2, 4 + data_len), data_ptr, false);
    return (data_ptr - pack);
}

// f1 d9 05 01 02 00 06 01 0f 38
// or
// f1 d9 05 00 02 00 06 01 0f 38
static int msg_array_change(const uint8 *pack, int pack_len, hd8122_id_ack_enum *ack_nak)
{
    if(pack_len == 0 || pack_len % 10)
    {
        LOGE("pack_len(%d) error.", pack_len);
        return -1;
    }
    int count = pack_len / 10;
    int i = 0;
    while(i < count)
    {
        const uint8 *ptr = pack + i * 10;
        if(ptr[0] != 0xf1 || ptr[1] != 0xd9)
        {
            LOGE("Pack head error : %02x %02x", ptr[0], ptr[1]);
            return -1;
        }

        if(ptr[2] != 0x05)
        {
            LOGE("Type not 0x05 : %02x", ptr[2]);
            return -1;
        }

        int index = msg_find(ptr[6], ptr[7]);
        if(index >= 0)
        {
            if(ptr[3] == 0x01)
            {
                msg_array[index].ack_nak = HD8122_ID_ACK_ACK;
            }
            else if(ptr[3] == 0x00)
            {
                msg_array[index].ack_nak = HD8122_ID_ACK_NAK;

                // There is a nak as a failure.
                *ack_nak = HD8122_ID_ACK_NAK;
            }
            else
            {
                LOGE("ID not 0x00 or 0x01 : %02x", ptr[3]);
                return -1;
            }

            msg_array[index].enable = FALSE;
        }
        else
        {
            LOGE("Unknown gid - %d, sid - %d", ptr[6], ptr[7]);
            return -1;
        }
        i++;
    }

    return 0;
}

static void gnss_cmd_rsp_process(const void *data, int data_len)
{
    const char *ptr = (const char*)data;
    log_hex("RSP", data, data_len);

    hd8122_id_ack_enum ack_nak = HD8122_ID_ACK_ACK;
    if(!msg_array_change((const uint8*)data, data_len, &ack_nak))
    {
        if(setting_waitting && msg_count() == 0)
        {
            if(ack_nak == HD8122_ID_ACK_ACK)
            {
                gnss_set_result = GNSS_ERR_OK;
            }
            else
            {
                gnss_set_result = GNSS_ERR_UNKNOWN;
            }

            mbtk_timer_clear();

            pthread_mutex_lock(&read_mutex);
            pthread_cond_signal(&read_cond);
            pthread_mutex_unlock(&read_mutex);
            setting_waitting = FALSE;
        }
    }
    else
    {
        LOGW("Unknown rsp data.");
    }
}

static gnss_err_enum gnss_8122_reset(int fd, uint8 reset)
{
    uint8 buff[GNSS_PACK_BUFF_SIZE];
    LOGD("RESET");
    int len = pack_create(HD8122_ID_TYPE_CFG, HD8122_ID_CFG_SIMPLERST, 1, (uint8*)(&reset), buff, sizeof(buff));
    if(len <= 0)
    {
        LOGE("pack_create() fail.");
        return GNSS_ERR_ARG;
    }
    log_hex("PACK", buff, len);
    gnss_write(fd, buff, len);
    return GNSS_ERR_OK;
}

static gnss_err_enum gnss_8122_syscfg(int fd, uint32 mode)
{
    uint8 buff[GNSS_PACK_BUFF_SIZE];
    LOGD("SYSCFG");
    //uint8 mode_str[4];
    //uint32_2_byte(mode, mode_str, TRUE);
    int len = pack_create(HD8122_ID_TYPE_CFG, HD8122_ID_CFG_NAVSAT, 4, (uint8*)(&mode), buff, sizeof(buff));
    if(len <= 0)
    {
        LOGE("pack_create() fail.");
        return GNSS_ERR_ARG;
    }
    log_hex("PACK", buff, len);
    gnss_write(fd, buff, len);
    msg_insert(HD8122_ID_TYPE_CFG, HD8122_ID_CFG_NAVSAT);
    return GNSS_ERR_OK;
}

static gnss_err_enum gnss_8122_freqcfg(int fd, uint8 mode)
{
    uint8 buff[GNSS_PACK_BUFF_SIZE];
    LOGD("FREQCFG");
    uint8 data[20];
    memset(data, 0x00, 20);
    data[1] = mode;
    data[2] = 0x66;
    int len = pack_create(HD8122_ID_TYPE_CFG, HD8122_ID_CFG_PWRCTL, 20, data, buff, sizeof(buff));
    if(len <= 0)
    {
        LOGE("pack_create() fail.");
        return GNSS_ERR_ARG;
    }
    log_hex("PACK", buff, len);
    gnss_write(fd, buff, len);
    msg_insert(HD8122_ID_TYPE_CFG, HD8122_ID_CFG_PWRCTL);
    return GNSS_ERR_OK;
}

static gnss_err_enum gnss_8122_msgcfg(int fd, uint8 type, uint8 id, uint8 period)
{
    uint8 buff[GNSS_PACK_BUFF_SIZE];
    LOGD("MSGCFG");
    uint8 data[3];
    data[0] = type;
    data[1] = id;
    data[2] = period;
    int len = pack_create(HD8122_ID_TYPE_CFG, HD8122_ID_CFG_MSG, 3, data, buff, sizeof(buff));
    if(len <= 0)
    {
        LOGE("pack_create() fail.");
        return GNSS_ERR_ARG;
    }
    log_hex("PACK", buff, len);
    gnss_write(fd, buff, len);
    msg_insert(HD8122_ID_TYPE_CFG, HD8122_ID_CFG_MSG);
    return GNSS_ERR_OK;
}

static gnss_err_enum gnss_8122_minel(int fd, float *elev)
{
    uint8 buff[GNSS_PACK_BUFF_SIZE];
    LOGD("ELEV");
    //uint8 elev_buff[4];
    //uint32_2_byte((uint32)elev, elev_buff, TRUE);
    int len = pack_create(HD8122_ID_TYPE_CFG, HD8122_ID_CFG_ELEV, 8, (uint8*)elev, buff, sizeof(buff));
    if(len <= 0)
    {
        LOGE("pack_create() fail.");
        return GNSS_ERR_ARG;
    }
    log_hex("PACK", buff, len);
    gnss_write(fd, buff, len);
    return GNSS_ERR_OK;
}

static gnss_err_enum gnss_8122_nmeaver(int fd, uint8 ver)
{
    uint8 buff[GNSS_PACK_BUFF_SIZE];
    LOGD("NMEA-VER");
    int len = pack_create(HD8122_ID_TYPE_CFG, HD8122_ID_CFG_NMEAVER, 1, (uint8*)(&ver), buff, sizeof(buff));
    if(len <= 0)
    {
        LOGE("pack_create() fail.");
        return GNSS_ERR_ARG;
    }
    log_hex("PACK", buff, len);
    gnss_write(fd, buff, len);
    return GNSS_ERR_OK;
}

int gnss_8122_dev_open()
{
    //return mbtk_gpio_value_set(GNSS_POWER_GPIO, MBTK_GPIO_DIRECT_OUT, 1);
    system("i2cset -y -f 2 0x31 0x15 0x86");
    return 0;
}

int gnss_8122_dev_close(int fd)
{
    //return mbtk_gpio_value_set(GNSS_POWER_GPIO, MBTK_GPIO_DIRECT_OUT, 0);
    system("i2cset -y -f 2 0x31 0x15 0x00");
    return 0;
}

int gnss_8122_open(const char *dev)
{
    pthread_mutex_init(&read_mutex, NULL);
    pthread_cond_init(&read_cond, NULL);
    return gnss_port_open(dev, O_RDWR | O_NONBLOCK | O_NOCTTY, UART_BITRATE_NMEA_DEF_FW, TRUE);
}

int gnss_8122_close(int fd)
{
    pthread_mutex_destroy(&read_mutex);
    pthread_cond_destroy(&read_cond);
    return gnss_port_close(fd);
}

int gnss_8122_fw_dl(int fd, const char *fw_name, const char *dev)
{
    int uart_fd = OpenUart(dev);//PORT_NAME234
    if (uart_fd < 0)
    {
        LOGE("open uart failed %d[%d]", uart_fd, errno);
        return GNSS_ERR_OPEN_DEV;
    }

    uint8_t fw_path[256] = {0};
    memset(fw_path, 0x0, 256);
    if(memcmp(fw_name, "gp_gl_ga", 8) == 0)
    {
        memcpy(fw_path, GNSS_FW_GQALS_PATH, strlen(GNSS_FW_GQALS_PATH));
    }
    else if(memcmp(fw_name, "gp_bd_ga", 8) == 0)
    {
        memcpy(fw_path, GNSS_FW_GAQBS_PATH, strlen(GNSS_FW_GAQBS_PATH));
    }
    else
    {
        LOGE("fw dl param error");
        return GNSS_ERR_ARG;
    }

    uint8_t *g_bin_buff = (uint8_t*)malloc(500*1024);
    if(g_bin_buff == NULL) {
        LOGE("malloc() fail : %d", errno);
        return GNSS_ERR_UNKNOWN;
    }
    memset(g_bin_buff, 0, 500*1024);
    uint32_t len = read_bin_file(fw_path, g_bin_buff);
    if (len <= 0)
    {
        LOGE("Read file failed ,len = %d", len);
        goto error;
    }
    if(gnss_8122_dev_open())
    {
        LOGE("open gnss device fail:%d", errno);
        goto error;
    }

    if(gnss_fwdl_enable() < 0)
    {
        LOGE("gnss_fwdl_enable fail");
        goto error;
    }
    
    int ret = fw_update_boot(uart_fd, g_bin_buff, len);
    if (ret < 0)
    {
        LOGE("fw_update_boot() fail : %d", ret);
        goto error;
    }
    if(ret == HDBD_UPG_SUCESS)
    {
        LOGD("upgrade sucess!");
    }
    else
    {
        LOGD("upgrade FAIL, fail style:%d", ret);
        goto error;
    }
    
    if(gnss_8122_dev_close(0))
    {
        LOGE("close gnss device fail:%d", errno);
        goto error;
    }
    free(g_bin_buff);
    g_bin_buff = NULL;
    uart_close(uart_fd);
    return GNSS_ERR_OK;
error:
    if(g_bin_buff)
    {
        free(g_bin_buff);
        g_bin_buff = NULL;
    }
    if(uart_fd > 0)
    {
        uart_close(uart_fd);
        uart_fd = -1;
    }
    return GNSS_ERR_DL_FW;
}

void gnss_8122_set_cb(const void *data, int data_len)
{
    const char *buff = (const char*)data;
    if(setting_busy)   // Has setting cmd process.
    {
        gnss_cmd_rsp_process(data, data_len);
    }
}

gnss_err_enum gnss_8122_set(int fd, const char *cmd, void *cmd_rsp, int cmd_rsp_len)
{
    if(setting_busy)
    {
        return GNSS_ERR_SET_BUSY;
    }
    else
    {
        bool should_wait_rsp = TRUE;
        setting_busy = TRUE;
        gnss_set_rsp_ptr = cmd_rsp;
        gnss_set_result = GNSS_ERR_OK;
        msg_init();
        mbtk_timer_set(gnss_set_timer_cb, GNSS_SET_TIMEOUT);

        if(memcmp(cmd, "$RESET", 6) == 0)   // $RESET,<mode>
        {
            gnss_reset_type_enum mode = (gnss_reset_type_enum)atoi(cmd + 7);
            LOGD("set reset: %d", mode);
            if(mode == GNSS_RESET_TYPE_HOT)
            {
                gnss_set_result = gnss_8122_reset(fd, 3);
            }
            else if(mode == GNSS_RESET_TYPE_WARM)
            {
                gnss_set_result = gnss_8122_reset(fd, 2);
            }
            else if(mode == GNSS_RESET_TYPE_COLD)
            {
                gnss_set_result = gnss_8122_reset(fd, 1);
            }
            else
            {
                gnss_set_result = GNSS_ERR_ARG;
                goto set_fail;
            }
            if(gnss_set_result != GNSS_ERR_OK)
            {
                goto set_fail;
            }
            should_wait_rsp = FALSE;
        }
        else if(memcmp(cmd, "$SYSCFG", 7) == 0)     // $SYSCFG,<mode>
        {
            uint32 mode = 0;
            mode = (uint32)atoi(cmd + 8);
            uint32 new_mode = 0;
            if(((GNSS_SET_SYSCFG_GPS | GNSS_SET_SYSCFG_BDS | GNSS_SET_SYSCFG_GLO | GNSS_SET_SYSCFG_GAL) & mode) != mode)
            {
                gnss_set_result = GNSS_ERR_ARG;
                goto set_fail;
            }

            if(mode & GNSS_SET_SYSCFG_GPS)   // GPS
            {
                new_mode |= 0x00000001;
            }
            if(mode & GNSS_SET_SYSCFG_BDS)   // BDS
            {
                new_mode |= 0x00000002;
            }
            if(mode & GNSS_SET_SYSCFG_GLO)   // GLO
            {
                new_mode |= 0x00000004;
            }
            if(mode & GNSS_SET_SYSCFG_GAL)   // GAL
            {
                new_mode |= 0x00000010;
            }

            gnss_set_result = gnss_8122_syscfg(fd, new_mode);
            if(gnss_set_result != GNSS_ERR_OK)
            {
                goto set_fail;
            }
            should_wait_rsp = TRUE;
        }
        else if(memcmp(cmd, "$FREQCFG", 8) == 0)        //$FREQCFG,<freq>
        {
            uint32 freq = 0;
            freq = (uint32)atoi(cmd + 9);
            LOGD("set freq: %d", freq);
            if((GNSS_SET_FREQCFG_1 != freq) && (GNSS_SET_FREQCFG_2 != freq) && (GNSS_SET_FREQCFG_5 != freq))
            {
                gnss_set_result = GNSS_ERR_ARG;
                goto set_fail;
            }
            gnss_set_result = gnss_8122_freqcfg(fd, (uint8)freq);
            if(gnss_set_result != GNSS_ERR_OK)
            {
                goto set_fail;
            }
            should_wait_rsp = TRUE;
        }
        else if(memcmp(cmd, "$MSGCFG", 7) == 0)     // $MSGCFG,<mode>,<rate>
        {
            uint32 mode;
            int rate;
            if(2 == sscanf(cmd, "$MSGCFG,%d,%d", &mode, &rate))
            {
                LOGD("set msgcfg: %d, %d", mode, rate);
                int time = rate / 1000; // s
                if(time < 0)
                {
                    gnss_set_result = GNSS_ERR_ARG;
                    goto set_fail;
                }

                if(((GNSS_SET_MSGCFG_RMC | GNSS_SET_MSGCFG_VTG | GNSS_SET_MSGCFG_GGA | GNSS_SET_MSGCFG_GSA
                     | GNSS_SET_MSGCFG_GRS | GNSS_SET_MSGCFG_GSV | GNSS_SET_MSGCFG_GLL | GNSS_SET_MSGCFG_ZDA
                     | GNSS_SET_MSGCFG_GST | GNSS_SET_MSGCFG_TXT) & mode) != mode)
                {
                    LOGD("msgcfg not support mode");
                    gnss_set_result = GNSS_ERR_ARG;
                    goto set_fail;
                }

                if(mode & GNSS_SET_MSGCFG_RMC)
                {
                    gnss_set_result = gnss_8122_msgcfg(fd, 0xF0, 0x05, time);
                    if(gnss_set_result != GNSS_ERR_OK)
                    {
                        goto set_fail;
                    }
                }

                if(mode & GNSS_SET_MSGCFG_VTG)
                {
                    gnss_set_result = gnss_8122_msgcfg(fd, 0xF0, 0x06, time);
                    if(gnss_set_result != GNSS_ERR_OK)
                    {
                        goto set_fail;
                    }
                }

                if(mode & GNSS_SET_MSGCFG_GGA)
                {
                    gnss_set_result = gnss_8122_msgcfg(fd, 0xF0, 0x00, time);
                    if(gnss_set_result != GNSS_ERR_OK)
                    {
                        goto set_fail;
                    }
                }

                if(mode & GNSS_SET_MSGCFG_GSA)
                {
                    gnss_set_result = gnss_8122_msgcfg(fd, 0xF0, 0x02, time);
                    if(gnss_set_result != GNSS_ERR_OK)
                    {
                        goto set_fail;
                    }
                }

                if(mode & GNSS_SET_MSGCFG_GRS)
                {
                    gnss_set_result = gnss_8122_msgcfg(fd, 0xF0, 0x03, time);
                    if(gnss_set_result != GNSS_ERR_OK)
                    {
                        goto set_fail;
                    }
                }

                if(mode & GNSS_SET_MSGCFG_GSV)
                {
                    gnss_set_result = gnss_8122_msgcfg(fd, 0xF0, 0x04, time);
                    if(gnss_set_result != GNSS_ERR_OK)
                    {
                        goto set_fail;
                    }
                }

                if(mode & GNSS_SET_MSGCFG_GLL)
                {
                    gnss_set_result = gnss_8122_msgcfg(fd, 0xF0, 0x01, time);
                    if(gnss_set_result != GNSS_ERR_OK)
                    {
                        goto set_fail;
                    }
                }

                if(mode & GNSS_SET_MSGCFG_ZDA)
                {
                    gnss_set_result = gnss_8122_msgcfg(fd, 0xF0, 0x07, time);
                    if(gnss_set_result != GNSS_ERR_OK)
                    {
                        goto set_fail;
                    }
                }

                if(mode & GNSS_SET_MSGCFG_GST)
                {
                    gnss_set_result = gnss_8122_msgcfg(fd, 0xF0, 0x08, time);
                    if(gnss_set_result != GNSS_ERR_OK)
                    {
                        goto set_fail;
                    }
                }

                if(mode & GNSS_SET_MSGCFG_TXT)
                {
                    gnss_set_result = gnss_8122_msgcfg(fd, 0xF0, 0x20, time);
                    if(gnss_set_result != GNSS_ERR_OK)
                    {
                        goto set_fail;
                    }
                }
            }
            else
            {
                gnss_set_result = GNSS_ERR_ARG;
                goto set_fail;
            }

            should_wait_rsp = TRUE;
        }
        else if(memcmp(cmd, "$MINEL", 6) == 0)     // $MINEL,<elev>
        {
#if 0
            float elev = 0.0f;
            elev = (float)atof(cmd + 7);
//            LOGD("ELEV : %f", elev);
//            log_hex("ELEV", &elev, sizeof(double));
            if(elev < -90.0f || elev > 90.0f)
            {
                gnss_set_result = GNSS_ERR_ARG;
                goto set_fail;
            }
            float elevs[2];
            elevs[0] = elev;
            elevs[1] = elev;
            gnss_set_result = gnss_8122_minel(fd, elevs);
            if(gnss_set_result != GNSS_ERR_OK)
            {
                goto set_fail;
            }
            should_wait_rsp = FALSE;
#else
            gnss_set_result = GNSS_ERR_UNSUPPORT;
            goto set_fail;
#endif
        }
        else if(memcmp(cmd, "$NMEACFG", 8) == 0)     // $NMEACFG,<ver>
        {
#if 0
            gnss_memaver_type_enum version = (gnss_memaver_type_enum)atoi(cmd + 9);
            if(version == GNSS_MEMAVER_TYPE_3_0)
            {
                gnss_set_result = gnss_8122_nmeaver(fd, 1);
            }
            else if(version == GNSS_MEMAVER_TYPE_4_0)
            {
                gnss_set_result = gnss_8122_nmeaver(fd, 2);
            }
            else if(version == GNSS_MEMAVER_TYPE_4_1)
            {
                gnss_set_result = gnss_8122_nmeaver(fd, 3);
            }
            else
            {
                gnss_set_result = GNSS_ERR_ARG;
                goto set_fail;
            }
            if(gnss_set_result != GNSS_ERR_OK)
            {
                goto set_fail;
            }
            should_wait_rsp = FALSE;
#else
            gnss_set_result = GNSS_ERR_UNSUPPORT;
            goto set_fail;
#endif
        }
        else
        {
            LOGW("Unknown cmd:%s", cmd);
            gnss_set_result = GNSS_ERR_UNSUPPORT;
            goto set_fail;
        }

    set_success:
        if(should_wait_rsp)
        {
            setting_waitting = TRUE;
            pthread_mutex_lock(&read_mutex);
            pthread_cond_wait(&read_cond, &read_mutex);
            pthread_mutex_unlock(&read_mutex);
        }
        else
        {
            mbtk_timer_clear();
        }

        setting_busy = FALSE;
        return gnss_set_result;
    set_fail:
        setting_busy = FALSE;
        mbtk_timer_clear();
        return gnss_set_result;
    }
}