| #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" |
| |
| #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 |
| |
| 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 |
| |
| static bool mbtk_gnss_time_set_flag = 0; |
| |
| 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(gnss_pty_open(&gnss_pty_master_fd, &gnss_pty_slave_fd, GNSS_PORT_PTY)) { |
| return GNSS_ERR_OPEN_DEV; |
| } |
| LOGD("Open PTY port success."); |
| } else { // Close |
| if(gnss_pty_slave_fd > 0) { |
| close(gnss_pty_slave_fd); |
| gnss_pty_slave_fd = -1; |
| unlink(GNSS_PORT_PTY); |
| } |
| 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(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); |
| } |
| |
| 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) { |
| write(nmea_log_fd, data, data_len); |
| debug_fd_len += data_len; |
| } |
| } else if(fd == debug_fd) { |
| if(debug_fd > 0) { |
| 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(gnss_uart_at_port_fd > 0) { |
| ret = write(gnss_uart_at_port_fd, nmea, nmea_len); |
| } |
| |
| if(gnss_pty_master_fd > 0) { |
| ret = write(gnss_pty_master_fd, nmea, nmea_len); |
| } |
| 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 int nmea_tokenizer_init(mbtk_nmeatokenizer* t, const char* head, const char* end, int param_num) |
| { |
| int count = 0; |
| const char* p = head; |
| const char* q = end; |
| const char* tmp = NULL; |
| // the initial '$' is optional |
| if (p < q && p[0] == '$') |
| { |
| p += 1; |
| } |
| else |
| { |
| return -1; |
| } |
| |
| //find '*',del '*25\r\n' |
| // get rid of checksum at the end of the sentecne |
| if (q >= p + 5 && q[-5] == '*') |
| { |
| q -= 5; |
| } |
| else |
| { |
| return -1; |
| } |
| |
| while (p <= q) |
| { |
| tmp = memchr(p, ',', q-p); |
| if (tmp == NULL) |
| { |
| tmp = q; |
| } |
| // if (q > p) { |
| // q >= p include empty token: ,, |
| if (tmp >= p) |
| { |
| if (count < MAX_NMEA_TOKENS) |
| { |
| t->tokens[count].head = p; |
| t->tokens[count].end = tmp; |
| count += 1; |
| } |
| } |
| |
| if (tmp <= q) |
| { |
| tmp += 1; |
| } |
| |
| p = tmp; |
| } |
| |
| if(count != param_num) |
| { |
| LOGD("count [%d], param_num [%d]", count, param_num); |
| return -1; |
| } |
| |
| t->count = count; |
| return count; |
| } |
| |
| static mbtk_token nmea_tokenizer_get(mbtk_nmeatokenizer* t, int index) |
| { |
| mbtk_token tok; |
| static const char* dummy = ""; |
| |
| if (index < 0 || index >= t->count) |
| { |
| tok.head = tok.end = dummy; |
| } |
| else |
| { |
| tok = t->tokens[index]; |
| } |
| return tok; |
| } |
| |
| static int mbtk_time_type_gnss_read() { |
| int type = 0; |
| char time_type[] ={0}; |
| property_get("persist.mbtk.time_type", time_type, "0"); |
| |
| type = atoi(time_type); |
| // LOGD("time_type :%d\n", type); |
| if(type != LYNQ_TIME_TYPE_GNSS) |
| mbtk_gnss_time_set_flag = 0; |
| |
| return type; |
| } |
| |
| static int strstr_n(const char *s1, const char *s2) |
| { |
| int n; |
| int strlen = 0; |
| |
| if(*s2) |
| { |
| while(*s1) |
| { |
| for(n = 0; *(s1+n) == *(s2 + n); n++) |
| { |
| if(!*(s2 + n + 1)) |
| { |
| strlen++; |
| return strlen; |
| } |
| } |
| s1++; |
| strlen++; |
| } |
| return 0; |
| } |
| |
| return 0; |
| } |
| static int nmea_update_date_time(mbtk_token date, mbtk_token time) |
| { |
| char tmp_char[4] = {0}; |
| struct tm tmp_time; |
| struct timeval tv; |
| |
| memset(&tmp_time, 0x0, sizeof(struct tm)); |
| if (date.head + 6 > date.end) |
| { |
| LOGD("date get fail"); |
| return -1; |
| } |
| |
| memcpy(tmp_char, date.head, 2); |
| tmp_time.tm_mday = atoi(tmp_char); |
| memcpy(tmp_char, date.head + 2, 2); |
| tmp_time.tm_mon = atoi(tmp_char) - 1; |
| memcpy(tmp_char, date.head + 4, 2); |
| tmp_time.tm_year = 100 + atoi(tmp_char); |
| |
| if (time.head + 6 > time.end) |
| { |
| LOGD("time get fail"); |
| return -1; |
| } |
| |
| memcpy(tmp_char, time.head, 2); |
| tmp_time.tm_hour = atoi(tmp_char); |
| memcpy(tmp_char, time.head + 2, 2); |
| tmp_time.tm_min = atoi(tmp_char); |
| memcpy(tmp_char, time.head + 4, 2); |
| tmp_time.tm_sec = atoi(tmp_char); |
| tmp_time.tm_isdst = -1; |
| |
| |
| LOGD("data:%d-%d-%d %d:%d:%d", tmp_time.tm_year + 1900, |
| tmp_time.tm_mon, |
| tmp_time.tm_mday, |
| tmp_time.tm_hour, |
| tmp_time.tm_min, |
| tmp_time.tm_sec); |
| |
| |
| time_t _t = mktime(&tmp_time);//parse location tmp_time |
| |
| tzset(); // auto set tz |
| _t = _t - timezone; |
| |
| LOGD("timestamp:%ld, %ld", _t, timezone); |
| |
| tv.tv_sec = _t; |
| if(settimeofday(&tv, NULL)) { |
| LOGD("%s: , Set time fail\n", __func__); |
| mbtk_gnss_time_set_flag = 0; |
| } else { |
| LOGD("%s: , Set time success \n", __func__); |
| system("hwclock -w rtc0"); |
| mbtk_gnss_time_set_flag = 1; |
| } |
| |
| return 0; |
| } |
| |
| static int ind_nmea_parse(const char *data, int data_len) |
| { |
| int ret; |
| mbtk_nmeatokenizer tzer = {0}; |
| if(strstr_n(data + 3, "RMC")) |
| { |
| ret = nmea_tokenizer_init(&tzer, data, data + data_len, NMEA_RMC_PARAM_NUM); |
| if(ret < 0) |
| { |
| LOGD("nmea_tokenizer_init fail"); |
| return -1; |
| } |
| |
| mbtk_token tok_time = nmea_tokenizer_get(&tzer,1); |
| mbtk_token tok_fixStatus = nmea_tokenizer_get(&tzer,2); |
| mbtk_token tok_date = nmea_tokenizer_get(&tzer,9); |
| |
| if(tok_fixStatus.head[0] == 'A') |
| { |
| ret = nmea_update_date_time(tok_date, tok_time); |
| if(ret < 0) |
| { |
| LOGD("nmea_update_date_time fail"); |
| return -1; |
| } |
| |
| } |
| } |
| |
| return 0; |
| } |
| |
| |
| 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_time_type_gnss_read() == LYNQ_TIME_TYPE_GNSS) && !mbtk_gnss_time_set_flag) |
| ind_nmea_parse(nmea, data_len); |
| |
| 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) |
| { |
| 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 |
| |
| 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; |
| } |
| 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 |
| { |
| 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(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."); |
| |
| 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(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) { |
| write(gnss_info.exit_fd[1], "exit", 4); |
| } |
| |
| gnss_info.state = GNSS_STATE_CLOSING; |
| 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; |
| } |
| |
| 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; |
| 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_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_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; |
| } 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_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; |
| } 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_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; |
| } 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_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; |
| } 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; |
| } |
| |
| // 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; |
| } |