mbtk/mbtk_gnssd/gnss_6228.c

#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <errno.h>
#include <fcntl.h>
#include <termios.h>
#include <fcntl.h>
#include <string.h>
#include <pthread.h>

#include "gnss_6228.h"
#include "gnss_utils.h"
#include "mbtk_str.h"
#include "mbtk_log.h"
#include "mbtk_utils.h"

typedef enum {
    GNSS_6228_STATE_NON,
    GNSS_6228_STATE_WAIT_YC,
    GNSS_6228_STATE_BL_DL,
    GNSS_6228_STATE_FW_DL,
    GNSS_6228_STATE_SETTING
} gnss_6228_state_enum;

typedef enum {
    XMODEM_STATE_C_NAK_WAIT,
    XMODEM_STATE_DATA_SENDING,
    XMODEM_STATE_EOT_SENDING,
    XMODEM_STATE_C_NAK_GET,
    XMODEM_STATE_DATA_ACK_GET,
    XMODEM_STATE_DATA_NAK_GET,
    XMODEM_STATE_EOT_ACK_GET,
    XMODEM_STATE_EOT_NAK_GET,
    XMODEM_STATE_CAN_GET,
} xmodem_state_enum;

#define VMIN 6
#define GPS_DEV "/sys/devices/soc.0/d4000000.apb/mbtk-dev-op/gps_power"
#define GNSS_6228_BOOTLOADER    "/etc/mbtk/bootloader_r3.0.0_build6773_uartboot_921600.pkg"
#define GNSS_6228_FIRMWARE      "/etc/mbtk/UC6228CI-R3.5.2.19Build4370_mfg.pkg"

#define GNSS_SET_TIMEOUT           3000     // 3s

#define UART_BITRATE_DL_BL          230400
#define UART_BITRATE_DL_FW          921600
#define UART_BITRATE_NMEA_DEF_FW    9600   // Default bitrate.
#define UART_BITRATE_NMEA           460800

#define GNSS_6228_WAIT_YC_COUNT     40

#define XMODEM_SOH 0x01
#define XMODEM_STX 0x02
#define XMODEM_EOT 0x04
#define XMODEM_ACK 0x06
#define XMODEM_NAK 0x15
#define XMODEM_CAN 0x18
#define XMODEM_CRC_CHR	'C'
#define XMODEM_CRC_SIZE 2		/* Crc_High Byte + Crc_Low Byte */
#define XMODEM_FRAME_ID_SIZE 2 		/* Frame_Id + 255-Frame_Id */
#define XMODEM_DATA_SIZE_SOH 128  	/* for Xmodem protocol */
#define XMODEM_DATA_SIZE_STX 1024 	/* for 1K xmodem protocol */
#define USE_1K_XMODEM 1  		/* 1 for use 1k_xmodem 0 for xmodem */
#define	TIMEOUT_USEC	0
#define	TIMEOUT_SEC		10

#if (USE_1K_XMODEM)
#define XMODEM_DATA_SIZE 	XMODEM_DATA_SIZE_STX
#define XMODEM_HEAD		XMODEM_STX
#else
#define XMODEM_DATA_SIZE 	XMODEM_DATA_SIZE_SOH
#define XMODEM_HEAD 		XMODEM_SOH
#endif
#define XMODEM_PACK_SIZE   (XMODEM_DATA_SIZE + XMODEM_CRC_SIZE + XMODEM_FRAME_ID_SIZE + 1)




// Default bitrate.
static gnss_6228_state_enum gnss_state = GNSS_6228_STATE_WAIT_YC;
static xmodem_state_enum xmodem_state = XMODEM_STATE_C_NAK_WAIT;
static pthread_cond_t read_cond;
static pthread_mutex_t read_mutex;
static char xmodem_pack_buff[XMODEM_PACK_SIZE];
static int xmodem_pack_index = 0;
static void *gnss_set_rsp_ptr = NULL;
static gnss_err_enum gnss_set_result = GNSS_ERR_OK;


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

static void gnss_set_timer_cb(int signo)
{
    if(gnss_state == GNSS_6228_STATE_SETTING) {
        pthread_mutex_lock(&read_mutex);
        pthread_cond_signal(&read_cond);
        pthread_mutex_unlock(&read_mutex);
        gnss_set_result = GNSS_ERR_TIMEOUT;
    }
    return;
}

int gnss_init_config(int fd)
{
    char *send_buf = "$ANTSTAT,1\r\n";
    gnss_write(fd, send_buf, strlen(send_buf));
    send_buf = "$ANTSTAT1\r\n";
    gnss_write(fd, send_buf, strlen(send_buf));
    send_buf = "$PDTINFO\r\n";
    gnss_write(fd, send_buf, strlen(send_buf));
    send_buf = "$CFGNMEA\r\n";
    gnss_write(fd, send_buf, strlen(send_buf));
    send_buf = "$CFGPRT,1\r\n";
    gnss_write(fd, send_buf, strlen(send_buf));
    send_buf = "$CFGAID,0\r\n";
    gnss_write(fd, send_buf, strlen(send_buf));
    return 0;
}

