mbtk/libmbtk_gnss/mbtk_gnss.c

/*
*    mbtk_gnss.c
*
*    MBTK GNSS API source.
*
*/
/******************************************************************************

                          EDIT HISTORY FOR FILE

  WHEN        WHO       WHAT,WHERE,WHY
--------    --------    -------------------------------------------------------
2024/7/11     LiuBin      Initial version

******************************************************************************/
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <errno.h>
#include <pthread.h>
#include <sys/socket.h>
#include <sys/un.h>
#include <netinet/in.h>
#include <fcntl.h>
#include <sys/epoll.h>

#include "mbtk_gnss_inter.h"
#include "mbtk_log.h"
#include "mbtk_utils.h"

#define GNSS_BUFF_SIZE 2048

static int gnss_cli_fd = -1;
static pthread_t read_thread_id;
static int exit_fd[2] = {-1};
static mbtk_gnss_callback_func gnss_cb = NULL;
static bool gnss_busy = FALSE;
static pthread_cond_t gnss_cond;
static pthread_mutex_t gnss_mutex;
static gnss_err_enum gnss_result;

static int sock_read(int fd, uint8 *msg, int data_len)
{
    memset(msg, 0, data_len);
    int len = 0;

    int read_len = 0;
    while(1)
    {
        len = read(fd, msg + read_len, data_len - read_len);
        if(len > 0)
        {
            read_len += len;
        }
        else if(len == 0)
        {
            LOGE("read() end.");
            break;
        }
        else
        {
            if(EAGAIN == errno)
            {
                LOGV("Read end, lenght = %d", read_len);
            }
            else
            {
                LOGE("read() error[%d].", errno);
            }
            break;
        }
    }

    if(read_len > 0)
    {
        // log_hex("DATA_RECV", msg, read_len);
        return read_len;
    }
    else
    {
        return -1;
    }
}

static int sock_write(int fd, uint8 *msg, int data_len)
{
    int len = 0;
    int write_len = 0;
    while(write_len < data_len)
    {
        len = write(fd, msg + write_len, data_len - write_len);
        if(len > 0)
        {
            write_len += len;
        }
        else if(len == 0)
        {
            LOGE("write() end.");
            break;
        }
        else
        {
            LOGE("write() error[%d].", errno);
            break;
        }
    }

    if(write_len > 0)
    {
        // log_hex("DATA_SEND", msg, write_len);
        return write_len;
    }
    else
    {
        return -1;
    }
}

static void gnss_timer_cb(int signo)
{
    if(gnss_busy) {
        pthread_mutex_lock(&gnss_mutex);
        pthread_cond_signal(&gnss_cond);
        pthread_mutex_unlock(&gnss_mutex);
        gnss_result = GNSS_ERR_TIMEOUT;
    }
    return;
}

static void gnss_rsp_process(const char *data, int data_len)
{
    int index = 0;
    char buff[GNSS_BUFF_SIZE];
    int buff_len = 0;
    while(index < data_len) {
        if(data[index] == MBTK_IND_START_FLAG) {
            memset(buff, 0, sizeof(buff));
            buff_len = 0;
        } else if(data[index] == MBTK_IND_END_FLAG) {
            buff[buff_len] = '\0';
            if(memcmp(MBTK_IND_LOCATION_TAG, buff, strlen(MBTK_IND_LOCATION_TAG)) == 0) {
                if(gnss_cb) {
                    gnss_cb(MBTK_GNSS_IND_LOCATION, buff, buff_len);
                }
            } else if(memcmp(MBTK_IND_NMEA_TAG, buff, strlen(MBTK_IND_NMEA_TAG)) == 0) {
                if(gnss_cb) {
                    gnss_cb(MBTK_GNSS_IND_NMEA, buff, buff_len);
                }
            } else {
                LOGD("RSP[len - %d] : %s", buff_len, buff);
                if(gnss_busy) {
                    // XXXXXX:<result>
                    char *ptr = strstr(buff, ":");
                    if(ptr) {
                        gnss_result = atoi(ptr + 1);
                    }
                    pthread_mutex_lock(&gnss_mutex);
                    pthread_cond_signal(&gnss_cond);
                    pthread_mutex_unlock(&gnss_mutex);
                }
            }
        } else {
            buff[buff_len++] = data[index];
        }
        index++;
    }
}

