#ifdef HAVE_CONFIG_H
#include <config.h>
#endif

#if defined(REFCLOCK) && defined(CLOCK_GARMIN)

/*
 *	NTP clock driver for Garmin GPS handheld receivers (1.4)
 *	Copyright (C) 2005 Bob Lindell (bob@jensar.us)
 *
 *	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.
 */

/*
 *	NTP clock driver for Garmin GPS handheld receivers using the
 *	Garmin proprietary protocol.  It is based on both documented
 *	and undocumented aspects of the Garmin proprietary protocol.
 *
 *	To use this driver, your Garmin GPS handheld receiver must be
 *	configured to use Garmin protocol interface mode, not NMEA.
 *
 *	This driver has been written and tested using a Linux based
 *	system.  To achieve the best quality results, turn off serial
 *	port FIFOs and enable low latency tty driver processing.
 *	In Linux, this is accomplished using the setserial application
 *	(e.g. setserial /dev/gps0 uart 16550 low_latency).  Even better
 *	results are obtained with kernel timestamps on received
 *	characters.  This is what I have been using for most of my tests.
 *
 *	This is a "sloppy" clock but I believe it is possible to recover
 *	sub-millisecond results with a good clock filter.  Without an
 *	accurate reference clock, I am unable to provide more quantitative
 *	performance results at this time.
 *
 *	I suspect that earlier versions of firmware may not return the
 *	undisciplined oscillator count as part of a date/time response.
 *	This information is needed to recover the higher accuracy results.
 *	Without this undocumented field, the driver will only achieve
 *	sub-second accuracy. I welcome technical support from the
 *	manufacturer to achieve universal support for the entire range
 *	of models and firmware versions.
 *
 *	This driver has been testing with the following units.
 *	I need feedback from users to populate this list!  Check for
 *	updates to this list, driver, and other related software
 *	at http://www.jensar.us/~bob/garmin/.
 *
 *		* Garmin GPS 12 firmware version 4.54
 *		* ??
 */

/*#define	TIMESTAMPS*/

#include <stdio.h>
#include <ctype.h>
#include <sys/time.h>

#include "ntpd.h"
#include "ntp_io.h"
#include "ntp_refclock.h"
#include "ntp_stdlib.h"
#ifdef	TIMESTAMPS
#include "ntp_unixtime.h"
#endif	/* TIMESTAMPS */

/*
 *	One day, POSIX types will be a given.
 */

typedef unsigned char uint8_x;
typedef short int16_x;
typedef unsigned short uint16_x;
typedef u_int32 uint32_x;

/*
 *	Generic functions
 */

#define	SIZEOF(x)		(sizeof(x) / sizeof((x)[0]))

/*
 *	Time constants
 */

#define	NSEC_PER_SEC		1000000000L
#define	SEC_PER_MIN		60
#define	MIN_PER_HOUR		60
#define	HOUR_PER_DAY		24
#define	SEC_PER_DAY		(SEC_PER_MIN * MIN_PER_HOUR * HOUR_PER_DAY)

/*
 *	The Garmin proprietary protocol is partially documented
 *	by the manufacturer.  The document, "Garmin GPS Interface
 *	Specification", dated December, 1999, is available at
 *	http://www.garmin.com/support/pdf/iop_spec.pdf.  A good reference
 *	for some of the undocumented packet formats can be found at
 *	http://artico.lma.fi.upm.es/numerico/miembros/antonio/async/.
 */

/*
 *	Packet IDs at the Garmin protocol link layer.
 */

#define	PID_ACK_BYTE		6
#define	PID_COMMAND_DATA	10
#define	PID_DATE_TIME_DATA	14
#define	PID_UNDOC_PVT		20
#define	PID_NAK_BYTE		21
#define	PID_UNDOC_ASYNC		28
#define	PID_PVT_DATA		51

/*
 *	Packet commands at the Garmin protocol application layer.
 */

#define	CMND_TRANSFER_TIME	5

/*
 *	Garmin data structure constants
 */

#define	FIX_2D			2		/* At least a 2D fix */
#define	CLK_SHIFT		9		/* LSB of oscillator */
#define	CLK_MASK		((1 << CLK_SHIFT) - 1)

/*
 *	Garmin packet functions
 */

#define	PKT_MAX			(2 + 2 * 257 + 2)
#define	PKT_PID(x)		((x)[0])
#define	PKT_SIZE(x)		((x)[1])