static void gnss_cmd_rsp_process(const void *data, int data_len) {
    const char *ptr = (const char*)data;
    LOGD("Setting RSP : %s", ptr);
    if(memcmp(ptr, "$PDTINFO,", 9) == 0) {
        if(gnss_set_rsp_ptr) {
            gnss_6228_dev_info_t *dev_info = (gnss_6228_dev_info_t*)gnss_set_rsp_ptr;
            if(6 == gnss_nmea_sscanf(ptr + 9, dev_info->pdtName, dev_info->Config,
                        dev_info->hwVer, dev_info->fwVer, dev_info->PN, dev_info->SN)) {
                LOGD("$PDTINFO : %s,%s,%s,%s,%s,%s", dev_info->pdtName, dev_info->Config,
                        dev_info->hwVer, dev_info->fwVer, dev_info->PN, dev_info->SN);
            } else {
                LOGW("Parse $PDTINFO error.");
            }
        }
    }


    else if(memcmp(ptr, "$OK", 3) == 0 || memcmp(ptr, "$Fail", 5) == 0)
    {
        // $Fail,0*3E
        if(memcmp(ptr, "$Fail", 5) == 0) {
            int code = atoi(ptr + 6);
            if(code == 1) {
                gnss_set_result = GNSS_ERR_CHECKSUM;
            } else if(code == 0) {
                gnss_set_result = GNSS_ERR_ARG;
            } else {
                gnss_set_result = GNSS_ERR_UNKNOWN;
            }
        }

        mbtk_timer_clear();

        pthread_mutex_lock(&read_mutex);
        pthread_cond_signal(&read_cond);
        pthread_mutex_unlock(&read_mutex);
    }
}

int gnss_6228_dev_open()
{
    int fd, ret;
    fd = open(GPS_DEV, O_RDWR | O_TRUNC, 0644);
    if(fd < 0)
    {
        LOGE("[%s]  file [%s] open error\n", __FUNCTION__, GPS_DEV);
        return -1;
    }
    ret = write(fd, "on", 2);
    if (ret < 0)
    {
        LOGE("%s: error writing to file!\n", __FUNCTION__);
        close(fd);
        return -2;
    }

    close(fd);
    return 0;
}

int gnss_6228_dev_close(int uart_fd)
{
    int fd, ret;
    fd = open(GPS_DEV, O_RDWR | O_TRUNC, 0644);
    if(fd < 0)
    {
        LOGE("[%s]  file [%s] open error\n", __FUNCTION__, GPS_DEV);
        return -1;
    }
    ret = write(fd, "off", 3);
    if (ret < 0)
    {
        LOGE("%s: error writing to file!\n", __FUNCTION__);
        close(fd);
        return -2;
    }

    close(fd);
    return 0;
}

int gnss_6228_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_6228_close(int fd)
{
    pthread_mutex_destroy(&read_mutex);
    pthread_cond_destroy(&read_cond);
    return gnss_port_close(fd);
}

static void xmodem_state_change(const void *data, int data_len)
{
    // log_hex("XModem-RECV", data, data_len);

    pthread_mutex_lock(&read_mutex);
    const char *ptr = (const char*)data;
    if(ptr[0] == XMODEM_CAN) {
        LOGD("GET CAN message.");
        xmodem_state = XMODEM_STATE_CAN_GET;
        pthread_cond_signal(&read_cond);
        pthread_mutex_unlock(&read_mutex);
        return;
    }

    switch(xmodem_state) {
        case XMODEM_STATE_C_NAK_WAIT:
        {
#if 0
            if(ptr[0] == XMODEM_CRC_CHR || ptr[0] == XMODEM_NAK) {
                LOGD("GET C/NAK(X-Modem start...)");
                xmodem_state = XMODEM_STATE_C_NAK_GET;
                pthread_cond_signal(&read_cond);
            }
#else
            if(ptr[0] == XMODEM_CRC_CHR) {
                LOGD("GET C (X-Modem start...)");
                xmodem_state = XMODEM_STATE_C_NAK_GET;
                pthread_cond_signal(&read_cond);
            }
#endif
            break;
        }
        case XMODEM_STATE_DATA_SENDING:
        {
            if(ptr[0] == XMODEM_NAK) {
                xmodem_state = XMODEM_STATE_DATA_NAK_GET;
                pthread_cond_signal(&read_cond);
            } else if(ptr[0] == XMODEM_ACK) {
                xmodem_state = XMODEM_STATE_DATA_ACK_GET;
                pthread_cond_signal(&read_cond);
            }
            break;
        }
        case XMODEM_STATE_EOT_SENDING:
        {
            if(ptr[0] == XMODEM_NAK) {
                xmodem_state = XMODEM_STATE_EOT_NAK_GET;
                pthread_cond_signal(&read_cond);
            } else if(ptr[0] == XMODEM_ACK) {
                xmodem_state = XMODEM_STATE_EOT_ACK_GET;
                pthread_cond_signal(&read_cond);
            }
            break;
        }
        default:
        {
            break;
        }
    }
    pthread_mutex_unlock(&read_mutex);
}

