add gps to serial example

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 <errno.h>
 
-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 <stdio.h>
 #include <sys/types.h>
 #include <sys/stat.h>
@@ -36,6 +38,7 @@
 #include <getopt.h>
 #include <time.h>
 #include <signal.h>
+#include <string.h>
 
 #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;
+	}
+
+	tcge