From 5450acde53db80f453a658f59ab887212680ca34 Mon Sep 17 00:00:00 2001 From: James Maki Date: Fri, 23 Apr 2010 12:32:53 -0500 Subject: initial commit --- src/Makefile.am | 8 + src/Makefile.in | 417 +++++++++++++++++++++++++++++++++++++ src/cbuffer.c | 107 ++++++++++ src/cbuffer.h | 22 ++ src/gps_test.rb | 43 ++++ src/log.h | 32 +++ src/utils.c | 217 +++++++++++++++++++ src/utils.h | 27 +++ src/venus_api.c | 487 +++++++++++++++++++++++++++++++++++++++++++ src/venus_api.h | 98 +++++++++ src/venus_gps.c | 631 ++++++++++++++++++++++++++++++++++++++++++++++++++++++++ src/venus_gps.h | 6 + 12 files changed, 2095 insertions(+) create mode 100644 src/Makefile.am create mode 100644 src/Makefile.in create mode 100644 src/cbuffer.c create mode 100644 src/cbuffer.h create mode 100755 src/gps_test.rb create mode 100644 src/log.h create mode 100644 src/utils.c create mode 100644 src/utils.h create mode 100644 src/venus_api.c create mode 100644 src/venus_api.h create mode 100644 src/venus_gps.c create mode 100644 src/venus_gps.h (limited to 'src') diff --git a/src/Makefile.am b/src/Makefile.am new file mode 100644 index 0000000..b9e40b1 --- /dev/null +++ b/src/Makefile.am @@ -0,0 +1,8 @@ +AUTOMAKE_OPTIONS = gnu +AM_CFLAGS = -Wall +bin_PROGRAMS = venus-gps +venus_gps_SOURCES = venus_gps.c cbuffer.c utils.c venus_api.c +noinst_HEADERS = venus_gps.h cbuffer.h log.h utils.h venus_api.h + +EXTRA_DIST = + diff --git a/src/Makefile.in b/src/Makefile.in new file mode 100644 index 0000000..599b96a --- /dev/null +++ b/src/Makefile.in @@ -0,0 +1,417 @@ +# Makefile.in generated by automake 1.10.1 from Makefile.am. +# @configure_input@ + +# Copyright (C) 1994, 1995, 1996, 1997, 1998, 1999, 2000, 2001, 2002, +# 2003, 2004, 2005, 2006, 2007, 2008 Free Software Foundation, Inc. +# This Makefile.in is free software; the Free Software Foundation +# gives unlimited permission to copy and/or distribute it, +# with or without modifications, as long as this notice is preserved. + +# This program is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY, to the extent permitted by law; without +# even the implied warranty of MERCHANTABILITY or FITNESS FOR A +# PARTICULAR PURPOSE. + +@SET_MAKE@ + + +VPATH = @srcdir@ +pkgdatadir = $(datadir)/@PACKAGE@ +pkglibdir = $(libdir)/@PACKAGE@ +pkgincludedir = $(includedir)/@PACKAGE@ +am__cd = CDPATH="$${ZSH_VERSION+.}$(PATH_SEPARATOR)" && cd +install_sh_DATA = $(install_sh) -c -m 644 +install_sh_PROGRAM = $(install_sh) -c +install_sh_SCRIPT = $(install_sh) -c +INSTALL_HEADER = $(INSTALL_DATA) +transform = $(program_transform_name) +NORMAL_INSTALL = : +PRE_INSTALL = : +POST_INSTALL = : +NORMAL_UNINSTALL = : +PRE_UNINSTALL = : +POST_UNINSTALL = : +bin_PROGRAMS = venus-gps$(EXEEXT) +subdir = src +DIST_COMMON = $(noinst_HEADERS) $(srcdir)/Makefile.am \ + $(srcdir)/Makefile.in +ACLOCAL_M4 = $(top_srcdir)/aclocal.m4 +am__aclocal_m4_deps = $(top_srcdir)/configure.in +am__configure_deps = $(am__aclocal_m4_deps) $(CONFIGURE_DEPENDENCIES) \ + $(ACLOCAL_M4) +mkinstalldirs = $(install_sh) -d +CONFIG_HEADER = $(top_builddir)/config.h +CONFIG_CLEAN_FILES = +am__installdirs = "$(DESTDIR)$(bindir)" +binPROGRAMS_INSTALL = $(INSTALL_PROGRAM) +PROGRAMS = $(bin_PROGRAMS) +am_venus_gps_OBJECTS = venus_gps.$(OBJEXT) cbuffer.$(OBJEXT) \ + utils.$(OBJEXT) venus_api.$(OBJEXT) +venus_gps_OBJECTS = $(am_venus_gps_OBJECTS) +venus_gps_LDADD = $(LDADD) +DEFAULT_INCLUDES = -I.@am__isrc@ -I$(top_builddir) +depcomp = $(SHELL) $(top_srcdir)/depcomp +am__depfiles_maybe = depfiles +COMPILE = $(CC) $(DEFS) $(DEFAULT_INCLUDES) $(INCLUDES) $(AM_CPPFLAGS) \ + $(CPPFLAGS) $(AM_CFLAGS) $(CFLAGS) +CCLD = $(CC) +LINK = $(CCLD) $(AM_CFLAGS) $(CFLAGS) $(AM_LDFLAGS) $(LDFLAGS) -o $@ +SOURCES = $(venus_gps_SOURCES) +DIST_SOURCES = $(venus_gps_SOURCES) +HEADERS = $(noinst_HEADERS) +ETAGS = etags +CTAGS = ctags +DISTFILES = $(DIST_COMMON) $(DIST_SOURCES) $(TEXINFOS) $(EXTRA_DIST) +ACLOCAL = @ACLOCAL@ +AMTAR = @AMTAR@ +AUTOCONF = @AUTOCONF@ +AUTOHEADER = @AUTOHEADER@ +AUTOMAKE = @AUTOMAKE@ +AWK = @AWK@ +CC = @CC@ +CCDEPMODE = @CCDEPMODE@ +CFLAGS = @CFLAGS@ +CPP = @CPP@ +CPPFLAGS = @CPPFLAGS@ +CYGPATH_W = @CYGPATH_W@ +DEFS = @DEFS@ +DEPDIR = @DEPDIR@ +ECHO_C = @ECHO_C@ +ECHO_N = @ECHO_N@ +ECHO_T = @ECHO_T@ +EGREP = @EGREP@ +EXEEXT = @EXEEXT@ +GREP = @GREP@ +INSTALL = @INSTALL@ +INSTALL_DATA = @INSTALL_DATA@ +INSTALL_PROGRAM = @INSTALL_PROGRAM@ +INSTALL_SCRIPT = @INSTALL_SCRIPT@ +INSTALL_STRIP_PROGRAM = @INSTALL_STRIP_PROGRAM@ +LDFLAGS = @LDFLAGS@ +LIBOBJS = @LIBOBJS@ +LIBS = @LIBS@ +LTLIBOBJS = @LTLIBOBJS@ +MAKEINFO = @MAKEINFO@ +MKDIR_P = @MKDIR_P@ +OBJEXT = @OBJEXT@ +PACKAGE = @PACKAGE@ +PACKAGE_BUGREPORT = @PACKAGE_BUGREPORT@ +PACKAGE_NAME = @PACKAGE_NAME@ +PACKAGE_STRING = @PACKAGE_STRING@ +PACKAGE_TARNAME = @PACKAGE_TARNAME@ +PACKAGE_VERSION = @PACKAGE_VERSION@ +PATH_SEPARATOR = @PATH_SEPARATOR@ +SET_MAKE = @SET_MAKE@ +SHELL = @SHELL@ +STRIP = @STRIP@ +VERSION = @VERSION@ +abs_builddir = @abs_builddir@ +abs_srcdir = @abs_srcdir@ +abs_top_builddir = @abs_top_builddir@ +abs_top_srcdir = @abs_top_srcdir@ +ac_ct_CC = @ac_ct_CC@ +am__include = @am__include@ +am__leading_dot = @am__leading_dot@ +am__quote = @am__quote@ +am__tar = @am__tar@ +am__untar = @am__untar@ +bindir = @bindir@ +build_alias = @build_alias@ +builddir = @builddir@ +datadir = @datadir@ +datarootdir = @datarootdir@ +docdir = @docdir@ +dvidir = @dvidir@ +exec_prefix = @exec_prefix@ +host_alias = @host_alias@ +htmldir = @htmldir@ +includedir = @includedir@ +infodir = @infodir@ +install_sh = @install_sh@ +libdir = @libdir@ +libexecdir = @libexecdir@ +localedir = @localedir@ +localstatedir = @localstatedir@ +mandir = @mandir@ +mkdir_p = @mkdir_p@ +oldincludedir = @oldincludedir@ +pdfdir = @pdfdir@ +prefix = @prefix@ +program_transform_name = @program_transform_name@ +psdir = @psdir@ +sbindir = @sbindir@ +sharedstatedir = @sharedstatedir@ +srcdir = @srcdir@ +sysconfdir = @sysconfdir@ +target_alias = @target_alias@ +top_builddir = @top_builddir@ +top_srcdir = @top_srcdir@ +AUTOMAKE_OPTIONS = gnu +AM_CFLAGS = -Wall +venus_gps_SOURCES = venus_gps.c cbuffer.c utils.c venus_api.c +noinst_HEADERS = venus_gps.h cbuffer.h log.h utils.h venus_api.h +EXTRA_DIST = +all: all-am + +.SUFFIXES: +.SUFFIXES: .c .o .obj +$(srcdir)/Makefile.in: $(srcdir)/Makefile.am $(am__configure_deps) + @for dep in $?; do \ + case '$(am__configure_deps)' in \ + *$$dep*) \ + cd $(top_builddir) && $(MAKE) $(AM_MAKEFLAGS) am--refresh \ + && exit 0; \ + exit 1;; \ + esac; \ + done; \ + echo ' cd $(top_srcdir) && $(AUTOMAKE) --gnu src/Makefile'; \ + cd $(top_srcdir) && \ + $(AUTOMAKE) --gnu src/Makefile +.PRECIOUS: Makefile +Makefile: $(srcdir)/Makefile.in $(top_builddir)/config.status + @case '$?' in \ + *config.status*) \ + cd $(top_builddir) && $(MAKE) $(AM_MAKEFLAGS) am--refresh;; \ + *) \ + echo ' cd $(top_builddir) && $(SHELL) ./config.status $(subdir)/$@ $(am__depfiles_maybe)'; \ + cd $(top_builddir) && $(SHELL) ./config.status $(subdir)/$@ $(am__depfiles_maybe);; \ + esac; + +$(top_builddir)/config.status: $(top_srcdir)/configure $(CONFIG_STATUS_DEPENDENCIES) + cd $(top_builddir) && $(MAKE) $(AM_MAKEFLAGS) am--refresh + +$(top_srcdir)/configure: $(am__configure_deps) + cd $(top_builddir) && $(MAKE) $(AM_MAKEFLAGS) am--refresh +$(ACLOCAL_M4): $(am__aclocal_m4_deps) + cd $(top_builddir) && $(MAKE) $(AM_MAKEFLAGS) am--refresh +install-binPROGRAMS: $(bin_PROGRAMS) + @$(NORMAL_INSTALL) + test -z "$(bindir)" || $(MKDIR_P) "$(DESTDIR)$(bindir)" + @list='$(bin_PROGRAMS)'; for p in $$list; do \ + p1=`echo $$p|sed 's/$(EXEEXT)$$//'`; \ + if test -f $$p \ + ; then \ + f=`echo "$$p1" | sed 's,^.*/,,;$(transform);s/$$/$(EXEEXT)/'`; \ + echo " $(INSTALL_PROGRAM_ENV) $(binPROGRAMS_INSTALL) '$$p' '$(DESTDIR)$(bindir)/$$f'"; \ + $(INSTALL_PROGRAM_ENV) $(binPROGRAMS_INSTALL) "$$p" "$(DESTDIR)$(bindir)/$$f" || exit 1; \ + else :; fi; \ + done + +uninstall-binPROGRAMS: + @$(NORMAL_UNINSTALL) + @list='$(bin_PROGRAMS)'; for p in $$list; do \ + f=`echo "$$p" | sed 's,^.*/,,;s/$(EXEEXT)$$//;$(transform);s/$$/$(EXEEXT)/'`; \ + echo " rm -f '$(DESTDIR)$(bindir)/$$f'"; \ + rm -f "$(DESTDIR)$(bindir)/$$f"; \ + done + +clean-binPROGRAMS: + -test -z "$(bin_PROGRAMS)" || rm -f $(bin_PROGRAMS) +venus-gps$(EXEEXT): $(venus_gps_OBJECTS) $(venus_gps_DEPENDENCIES) + @rm -f venus-gps$(EXEEXT) + $(LINK) $(venus_gps_OBJECTS) $(venus_gps_LDADD) $(LIBS) + +mostlyclean-compile: + -rm -f *.$(OBJEXT) + +distclean-compile: + -rm -f *.tab.c + +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/cbuffer.Po@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/utils.Po@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/venus_api.Po@am__quote@ +@AMDEP_TRUE@@am__include@ @am__quote@./$(DEPDIR)/venus_gps.Po@am__quote@ + +.c.o: +@am__fastdepCC_TRUE@ $(COMPILE) -MT $@ -MD -MP -MF $(DEPDIR)/$*.Tpo -c -o $@ $< +@am__fastdepCC_TRUE@ mv -f $(DEPDIR)/$*.Tpo $(DEPDIR)/$*.Po +@AMDEP_TRUE@@am__fastdepCC_FALSE@ source='$<' object='$@' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCC_FALSE@ DEPDIR=$(DEPDIR) $(CCDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCC_FALSE@ $(COMPILE) -c $< + +.c.obj: +@am__fastdepCC_TRUE@ $(COMPILE) -MT $@ -MD -MP -MF $(DEPDIR)/$*.Tpo -c -o $@ `$(CYGPATH_W) '$<'` +@am__fastdepCC_TRUE@ mv -f $(DEPDIR)/$*.Tpo $(DEPDIR)/$*.Po +@AMDEP_TRUE@@am__fastdepCC_FALSE@ source='$<' object='$@' libtool=no @AMDEPBACKSLASH@ +@AMDEP_TRUE@@am__fastdepCC_FALSE@ DEPDIR=$(DEPDIR) $(CCDEPMODE) $(depcomp) @AMDEPBACKSLASH@ +@am__fastdepCC_FALSE@ $(COMPILE) -c `$(CYGPATH_W) '$<'` + +ID: $(HEADERS) $(SOURCES) $(LISP) $(TAGS_FILES) + list='$(SOURCES) $(HEADERS) $(LISP) $(TAGS_FILES)'; \ + unique=`for i in $$list; do \ + if test -f "$$i"; then echo $$i; else echo $(srcdir)/$$i; fi; \ + done | \ + $(AWK) '{ files[$$0] = 1; nonemtpy = 1; } \ + END { if (nonempty) { for (i in files) print i; }; }'`; \ + mkid -fID $$unique +tags: TAGS + +TAGS: $(HEADERS) $(SOURCES) $(TAGS_DEPENDENCIES) \ + $(TAGS_FILES) $(LISP) + tags=; \ + here=`pwd`; \ + list='$(SOURCES) $(HEADERS) $(LISP) $(TAGS_FILES)'; \ + unique=`for i in $$list; do \ + if test -f "$$i"; then echo $$i; else echo $(srcdir)/$$i; fi; \ + done | \ + $(AWK) '{ files[$$0] = 1; nonempty = 1; } \ + END { if (nonempty) { for (i in files) print i; }; }'`; \ + if test -z "$(ETAGS_ARGS)$$tags$$unique"; then :; else \ + test -n "$$unique" || unique=$$empty_fix; \ + $(ETAGS) $(ETAGSFLAGS) $(AM_ETAGSFLAGS) $(ETAGS_ARGS) \ + $$tags $$unique; \ + fi +ctags: CTAGS +CTAGS: $(HEADERS) $(SOURCES) $(TAGS_DEPENDENCIES) \ + $(TAGS_FILES) $(LISP) + tags=; \ + list='$(SOURCES) $(HEADERS) $(LISP) $(TAGS_FILES)'; \ + unique=`for i in $$list; do \ + if test -f "$$i"; then echo $$i; else echo $(srcdir)/$$i; fi; \ + done | \ + $(AWK) '{ files[$$0] = 1; nonempty = 1; } \ + END { if (nonempty) { for (i in files) print i; }; }'`; \ + test -z "$(CTAGS_ARGS)$$tags$$unique" \ + || $(CTAGS) $(CTAGSFLAGS) $(AM_CTAGSFLAGS) $(CTAGS_ARGS) \ + $$tags $$unique + +GTAGS: + here=`$(am__cd) $(top_builddir) && pwd` \ + && cd $(top_srcdir) \ + && gtags -i $(GTAGS_ARGS) $$here + +distclean-tags: + -rm -f TAGS ID GTAGS GRTAGS GSYMS GPATH tags + +distdir: $(DISTFILES) + @srcdirstrip=`echo "$(srcdir)" | sed 's/[].[^$$\\*]/\\\\&/g'`; \ + topsrcdirstrip=`echo "$(top_srcdir)" | sed 's/[].[^$$\\*]/\\\\&/g'`; \ + list='$(DISTFILES)'; \ + dist_files=`for file in $$list; do echo $$file; done | \ + sed -e "s|^$$srcdirstrip/||;t" \ + -e "s|^$$topsrcdirstrip/|$(top_builddir)/|;t"`; \ + case $$dist_files in \ + */*) $(MKDIR_P) `echo "$$dist_files" | \ + sed '/\//!d;s|^|$(distdir)/|;s,/[^/]*$$,,' | \ + sort -u` ;; \ + esac; \ + for file in $$dist_files; do \ + if test -f $$file || test -d $$file; then d=.; else d=$(srcdir); fi; \ + if test -d $$d/$$file; then \ + dir=`echo "/$$file" | sed -e 's,/[^/]*$$,,'`; \ + if test -d $(srcdir)/$$file && test $$d != $(srcdir); then \ + cp -pR $(srcdir)/$$file $(distdir)$$dir || exit 1; \ + fi; \ + cp -pR $$d/$$file $(distdir)$$dir || exit 1; \ + else \ + test -f $(distdir)/$$file \ + || cp -p $$d/$$file $(distdir)/$$file \ + || exit 1; \ + fi; \ + done +check-am: all-am +check: check-am +all-am: Makefile $(PROGRAMS) $(HEADERS) +installdirs: + for dir in "$(DESTDIR)$(bindir)"; do \ + test -z "$$dir" || $(MKDIR_P) "$$dir"; \ + done +install: install-am +install-exec: install-exec-am +install-data: install-data-am +uninstall: uninstall-am + +install-am: all-am + @$(MAKE) $(AM_MAKEFLAGS) install-exec-am install-data-am + +installcheck: installcheck-am +install-strip: + $(MAKE) $(AM_MAKEFLAGS) INSTALL_PROGRAM="$(INSTALL_STRIP_PROGRAM)" \ + install_sh_PROGRAM="$(INSTALL_STRIP_PROGRAM)" INSTALL_STRIP_FLAG=-s \ + `test -z '$(STRIP)' || \ + echo "INSTALL_PROGRAM_ENV=STRIPPROG='$(STRIP)'"` install +mostlyclean-generic: + +clean-generic: + +distclean-generic: + -test -z "$(CONFIG_CLEAN_FILES)" || rm -f $(CONFIG_CLEAN_FILES) + +maintainer-clean-generic: + @echo "This command is intended for maintainers to use" + @echo "it deletes files that may require special tools to rebuild." +clean: clean-am + +clean-am: clean-binPROGRAMS clean-generic mostlyclean-am + +distclean: distclean-am + -rm -rf ./$(DEPDIR) + -rm -f Makefile +distclean-am: clean-am distclean-compile distclean-generic \ + distclean-tags + +dvi: dvi-am + +dvi-am: + +html: html-am + +info: info-am + +info-am: + +install-data-am: + +install-dvi: install-dvi-am + +install-exec-am: install-binPROGRAMS + +install-html: install-html-am + +install-info: install-info-am + +install-man: + +install-pdf: install-pdf-am + +install-ps: install-ps-am + +installcheck-am: + +maintainer-clean: maintainer-clean-am + -rm -rf ./$(DEPDIR) + -rm -f Makefile +maintainer-clean-am: distclean-am maintainer-clean-generic + +mostlyclean: mostlyclean-am + +mostlyclean-am: mostlyclean-compile mostlyclean-generic + +pdf: pdf-am + +pdf-am: + +ps: ps-am + +ps-am: + +uninstall-am: uninstall-binPROGRAMS + +.MAKE: install-am install-strip + +.PHONY: CTAGS GTAGS all all-am check check-am clean clean-binPROGRAMS \ + clean-generic ctags distclean distclean-compile \ + distclean-generic distclean-tags distdir dvi dvi-am html \ + html-am info info-am install install-am install-binPROGRAMS \ + install-data install-data-am install-dvi install-dvi-am \ + install-exec install-exec-am install-html install-html-am \ + install-info install-info-am install-man install-pdf \ + install-pdf-am install-ps install-ps-am install-strip \ + installcheck installcheck-am installdirs maintainer-clean \ + maintainer-clean-generic mostlyclean mostlyclean-compile \ + mostlyclean-generic pdf pdf-am ps ps-am tags uninstall \ + uninstall-am uninstall-binPROGRAMS + +# Tell versions [3.59,3.63) of GNU make to not export all variables. +# Otherwise a system limit (for SysV at least) may be exceeded. +.NOEXPORT: diff --git a/src/cbuffer.c b/src/cbuffer.c new file mode 100644 index 0000000..1242496 --- /dev/null +++ b/src/cbuffer.c @@ -0,0 +1,107 @@ +/* + * Venus GPS Server + * + * Copyright (C) 2010 by Multi-Tech Systems + * + * Author: James Maki + * + * 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 "log.h" +#include "cbuffer.h" +#include "utils.h" +#include + +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)) { + len = full_write(fd, buf->buffer + i, buffer_size(buf) - i); + if(len <= 0) { + log_error("full_write failed errno: %d", errno); + return len; + } + + total += len; + count -= len; + + if(len < buffer_size(buf) - i) { + buf->read_index = i + len; + return total; + } + + i = 0; + } + + len = full_write(fd, buf->buffer + i, count); + if(len <= 0) { + if(total) { + return total; + } else { + log_error("full_write failed errno: %d", errno); + return len; + } + } + buf->read_index = i + len; + total += len; + + return total; +} + +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)) { + len = safe_read(fd, buf->buffer + i, buffer_size(buf) - i); + if(len <= 0) { + log_warning("safe_read failed errno: %d", errno); + return len; + } + + total += len; + count -= len; + + if(len < buffer_size(buf) - i) { + buf->write_index = i + len; + return total; + } + + i = 0; + } + + len = safe_read(fd, buf->buffer + i, count); + if(len <= 0) { + if(total) { + return total; + } else { + log_warning("safe_read failed errno: %d", errno); + return len; + } + } + buf->write_index = i + len; + total += len; + + return total; +} + diff --git a/src/cbuffer.h b/src/cbuffer.h new file mode 100644 index 0000000..5fc2de0 --- /dev/null +++ b/src/cbuffer.h @@ -0,0 +1,22 @@ +#include + +#ifndef __CBUFFER_H +#define __CBUFFER_H + +struct cbuffer { + uint8_t buffer[1 << 10]; + int read_index; + int write_index; +}; + +#define buffer_size(buf) (sizeof((buf)->buffer)) +#define buffer_used(buf) (((buf)->write_index - (buf)->read_index) & (buffer_size(buf) - 1)) +#define buffer_free(buf) (buffer_size(buf) - 1 - buffer_used(buf)) + +int buffered_write(int fd, struct cbuffer *buf); +int buffered_read(int fd, struct cbuffer *buf); + +#define DIR_REV(i) ((i == 1) ? 0 : 1) + +#endif /* ~__CBUFFER_H */ + diff --git a/src/gps_test.rb b/src/gps_test.rb new file mode 100755 index 0000000..4f35ec5 --- /dev/null +++ b/src/gps_test.rb @@ -0,0 +1,43 @@ +#!/usr/bin/env ruby1.8 + +require 'socket' + +GPS_SERVER_IP = "192.168.2.1" +GPS_SERVER_PORT = 5445 +MODE = :client +PROTOCOL = :tcp + +def tcp_read(sock) + loop { + data = sock.readline() + puts data.inspect + } +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) + + loop { + begin + msg, sender = sock.recvfrom(1024) + rescue Errno::EAGAIN, Errno::EWOULDBLOCK => e + puts "recvfrom error: #{e.class}: #{e}" + next + end + puts msg.inspect + } +end + + diff --git a/src/log.h b/src/log.h new file mode 100644 index 0000000..5f0eceb --- /dev/null +++ b/src/log.h @@ -0,0 +1,32 @@ +#ifndef __LOG_H +#define __LOG_H + +#include "config.h" + +#if CONFIG_USE_SYSLOG +# include +# define LOG_FACILITY LOG_USER + +# define __log(level, name, format, args...) \ + syslog(LOG_FACILITY | level, "[" name "] %s:%s:%d: " format "\n" , \ + __FILE__ , __func__ , __LINE__ , ## args) +#else +# define __log(level, name, format, args...) \ + fprintf(stderr, "[" name "] %s:%s:%d: " format "\n" , \ + __FILE__ , __func__ , __LINE__ , ## args) +#endif + +#define log_emerg(format, args...) __log(LOG_EMERG, "EMERG", format , ## args) +#define log_alert(format, args...) __log(LOG_ALERT, "ALERT", format , ## args) +#define log_crit(format, args...) __log(LOG_CRIT, "CRIT", format , ## args) +#define log_error(format, args...) __log(LOG_ERR, "ERROR", format , ## args) +#define log_warning(format, args...) __log(LOG_WARNING, "WARNING", format , ## args) +#define log_notice(format, args...) __log(LOG_NOTICE, "NOTICE", format , ## args) +#define log_info(format, args...) __log(LOG_INFO, "INFO", format , ## args) +#if DEBUG +# define log_debug(format, args...) __log(LOG_DEBUG, "DEBUG", format , ## args) +#else +# define log_debug(format, args...) do {} while (0) +#endif + +#endif /* ~__LOG_H */ diff --git a/src/utils.c b/src/utils.c new file mode 100644 index 0000000..6f98ab0 --- /dev/null +++ b/src/utils.c @@ -0,0 +1,217 @@ +/* + * Venus GPS Server + * + * Copyright (C) 2010 by Multi-Tech Systems + * Copyright (C) 2004-2009 by James Maki + * + * Author: James Maki + * + * safe_read, safe_write, full_write are from Busybox + * + * 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 + * + */ +#define _GNU_SOURCE + +#include +#include +#include + +#include "log.h" +#include "utils.h" + +int systemf(const char *fmt, ...) { + int err; + va_list ap; + char *buf; + + va_start(ap, fmt); + err = vasprintf(&buf, fmt, ap); + va_end(ap); + if(err == -1) { + log_error("out of memory"); + return -1; + } + + log_debug("%s", buf); + + err = system(buf); + free(buf); + + return err; +} + +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); + + return n; +} + +ssize_t safe_readn(int fd, void *buf, size_t len) { + ssize_t cc; + ssize_t total; + + total = 0; + + while(len) { + cc = safe_read(fd, buf, len); + + if(cc < 0) { + if(total) { + return total; + } + return cc; + } + + total += cc; + buf = buf + cc; + len -= cc; + } + + return total; +} + +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); + + return n; +} + +ssize_t full_write(int fd, const void *buf, size_t len) { + ssize_t cc; + ssize_t total; + + total = 0; + + while(len) { + cc = safe_write(fd, buf, len); + + if(cc < 0) { + if(total) { + return total; + } + return cc; + } + + total += cc; + buf = ((const char *)buf) + cc; + len -= cc; + } + + return total; +} + +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); + 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); + return err; + } + + return err; +} + +int host_to_inet(const char *host, struct in_addr *in) { + 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) { + log_error("gethostbyname_r: %d", herrno); + return -1; + } + + memcpy(&in->s_addr, h.h_addr_list[0], 4); + + return 0; +} + +int inet_conn_str(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) { + log_error("gethostbyname_r: %d", herrno); + return -1; + } + + int sd = socket(PF_INET, type, 0); + if(sd == -1) { + log_error("socket: %d", errno); + return -1; + } + + struct sockaddr_in serv_addr; + 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); + close(sd); + return -1; + } + + return sd; +} + +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); + return -1; + } + + struct sockaddr_in serv_addr; + memset(&serv_addr, 0, sizeof(serv_addr)); + 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); + close(sd); + return -1; + } + + return sd; +} + diff --git a/src/utils.h b/src/utils.h new file mode 100644 index 0000000..71d3e9c --- /dev/null +++ b/src/utils.h @@ -0,0 +1,27 @@ + +#define _GNU_SOURCE + +#include +#include +#include +#include +#include +#include + +#ifndef __UTILS_H +#define __UTILS_H + +#define match(a, b) (!strcmp(a, b)) +#define matchn(a, b, n) (!strncmp(a, b, n)) + +int host_to_inet(const char *host, struct in_addr *in); +int systemf(const char *fmt, ...); +ssize_t safe_read(int fd, void *buf, size_t count); +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_ia(struct in_addr *host, int port, int type); + +#endif /* ~__UTILS_H */ diff --git a/src/venus_api.c b/src/venus_api.c new file mode 100644 index 0000000..52a4a96 --- /dev/null +++ b/src/venus_api.c @@ -0,0 +1,487 @@ +/* + * SkyTraq Venus 5 GPS API + * + * Copyright (C) 2010 by Multi-Tech Systems + * + * Author: James Maki + * + * 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 + * + */ +#define _GNU_SOURCE + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "log.h" +#include "cbuffer.h" +#include "utils.h" +#include "venus_api.h" + +static const struct baud_map __baud_map[] = { + {B4800, 4800}, + {B9600, 9600}, + {B19200, 19200}, + {B38400, 38400}, + {B57600, 57600}, + {B115200, 115200}, +}; + +static const struct baud_map __venus_baud_map[] = { + {B4800, VENUS_4800}, + {B9600, VENUS_9600}, + {B19200, VENUS_19200}, + {B38400, VENUS_38400}, + {B57600, VENUS_57600}, + {B115200, VENUS_115200}, +}; + +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) { + return __baud_map[i].baud; + } + } + + log_warning("baud rate not valid: %lu", (unsigned long) value); + + return (speed_t) -1; +} + +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) { + return __venus_baud_map[i].value; + } + } + + log_warning("baud rate not valid: %lu", (unsigned long) baud); + + return (speed_t) -1; +} + +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) { + return; + } + + if(msg->data) { + free(msg->data); + } + msg->data = NULL; +} + +uint8_t checksum(void *data, uint16_t len) { + int i; + uint8_t cs = 0; + + for(i = 0; i < len; i++) { + cs ^= ((char *) data)[i]; + } + + return cs; +} + +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); + } + 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); + } + 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); + } + 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); + } + 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); + } + + return 0; +} + +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) { + safe_readn(fd, seq, 2); + //log_error("start seq %c %c", seq[0], seq[1]); + if(!memcmp(seq, seq_start, 2)) { + break; + } + attempts++; + if(attempts > 1024) { + log_error("max attempts reached"); + return -1; + } + } + + safe_readn(fd, &len, 2); + len = __be16_to_cpu(len); + data = malloc(len); + safe_readn(fd, data, len); + safe_readn(fd, &cs, 1); + if(checksum(data, len) != cs) { + log_error("checksum mismatch"); + return -1; + } + safe_readn(fd, seq, 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) { + log_debug("read ACK ID_NONE"); + free(data); + goto again; + } + + msg->data = data; + msg->len = len; + +#if DEBUG + { + 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) { + int err; + char c; + ssize_t total = 0; + char *cp = buf; + + int start = 0; + + while(1) { + err = safe_read(fd, &c, 1); + if(err <= 0) { + log_error("read from gps failed: %d", errno); + return -1; + } + if(!start) { + if(c != '$') { + continue; + } + start = 1; + } + + if(total < count) { + *cp++ = c; + total++; + } + + if(c == '\n') { + break; + } + } + + return total; +} + +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]; + struct venus_msg msg; + + struct tm broken; + struct tm *tmp; + + tmp = gmtime_r(&utc, &broken); + if (tmp == NULL) { + log_error("gmtime_r: %d", errno); + return -1; + } + + data[0] = ID_SYSTEM_RESTART; + data[1] = mode; + + uint16_t year = __cpu_to_le16(((uint16_t) broken.tm_year) + 1900); + memcpy(data + 2, &year, 2); + data[4] = (uint8_t) (broken.tm_mon + 1); + data[5] = (uint8_t) broken.tm_mday; + data[6] = (uint8_t) broken.tm_hour; + data[7] = (uint8_t) broken.tm_min; + data[8] = (uint8_t) broken.tm_sec; + + latitude = (int16_t) __cpu_to_le16((uint16_t) latitude); + memcpy(data + 9, &latitude, 2); + + longitude = (int16_t) __cpu_to_le16((uint16_t) longitude); + memcpy(data + 11, &longitude, 2); + + altitude = (int16_t) __cpu_to_le16((uint16_t) altitude); + memcpy(data + 13, &altitude, 2); + + msg.data = data; + msg.len = sizeof(data); + err = venus_write_msg(fd, &msg); + if(err < 0) { + log_error("venus_write_msg: %d", err); + return -1; + } + + err = venus_read_msg(fd, &msg); + if(err < 0) { + log_error("venus_read_msg: %d", err); + return -1; + } + free_venus_msg_data(&msg); + + return 0; +} + +int venus_factory_defaults(int fd, uint8_t type) { + int err; + uint8_t data[2]; + struct venus_msg msg; + + data[0] = ID_FACTORY_DEFAULTS; + data[1] = type; + + msg.data = data; + msg.len = sizeof(data); + err = venus_write_msg(fd, &msg); + if(err < 0) { + log_error("venus_write_msg: %d", err); + return -1; + } + + err = venus_read_msg(fd, &msg); + if(err < 0) { + log_error("venus_read_msg: %d", err); + return -1; + } + free_venus_msg_data(&msg); + + return 0; +} + +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; + + data[0] = ID_CONF_SERIAL; + data[1] = com; + data[2] = baud; + data[3] = attr; + + msg.data = data; + msg.len = sizeof(data); + err = venus_write_msg(fd, &msg); + if(err < 0) { + log_error("venus_write_msg: %d", err); + return -1; + } + + err = venus_read_msg(fd, &msg); + if(err < 0) { + log_error("venus_read_msg: %d", err); + return -1; + } + free_venus_msg_data(&msg); + + return 0; +} + +int venus_conf_format(int fd, uint8_t type, uint8_t attr) { + int err; + uint8_t data[3]; + struct venus_msg msg; + + data[0] = ID_CONF_FORMAT; + data[1] = type; + data[2] = attr; + + msg.data = data; + msg.len = sizeof(data); + err = venus_write_msg(fd, &msg); + if(err < 0) { + log_error("venus_write_msg: %d", err); + return -1; + } + + err = venus_read_msg(fd, &msg); + if(err < 0) { + log_error("venus_read_msg: %d", err); + return -1; + } + free_venus_msg_data(&msg); + + return 0; +} + +int venus_query_sw_version(int fd, uint8_t type) { + int err; + uint8_t data[2]; + struct venus_msg msg; + + data[0] = ID_QUERY_SW_VERSION; + data[1] = type; + + msg.data = data; + msg.len = sizeof(data); + err = venus_write_msg(fd, &msg); + if(err < 0) { + log_error("venus_write_msg: %d", err); + return -1; + } + + err = venus_read_msg(fd, &msg); + 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) { + log_error("venus_read_msg: %d", err); + return -1; + } + free_venus_msg_data(&msg); + + 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 err; + uint8_t data[9]; + struct venus_msg msg; + + data[0] = ID_CONF_NMEA; + data[1] = gga; + data[2] = gsa; + data[3] = gsv; + data[4] = gll; + data[5] = rmc; + data[6] = vtg; + data[7] = zda; + data[8] = attr; + + msg.data = data; + msg.len = sizeof(data); + err = venus_write_msg(fd, &msg); + if(err < 0) { + log_error("venus_write_msg: %d", err); + return -1; + } + + err = venus_read_msg(fd, &msg); + if(err < 0) { + log_error("venus_read_msg: %d", err); + return -1; + } + free_venus_msg_data(&msg); + + return 0; +} + +int venus_tty_configure(int fd, speed_t baud_rate) { + struct termios tio; + tcgetattr(fd, &tio); + cfmakeraw(&tio); + cfsetspeed(&tio, baud_rate); + tcsetattr(fd, TCSANOW, &tio); + + return 0; +} + +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); + exit(1); + } + + venus_tty_configure(tty, baud_rate); + tcflush(tty, TCIOFLUSH); + + return tty; +} + + + + diff --git a/src/venus_api.h b/src/venus_api.h new file mode 100644 index 0000000..75f189f --- /dev/null +++ b/src/venus_api.h @@ -0,0 +1,98 @@ + +#include + +#ifndef __VENUS_API_H +#define __VENUS_API_H + +#define NMEA_SENTENCE_MAX 82 + +enum venus_message_id { + ID_NONE = 0x00, + ID_SYSTEM_RESTART = 0x01, + ID_QUERY_SW_VERSION = 0x02, + ID_QUERY_SW_CRC = 0x03, + ID_FACTORY_DEFAULTS = 0x04, + ID_CONF_SERIAL = 0x05, + ID_CONF_NMEA = 0x08, + ID_CONF_FORMAT = 0x09, + ID_GET_EPHEMERIS = 0x30, + ID_SET_EPHEMERIS = 0x31, + ID_SW_VERSION = 0x80, + ID_SW_CRC = 0x81, + ID_ACK = 0x83, + ID_NACK = 0x84, + ID_EPHEMERIS_DATA = 0xB1, +}; + +enum venus_message_type { + MSG_TYPE_NONE = 0, + MSG_TYPE_NMEA = 1, + MSG_TYPE_BINARY = 2, +}; + +enum venus_sw_type { + SW_TYPE_RESERVED = 0, + SW_TYPE_SYSTEM = 1, +}; + +enum venus_update_attr { + UPDATE_ATTR_SRAM = 0, + UPDATE_ATTR_SRAM_FLASH = 1, +}; + +enum venus_start_mode { + SYSTEM_RESET_NO_CHANGE = 0, + SYSTEM_RESET_HOT = 1, + SYSTEM_RESET_WARM = 2, + SYSTEM_RESET_COLD = 3, + SYSTEM_RESET_TEST_MODE = 4, +}; + +enum venus_com_port { + VENUS_COM1 = 0, +}; + +enum venus_baud_rate { + VENUS_4800 = 0, + VENUS_9600 = 1, + VENUS_19200 = 2, + VENUS_38400 = 3, + VENUS_57600 = 4, + VENUS_115200 = 5, +}; + +enum venus_factory_defaults_type { + FACTORY_DEFAULTS_RESERVED = 0, + FACTORY_DEFAULTS_REBOOT = 1, +}; + +struct baud_map { + speed_t baud; + speed_t value; +}; + +struct venus_msg { + uint8_t *data; + uint16_t len; +}; + +speed_t value_to_baud(speed_t value); +speed_t baud_to_venus(speed_t baud); +void free_venus_msg_data(struct venus_msg *msg); +uint8_t checksum(void *data, uint16_t len); +int venus_write_msg(int fd, struct venus_msg *msg); +int venus_read_msg(int fd, struct venus_msg *msg); +ssize_t read_nmea_sentence(int fd, void *buf, size_t count); +int venus_system_restart(int fd, uint8_t mode, time_t utc, int16_t latitude, + int16_t longitude, int16_t altitude); +int venus_factory_defaults(int fd, uint8_t type); +int venus_conf_serial(int fd, uint8_t com, uint8_t baud, uint8_t attr); +int venus_conf_format(int fd, uint8_t type, uint8_t attr); +int venus_query_sw_version(int fd, uint8_t type); +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_tty_configure(int fd, speed_t baud_rate); +int venus_open(const char *dev, speed_t baud_rate); + +#endif /* ~__VENUS_API_H */ + diff --git a/src/venus_gps.c b/src/venus_gps.c new file mode 100644 index 0000000..4a02096 --- /dev/null +++ b/src/venus_gps.c @@ -0,0 +1,631 @@ +/* + * Venus GPS Controller Example + * + * Copyright (C) 2010 by Multi-Tech Systems + * + * Author: James Maki + * + * 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 +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#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 (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"); +#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; +} diff --git a/src/venus_gps.h b/src/venus_gps.h new file mode 100644 index 0000000..895d97e --- /dev/null +++ b/src/venus_gps.h @@ -0,0 +1,6 @@ +#ifndef __VENUS_GPS_H +#define __VENUS_GPS_H + +#include "config.h" + +#endif /* ~__VENUS_GPS_H */ -- cgit v1.2.3