path: root/src/venus_gps.c

/*
 * Venus GPS Controller Example
 *
 * Copyright (C) 2010 by Multi-Tech Systems
 *
 * Author: James Maki <jmaki@multitech.com>
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
 * General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
 *
 */

#include <stdio.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <stdint.h>
#include <fcntl.h>
#include <errno.h>
#include <stdlib.h>
#include <termios.h>
#include <unistd.h>
#include <string.h>
#include <stdarg.h>
#include <poll.h>
#include <getopt.h>
#include <time.h>
#include <signal.h>

#include "log.h"
#include "cbuffer.h"
#include "utils.h"
#include "venus_gps.h"
#include "venus_api.h"

static sig_atomic_t timer_expired;
static void sigalarm_handler(int arg) {
	timer_expired = 1;
}

static sig_atomic_t sigpipe = 0;
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;
static uint8_t gsa = 1;
static uint8_t gsv = 1;
static uint8_t gll = 1;
static uint8_t rmc = 1;
static uint8_t vtg = 1;
static uint8_t zda = 1;

/*
 * XXX: I don't know if these work, yet. I haven't tried them out.
 *
 */
static uint8_t start_mode = (uint8_t) -1;
static int16_t latitude = 45;
static int16_t longitude = -93;
static int16_t altitude = 256;


static int tcp_exchange_data(int sd, int tty) {
	int i;
	int err;
	struct pollfd fds[2];
	nfds_t nfds = 2;
	struct cbuffer bufs[2];

	log_debug("starting");

	memset(fds, 0, sizeof(fds));

	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;

	while(1) {
		for(i = 0; i < nfds; i++) {
			fds[i].events = 0;
		}

		for(i = 0; i < nfds; i++) {
			if(buffer_free(&bufs[i])) {
				fds[i].events |= POLLIN;
			}
			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);
			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);

				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)]));

					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);
						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]));

					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);
						return -1;
					}
				}
			}
		}
	}

	return -1;
}

static int udp_send_msgs(int sd, int tty) {
	char buf[NMEA_SENTENCE_MAX];
	int count;
	int err;

	while(1) {
		count = read_nmea_sentence(tty, buf, sizeof(buf));
		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);
			return -1;
		}
	}

	return -1;
}

static int udp_server() {
	log_error("UDP Server mode is not implemented");
	exit(1);
	return -1;
}

static int udp_client() {
	int tty;
	int sd;

	log_notice("Venus GPS UDP Client connecting to %s:%d", remote_host, port);

	while(1) {
		sd = inet_conn_str(remote_host, port, SOCK_DGRAM);
		if(sd < 0) {
			log_error("inet_conn_str failed: %d", errno);
			sleep(60);
		} else {
			log_debug("connection opened");

			tty = venus_open(device, baud_rate);

			set_nonblocking(sd);

			udp_send_msgs(sd, tty);

			close(sd);
			close(tty);
		}
	}

	return -1;
}

static int tcp_client() {
	int tty;
	int sd;

	log_notice("Venus GPS TCP Client connecting to %s:%d", remote_host, port);

	while(1) {
		sd = inet_conn_str(remote_host, port, SOCK_STREAM);
		if(sd < 0) {
			log_error("inet_conn_str failed: %d", errno);
			sleep(60);
		} else {
			log_debug("connection opened");

			tty = venus_open(device, baud_rate);

			set_nonblocking(tty);
			set_nonblocking(sd);

			tcp_exchange_data(sd, tty);

			close(sd);
			close(tty);
		}
	}

	return -1;
}

