mbtk/mbtk_gnssd/gnss_utils.c

#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <errno.h>
#include <fcntl.h>
#include <termios.h>
#include <string.h>
#include <stdarg.h>
#include <pty.h>
#include <libubox/uloop.h>
#include <time.h>
#include <sys/time.h>

#include "gnss_utils.h"
#include "mbtk_log.h"
#include "gnss_log.h"
#include "mbtk_gnss.h"

#define DATABITS    CS8
#define STOPBITS    0
#define PARITYON    0
#define PARITY      0
#define MBTK_SLAVE_DEV_NAME_MAX_LEN       24

int uart_baud_get(int baud)
{
    int rate = 0;
    switch(baud)
    {
        case 300:
            rate = B300;
            break;
        case 600:
            rate = B600;
            break;
        case 1200:
            rate = B1200;
            break;
        case 2400:
            rate = B2400;
            break;
        case 4800:
            rate = B4800;
            break;
        case 9600:
            rate = B9600;
            break;
        case 19200:
            rate = B19200;
            break;
        case 38400:
            rate = B38400;
            break;
        case 57600:
            rate = B57600;
            break;
        case 115200:
            rate = B115200;
            break;
        case 230400:
            rate = B230400;
            break;
        case 460800:
            rate = B460800;
            break;
        case 921600:
            rate = B921600;
            break;
        case 1500000:
            rate = B1500000;
            break;
        case 2000000:
            rate = B2000000;
            break;
        case 3000000:
            rate = B3000000;
            break;
        case 4000000:
            rate = B4000000;
            break;
        default:
            rate = B115200;
            break;
    }

    return rate;
}

int gnss_port_open(const char *dev, int flag, int baud, bool tty)
{

    int fd = -1;
    if((fd = open(dev, flag)) < 0)
    {
        LOGE("Open %s fail errno = [%d].", dev, errno);
        return -1;
    }

    LOGD("Open %s success.", dev);
    if (tty)
    {
        int rate = uart_baud_get(baud);
        /* set newtio */
        struct termios newtio;
        memset(&newtio, 0, sizeof(newtio));
        //(void)fcntl(fd, F_SETFL, 0);
        /* no flow control for uart by default */
        newtio.c_cflag = rate | DATABITS | STOPBITS | PARITYON | PARITY | CLOCAL | CREAD;
        newtio.c_iflag = IGNPAR;
        //newtio.c_iflag &= ~(BRKINT | ICRNL | INPCK | ISTRIP | IXON);
        newtio.c_oflag = 0;
        newtio.c_lflag = 0;    /* disable ECHO, ICANON, etc... */

        newtio.c_cc[VERASE]   = 0x8;      /* del */
        newtio.c_cc[VEOF]     = 4;      /* Ctrl-d */
        newtio.c_cc[VMIN]     = 1;      /* blocking read until 1 character arrives */
        newtio.c_cc[VEOL]     = 0xD;      /* '\0' */

        tcflush(fd, TCIOFLUSH);
        tcsetattr(fd, TCSANOW, &newtio);
    }

    return fd;
}

int gnss_port_close(int fd)
{
    if(fd > 0)
    {
        close(fd);
    }
    return 0;
}

int gnss_set_baudrate(int fd, int baudrate)
{
	struct termios options, oldtio;

	if(fcntl(fd, F_SETFL, 0) < 0) {
		LOGE("fcntl failed!");
		return -1;
	}

	if(tcgetattr(fd, &oldtio) != 0) {
		LOGE("setup serial error!");
		return -1;
	}

	/* Get the current options for the port... */
	tcgetattr(fd, &options);

	/* Set the baud rates to baudrate... */
	cfsetispeed(&options,baudrate);
	cfsetospeed(&options,baudrate);
	tcsetattr(fd, TCSANOW, &options);

	if (0 != tcgetattr(fd, &options))
	{
		LOGE("get options error!");
		return -1;
	}

	/*
	 * 8bit Data,no partity,1 stop bit...
	 */
	options.c_cflag &= ~PARENB;//无奇偶校验
	options.c_cflag &= ~CSTOPB;//停止位，1位
	options.c_cflag &= ~CSIZE; //数据位的位掩码
	options.c_cflag |= CS8;    //数据位，8位

	cfmakeraw(&options);

	/*
	 * Set the new options for the port...
	 */
	if (tcsetattr(fd, TCSANOW, &options) != 0)
	{
		LOGE("setup serial error!");
		return -1 ;
	}

	return 0 ;
}

uint16 get_crc16(const char *ptr, uint16 count)
{
	uint16 crc, i;

	crc = 0;
	while(count--)
	{
		crc = crc ^ (int) *ptr++ << 8;

		for(i = 0; i < 8; i++)
		{
			if(crc & 0x8000)
				crc = crc << 1 ^ 0x1021;
			else
				crc = crc << 1;
		}
	}

	return (crc & 0xFFFF);
}

