| #include <stdio.h> |
| #include <stdlib.h> |
| #include <unistd.h> |
| #include <errno.h> |
| #include <pthread.h> |
| #include <fcntl.h> |
| #include <libubox/ustream.h> |
| #include <libubus.h> |
| #include <signal.h> |
| #include <cutils/properties.h> |
| #include <ctype.h> |
| #include <termios.h> |
| |
| #include "mbtk_type.h" |
| #include "mbtk_log.h" |
| #include "gnss_info.h" |
| #include "gnss_utils.h" |
| #include "gnss_6228.h" |
| #include "gnss_hd8122.h" |
| #include "gnss_asr5311.h" |
| #include "gnss_n50db.h" |
| #include "gnss_log.h" |
| #include "mbtk_utils.h" |
| |
| #define GNSS_DEBUG 1 |
| #define GNSS_UBUS_ENABLE 1 |
| |
| #define GNSS_TAG "MBTK_GNSS" |
| #define GNSS_BUFF_SIZE 2048 |
| #define MBTK_PROP_GNSS_LOG "persist.mbtk.gnss_log_enable" |
| #define GNSS_PORT_PTY "/dev/tty_gnss_nmea" |
| #define GNSS_PORT_USB_AT "/dev/ttyGS0" |
| #define GNSS_PORT_USB_NMEA "/dev/ttymodem0" |
| #define GNSS_PORT_UART_AT "/dev/ttyS1" |
| #define GNSS_CLI_IND_MAX 10 |
| |
| #ifdef GNSS_DEBUG |
| #define GNSS_NMEA_FILE_LOG "/tmp/mbtk_gnss_nmea.log" |
| #define GNSS_FILE_LOG "/tmp/mbtk_gnss.log" |
| #define GNSS_FILE_LOG_MAX 104857600 // 100MB |
| #endif |
| |
| #define MBTK_GNSS_PTY_AUTO_INIT 1 |
| |
| gnss_info_t gnss_info; |
| |
| #ifdef MBTK_GNSS_UBUS_ENABLE |
| struct ubus_context *gnss_ubus_init(void); |
| #else |
| int gnss_ipc_service_start(); |
| #endif |
| |
| int gnss_init_config(int fd); |
| |
| static char nmea_buff[GNSS_BUFF_SIZE*4] = {0}; |
| static char data_buff[GNSS_BUFF_SIZE*4] = {0}; |
| static uint32 nmea_buff_len = 0; |
| static uint32 data_buff_len = 0; |
| static gnss_ind_info_t ind_info[GNSS_CLI_IND_MAX]; |
| |
| static bool nmea_found = FALSE; |
| #ifdef GNSS_DEBUG |
| static bool nmea_log_enable = FALSE; |
| static int nmea_log_fd = -1; |
| static int debug_fd = -1; |
| static int debug_fd_len = 0; |
| #endif |
| |
| #if MBTK_GNSS_PTY_AUTO_INIT |
| static bool gnss_pty_print_enable = FALSE; |
| #endif |
| static int gnss_pty_master_fd = -1; |
| static int gnss_pty_slave_fd = -1; |
| static int gnss_usb_at_port_fd = -1; |
| static int gnss_usb_nmea_port_fd = -1; |
| static int gnss_uart_at_port_fd = -1; |
| static char *gnss_filter_info[] = {"RMC", "VTG", "GGA", "GSA", "GSV", "GLL", "ZDA", "GST", "TXT", "DHV", "DTM", NULL}; |
| |
| int gnss_write(int fd, const void* buf, int buf_len); |
| |
| static void help() |
| { |
| LOGD("mbtk_gnssd <6228/8122/5311/N50DB> <gnss_dev> <0/1>"); |
| } |
| |
| static int arg_check(int argc, char *argv[]) |
| { |
| if(argc != 4) { |
| LOGE("argc = %d", argc); |
| goto check_fail; |
| } |
| |
| // Only support 6228/8122/5311. |
| if(strcmp(argv[1], GNSS_ID_6228) && strcmp(argv[1], GNSS_ID_8122) && strcmp(argv[1], GNSS_ID_5311) |
| && strcmp(argv[1], GNSS_ID_N50DB)) { |
| LOGE("argv[1] = %s", argv[1]); |
| goto check_fail; |
| } |
| |
| if(access(argv[2], R_OK | W_OK)) { |
| LOGE("access(%s) rw fail. ", argv[2]); |
| goto check_fail; |
| } |
| |
| #if 0 |
| int init_mode = atoi(argv[3]); |
| if(((GNSS_PRINT_PORT_UART1 | GNSS_PRINT_PORT_USB_NMEA | GNSS_PRINT_PORT_USB_AT | GNSS_PRINT_PORT_TTY_AT) & init_mode) != init_mode) { |
| goto check_fail; |
| } |
| #endif |
| |
| return 0; |
| check_fail: |
| help(); |
| return -1; |
| } |
| |
| static int gnss_ports_open(uint32 print_port) |
| { |
| // TTY AT change. |
| if((gnss_info.print_port & GNSS_PRINT_PORT_TTY_AT) != (print_port & GNSS_PRINT_PORT_TTY_AT)) { |
| if(print_port & GNSS_PRINT_PORT_TTY_AT) { // Open |
| #if MBTK_GNSS_PTY_AUTO_INIT |
| gnss_pty_print_enable = TRUE; |
| #else |
| if(gnss_pty_open(&gnss_pty_master_fd, &gnss_pty_slave_fd, GNSS_PORT_PTY)) { |
| return GNSS_ERR_OPEN_DEV; |
| } |
| #endif |
| LOGD("Open PTY port success."); |
| } else { // Close |
| #if MBTK_GNSS_PTY_AUTO_INIT |
| gnss_pty_print_enable = FALSE; |
| #else |
| if(gnss_pty_slave_fd > 0) { |
| close(gnss_pty_slave_fd); |
| gnss_pty_slave_fd = -1; |
| unlink(GNSS_PORT_PTY); |
| } |
| #endif |
| LOGD("Close PTY port success."); |
| } |
| } |
| |
| // USB AT change. |
| if((gnss_info.print_port & GNSS_PRINT_PORT_USB_AT) != (print_port & GNSS_PRINT_PORT_USB_AT)) { |
| if(print_port & GNSS_PRINT_PORT_USB_AT) { // Open |
| if((gnss_usb_at_port_fd = gnss_port_open(GNSS_PORT_USB_AT, O_RDWR | O_NONBLOCK | O_NOCTTY, 115200, TRUE)) <= 0) { |
| return GNSS_ERR_OPEN_DEV; |
| } |
| LOGD("Open USB AT port success."); |
| } else { // Close |
| if(gnss_usb_at_port_fd > 0) { |
| close(gnss_usb_at_port_fd); |
| gnss_usb_at_port_fd = -1; |
| } |
| LOGD("Close USB AT port success."); |
| } |
| } |
| |
| // USB NMEA change. |
| if((gnss_info.print_port & GNSS_PRINT_PORT_USB_NMEA) != (print_port & GNSS_PRINT_PORT_USB_NMEA)) { |
| if(print_port & GNSS_PRINT_PORT_USB_NMEA) { // Open |
| if((gnss_usb_nmea_port_fd = gnss_port_open(GNSS_PORT_USB_NMEA, O_RDWR | O_NONBLOCK | O_NOCTTY, 115200, TRUE)) <= 0) { |
| return GNSS_ERR_OPEN_DEV; |
| } |
| LOGD("Open USB NMEA port success."); |
| } else { // Close |
| if(gnss_usb_nmea_port_fd > 0) { |
| close(gnss_usb_nmea_port_fd); |
| gnss_usb_nmea_port_fd = -1; |
| } |
| LOGD("Close USB NMEA port success."); |
| } |
| } |
| |
| // Uart AT change. |
| if((gnss_info.print_port & GNSS_PRINT_PORT_UART1) != (print_port & GNSS_PRINT_PORT_UART1)) { |
| if(print_port & GNSS_PRINT_PORT_UART1) { // Open |
| if((gnss_uart_at_port_fd = gnss_port_open(GNSS_PORT_UART_AT, O_RDWR | O_NONBLOCK | O_NOCTTY, 115200, TRUE)) <= 0) { |
| return GNSS_ERR_OPEN_DEV; |
| } |
| LOGD("Open UART AT port success."); |
| } else { // Close |
| if(gnss_uart_at_port_fd > 0) { |
| close(gnss_uart_at_port_fd); |
| gnss_uart_at_port_fd = -1; |
| } |
| LOGD("Close UART AT port success."); |
| } |
| } |
| |
| gnss_info.print_port = print_port; |
| |
| return GNSS_ERR_OK; |
| } |
| |
| static int gnss_ports_close() |
| { |
| if(gnss_usb_at_port_fd > 0) { |
| tcflush(gnss_usb_at_port_fd, TCIOFLUSH); |
| close(gnss_usb_at_port_fd); |
| gnss_usb_at_port_fd = -1; |
| } |
| |
| if(gnss_usb_nmea_port_fd > 0) { |
| tcflush(gnss_usb_nmea_port_fd, TCIOFLUSH); |
| close(gnss_usb_nmea_port_fd); |
| gnss_usb_nmea_port_fd = -1; |
| } |
| |
| if(gnss_uart_at_port_fd > 0) { |
| tcflush(gnss_uart_at_port_fd, TCIOFLUSH); |
| close(gnss_uart_at_port_fd); |
| gnss_uart_at_port_fd = -1; |
| } |
| #if MBTK_GNSS_PTY_AUTO_INIT |
| gnss_pty_print_enable = FALSE; |
| #else |
| if(gnss_pty_master_fd > 0) { |
| tcflush(gnss_pty_master_fd, TCIOFLUSH); |
| close(gnss_pty_master_fd); |
| gnss_pty_master_fd = -1; |
| } |
| |
| if(gnss_pty_slave_fd > 0) { |
| tcflush(gnss_pty_slave_fd, TCIOFLUSH); |
| close(gnss_pty_slave_fd); |
| gnss_pty_slave_fd = -1; |
| unlink(GNSS_PORT_PTY); |
| } |
| #endif |
| gnss_info.print_port = 0; |
| |
| return 0; |
| } |
| |
| #ifdef GNSS_DEBUG |
| static void log_save(int fd, const char *data, int data_len) |
| { |
| if(nmea_log_enable){ |
| if(0 /* debug_fd_len > GNSS_FILE_LOG_MAX */) { |
| LOGD("Reopen file:%s(len = %d)", GNSS_FILE_LOG, debug_fd_len); |
| close(debug_fd); |
| debug_fd = open(GNSS_FILE_LOG, O_WRONLY | O_CREAT | O_TRUNC, 0666); |
| if(debug_fd < 0) { |
| LOGE("Open debug fd fail."); |
| } |
| debug_fd_len = 0; |
| |
| LOGD("Reopen file:%s", GNSS_NMEA_FILE_LOG); |
| close(nmea_log_fd); |
| nmea_log_fd = open(GNSS_NMEA_FILE_LOG, O_WRONLY | O_CREAT | O_TRUNC, 0666); |
| if(nmea_log_fd < 0) { |
| LOGE("Open debug fd fail."); |
| } |
| } |
| |
| if(fd == nmea_log_fd) { |
| if(nmea_log_fd > 0) { |
| mbtk_write(nmea_log_fd, data, data_len); |
| debug_fd_len += data_len; |
| } |
| } else if(fd == debug_fd) { |
| if(debug_fd > 0) { |
| mbtk_write(debug_fd, data, data_len); |
| debug_fd_len += data_len; |
| } |
| } |
| } |
| } |
| #endif |
| |
| static void ind_location_print(const char *data) |
| { |
| int index = 0; |
| int buff_len = 0; |
| char buff[GNSS_BUFF_SIZE] = {0}; |
| buff[0] = MBTK_IND_START_FLAG; |
| buff_len++; |
| memcpy(buff + 1, MBTK_IND_LOCATION_TAG, strlen(MBTK_IND_LOCATION_TAG)); |
| buff_len += strlen(MBTK_IND_LOCATION_TAG); |
| memcpy(buff + strlen(buff), data, strlen(data)); |
| buff_len += strlen(data); |
| buff[strlen(buff)] = MBTK_IND_END_FLAG; |
| buff_len++; |
| |
| while(index < GNSS_CLI_IND_MAX) { |
| if(ind_info[index].cli_fd > 0 && (ind_info[index].ind_flag & MBTK_GNSS_IND_LOCATION)) { |
| gnss_write(ind_info[index].cli_fd, buff, buff_len); |
| } |
| index++; |
| } |
| } |
| |
| static void ind_nmea_print(const char *data) |
| { |
| int index = 0; |
| int buff_len = 0; |
| char buff[GNSS_BUFF_SIZE] = {0}; |
| buff[0] = MBTK_IND_START_FLAG; |
| buff_len++; |
| memcpy(buff + 1, MBTK_IND_NMEA_TAG, strlen(MBTK_IND_NMEA_TAG)); |
| buff_len += strlen(MBTK_IND_NMEA_TAG); |
| memcpy(buff + strlen(buff), data, strlen(data)); |
| buff_len += strlen(data); |
| buff[strlen(buff)] = MBTK_IND_END_FLAG; |
| buff_len++; |
| while(index < GNSS_CLI_IND_MAX) { |
| if(ind_info[index].cli_fd > 0 && (ind_info[index].ind_flag & MBTK_GNSS_IND_NMEA)) { |
| gnss_write(ind_info[index].cli_fd, buff, buff_len); |
| } |
| index++; |
| } |
| } |
| |
| static void nmea_print(const char *nmea, int nmea_len) |
| { |
| int ret = -1; |
| if(gnss_usb_at_port_fd > 0) { |
| ret = write(gnss_usb_at_port_fd, nmea, nmea_len); |
| } |
| |
| if(gnss_usb_nmea_port_fd > 0) { |
| ret = write(gnss_usb_nmea_port_fd, nmea, nmea_len); |
| if(ret < 0 && errno != EAGAIN) { |
| LOGE("gnss_port_fd write fail [errno = %d].", errno); |
| if(errno == EIO) { |
| tcflush(gnss_usb_nmea_port_fd, TCIOFLUSH); |
| close(gnss_usb_nmea_port_fd); |
| gnss_usb_nmea_port_fd = -1; |
| gnss_usb_nmea_port_fd = gnss_port_open(GNSS_PORT_USB_NMEA, O_RDWR | O_NONBLOCK | O_NOCTTY, 115200, TRUE); |
| } |
| } |
| } |
| |
| if(gnss_uart_at_port_fd > 0) { |
| ret = write(gnss_uart_at_port_fd, nmea, nmea_len); |
| } |
| |
| if(gnss_pty_master_fd > 0) { |
| #if MBTK_GNSS_PTY_AUTO_INIT |
| if(gnss_pty_print_enable) { |
| ret = write(gnss_pty_master_fd, nmea, nmea_len); |
| } |
| #else |
| ret = write(gnss_pty_master_fd, nmea, nmea_len); |
| #endif |
| } |
| ind_nmea_print(nmea); |
| ind_location_print(nmea); |
| } |
| |
| static unsigned char nmea_checksum(const char *nmea) |
| { |
| const char *p = nmea; |
| unsigned char chs = 0; |
| |
| while (*p == '$') // skip '$' |
| p++; |
| while (*p != '*' && *p != 0) |
| chs ^= *p++; |
| |
| return chs; |
| } |
| |
| static bool nmea_check(const char *nmea, int nmea_len) |
| { |
| char **ptr = gnss_filter_info; |
| while(*ptr) { |
| if(strstr(nmea, *ptr)) { |
| break; |
| } |
| ptr++; |
| } |
| |
| if(*ptr == NULL) { |
| LOGD("Unknown NMEA[%d]:%s", nmea_len, nmea); |
| return FALSE; |
| } |
| |
| char *checksum_str = strstr(nmea, "*"); |
| checksum_str++; // Jump '*' |
| char checksum_buf[3] = {0}; |
| snprintf(checksum_buf, 3, "%02x", nmea_checksum(nmea)); |
| if(strncasecmp(checksum_buf, checksum_str, 2)) { |
| LOGD("Checksum error[%d](checksum - %s):%s", nmea_len, checksum_buf, nmea); |
| return FALSE; |
| } |
| |
| return TRUE; |
| } |
| |
| static void gnss_nmea_process(const char *data, int data_len) |
| { |
| // LOGD("gnss_nmea_process() : data_len - %d", data_len); |
| #if 0 |
| char nmea[GNSS_BUFF_SIZE] = {0}; |
| memcpy(nmea, data, data_len); |
| #else |
| const char *nmea = data; |
| #endif |
| |
| if(!nmea_check(nmea, data_len)) { |
| // No print "$HOSTSLEEP". |
| if(memcmp(nmea, "$HOSTSLEEP", strlen("$HOSTSLEEP"))) { |
| LOGD("NO-NMEA:%s", nmea); |
| } |
| #if GNSS_DEBUG |
| log_save(nmea_log_fd, "/**/", 4); |
| log_save(nmea_log_fd, nmea, data_len); |
| #endif |
| if(gnss_info.gnss_set_cb) |
| gnss_info.gnss_set_cb(nmea, data_len); |
| return; |
| } |
| |
| #ifdef GNSS_DEBUG |
| if(nmea_log_enable) { |
| LOGD("NMEA[%d]:%s", data_len, nmea); |
| } |
| |
| log_save(nmea_log_fd, nmea, data_len); |
| #endif |
| |
| #if MBTK_GNSS_PARAM_PARSE |
| gnss_ind_nmea_parse(nmea, data_len); |
| #endif |
| |
| nmea_print(nmea, data_len); |
| } |
| |
| #if 0 |
| static void gnss_cmd_rsp_process(const char *data, int data_len) |
| { |
| char rsp[GNSS_BUFF_SIZE] = {0}; |
| memcpy(rsp, data, data_len); |
| LOGD("RSP[%d]:%s", data_len, rsp); |
| } |
| #endif |
| |
| static bool nmea_char_check(char ch) |
| { |
| if(isalnum(ch) || ch == '$' || ch == '\r' || ch == '\n' || ch == '.' |
| || ch == ',' || ch == '*' || ch == '\0' || ch == '/' || ch == '_' || ch == '=' || ch == '-') |
| return TRUE; |
| |
| return FALSE; |
| } |
| |
| static void gnss_data_process(const char *data, int data_len) |
| { |
| if(gnss_info.state == GNSS_STATE_OPEN) { |
| LOGD("GNSS_OPEN[%d]:%s", data_len, data); |
| } else if(gnss_info.state == GNSS_STATE_DOWNLOAD) { |
| // LOGD("GNSS_DL[%d]:%s", data_len, data); |
| if(gnss_info.gnss_dl_read_cb) { |
| gnss_info.gnss_dl_read_cb(data, data_len); |
| } |
| } else if(gnss_info.state == GNSS_STATE_READY) { |
| int index = 0; |
| while(index < data_len) { |
| if(nmea_found) { |
| if(!nmea_char_check(data[index])) { |
| // Copy nmea_buff to data_buff |
| // Start with '$', but not nmea data, so copy to data_buff. |
| memcpy(data_buff + data_buff_len, nmea_buff, nmea_buff_len); |
| data_buff_len += nmea_buff_len; |
| data_buff[data_buff_len++] = data[index]; |
| |
| nmea_buff_len = 0; |
| nmea_found = FALSE; |
| continue; |
| } |
| |
| if(data[index] != '\0') { |
| nmea_buff[nmea_buff_len++] = data[index]; |
| if(nmea_buff[nmea_buff_len - 1] == '\n') { |
| if(data_buff_len > 0) { |
| #if GNSS_DEBUG |
| log_save(nmea_log_fd, "/**/", 4); |
| log_save(nmea_log_fd, data_buff, data_buff_len); |
| #endif |
| if(gnss_info.gnss_set_cb) { |
| gnss_info.gnss_set_cb(data_buff, data_buff_len); |
| } |
| data_buff_len = 0; |
| } |
| |
| if(nmea_buff_len > 6 && nmea_buff[nmea_buff_len - 5] == '*') { // $XXX*YY\r\n |
| nmea_buff[nmea_buff_len] = '\0'; |
| gnss_nmea_process(nmea_buff, nmea_buff_len); |
| } else if(nmea_buff_len > 0) { |
| nmea_buff[nmea_buff_len] = '\0'; |
| if(memcmp(nmea_buff, "$HOSTSLEEP", strlen("$HOSTSLEEP"))) { |
| LOGD("NO-NMEA:%s", nmea_buff); |
| } |
| #if GNSS_DEBUG |
| log_save(nmea_log_fd, "/**/", 4); |
| log_save(nmea_log_fd, nmea_buff, nmea_buff_len); |
| #endif |
| } |
| |
| nmea_buff_len = 0; |
| nmea_found = FALSE; |
| } |
| } |
| } else { |
| if(data[index] == '$') { |
| nmea_buff_len = 0; |
| nmea_found = TRUE; |
| nmea_buff[nmea_buff_len++] = data[index]; |
| } else { |
| data_buff[data_buff_len++] = data[index]; |
| } |
| } |
| index++; |
| } |
| } else { |
| LOGW("Unknown state : %d", gnss_info.state); |
| } |
| } |
| |
| void* gnss_read_pthread(void* arg) |
| { |
| #if MBTK_GNSS_TEST_LOG |
| gnss_test_log("gnss_read_pthread enter."); |
| #endif |
| LOGD("gnss_read_pthread enter."); |
| |
| char buffer[GNSS_BUFF_SIZE]; |
| int len = 0; |
| int ret = 0; |
| fd_set fdr, fdw; |
| int fd_max = 0; |
| |
| fd_max = (gnss_info.fd > fd_max) ? gnss_info.fd : fd_max; |
| fd_max = (gnss_info.exit_fd[0] > fd_max) ? gnss_info.exit_fd[0] : fd_max; |
| memset(nmea_buff, 0, sizeof(nmea_buff)); |
| memset(data_buff, 0, sizeof(data_buff)); |
| nmea_buff_len = 0; |
| data_buff_len = 0; |
| #if GNSS_DEBUG |
| if(nmea_log_enable) { |
| debug_fd = open(GNSS_FILE_LOG, O_WRONLY | O_CREAT | O_TRUNC, 0666); |
| if(debug_fd < 0) { |
| LOGE("Open debug fd fail."); |
| } |
| nmea_log_fd = open(GNSS_NMEA_FILE_LOG, O_WRONLY | O_CREAT | O_TRUNC, 0666); |
| if(nmea_log_fd < 0) { |
| LOGE("Open nmea fd fail."); |
| } |
| debug_fd_len = 0; |
| } |
| #endif |
| |
| #if MBTK_GNSS_TEST_LOG |
| gnss_test_log("uart_fd - %d, exit_fd - %d", gnss_info.fd, gnss_info.exit_fd[0]); |
| #endif |
| LOGD("uart_fd - %d, exit_fd - %d", gnss_info.fd, gnss_info.exit_fd[0]); |
| |
| while(gnss_info.state >= GNSS_STATE_OPEN) { |
| FD_ZERO(&fdw); |
| FD_ZERO(&fdr); |
| FD_SET(gnss_info.fd, &fdr); |
| FD_SET(gnss_info.exit_fd[0], &fdr); |
| ret = select(fd_max + 1, &fdr, &fdw, 0, NULL); |
| //LOGD("select - %d", ret); |
| if(gnss_info.state < GNSS_STATE_OPEN) { |
| LOGD("State = %d, ret = %d", gnss_info.state, ret); |
| if(ret > 0) { |
| if (FD_ISSET(gnss_info.fd, &fdr)) { |
| LOGD("gnss_fd can read."); |
| } else if (FD_ISSET(gnss_info.exit_fd[0], &fdr)) { |
| LOGD("exit_fd can read."); |
| } else { |
| LOGW("Unknown select event."); |
| } |
| } |
| break; |
| } |
| |
| if (ret < 0) |
| { |
| if (errno == EINTR) |
| { |
| continue; |
| } |
| #if MBTK_GNSS_TEST_LOG |
| gnss_test_log("select error, errno = %d (%s)", errno, strerror(errno)); |
| #endif |
| LOGE("select error, errno = %d (%s)", errno, strerror(errno)); |
| break; |
| } |
| else if (ret == 0) |
| { |
| LOGE("select ret == 0"); |
| break; |
| } |
| |
| if (FD_ISSET(gnss_info.fd, &fdr)) |
| { |
| memset(buffer, 0, GNSS_BUFF_SIZE); |
| len = read(gnss_info.fd, buffer, GNSS_BUFF_SIZE - 1); |
| if(len > 0) { |
| //log_hex("READ", buffer, len); |
| |
| #if GNSS_DEBUG |
| //LOGD("read data_len = %d", len); |
| log_save(debug_fd, buffer, len); |
| #endif |
| |
| gnss_data_process(buffer, len); |
| |
| } else if(len ==0 ){ |
| LOGE("Read end : len = 0"); |
| break; |
| } else { |
| if(EAGAIN == errno) { |
| usleep(50000); |
| continue; |
| } else { |
| LOGD("Read ret = -1 ,errno = %d", errno); |
| break; |
| } |
| } |
| } |
| else if (FD_ISSET(gnss_info.exit_fd[0], &fdr)) |
| { |
| LOGD("exit_fd select event."); |
| memset(buffer, 0, GNSS_BUFF_SIZE); |
| len = read(gnss_info.exit_fd[0], buffer, GNSS_BUFF_SIZE); |
| if(len > 0) { |
| if(strcmp(buffer, "exit") == 0) { |
| LOGD("Get thread exit message."); |
| break; |
| } |
| } |
| } |
| else |
| { |
| #if MBTK_GNSS_TEST_LOG |
| gnss_test_log("Unknown select event."); |
| #endif |
| LOGW("Unknown select event."); |
| continue; |
| } |
| } |
| |
| #if GNSS_DEBUG |
| if(debug_fd > 0) { |
| close(debug_fd); |
| debug_fd = -1; |
| } |
| if(nmea_log_fd > 0) { |
| close(nmea_log_fd); |
| nmea_log_fd = -1; |
| } |
| #endif |
| |
| gnss_info.state = GNSS_STATE_CLOSE; |
| LOGD("gnss_read_pthread exit."); |
| return NULL; |
| } |
| |
| #if 0 |
| int gnss_write(int fd, const void *data, int data_len) |
| { |
| int count = 0; |
| int len = 0; |
| while(1) |
| { |
| len = write(fd, data + count, data_len - count); |
| if (len > 0) |
| { |
| count += len; |
| } |
| else |
| { |
| LOGE("write() fail,ret = %d,errno = %d", len, errno); |
| break; |
| } |
| |
| if (count == data_len) |
| break; |
| } |
| |
| return count; |
| } |
| #else |
| int gnss_write(int fd, const void* buf, int buf_len) |
| { |
| ssize_t size; |
| ssize_t size_to_wr; |
| ssize_t size_written; |
| if(GNSS_BUFF_SIZE < buf_len) |
| { |
| return -1; |
| } |
| for(size = 0; size < buf_len;) |
| { |
| size_to_wr = buf_len - size; |
| if( size_to_wr > GNSS_BUFF_SIZE) |
| size_to_wr = GNSS_BUFF_SIZE; |
| |
| size_written = write(fd, (const uint8*)buf + size, size_to_wr); |
| if (size_written==-1) |
| { |
| return -1; |
| } |
| size += size_written; |
| if(size_written != size_to_wr) |
| { |
| return size; |
| } |
| } |
| // LOGD("SEND %d / %d", size, buf_len); |
| return size; |
| } |
| #endif |
| |
| int gnss_init(uint32 print_port) |
| { |
| #if MBTK_GNSS_TEST_LOG |
| gnss_test_log("[gnss_init] gnss state [%d]", gnss_info.state); |
| #endif |
| |
| if(gnss_info.state != GNSS_STATE_CLOSE) { |
| LOGW("GNSS not close:%d", gnss_info.state); |
| if(gnss_info.state == GNSS_STATE_READY) { |
| LOGD("Reset print port : %d -> %d", gnss_info.print_port, print_port); |
| if(gnss_info.print_port != print_port) { |
| return gnss_ports_open(print_port); |
| } else { |
| return GNSS_ERR_OK; |
| } |
| } else { |
| return GNSS_ERR_OK; |
| } |
| } |
| |
| int ret = 0; |
| if(gnss_info.dl_befor_open) { |
| //if(gnss_info.auto_dl_fw) { |
| gnss_info.state = GNSS_STATE_DOWNLOAD; |
| ret = gnss_info.gnss_fw_dl(gnss_info.fd, NULL, gnss_info.dev_name); |
| if(ret) { |
| LOGE("gnss_fw_dl() fail : %d", ret); |
| gnss_info.state = GNSS_STATE_CLOSE; |
| return GNSS_ERR_DL_FW; |
| } |
| |
| gnss_info.fd = gnss_info.gnss_open(gnss_info.dev_name); |
| if(gnss_info.fd <= 0) { |
| LOGE("gnss_open(%s) fail : %d", gnss_info.dev_name, gnss_info.fd); |
| gnss_info.state = GNSS_STATE_CLOSE; |
| return GNSS_ERR_OPEN_DEV; |
| } |
| if(pipe(gnss_info.exit_fd)) { |
| LOGE("pipe() fail[%d].", errno); |
| return GNSS_ERR_UNKNOWN; |
| } |
| // GNSS is opened. |
| gnss_info.state = GNSS_STATE_OPEN; |
| |
| #if 0 |
| // Start gnss read thread. |
| pthread_attr_t thread_attr; |
| pthread_attr_init(&thread_attr); |
| if(pthread_attr_setdetachstate(&thread_attr, PTHREAD_CREATE_DETACHED)) |
| { |
| LOGE("pthread_attr_setdetachstate() fail."); |
| goto main_exit; |
| } |
| |
| if(pthread_create(&gnss_info.read_pid, &thread_attr, gnss_read_pthread, NULL)) |
| #else |
| if(pthread_create(&gnss_info.read_pid, NULL, gnss_read_pthread, NULL)) |
| #endif |
| { |
| LOGE("pthread_create() fail."); |
| goto exit_with_close; |
| } |
| |
| ret = gnss_info.gnss_dev_open(); |
| if(ret) { |
| LOGE("gnss_dev_open() fail : %d", ret); |
| goto exit_with_thread_exit; |
| } |
| //} |
| } else { |
| gnss_info.fd = gnss_info.gnss_open(gnss_info.dev_name); |
| if(gnss_info.fd <= 0) { |
| LOGE("gnss_open(%s) fail : %d", gnss_info.dev_name, gnss_info.fd); |
| gnss_info.state = GNSS_STATE_CLOSE; |
| return GNSS_ERR_OPEN_DEV; |
| } |
| if(pipe(gnss_info.exit_fd)) { |
| LOGE("pipe() fail[%d].", errno); |
| return GNSS_ERR_UNKNOWN; |
| } |
| // GNSS is opened. |
| gnss_info.state = GNSS_STATE_OPEN; |
| |
| #if 0 |
| // Start gnss read thread. |
| pthread_attr_t thread_attr; |
| pthread_attr_init(&thread_attr); |
| if(pthread_attr_setdetachstate(&thread_attr, PTHREAD_CREATE_DETACHED)) |
| { |
| LOGE("pthread_attr_setdetachstate() fail."); |
| goto main_exit; |
| } |
| |
| if(pthread_create(&gnss_info.read_pid, &thread_attr, gnss_read_pthread, NULL)) |
| #else |
| if(pthread_create(&gnss_info.read_pid, NULL, gnss_read_pthread, NULL)) |
| #endif |
| { |
| LOGE("pthread_create() fail."); |
| goto exit_with_close; |
| } |
| |
| ret = gnss_info.gnss_dev_open(); |
| if(ret) { |
| LOGE("gnss_dev_open() fail : %d", ret); |
| goto exit_with_thread_exit; |
| } |
| } |
| |
| // GNSS is ready, NMEA can print from uart. |
| gnss_info.state = GNSS_STATE_READY; |
| |
| LOGD("GNSS open success."); |
| |
| if(NULL != gnss_info.gnss_init_set) |
| { |
| gnss_info.gnss_init_set(gnss_info.fd); |
| } |
| #if MBTK_GNSS_TEST_LOG |
| gnss_test_log("gnss open success."); |
| #endif |
| |
| return gnss_ports_open(print_port); |
| |
| exit_with_thread_exit: |
| gnss_info.state = GNSS_STATE_CLOSING; |
| // Wait for read thread exit. |
| ret = pthread_join(gnss_info.read_pid, NULL); |
| if(ret){ |
| LOGE("pthrad_join fail(%d)",ret); |
| } |
| exit_with_close: |
| if(gnss_info.gnss_close(gnss_info.fd)) { |
| LOGE("gnss_close() fail."); |
| } |
| if(gnss_info.exit_fd[0] > 0) { |
| close(gnss_info.exit_fd[0]); |
| gnss_info.exit_fd[0] = -1; |
| } |
| if(gnss_info.exit_fd[1] > 0) { |
| close(gnss_info.exit_fd[1]); |
| gnss_info.exit_fd[1] = -1; |
| } |
| gnss_info.state = GNSS_STATE_CLOSE; |
| return GNSS_ERR_UNKNOWN; |
| } |
| |
| int gnss_deinit() |
| { |
| #if MBTK_GNSS_TEST_LOG |
| gnss_test_log("[gnss_deinit] gnss state [%d]", gnss_info.state); |
| #endif |
| |
| if(gnss_info.state == GNSS_STATE_CLOSE) { |
| LOGW("GNSS is closed."); |
| return GNSS_ERR_OK; |
| } else if(gnss_info.state == GNSS_STATE_CLOSING) { |
| LOGW("GNSS is closing..."); |
| return GNSS_ERR_UNKNOWN; |
| } else if(gnss_info.state == GNSS_STATE_DOWNLOAD) { |
| LOGW("GNSS is downloading..."); |
| return GNSS_ERR_UNKNOWN; |
| } |
| |
| if(gnss_info.gnss_dev_close(gnss_info.fd)) { |
| LOGE("gnss_dev_close() fail."); |
| return GNSS_ERR_UNKNOWN; |
| } |
| |
| // Wait for read thread exit. |
| if(gnss_info.exit_fd[1] > 0) { |
| mbtk_write(gnss_info.exit_fd[1], "exit", 4); |
| } |
| |
| gnss_info.state = GNSS_STATE_CLOSING; |
| |
| #if MBTK_GNSS_TEST_LOG |
| gnss_test_log("wait gnss pthread exit..."); |
| #endif |
| int ret = pthread_join(gnss_info.read_pid, NULL); |
| if(ret){ |
| LOGE("pthrad_join fail(%d)",ret); |
| return GNSS_ERR_UNKNOWN; |
| } |
| |
| if(gnss_info.gnss_close(gnss_info.fd)) { |
| LOGE("gnss_close() fail."); |
| return GNSS_ERR_UNKNOWN; |
| } |
| |
| if(gnss_ports_close()) { |
| LOGE("gnss_ports_close fail."); |
| return GNSS_ERR_UNKNOWN; |
| } |
| |
| #if MBTK_GNSS_TEST_LOG |
| gnss_test_log("gnss_ports_close() complete."); |
| #endif |
| LOGD("gnss_ports_close() complete."); |
| |
| gnss_info.fd = -1; |
| if(gnss_info.exit_fd[0] > 0) { |
| close(gnss_info.exit_fd[0]); |
| gnss_info.exit_fd[0] = -1; |
| } |
| if(gnss_info.exit_fd[1] > 0) { |
| close(gnss_info.exit_fd[1]); |
| gnss_info.exit_fd[1] = -1; |
| } |
| gnss_info.state = GNSS_STATE_CLOSE; |
| #if MBTK_GNSS_TEST_LOG |
| gnss_test_log("gnss close success."); |
| #endif |
| LOGD("GNSS close success."); |
| |
| return GNSS_ERR_OK; |
| } |
| |
| int gnss_set(const void* buf, unsigned int buf_len, void *cmd_rsp, int cmd_rsp_len) |
| { |
| if(buf && buf_len > 0) { |
| if(cmd_rsp && cmd_rsp_len > 0) { |
| memset(cmd_rsp, 0, cmd_rsp_len); |
| } |
| return gnss_info.gnss_set(gnss_info.fd, buf, cmd_rsp, cmd_rsp_len); |
| } else { |
| return GNSS_ERR_UNKNOWN; |
| } |
| } |
| |
| int gnss_dl_fw(const char* fw_name, void *rsp, int rsp_len) |
| { |
| // Only 8122 support download fw. |
| if(gnss_info.gnss_id != GNSS_TYPE_8122) { |
| return GNSS_ERR_UNSUPPORT; |
| } |
| |
| if(rsp && rsp_len > 0) { |
| memset(rsp, 0, rsp_len); |
| } |
| |
| if(gnss_info.gnss_fw_dl) { |
| if(GNSS_ERR_OK != gnss_deinit()) { |
| LOGE("Close gnss fail."); |
| return GNSS_ERR_UNKNOWN; |
| } else { |
| LOGD("Start gnss fw dl."); |
| return gnss_info.gnss_fw_dl(gnss_info.fd, fw_name, gnss_info.dev_name); |
| } |
| } else { |
| return GNSS_ERR_UNSUPPORT; |
| } |
| } |
| |
| int gnss_agnss_get_eph(const void* param) |
| { |
| if(gnss_info.gnss_agnss_get_eph) |
| { |
| return gnss_info.gnss_agnss_get_eph(param); |
| } |
| else |
| { |
| return GNSS_ERR_UNSUPPORT; |
| } |
| |
| return GNSS_ERR_OK; |
| } |
| |
| int gnss_agnss_inject(void) |
| { |
| if(gnss_info.state != GNSS_STATE_READY) |
| { |
| LOGE("gnss not ready"); |
| return GNSS_ERR_UNKNOWN; |
| } |
| |
| if(gnss_info.gnss_agnss_inject) |
| { |
| return gnss_info.gnss_agnss_inject(gnss_info.fd); |
| } |
| else |
| { |
| return GNSS_ERR_UNSUPPORT; |
| } |
| |
| return GNSS_ERR_OK; |
| } |
| |
| int gnss_ind_set(int fd, int ind_type) |
| { |
| int index = 0; |
| if(ind_type) { // Add IND flag. |
| while(index < GNSS_CLI_IND_MAX) { |
| if(ind_info[index].cli_fd == fd) |
| break; |
| index++; |
| } |
| |
| if(index == GNSS_CLI_IND_MAX) { // Add flag |
| index = 0; |
| while(index < GNSS_CLI_IND_MAX) { |
| if(ind_info[index].cli_fd <= 0) |
| break; |
| index++; |
| } |
| if(index == GNSS_CLI_IND_MAX) |
| { |
| LOGE("ind flag is full."); |
| return GNSS_ERR_CLI_FULL; |
| } |
| ind_info[index].cli_fd = fd; |
| ind_info[index].ind_flag = (uint32)ind_type; |
| } else { // Change flag |
| ind_info[index].cli_fd = fd; |
| ind_info[index].ind_flag = (uint32)ind_type; |
| } |
| } else { // Clear IND flag. |
| while(index < GNSS_CLI_IND_MAX) { |
| if(ind_info[index].cli_fd == fd) |
| break; |
| index++; |
| } |
| |
| if(index == GNSS_CLI_IND_MAX) { |
| return GNSS_ERR_ARG; |
| } |
| ind_info[index].cli_fd = 0; |
| ind_info[index].ind_flag = 0; |
| } |
| |
| return GNSS_ERR_OK; |
| } |
| |
| static void sig_process(int sig) |
| { |
| LOGI("I got signal %d\n", sig); |
| if(gnss_deinit()) { |
| LOGE("gnss_deinit() fail, no exist..."); |
| return; |
| } |
| |
| switch(sig) |
| { |
| case SIGINT: // Ctrl + C |
| { |
| LOGI("Exit by SIGINT.\n"); |
| exit(0); |
| } |
| case SIGQUIT: // Ctrl + \ (\C0\E0\CB\C6 SIGINT \A3\AC\B5\ABҪ\B2\FA\C9\FAcore\CEļ\FE) |
| { |
| LOGI("Exit by SIGQUIT.\n"); |
| exit(0); |
| } |
| case SIGTERM:// Ĭ\C8\CFkill (ͬ SIGKILL \A3\AC\B5\AB SIGKILL \B2\BB\BFɲ\B6\BB\F1) |
| { |
| LOGI("Exit by SIGTERM.\n"); |
| exit(0); |
| } |
| case SIGTSTP:// Ctrl + Z (ͬ SIGSTOP \A3\AC\B5\AB SIGSTOP \B2\BB\BFɲ\B6\BB\F1) |
| { |
| LOGI("Exit by SIGTSTP.\n"); |
| exit(0); |
| } |
| case SIGSEGV: // \C8\E7\BF\D5ָ\D5\EB |
| { |
| LOGI("Exit by SIGSEGV.\n"); |
| exit(0); |
| } |
| default: |
| { |
| LOGI("Unknown sig:%d\n",sig); |
| break; |
| } |
| } |
| } |
| |
| |
| // mbtk_gnssd 6228 /dev/ttyS2 baud 0/1 <port_type> |
| int main(int argc, char *argv[]) |
| { |
| mbtk_log_init("radio", GNSS_TAG); |
| |
| MBTK_SOURCE_INFO_PRINT("mbtk_gnssd"); |
| |
| #ifdef MBTK_DUMP_SUPPORT |
| mbtk_debug_open(NULL, TRUE); |
| #endif |
| |
| signal(SIGINT, sig_process); |
| signal(SIGQUIT, sig_process); |
| signal(SIGTERM, sig_process); |
| |
| if(arg_check(argc, argv)) { |
| return -1; |
| } |
| |
| #ifdef GNSS_DEBUG |
| char buff[10]; |
| memset(buff, 0, 10); |
| property_get(MBTK_PROP_GNSS_LOG, buff, ""); |
| if(strlen(buff) > 0 && atoi(buff) > 0) { |
| nmea_log_enable = TRUE; |
| } |
| #endif |
| |
| memset(&gnss_info, 0, sizeof(gnss_info_t)); |
| memcpy(gnss_info.dev_name, argv[2], strlen(argv[2])); |
| gnss_info.state = GNSS_STATE_CLOSE; |
| if(!strcmp(argv[1], GNSS_ID_6228)) { |
| gnss_info.gnss_id = GNSS_TYPE_6228; |
| gnss_info.auto_open = (bool)atoi(argv[3]); |
| gnss_info.auto_dl_fw = TRUE; |
| gnss_info.dl_befor_open = FALSE; |
| gnss_info.gnss_dev_open = gnss_6228_dev_open; |
| gnss_info.gnss_dev_close = gnss_6228_dev_close; |
| gnss_info.gnss_open = gnss_6228_open; |
| gnss_info.gnss_close = gnss_6228_close; |
| gnss_info.gnss_init_set = NULL; |
| gnss_info.gnss_fw_dl = gnss_6228_fw_dl; |
| gnss_info.gnss_dl_read_cb = gnss_6228_dl_read_cb; |
| gnss_info.gnss_set = gnss_6228_set; |
| gnss_info.gnss_set_cb = gnss_6228_set_cb; |
| gnss_info.gnss_agnss_get_eph = NULL; |
| gnss_info.gnss_agnss_inject = NULL; |
| } else if(!strcmp(argv[1], GNSS_ID_8122)) { |
| gnss_info.gnss_id = GNSS_TYPE_8122; |
| gnss_info.auto_open = (bool)atoi(argv[3]); |
| gnss_info.auto_dl_fw = FALSE; |
| gnss_info.dl_befor_open = FALSE; |
| gnss_info.gnss_dev_open = gnss_8122_dev_open; |
| gnss_info.gnss_dev_close = gnss_8122_dev_close; |
| gnss_info.gnss_open = gnss_8122_open; |
| gnss_info.gnss_close = gnss_8122_close; |
| gnss_info.gnss_init_set = gnss_8122_init_set; |
| gnss_info.gnss_fw_dl = gnss_8122_fw_dl; |
| gnss_info.gnss_dl_read_cb = NULL; |
| gnss_info.gnss_set = gnss_8122_set; |
| gnss_info.gnss_set_cb = gnss_8122_set_cb; |
| gnss_info.gnss_agnss_get_eph = gnss_8122_agnss_get_eph; |
| gnss_info.gnss_agnss_inject = gnss_8122_agnss_inject; |
| } else if(!strcmp(argv[1], GNSS_ID_5311)) { |
| gnss_info.gnss_id = GNSS_TYPE_5311; |
| gnss_info.auto_open = (bool)atoi(argv[3]); |
| gnss_info.auto_dl_fw = TRUE; |
| gnss_info.dl_befor_open = TRUE; |
| gnss_info.gnss_dev_open = gnss_5311_dev_open; |
| gnss_info.gnss_dev_close = gnss_5311_dev_close; |
| gnss_info.gnss_open = gnss_5311_open; |
| gnss_info.gnss_close = gnss_5311_close; |
| gnss_info.gnss_init_set = NULL; |
| gnss_info.gnss_fw_dl = gnss_5311_fw_dl; |
| gnss_info.gnss_dl_read_cb = NULL; |
| gnss_info.gnss_set = gnss_5311_set; |
| gnss_info.gnss_set_cb = gnss_5311_set_cb; |
| gnss_info.gnss_agnss_get_eph = NULL; |
| gnss_info.gnss_agnss_inject = NULL; |
| } else if(!strcmp(argv[1], GNSS_ID_N50DB)) { |
| gnss_info.gnss_id = GNSS_TYPE_N50DB; |
| gnss_info.auto_open = (bool)atoi(argv[3]); |
| gnss_info.auto_dl_fw = FALSE; |
| gnss_info.dl_befor_open = FALSE; |
| gnss_info.gnss_dev_open = gnss_n50db_dev_open; |
| gnss_info.gnss_dev_close = gnss_n50db_dev_close; |
| gnss_info.gnss_open = gnss_n50db_open; |
| gnss_info.gnss_close = gnss_n50db_close; |
| gnss_info.gnss_init_set = NULL; |
| gnss_info.gnss_fw_dl = gnss_n50db_fw_dl; |
| gnss_info.gnss_dl_read_cb = NULL; |
| gnss_info.gnss_set = gnss_n50db_set; |
| gnss_info.gnss_set_cb = gnss_n50db_set_cb; |
| gnss_info.gnss_agnss_get_eph = NULL; |
| gnss_info.gnss_agnss_inject = NULL; |
| } else { |
| LOGE("No support : %s", argv[1]); |
| return -1; |
| } |
| |
| LOGD("GNSS : %s, Device: %s", argv[1], gnss_info.dev_name); |
| // Auto open gnss. |
| if(gnss_info.auto_open) { |
| int init_mode = atoi(argv[3]); |
| if(((GNSS_PRINT_PORT_UART1 | GNSS_PRINT_PORT_USB_NMEA | GNSS_PRINT_PORT_USB_AT | GNSS_PRINT_PORT_TTY_AT) & init_mode) != init_mode) { |
| init_mode = 0; |
| } |
| if(gnss_init((uint32)init_mode)) { |
| LOGE("gnss_init() fail."); |
| // return -1; |
| } |
| } else { |
| gnss_info.print_port = 0; |
| } |
| |
| #if MBTK_GNSS_PTY_AUTO_INIT |
| if(gnss_pty_open(&gnss_pty_master_fd, &gnss_pty_slave_fd, GNSS_PORT_PTY)) { |
| LOGE("gnss_pty_open() fail."); |
| return -1; |
| } |
| #endif |
| |
| #if MBTK_GNSS_TEST_LOG |
| gnss_test_log("gnss init success."); |
| #endif |
| // Init ubus and waitting IPC commands. |
| #ifdef MBTK_GNSS_UBUS_ENABLE |
| if(gnss_ubus_init()) { |
| LOGD("main() run..."); |
| uloop_run(); |
| } else { |
| LOGE("gnss_ubus_init() fail."); |
| } |
| #else |
| if(!gnss_ipc_service_start()) { |
| LOGD("main() run..."); |
| while(1) { |
| sleep(24 * 60 * 60); |
| } |
| } else { |
| LOGE("gnss_ipc_service_start() fail."); |
| } |
| #endif |
| |
| LOGD("main() exit."); |
| return 0; |
| } |