1. Kernel tty device is substituted with pseudo-tty device. 2. NMEA flow doesn't start automatically, but on request. 3. NMEA flow is controlled externally with signals: - SIGUSR1 to stop - SIGUSR2 to start 4. Bash script file is added for sending NMEA flow controlling signals. The script receives input commands: start and stop
Signed-off-by: Ramon Fried rfried@codeaurora.org --- gps | 24 ++++ gps_proxy.c | 410 ++++++++++++++++++++++++++++++++++++++++++++---------------- 2 files changed, 323 insertions(+), 111 deletions(-) create mode 100755 gps
diff --git a/gps b/gps new file mode 100755 index 0000000..cccf259 --- /dev/null +++ b/gps @@ -0,0 +1,24 @@ +#!/bin/bash + +if [ 1 != $# ] +then echo "Unknown command" +exit -1 +fi + +GPS_PID=$(pidof gps_proxy) + +if [ "" = "$GPS_PID" ] +then echo "gps_proxy isn't running" +exit -1 +fi + +if [ "stop" = "$1" ] +then kill -s SIGUSR1 $GPS_PID + +elif [ "start" = "$1" ] +then kill -s SIGUSR2 $GPS_PID + +else +echo "Unknown command" +exit -1 +fi diff --git a/gps_proxy.c b/gps_proxy.c index 9980932..479a64d 100644 --- a/gps_proxy.c +++ b/gps_proxy.c @@ -27,8 +27,12 @@ * IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */
+#define _GNU_SOURCE /* posix pseudo tty */ + #include <sys/mman.h> +#include <sys/signalfd.h> #include <sys/socket.h> +#include <sys/stat.h> #include <sys/types.h> #include <sys/ioctl.h> #include <ctype.h> @@ -36,6 +40,7 @@ #include <errno.h> #include <fcntl.h> #include <libqrtr.h> +#include <signal.h> #include <stdarg.h> #include <stdbool.h> #include <stdint.h> @@ -53,6 +58,19 @@ #define QMI_LOC_EVENT_MASK_NMEA_V02 ((uint64_t)0x00000004ull) #define MAX_BUF_SZ 512
+#define MAX_ATTEMPTS_CONNECT_TO_QRTR 5 +#define QRTR_CONNECTION_ATTEMPTS_INTERVAL 10 +#define MAX_SLAVE_DEV_NAME_SIZE 216 +#define MAX_COMMAND_SIZE 32 +#define CHECK_IF_ACTIVE_TIME 1 + +#define GPS_DEVICE_NAME "/dev/ttyGPS" + +#define STOP_SIG SIGUSR1 +#define START_SIG SIGUSR2 + +#define RD_WR_ALL_PERM 0666 + typedef enum { NEED_SEND = 1, NEED_RECV @@ -77,13 +95,14 @@ static unsigned g_txn = 1; static uint8_t g_session_id = 0; static uint32_t g_loc_node = -1; static uint32_t g_loc_port = -1; -static bool dbgprintf_enabled; +static bool g_dbgprintf_enabled; +static bool g_is_active = true;
static void dbgprintf(const char *fmt, ...) { va_list ap;
- if (!dbgprintf_enabled) + if (!g_dbgprintf_enabled) return;
va_start(ap, fmt); @@ -91,7 +110,7 @@ static void dbgprintf(const char *fmt, ...) va_end(ap); }
-int qrtr_send_recv(int sock, unsigned node, unsigned port, void *ptr, size_t len, char *buf, +static int qrtr_send_recv(int sock, unsigned node, unsigned port, void *ptr, size_t len, char *buf, size_t buf_len, unsigned *in_node, unsigned *in_port, struct qmi_packet **qmi, int op_mask) { @@ -128,17 +147,19 @@ static int check_result(struct gps_qmi_result *result) fprintf(stderr, "[GPS] Can't get response result from message\n"); return -EINVAL; } + if (result->result != QMI_GPS_RESULT_SUCCESS) { fprintf(stderr, "[GPS] Stop Response error: %d, %d\n", - (int)result->result,(int)result->error); + (int)result->result, (int)result->error); return -EINVAL; } + ++g_txn;
return 0; }
-int gps_send_start(int sock, unsigned node, unsigned port) +static int gps_send_start(int sock, unsigned node, unsigned port) { int ret = 0; struct gps_qmi_result *result; @@ -146,62 +167,58 @@ int gps_send_start(int sock, unsigned node, unsigned port) struct gps_loc_start_resp *resp; size_t len; void *ptr; - unsigned in_node,in_port; + unsigned in_node, in_port; char buf[MAX_BUF_SZ]; struct qmi_packet *qmi; unsigned txn;
- dbgprintf("[GPS] send LOC_START to (%d:%d)\n", node, port);
req = gps_loc_start_req_alloc(g_txn); - gps_loc_start_req_set_session_id(req,g_session_id); + gps_loc_start_req_set_session_id(req, g_session_id); ptr = gps_loc_start_req_encode(req, &len); if (!ptr) - goto free_resp; + goto free_req;
ret = qrtr_send_recv(sock, node, port, ptr, len, buf, sizeof(buf), &in_node, &in_port, &qmi, NEED_SEND | NEED_RECV); if (!ret) - goto free_resp; + goto free_req;
/* check response status */ - switch (qmi->msg_id) { - case QMI_GPS_LOC_START: + if (QMI_GPS_LOC_START == qmi->msg_id) { resp = gps_loc_start_resp_parse(qmi, qmi->msg_len, &txn); if ((!resp) || (txn != g_txn)) { fprintf(stderr, "[GPS] Invalid response message\n"); ret = -EINVAL; - goto free_resp; + goto free_req; } + result = gps_loc_start_resp_get_result(resp); ret = check_result(result); if (ret) - goto free_resp; - - break; + goto free_req; + }
- default: + else { fprintf(stderr, "[GPS] Unknown message: id=%d size=%d\n", qmi->msg_id, qmi->msg_len); ret = -EINVAL; - break; }
- -free_resp: +free_req: gps_loc_start_req_free(req);
return ret; }
-int gps_send_stop(int sock, unsigned node, unsigned port) +static int gps_send_stop(int sock, unsigned node, unsigned port) { int ret = 0; struct gps_qmi_result *result; struct gps_loc_stop_req *req; struct gps_loc_stop_resp *resp; void *ptr; - unsigned in_node,in_port; + unsigned in_node, in_port; char buf[MAX_BUF_SZ]; struct qmi_packet *qmi; unsigned txn; @@ -211,21 +228,21 @@ int gps_send_stop(int sock, unsigned node, unsigned port) dbgprintf("[GPS] send LOC_STOP to (%d:%d)\n", node, port);
req = gps_loc_stop_req_alloc(g_txn); - gps_loc_stop_req_set_session_id(req,g_session_id); + gps_loc_stop_req_set_session_id(req, g_session_id); ptr = gps_loc_stop_req_encode(req, &len); if (!ptr) - goto free_resp; + goto free_req;
ret = qrtr_send_recv(sock, node, port, ptr, len, buf, sizeof(buf), &in_node, &in_port, &qmi, NEED_SEND); if (!ret) - goto free_resp; + goto free_req;
while (!stop) { ret = qrtr_send_recv(sock, node, port, ptr, len, buf, sizeof(buf), &in_node, &in_port, &qmi, NEED_RECV); if (!ret) - goto free_resp; + goto free_req;
/* check response status */ switch (qmi->msg_id) { @@ -234,12 +251,13 @@ int gps_send_stop(int sock, unsigned node, unsigned port) if ((!resp) || (txn != g_txn)) { fprintf(stderr, "[GPS] Invalid response message\n"); ret = -EINVAL; - goto free_resp; + goto free_req; } + result = gps_loc_stop_resp_get_result(resp); ret = check_result(result); if (ret) - goto free_resp; + goto free_req; stop = 1;
break; @@ -254,13 +272,13 @@ int gps_send_stop(int sock, unsigned node, unsigned port) } }
-free_resp: +free_req: gps_loc_stop_req_free(req);
return ret; }
-int gps_send_register_nmea_event(int sock, unsigned node, unsigned port) +static int gps_send_register_nmea_event(int sock, unsigned node, unsigned port) { int ret = 0; struct gps_qmi_result *result; @@ -268,67 +286,62 @@ int gps_send_register_nmea_event(int sock, unsigned node, unsigned port) struct gps_loc_reg_events_resp *resp; size_t len; void *ptr; - unsigned in_node,in_port; + unsigned in_node, in_port; char buf[MAX_BUF_SZ]; struct qmi_packet *qmi; unsigned txn;
- dbgprintf("[GPS] send LOC_REG_EVENT to (%d:%d)\n", node, port);
req = gps_loc_reg_events_req_alloc(g_txn); - gps_loc_reg_events_req_set_event_reg_mask(req,QMI_LOC_EVENT_MASK_NMEA_V02); + gps_loc_reg_events_req_set_event_reg_mask(req, QMI_LOC_EVENT_MASK_NMEA_V02); ptr = gps_loc_reg_events_req_encode(req, &len); if (!ptr) - goto free_resp; + goto free_req;
ret = qrtr_send_recv(sock, node, port, ptr, len, buf, sizeof(buf), &in_node, &in_port, &qmi, NEED_SEND | NEED_RECV); if (!ret) - goto free_resp; + goto free_req;
/* check response status */ - switch (qmi->msg_id) { - case QMI_GPS_LOC_REG_EVENTS: + if (QMI_GPS_LOC_REG_EVENTS == qmi->msg_id) { resp = gps_loc_reg_events_resp_parse(qmi, qmi->msg_len, &txn); if ((!resp) || (txn != g_txn)) { fprintf(stderr, "[GPS] Invalid response message\n"); ret = -EINVAL; - goto free_resp; + goto free_req; } result = gps_loc_reg_events_resp_get_result(resp); ret = check_result(result); if (ret) - goto free_resp; - - break; + goto free_req; + }
- default: + else { fprintf(stderr, "[GPS] Unknown message: id=%d size=%d\n", qmi->msg_id, qmi->msg_len); ret = -EINVAL; - break; }
- -free_resp: +free_req: gps_loc_reg_events_req_free(req);
return ret; }
-void loc_service_cb(void *udata,uint32_t service,uint32_t instance,uint32_t node,uint32_t port) +static void loc_service_cb(void *udata, uint32_t service, uint32_t instance, uint32_t node, uint32_t port) { dbgprintf("[GPS] New LOC service found at (%u:%u)\n", node, port); g_loc_node = node; g_loc_port = port; }
-int handle_gps_ind(int sock,int dev) +static int handle_gps_ind(int sock,int dev) { struct sockaddr_qrtr sq; socklen_t sl; char buf[MAX_BUF_SZ]; - int ret,len; + int ret, len; unsigned txn; struct gps_loc_event_nmea_ind *req; struct gps_proxy_data nmea_data; @@ -336,106 +349,281 @@ int handle_gps_ind(int sock,int dev) sl = sizeof(sq); len = ret = recvfrom(sock, buf, sizeof(buf), 0, (void *)&sq, &sl); if (ret < 0) { - fprintf(stderr, "[GPS] recvfrom failed: %d\n", ret); + perror("[GPS] recvfrom failed"); return ret; } - req = gps_loc_event_nmea_ind_parse(buf,len,&txn); + + req = gps_loc_event_nmea_ind_parse(buf, len, &txn); if (req) { - if (0 < gps_loc_event_nmea_ind_get_nmea(req,nmea_data.nmea_string,sizeof(nmea_data.nmea_string))) { + if (0 < gps_loc_event_nmea_ind_get_nmea(req, nmea_data.nmea_string, sizeof(nmea_data.nmea_string))) { nmea_data.nmea_length = strnlen(nmea_data.nmea_string, sizeof(nmea_data.nmea_string)-1) +1; - ret = ioctl(dev, QGPS_SEND_NMEA, &nmea_data); - if ( ret < 0) - { - fprintf(stderr, "[GPS] can't post nmea string: %d\n", ret); + ret = write(dev, nmea_data.nmea_string, strlen(nmea_data.nmea_string) + 1); + if (ret < 0) { + perror("[GPS] can't post nmea string"); }
} }
dbgprintf("[GPS] packet; from: %d:%d\n", sq.sq_node, sq.sq_port); - if (dbgprintf_enabled) + if (g_dbgprintf_enabled) print_hex_dump("[GPS <-]", buf, len);
- return 0; + return (ret < 0) ? ret : 0; }
-int main(int argc, char **argv) +static int pty_master_init(char* pty_slave_name) { - int gps_fd; - fd_set rfds; - int nfds; - int ret; - int ch_fd = 0; + int fd, fail_val = -1;
- if (argc == 2 && strcmp(argv[1], "-v") == 0) - dbgprintf_enabled = true; + fd = posix_openpt(O_RDWR | O_NOCTTY); + if (fd < 0) { + perror("Failed to open pty master"); + return fail_val; + } + + if (grantpt(fd)) { + perror("Failed to grant pty"); + return fail_val; + } + + if (unlockpt(fd)) { + perror("Failed to unlock pty"); + return fail_val; + } + + if (ptsname_r(fd, pty_slave_name, MAX_SLAVE_DEV_NAME_SIZE)) { + perror("Failed to get slave device name"); + return fail_val; + } + + if (chmod(pty_slave_name, RD_WR_ALL_PERM)) + perror("Failed to change slave device permission"); + + if (unlink(GPS_DEVICE_NAME) && ENOENT != errno) { + perror("Failed to remove yhr old GPS device"); + return fail_val; + }
- ch_fd = open("/dev/gps_proxy_ch", O_RDONLY); - if (ch_fd < 0){ - fprintf(stdout,"OPEN: %s\n", strerror(errno)); - return -1; + if (symlink(pty_slave_name, GPS_DEVICE_NAME)) { + perror("Failed to rename slave device"); + return fail_val; }
+ return fd; +} + +static int qrtr_init(void) +{ + int ret, gps_fd, fail_val = -1; + int attempt_to_connect_to_qrtr = MAX_ATTEMPTS_CONNECT_TO_QRTR; + gps_fd = qrtr_open(0); if (gps_fd < 0) { - fprintf(stderr, "failed to create qrtr socket"); - goto clean_file; + perror("failed to create qrtr socket"); + return fail_val; }
- ret = qrtr_lookup(gps_fd, QMI_LOC_SERVICE, QMI_LOC_VERSION, - QMI_LOC_INSTANCE,0xFFFFFFFF,loc_service_cb,NULL); - if (ret < 0 || g_loc_node == -1 || g_loc_port == -1) { - fprintf(stderr, "failed to lookup LOC service"); - goto clean; + /* perform several atempts to connect to modem */ + while (attempt_to_connect_to_qrtr--) { + ret = qrtr_lookup(gps_fd, QMI_LOC_SERVICE, QMI_LOC_VERSION, + QMI_LOC_INSTANCE, 0xFFFFFFFF, loc_service_cb, NULL); + + if (ret < 0 || g_loc_node == -1 || g_loc_port == -1) { + fprintf(stderr, "failed to lookup LOC service\n"); + + if (attempt_to_connect_to_qrtr) + sleep(QRTR_CONNECTION_ATTEMPTS_INTERVAL); + } + else + break; } - - for (;;) { - ret = ioctl(ch_fd, QGPS_REGISTER_HANDLE, 0);
- ret = gps_send_register_nmea_event(gps_fd,g_loc_node,g_loc_port); - if (ret < 0) { - fprintf(stderr, "failed to send LOC_REG_EVENTS"); - goto clean; + if (attempt_to_connect_to_qrtr < 0) { + fprintf(stderr, "failed to lookup LOC service for %d times with %d seconds interval\n", MAX_ATTEMPTS_CONNECT_TO_QRTR, QRTR_CONNECTION_ATTEMPTS_INTERVAL); + return fail_val; + } + + else { + fprintf(stdout, "GPS is activated\n"); + fflush(stdout); + } + + return gps_fd; +} + +static int sig_handler_init(void) +{ + int sig_fd, fail_val = -1; + sigset_t mask; + + sigemptyset(&mask); + sigaddset(&mask, STOP_SIG); + sigaddset(&mask, START_SIG); + + if (sigprocmask(SIG_BLOCK, &mask, NULL) < 0) { + perror("sigprocmask failed"); + return fail_val; + } + + sig_fd = signalfd(-1, &mask, 0); + if (sig_fd < 0) { + perror("signalfd failed"); + return fail_val; + } + + return sig_fd; +} + +static int gps_start(int gps_fd) +{ + int ret, fail_val = -1; + + ret = gps_send_register_nmea_event(gps_fd,g_loc_node,g_loc_port); + if (ret < 0) { + fprintf(stderr, "failed to send LOC_REG_EVENTS\n"); + return fail_val; + } + + ret = gps_send_start(gps_fd,g_loc_node,g_loc_port); + if (ret < 0) { + fprintf(stderr, "failed to send LOC_START\n"); + return fail_val; + } + + return 0; +} + +static int gps_stop(int gps_fd) +{ + int ret, fail_val = -1; + + ret = gps_send_stop(gps_fd, g_loc_node, g_loc_port); + if (ret < 0) { + fprintf(stderr, "failed to send LOC_STOP\n"); + return fail_val; + } + + return 0; +} + +static void nmea_disable(void) +{ + g_is_active = false; +} + +static void nmea_enable(void) +{ + g_is_active = true; +} + +static int check_if_signal(int socket, int signal) +{ + struct signalfd_siginfo sig_info; + ssize_t res; + int fail_val = 0; + + res = read(socket, &sig_info, sizeof(sig_info)); + + if (res < 0 || res != sizeof(sig_info)) { + perror("sig_fd read failed"); + return fail_val; + } + + return sig_info.ssi_signo == signal; +} + +static int check_if_stop(int socket) +{ + return check_if_signal(socket, STOP_SIG); +} + +static int check_if_start(int socket) +{ + return check_if_signal(socket, START_SIG); +} + +static void wait_for_start(int socket) +{ + while (true) { + if (check_if_start(socket)) { + nmea_enable(); + break; }
- ret = gps_send_start(gps_fd,g_loc_node,g_loc_port); + } +} + +static void transit_gps_data(int source_fd, int destination_fd, int sig_fd) +{ + fd_set rfds; + int nfds; + int ret; + + nfds = MAX(source_fd, sig_fd) + 1; + + while (g_is_active) { + FD_ZERO(&rfds); + FD_SET(source_fd, &rfds); + FD_SET(sig_fd, &rfds); + + ret = select(nfds, &rfds, NULL, NULL, NULL); if (ret < 0) { - fprintf(stderr, "failed to send LOC_START"); - goto clean; + perror("select failed"); + break; + } else if (ret == 0) { + continue; }
- for (;;) { - FD_ZERO(&rfds); - FD_SET(gps_fd, &rfds); + if (FD_ISSET(sig_fd, &rfds) && check_if_stop(sig_fd)) { + nmea_disable(); + break; + }
- nfds = gps_fd + 1; - ret = select(nfds, &rfds, NULL, NULL, NULL); - if (ret < 0) { - fprintf(stderr, "select failed: %d\n", ret); - break; - } else if (ret == 0) { - continue; - } + if (FD_ISSET(source_fd, &rfds)) + handle_gps_ind(source_fd, destination_fd); + } +} + +int main(int argc, char **argv) +{ + int gps_fd, pty_m_fd, sig_fd; + int ret; + char pty_slave_name[MAX_SLAVE_DEV_NAME_SIZE]; + + if (argc == 2 && strcmp(argv[1], "-v") == 0) + g_dbgprintf_enabled = true;
- if (FD_ISSET(gps_fd, &rfds)) - handle_gps_ind(gps_fd,ch_fd); + sig_fd = sig_handler_init(); + if (sig_fd < 0) + goto clean_signal; + + pty_m_fd = pty_master_init(pty_slave_name); + if (pty_m_fd < 0) + goto clean_file; + + gps_fd = qrtr_init(); + if (gps_fd < 0) + goto clean; + + while (true) { + wait_for_start(sig_fd); - ret = ioctl(ch_fd, QGPS_IS_ACTIVE, 0); - if (0!=ret) { - dbgprintf("All clients have disconnected\n"); - break; - } - } + ret = gps_start(gps_fd); + if (ret < 0) + goto clean;
- ret = gps_send_stop(gps_fd,g_loc_node,g_loc_port); - if (ret < 0) { - fprintf(stderr, "failed to send LOC_STOP"); + transit_gps_data(gps_fd, pty_m_fd, sig_fd); + + ret = gps_stop(gps_fd); + if (ret < 0) goto clean; - } } clean: qrtr_close(gps_fd); clean_file: - close(ch_fd); - return 0; + close(pty_m_fd); +clean_signal: + close(sig_fd); + return EXIT_SUCCESS; }
dragonboard@lists.96boards.org
-
Ramon Fried