void gnss_6228_dl_read_cb(const void *data, int data_len)
{
//    const char *buff = (const char*)data;
//    int index = 0;
    switch(gnss_state) {
        case GNSS_6228_STATE_WAIT_YC:
        {
#if 0
            static bool found_y = FALSE;
            while(index < data_len) {
                if(found_y) {
                    if(buff[index] == 'C') { // Found "YC"
                        gnss_state = GNSS_6228_STATE_BL_DL;
                        bool found_y = FALSE; // Should reset.
                        LOGD("Get YC");
                        break;
                    } else {
                        found_y = FALSE;  // Is "Yxxx" and not "YC".
                    }
                } else {
                    if(buff[index] == 'Y') { // Only found "Y"
                        found_y = TRUE;
                    }
                }
                index++;
            }
#else
            if(strstr(data, "YC")) {
                gnss_state = GNSS_6228_STATE_BL_DL;
                LOGD("Get YC");
            }
#endif
            break;
        }
        case GNSS_6228_STATE_BL_DL:
        case GNSS_6228_STATE_FW_DL:
        {
            xmodem_state_change(data, data_len);
            break;
        }
        default:
        {
            LOGW("Unknown 6228 state:%d", gnss_state);
            break;
        }
    }
}

void gnss_6228_set_cb(const void *data, int data_len)
{
//    const char *buff = (const char*)data;
    switch(gnss_state) {
        case GNSS_6228_STATE_SETTING:
        {
            gnss_cmd_rsp_process(data, data_len);
            break;
        }
        default:
        {
            break;
        }
    }
}


static int gnss_xmodem_pack_send(int uart_fd, int pack_index, char *data, int data_len)
{
    if(data_len > XMODEM_DATA_SIZE) {
        LOGE("X-Modem data lenght error:%d", data_len);
        return -1;
    }

    if(pack_index != xmodem_pack_index) {
        memset(xmodem_pack_buff, 0x1A, XMODEM_PACK_SIZE);
        xmodem_pack_buff[0] = XMODEM_HEAD;  // 帧开始字符
        xmodem_pack_buff[1] = (char)pack_index;  // 信息包序号
        xmodem_pack_buff[2] = (char)(255 - xmodem_pack_buff[1]);  // 信息包序号的补码

        memcpy(xmodem_pack_buff + 3, data, data_len);

        uint16 crc_value = get_crc16(xmodem_pack_buff + 3, XMODEM_DATA_SIZE); // 16位crc校验
        xmodem_pack_buff[XMODEM_DATA_SIZE+3] = (unsigned char)(crc_value >> 8);// 高八位数据
        xmodem_pack_buff[XMODEM_DATA_SIZE+4] = (unsigned char)(crc_value);     //低八位数据
        xmodem_pack_index = pack_index;
    }

    return gnss_write(uart_fd, xmodem_pack_buff, XMODEM_PACK_SIZE);
}

