| /* | |
| * 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__); | |
| mbtk_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) { | |
| mbtk_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(); | |
| mbtk_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; | |
| } | |
| } | |