static void* gnss_read_run(void* arg)
{
    int epoll_fd = epoll_create(5);
    if(epoll_fd < 0)
    {
        LOGE("epoll_create() fail[%d].", errno);
        return NULL;
    }

    uint32 event = EPOLLIN | EPOLLET;
    struct epoll_event ev_cli, ev_exit;
    ev_cli.data.fd = gnss_cli_fd;
    ev_cli.events = event; //EPOLLIN | EPOLLERR | EPOLLET;
    epoll_ctl(epoll_fd,EPOLL_CTL_ADD,gnss_cli_fd,&ev_cli);

    ev_exit.data.fd = exit_fd[0];
    ev_exit.events = event; //EPOLLIN | EPOLLERR | EPOLLET;
    epoll_ctl(epoll_fd,EPOLL_CTL_ADD,exit_fd[0],&ev_exit);

    int nready = -1;
    struct epoll_event epoll_events[EPOLL_LISTEN_MAX];
    while(1)
    {
        nready = epoll_wait(epoll_fd, epoll_events, EPOLL_LISTEN_MAX, -1);
        if(nready > 0)
        {
            int i;
            for(i = 0; i < nready; i++)
            {
                LOGV("fd[%d] event = %x",epoll_events[i].data.fd, epoll_events[i].events);
                if(epoll_events[i].events & EPOLLHUP)   // Closed by server.
                {

                }
                else if(epoll_events[i].events & EPOLLIN)
                {
                    if(gnss_cli_fd == epoll_events[i].data.fd)  // Server data arrive.
                    {
                        char buff[GNSS_BUFF_SIZE + 1];
                        int len = sock_read(gnss_cli_fd, buff, GNSS_BUFF_SIZE);
                        if(len > 0) {
                            gnss_rsp_process(buff, len);
                        }
                    }
                    else if(exit_fd[0] == epoll_events[i].data.fd) //
                    {
                        char buff[100] = {0};
                        int len = read(exit_fd[0], buff, 100);
                        if(len > 0) {
                            LOGI("CMD : %s", buff);
                            if(strcmp(buff, "EXIT") == 0) {
                                goto read_thread_exit;
                            } else {
                                LOGD("Unkonw cmd : %s", buff);
                            }
                        } else {
                            LOGE("sock_read() fail.");
                        }
                    }
                    else
                    {
                        LOGW("Unknown socket : %d", epoll_events[i].data.fd);
                    }
                }
                else
                {
                    LOGW("Unknown event : %x", epoll_events[i].events);
                }
            }
        }
        else
        {
            LOGW("epoll_wait() fail[%d].", errno);
        }
    }

read_thread_exit:
    LOGD("info_read thread exit.");
    return NULL;
}

gnss_err_enum mbtk_gnss_init(mbtk_gnss_callback_func cb)
{
    if(gnss_cli_fd > 0) {
        LOGW("GNSS client has inited.");
        return GNSS_ERR_OK;
    }

    gnss_cli_fd = socket(AF_LOCAL, SOCK_STREAM, 0);
    if(gnss_cli_fd < 0)
    {
        LOGE("socket() fail[%d].", errno);
        goto error;
    }

    // Set O_NONBLOCK
    int flags = fcntl(gnss_cli_fd, F_GETFL, 0);
    if (flags < 0)
    {
        LOGE("Get flags error:%d", errno);
        goto error;
    }
    flags |= O_NONBLOCK;
    if (fcntl(gnss_cli_fd, F_SETFL, flags) < 0)
    {
        LOGE("Set flags error:%d", errno);
        goto error;
    }

    struct sockaddr_un cli_addr;
    memset(&cli_addr, 0, sizeof(cli_addr));
    cli_addr.sun_family = AF_LOCAL;
    strcpy(cli_addr.sun_path, SOCK_GNSS_PATH);
    if(connect(gnss_cli_fd, (struct sockaddr *)&cli_addr, sizeof(cli_addr)))
    {
        LOGE("connect() fail[%d].", errno);
        goto error;
    }

    if(pipe(exit_fd)) {
        LOGE("pipe() fail[%d].", errno);
        goto error;
    }

    if(pthread_create(&read_thread_id, NULL, gnss_read_run, NULL))
    {
        LOGE("pthread_create() fail.");
        goto error;
    }
    pthread_mutex_init(&gnss_mutex, NULL);
    pthread_cond_init(&gnss_cond, NULL);
    gnss_cb = cb;
    return GNSS_ERR_OK;

error:
    if(gnss_cli_fd > 0) {
        close(gnss_cli_fd);
        gnss_cli_fd = -1;
    }
    if(exit_fd[0] > 0) {
        close(exit_fd[0]);
        exit_fd[0] = -1;
    }
    if(exit_fd[1] > 0) {
        close(exit_fd[1]);
        exit_fd[1] = -1;
    }

    return GNSS_ERR_UNKNOWN;
}

gnss_err_enum mbtk_gnss_deinit()
{
    if(gnss_cli_fd < 0) {
        LOGW("GNSS client not inited.");
        return GNSS_ERR_UNKNOWN;
    }

    if(gnss_busy) {
        LOGE("BUSY");
        return GNSS_ERR_BUSY;
    }

    if(exit_fd[1] > 0) {
        write(exit_fd[1], "EXIT", 4);
    }

    int ret = pthread_join(read_thread_id, NULL);
    if(ret){
        LOGE("pthrad_join fail(%d)",ret);
        return GNSS_ERR_UNKNOWN;
    }

    close(gnss_cli_fd);
    gnss_cli_fd = -1;
    close(exit_fd[0]);
    exit_fd[0] = -1;
    close(exit_fd[1]);
    exit_fd[1] = -1;
    gnss_cb = NULL;
    gnss_busy = FALSE;
    pthread_mutex_destroy(&gnss_mutex);
    pthread_cond_destroy(&gnss_cond);

    return GNSS_ERR_OK;
}