static int gnss_xmodem_send(int fd, char *file_name)
{
    xmodem_state = XMODEM_STATE_C_NAK_WAIT;

    bool running = TRUE;
    int result = 0;
    xmodem_pack_index = 0;
    int pack_index = 1;
    int file_fd = open(file_name, O_RDONLY);
    if(file_fd <= 0) {
        LOGE("Can't open (%s) or not exist!(errno=%d)", file_name, errno);
        return -1;
    }
    char buff[XMODEM_DATA_SIZE];
    int len = 0;

    pthread_mutex_lock(&read_mutex);
    while(running) {
        //LOGD("Waitting...");
        pthread_cond_wait(&read_cond, &read_mutex);
        //LOGD("Continue : %d", xmodem_state);
        switch(xmodem_state)
        {
            case XMODEM_STATE_C_NAK_GET:
            case XMODEM_STATE_DATA_ACK_GET:
            {
                len = read(file_fd, buff, XMODEM_DATA_SIZE);
                if(len > 0) {
                    if(XMODEM_PACK_SIZE != gnss_xmodem_pack_send(fd, pack_index, buff, len)) {
                        LOGE(
"Send xmodem package fail.");
                        running = FALSE;
                        result = -1;
                    }
                    xmodem_state = XMODEM_STATE_DATA_SENDING;
                } else {
                    LOGW("Read file complete[len = %d, errno = %d]", len, errno);
                    //running = FALSE;
                    char end_cmd = XMODEM_EOT;
                    if(gnss_write(fd, &end_cmd, 1) != 1) {
                        LOGE("Send EOT fail.");
                        //running = FALSE;
                    } else {
                        xmodem_state = XMODEM_STATE_EOT_SENDING;
                    }
                }
                pack_index++;
                break;
            }
            case XMODEM_STATE_DATA_NAK_GET:
            {
                // Retry
                if(XMODEM_PACK_SIZE != gnss_xmodem_pack_send(fd, pack_index - 1, NULL, XMODEM_DATA_SIZE)) {
                    LOGE(
"Retry send xmodem package fail.");
                    running = FALSE;
                    result = -1;
                }
                xmodem_state = XMODEM_STATE_DATA_SENDING;
                break;
            }

            case XMODEM_STATE_EOT_ACK_GET:
            {
                LOGD("Send EOT success.");
                running = FALSE;
                break;
            }
            case XMODEM_STATE_EOT_NAK_GET:
            {
                LOGW("Send EOT fail, retry...");
                char end_cmd = XMODEM_EOT;
                if(gnss_write(fd, &end_cmd, 1) != 1) {
                    LOGE("Send EOT fail.");
                    //running = FALSE;
                } else {
                    xmodem_state = XMODEM_STATE_EOT_SENDING;
                }
                break;
            }
            case XMODEM_STATE_CAN_GET:
            {
                LOGD("Recv CAN message.");
                running = FALSE;
                break;
            }
            default:
            {
                LOGE("Unknown X-Modem state : %d", xmodem_state);
                running = FALSE;
                result = -1;
                break;
            }
        }
    }
    pthread_mutex_unlock(&read_mutex);

    if(file_fd > 0) {
        close(file_fd);
    }
    return result;
}

int gnss_6228_fw_dl(int fd, const char *fw_name, const char *dev)
{
    // Set bootloader baudrate.
    gnss_set_baudrate(fd, uart_baud_get(UART_BITRATE_DL_BL));

    gnss_state = GNSS_6228_STATE_WAIT_YC;
    int wait_yc_num = 0;
    while(gnss_state == GNSS_6228_STATE_WAIT_YC && wait_yc_num < GNSS_6228_WAIT_YC_COUNT) {
        mbtk_write(fd, "M!T", 3);
        usleep(500);
        wait_yc_num++;
    }

    if(wait_yc_num >= GNSS_6228_WAIT_YC_COUNT) {
        LOGE("Wait YC timeout : %d / %d", wait_yc_num, GNSS_6228_WAIT_YC_COUNT);
        return -1;
    }

    // Start download bootloader.
    LOGD("Start download bootloader : %s", GNSS_6228_BOOTLOADER);
    gnss_state = GNSS_6228_STATE_BL_DL;
    if(gnss_xmodem_send(fd, GNSS_6228_BOOTLOADER)) {
        LOGE("Download bootloader fail.");
        return -1;
    }
    LOGD("Bootloader download success.");

    // Set firmware baudrate.
    gnss_set_baudrate(fd, uart_baud_get(UART_BITRATE_DL_FW));

    // Start download firmware.
    LOGD("Start download firmware : %s", GNSS_6228_FIRMWARE);
    gnss_state = GNSS_6228_STATE_FW_DL;
    if(gnss_xmodem_send(fd, GNSS_6228_FIRMWARE)) {
        LOGE("Download firmware fail.");
        return -1;
    }
    LOGD("Firmware download success.");

    // Set NMEA baudrate.
    gnss_set_baudrate(fd, uart_baud_get(UART_BITRATE_NMEA));

    return 0;
}

gnss_err_enum gnss_6228_set(int fd, const char *cmd, void *cmd_rsp, int cmd_rsp_len)
{
    gnss_state = GNSS_6228_STATE_SETTING;
    gnss_set_rsp_ptr = cmd_rsp;
    gnss_set_result = GNSS_ERR_OK;
    mbtk_timer_set(gnss_set_timer_cb, GNSS_SET_TIMEOUT);

    char cmd_tmp[128] = {0};
    snprintf(cmd_tmp, sizeof(cmd_tmp), "%s\r\n", cmd);
    gnss_write(fd, cmd_tmp, strlen(cmd_tmp));

    pthread_mutex_lock(&read_mutex);
    pthread_cond_wait(&read_cond, &read_mutex);
    pthread_mutex_unlock(&read_mutex);

    gnss_state = GNSS_6228_STATE_NON;
    return gnss_set_result;
}