mbtk/mbtk_gnssd/gnss_asr5311.c

/*
*    gnss_asr5311.c
*
*    ASR 5311 gnss support source.
*
*/
/******************************************************************************

                          EDIT HISTORY FOR FILE

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

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

#include "mbtk_log.h"
#include "mbtk_type.h"
#include "mbtk_gpio.h"
#include "mbtk_utils.h"
#include "gnss_utils.h"
#include "gnss_asr5311.h"

#define UART_BITRATE_NMEA_DEF_FW     115200   // Default bitrate.
#define GNSS_SET_TIMEOUT             3000     // 3s
#define GNSS_FW_DOWNLOAD_TIMER_VALUE 6
#define GNSS_PLATFORM_CTRL_PATH      "/sys/devices/platform/asr-gps/ctrl"
#define GNSS_PLATFORM_STATUS_PATH    "/sys/devices/platform/asr-gps/status"

static char config_msg_pm5[]    = "PMTEST,5";
static char config_msg_pm4[]    = "PMTEST,4";
static char config_msg_boot[]   = "START,1";

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 timer_t  gnss_fw_download_timer_id;
static int gnss_fw_download_timeout_state = 0;
static int gnss_fw_download_timer_init_state = 0;

int gnss_write(int fd, const void *data, int data_len);

static void gnss_fw_download_state(__attribute__( (unused)) union sigval sv)
{
    LOGD("aboot fw_download timeout");
    gnss_fw_download_timeout_state = 1;
    //log2file("%s: gnss fw download timeout\n", __func__);
    
    system("killall aboot-tiny");
}

static int gnss_fw_download_timer_init(void)
{
    int rc = 0;
    struct   sigevent    sigev;

    memset (&sigev, 0, sizeof (struct sigevent));
    sigev.sigev_value.sival_ptr = &gnss_fw_download_timer_id;
    sigev.sigev_notify = SIGEV_THREAD;
    sigev.sigev_notify_attributes = NULL;
    sigev.sigev_notify_function = gnss_fw_download_state;
    rc = timer_create(CLOCK_REALTIME, &sigev, &gnss_fw_download_timer_id);
    if(rc == 0)
    {
        gnss_fw_download_timer_init_state = 1;
    }
    LOGD("fw_download timer init %d", rc);
    return rc;
}

static int gnss_fw_download_timer_start(void)
{
    int                rc = 0;
    if(gnss_fw_download_timer_init_state != 1)
    {
        rc = gnss_fw_download_timer_init();
        if(rc != 0)
        {
            LOGD("gnss_fw_download_timer_init timer retry fail");
            return -1;
        }
    }
    
    struct itimerspec  ts;

    memset (&ts, 0x00, sizeof (struct itimerspec));
    ts.it_value.tv_sec = GNSS_FW_DOWNLOAD_TIMER_VALUE;
    ts.it_value.tv_nsec = 0;
    ts.it_interval.tv_sec = 0;
    ts.it_interval.tv_nsec = 0;
    rc = timer_settime(gnss_fw_download_timer_id, 0, &ts, NULL);
    gnss_fw_download_timeout_state = 0;
    LOGD("fw_download timer start %d", rc);
    return rc;
}

static int gnss_fw_download_timer_stop(void)
{
    if(gnss_fw_download_timer_init_state != 1)
    {
        LOGD("gps_fw_download_timer_stop timer not creat");
        return -1;
    }
    int rc = 0;
    struct   itimerspec  ts;

    memset (&ts, 0x00, sizeof (struct itimerspec));
    ts.it_value.tv_sec = 0;
    ts.it_value.tv_nsec = 0;
    ts.it_interval.tv_sec = 0;          
    ts.it_interval.tv_nsec = 0;
    rc = timer_settime(gnss_fw_download_timer_id, 0, &ts, NULL);
    LOGD("fw_download timer stop %d", rc);
    return rc;
}

static int gnss_send_cmd(int fd, const char *cmd, int cmd_len)
{
    unsigned char check_sum = 0;
    unsigned int index;
    int len  = 0;
    char cmd_buff[64] = {0};

    for(index = 0; index < strlen( (char *)cmd); index++)
        check_sum ^= cmd[index];

    sprintf((char *)cmd_buff, "$%s*%02x\r\n", cmd, check_sum);
    len = strlen((char *)cmd_buff)+1;
    return gnss_write(fd, cmd_buff, len);
}

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);
        if(setting_waitting)
        {
            setting_waitting = FALSE;
        }
        gnss_set_result = GNSS_ERR_TIMEOUT;
    }
    return;
}

static void gnss_hal_gpioctrl(char* str)
{
    int fd;
    fd = open(GNSS_PLATFORM_CTRL_PATH, O_WRONLY);

    if (fd >= 0) {
        write(fd, str, strlen(str));
        close(fd);
    }
}

static int gnss_module_wait_fw_download_done()
{
    int fd;
    char buf[64];
    int len;
    int ret = -1;

    fd = open(GNSS_PLATFORM_STATUS_PATH, O_RDONLY);
    if (fd >= 0)  {
        sleep(1);
        len = read(fd, buf, 64);
        LOGD("FW DONE %s, %d", buf, len);
        if(!memcmp(buf, "status: on", 10))  {
           /* FW Downloading is OK. */
            ret = 0;
        }

        close(fd);
    }

    if(!ret)
        gnss_hal_gpioctrl("off");

    return ret;
}

