mbtk/mbtk_gnssd/gnss_main.c

#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <errno.h>
#include <pthread.h>
#include <fcntl.h>
#include <libubox/ustream.h>
#include <libubus.h>
#include <signal.h>
#include <cutils/properties.h>
#include <ctype.h>
#include <termios.h>

#include "mbtk_type.h"
#include "mbtk_log.h"
#include "gnss_info.h"
#include "gnss_utils.h"
#include "gnss_6228.h"
#include "gnss_hd8122.h"
#include "gnss_asr5311.h"
#include "gnss_n50db.h"

#define GNSS_DEBUG 1
#define GNSS_UBUS_ENABLE 1

#define GNSS_TAG "MBTK_GNSS"
#define GNSS_BUFF_SIZE 2048
#define MBTK_PROP_GNSS_LOG      "persist.mbtk.gnss_log_enable"
#define GNSS_PORT_PTY           "/dev/tty_gnss_nmea"
#define GNSS_PORT_USB_AT        "/dev/ttyGS0"
#define GNSS_PORT_USB_NMEA      "/dev/ttymodem0"
#define GNSS_PORT_UART_AT       "/dev/ttyS1"
#define GNSS_CLI_IND_MAX        10

#ifdef GNSS_DEBUG
#define GNSS_NMEA_FILE_LOG      "/tmp/mbtk_gnss_nmea.log"
#define GNSS_FILE_LOG           "/tmp/mbtk_gnss.log"
#define GNSS_FILE_LOG_MAX       104857600    // 100MB
#endif

gnss_info_t gnss_info; 

#ifdef MBTK_GNSS_UBUS_ENABLE
struct ubus_context *gnss_ubus_init(void);
#else
int gnss_ipc_service_start();
#endif

int gnss_init_config(int fd);

static char nmea_buff[GNSS_BUFF_SIZE*4] = {0};
static char data_buff[GNSS_BUFF_SIZE*4] = {0};
static uint32 nmea_buff_len = 0;
static uint32 data_buff_len = 0;
static gnss_ind_info_t ind_info[GNSS_CLI_IND_MAX];

static bool nmea_found = FALSE;
#ifdef GNSS_DEBUG
static bool nmea_log_enable = FALSE;
static int nmea_log_fd = -1;
static int debug_fd = -1;
static int debug_fd_len = 0;
#endif
static int gnss_pty_master_fd = -1;
static int gnss_pty_slave_fd = -1;
static int gnss_usb_at_port_fd = -1;
static int gnss_usb_nmea_port_fd = -1;
static int gnss_uart_at_port_fd = -1;
static char *gnss_filter_info[] = {"RMC", "VTG", "GGA", "GSA", "GSV", "GLL", "ZDA", "GST", "TXT", "DHV", "DTM", NULL};

int gnss_write(int fd, const void* buf, int buf_len);

static void help()
{
    LOGD("mbtk_gnssd <6228/8122/5311/N50DB> <gnss_dev> <0/1>");
}

static int arg_check(int argc, char *argv[])
{
    if(argc != 4) {
        LOGE("argc = %d", argc);
        goto check_fail;
    }

    // Only support 6228/8122/5311.
    if(strcmp(argv[1], GNSS_ID_6228) && strcmp(argv[1], GNSS_ID_8122) && strcmp(argv[1], GNSS_ID_5311)
        && strcmp(argv[1], GNSS_ID_N50DB)) {
        LOGE("argv[1] = %s", argv[1]);
        goto check_fail;
    }

    if(access(argv[2], R_OK | W_OK)) {
        LOGE("access(%s) rw fail. ", argv[2]);
        goto check_fail;
    }

#if 0
    int init_mode = atoi(argv[3]);
    if(((GNSS_PRINT_PORT_UART1 | GNSS_PRINT_PORT_USB_NMEA | GNSS_PRINT_PORT_USB_AT | GNSS_PRINT_PORT_TTY_AT) & init_mode) != init_mode) {
        goto check_fail;
    }
#endif

    return 0;
check_fail:
    help();
    return -1;
}

static int gnss_ports_open(uint32 print_port)
{
    // TTY AT change.
    if((gnss_info.print_port & GNSS_PRINT_PORT_TTY_AT) != (print_port & GNSS_PRINT_PORT_TTY_AT)) {
        if(print_port & GNSS_PRINT_PORT_TTY_AT) { // Open
            if(gnss_pty_open(&gnss_pty_master_fd, &gnss_pty_slave_fd, GNSS_PORT_PTY)) {
                return GNSS_ERR_OPEN_DEV;
            }
            LOGD("Open PTY port success.");
        } else { // Close
            if(gnss_pty_slave_fd > 0) {
                close(gnss_pty_slave_fd);
                gnss_pty_slave_fd = -1;
                unlink(GNSS_PORT_PTY);
            }
            LOGD("Close PTY port success.");
        }
    }

    // USB AT change.
    if((gnss_info.print_port & GNSS_PRINT_PORT_USB_AT) != (print_port & GNSS_PRINT_PORT_USB_AT)) {
        if(print_port & GNSS_PRINT_PORT_USB_AT) { // Open
            if((gnss_usb_at_port_fd = gnss_port_open(GNSS_PORT_USB_AT, O_RDWR | O_NONBLOCK | O_NOCTTY, 115200, TRUE)) <= 0) {
                return GNSS_ERR_OPEN_DEV;
            }
            LOGD("Open USB AT port success.");
        } else { // Close
            if(gnss_usb_at_port_fd > 0) {
                close(gnss_usb_at_port_fd);
                gnss_usb_at_port_fd = -1;
            }
            LOGD("Close USB AT port success.");
        }
    }

    // USB NMEA change.
    if((gnss_info.print_port & GNSS_PRINT_PORT_USB_NMEA) != (print_port & GNSS_PRINT_PORT_USB_NMEA)) {
        if(print_port & GNSS_PRINT_PORT_USB_NMEA) { // Open
            if((gnss_usb_nmea_port_fd = gnss_port_open(GNSS_PORT_USB_NMEA, O_RDWR | O_NONBLOCK | O_NOCTTY, 115200, TRUE)) <= 0) {
                return GNSS_ERR_OPEN_DEV;
            }
            LOGD("Open USB NMEA port success.");
        } else { // Close
            if(gnss_usb_nmea_port_fd > 0) {
                close(gnss_usb_nmea_port_fd);
                gnss_usb_nmea_port_fd = -1;
            }
            LOGD("Close USB NMEA port success.");
        }
    }

    // Uart AT change.
    if((gnss_info.print_port & GNSS_PRINT_PORT_UART1) != (print_port & GNSS_PRINT_PORT_UART1)) {
        if(print_port & GNSS_PRINT_PORT_UART1) { // Open
            if((gnss_uart_at_port_fd = gnss_port_open(GNSS_PORT_UART_AT, O_RDWR | O_NONBLOCK | O_NOCTTY, 115200, TRUE)) <= 0) {
                return GNSS_ERR_OPEN_DEV;
            }
            LOGD("Open UART AT port success.");
        } else { // Close
            if(gnss_uart_at_port_fd > 0) {
                close(gnss_uart_at_port_fd);
                gnss_uart_at_port_fd = -1;
            }
            LOGD("Close UART AT port success.");
        }
    }

    gnss_info.print_port = print_port;

    return GNSS_ERR_OK;
}