#if	(defined(XNTP_BIG_ENDIAN)  || defined(__alpha__))
#define	GTOH16(buf,i)		(((buf)[(i) + 1] << 8) | (buf)[i])
#define	GTOH32(buf,i)		((GTOH16(buf,(i) + 2) << 16) | GTOH16(buf,i))
#define	GTOHD(buf,i)		gtohd(buf,i)
static
double
gtohd(uint8_x *buf,int i) {
#ifdef	__alpha__
	double x;

	return(*((double *) memcpy(&x,&buf[i],sizeof(x))));
#else	/* __alpha__ */
	union {
		uint32_x l[2];
		double d;
	} x;

	x.l[1] = GTOH32(buf,i);
	x.l[0] = GTOH32(buf,i + 4);
	return(x.d);
#endif	/* __alpha__ */
}
#else	/* (defined(XNTP_BIG_ENDIAN)  || defined(__alpha__)) */
#define	GTOH16(buf,i)		(*((uint16_x *) &(buf)[i]))
#define	GTOH32(buf,i)		(*((uint32_x *) &(buf)[i]))
#define	GTOHD(buf,i)		(*((double *) &(buf)[i]))
#endif	/* (defined(XNTP_BIG_ENDIAN)  || defined(__alpha__)) */

/*
 *	NTP daemon definitions
 */

#ifdef SYS_WINNT
#define	DEVICE		"COM%d:"		/* COM 1 - 3 supported */
#else
#define	DEVICE		"/dev/gps%d"		/* name of radio device */
#endif
#define	SPEED232	B9600			/* UART speed (9600 bps) */
#define	PRECISION	(-10)			/* precision (about 1 ms) */
#define	REFID		"GPS\0"			/* reference id */
#define	DESCRIPTION	"Garmin GPS Clock"	/* who we are */

#define	BUF_SIZE		1024	/* ring buffer size */

/*
 *	NTP daemon unit control structure
 */

struct unit {
	uint8_x buffer[BUF_SIZE];	/* received data */
	l_fp ts[BUF_SIZE];		/* time of byte arrival */
	uint16_x rp;			/* read pointer */
	uint16_x wp;			/* write pointer */
	double period;			/* period of counter (nsecs) */
	double tow0;			/* previous value of GPS tow */
	uint32_x counter0;		/* previous value of counter */
	int16_x leap;			/* GPS leap second offset to UTC */
	int high;			/* High accuracy mode enabled */
#ifdef	TIMESTAMPS
	uint8_x tcnt;			/* byte count for timeval data */
	struct timeval tv;		/* time of byte arrival */
#endif	/* TIMESTAMPS */
};

/*
 *	Function prototypes
 */

typedef void (*callback)(struct recvbuf *,uint8_x *,l_fp *);

static	int	gstart		P((int,struct peer *));
static	void	gshutdown	P((int,struct peer *));
static	void	greceive	P((struct recvbuf *));
static	void	gpoll		P((int,struct peer *));
static	int	gwrite		P((int,uint8_x *));
static	void	gread		P((struct recvbuf *,callback));
static	void	process		P((struct recvbuf *,uint8_x *,l_fp *));

/*
 *	NTP daemon transfer vector
 */

struct	refclock refclock_garmin = {
	gstart,			/* start up driver */
	gshutdown,		/* shut down driver */
	gpoll,			/* transmit poll message */
	noentry,		/* handle control */
	noentry,		/* initialize driver */
	noentry,		/* buginfo */
	NOFLAGS			/* not used */
};

/* Start async message generation of PID_PVT_DATA and PID_UNDOC_PVT */
static uint8_x async[] = { PID_UNDOC_ASYNC, 2, 4, 3 };

/* #define	GDEBUG */
#ifdef	GDEBUG
FILE *fp; 
#endif	/* GDEBUG */

/*
 *	start - open the devices and initialize data for
 *	processing.
 */

static
int
gstart(int unit,struct peer *peer)
{
	struct unit *up;
	struct refclockproc *pp;
	char device[sizeof(DEVICE) + 1];

	/*
	 * Allocate and initialize structures
	 */
	up = (struct unit *) emalloc(sizeof(*up));
	if (up == NULL)
		return (FALSE);
	memset((char *)up,0,sizeof(*up));
/*
	up->high = TRUE;
*/
	pp = peer->procptr;
	pp->unitptr = (caddr_t)up;
	pp->io.srcclock = (caddr_t)peer;
	pp->io.clock_recv = greceive;
	pp->io.datalen = 0;
	peer->precision = PRECISION;
	pp->clockdesc = DESCRIPTION;
	memcpy((char *)&pp->refid,REFID,4);
	(void)sprintf(device,DEVICE,unit);
	pp->io.fd = refclock_open(device,SPEED232,LDISC_RAW);
	if (pp->io.fd <= 0) {
		free(up);
		pp->unitptr = NULL;
		return (FALSE);
	}
	if (!io_addclock(&pp->io)) {
		(void) close(pp->io.fd);
		free(up);
		pp->unitptr = NULL;
		return (FALSE);
	}
	gwrite(pp->io.fd,async);
#ifdef	GDEBUG
	fp = fopen("/tmp/ntpd.log","w+");
#endif	/* GDEBUG */
	return (TRUE);
}