static void gnss_cmd_rsp_process(const void *data, int data_len)
{
    const char *ptr = (const char*)data;
    bool flag = FALSE;
    LOGD("Setting RSP : %s", ptr);
    if(memcmp(ptr, "$ACKOK", 6) == 0)
    {
        flag = TRUE;
    }
    else if(memcmp(ptr, "$ACKFAIL", 8) == 0)
    {
        //$ACKFAIL,1*56
        int code = *(ptr + 9) - '0';
        if(code == 1)
        {
            gnss_set_result = GNSS_ERR_FORMAT;
        }
        else if(code == 2)
        {
            gnss_set_result = GNSS_ERR_CHECKSUM;
        }
        else if(code == 3)
        {
            gnss_set_result = GNSS_ERR_ARG;
        }
        else
        {
            gnss_set_result = GNSS_ERR_UNKNOWN;
        }
        flag = TRUE;
    }
    else
    {
        //gnss_set_result = GNSS_ERR_UNKNOWN;
    }

    if(setting_waitting && flag)
    {
        setting_waitting = FALSE;
        mbtk_timer_clear();
        pthread_mutex_lock(&read_mutex);
        pthread_cond_signal(&read_cond);
        pthread_mutex_unlock(&read_mutex);
    }
}


int gnss_5311_dev_open()
{
    return 0;
}

int gnss_5311_dev_close(int fd)
{
    gnss_send_cmd(fd, config_msg_pm5, strlen(config_msg_pm5));
    return 0;
}

int gnss_5311_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_5311_close(int fd)
{
    pthread_mutex_destroy(&read_mutex);
    pthread_cond_destroy(&read_cond);

    return gnss_port_close(fd);
}

int gnss_5311_fw_dl(int fd, const char *fw_name, const char *dev)
{
    char cmd[256] = {0};
    if(memcmp(dev, "/dev/", 5) == 0) {
        snprintf(cmd, sizeof(cmd), "nice -n -10 aboot-tiny -B 2000000 -s %s -F /etc/jacana_fw.bin -P /etc/jacana_pvt.bin", dev + 5);
    } else {
        snprintf(cmd, sizeof(cmd), "nice -n -10 aboot-tiny -B 2000000 -s %s -F /etc/jacana_fw.bin -P /etc/jacana_pvt.bin", dev);
    }

    int num = 1;
	LOGD("aboot-tiny start");
retry_download:
    gnss_fw_download_timer_start();
    system(cmd);
    gnss_fw_download_timer_stop();
    if(gnss_fw_download_timeout_state == 1)
    {
        if(num < 3)
        {
            sleep(1);
            num++;
            goto retry_download;
        }
        else
        {
            gnss_fw_download_timeout_state = 0;
            LOGD("aboot-tiny timeout");
			LOGD("aboot-tiny stop");
            return -1;
        }
    }
    else
    {
        int ret = gnss_module_wait_fw_download_done();
        if(ret < 0)
        {
            if(num < 3)
            {
                num++;
                goto retry_download;
            }
            else
            {
                LOGD("aboot-tiny 3 download fail");
				LOGD("aboot-tiny stop");
                return -1;
            }
        }
    }
	
	LOGD("aboot-tiny stop");

    return 0;
}