static int tcp_server() {
	int err;
	int tty;
	int lsd;
	int sd;
	int cli_addr_len;
	int serv_addr_len;
	int reuse = 1;

	struct sockaddr_in serv_addr;
	struct sockaddr_in cli_addr;

	lsd = socket(PF_INET, SOCK_STREAM, 0);
	if(lsd == -1) {
		log_error("socket failed: %d", errno);
		exit(1);
	}

	memset(&serv_addr, 0, sizeof(serv_addr));
	serv_addr.sin_family = AF_INET;
	serv_addr.sin_addr.s_addr = htonl(INADDR_ANY);
	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);
		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);
		exit(1);
	}

	err = listen(lsd, 1);
	if(err == -1) {
		log_error("listen failed: %d", errno);
		exit(1);
	}

	log_notice("Venus GPS TCP Server listening on port %d", port);

	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);
		} else {
			log_debug("accepted");

			tty = venus_open(device, baud_rate);

			set_nonblocking(tty);
			set_nonblocking(sd);

			tcp_exchange_data(sd, tty);

			close(sd);
			close(tty);
		}
	}

	close(lsd);

	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(
"This program is free software; you may redistribute it under the terms of\n"
"the GNU General Public License version 2 or (at your option) any later version.\n"
"This program has absolutely no warranty.\n");
}

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 <device> (default: /dev/ttyS3) |\n");
	fprintf(out, "         -p --port <port> (default: 5445) |\n");
	fprintf(out, "         --protocol { tcp (default) | udp } |\n");
	fprintf(out, "         --mode { client | server (default) } |\n");
	fprintf(out, "         --remote-host <host> |\n");
#if CONFIG_CAN_DEFAULT
	fprintf(out, "         -f --factory-defaults |\n");
#endif
#if CONFIG_CAN_RESET
	fprintf(out, "\n");
	fprintf(out, "         --start-mode 0-5 (default: 0) |\n");
	fprintf(out, "         --latitude VAL (default: ) |\n");
	fprintf(out, "         --longitude VAL (default: ) |\n");
	fprintf(out, "         --altitude VAL (default: ) |\n");
#endif
	fprintf(out, "\n");
	fprintf(out, "         --gga 0-255 (default: 1) |\n");
	fprintf(out, "         --gsa 0-255 (default: 1) |\n");
	fprintf(out, "         --gsv 0-255 (default: 1) |\n");
	fprintf(out, "         --gll 0-255 (default: 1) |\n");
	fprintf(out, "         --rmc 0-255 (default: 1) |\n");
#if CONFIG_HAVE_ZDA
	fprintf(out, "         --zda 0-255 (default: 1) |\n");
#endif
	fprintf(out, "         --vtg 0-255 (default: 1)\n");
	fprintf(out, "       }\n");
	fprintf(out, "\n");
}

#define OPT_DEVICE		'd'
#define OPT_PORT		'p'
#define OPT_FACTORY_DEFAULTS	'f'
#define OPT_BAUD_RATE		'b'

enum {
	OPT_VERSION = 128,
	OPT_HELP,
	OPT_GGA,
	OPT_GSA,
	OPT_GSV,
	OPT_GLL,
	OPT_RMC,
	OPT_VTG,
	OPT_ZDA,
	OPT_START_MODE,
	OPT_LATITUDE,
	OPT_LONGITUDE,
	OPT_ALTITUDE,
	OPT_REMOTE_HOST,
	OPT_DAEMONIZE,
	OPT_PROTOCOL,
	OPT_MODE,
};

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},
#if CONFIG_CAN_DEFAULT
	{"factory-defaults", 0, 0, OPT_FACTORY_DEFAULTS},
#endif
	{"gga", 1, 0, OPT_GGA},
	{"gsa", 1, 0, OPT_GSA},
	{"gsv", 1, 0, OPT_GSV},
	{"gll", 1, 0, OPT_GLL},
	{"rmc", 1, 0, OPT_RMC},
	{"vtg", 1, 0, OPT_VTG},
#if CONFIG_HAVE_ZDA
	{"zda", 1, 0, OPT_ZDA},
#endif
#if CONFIG_CAN_RESET
	{"start-mode", 1, 0, OPT_START_MODE},
	{"latitude", 1, 0, OPT_LATITUDE},
	{"longitude", 1, 0, OPT_LONGITUDE},
	{"altitude", 1, 0, OPT_ALTITUDE},