/*
 *	shutdown - shut down the clock.
 */

static
void
gshutdown(int unit,struct peer *peer)
{
	struct unit *up;
	struct refclockproc *pp;
	/* Stop async message generation */
	static uint8_x stop[] = { PID_UNDOC_ASYNC, 2, 0, 0 };

	pp = peer->procptr;
	up = (struct unit *)pp->unitptr;
	gwrite(pp->io.fd,stop);
	io_closeclock(&pp->io);
	free(up);
	pp->unitptr = NULL;
#ifdef	GDEBUG
	fclose(fp);
#endif	/* GDEBUG */
}

/*
 *	receive - receive data from the clock.
 */

static
void
greceive(struct recvbuf *rbufp)
{
	gread(rbufp,process);
}

/*
 *	poll - called by the transmit procedure.
 */

static
void
gpoll(int unit,struct peer *peer)
{
	struct refclockproc *pp;
	struct unit *up;

	pp = peer->procptr;
	up = (struct unit *)pp->unitptr;
	pp->polls++;
	/*
	 *	Reexamine the GPS leap offset every poll period.
	 *	Also support hot disconnects and reconnects of the GPS
	 *	clock.  We don't want to deprive the user from going on
	 *	an enjoyable hike with their receiver.
	 */
	gwrite(pp->io.fd,async);
#ifdef	GDEBUG
	fprintf(fp,"poll recv=%d proc=%d\n",pp->coderecv,pp->codeproc);
	fflush(fp);
#endif	/* GDEBUG */
	if (pp->coderecv == pp->codeproc) {
		refclock_report(peer,CEVNT_TIMEOUT);
		up->rp = up->wp;
#ifdef	TIMESTAMPS
		up->tcnt = 0;
#endif	/* TIMESTAMPS */
		return;
	}
#ifdef	GDEBUG
		fprintf(fp,"receive %s\n",pp->a_lastcode);
		fflush(fp);
#endif	/* GDEBUG */
	record_clock_stats(&peer->srcadr,pp->a_lastcode);
	pp->lastref = pp->lastrec;
	refclock_receive(peer);
	refclock_report(peer,CEVNT_NOMINAL);
}


/*
 *	Process packets of the Garmin protocol.  The Garmin specification
 *	refers to this as the application layer processing.
 */

