LibertyBSD_kernel/sys/kern/tty_nmea.c

/*	$OpenBSD: tty_nmea.c,v 1.44 2014/11/03 03:08:00 deraadt Exp $ */

/*
 * Copyright (c) 2006, 2007, 2008 Marc Balmer <mbalmer@openbsd.org>
 *
 * Permission to use, copy, modify, and distribute this software for any
 * purpose with or without fee is hereby granted, provided that the above
 * copyright notice and this permission notice appear in all copies.
 *
 * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
 * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
 * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
 * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
 * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
 * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
 * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
 */

/* A tty line discipline to decode NMEA 0183 data to get the time. */

#include <sys/param.h>
#include <sys/systm.h>
#include <sys/malloc.h>
#include <sys/sensors.h>
#include <sys/tty.h>
#include <sys/conf.h>
#include <sys/time.h>

#ifdef NMEA_DEBUG
#define DPRINTFN(n, x)	do { if (nmeadebug > (n)) printf x; } while (0)
int nmeadebug = 0;
#else
#define DPRINTFN(n, x)
#endif
#define DPRINTF(x)	DPRINTFN(0, x)

int	nmeaopen(dev_t, struct tty *, struct proc *);
int	nmeaclose(struct tty *, int, struct proc *);
int	nmeainput(int, struct tty *);
void	nmeaattach(int);

#define NMEAMAX		82
#define MAXFLDS		32
#ifdef NMEA_DEBUG
#define TRUSTTIME	30
#else
#define TRUSTTIME	(10 * 60)	/* 10 minutes */
#endif

int nmea_count, nmea_nxid;

struct nmea {
	char			cbuf[NMEAMAX];	/* receive buffer */
	struct ksensor		time;		/* the timedelta sensor */
	struct ksensor		signal;		/* signal status */
	struct ksensor		latitude;
	struct ksensor		longitude;
	struct ksensordev	timedev;
	struct timespec		ts;		/* current timestamp */
	struct timespec		lts;		/* timestamp of last '$' seen */
	struct timeout		nmea_tout;	/* invalidate sensor */
	int64_t			gap;		/* gap between two sentences */
#ifdef NMEA_DEBUG
	int			gapno;
#endif
	int64_t			last;		/* last time rcvd */
	int			sync;		/* if 1, waiting for '$' */
	int			pos;		/* position in rcv buffer */
	int			no_pps;		/* no PPS although requested */
	char			mode;		/* GPS mode */
};

/* NMEA decoding */
void	nmea_scan(struct nmea *, struct tty *);
void	nmea_gprmc(struct nmea *, struct tty *, char *fld[], int fldcnt);

/* date and time conversion */
int	nmea_date_to_nano(char *s, int64_t *nano);
int	nmea_time_to_nano(char *s, int64_t *nano);

/* longitude and latitude conversion */
int	nmea_degrees(int64_t *dst, char *src, int neg);

/* degrade the timedelta sensor */
void	nmea_timeout(void *);

void
nmeaattach(int dummy)
{
	/* noop */
}

int
nmeaopen(dev_t dev, struct tty *tp, struct proc *p)
{
	struct nmea *np;
	int error;

	if (tp->t_line == NMEADISC)
		return (ENODEV);
	if ((error = suser(p, 0)) != 0)
		return (error);
	np = malloc(sizeof(struct nmea), M_DEVBUF, M_WAITOK | M_ZERO);
	snprintf(np->timedev.xname, sizeof(np->timedev.xname), "nmea%d",
	    nmea_nxid++);
	nmea_count++;
	np->time.status = SENSOR_S_UNKNOWN;
	np->time.type = SENSOR_TIMEDELTA;
	np->time.flags = SENSOR_FINVALID;
	sensor_attach(&np->timedev, &np->time);

	np->signal.type = SENSOR_INDICATOR;
	np->signal.status = SENSOR_S_UNKNOWN;
	np->signal.value = 0;
	strlcpy(np->signal.desc, "Signal", sizeof(np->signal.desc));
	sensor_attach(&np->timedev, &np->signal);

	np->latitude.type = SENSOR_ANGLE;
	np->latitude.status = SENSOR_S_UNKNOWN;
	np->latitude.flags = SENSOR_FINVALID;
	np->latitude.value = 0;
	strlcpy(np->latitude.desc, "Latitude", sizeof(np->latitude.desc));
	sensor_attach(&np->timedev, &np->latitude);

	np->longitude.type = SENSOR_ANGLE;
	np->longitude.status = SENSOR_S_UNKNOWN;
	np->longitude.flags = SENSOR_FINVALID;
	np->longitude.value = 0;
	strlcpy(np->longitude.desc, "Longitude", sizeof(np->longitude.desc));
	sensor_attach(&np->timedev, &np->longitude);

	np->sync = 1;
	tp->t_sc = (caddr_t)np;

	error = linesw[TTYDISC].l_open(dev, tp, p);
	if (error) {
		free(np, M_DEVBUF, sizeof(*np));
		tp->t_sc = NULL;
	} else {
		sensordev_install(&np->timedev);
		timeout_set(&np->nmea_tout, nmea_timeout, np);
	}
	return (error);
}