static int gnss_ports_close()
{
    if(gnss_usb_at_port_fd > 0) {
        tcflush(gnss_usb_at_port_fd, TCIOFLUSH);
        close(gnss_usb_at_port_fd);
        gnss_usb_at_port_fd = -1;
    }

    if(gnss_usb_nmea_port_fd > 0) {
        tcflush(gnss_usb_nmea_port_fd, TCIOFLUSH);
        close(gnss_usb_nmea_port_fd);
        gnss_usb_nmea_port_fd = -1;
    }

    if(gnss_uart_at_port_fd > 0) {
        tcflush(gnss_uart_at_port_fd, TCIOFLUSH);
        close(gnss_uart_at_port_fd);
        gnss_uart_at_port_fd = -1;
    }

    if(gnss_pty_master_fd > 0) {
        tcflush(gnss_pty_master_fd, TCIOFLUSH);
        close(gnss_pty_master_fd);
        gnss_pty_master_fd = -1;
    }

    if(gnss_pty_slave_fd > 0) {
        tcflush(gnss_pty_slave_fd, TCIOFLUSH);
        close(gnss_pty_slave_fd);
        gnss_pty_slave_fd = -1;
        unlink(GNSS_PORT_PTY);
    }

    gnss_info.print_port = 0;

    return 0;
}

#ifdef GNSS_DEBUG
static void log_save(int fd, const char *data, int data_len)
{
    if(nmea_log_enable){
        if(0 /* debug_fd_len > GNSS_FILE_LOG_MAX */) {
            LOGD("Reopen file:%s(len = %d)", GNSS_FILE_LOG, debug_fd_len);
            close(debug_fd);
            debug_fd = open(GNSS_FILE_LOG, O_WRONLY | O_CREAT | O_TRUNC, 0666);
            if(debug_fd < 0) {
                LOGE("Open debug fd fail.");
            }
            debug_fd_len = 0;

            LOGD("Reopen file:%s", GNSS_NMEA_FILE_LOG);
            close(nmea_log_fd);
            nmea_log_fd = open(GNSS_NMEA_FILE_LOG, O_WRONLY | O_CREAT | O_TRUNC, 0666);
            if(nmea_log_fd < 0) {
                LOGE("Open debug fd fail.");
            }
        }

        if(fd == nmea_log_fd) {
            if(nmea_log_fd > 0) {
                write(nmea_log_fd, data, data_len);
                debug_fd_len += data_len;
            }
        } else if(fd == debug_fd) {
            if(debug_fd > 0) {
                write(debug_fd, data, data_len);
                debug_fd_len += data_len;
            }
        }
    }
}
#endif

static void ind_location_print(const char *data)
{
    int index = 0;
    int buff_len = 0;
    char buff[GNSS_BUFF_SIZE] = {0};
    buff[0] = MBTK_IND_START_FLAG;
    buff_len++;
    memcpy(buff + 1, MBTK_IND_LOCATION_TAG, strlen(MBTK_IND_LOCATION_TAG));
    buff_len += strlen(MBTK_IND_LOCATION_TAG);
    memcpy(buff + strlen(buff), data, strlen(data));
    buff_len += strlen(data);
    buff[strlen(buff)] = MBTK_IND_END_FLAG;
    buff_len++;

    while(index < GNSS_CLI_IND_MAX) {
        if(ind_info[index].cli_fd > 0 && (ind_info[index].ind_flag & MBTK_GNSS_IND_LOCATION)) {
            gnss_write(ind_info[index].cli_fd, buff, buff_len);
        }
        index++;
    }
}

static void ind_nmea_print(const char *data)
{
    int index = 0;
    int buff_len = 0;
    char buff[GNSS_BUFF_SIZE] = {0};
    buff[0] = MBTK_IND_START_FLAG;
    buff_len++;
    memcpy(buff + 1, MBTK_IND_NMEA_TAG, strlen(MBTK_IND_NMEA_TAG));
    buff_len += strlen(MBTK_IND_NMEA_TAG);
    memcpy(buff + strlen(buff), data, strlen(data));
    buff_len += strlen(data);
    buff[strlen(buff)] = MBTK_IND_END_FLAG;
    buff_len++;
    while(index < GNSS_CLI_IND_MAX) {
        if(ind_info[index].cli_fd > 0 && (ind_info[index].ind_flag & MBTK_GNSS_IND_NMEA)) {
            gnss_write(ind_info[index].cli_fd, buff, buff_len);
        }
        index++;
    }
}