static
void
process(struct recvbuf *rbufp,uint8_x *buf,l_fp *t)
{
	uint32_x counter;
	struct unit *up;
	struct refclockproc *pp;
	struct peer *peer;
	double tow;
	/* Poll for date and time */
	static uint8_x date[] = { PID_COMMAND_DATA, 2, CMND_TRANSFER_TIME, 0 };
	/* Start async message generation of PID_UNDOC_PVT only */
	static uint8_x simplepvt[] = { PID_UNDOC_ASYNC, 2, 4, 0 };

	peer = (struct peer *)rbufp->recv_srcclock;
	pp = peer->procptr;
	up = (struct unit *)pp->unitptr;
	if (PKT_PID(buf) == PID_UNDOC_PVT) {
		/*
		 *	Every second we get the current count of the
		 *	undisciplined oscillator at the beginning of
		 *	the second.  The nominal frequency is 0.5115
		 *	Mhz which is half of 1.023 Mhz GPS data rate. We
		 *	compute the period of this oscillator by measuring
		 *	it against the computed GPS time of week (tow)
		 *	field.  The tow field is actually reported in two
		 *	parts, a value which drifts with the oscillator
		 *	and a correction factor.
		 */
		if (GTOH16(buf,2) < FIX_2D)
			return;
		gwrite(pp->io.fd,date);	/* Ask for the data and time */
		if (!up->high)
			return;
		counter = (GTOH32(buf,22) << CLK_SHIFT)
			| (GTOH32(buf,26) & CLK_MASK);
		tow = GTOHD(buf,6) - GTOHD(buf,70);
		up->period = NSEC_PER_SEC * (tow - up->tow0)
			/ (counter - up->counter0);
		up->counter0 = counter;
		up->tow0 = tow;
		return;
	}
	if (PKT_PID(buf) == PID_PVT_DATA) {
		/*
		 *	Every second we get a position, velocity,
		 *	and time (PVT) solution of the GPS receiver.
		 *	This gives the GPS leap second offset to UTC.
		 *	The only reason we need this message is to
		 *	correct the date and time packets.
		 */
		if (GTOH16(buf,18) < FIX_2D)
			return;
		up->leap = GTOH16(buf,60);
		if (up->high)
			/* turn off PID_PVT_DATA */
			gwrite(pp->io.fd,simplepvt);
		else
			up->tow0 = GTOHD(buf,20);
		return;
	}
	if (PKT_PID(buf) == PID_DATE_TIME_DATA) {
		/*
		 *	The Garmin documentation says we get back
		 *	8 bytes including the data and time to the
		 *	nearest second.  Some versions of the firmware
		 *	throw in an additional 4 bytes containing the
		 *	upper bits of the undisciplined oscillator count.
		 */
		pp->lastrec = *t;
		pp->year = GTOH16(buf,4);
		pp->day = ymd2yd(pp->year,buf[2],buf[3]);
		pp->hour = GTOH16(buf,6);
		pp->minute = buf[8];
		pp->second = buf[9];
		pp->leap = LEAP_NOWARNING;
		pp->lencode = sprintf(pp->a_lastcode,
			"%.02d/%.02d/%d %.02d:%.02d:%.02d",buf[2],buf[3],
			pp->year,pp->hour,pp->minute,pp->second);
#ifdef	GDEBUG
		fprintf(fp,"%s\n",pp->a_lastcode);
		fflush(fp);
#endif	/* GDEBUG */
		/*
		 *	The time, which is reported to the nearest
		 *	second, seems to drift relative to real time by
		 *	a substantial fraction of a second.  This means
		 *	that we may accidently report that we are in
		 *	the previous or next second.  Use the GPS tow
		 *	corrected to UTC instead.
		 */
		pp->hour = 0;
		pp->minute = 0;
		pp->nsec = NSEC_PER_SEC * modf(up->tow0,&tow);
		pp->second = tow;
		pp->second = (pp->second + SEC_PER_DAY - up->leap)
			% SEC_PER_DAY;
		/*
		 *	Compute the offset into the current second
		 *	by multiplying the elapsed count since the
		 *	beginning of the second with the estimated period
		 *	of the counter.  The response is missing the
		 *	least significant bits of the oscillator count.
		 *	This does not reduce the accuracy of the result
		 *	since device responses seem to occur at some
		 *	large multiple of the oscillator frequency.
		 */
		if (PKT_SIZE(buf) >= 12) {
			counter = GTOH32(buf,10) << CLK_SHIFT;
			pp->nsec += up->period * (counter - up->counter0);
			while (pp->nsec >= NSEC_PER_SEC) {
				pp->nsec -= NSEC_PER_SEC;
				pp->second++;
			}
			up->high = TRUE;
		}
		else
			up->high = FALSE;
#ifdef	GDEBUG
		fprintf(fp,"sec = %d,nsec =%ld\n",pp->second,pp->nsec);
		fflush(fp);
#endif	/* GDEBUG */
		if (!refclock_process(pp))
			refclock_report(peer,CEVNT_BADTIME);
		return;
	}
}

/*
 *	An implementation of the Garmin link layer protocol.  As defined
 *	in the specification, all packet ACKs and NACKs are done only
 *	at the link layer.
 */

#define	DLE		16
#define	ETX		3

#define	GETBYTE(c,up,rp)	{ \
	if (rp != up->wp) { \
		(c) = up->buffer[rp++]; \
		rp = rp % SIZEOF(up->buffer); \
	} \
	else \
		return(0); \
}

#define	GETBYTE_ESCAPED(c,up,rp)	{ \
	GETBYTE(c,up,rp); \
	if (c == DLE) \
		GETBYTE(c,up,rp); \
}

#define	PUTBYTE(c,buf,len)	{ \
	buf[len++] = c; \
}

#define	PUTBYTE_ESCAPED(c,buf,len)	{ \
	PUTBYTE(c,buf,len); \
	if (c == DLE) \
		PUTBYTE(c,buf,len); \
}

/*
 *	Write a packet to the GPS receiver.  Add the proper header,
 *	trailer, and checksum.  Perform DLE escaping as defined in the
 *	specification.
 */