void gnss_5311_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_5311_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;
        char send_cmd[128] = {0};
        int send_cmd_len = 0;

        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);
            if(mode == GNSS_RESET_TYPE_HOT || mode == GNSS_RESET_TYPE_WARM || mode == GNSS_RESET_TYPE_COLD) {
                memset(send_cmd, 0x0, 128);
                snprintf(send_cmd, 128, "RESET,%d", mode);
                send_cmd_len = strlen(send_cmd);
                LOGD("cmd: %s", send_cmd);
                gnss_send_cmd(fd, send_cmd, send_cmd_len);
            } else {
                gnss_set_result = GNSS_ERR_ARG;
                goto set_fail;
            }
            should_wait_rsp = TRUE;
        }
        else if(memcmp(cmd, "$NAVISYSCFG", 11) == 0) //$NAVISYSCFG,0x1,2
        {
            uint32 navisys = 0,navirate = 0;
            int ret_num = sscanf(cmd, "$NAVISYSCFG,%d,%d", &navisys, &navirate);
            if(ret_num < 1 || navisys == 0)
            {
                gnss_set_result = GNSS_ERR_ARG;
                goto set_fail;
            }
            
            uint32 new_mode = 0;
            if(ret_num >= 1)
            {
                if(((GNSS_SET_SYSCFG_GPS | GNSS_SET_SYSCFG_BDS | GNSS_SET_SYSCFG_GLO) & navisys) != navisys) {
                    gnss_set_result = GNSS_ERR_ARG;
                    goto set_fail;
                }

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

            if(ret_num == 2)
            {
                if((GNSS_SET_FREQCFG_1 != navirate) && (GNSS_SET_FREQCFG_2 != navirate) && (GNSS_SET_FREQCFG_5 != navirate) && (GNSS_SET_FREQCFG_10 != navirate))
                {
                    gnss_set_result = GNSS_ERR_ARG;
                    goto set_fail;
                }
            }

            memset(send_cmd, 0x0, 128);
            if(ret_num == 1)
            {
                snprintf(send_cmd, 128, "NAVISYSCFG,0x%X", new_mode);
            }
            else
            {
                snprintf(send_cmd, 128, "NAVISYSCFG,0x%X,%d", new_mode, navirate);
            }
            send_cmd_len = strlen(send_cmd);
            LOGD("cmd: %s", send_cmd);
            gnss_send_cmd(fd, send_cmd, send_cmd_len);
            should_wait_rsp = TRUE;
        }
        else if(memcmp(cmd, "$MSGCFG", 7) == 0)  //$MSGCFG,<mode>,<rate>
        {
            uint32 mode;
            uint32 msgmask = 0;
            int rate;
            if(2 == sscanf(cmd, "$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_GST | GNSS_SET_MSGCFG_GSV | GNSS_SET_MSGCFG_GLL | GNSS_SET_MSGCFG_ZDA
                    | GNSS_SET_MSGCFG_DHV | GNSS_SET_MSGCFG_TXT | GNSS_SET_MSGCFG_DTM) & mode) != mode) {
                    gnss_set_result = GNSS_ERR_ARG;
                    goto set_fail;
                }

                if(mode & GNSS_SET_MSGCFG_RMC)
                {
                    msgmask |= 0x00000001;
                }
                if(mode & GNSS_SET_MSGCFG_VTG)
                {
                    msgmask |= 0x00000002;
                }
                if(mode & GNSS_SET_MSGCFG_GGA)
                {
                    msgmask |= 0x00000004;
                }
                if(mode & GNSS_SET_MSGCFG_GSA)
                {
                    msgmask |= 0x00000008;
                }
                if(mode & GNSS_SET_MSGCFG_GSV)
                {
                    msgmask |= 0x00000010;
                }
                if(mode & GNSS_SET_MSGCFG_GLL)
                {
                    msgmask |= 0x00000020;
                }
                if(mode & GNSS_SET_MSGCFG_ZDA)
                {
                    msgmask |= 0x00000040;
                }
                if(mode & GNSS_SET_MSGCFG_GST)
                {
                    msgmask |= 0x00000080;
                }
                if(mode & GNSS_SET_MSGCFG_TXT)
                {
                    msgmask |= 0x00000100;
                }
                if(mode & GNSS_SET_MSGCFG_DHV)
                {
                    msgmask |= 0x00000200;
                }
                if(mode & GNSS_SET_MSGCFG_DTM)
                {
                    msgmask |= 0x00000400;
                }
                memset(send_cmd, 0x0, 128);
                snprintf(send_cmd, 128, "MSGCFG,0,0x%X,%d", msgmask, time);
                send_cmd_len = strlen(send_cmd);
                LOGD("cmd: %s", send_cmd);
                gnss_send_cmd(fd, send_cmd, send_cmd_len);
            }
            else
            {
                gnss_set_result = GNSS_ERR_ARG;
                goto set_fail;
            }

            should_wait_rsp = TRUE;
        } 
        else if(memcmp(cmd, "$MINEL", 6) == 0) //$MINEL,<elev>
        {
            uint32 elev = 0;
            elev = (uint32)atoi(cmd + 7);
            if(elev != GNSS_SET_EVEL_5 && elev != GNSS_SET_EVEL_15)
            {
                gnss_set_result = GNSS_ERR_ARG;
                goto set_fail;
            }

            memset(send_cmd, 0x0, 128);
            snprintf(send_cmd, 128, "MINEL,%d", elev);
            send_cmd_len = strlen(send_cmd);
            LOGD("cmd: %s", send_cmd);
            gnss_send_cmd(fd, send_cmd, send_cmd_len);
            should_wait_rsp = TRUE;
        } 
        else if(memcmp(cmd, "$NMEACFG", 8) == 0) // $NMEACFG,<ver>
        {
            uint32 mode,ver = 0;
            mode = (uint32)atoi(cmd + 9);
            if(mode == GNSS_SET_NMEAVER_4_00)
            {
                ver = 0x40;
            }
            else if(mode == GNSS_SET_NMEAVER_4_10)
            {
                ver = 0x41;
            }
            else
            {
                gnss_set_result = GNSS_ERR_ARG;
                goto set_fail;
            }

            memset(send_cmd, 0x0, 128);
            snprintf(send_cmd, 128, "NMEACFG,0x%X", ver);
            send_cmd_len = strlen(send_cmd);
            LOGD("cmd: %s", send_cmd);
            gnss_send_cmd(fd, send_cmd, send_cmd_len);
            should_wait_rsp = TRUE;
        }
        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;
    }
}