int gnss_pty_open(int *master_fd, int *slave_fd, const char *dev)
{
    int flags = -1;
    int ret = -1;
    if(*master_fd > 0) {
        LOGD("PTY has inited.");
        return 0;
    }
    char spty_name[MBTK_SLAVE_DEV_NAME_MAX_LEN] = {0};
    int result = openpty(master_fd, slave_fd, spty_name, NULL, NULL);
    if (-1 == result) {
        LOGE("Failed to get a pty.");
        return -1;
    }

    LOGD("Get a pty pair, FD -- master[%d] slave[%d]", *master_fd, *slave_fd);
    LOGD("Slave name is:%s", spty_name);

    if(access(dev, F_OK) == -1)
    {
        LOGD("symlink %s -> %s", spty_name, dev);
        result = symlink(spty_name, dev);
        if (-1 == result) {
            LOGE("symlink error.");
            goto ERROR;
        }
    }

    flags = fcntl(*master_fd, F_GETFL);
    if (flags == -1)
    {
        LOGE("fcntl get error.");
        goto ERROR;
    }
    flags |= O_NONBLOCK;
    flags |= O_NOCTTY;
    ret = fcntl(*master_fd, F_SETFL, flags);
    if(ret == -1)
    {
        LOGE("fcntl set error.");
        goto ERROR;
    }

    if (1) {
		/* set newtio */
		struct termios newtio;
		memset(&newtio, 0, sizeof(newtio));
		/* no flow control for uart by default */
		newtio.c_cflag = B115200 | CRTSCTS | DATABITS | STOPBITS | PARITYON | PARITY | CLOCAL | CREAD;
		newtio.c_iflag = IGNPAR;
                //newtio.c_iflag &= ~(BRKINT | ICRNL | INPCK | ISTRIP | IXON);
		newtio.c_oflag = 0;
		newtio.c_lflag = 0;    /* disable ECHO, ICANON, etc... */

		newtio.c_cc[VERASE]   = 0x8;      /* del */
		newtio.c_cc[VEOF]     = 4;      /* Ctrl-d */
		newtio.c_cc[VMIN]     = 1;      /* blocking read until 1 character arrives */
		newtio.c_cc[VEOL]     = 0xD;      /* '\0' */

		tcflush(*master_fd, TCIFLUSH);
		tcsetattr(*master_fd, TCSANOW, &newtio);
	}
    return 0;

ERROR:
    if (0 < *master_fd) {
        close(*master_fd);
        *master_fd = -1;
    }

    if (0 < *slave_fd) {
        close(*slave_fd);
        *slave_fd = -1;
    }

    return -1;
}

int gnss_nmea_sscanf(const char *str, char *ret,...)
{
    const char *ptr = str;
    char *argv[16];
    int argc;
    va_list ap;
    int i = 0;

    va_start(ap, ret);
    argc = 0;
    argv[argc] = ret; // First arg.

    do {
        i = 0;
        while(*ptr && *ptr != ',' && *ptr != '*') {
            argv[argc][i++] = *ptr++;
        }
        ptr++; // Jump ',' or '*'
        argc++;
    } while((argv[argc] = va_arg(ap, char*)) != 0);

    va_end(ap);

    return argc;
}

void gnssStartTimer(struct uloop_timeout *timeout, int timeVal)
{
    //UNUSED(timeout);
    LOGD("%s: timeVal=%lu.", __FUNCTION__, timeVal);
    uloop_timeout_set(timeout, timeVal);
    return;
}

void gnssStopTimer(struct uloop_timeout *timeout)
{
    //UNUSED(timeout);
    uloop_timeout_cancel(timeout);
    return;
}

#if MBTK_GNSS_PARAM_PARSE
static bool mbtk_gnss_time_set_flag = 0;

extern long timezone;

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

static time_t nmea_get_sec(mbtk_token date, mbtk_token time)
{
    char tmp_char[4] = {0};
    struct tm tmp_time;

    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;

#if 0
    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);
#endif

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

    

    return _t;
}

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

int gnss_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')
        {
            time_t _t = 0;
            _t = nmea_get_sec(tok_date, tok_time);
            if(_t < 0)
            {
                LOGD("nmea_update_date_time fail");
                return -1;
            }
            else
            {
#ifdef MBTK_GNSS_TIME_CHECK
                if(mbtk_check_num() == 0)
                {
                    mbtk_gnss_time_check(_t);
                    gnss_test_log("%s", data);
                }
#endif
                if( (mbtk_time_type_gnss_read() == LYNQ_TIME_TYPE_GNSS) && !mbtk_gnss_time_set_flag)
                {
#if 0
                    struct timeval tv;
                    tzset();   // auto set tz
                //    _t = _t - timezone;
                    LOGD("timestamp:%ld,  %ld", _t,  timezone);
                    tv.tv_sec = _t;

#else
                    LOGD("_t: %ld\n", _t);
                  struct tm CurlocalTime;
                  localtime_r(&_t, &CurlocalTime);
                  CurlocalTime.tm_hour += 8;    //cst+8 set for UTC
                  char dateTime[30];
                  strftime(dateTime, 30, "%Y-%m-%d %H:%M:%S %A", &CurlocalTime);
                  LOGD("dateTime:%s, %ld\n", dateTime, _t);

                  struct timeval tv;
                  tv.tv_sec = _t;
                  tv.tv_sec += 28800;  //cst 
                  tv.tv_usec = 0;

#endif
                    if(settimeofday(&tv, NULL)) {
                        LOGD("%s: 1111, Set time fail\n", __func__);
                        mbtk_gnss_time_set_flag = 0;
                    } else {
                        LOGD("%s: 1111, Set time success \n", __func__);
                        system("hwclock -w rtc0");
                        mbtk_gnss_time_set_flag = 1;
                    }
                }
            }
        }
    }

    return 0;
}
#endif