static void nmea_print(const char *nmea, int nmea_len)
{
    int ret = -1;
    if(gnss_usb_at_port_fd > 0) {
        ret = write(gnss_usb_at_port_fd, nmea, nmea_len);
    }

    if(gnss_usb_nmea_port_fd > 0) {
        ret = write(gnss_usb_nmea_port_fd, nmea, nmea_len);
    }

    if(gnss_uart_at_port_fd > 0) {
        ret = write(gnss_uart_at_port_fd, nmea, nmea_len);
    }

    if(gnss_pty_master_fd > 0) {
        ret = write(gnss_pty_master_fd, nmea, nmea_len);
    }
    ind_nmea_print(nmea);
    ind_location_print(nmea);
}

static unsigned char nmea_checksum(const char *nmea)
{
    const char *p = nmea;
    unsigned char chs = 0;

    while (*p == '$')   // skip '$'
        p++;
    while (*p != '*' && *p != 0)
        chs ^= *p++;

    return chs;
}

static bool nmea_check(const char *nmea, int nmea_len)
{
    char **ptr = gnss_filter_info;
    while(*ptr) {
        if(strstr(nmea, *ptr)) {
            break;
        }
        ptr++;
    }

    if(*ptr == NULL) {
        LOGD("Unknown NMEA[%d]:%s", nmea_len, nmea);
        return FALSE;
    }

    char *checksum_str = strstr(nmea, "*");
    checksum_str++; // Jump '*'
    char checksum_buf[3] = {0};
    snprintf(checksum_buf, 3, "%02x", nmea_checksum(nmea));
    if(strncasecmp(checksum_buf, checksum_str, 2)) {
        LOGD("Checksum error[%d](checksum - %s):%s", nmea_len, checksum_buf, nmea);
        return FALSE;
    }

    return TRUE;
}

static int nmea_tokenizer_init(mbtk_nmeatokenizer* t, const char* head, const char* end, int param_num)
{
    int count = 0;
    const char* p = head;
    const char* q = end;
    const char* tmp = NULL;
    // the initial '$' is optional
    if (p < q && p[0] == '$')
    {
        p += 1;
    }
    else
    {
        return -1;
    }

    //find '*',del '*25\r\n'
    // get rid of checksum at the end of the sentecne
    if (q >= p + 5 && q[-5] == '*')
    {
        q -= 5;
    }
    else
    {
        return -1;
    }

    while (p <= q) 
    {
        tmp = memchr(p, ',', q-p);
        if (tmp == NULL)
        {
            tmp = q;
        }
        // if (q > p) {
        // q >= p include empty token: ,,
        if (tmp >= p)
        {
            if (count < MAX_NMEA_TOKENS)
            {
                t->tokens[count].head = p;
                t->tokens[count].end = tmp;
                count += 1;
            }
        }
        
        if (tmp <= q)
        {
            tmp += 1;
        }

        p = tmp;
    }

    if(count != param_num)
    {
        LOGD("count [%d], param_num [%d]", count, param_num);
        return -1;
    }

    t->count = count;
    return count;
}

static mbtk_token nmea_tokenizer_get(mbtk_nmeatokenizer* t, int  index)
{
    mbtk_token tok;
    static const char*  dummy = "";

    if (index < 0 || index >= t->count)
    {
        tok.head = tok.end = dummy;
    }
    else
    {
        tok = t->tokens[index];
    }
    return tok;
}

typedef enum {
    LYNQ_TIME_TYPE_CELL = 0,  //NITZ
    LYNQ_TIME_TYPE_NTP,
    LYNQ_TIME_TYPE_GNSS,
    LYNQ_TIME_TYPE_USER,
    
    LYNQ_TIME_TYPE_UNUSE
} lynq_time_type_enum;

bool mbtk_gnss_time_set_flag = 0;

static int mbtk_time_type_gnss_read() {
    int type = 0;
    char time_type[] ={0};
    property_get("persist.mbtk.time_type", time_type, "0");

    type = atoi(time_type);
//    LOGD("time_type :%d\n", type);
    if(type != LYNQ_TIME_TYPE_GNSS)
        mbtk_gnss_time_set_flag = 0;

    return type;
}

static int strstr_n(const char *s1, const char *s2)
{
    int n;
    int strlen = 0;

    if(*s2)
    {
        while(*s1)
        {
            for(n = 0; *(s1+n) == *(s2 + n); n++)
            {
                if(!*(s2 + n + 1))
                {
                    strlen++;
                    return strlen;
                }
            }
            s1++;
            strlen++;
        }
        return 0;
    }

    return 0;
}
static int nmea_update_date_time(mbtk_token date, mbtk_token time)
{
    char tmp_char[4] = {0};
    struct tm tmp_time;
    struct timeval tv;

    memset(&tmp_time, 0x0, sizeof(struct tm));
    if (date.head + 6 > date.end)
    {
        LOGD("date get fail");
        return -1;
    }

    memcpy(tmp_char, date.head, 2);
    tmp_time.tm_mday = atoi(tmp_char);
    memcpy(tmp_char, date.head + 2, 2);
    tmp_time.tm_mon = atoi(tmp_char) - 1;
    memcpy(tmp_char, date.head + 4, 2);
    tmp_time.tm_year = 100 + atoi(tmp_char);

    if (time.head + 6 > time.end)
    {
        LOGD("time get fail");
        return -1;
    }

    memcpy(tmp_char, time.head, 2);
    tmp_time.tm_hour = atoi(tmp_char);
    memcpy(tmp_char, time.head + 2, 2);
    tmp_time.tm_min = atoi(tmp_char);
    memcpy(tmp_char, time.head + 4, 2);
    tmp_time.tm_sec = atoi(tmp_char);
    tmp_time.tm_isdst = -1;
    

    LOGD("data:%d-%d-%d %d:%d:%d", tmp_time.tm_year + 1900, 
                                    tmp_time.tm_mon,
                                    tmp_time.tm_mday,
                                    tmp_time.tm_hour,
                                    tmp_time.tm_min,
                                    tmp_time.tm_sec);


    time_t _t = mktime(&tmp_time);//parse location tmp_time

    tzset();   // auto set tz
    _t = _t - timezone;

    LOGD("timestamp:%ld,  %ld", _t,  timezone);

    tv.tv_sec = _t;
    if(settimeofday(&tv, NULL)) {
        LOGD("%s: , Set time fail\n", __func__);
        mbtk_gnss_time_set_flag = 0;
    } else {
        LOGD("%s: , Set time success \n", __func__);
	mbtk_gnss_time_set_flag = 1;
    }

    return 0;
}