static
int
gwrite(int fd,uint8_x *buf)
{
	uint8_x c,sum,packet[PKT_MAX];
	uint16_x i,n,len;

	len = 0;
	PUTBYTE(DLE,packet,len);
	sum = PKT_PID(buf);
	PUTBYTE(sum,packet,len);
	n = PKT_SIZE(buf) + 2;
	for (i = 1;i < n; i++) {
		c = buf[i];
		PUTBYTE_ESCAPED(c,packet,len);
		sum += c;
	}
	sum = 256 - sum;
	PUTBYTE_ESCAPED(sum,packet,len);
	PUTBYTE(DLE,packet,len);
	PUTBYTE(ETX,packet,len);
	return(write(fd,packet,len) == len);
}

/*
 *	Attempt to parse whole packets.  If successful, ACK the packet if
 *	necessary and return true.  Bad packets are dropped and get NACKed
 *	if the PID is known.  There is a subtle flaw in the protocol here.
 *	A NACK is susposed to return the PID.  But what if the received
 *	PID byte was corrupted during transmission?  We send back the
 *	PID we received, right or wrong.
 */

static
int
parse(int fd,struct unit *up,uint8_x *buf,l_fp *t)
{
	uint8_x c,sum;
	uint16_x i,n,rp;
	static uint8_x response[] = { 0, 2, 0, 0 };
	/* This is partial list of packets that we should not ACK or NACK */
	static uint8_x list[] = { PID_ACK_BYTE, PID_NAK_BYTE, PID_PVT_DATA,
		PID_UNDOC_PVT };

	rp = up->rp;
	do {
		*t = up->ts[rp];
		GETBYTE(sum,up,rp);
		if (sum != DLE)
			continue;
		GETBYTE(sum,up,rp);
		if (sum == ETX)
			continue;
	} while (sum == DLE);
	PKT_PID(buf) = sum;
	GETBYTE_ESCAPED(n,up,rp);
	sum += n;
	PKT_SIZE(buf) = n;
	n += 3;
	for (i = 2;i < n; i++) {
		GETBYTE_ESCAPED(c,up,rp);
		sum += c;
		buf[i] = c;
	}
	GETBYTE(c,up,rp);
	PKT_PID(response) = PID_NAK_BYTE;
	if (c == DLE) {
		GETBYTE(c,up,rp);
		if (c == ETX)
			if (sum == 0)
				PKT_PID(response) = PID_ACK_BYTE;
	}
	response[2] = PKT_PID(buf);
	for (i = 0;i < SIZEOF(list); i++)
		if (list[i] == PKT_PID(buf))
			break;
	if (i == SIZEOF(list))
		gwrite(fd,response);
	up->rp = rp;
	if (PKT_PID(response) == PID_NAK_BYTE)
		return(-1);
	return(1);
}

/*
 *	Collect the bytes as they come in from the serial port.  When
 *	entire packets have been collected, pass them up to be processed.
 *	Bad packets are silently dropped as if they were not received.
 */

static
void
gread(struct recvbuf *rbufp,callback func)
{
	uint8_x c,packet[PKT_MAX];
	uint16_x i;
	l_fp t;
	int overflow;
	struct refclockproc *pp;
	struct peer *peer;
	struct unit *up;

	peer = (struct peer *)rbufp->recv_srcclock;
	pp = peer->procptr;
	up = (struct unit *)pp->unitptr;
	overflow = FALSE;
	for (i = 0;i < rbufp->recv_length; i++) {
		c = rbufp->recv_buffer[i];
#ifdef	TIMESTAMPS
		if (up->tcnt < sizeof(up->tv)) {
			((char *) &up->tv)[up->tcnt++] = c;
			continue;
		}
		up->tcnt = 0;
		up->tv.tv_sec += JAN_1970;
		TVTOTS(&up->tv,&up->ts[up->wp]);
#else	/* TIMESTAMPS */
		up->ts[up->wp] = rbufp->recv_time;
#endif	/* TIMESTAMPS */
		up->buffer[up->wp++] = c;
		up->wp = up->wp % SIZEOF(up->buffer);
		overflow |= (up->wp == up->rp);
	}
	if (overflow)
		up->rp = (up->wp + 1) % SIZEOF(up->buffer);
	while (parse(pp->io.fd,up,packet,&t) > 0)
		(*func)(rbufp,packet,&t);
}

#else
int refclock_garmin_bs;
#endif	/* REFCLOCK */