From f6100159e49f3cc087bc757fe5b175cbd0019437 Mon Sep 17 00:00:00 2001 From: James Maki Date: Wed, 12 May 2010 13:23:59 -0500 Subject: add gps to serial example --- src/cbuffer.c | 35 ++--- src/gps_test.rb | 49 ++++--- src/utils.c | 82 ++++++----- src/utils.h | 2 +- src/venus_api.c | 176 ++++++++++++------------ src/venus_gps.c | 420 ++++++++++++++++++++++++++++++++++++-------------------- 6 files changed, 447 insertions(+), 317 deletions(-) diff --git a/src/cbuffer.c b/src/cbuffer.c index 1242496..b0b8237 100644 --- a/src/cbuffer.c +++ b/src/cbuffer.c @@ -25,24 +25,25 @@ #include "utils.h" #include -int buffered_write(int fd, struct cbuffer *buf) { +int buffered_write(int fd, struct cbuffer *buf) +{ int i; int len; int count = buffer_used(buf); int total = 0; i = buf->read_index; - if(i + count > buffer_size(buf)) { + if (i + count > buffer_size(buf)) { len = full_write(fd, buf->buffer + i, buffer_size(buf) - i); - if(len <= 0) { - log_error("full_write failed errno: %d", errno); + if (len <= 0) { + log_error("full_write failed %m"); return len; } total += len; count -= len; - if(len < buffer_size(buf) - i) { + if (len < buffer_size(buf) - i) { buf->read_index = i + len; return total; } @@ -51,11 +52,11 @@ int buffered_write(int fd, struct cbuffer *buf) { } len = full_write(fd, buf->buffer + i, count); - if(len <= 0) { - if(total) { + if (len <= 0) { + if (total) { return total; } else { - log_error("full_write failed errno: %d", errno); + log_error("full_write failed %m"); return len; } } @@ -65,24 +66,25 @@ int buffered_write(int fd, struct cbuffer *buf) { return total; } -int buffered_read(int fd, struct cbuffer *buf) { +int buffered_read(int fd, struct cbuffer *buf) +{ int i; int len; int count = buffer_free(buf); int total = 0; i = buf->write_index; - if(i + count > buffer_size(buf)) { + if (i + count > buffer_size(buf)) { len = safe_read(fd, buf->buffer + i, buffer_size(buf) - i); - if(len <= 0) { - log_warning("safe_read failed errno: %d", errno); + if (len <= 0) { + log_warning("safe_read failed %m"); return len; } total += len; count -= len; - if(len < buffer_size(buf) - i) { + if (len < buffer_size(buf) - i) { buf->write_index = i + len; return total; } @@ -91,11 +93,11 @@ int buffered_read(int fd, struct cbuffer *buf) { } len = safe_read(fd, buf->buffer + i, count); - if(len <= 0) { - if(total) { + if (len <= 0) { + if (total) { return total; } else { - log_warning("safe_read failed errno: %d", errno); + log_warning("safe_read failed %m"); return len; } } @@ -104,4 +106,3 @@ int buffered_read(int fd, struct cbuffer *buf) { return total; } - diff --git a/src/gps_test.rb b/src/gps_test.rb index 4f35ec5..8f64112 100755 --- a/src/gps_test.rb +++ b/src/gps_test.rb @@ -4,40 +4,47 @@ require 'socket' GPS_SERVER_IP = "192.168.2.1" GPS_SERVER_PORT = 5445 -MODE = :client -PROTOCOL = :tcp +MODE = :tcp_server +#MODE = :tcp_client +#MODE = :udp_server +#MODE = :serial def tcp_read(sock) loop { - data = sock.readline() - puts data.inspect + begin + data = sock.readline() + p data + rescue Errno::EAGAIN, Errno::EWOULDBLOCK => e + puts "read error: #{e.class}: #{e}" + return + end } end -if PROTOCOL == :tcp - if MODE == :client - sock = TCPSocket.new(GPS_SERVER_IP, GPS_SERVER_PORT) - tcp_read(sock) - sock.close - else - serv = TCPServer.new(GPS_SERVER_PORT) - sock = serv.accept - tcp_read(sock) - sock.close - end -elsif PROTOCOL == :udp - sock = UDPSocket.new - sock.bind('0.0.0.0', GPS_SERVER_PORT) - +def udp_read(sock) loop { begin msg, sender = sock.recvfrom(1024) + p msg rescue Errno::EAGAIN, Errno::EWOULDBLOCK => e puts "recvfrom error: #{e.class}: #{e}" next end - puts msg.inspect } end - +if MODE == :tcp_client + sock = TCPSocket.new(GPS_SERVER_IP, GPS_SERVER_PORT) + tcp_read(sock) + sock.close +elsif MODE == :tcp_server + serv = TCPServer.new(GPS_SERVER_PORT) + sock = serv.accept + tcp_read(sock) + sock.close +elsif MODE == :udp_server + sock = UDPSocket.new + sock.bind('0.0.0.0', GPS_SERVER_PORT) + udp_read(sock) + sock.close +end diff --git a/src/utils.c b/src/utils.c index 6f98ab0..7a15427 100644 --- a/src/utils.c +++ b/src/utils.c @@ -32,7 +32,8 @@ #include "log.h" #include "utils.h" -int systemf(const char *fmt, ...) { +int systemf(const char *fmt, ...) +{ int err; va_list ap; char *buf; @@ -40,7 +41,7 @@ int systemf(const char *fmt, ...) { va_start(ap, fmt); err = vasprintf(&buf, fmt, ap); va_end(ap); - if(err == -1) { + if (err == -1) { log_error("out of memory"); return -1; } @@ -53,27 +54,29 @@ int systemf(const char *fmt, ...) { return err; } -ssize_t safe_read(int fd, void *buf, size_t count) { +ssize_t safe_read(int fd, void *buf, size_t count) +{ ssize_t n; do { n = read(fd, buf, count); - } while(n < 0 && errno == EINTR); + } while (n < 0 && errno == EINTR); return n; } -ssize_t safe_readn(int fd, void *buf, size_t len) { +ssize_t safe_readn(int fd, void *buf, size_t len) +{ ssize_t cc; ssize_t total; total = 0; - while(len) { + while (len) { cc = safe_read(fd, buf, len); - if(cc < 0) { - if(total) { + if (cc < 0) { + if (total) { return total; } return cc; @@ -87,58 +90,62 @@ ssize_t safe_readn(int fd, void *buf, size_t len) { return total; } -ssize_t safe_write(int fd, const void *buf, size_t count) { +ssize_t safe_write(int fd, const void *buf, size_t count) +{ ssize_t n; do { n = write(fd, buf, count); - } while(n < 0 && errno == EINTR); + } while (n < 0 && errno == EINTR); return n; } -ssize_t full_write(int fd, const void *buf, size_t len) { +ssize_t full_write(int fd, const void *buf, size_t len) +{ ssize_t cc; ssize_t total; total = 0; - while(len) { + while (len) { cc = safe_write(fd, buf, len); - if(cc < 0) { - if(total) { + if (cc < 0) { + if (total) { return total; } return cc; } total += cc; - buf = ((const char *)buf) + cc; + buf = ((const char *) buf) + cc; len -= cc; } return total; } -int set_nonblocking(int fd) { +int set_nonblocking(int fd) +{ int err; err = fcntl(fd, F_GETFL, 0); - if(err < 0) { - log_warning("fcntl(fd, F_GETFL, 0) failed: errno %d", errno); + if (err < 0) { + log_warning("fcntl(fd, F_GETFL, 0) failed: %m"); return err; } err = fcntl(fd, F_SETFL, err | O_NONBLOCK); - if(err < 0) { - log_warning("fcntl(fd, F_SETFL, opts | O_NONBLOCK) failed: errno %d", errno); + if (err < 0) { + log_warning("fcntl(fd, F_SETFL, opts | O_NONBLOCK) failed: %m"); return err; } return err; } -int host_to_inet(const char *host, struct in_addr *in) { +int host_to_inet(const char *host, struct in_addr *in) +{ struct hostent h; struct hostent *hp; char buf[1024]; @@ -146,7 +153,7 @@ int host_to_inet(const char *host, struct in_addr *in) { int err; err = gethostbyname_r(host, &h, buf, sizeof(buf), &hp, &herrno); - if(hp == NULL) { + if (hp == NULL) { log_error("gethostbyname_r: %d", herrno); return -1; } @@ -156,33 +163,34 @@ int host_to_inet(const char *host, struct in_addr *in) { return 0; } -int inet_conn_str(char *host, int port, int type) { - struct hostent h; +int inet_conn_str(const char *host, int port, int type) +{ + struct hostent h; struct hostent *hp; char buf[1024]; int herrno; int err; err = gethostbyname_r(host, &h, buf, sizeof(buf), &hp, &herrno); - if(hp == NULL) { + if (hp == NULL) { log_error("gethostbyname_r: %d", herrno); return -1; } int sd = socket(PF_INET, type, 0); - if(sd == -1) { - log_error("socket: %d", errno); + if (sd == -1) { + log_error("socket: %m"); return -1; } struct sockaddr_in serv_addr; - serv_addr.sin_family = AF_INET; + serv_addr.sin_family = AF_INET; serv_addr.sin_port = htons(port); memcpy(&serv_addr.sin_addr.s_addr, h.h_addr_list[0], 4); err = connect(sd, (struct sockaddr *) &serv_addr, sizeof(serv_addr)); - if(err == -1) { - log_error("connect: %d", errno); + if (err == -1) { + log_error("connect: %m"); close(sd); return -1; } @@ -190,28 +198,28 @@ int inet_conn_str(char *host, int port, int type) { return sd; } -int inet_conn_ia(struct in_addr *host, int port, int type) { +int inet_conn_ia(struct in_addr *host, int port, int type) +{ int err; int sd = socket(PF_INET, type, 0); - if(sd == -1) { - log_error("socket: %d", errno); + if (sd == -1) { + log_error("socket: %m"); return -1; } struct sockaddr_in serv_addr; memset(&serv_addr, 0, sizeof(serv_addr)); - serv_addr.sin_family = AF_INET; + serv_addr.sin_family = AF_INET; serv_addr.sin_port = htons(port); serv_addr.sin_addr.s_addr = host->s_addr; err = connect(sd, (struct sockaddr *) &serv_addr, sizeof(serv_addr)); - if(err == -1) { - log_error("connect: %d", errno); + if (err == -1) { + log_error("connect: %m"); close(sd); return -1; } return sd; } - diff --git a/src/utils.h b/src/utils.h index 71d3e9c..e9ab44d 100644 --- a/src/utils.h +++ b/src/utils.h @@ -21,7 +21,7 @@ ssize_t safe_readn(int fd, void *buf, size_t len); ssize_t safe_write(int fd, const void *buf, size_t count); ssize_t full_write(int fd, const void *buf, size_t len); int set_nonblocking(int fd); -int inet_conn_str(char *host, int port, int type); +int inet_conn_str(const char *host, int port, int type); int inet_conn_ia(struct in_addr *host, int port, int type); #endif /* ~__UTILS_H */ diff --git a/src/venus_api.c b/src/venus_api.c index 52a4a96..26e2787 100644 --- a/src/venus_api.c +++ b/src/venus_api.c @@ -60,110 +60,116 @@ static const struct baud_map __venus_baud_map[] = { {B115200, VENUS_115200}, }; -speed_t value_to_baud(speed_t value) { +speed_t value_to_baud(speed_t value) +{ int n = sizeof(__baud_map) / sizeof(struct baud_map); int i; - for(i = 0; i < n; ++i) { - if(__baud_map[i].value == value) { + for (i = 0; i < n; ++i) { + if (__baud_map[i].value == value) { return __baud_map[i].baud; } } log_warning("baud rate not valid: %lu", (unsigned long) value); - return (speed_t) -1; + return (speed_t) - 1; } -speed_t baud_to_venus(speed_t baud) { +speed_t baud_to_venus(speed_t baud) +{ int n = sizeof(__venus_baud_map) / sizeof(struct baud_map); int i; - for(i = 0; i < n; ++i) { - if(__venus_baud_map[i].baud == baud) { + for (i = 0; i < n; ++i) { + if (__venus_baud_map[i].baud == baud) { return __venus_baud_map[i].value; } } log_warning("baud rate not valid: %lu", (unsigned long) baud); - return (speed_t) -1; + return (speed_t) - 1; } -static uint8_t seq_start[] = {0xA0, 0xA1}; -static uint8_t seq_end[] = {0x0D, 0x0A}; +static uint8_t seq_start[] = { 0xA0, 0xA1 }; +static uint8_t seq_end[] = { 0x0D, 0x0A }; -void free_venus_msg_data(struct venus_msg *msg) { - if(!msg) { +void free_venus_msg_data(struct venus_msg *msg) +{ + if (!msg) { return; } - if(msg->data) { + if (msg->data) { free(msg->data); } msg->data = NULL; } -uint8_t checksum(void *data, uint16_t len) { +uint8_t checksum(void *data, uint16_t len) +{ int i; uint8_t cs = 0; - for(i = 0; i < len; i++) { + for (i = 0; i < len; i++) { cs ^= ((char *) data)[i]; } return cs; } -int venus_write_msg(int fd, struct venus_msg *msg) { +int venus_write_msg(int fd, struct venus_msg *msg) +{ int err; uint8_t cs = 0; log_debug("id: %d", msg->data[0]); err = full_write(fd, seq_start, sizeof(seq_start)); - if(err != sizeof(seq_start)) { - log_error("failed to write seq_start: %d %d", err, errno); + if (err != sizeof(seq_start)) { + log_error("failed to write seq_start: %d %m", err); } msg->len = __cpu_to_be16(msg->len); err = full_write(fd, &msg->len, 2); - if(err != 2) { - log_error("failed to write len: %d %d", err, errno); + if (err != 2) { + log_error("failed to write len: %d %m", err); } msg->len = __be16_to_cpu(msg->len); err = full_write(fd, msg->data, msg->len); - if(err != msg->len) { - log_error("failed to write data: %d %d", err, errno); + if (err != msg->len) { + log_error("failed to write data: %d %m", err); } cs = checksum(msg->data, msg->len); err = full_write(fd, &cs, 1); - if(err != 1) { - log_error("failed to write checksum: %d %d", err, errno); + if (err != 1) { + log_error("failed to write checksum: %d %m", err); } err = full_write(fd, seq_end, sizeof(seq_end)); - if(err != sizeof(seq_end)) { - log_error("failed to write seq_end: %d %d", err, errno); + if (err != sizeof(seq_end)) { + log_error("failed to write seq_end: %d %m", err); } return 0; } -int venus_read_msg(int fd, struct venus_msg *msg) { +int venus_read_msg(int fd, struct venus_msg *msg) +{ uint8_t cs; uint8_t *data; uint16_t len; uint8_t seq[2]; int attempts = 0; -again: - while(1) { + again: + while (1) { safe_readn(fd, seq, 2); //log_error("start seq %c %c", seq[0], seq[1]); - if(!memcmp(seq, seq_start, 2)) { + if (!memcmp(seq, seq_start, 2)) { break; } attempts++; - if(attempts > 1024) { + if (attempts > 1024) { log_error("max attempts reached"); return -1; } @@ -174,17 +180,17 @@ again: data = malloc(len); safe_readn(fd, data, len); safe_readn(fd, &cs, 1); - if(checksum(data, len) != cs) { + if (checksum(data, len) != cs) { log_error("checksum mismatch"); return -1; } safe_readn(fd, seq, 2); - if(memcmp(seq, seq_end, 2)) { + if (memcmp(seq, seq_end, 2)) { log_error("expected seq end"); return -1; } - if(len == 2 && data[0] == ID_ACK && data[1] == ID_NONE) { + if (len == 2 && data[0] == ID_ACK && data[1] == ID_NONE) { log_debug("read ACK ID_NONE"); free(data); goto again; @@ -195,19 +201,20 @@ again: #if DEBUG { - int i; - printf("READ MSG: "); - for(i = 0; i < msg->len; i++) { - printf("%02X ", msg->data[i]); - } - printf("\n"); + int i; + printf("READ MSG: "); + for (i = 0; i < msg->len; i++) { + printf("%02X ", msg->data[i]); + } + printf("\n"); } #endif return 0; } -ssize_t read_nmea_sentence(int fd, void *buf, size_t count) { +ssize_t read_nmea_sentence(int fd, void *buf, size_t count) +{ int err; char c; ssize_t total = 0; @@ -215,25 +222,25 @@ ssize_t read_nmea_sentence(int fd, void *buf, size_t count) { int start = 0; - while(1) { + while (1) { err = safe_read(fd, &c, 1); - if(err <= 0) { - log_error("read from gps failed: %d", errno); + if (err <= 0) { + log_error("read from gps failed: %m"); return -1; } - if(!start) { - if(c != '$') { + if (!start) { + if (c != '$') { continue; } start = 1; } - if(total < count) { + if (total < count) { *cp++ = c; total++; } - if(c == '\n') { + if (c == '\n') { break; } } @@ -241,13 +248,7 @@ ssize_t read_nmea_sentence(int fd, void *buf, size_t count) { return total; } -int venus_system_restart( - int fd, - uint8_t mode, - time_t utc, - int16_t latitude, - int16_t longitude, - int16_t altitude) +int venus_system_restart(int fd, uint8_t mode, time_t utc, int16_t latitude, int16_t longitude, int16_t altitude) { int err; uint8_t data[15]; @@ -258,7 +259,7 @@ int venus_system_restart( tmp = gmtime_r(&utc, &broken); if (tmp == NULL) { - log_error("gmtime_r: %d", errno); + log_error("gmtime_r: %m"); return -1; } @@ -285,13 +286,13 @@ int venus_system_restart( msg.data = data; msg.len = sizeof(data); err = venus_write_msg(fd, &msg); - if(err < 0) { + if (err < 0) { log_error("venus_write_msg: %d", err); return -1; } err = venus_read_msg(fd, &msg); - if(err < 0) { + if (err < 0) { log_error("venus_read_msg: %d", err); return -1; } @@ -300,7 +301,8 @@ int venus_system_restart( return 0; } -int venus_factory_defaults(int fd, uint8_t type) { +int venus_factory_defaults(int fd, uint8_t type) +{ int err; uint8_t data[2]; struct venus_msg msg; @@ -311,13 +313,13 @@ int venus_factory_defaults(int fd, uint8_t type) { msg.data = data; msg.len = sizeof(data); err = venus_write_msg(fd, &msg); - if(err < 0) { + if (err < 0) { log_error("venus_write_msg: %d", err); return -1; } err = venus_read_msg(fd, &msg); - if(err < 0) { + if (err < 0) { log_error("venus_read_msg: %d", err); return -1; } @@ -326,7 +328,8 @@ int venus_factory_defaults(int fd, uint8_t type) { return 0; } -int venus_conf_serial(int fd, uint8_t com, uint8_t baud, uint8_t attr) { +int venus_conf_serial(int fd, uint8_t com, uint8_t baud, uint8_t attr) +{ int err; uint8_t data[4]; struct venus_msg msg; @@ -339,13 +342,13 @@ int venus_conf_serial(int fd, uint8_t com, uint8_t baud, uint8_t attr) { msg.data = data; msg.len = sizeof(data); err = venus_write_msg(fd, &msg); - if(err < 0) { + if (err < 0) { log_error("venus_write_msg: %d", err); return -1; } err = venus_read_msg(fd, &msg); - if(err < 0) { + if (err < 0) { log_error("venus_read_msg: %d", err); return -1; } @@ -354,7 +357,8 @@ int venus_conf_serial(int fd, uint8_t com, uint8_t baud, uint8_t attr) { return 0; } -int venus_conf_format(int fd, uint8_t type, uint8_t attr) { +int venus_conf_format(int fd, uint8_t type, uint8_t attr) +{ int err; uint8_t data[3]; struct venus_msg msg; @@ -366,13 +370,13 @@ int venus_conf_format(int fd, uint8_t type, uint8_t attr) { msg.data = data; msg.len = sizeof(data); err = venus_write_msg(fd, &msg); - if(err < 0) { + if (err < 0) { log_error("venus_write_msg: %d", err); return -1; } err = venus_read_msg(fd, &msg); - if(err < 0) { + if (err < 0) { log_error("venus_read_msg: %d", err); return -1; } @@ -381,7 +385,8 @@ int venus_conf_format(int fd, uint8_t type, uint8_t attr) { return 0; } -int venus_query_sw_version(int fd, uint8_t type) { +int venus_query_sw_version(int fd, uint8_t type) +{ int err; uint8_t data[2]; struct venus_msg msg; @@ -392,20 +397,20 @@ int venus_query_sw_version(int fd, uint8_t type) { msg.data = data; msg.len = sizeof(data); err = venus_write_msg(fd, &msg); - if(err < 0) { + if (err < 0) { log_error("venus_write_msg: %d", err); return -1; } err = venus_read_msg(fd, &msg); - if(err < 0) { + if (err < 0) { log_error("venus_read_msg: %d", err); return -1; } free_venus_msg_data(&msg); err = venus_read_msg(fd, &msg); - if(err < 0) { + if (err < 0) { log_error("venus_read_msg: %d", err); return -1; } @@ -414,16 +419,7 @@ int venus_query_sw_version(int fd, uint8_t type) { return 0; } -int venus_conf_nmea( - int fd, - uint8_t gga, - uint8_t gsa, - uint8_t gsv, - uint8_t gll, - uint8_t rmc, - uint8_t vtg, - uint8_t zda, - uint8_t attr) +int venus_conf_nmea(int fd, uint8_t gga, uint8_t gsa, uint8_t gsv, uint8_t gll, uint8_t rmc, uint8_t vtg, uint8_t zda, uint8_t attr) { int err; uint8_t data[9]; @@ -442,13 +438,13 @@ int venus_conf_nmea( msg.data = data; msg.len = sizeof(data); err = venus_write_msg(fd, &msg); - if(err < 0) { + if (err < 0) { log_error("venus_write_msg: %d", err); return -1; } err = venus_read_msg(fd, &msg); - if(err < 0) { + if (err < 0) { log_error("venus_read_msg: %d", err); return -1; } @@ -457,7 +453,8 @@ int venus_conf_nmea( return 0; } -int venus_tty_configure(int fd, speed_t baud_rate) { +int venus_tty_configure(int fd, speed_t baud_rate) +{ struct termios tio; tcgetattr(fd, &tio); cfmakeraw(&tio); @@ -467,12 +464,13 @@ int venus_tty_configure(int fd, speed_t baud_rate) { return 0; } -int venus_open(const char *dev, speed_t baud_rate) { +int venus_open(const char *dev, speed_t baud_rate) +{ int tty; tty = open(dev, O_RDWR | O_NOCTTY); - if(tty < 0) { - log_error("failed to open %s: errno: %d", dev, errno); + if (tty < 0) { + log_error("failed to open %s: %m", dev); exit(1); } @@ -481,7 +479,3 @@ int venus_open(const char *dev, speed_t baud_rate) { return tty; } - - - - diff --git a/src/venus_gps.c b/src/venus_gps.c index 4a02096..1f317a0 100644 --- a/src/venus_gps.c +++ b/src/venus_gps.c @@ -21,6 +21,8 @@ * */ +#define _GNU_SOURCE + #include #include #include @@ -36,6 +38,7 @@ #include #include #include +#include #include "log.h" #include "cbuffer.h" @@ -44,23 +47,18 @@ #include "venus_api.h" static sig_atomic_t timer_expired; -static void sigalarm_handler(int arg) { +static void sigalarm_handler(int arg) +{ timer_expired = 1; } static sig_atomic_t sigpipe = 0; -static void sigpipe_handler(int arg) { +static void sigpipe_handler(int arg) +{ sigpipe = 1; } -enum { - MODE_SERVER = 0, - MODE_CLIENT = 1, -}; - static char *device = "/dev/ttyS3"; -static char remote_host[256]; -static int port = 5445; static speed_t baud_rate = B9600; static uint8_t gga = 1; @@ -71,17 +69,20 @@ static uint8_t rmc = 1; static uint8_t vtg = 1; static uint8_t zda = 1; +#if CONFIG_CAN_RESET /* * XXX: I don't know if these work, yet. I haven't tried them out. * */ -static uint8_t start_mode = (uint8_t) -1; +static uint8_t start_mode = (uint8_t) - 1; static int16_t latitude = 45; static int16_t longitude = -93; static int16_t altitude = 256; +#endif -static int tcp_exchange_data(int sd, int tty) { +static int exchange_data(int fd0, int fd1) +{ int i; int err; struct pollfd fds[2]; @@ -92,57 +93,59 @@ static int tcp_exchange_data(int sd, int tty) { memset(fds, 0, sizeof(fds)); - for(i = 0; i < nfds; i++) { + for (i = 0; i < nfds; i++) { memset(&fds[i], 0, sizeof(fds[i])); bufs[i].read_index = bufs[i].write_index = 0; } - fds[0].fd = tty; - fds[1].fd = sd; + fds[0].fd = fd0; + fds[1].fd = fd1; - while(1) { - for(i = 0; i < nfds; i++) { + while (1) { + for (i = 0; i < nfds; i++) { fds[i].events = 0; } - for(i = 0; i < nfds; i++) { - if(buffer_free(&bufs[i])) { + for (i = 0; i < nfds; i++) { + if (buffer_free(&bufs[i])) { fds[i].events |= POLLIN; } - if(buffer_used(&bufs[i])) { + if (buffer_used(&bufs[i])) { fds[DIR_REV(i)].events |= POLLOUT; } } err = poll(fds, nfds, -1); - if(err <= 0) { - log_error("poll returned %d errno: %d", err, errno); + if (err <= 0) { + log_error("poll returned %d %m", err); break; } log_debug("poll returned %d", err); - for(i = 0; i < nfds; i++) { - if(fds[i].revents) { - log_debug("fds[%d]: %d revents: %04X POLLIN: %04X POLLOUT: %04X POLLHUP: %04X", i, fds[i].fd, fds[i].revents, POLLIN, POLLOUT, POLLHUP); + for (i = 0; i < nfds; i++) { + if (fds[i].revents) { + log_debug("fds[%d]: %d revents: %04X", i, fds[i].fd, fds[i].revents); - if(fds[i].revents & POLLHUP) { + if (fds[i].revents & POLLHUP) { log_error("fds[%d]: %d POLLHUP", i, fds[i].fd); return 0; - } else if(fds[i].revents & POLLOUT) { - log_debug("fds[%d]: %d POLLOUT buffer_used: %lu", i, fds[i].fd, (unsigned long) buffer_used(&bufs[DIR_REV(i)])); + } else if (fds[i].revents & POLLOUT) { + log_debug("fds[%d]: %d POLLOUT buffer_used: %lu", + i, fds[i].fd, (unsigned long) buffer_used(&bufs[DIR_REV(i)])); err = buffered_write(fds[i].fd, &bufs[DIR_REV(i)]); - if(err < 0) { - log_error("fds[%d]: %d safe_write failed: errno: %d", i, fds[i].fd, errno); + if (err < 0) { + log_error("fds[%d]: %d safe_write failed: %m", i, fds[i].fd); return -1; } - } else if(fds[i].revents & POLLIN) { - log_debug("fds[%d]: %d POLLIN buffer_free: %lu", i, fds[i].fd, (unsigned long) buffer_free(&bufs[i])); + } else if (fds[i].revents & POLLIN) { + log_debug("fds[%d]: %d POLLIN buffer_free: %lu", + i, fds[i].fd, (unsigned long) buffer_free(&bufs[i])); err = buffered_read(fds[i].fd, &bufs[i]); - if(err <= 0) { - log_debug("fds[%d]: %d safe_read failed: errno: %d", i, fds[i].fd, errno); + if (err <= 0) { + log_debug("fds[%d]: %d safe_read failed: %m", i, fds[i].fd); return -1; } } @@ -153,20 +156,21 @@ static int tcp_exchange_data(int sd, int tty) { return -1; } -static int udp_send_msgs(int sd, int tty) { +static int udp_send_msgs(int sd, int tty) +{ char buf[NMEA_SENTENCE_MAX]; int count; int err; - while(1) { + while (1) { count = read_nmea_sentence(tty, buf, sizeof(buf)); - if(count <= 0) { + if (count <= 0) { log_error("read_nmea_sentence failed: quiting"); return -1; } err = send(sd, buf, count, 0); - if(err < 0) { - log_debug("send failed: %d", errno); + if (err < 0) { + log_debug("send failed: %m"); return -1; } } @@ -174,22 +178,17 @@ static int udp_send_msgs(int sd, int tty) { return -1; } -static int udp_server() { - log_error("UDP Server mode is not implemented"); - exit(1); - return -1; -} - -static int udp_client() { +static int gps_to_udp_client(const char *host, int port) +{ int tty; int sd; - log_notice("Venus GPS UDP Client connecting to %s:%d", remote_host, port); + log_notice("Venus GPS UDP Client connecting to %s:%d", host, port); - while(1) { - sd = inet_conn_str(remote_host, port, SOCK_DGRAM); - if(sd < 0) { - log_error("inet_conn_str failed: %d", errno); + while (1) { + sd = inet_conn_str(host, port, SOCK_DGRAM); + if (sd < 0) { + log_error("inet_conn_str failed: %m"); sleep(60); } else { log_debug("connection opened"); @@ -208,16 +207,17 @@ static int udp_client() { return -1; } -static int tcp_client() { +static int gps_to_tcp_client(const char *host, int port) +{ int tty; int sd; - log_notice("Venus GPS TCP Client connecting to %s:%d", remote_host, port); + log_notice("Venus GPS TCP Client connecting to %s:%d", host, port); - while(1) { - sd = inet_conn_str(remote_host, port, SOCK_STREAM); - if(sd < 0) { - log_error("inet_conn_str failed: %d", errno); + while (1) { + sd = inet_conn_str(host, port, SOCK_STREAM); + if (sd < 0) { + log_error("inet_conn_str failed: %m"); sleep(60); } else { log_debug("connection opened"); @@ -227,7 +227,7 @@ static int tcp_client() { set_nonblocking(tty); set_nonblocking(sd); - tcp_exchange_data(sd, tty); + exchange_data(sd, tty); close(sd); close(tty); @@ -237,7 +237,8 @@ static int tcp_client() { return -1; } -static int tcp_server() { +static int gps_to_tcp_server(int port) +{ int err; int tty; int lsd; @@ -250,8 +251,8 @@ static int tcp_server() { struct sockaddr_in cli_addr; lsd = socket(PF_INET, SOCK_STREAM, 0); - if(lsd == -1) { - log_error("socket failed: %d", errno); + if (lsd == -1) { + log_error("socket failed: %m"); exit(1); } @@ -261,33 +262,33 @@ static int tcp_server() { serv_addr.sin_port = htons(port); err = setsockopt(lsd, SOL_SOCKET, SO_REUSEADDR, &reuse, sizeof(reuse)); - if(err == -1) { - log_error("setsockopt failed: %d", errno); + if (err == -1) { + log_error("setsockopt failed: %m"); exit(1); } serv_addr_len = sizeof(serv_addr); - + err = bind(lsd, (struct sockaddr *) &serv_addr, sizeof(serv_addr)); - if(err == -1) { - log_error("bind failed: %d", errno); + if (err == -1) { + log_error("bind failed: %m"); exit(1); } err = listen(lsd, 1); - if(err == -1) { - log_error("listen failed: %d", errno); + if (err == -1) { + log_error("listen failed: %m"); exit(1); } log_notice("Venus GPS TCP Server listening on port %d", port); - while(1) { + while (1) { cli_addr_len = sizeof(cli_addr); - sd = accept(lsd, (struct sockaddr *) &cli_addr, (socklen_t *) &cli_addr_len); - if(sd == -1) { - log_error("accept failed: %d", errno); + sd = accept(lsd, (struct sockaddr *) &cli_addr, (socklen_t *) & cli_addr_len); + if (sd == -1) { + log_error("accept failed: %m"); } else { log_debug("accepted"); @@ -296,7 +297,7 @@ static int tcp_server() { set_nonblocking(tty); set_nonblocking(sd); - tcp_exchange_data(sd, tty); + exchange_data(sd, tty); close(sd); close(tty); @@ -308,7 +309,88 @@ static int tcp_server() { return -1; } -static void print_version(const char *name) { +static int gps_to_file(const char *file) +{ + int tty; + int fd; + + log_notice("Venus GPS to file %s", file); + + if (!strcmp(file, "-")) { + fd = fileno(stdout); + } else { + fd = open(file, O_WRONLY); + if (fd < 0) { + log_error("failed to open %s: %m", file); + return -1; + } + } + + set_nonblocking(fd); + + while (1) { + tty = venus_open(device, baud_rate); + + set_nonblocking(tty); + + exchange_data(fd, tty); + + close(tty); + } + + return -1; +} + +int gps_to_serial(const char *port) +{ + int tty; + int fd; + struct termios tio; + + log_notice("Venus GPS to serial %s", port); + + fd = open(port, O_RDWR | O_NOCTTY); + if (fd < 0) { + log_error("failed to open %s: %m", port); + return -1; + } + + tcgetattr(fd, &tio); + cfmakeraw(&tio); + cfsetspeed(&tio, B115200); + + tio.c_cflag &= ~CSIZE; + tio.c_cflag |= CS8; + + tio.c_cflag &= ~(PARENB | PARODD); + tio.c_cflag |= 0; + + tio.c_cflag &= ~CSTOPB; + tio.c_cflag |= 0; + + tio.c_cflag &= ~CRTSCTS; + tio.c_cflag |= 0; + + tcsetattr(fd, TCSANOW, &tio); + tcflush(fd, TCIOFLUSH); + + set_nonblocking(fd); + + while (1) { + tty = venus_open(device, baud_rate); + + set_nonblocking(tty); + + exchange_data(fd, tty); + + close(tty); + } + + return -1; +} + +static void print_version(const char *name) +{ printf("%s (" PACKAGE ") " VERSION " (" __DATE__ " " __TIME__ ")\n", name); printf("Copyright (C) 2010 by Multi-Tech Systems\n"); printf( @@ -317,15 +399,17 @@ static void print_version(const char *name) { "This program has absolutely no warranty.\n"); } -static void usage(FILE *out) { +static void usage(FILE * out) +{ fprintf(out, "usage: venus-gps [ OPTIONS ... ]\n"); fprintf(out, "where OPTIONS := { \n"); fprintf(out, " --daemonize |\n"); fprintf(out, " -d --device (default: /dev/ttyS3) |\n"); - fprintf(out, " -p --port (default: 5445) |\n"); - fprintf(out, " --protocol { tcp (default) | udp } |\n"); - fprintf(out, " --mode { client | server (default) } |\n"); - fprintf(out, " --remote-host |\n"); + fprintf(out, " --file |\n"); + fprintf(out, " --tcp-server |\n"); + fprintf(out, " --tcp-client |\n"); + fprintf(out, " --udp-client |\n"); + fprintf(out, " --serial |\n"); #if CONFIG_CAN_DEFAULT fprintf(out, " -f --factory-defaults |\n"); #endif @@ -351,9 +435,9 @@ static void usage(FILE *out) { } #define OPT_DEVICE 'd' +#define OPT_BAUD_RATE 'b' #define OPT_PORT 'p' #define OPT_FACTORY_DEFAULTS 'f' -#define OPT_BAUD_RATE 'b' enum { OPT_VERSION = 128, @@ -369,21 +453,25 @@ enum { OPT_LATITUDE, OPT_LONGITUDE, OPT_ALTITUDE, - OPT_REMOTE_HOST, OPT_DAEMONIZE, - OPT_PROTOCOL, - OPT_MODE, + OPT_FILE, + OPT_TCP_SERVER, + OPT_TCP_CLIENT, + OPT_UDP_CLIENT, + OPT_SERIAL, }; static char *short_options = "b:d:p:f"; static struct option long_options[] = { - {"protocol", 1, 0, OPT_PROTOCOL}, {"daemonize", 0, 0, OPT_DAEMONIZE}, {"device", 1, 0, OPT_DEVICE}, - {"mode", 1, 0, OPT_MODE}, - {"remote-host", 1, 0, OPT_REMOTE_HOST}, - {"port", 1, 0, OPT_PORT}, {"baud-rate", 1, 0, OPT_BAUD_RATE}, + {"port", 1, 0, OPT_PORT}, + {"file", 1, 0, OPT_FILE}, + {"tcp-server", 0, 0, OPT_TCP_SERVER}, + {"tcp-client", 1, 0, OPT_TCP_CLIENT}, + {"udp-client", 1, 0, OPT_UDP_CLIENT}, + {"serial", 1, 0, OPT_SERIAL}, #if CONFIG_CAN_DEFAULT {"factory-defaults", 0, 0, OPT_FACTORY_DEFAULTS}, #endif @@ -407,14 +495,27 @@ static struct option long_options[] = { {0, 0, 0, 0}, }; -int main(int argc, char *argv[]) { +int main(int argc, char *argv[]) +{ int i; + int tmp; + char *cp; int option_index; int tty; +#if CONFIG_CAN_DEFAULT int factory_defaults = 0; +#endif int daemonize = 0; - int proto = IPPROTO_TCP; - int mode = MODE_SERVER; + int port = 5445; + + char *file = "-"; + char tcp_server_enabled = 0; + int tcp_server_port = port; + char *tcp_client_host = NULL; + int tcp_client_port = port; + char *udp_client_host = NULL; + int udp_client_port = port; + char *serial_port = NULL; struct sigaction sa; @@ -428,56 +529,83 @@ int main(int argc, char *argv[]) { sa.sa_flags = 0; sigaction(SIGPIPE, &sa, NULL); - while((i = getopt_long(argc, argv, short_options, long_options, &option_index)) >= 0) { - switch(i) { + while ((i = getopt_long(argc, argv, short_options, long_options, &option_index)) >= 0) { + switch (i) { case 0: break; - case OPT_PROTOCOL: - if(!strcasecmp("tcp", optarg)) { - proto = IPPROTO_TCP; - } else if(!strcasecmp("udp", optarg)) { - proto = IPPROTO_UDP; - } else { - log_error("invalid transport protocol"); - usage(stderr); - exit(1); - } - break; - case OPT_DAEMONIZE: daemonize = 1; break; - case OPT_MODE: - if(!strcasecmp("server", optarg)) { - mode = MODE_SERVER; - } else if(!strcasecmp("client", optarg)) { - mode = MODE_CLIENT; - } else { - log_error("invalid mode"); - usage(stderr); - exit(1); - } - break; - case OPT_DEVICE: device = optarg; break; case OPT_PORT: port = atoi(optarg); - if(port <= 0 || port >= (1 << 16)) { + if (port <= 0 || port >= (1 << 16)) { log_error("invalid port"); usage(stderr); exit(1); } break; + case OPT_FILE: + file = optarg; + break; + + case OPT_SERIAL: + serial_port = optarg; + break; + + case OPT_TCP_SERVER: + tcp_server_enabled = 1; + tcp_server_port = port; + break; + + case OPT_TCP_CLIENT: + cp = strchr(optarg, ':'); + if (cp) { + tcp_client_host = strndup(optarg, cp - optarg); + + cp++; + + tcp_client_port = atoi(cp); + if (tcp_client_port <= 0 || tcp_client_port >= (1 << 16)) { + log_error("invalid port"); + usage(stderr); + exit(1); + } + } else { + tcp_client_host = strdup(optarg); + tcp_client_port = port; + } + break; + + case OPT_UDP_CLIENT: + cp = strchr(optarg, ':'); + if (cp) { + udp_client_host = strndup(optarg, cp - optarg); + + cp++; + + udp_client_port = atoi(cp); + if (udp_client_port <= 0 || udp_client_port >= (1 << 16)) { + log_error("invalid port"); + usage(stderr); + exit(1); + } + } else { + udp_client_host = strdup(optarg); + udp_client_port = port; + } + break; + case OPT_BAUD_RATE: baud_rate = atoi(optarg); baud_rate = value_to_baud(baud_rate); - if(baud_rate == (speed_t) -1) { + if (baud_rate == (speed_t) - 1) { usage(stderr); exit(1); } @@ -537,10 +665,6 @@ int main(int argc, char *argv[]) { break; #endif - case OPT_REMOTE_HOST: - snprintf(remote_host, sizeof(remote_host), "%s", optarg); - break; - case OPT_VERSION: print_version("venus-gps"); exit(0); @@ -557,19 +681,18 @@ int main(int argc, char *argv[]) { } } - if(optind < argc) { + if (optind < argc) { usage(stderr); exit(1); } - if(daemonize) { + if (daemonize) { #if CONFIG_USE_SYSLOG - daemon(0, 0); + tmp = daemon(0, 0); #else - daemon(0, 1); + tmp = daemon(0, 1); #endif } - #if CONFIG_USE_SYSLOG openlog("VenusGPS", LOG_NDELAY, LOG_FACILITY); # if DEBUG @@ -580,24 +703,28 @@ int main(int argc, char *argv[]) { #endif tty = venus_open(device, baud_rate); - if(tty < 0) { - log_error("failed to open tty: errno: %d", errno); + if (tty < 0) { + log_error("failed to open tty: %m"); exit(1); } venus_conf_format(tty, MSG_TYPE_BINARY, UPDATE_ATTR_SRAM); venus_query_sw_version(tty, SW_TYPE_RESERVED); - if(factory_defaults) { +#if CONFIG_CAN_DEFAULT + if (factory_defaults) { venus_factory_defaults(tty, FACTORY_DEFAULTS_REBOOT); log_notice("setting factory defaults"); exit(0); } - if(start_mode != (typeof(start_mode)) -1) { +#endif +#if CONFIG_CAN_RESET + if (start_mode != (typeof(start_mode)) - 1) { venus_system_restart(tty, start_mode, time(NULL), latitude, longitude, altitude); log_notice("issuing system restart"); exit(0); } +#endif venus_conf_nmea(tty, gga, gsa, gsv, gll, rmc, vtg, zda, UPDATE_ATTR_SRAM); @@ -605,26 +732,19 @@ int main(int argc, char *argv[]) { close(tty); - if(proto == IPPROTO_TCP) { - if(mode == MODE_SERVER) { - tcp_server(); - } else { - if(!*remote_host) { - log_notice("remote-host must be specified"); - exit(1); - } - tcp_client(); - } - } else if(proto == IPPROTO_UDP) { - if(mode == MODE_SERVER) { - udp_server(); - } else { - if(!*remote_host) { - log_notice("remote-host must be specified"); - exit(1); - } - udp_client(); - } + /* + * FIXME: Support more than one mode at a time + */ + if (tcp_server_enabled) { + gps_to_tcp_server(tcp_server_port); + } else if (tcp_client_host) { + gps_to_tcp_client(tcp_client_host, tcp_client_port); + } else if (udp_client_host) { + gps_to_udp_client(udp_client_host, udp_client_port); + } else if (serial_port) { + gps_to_serial(serial_port); + } else if (file) { + gps_to_file(file); } return 0; -- cgit v1.2.3