static int ind_nmea_parse(const char *data, int data_len)
{
    int ret;
    mbtk_nmeatokenizer tzer = {0};
    if(strstr_n(data + 3, "RMC"))
    {
        ret = nmea_tokenizer_init(&tzer, data, data + data_len, NMEA_RMC_PARAM_NUM);
        if(ret < 0)
        {
            LOGD("nmea_tokenizer_init fail");
            return -1;
        }

        mbtk_token  tok_time      = nmea_tokenizer_get(&tzer,1);
        mbtk_token  tok_fixStatus = nmea_tokenizer_get(&tzer,2);
        mbtk_token  tok_date      = nmea_tokenizer_get(&tzer,9);

        if(tok_fixStatus.head[0] == 'A')
        {
            ret = nmea_update_date_time(tok_date, tok_time);
            if(ret < 0)
            {
                LOGD("nmea_update_date_time fail");
                return -1;
            }

        }
    }

    return 0;
}


static void gnss_nmea_process(const char *data, int data_len)
{
//     LOGD("gnss_nmea_process() : data_len - %d", data_len);
#if 0
    char nmea[GNSS_BUFF_SIZE] = {0};
    memcpy(nmea, data, data_len);
#else
    const char *nmea = data;
#endif

    if(!nmea_check(nmea, data_len)) {
        // No print "$HOSTSLEEP".
        if(memcmp(nmea, "$HOSTSLEEP", strlen("$HOSTSLEEP"))) {
            LOGD("NO-NMEA:%s", nmea);
        }
#if GNSS_DEBUG
        log_save(nmea_log_fd, "/**/", 4);
        log_save(nmea_log_fd, nmea, data_len);
#endif
        if(gnss_info.gnss_set_cb)
            gnss_info.gnss_set_cb(nmea, data_len);
        return;
    }

#ifdef GNSS_DEBUG
    if(nmea_log_enable) {
        LOGD("NMEA[%d]:%s", data_len, nmea);
    }

    log_save(nmea_log_fd, nmea, data_len);
#endif

    if( (mbtk_time_type_gnss_read() == LYNQ_TIME_TYPE_GNSS) && !mbtk_gnss_time_set_flag)
        ind_nmea_parse(nmea, data_len);

    nmea_print(nmea, data_len);
}

#if 0
static void gnss_cmd_rsp_process(const char *data, int data_len)
{
    char rsp[GNSS_BUFF_SIZE] = {0};
    memcpy(rsp, data, data_len);
    LOGD("RSP[%d]:%s", data_len, rsp);
}
#endif

static bool nmea_char_check(char ch)
{
    if(isalnum(ch) || ch == '$' || ch == '\r' || ch == '\n' || ch == '.'
        || ch == ',' || ch == '*' || ch == '\0' || ch == '/' || ch == '_' || ch == '=' || ch == '-')
        return TRUE;

    return FALSE;
}

static void gnss_data_process(const char *data, int data_len)
{
    if(gnss_info.state == GNSS_STATE_OPEN) {
        LOGD("GNSS_OPEN[%d]:%s", data_len, data);
    } else if(gnss_info.state == GNSS_STATE_DOWNLOAD) {
        // LOGD("GNSS_DL[%d]:%s", data_len, data);
        if(gnss_info.gnss_dl_read_cb) {
            gnss_info.gnss_dl_read_cb(data, data_len);
        }
    } else if(gnss_info.state == GNSS_STATE_READY) {
        int index = 0;
        while(index < data_len) {
            if(nmea_found) {
                if(!nmea_char_check(data[index])) {
                    // Copy nmea_buff to data_buff
                    // Start with '$', but not nmea data, so copy to data_buff.
                    memcpy(data_buff + data_buff_len, nmea_buff, nmea_buff_len);
                    data_buff_len += nmea_buff_len;
                    data_buff[data_buff_len++] = data[index];

                    nmea_buff_len = 0;
                    nmea_found = FALSE;
                    continue;
                }

                if(data[index] != '\0') {
                    nmea_buff[nmea_buff_len++] = data[index];
                    if(nmea_buff[nmea_buff_len - 1] == '\n') {
                        if(data_buff_len > 0) {
#if GNSS_DEBUG
                            log_save(nmea_log_fd, "/**/", 4);
                            log_save(nmea_log_fd, data_buff, data_buff_len);
#endif
                            if(gnss_info.gnss_set_cb) {
                                gnss_info.gnss_set_cb(data_buff, data_buff_len);
                            }
                            data_buff_len = 0;
                        }

                        if(nmea_buff_len > 6 && nmea_buff[nmea_buff_len - 5] == '*') { // $XXX*YY\r\n
                            nmea_buff[nmea_buff_len] = '\0';
                            gnss_nmea_process(nmea_buff, nmea_buff_len);
                        } else if(nmea_buff_len > 0) {
                            nmea_buff[nmea_buff_len] = '\0';
                            if(memcmp(nmea_buff, "$HOSTSLEEP", strlen("$HOSTSLEEP"))) {
                                LOGD("NO-NMEA:%s", nmea_buff);
                            }
#if GNSS_DEBUG
                            log_save(nmea_log_fd, "/**/", 4);
                            log_save(nmea_log_fd, nmea_buff, nmea_buff_len);
#endif
                        }

                        nmea_buff_len = 0;
                        nmea_found = FALSE;
                    }
                }
            } else {
                if(data[index] == '$') {
                    nmea_buff_len = 0;
                    nmea_found = TRUE;
                    nmea_buff[nmea_buff_len++] = data[index];
                } else {
                    data_buff[data_buff_len++] = data[index];
                }
            }
            index++;
        }
    } else {
        LOGW("Unknown state : %d", gnss_info.state);
    }
}

