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 "mbtk_type.h"
#include "mbtk_log.h"
#include "gnss_info.h"

#include "gnss_6228.h"

#define GNSS_DEBUG 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"

#ifdef GNSS_DEBUG
#define GNSS_NMEA_FILE_LOG      "/tmp/mbtk_gnss_nmea.log"
#define GNSS_NMEA_FILE_LOG_MAX  10485760    // 10MB

#define GNSS_FILE_LOG           "/tmp/mbtk_gnss.log"
#define GNSS_FILE_LOG_MAX       10485760    // 10MB
#endif

gnss_info_t gnss_info;

struct ubus_context *gnss_ubus_init(void);
int gnss_init_config(int fd);

static char gnss_buff[GNSS_BUFF_SIZE*2] = {0};
static uint32 gnss_buff_len = 0;
static bool nmea_found = FALSE;
#ifdef GNSS_DEBUG
static bool nmea_log_enable = FALSE;
static int nmea_log_fd = -1;
static int nmea_log_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};

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

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

    // Only support 6228.
    if(strcmp(argv[1], GNSS_ID_6228)) {
        goto check_fail;
    }

    if(access(argv[2], R_OK | W_OK)) {
        goto check_fail;
    }

    if(strcmp(argv[3], "0") && strcmp(argv[3], "1")) {
        goto check_fail;
    }

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

static int gnss_ports_open(uint32 print_port)
{
    if(print_port & GNSS_PRINT_PORT_TTY_AT) {
        if(gnss_pty_open(&gnss_pty_master_fd, &gnss_pty_slave_fd, GNSS_PORT_PTY)) {
            return -1;
        }
        LOGD("Open PTY port success.");
    }

    if(print_port & GNSS_PRINT_PORT_USB_AT) {
        if((gnss_usb_at_port_fd = gnss_port_open(GNSS_PORT_USB_AT, O_RDWR | O_NONBLOCK | O_NOCTTY, 115200, FALSE)) <= 0) {
            return -1;
        }
        LOGD("Open USB AT port success.");
    }

    if(print_port & GNSS_PRINT_PORT_USB_NMEA) {
        if((gnss_usb_nmea_port_fd = gnss_port_open(GNSS_PORT_USB_NMEA, O_RDWR | O_NONBLOCK | O_NOCTTY, 115200, FALSE)) <= 0) {
            return -1;
        }
        LOGD("Open USB NMEA port success.");
    }

    if(print_port & GNSS_PRINT_PORT_UART1) {
        if((gnss_uart_at_port_fd = gnss_port_open(GNSS_PORT_UART_AT, O_RDWR | O_NONBLOCK | O_NOCTTY, 115200, TRUE)) <= 0) {
            return -1;
        }
        LOGD("Open UART AT port success.");
    }

    return 0;
}

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

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

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

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

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

    return 0;
}

static void nmea_print(const char *nmea, int nmea_len)
{
#ifdef GNSS_DEBUG
    if(nmea_log_enable){
        if(nmea_log_fd_len > GNSS_NMEA_FILE_LOG_MAX) {
            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.");
            }
            nmea_log_fd_len = 0;
        }

        if(nmea_log_fd > 0) {
            write(nmea_log_fd, nmea, nmea_len);
            nmea_log_fd_len += nmea_len;
        }
    }