int
nmeaclose(struct tty *tp, int flags, struct proc *p)
{
	struct nmea *np = (struct nmea *)tp->t_sc;

	tp->t_line = TTYDISC;	/* switch back to termios */
	timeout_del(&np->nmea_tout);
	sensordev_deinstall(&np->timedev);
	free(np, M_DEVBUF, sizeof(*np));
	tp->t_sc = NULL;
	nmea_count--;
	if (nmea_count == 0)
		nmea_nxid = 0;
	return (linesw[TTYDISC].l_close(tp, flags, p));
}

/* Collect NMEA sentences from the tty. */
int
nmeainput(int c, struct tty *tp)
{
	struct nmea *np = (struct nmea *)tp->t_sc;
	struct timespec ts;
	int64_t gap;
	long tmin, tmax;

	switch (c) {
	case '$':
		nanotime(&ts);
		np->pos = np->sync = 0;
		gap = (ts.tv_sec * 1000000000LL + ts.tv_nsec) -
		    (np->lts.tv_sec * 1000000000LL + np->lts.tv_nsec);

		np->lts.tv_sec = ts.tv_sec;
		np->lts.tv_nsec = ts.tv_nsec;

		if (gap <= np->gap)
			break;

		np->ts.tv_sec = ts.tv_sec;
		np->ts.tv_nsec = ts.tv_nsec;

#ifdef NMEA_DEBUG
		if (nmeadebug > 0) {
			linesw[TTYDISC].l_rint('[', tp);
			linesw[TTYDISC].l_rint('0' + np->gapno++, tp);
			linesw[TTYDISC].l_rint(']', tp);
		}
#endif
		np->gap = gap;

		/*
		 * If a tty timestamp is available, make sure its value is
		 * reasonable by comparing against the timestamp just taken.
		 * If they differ by more than 2 seconds, assume no PPS signal
		 * is present, note the fact, and keep using the timestamp
		 * value.  When this happens, the sensor state is set to
		 * CRITICAL later when the GPRMC sentence is decoded.
		 */
		if (tp->t_flags & (TS_TSTAMPDCDSET | TS_TSTAMPDCDCLR |
		    TS_TSTAMPCTSSET | TS_TSTAMPCTSCLR)) {
			tmax = lmax(np->ts.tv_sec, tp->t_tv.tv_sec);
			tmin = lmin(np->ts.tv_sec, tp->t_tv.tv_sec);
			if (tmax - tmin > 1)
				np->no_pps = 1;
			else {
				np->ts.tv_sec = tp->t_tv.tv_sec;
				np->ts.tv_nsec = tp->t_tv.tv_usec *
				    1000L;
				np->no_pps = 0;
			}
		}
		break;
	case '\r':
	case '\n':
		if (!np->sync) {
			np->cbuf[np->pos] = '\0';
			nmea_scan(np, tp);
			np->sync = 1;
		}
		break;
	default:
		if (!np->sync && np->pos < (NMEAMAX - 1))
			np->cbuf[np->pos++] = c;
		break;
	}
	/* pass data to termios */
	return (linesw[TTYDISC].l_rint(c, tp));
}

/* Scan the NMEA sentence just received. */
void
nmea_scan(struct nmea *np, struct tty *tp)
{
	int fldcnt = 0, cksum = 0, msgcksum, n;
	char *fld[MAXFLDS], *cs;

	/* split into fields and calculate the checksum */
	fld[fldcnt++] = &np->cbuf[0];	/* message type */
	for (cs = NULL, n = 0; n < np->pos && cs == NULL; n++) {
		switch (np->cbuf[n]) {
		case '*':
			np->cbuf[n] = '\0';
			cs = &np->cbuf[n + 1];
			break;
		case ',':
			if (fldcnt < MAXFLDS) {
				cksum ^= np->cbuf[n];
				np->cbuf[n] = '\0';
				fld[fldcnt++] = &np->cbuf[n + 1];
			} else {
				DPRINTF(("nr of fields in %s sentence exceeds "
				    "maximum of %d\n", fld[0], MAXFLDS));
				return;
			}
			break;
		default:
			cksum ^= np->cbuf[n];
		}
	}

	/* we only look at the GPRMC message */
	if (strcmp(fld[0], "GPRMC"))
		return;

	/* if we have a checksum, verify it */
	if (cs != NULL) {
		msgcksum = 0;
		while (*cs) {
			if ((*cs >= '0' && *cs <= '9') ||
			    (*cs >= 'A' && *cs <= 'F')) {
				if (msgcksum)
					msgcksum <<= 4;
				if (*cs >= '0' && *cs<= '9')
					msgcksum += *cs - '0';
				else if (*cs >= 'A' && *cs <= 'F')
					msgcksum += 10 + *cs - 'A';
				cs++;
			} else {
				DPRINTF(("bad char %c in checksum\n", *cs));
				return;
			}
		}
		if (msgcksum != cksum) {
			DPRINTF(("checksum mismatch\n"));
			return;
		}
	}
	nmea_gprmc(np, tp, fld, fldcnt);
}