void* gnss_read_pthread(void* arg)
{
    LOGD("gnss_read_pthread enter.");
    char buffer[GNSS_BUFF_SIZE];
    int len = 0;
    int ret = 0;
    fd_set fdr, fdw;
    int fd_max = 0;

    fd_max = (gnss_info.fd > fd_max) ? gnss_info.fd : fd_max;
    fd_max = (gnss_info.exit_fd[0] > fd_max) ? gnss_info.exit_fd[0] : fd_max;
    memset(nmea_buff, 0, sizeof(nmea_buff));
    memset(data_buff, 0, sizeof(data_buff));
    nmea_buff_len = 0;
    data_buff_len = 0;
#if GNSS_DEBUG
    if(nmea_log_enable) {
        debug_fd = open(GNSS_FILE_LOG, O_WRONLY | O_CREAT | O_TRUNC, 0666);
        if(debug_fd < 0) {
            LOGE("Open debug fd fail.");
        }
        nmea_log_fd = open(GNSS_NMEA_FILE_LOG, O_WRONLY | O_CREAT | O_TRUNC, 0666);
        if(nmea_log_fd < 0) {
            LOGE("Open nmea fd fail.");
        }
        debug_fd_len = 0;
    }
#endif

    LOGD("uart_fd - %d, exit_fd - %d", gnss_info.fd, gnss_info.exit_fd[0]);

    while(gnss_info.state >= GNSS_STATE_OPEN) {
        FD_ZERO(&fdw);
        FD_ZERO(&fdr);
        FD_SET(gnss_info.fd, &fdr);
        FD_SET(gnss_info.exit_fd[0], &fdr);
        ret = select(fd_max + 1, &fdr, &fdw, 0, NULL);
        //LOGD("select - %d", ret);
        if(gnss_info.state < GNSS_STATE_OPEN) {
            LOGD("State = %d, ret = %d", gnss_info.state, ret);
            if(ret > 0) {
                if (FD_ISSET(gnss_info.fd, &fdr)) {
                    LOGD("gnss_fd can read.");
                } else if (FD_ISSET(gnss_info.exit_fd[0], &fdr)) {
                    LOGD("exit_fd can read.");
                } else {
                    LOGW("Unknown select event.");
                }
            }
            break;
        }

        if (ret < 0)
        {
            if (errno == EINTR)
            {
                continue;
            }
            LOGE("select error, errno = %d (%s)", errno, strerror(errno));
            break;
        }
        else if (ret == 0)
        {
            LOGE("select ret == 0");
            break;
        }

        if (FD_ISSET(gnss_info.fd, &fdr))
        {
            memset(buffer, 0, GNSS_BUFF_SIZE);
            len = read(gnss_info.fd, buffer, GNSS_BUFF_SIZE - 1);
            if(len > 0) {
                //log_hex("READ", buffer, len);

#if GNSS_DEBUG
                //LOGD("read data_len = %d", len);
                log_save(debug_fd, buffer, len);
#endif

                gnss_data_process(buffer, len);

            } else if(len ==0 ){
                LOGE("Read end : len = 0");
                break;
            } else {
                if(EAGAIN == errno) {
                    usleep(50000);
                    continue;
                } else {
                    LOGD("Read ret = -1 ,errno = %d", errno);
                    break;
                }
            }
        }
        else if (FD_ISSET(gnss_info.exit_fd[0], &fdr))
        {
            LOGD("exit_fd select event.");
            memset(buffer, 0, GNSS_BUFF_SIZE);
            len = read(gnss_info.exit_fd[0], buffer, GNSS_BUFF_SIZE);
            if(len > 0) {
                if(strcmp(buffer, "exit") == 0) {
                    LOGD("Get thread exit message.");
                    break;
                }
            }
        }
        else
        {
            LOGW("Unknown select event.");
            continue;
        }
    }

#if GNSS_DEBUG
    if(debug_fd > 0) {
        close(debug_fd);
        debug_fd = -1;
    }
    if(nmea_log_fd > 0) {
        close(nmea_log_fd);
        nmea_log_fd = -1;
    }
#endif

    gnss_info.state = GNSS_STATE_CLOSE;
    LOGD("gnss_read_pthread exit.");
    return NULL;
}

#if 0
int gnss_write(int fd, const void *data, int data_len)
{
    int count = 0;
    int len = 0;
    while(1)
    {
        len = write(fd, data + count, data_len - count);
        if (len > 0)
        {
            count += len;
        }
        else
        {
            LOGE("write() fail,ret = %d,errno = %d", len, errno);
            break;
        }

        if (count == data_len)
            break;
    }

    return count;
}
#else
int gnss_write(int fd, const void* buf, int buf_len)
{
    ssize_t size;
    ssize_t size_to_wr;
    ssize_t size_written;
    if(GNSS_BUFF_SIZE < buf_len)
    {
        return -1;
    }
    for(size = 0; size < buf_len;)
    {
        size_to_wr = buf_len - size;
        if( size_to_wr > GNSS_BUFF_SIZE)
            size_to_wr = GNSS_BUFF_SIZE;

        size_written = write(fd, (const uint8*)buf + size, size_to_wr);
        if (size_written==-1)
        {
            return -1;
        }
        size += size_written;
        if(size_written != size_to_wr)
        {
            return size;
        }
    }
    // LOGD("SEND %d / %d", size, buf_len);
    return size;
}
#endif