gnss_err_enum mbtk_gnss_open(int type, int timeout)
{
    if(gnss_cli_fd < 0) {
        LOGW("GNSS client not inited.");
        return GNSS_ERR_UNKNOWN;
    }

    if(gnss_busy) {
        LOGE("BUSY");
        return GNSS_ERR_BUSY;
    } else {
        gnss_result = GNSS_ERR_OK;
        gnss_busy = TRUE;
        if(timeout > 0) {
            mbtk_timer_set(gnss_timer_cb, timeout * 1000);
        }

        // gnss_init:x
        char cmd[32] = {0};
        snprintf(cmd, sizeof(cmd), "gnss_init:%d", type);
        sock_write(gnss_cli_fd, cmd, strlen(cmd));

        pthread_mutex_lock(&gnss_mutex);
        pthread_cond_wait(&gnss_cond, &gnss_mutex);
        pthread_mutex_unlock(&gnss_mutex);

        gnss_busy = FALSE;

        return gnss_result;
    }
}

gnss_err_enum mbtk_gnss_close(int timeout)
{
    if(gnss_cli_fd < 0) {
        LOGW("GNSS client not inited.");
        return GNSS_ERR_UNKNOWN;
    }

    if(gnss_busy) {
        LOGE("BUSY");
        return GNSS_ERR_BUSY;
    } else {
        gnss_result = GNSS_ERR_OK;
        gnss_busy = TRUE;
        if(timeout > 0) {
            mbtk_timer_set(gnss_timer_cb, timeout * 1000);
        }

        // gnss_deinit
        char *cmd = "gnss_deinit";
        sock_write(gnss_cli_fd, cmd, strlen(cmd));

        pthread_mutex_lock(&gnss_mutex);
        pthread_cond_wait(&gnss_cond, &gnss_mutex);
        pthread_mutex_unlock(&gnss_mutex);

        gnss_busy = FALSE;

        return gnss_result;
    }
}


gnss_err_enum mbtk_gnss_setting(const char *setting_cmd, int timeout)
{
    if(gnss_cli_fd < 0) {
        LOGW("GNSS client not inited.");
        return GNSS_ERR_UNKNOWN;
    }

    if(gnss_busy) {
        LOGE("BUSY");
        return GNSS_ERR_BUSY;
    } else {
        gnss_result = GNSS_ERR_OK;
        gnss_busy = TRUE;
        if(timeout > 0) {
            mbtk_timer_set(gnss_timer_cb, timeout * 1000);
        }

        // gnss_setting:cmd
        char cmd[32] = {0};
        snprintf(cmd, sizeof(cmd), "gnss_setting:%s", setting_cmd);
        sock_write(gnss_cli_fd, cmd, strlen(cmd));

        pthread_mutex_lock(&gnss_mutex);
        pthread_cond_wait(&gnss_cond, &gnss_mutex);
        pthread_mutex_unlock(&gnss_mutex);

        gnss_busy = FALSE;

        return gnss_result;
    }
}

gnss_err_enum mbtk_gnss_dl(const char *fw_path, int timeout)
{
    if(gnss_cli_fd < 0) {
        LOGW("GNSS client not inited.");
        return GNSS_ERR_UNKNOWN;
    }

    if(gnss_busy) {
        LOGE("BUSY");
        return GNSS_ERR_BUSY;
    } else {
        gnss_result = GNSS_ERR_OK;
        gnss_busy = TRUE;
        if(timeout > 0) {
            mbtk_timer_set(gnss_timer_cb, timeout * 1000);
        }

        // gnss_dl:fw_name
        char cmd[32] = {0};
        snprintf(cmd, sizeof(cmd), "gnss_dl:%s", fw_path);
        sock_write(gnss_cli_fd, cmd, strlen(cmd));

        pthread_mutex_lock(&gnss_mutex);
        pthread_cond_wait(&gnss_cond, &gnss_mutex);
        pthread_mutex_unlock(&gnss_mutex);

        gnss_busy = FALSE;

        return gnss_result;
    }
}


gnss_err_enum mbtk_gnss_ind_set(uint32 gnss_ind, int timeout)
{
    if(gnss_cli_fd < 0) {
        LOGW("GNSS client not inited.");
        return GNSS_ERR_UNKNOWN;
    }

    if(gnss_busy) {
        LOGE("BUSY");
        return GNSS_ERR_BUSY;
    } else {
        gnss_result = GNSS_ERR_OK;
        gnss_busy = TRUE;
        if(timeout > 0) {
            mbtk_timer_set(gnss_timer_cb, timeout * 1000);
        }

        // gnss_ind:ind_type
        char cmd[32] = {0};
        snprintf(cmd, sizeof(cmd), "gnss_ind:%d", gnss_ind);
        sock_write(gnss_cli_fd, cmd, strlen(cmd));

        pthread_mutex_lock(&gnss_mutex);
        pthread_cond_wait(&gnss_cond, &gnss_mutex);
        pthread_mutex_unlock(&gnss_mutex);

        gnss_busy = FALSE;

        return gnss_result;
    }
}