/* Decode the recommended minimum specific GPS/TRANSIT data. */
void
nmea_gprmc(struct nmea *np, struct tty *tp, char *fld[], int fldcnt)
{
	int64_t date_nano, time_nano, nmea_now;
	int jumped = 0;

	if (fldcnt != 12 && fldcnt != 13) {
		DPRINTF(("gprmc: field count mismatch, %d\n", fldcnt));
		return;
	}
	if (nmea_time_to_nano(fld[1], &time_nano)) {
		DPRINTF(("gprmc: illegal time, %s\n", fld[1]));
		return;
	}
	if (nmea_date_to_nano(fld[9], &date_nano)) {
		DPRINTF(("gprmc: illegal date, %s\n", fld[9]));
		return;
	}
	nmea_now = date_nano + time_nano;
	if (nmea_now <= np->last) {
		DPRINTF(("gprmc: time not monotonically increasing\n"));
		jumped = 1;
	}
	np->last = nmea_now;
	np->gap = 0LL;
#ifdef NMEA_DEBUG
	if (np->time.status == SENSOR_S_UNKNOWN) {
		np->time.status = SENSOR_S_OK;
		timeout_add_sec(&np->nmea_tout, TRUSTTIME);
	}
	np->gapno = 0;
	if (nmeadebug > 0) {
		linesw[TTYDISC].l_rint('[', tp);
		linesw[TTYDISC].l_rint('C', tp);
		linesw[TTYDISC].l_rint(']', tp);
	}
#endif

	np->time.value = np->ts.tv_sec * 1000000000LL +
	    np->ts.tv_nsec - nmea_now;
	np->time.tv.tv_sec = np->ts.tv_sec;
	np->time.tv.tv_usec = np->ts.tv_nsec / 1000L;

	if (fldcnt != 13)
		strlcpy(np->time.desc, "GPS", sizeof(np->time.desc));
	else if (fldcnt == 13 && *fld[12] != np->mode) {
		np->mode = *fld[12];
		switch (np->mode) {
		case 'S':
			strlcpy(np->time.desc, "GPS simulated",
			    sizeof(np->time.desc));
			break;
		case 'E':
			strlcpy(np->time.desc, "GPS estimated",
			    sizeof(np->time.desc));
			break;
		case 'A':
			strlcpy(np->time.desc, "GPS autonomous",
			    sizeof(np->time.desc));
			break;
		case 'D':
			strlcpy(np->time.desc, "GPS differential",
			    sizeof(np->time.desc));
			break;
		case 'N':
			strlcpy(np->time.desc, "GPS invalid",
			    sizeof(np->time.desc));
			break;
		default:
			strlcpy(np->time.desc, "GPS unknown",
			    sizeof(np->time.desc));
			DPRINTF(("gprmc: unknown mode '%c'\n", np->mode));
		}
	}
	switch (*fld[2]) {
	case 'A':	/* The GPS has a fix, (re)arm the timeout. */
			/* XXX is 'D' also a valid state? */
		np->time.status = SENSOR_S_OK;
		np->signal.value = 1;
		np->signal.status = SENSOR_S_OK;
		np->latitude.status = SENSOR_S_OK;
		np->longitude.status = SENSOR_S_OK;
		np->time.flags &= ~SENSOR_FINVALID;
		np->latitude.flags &= ~SENSOR_FINVALID;
		np->longitude.flags &= ~SENSOR_FINVALID;
		break;
	case 'V':	/*
			 * The GPS indicates a warning status, do not add to
			 * the timeout, if the condition persist, the sensor
			 * will be degraded.  Signal the condition through
			 * the signal sensor.
			 */
		np->signal.value = 0;
		np->signal.status = SENSOR_S_CRIT;
		np->latitude.status = SENSOR_S_WARN;
		np->longitude.status = SENSOR_S_WARN;
		break;
	}
	if (nmea_degrees(&np->latitude.value, fld[3], *fld[4] == 'S' ? 1 : 0))
		np->latitude.status = SENSOR_S_WARN;
	if (nmea_degrees(&np->longitude.value,fld[5], *fld[6] == 'W' ? 1 : 0))
		np->longitude.status = SENSOR_S_WARN;

	if (jumped)
		np->time.status = SENSOR_S_WARN;
	if (np->time.status == SENSOR_S_OK)
		timeout_add_sec(&np->nmea_tout, TRUSTTIME);
	/*
	 * If tty timestamping is requested, but no PPS signal is present, set
	 * the sensor state to CRITICAL.
	 */
	if (np->no_pps)
		np->time.status = SENSOR_S_CRIT;
}