int gnss_init(uint32 print_port)
{
    if(gnss_info.state != GNSS_STATE_CLOSE) {
        LOGW("GNSS not close:%d", gnss_info.state);
        if(gnss_info.state == GNSS_STATE_READY) {
            LOGD("Reset print port : %d -> %d", gnss_info.print_port, print_port);
            if(gnss_info.print_port != print_port) {
                return gnss_ports_open(print_port);
            } else {
                return GNSS_ERR_OK;
            }
        } else {
            return GNSS_ERR_OK;
        }
    }

    int ret = 0;
    if(gnss_info.dl_befor_open) {
        //if(gnss_info.auto_dl_fw) {
            gnss_info.state = GNSS_STATE_DOWNLOAD;
            ret = gnss_info.gnss_fw_dl(gnss_info.fd, NULL, gnss_info.dev_name);
            if(ret) {
                LOGE("gnss_fw_dl() fail : %d", ret);
                gnss_info.state = GNSS_STATE_CLOSE;
                return GNSS_ERR_DL_FW;
            }

            gnss_info.fd = gnss_info.gnss_open(gnss_info.dev_name);
            if(gnss_info.fd <= 0) {
                LOGE("gnss_open(%s) fail : %d", gnss_info.dev_name, gnss_info.fd);
                gnss_info.state = GNSS_STATE_CLOSE;
                return GNSS_ERR_OPEN_DEV;
            }
            if(pipe(gnss_info.exit_fd)) {
                LOGE("pipe() fail[%d].", errno);
                return GNSS_ERR_UNKNOWN;
            }
            // GNSS is opened.
            gnss_info.state = GNSS_STATE_OPEN;

#if 0
            // Start gnss read thread.
            pthread_attr_t thread_attr;
            pthread_attr_init(&thread_attr);
            if(pthread_attr_setdetachstate(&thread_attr, PTHREAD_CREATE_DETACHED))
            {
                LOGE("pthread_attr_setdetachstate() fail.");
                goto main_exit;
            }

            if(pthread_create(&gnss_info.read_pid, &thread_attr, gnss_read_pthread, NULL))
#else
            if(pthread_create(&gnss_info.read_pid, NULL, gnss_read_pthread, NULL))
#endif
            {
                LOGE("pthread_create() fail.");
                goto exit_with_close;
            }

            ret = gnss_info.gnss_dev_open();
            if(ret) {
                LOGE("gnss_dev_open() fail : %d", ret);
                goto exit_with_thread_exit;
            }
        //}
    } else {
        gnss_info.fd = gnss_info.gnss_open(gnss_info.dev_name);
        if(gnss_info.fd <= 0) {
            LOGE("gnss_open(%s) fail : %d", gnss_info.dev_name, gnss_info.fd);
            gnss_info.state = GNSS_STATE_CLOSE;
            return GNSS_ERR_OPEN_DEV;
        }
        if(pipe(gnss_info.exit_fd)) {
            LOGE("pipe() fail[%d].", errno);
            return GNSS_ERR_UNKNOWN;
        }
        // GNSS is opened.
        gnss_info.state = GNSS_STATE_OPEN;

#if 0
        // Start gnss read thread.
        pthread_attr_t thread_attr;
        pthread_attr_init(&thread_attr);
        if(pthread_attr_setdetachstate(&thread_attr, PTHREAD_CREATE_DETACHED))
        {
            LOGE("pthread_attr_setdetachstate() fail.");
            goto main_exit;
        }

        if(pthread_create(&gnss_info.read_pid, &thread_attr, gnss_read_pthread, NULL))
#else
        if(pthread_create(&gnss_info.read_pid, NULL, gnss_read_pthread, NULL))
#endif
        {
            LOGE("pthread_create() fail.");
            goto exit_with_close;
        }

        ret = gnss_info.gnss_dev_open();
        if(ret) {
            LOGE("gnss_dev_open() fail : %d", ret);
            goto exit_with_thread_exit;
        }
    }

    // GNSS is ready, NMEA can print from uart.
    gnss_info.state = GNSS_STATE_READY;

    LOGD("GNSS open success.");

    return gnss_ports_open(print_port);

exit_with_thread_exit:
    gnss_info.state = GNSS_STATE_CLOSING;
    // Wait for read thread exit.
    ret = pthread_join(gnss_info.read_pid, NULL);
    if(ret){
        LOGE("pthrad_join fail(%d)",ret);
    }
exit_with_close:
    if(gnss_info.gnss_close(gnss_info.fd)) {
        LOGE("gnss_close() fail.");
    }
    if(gnss_info.exit_fd[0] > 0) {
        close(gnss_info.exit_fd[0]);
        gnss_info.exit_fd[0] = -1;
    }
    if(gnss_info.exit_fd[1] > 0) {
        close(gnss_info.exit_fd[1]);
        gnss_info.exit_fd[1] = -1;
    }
    gnss_info.state = GNSS_STATE_CLOSE;
    return GNSS_ERR_UNKNOWN;
}