#endif

    if(gnss_usb_at_port_fd > 0) {
        write(gnss_usb_at_port_fd, nmea, nmea_len);
    }

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

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

    if(gnss_pty_master_fd > 0) {
        write(gnss_pty_master_fd, nmea, nmea_len);
    }
}

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 void gnss_nmea_process(const char *data, int data_len)
{
    char nmea[GNSS_BUFF_SIZE] = {0};
    memcpy(nmea, data, data_len);

    if(!nmea_check(nmea, data_len)) {
        gnss_info.gnss_set_cb(nmea, data_len);
        return;
    }

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

    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 == '/')
        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);
        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])) {
                    gnss_buff_len = 0;
                    nmea_found = FALSE;
#if 0
                    if(isascii(data[index])) {
                        LOGD("\n\n###%c###%s\n\n", data[index], data);
                    }
#endif
                    continue;
                }

                if(data[index] != '\0') {
                    gnss_buff[gnss_buff_len++] = data[index];
                    if(gnss_buff[gnss_buff_len - 1] == '\n') {

                        if(gnss_buff_len > 6 && gnss_buff[gnss_buff_len - 5] == '*') { // $XXX*YY\r\n
                            gnss_nmea_process(gnss_buff, gnss_buff_len);
                        }

                        gnss_buff_len = 0;
                        nmea_found = FALSE;
                    }
                }
            } else {
                if(data[index] == '$') {
                    gnss_buff_len = 0;
                    nmea_found = TRUE;
                    gnss_buff[gnss_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_ZERO(&fdw);
    FD_ZERO(&fdr);
    FD_SET(gnss_info.fd, &fdr);
    fd_max = (gnss_info.fd > fd_max) ? gnss_info.fd : fd_max;
    FD_SET(gnss_info.exit_fd[0], &fdr);
    fd_max = (gnss_info.exit_fd[0] > fd_max) ? gnss_info.exit_fd[0] : fd_max;
    memset(gnss_buff, 0, sizeof(gnss_buff));
    gnss_buff_len = 0;
#if GNSS_DEBUG
    int debug_fd = -1;
    int debug_fd_len = 0;
    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.");
        }
        nmea_log_fd_len = 0;
    }
#endif

    while(gnss_info.state >= GNSS_STATE_OPEN) {
        ret = select(fd_max + 1, &fdr, &fdw, 0, NULL);
        //LOGD("select - %d", ret);
        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);
            if(len > 0) {
                //log_hex("READ", buffer, len);

#if GNSS_DEBUG
                if(nmea_log_enable){
                    if(debug_fd_len > GNSS_FILE_LOG_MAX) {
                        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;
                    }

                    if(debug_fd > 0) {
                        write(debug_fd, buffer, len);
                        debug_fd_len += 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))
        {
            memset(buffer, 0, GNSS_BUFF_SIZE);
            len = read(gnss_info.fd, 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, unsigned int buf_len)
{
    size_t size;
    size_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, &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);
        return 0;
    }

    int ret = 0;

    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 -1;
    }
    if(pipe(gnss_info.exit_fd)) {
        LOGE("pipe() fail[%d].", errno);
        return -1;
    }
    // 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;
    }

    if(gnss_info.auto_dl_fw) {
        gnss_info.state = GNSS_STATE_DOWNLOAD;
        ret = gnss_info.gnss_fw_dl(gnss_info.fd);
        if(ret) {
            LOGE("gnss_fw_dl() fail : %d", ret);
            goto exit_with_dev_close;
        }
    }

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

    LOGD("GNSS open success.");

    return gnss_ports_open(gnss_info.print_port);

exit_with_dev_close:
    if(gnss_info.gnss_dev_close()) {
        LOGE("gnss_dev_close() fail.");
    }
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 -1;
}

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

    // 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 -1;
    }

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

    if(gnss_info.gnss_dev_close()) {
        LOGE("gnss_dev_close() fail.");
        return -1;
    }

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

    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 0;
}

int gnss_set(const void* buf, unsigned int buf_len, void *cmd_rsp, int cmd_rsp_len)
{
    if(cmd_rsp && buf && buf_len > 0 && 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 -1;
    }
}

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 + \ (ÀàËÆ SIGINT £¬µ«Òª²úÉúcoreÎļþ)
        {
            LOGI("Exit by SIGQUIT.\n");
            exit(0);
        }
        case SIGTERM:// ĬÈÏkill   (ͬ SIGKILL £¬µ« SIGKILL ²»¿É²¶»ñ)
        {
            LOGI("Exit by SIGTERM.\n");
            exit(0);
        }
        case SIGTSTP:// Ctrl + Z (ͬ SIGSTOP £¬µ« SIGSTOP ²»¿É²¶»ñ)
        {
            LOGI("Exit by SIGTSTP.\n");
            exit(0);
        }
        case SIGSEGV: // Èç¿ÕÖ¸Õë
        {
            LOGI("Exit by SIGSEGV.\n");
            exit(0);
        }
        default:
        {
            LOGI("Unknown sig:%d\n",sig);
            break;
        }
    }
}


// mbtk_gnssd 6228 /dev/ttyS2 baud 0/1
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.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 {
        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) {
        if(gnss_init(0)) { // No print to any port.
            LOGE("gnss_init() fail.");
            return -1;
        }
    }

    // Init ubus and waitting IPC commands.
    if(gnss_ubus_init()) {
        LOGD("main() run...");
        uloop_run();
    } else {
        LOGE("gnss_ubus_init() fail.");
    }

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