mbtk/mbtk_gnssd/gnss_hd8122.c

/*
*    gnss_hd8122.c
*
*    HD8122 GNSS source.
*
*/
/******************************************************************************

                          EDIT HISTORY FOR FILE

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

******************************************************************************/
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <errno.h>
#include <fcntl.h>

#include "mbtk_log.h"
#include "mbtk_type.h"
#include "mbtk_gpio.h"
#include "gnss_utils.h"
#include "gnss_hd8122.h"

#define UART_BITRATE_NMEA_DEF_FW    115200   // Default bitrate.
#define GNSS_POWER_GPIO 43
#define GNSS_SET_TIMEOUT           3000     // 3s
#define GNSS_PACK_BUFF_SIZE           1024


static pthread_cond_t read_cond;
static pthread_mutex_t read_mutex;
static bool setting_busy = FALSE;
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 uint16 fletcher16(const uint8_t* data, int data_len) {
    uint16_t sum1 = 0;
    uint16_t sum2 = 0;
    int index;

    for (index = 0; index < data_len; ++index ) {
        sum1 = (sum1 + data[index]) % 0xff;
        sum2 = (sum2 + sum1) % 0xff;
    }

    // ???
    sum2--;

    return (sum2 << 8) | sum1;
}

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);
        gnss_set_result = GNSS_ERR_TIMEOUT;
    }
    return;
}


static int pack_create(hd8122_id_type_enum id_type, uint8 id, uint16 data_len, const uint8 *data, uint8 *pack, int pack_len)
{
    if(pack == NULL || pack_len < HD8122_PACK_LEN_MIN) {
        return -1;
    }
    memset(pack, 0, pack_len);
    uint8 *data_ptr = pack;
    data_ptr += uint16_2_byte(HD8122_PACK_HEAD, data_ptr, false);
    *data_ptr++ = (uint8)id_type;
    *data_ptr++ = id;
    data_ptr += uint16_2_byte(data_len, data_ptr, false);
    if(data_len > 0) {
        memcpy(data_ptr, data, data_len);
        data_ptr += data_len;
    }
    data_ptr += uint16_2_byte(fletcher16(pack + 2, 4 + data_len), data_ptr, false);
    return (data_ptr - pack);
}

static void gnss_cmd_rsp_process(const void *data, int data_len) {
    const char *ptr = (const char*)data;
    log_hex("RSP", data, data_len);
    if(0)
    {
        mbtk_timer_clear();

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

static gnss_err_enum gnss_8122_reset(int fd, int reset)
{
    uint8 buff[GNSS_PACK_BUFF_SIZE];
    LOGD("RESET");
    int len = pack_create(HD8122_ID_TYPE_CFG, HD8122_ID_CFG_SIMPLERST, 1, (uint8*)(&reset), buff, sizeof(buff));
    if(len <= 0) {
        LOGE("pack_create() fail.");
        return GNSS_ERR_ARG;
    }
    log_hex("PACK", buff, len);
    gnss_write(fd, buff, len);
    return GNSS_ERR_OK;
}

int gnss_8122_dev_open()
{
    return mbtk_gpio_value_set(GNSS_POWER_GPIO, MBTK_GPIO_DIRECT_OUT, 1);
}

int gnss_8122_dev_close()
{
    return mbtk_gpio_value_set(GNSS_POWER_GPIO, MBTK_GPIO_DIRECT_OUT, 0);
}

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

int gnss_8122_fw_dl()
{
    return 0;
}

void gnss_8122_set_cb(const void *data, int data_len)
{
    const char *buff = (const char*)data;
    if(setting_busy) {
        gnss_cmd_rsp_process(data, data_len);
    }
}

gnss_err_enum gnss_8122_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;
        mbtk_timer_set(gnss_set_timer_cb, GNSS_SET_TIMEOUT);

        if(memcmp(cmd, "reset", 5) == 0) {
            gnss_set_result = gnss_8122_reset(fd, atoi(cmd + 6));
            if(gnss_set_result != GNSS_ERR_OK) {
                goto set_fail;
            }
            should_wait_rsp = FALSE;
        }
        else
        {
            LOGW("Unknown cmd:%s", cmd);
            gnss_set_result = GNSS_ERR_UNSUPPORT;
            goto set_fail;
        }

set_success:
        if(should_wait_rsp) {
            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;
    }
}