int gnss_deinit()
{
    if(gnss_info.state == GNSS_STATE_CLOSE) {
        LOGW("GNSS is closed.");
        return GNSS_ERR_OK;
    } else if(gnss_info.state == GNSS_STATE_CLOSING) {
        LOGW("GNSS is closing...");
        return GNSS_ERR_UNKNOWN;
    } else if(gnss_info.state == GNSS_STATE_DOWNLOAD) {
        LOGW("GNSS is downloading...");
        return GNSS_ERR_UNKNOWN;
    }

    if(gnss_info.gnss_dev_close(gnss_info.fd)) {
        LOGE("gnss_dev_close() fail.");
        return GNSS_ERR_UNKNOWN;
    }

    // Wait for read thread exit.
    if(gnss_info.exit_fd[1] > 0) {
        write(gnss_info.exit_fd[1], "exit", 4);
    }

    gnss_info.state = GNSS_STATE_CLOSING;
    int ret = pthread_join(gnss_info.read_pid, NULL);
    if(ret){
        LOGE("pthrad_join fail(%d)",ret);
        return GNSS_ERR_UNKNOWN;
    }

    if(gnss_info.gnss_close(gnss_info.fd)) {
        LOGE("gnss_close() fail.");
        return GNSS_ERR_UNKNOWN;
    }

    if(gnss_ports_close()) {
        LOGE("gnss_ports_close fail.");
        return GNSS_ERR_UNKNOWN;
    }

    LOGD("gnss_ports_close() complete.");

    gnss_info.fd = -1;
    if(gnss_info.exit_fd[0] > 0) {
        close(gnss_info.exit_fd[0]);
        gnss_info.exit_fd[0] = -1;
    }
    if(gnss_info.exit_fd[1] > 0) {
        close(gnss_info.exit_fd[1]);
        gnss_info.exit_fd[1] = -1;
    }
    gnss_info.state = GNSS_STATE_CLOSE;
    LOGD("GNSS close success.");
    return GNSS_ERR_OK;
}

int gnss_set(const void* buf, unsigned int buf_len, void *cmd_rsp, int cmd_rsp_len)
{
    if(buf && buf_len > 0) {
        if(cmd_rsp && cmd_rsp_len > 0) {
            memset(cmd_rsp, 0, cmd_rsp_len);
        }
        return gnss_info.gnss_set(gnss_info.fd, buf, cmd_rsp, cmd_rsp_len);
    } else {
        return GNSS_ERR_UNKNOWN;
    }
}

int gnss_dl_fw(const char* fw_name, void *rsp, int rsp_len)
{
    // Only 8122 support download fw.
    if(gnss_info.gnss_id != GNSS_TYPE_8122) {
        return GNSS_ERR_UNSUPPORT;
    }

    if(rsp && rsp_len > 0) {
        memset(rsp, 0, rsp_len);
    }

    if(gnss_info.gnss_fw_dl) {
        if(GNSS_ERR_OK != gnss_deinit()) {
            LOGE("Close gnss fail.");
            return GNSS_ERR_UNKNOWN;
        } else {
            LOGD("Start gnss fw dl.");
            return gnss_info.gnss_fw_dl(gnss_info.fd, fw_name, gnss_info.dev_name);
        }
    } else {
        return GNSS_ERR_UNSUPPORT;
    }
}

int gnss_ind_set(int fd, int ind_type)
{
    int index = 0;
    if(ind_type) { // Add IND flag.
        while(index < GNSS_CLI_IND_MAX) {
            if(ind_info[index].cli_fd == fd)
                break;
            index++;
        }

        if(index == GNSS_CLI_IND_MAX) { // Add flag
            index = 0;
            while(index < GNSS_CLI_IND_MAX) {
                if(ind_info[index].cli_fd <= 0)
                    break;
                index++;
            }
            if(index == GNSS_CLI_IND_MAX)
            {
                LOGE("ind flag is full.");
                return GNSS_ERR_CLI_FULL;
            }
            ind_info[index].cli_fd = fd;
            ind_info[index].ind_flag = (uint32)ind_type;
        } else { // Change flag     
            ind_info[index].cli_fd = fd;
            ind_info[index].ind_flag = (uint32)ind_type;
        }
    } else { // Clear IND flag.
        while(index < GNSS_CLI_IND_MAX) {
            if(ind_info[index].cli_fd == fd)
                break;
            index++;
        }

        if(index == GNSS_CLI_IND_MAX) {
            return GNSS_ERR_ARG;
        }    
        ind_info[index].cli_fd = 0;
        ind_info[index].ind_flag = 0;
    }

    return GNSS_ERR_OK;
}

static void sig_process(int sig)
{
    LOGI("I got signal %d\n", sig);
    if(gnss_deinit()) {
        LOGE("gnss_deinit() fail, no exist...");
        return;
    }

    switch(sig)
    {
        case SIGINT: // Ctrl + C
        {
            LOGI("Exit by SIGINT.\n");
            exit(0);
        }
        case SIGQUIT: // Ctrl + \ (\C0\E0\CB\C6 SIGINT \A3\AC\B5\ABҪ\B2\FA\C9\FAcore\CEļ\FE)
        {
            LOGI("Exit by SIGQUIT.\n");
            exit(0);
        }
        case SIGTERM:// Ĭ\C8\CFkill   (ͬ SIGKILL \A3\AC\B5\AB SIGKILL \B2\BB\BFɲ\B6\BB\F1)
        {
            LOGI("Exit by SIGTERM.\n");
            exit(0);
        }
        case SIGTSTP:// Ctrl + Z (ͬ SIGSTOP \A3\AC\B5\AB SIGSTOP \B2\BB\BFɲ\B6\BB\F1)
        {
            LOGI("Exit by SIGTSTP.\n");
            exit(0);
        }
        case SIGSEGV: // \C8\E7\BF\D5ָ\D5\EB
        {
            LOGI("Exit by SIGSEGV.\n");
            exit(0);
        }
        default:
        {
            LOGI("Unknown sig:%d\n",sig);
            break;
        }
    }
}