#endif
	{"version", 0, NULL, OPT_VERSION},
	{"help", 0, NULL, OPT_HELP},
	{0, 0, 0, 0},
};

int main(int argc, char *argv[]) {
	int i;
	int option_index;
	int tty;
	int factory_defaults = 0;
	int daemonize = 0;
	int proto = IPPROTO_TCP;
	int mode = MODE_SERVER;

	struct sigaction sa;

	sa.sa_handler = sigalarm_handler;
	sigemptyset(&sa.sa_mask);
	sa.sa_flags = 0;
	sigaction(SIGALRM, &sa, NULL);

	sa.sa_handler = sigpipe_handler;
	sigemptyset(&sa.sa_mask);
	sa.sa_flags = 0;
	sigaction(SIGPIPE, &sa, NULL);

	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)) {
				log_error("invalid port");
				usage(stderr);
				exit(1);
			}
			break;

		case OPT_BAUD_RATE:
			baud_rate = atoi(optarg);
			baud_rate = value_to_baud(baud_rate);
			if(baud_rate == (speed_t) -1) {
				usage(stderr);
				exit(1);
			}
			break;

		case OPT_GGA:
			gga = atoi(optarg);
			break;

		case OPT_GSA:
			gsa = atoi(optarg);
			break;

		case OPT_GSV:
			gsv = atoi(optarg);
			break;

		case OPT_GLL:
			gll = atoi(optarg);
			break;

		case OPT_RMC:
			rmc = atoi(optarg);
			break;

		case OPT_VTG:
			vtg = atoi(optarg);
			break;

#if CONFIG_HAVE_ZDA
		case OPT_ZDA:
			zda = atoi(optarg);
			break;
#endif

#if CONFIG_CAN_RESET
		case OPT_START_MODE:
			start_mode = (uint8_t) atoi(optarg);
			break;

		case OPT_LATITUDE:
			latitude = (int16_t) atoi(optarg);
			break;

		case OPT_LONGITUDE:
			longitude = (int16_t) atoi(optarg);
			break;

		case OPT_ALTITUDE:
			altitude = (int16_t) atoi(optarg);
			break;
#endif

#if CONFIG_CAN_DEFAULT
		case OPT_FACTORY_DEFAULTS:
			factory_defaults = 1;
			break;
#endif

		case OPT_REMOTE_HOST:
			snprintf(remote_host, sizeof(remote_host), "%s", optarg);
			break;

		case OPT_VERSION:
			print_version("venus-gps");
			exit(0);
			break;

		case OPT_HELP:
			usage(stdout);
			exit(0);
			break;

		default:
			usage(stderr);
			exit(1);
		}
	}

	if(optind < argc) {
		usage(stderr);
		exit(1);
	}

	if(daemonize) {
#if CONFIG_USE_SYSLOG
		daemon(0, 0);
#else
		daemon(0, 1);
#endif
	}

#if CONFIG_USE_SYSLOG
	openlog("VenusGPS", LOG_NDELAY, LOG_FACILITY);
# if DEBUG
	setlogmask(LOG_UPTO(LOG_DEBUG));
# else
	setlogmask(LOG_UPTO(LOG_INFO));
# endif
#endif

	tty = venus_open(device, baud_rate);
	if(tty < 0) {
		log_error("failed to open tty: errno: %d", errno);
		exit(1);
	}

	venus_conf_format(tty, MSG_TYPE_BINARY, UPDATE_ATTR_SRAM);
	venus_query_sw_version(tty, SW_TYPE_RESERVED);

	if(factory_defaults) {
		venus_factory_defaults(tty, FACTORY_DEFAULTS_REBOOT);
		log_notice("setting factory defaults");
		exit(0);
	}
	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);
	}

	venus_conf_nmea(tty, gga, gsa, gsv, gll, rmc, vtg, zda, UPDATE_ATTR_SRAM);

	venus_conf_format(tty, MSG_TYPE_NMEA, UPDATE_ATTR_SRAM);

	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();
		}
	}

	return 0;
}