/*
 * Convert a nmea position in the form DDDMM.MMMM to an
 * angle sensor value (degrees*1000000)
 */
int
nmea_degrees(int64_t *dst, char *src, int neg)
{
	size_t ppos;
	int i, n;
	int64_t deg = 0, min = 0;
	char *p;

	while (*src == '0')
		++src;	/* skip leading zeroes */

	for (p = src, ppos = 0; *p; ppos++)
		if (*p++ == '.')
			break;

	if (*p == '\0')
		return (-1);	/* no decimal point */

	for (n = 0; *src && n + 2 < ppos; n++)
		deg = deg * 10 + (*src++ - '0');

	for (; *src && n < ppos; n++)
		min = min * 10 + (*src++ - '0');

	src++;		/* skip decimal point */

	for (; *src && n < (ppos + 4); n++)
		min = min * 10 + (*src++ - '0');

	for (i=0; i < 6 + ppos - n; i++)
		min *= 10;

	deg = deg * 1000000 + (min/60);

	*dst = neg ? -deg : deg;
	return (0);
}

/*
 * Convert a NMEA 0183 formatted date string to seconds since the epoch.
 * The string must be of the form DDMMYY.
 * Return 0 on success, -1 if illegal characters are encountered.
 */
int
nmea_date_to_nano(char *s, int64_t *nano)
{
	struct clock_ymdhms ymd;
	time_t secs;
	char *p;
	int n;

	/* make sure the input contains only numbers and is six digits long */
	for (n = 0, p = s; n < 6 && *p && *p >= '0' && *p <= '9'; n++, p++)
		;
	if (n != 6 || (*p != '\0'))
		return (-1);

	ymd.dt_year = 2000 + (s[4] - '0') * 10 + (s[5] - '0');
	ymd.dt_mon = (s[2] - '0') * 10 + (s[3] - '0');
	ymd.dt_day = (s[0] - '0') * 10 + (s[1] - '0');
	ymd.dt_hour = ymd.dt_min = ymd.dt_sec = 0;

	secs = clock_ymdhms_to_secs(&ymd);
	*nano = secs * 1000000000LL;
	return (0);
}

/*
 * Convert NMEA 0183 formatted time string to nanoseconds since midnight.
 * The string must be of the form HHMMSS[.[sss]] (e.g. 143724 or 143723.615).
 * Return 0 on success, -1 if illegal characters are encountered.
 */
int
nmea_time_to_nano(char *s, int64_t *nano)
{
	long fac = 36000L, div = 6L, secs = 0L, frac = 0L;
	char ul = '2';
	int n;

	for (n = 0, secs = 0; fac && *s && *s >= '0' && *s <= ul; s++, n++) {
		secs += (*s - '0') * fac;
		div = 16 - div;
		fac /= div;
		switch (n) {
		case 0:
			if (*s <= '1')
				ul = '9';
			else
				ul = '3';
			break;
		case 1:
		case 3:
			ul = '5';
			break;
		case 2:
		case 4:
			ul = '9';
			break;
		}
	}
	if (fac)
		return (-1);

	/* Handle the fractions of a second, up to a maximum of 6 digits. */
	div = 1L;
	if (*s == '.') {
		for (++s; div < 1000000 && *s && *s >= '0' && *s <= '9'; s++) {
			frac *= 10;
			frac += (*s - '0');
			div *= 10;
		}
	}

	if (*s != '\0')
		return (-1);

	*nano = secs * 1000000000LL + (int64_t)frac * (1000000000 / div);
	return (0);
}

/*
 * Degrade the sensor state if we received no NMEA sentences for more than
 * TRUSTTIME seconds.
 */
void
nmea_timeout(void *xnp)
{
	struct nmea *np = xnp;

	np->signal.value = 0;
	np->signal.status = SENSOR_S_CRIT;
	if (np->time.status == SENSOR_S_OK) {
		np->time.status = SENSOR_S_WARN;
		np->latitude.status = SENSOR_S_WARN;
		np->longitude.status = SENSOR_S_WARN;
		/*
		 * further degrade in TRUSTTIME seconds if no new valid NMEA
		 * sentences are received.
		 */
		timeout_add_sec(&np->nmea_tout, TRUSTTIME);
	} else {
		np->time.status = SENSOR_S_CRIT;
		np->latitude.status = SENSOR_S_CRIT;
		np->longitude.status = SENSOR_S_CRIT;
	}
}