// mbtk_gnssd 6228 /dev/ttyS2 baud 0/1 <port_type>
int main(int argc, char *argv[])
{
    mbtk_log_init("radio", GNSS_TAG);

#ifdef MBTK_DUMP_SUPPORT
    mbtk_debug_open(NULL, TRUE);
#endif

    signal(SIGINT, sig_process);
    signal(SIGQUIT, sig_process);
    signal(SIGTERM, sig_process);

    if(arg_check(argc, argv)) {
        return -1;
    }

#ifdef GNSS_DEBUG
    char buff[10];
    memset(buff, 0, 10);
    property_get(MBTK_PROP_GNSS_LOG, buff, "");
    if(strlen(buff) > 0 && atoi(buff) > 0) {
        nmea_log_enable = TRUE;
    }
#endif

    memset(&gnss_info, 0, sizeof(gnss_info_t));
    memcpy(gnss_info.dev_name, argv[2], strlen(argv[2]));
    gnss_info.state = GNSS_STATE_CLOSE;
    if(!strcmp(argv[1], GNSS_ID_6228)) {
        gnss_info.gnss_id = GNSS_TYPE_6228;
        gnss_info.auto_open = (bool)atoi(argv[3]);
        gnss_info.auto_dl_fw = TRUE;
        gnss_info.dl_befor_open = FALSE;
        gnss_info.gnss_dev_open = gnss_6228_dev_open;
        gnss_info.gnss_dev_close = gnss_6228_dev_close;
        gnss_info.gnss_open = gnss_6228_open;
        gnss_info.gnss_close = gnss_6228_close;
        gnss_info.gnss_fw_dl = gnss_6228_fw_dl;
        gnss_info.gnss_dl_read_cb = gnss_6228_dl_read_cb;
        gnss_info.gnss_set = gnss_6228_set;
        gnss_info.gnss_set_cb = gnss_6228_set_cb;
    } else if(!strcmp(argv[1], GNSS_ID_8122)) {
        gnss_info.gnss_id = GNSS_TYPE_8122;
        gnss_info.auto_open = (bool)atoi(argv[3]);
        gnss_info.auto_dl_fw = FALSE;
        gnss_info.dl_befor_open = FALSE;
        gnss_info.gnss_dev_open = gnss_8122_dev_open;
        gnss_info.gnss_dev_close = gnss_8122_dev_close;
        gnss_info.gnss_open = gnss_8122_open;
        gnss_info.gnss_close = gnss_8122_close;
        gnss_info.gnss_fw_dl = gnss_8122_fw_dl;
        gnss_info.gnss_dl_read_cb = NULL;
        gnss_info.gnss_set = gnss_8122_set;
        gnss_info.gnss_set_cb = gnss_8122_set_cb;
    } else if(!strcmp(argv[1], GNSS_ID_5311)) {
        gnss_info.gnss_id = GNSS_TYPE_5311;
        gnss_info.auto_open = (bool)atoi(argv[3]);
        gnss_info.auto_dl_fw = TRUE;
        gnss_info.dl_befor_open = TRUE;
        gnss_info.gnss_dev_open = gnss_5311_dev_open;
        gnss_info.gnss_dev_close = gnss_5311_dev_close;
        gnss_info.gnss_open = gnss_5311_open;
        gnss_info.gnss_close = gnss_5311_close;
        gnss_info.gnss_fw_dl = gnss_5311_fw_dl;
        gnss_info.gnss_dl_read_cb = NULL;
        gnss_info.gnss_set = gnss_5311_set;
        gnss_info.gnss_set_cb = gnss_5311_set_cb;
    } else if(!strcmp(argv[1], GNSS_ID_N50DB)) {
        gnss_info.gnss_id = GNSS_TYPE_N50DB;
        gnss_info.auto_open = (bool)atoi(argv[3]);
        gnss_info.auto_dl_fw = FALSE;
        gnss_info.dl_befor_open = FALSE;
        gnss_info.gnss_dev_open = gnss_n50db_dev_open;
        gnss_info.gnss_dev_close = gnss_n50db_dev_close;
        gnss_info.gnss_open = gnss_n50db_open;
        gnss_info.gnss_close = gnss_n50db_close;
        gnss_info.gnss_fw_dl = gnss_n50db_fw_dl;
        gnss_info.gnss_dl_read_cb = NULL;
        gnss_info.gnss_set = gnss_n50db_set;
        gnss_info.gnss_set_cb = gnss_n50db_set_cb;
    } else {
        LOGE("No support : %s", argv[1]);
        return -1;
    }

    LOGD("GNSS : %s, Device: %s", argv[1], gnss_info.dev_name);
    // Auto open gnss.
    if(gnss_info.auto_open) {
        int init_mode = atoi(argv[3]);
        if(((GNSS_PRINT_PORT_UART1 | GNSS_PRINT_PORT_USB_NMEA | GNSS_PRINT_PORT_USB_AT | GNSS_PRINT_PORT_TTY_AT) & init_mode) != init_mode) {
            init_mode = 0;
        }
        if(gnss_init((uint32)init_mode)) {
            LOGE("gnss_init() fail.");
            // return -1;
        }
    } else {
        gnss_info.print_port = 0;
    }

    // Init ubus and waitting IPC commands.
#ifdef MBTK_GNSS_UBUS_ENABLE
    if(gnss_ubus_init()) {
        LOGD("main() run...");
        uloop_run();
    } else {
        LOGE("gnss_ubus_init() fail.");
    }
#else
    if(!gnss_ipc_service_start()) {
        LOGD("main() run...");
        while(1) {
            sleep(24 * 60 * 60);
        }
    } else {
        LOGE("gnss_ipc_service_start() fail.");
    }
#endif

    LOGD("main() exit.");
    return 0;
}