xref: /openbsd/sys/kern/tty_nmea.c (revision 404b540a) 
1 /*	$OpenBSD: tty_nmea.c,v 1.35 2009/06/02 21:17:35 ckuethe Exp $ */
2 
3 /*
4  * Copyright (c) 2006, 2007, 2008 Marc Balmer <mbalmer@openbsd.org>
5  *
6  * Permission to use, copy, modify, and distribute this software for any
7  * purpose with or without fee is hereby granted, provided that the above
8  * copyright notice and this permission notice appear in all copies.
9  *
10  * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
11  * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
12  * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
13  * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
14  * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
15  * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
16  * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
17  */
18 
19 /* A tty line discipline to decode NMEA 0183 data to get the time. */
20 
21 #include <sys/param.h>
22 #include <sys/systm.h>
23 #include <sys/proc.h>
24 #include <sys/malloc.h>
25 #include <sys/sensors.h>
26 #include <sys/tty.h>
27 #include <sys/conf.h>
28 #include <sys/time.h>
29 
30 #ifdef NMEA_DEBUG
31 #define DPRINTFN(n, x)	do { if (nmeadebug > (n)) printf x; } while (0)
32 int nmeadebug = 0;
33 #else
34 #define DPRINTFN(n, x)
35 #endif
36 #define DPRINTF(x)	DPRINTFN(0, x)
37 
38 int	nmeaopen(dev_t, struct tty *);
39 int	nmeaclose(struct tty *, int);
40 int	nmeainput(int, struct tty *);
41 void	nmeaattach(int);
42 
43 #define NMEAMAX		82
44 #define MAXFLDS		32
45 #ifdef NMEA_DEBUG
46 #define TRUSTTIME	30
47 #else
48 #define TRUSTTIME	(10 * 60)	/* 10 minutes */
49 #endif
50 
51 int nmea_count, nmea_nxid;
52 
53 struct nmea {
54 	char			cbuf[NMEAMAX];	/* receive buffer */
55 	struct ksensor		time;		/* the timedelta sensor */
56 	struct ksensor		signal;		/* signal status */
57 	struct ksensordev	timedev;
58 	struct timespec		ts;		/* current timestamp */
59 	struct timespec		lts;		/* timestamp of last '$' seen */
60 	struct timeout		nmea_tout;	/* invalidate sensor */
61 	int64_t			gap;		/* gap between two sentences */
62 #ifdef NMEA_DEBUG
63 	int			gapno;
64 #endif
65 	int64_t			last;		/* last time rcvd */
66 	int			sync;		/* if 1, waiting for '$' */
67 	int			pos;		/* position in rcv buffer */
68 	int			no_pps;		/* no PPS although requested */
69 	char			mode;		/* GPS mode */
70 };
71 
72 /* NMEA decoding */
73 void	nmea_scan(struct nmea *, struct tty *);
74 void	nmea_gprmc(struct nmea *, struct tty *, char *fld[], int fldcnt);
75 
76 /* date and time conversion */
77 int	nmea_date_to_nano(char *s, int64_t *nano);
78 int	nmea_time_to_nano(char *s, int64_t *nano);
79 
80 #if NMEA_POS_IN_DESC
81 /* longitude and latitude formatting and copying */
82 void	nmea_degrees(char *dst, char *src, int neg, size_t len);
83 #endif
84 
85 /* degrade the timedelta sensor */
86 void	nmea_timeout(void *);
87 
88 void
89 nmeaattach(int dummy)
90 {
91 }
92 
93 int
94 nmeaopen(dev_t dev, struct tty *tp)
95 {
96 	struct proc *p = curproc;
97 	struct nmea *np;
98 	int error;
99 
100 	if (tp->t_line == NMEADISC)
101 		return ENODEV;
102 	if ((error = suser(p, 0)) != 0)
103 		return error;
104 	np = malloc(sizeof(struct nmea), M_DEVBUF, M_WAITOK | M_ZERO);
105 	snprintf(np->timedev.xname, sizeof(np->timedev.xname), "nmea%d",
106 	    nmea_nxid++);
107 	nmea_count++;
108 	np->time.status = SENSOR_S_UNKNOWN;
109 	np->time.type = SENSOR_TIMEDELTA;
110 	sensor_attach(&np->timedev, &np->time);
111 
112 	np->signal.type = SENSOR_PERCENT;
113 	np->signal.status = SENSOR_S_UNKNOWN;
114 	np->signal.value = 100000LL;
115 	strlcpy(np->signal.desc, "Signal", sizeof(np->signal.desc));
116 	sensor_attach(&np->timedev, &np->signal);
117 
118 	np->sync = 1;
119 	tp->t_sc = (caddr_t)np;
120 
121 	error = linesw[TTYDISC].l_open(dev, tp);
122 	if (error) {
123 		free(np, M_DEVBUF);
124 		tp->t_sc = NULL;
125 	} else {
126 		sensordev_install(&np->timedev);
127 		timeout_set(&np->nmea_tout, nmea_timeout, np);
128 	}
129 	return error;
130 }
131 
132 int
133 nmeaclose(struct tty *tp, int flags)
134 {
135 	struct nmea *np = (struct nmea *)tp->t_sc;
136 
137 	tp->t_line = TTYDISC;	/* switch back to termios */
138 	timeout_del(&np->nmea_tout);
139 	sensordev_deinstall(&np->timedev);
140 	free(np, M_DEVBUF);
141 	tp->t_sc = NULL;
142 	nmea_count--;
143 	if (nmea_count == 0)
144 		nmea_nxid = 0;
145 	return linesw[TTYDISC].l_close(tp, flags);
146 }
147 
148 /* Collect NMEA sentences from the tty. */
149 int
150 nmeainput(int c, struct tty *tp)
151 {
152 	struct nmea *np = (struct nmea *)tp->t_sc;
153 	struct timespec ts;
154 	int64_t gap;
155 	long tmin, tmax;
156 
157 	switch (c) {
158 	case '$':
159 		nanotime(&ts);
160 		np->pos = np->sync = 0;
161 		gap = (ts.tv_sec * 1000000000LL + ts.tv_nsec) -
162 		    (np->lts.tv_sec * 1000000000LL + np->lts.tv_nsec);
163 
164 		np->lts.tv_sec = ts.tv_sec;
165 		np->lts.tv_nsec = ts.tv_nsec;
166 
167 		if (gap <= np->gap)
168 			break;
169 
170 		np->ts.tv_sec = ts.tv_sec;
171 		np->ts.tv_nsec = ts.tv_nsec;
172 
173 #ifdef NMEA_DEBUG
174 		if (nmeadebug > 0) {
175 			linesw[TTYDISC].l_rint('[', tp);
176 			linesw[TTYDISC].l_rint('0' + np->gapno++, tp);
177 			linesw[TTYDISC].l_rint(']', tp);
178 		}
179 #endif
180 		np->gap = gap;
181 
182 		/*
183 		 * If a tty timestamp is available, make sure its value is
184 		 * reasonable by comparing against the timestamp just taken.
185 		 * If they differ by more than 2 seconds, assume no PPS signal
186 		 * is present, note the fact, and keep using the timestamp
187 		 * value.  When this happens, the sensor state is set to
188 		 * CRITICAL later when the GPRMC sentence is decoded.
189 		 */
190 		if (tp->t_flags & (TS_TSTAMPDCDSET | TS_TSTAMPDCDCLR |
191 		    TS_TSTAMPCTSSET | TS_TSTAMPCTSCLR)) {
192 			tmax = lmax(np->ts.tv_sec, tp->t_tv.tv_sec);
193 			tmin = lmin(np->ts.tv_sec, tp->t_tv.tv_sec);
194 			if (tmax - tmin > 1)
195 				np->no_pps = 1;
196 			else {
197 				np->ts.tv_sec = tp->t_tv.tv_sec;
198 				np->ts.tv_nsec = tp->t_tv.tv_usec *
199 				    1000L;
200 				np->no_pps = 0;
201 			}
202 		}
203 		break;
204 	case '\r':
205 	case '\n':
206 		if (!np->sync) {
207 			np->cbuf[np->pos] = '\0';
208 			nmea_scan(np, tp);
209 			np->sync = 1;
210 		}
211 		break;
212 	default:
213 		if (!np->sync && np->pos < (NMEAMAX - 1))
214 			np->cbuf[np->pos++] = c;
215 		break;
216 	}
217 	/* pass data to termios */
218 	return linesw[TTYDISC].l_rint(c, tp);
219 }
220 
221 /* Scan the NMEA sentence just received. */
222 void
223 nmea_scan(struct nmea *np, struct tty *tp)
224 {
225 	int fldcnt = 0, cksum = 0, msgcksum, n;
226 	char *fld[MAXFLDS], *cs;
227 
228 	/* split into fields and calculate the checksum */
229 	fld[fldcnt++] = &np->cbuf[0];	/* message type */
230 	for (cs = NULL, n = 0; n < np->pos && cs == NULL; n++) {
231 		switch (np->cbuf[n]) {
232 		case '*':
233 			np->cbuf[n] = '\0';
234 			cs = &np->cbuf[n + 1];
235 			break;
236 		case ',':
237 			if (fldcnt < MAXFLDS) {
238 				cksum ^= np->cbuf[n];
239 				np->cbuf[n] = '\0';
240 				fld[fldcnt++] = &np->cbuf[n + 1];
241 			} else {
242 				DPRINTF(("nr of fields in %s sentence exceeds "
243 				    "maximum of %d\n", fld[0], MAXFLDS));
244 				return;
245 			}
246 			break;
247 		default:
248 			cksum ^= np->cbuf[n];
249 		}
250 	}
251 
252 	/* we only look at the GPRMC message */
253 	if (strcmp(fld[0], "GPRMC"))
254 		return;
255 
256 	/* if we have a checksum, verify it */
257 	if (cs != NULL) {
258 		msgcksum = 0;
259 		while (*cs) {
260 			if ((*cs >= '0' && *cs <= '9') ||
261 			    (*cs >= 'A' && *cs <= 'F')) {
262 				if (msgcksum)
263 					msgcksum <<= 4;
264 				if (*cs >= '0' && *cs<= '9')
265 					msgcksum += *cs - '0';
266 				else if (*cs >= 'A' && *cs <= 'F')
267 					msgcksum += 10 + *cs - 'A';
268 				cs++;
269 			} else {
270 				DPRINTF(("bad char %c in checksum\n", *cs));
271 				return;
272 			}
273 		}
274 		if (msgcksum != cksum) {
275 			DPRINTF(("checksum mismatch\n"));
276 			return;
277 		}
278 	}
279 	nmea_gprmc(np, tp, fld, fldcnt);
280 }
281 
282 /* Decode the recommended minimum specific GPS/TRANSIT data. */
283 void
284 nmea_gprmc(struct nmea *np, struct tty *tp, char *fld[], int fldcnt)
285 {
286 	int64_t date_nano, time_nano, nmea_now;
287 	int jumped = 0;
288 
289 	if (fldcnt != 12 && fldcnt != 13) {
290 		DPRINTF(("gprmc: field count mismatch, %d\n", fldcnt));
291 		return;
292 	}
293 	if (nmea_time_to_nano(fld[1], &time_nano)) {
294 		DPRINTF(("gprmc: illegal time, %s\n", fld[1]));
295 		return;
296 	}
297 	if (nmea_date_to_nano(fld[9], &date_nano)) {
298 		DPRINTF(("gprmc: illegal date, %s\n", fld[9]));
299 		return;
300 	}
301 	nmea_now = date_nano + time_nano;
302 	if (nmea_now <= np->last) {
303 		DPRINTF(("gprmc: time not monotonically increasing\n"));
304 		jumped = 1;
305 	}
306 	np->last = nmea_now;
307 	np->gap = 0LL;
308 #ifdef NMEA_DEBUG
309 	if (np->time.status == SENSOR_S_UNKNOWN) {
310 		np->time.status = SENSOR_S_OK;
311 		timeout_add_sec(&np->nmea_tout, TRUSTTIME);
312 	}
313 	np->gapno = 0;
314 	if (nmeadebug > 0) {
315 		linesw[TTYDISC].l_rint('[', tp);
316 		linesw[TTYDISC].l_rint('C', tp);
317 		linesw[TTYDISC].l_rint(']', tp);
318 	}
319 #endif
320 
321 	np->time.value = np->ts.tv_sec * 1000000000LL +
322 	    np->ts.tv_nsec - nmea_now;
323 	np->time.tv.tv_sec = np->ts.tv_sec;
324 	np->time.tv.tv_usec = np->ts.tv_nsec / 1000L;
325 
326 	if (fldcnt != 13)
327 		strlcpy(np->time.desc, "GPS", sizeof(np->time.desc));
328 	else if (fldcnt == 13 && *fld[12] != np->mode) {
329 		np->mode = *fld[12];
330 		switch (np->mode) {
331 		case 'S':
332 			strlcpy(np->time.desc, "GPS simulated",
333 			    sizeof(np->time.desc));
334 			break;
335 		case 'E':
336 			strlcpy(np->time.desc, "GPS estimated",
337 			    sizeof(np->time.desc));
338 			break;
339 		case 'A':
340 			strlcpy(np->time.desc, "GPS autonomous",
341 			    sizeof(np->time.desc));
342 			break;
343 		case 'D':
344 			strlcpy(np->time.desc, "GPS differential",
345 			    sizeof(np->time.desc));
346 			break;
347 		case 'N':
348 			strlcpy(np->time.desc, "GPS invalid",
349 			    sizeof(np->time.desc));
350 			break;
351 		default:
352 			strlcpy(np->time.desc, "GPS unknown",
353 			    sizeof(np->time.desc));
354 			DPRINTF(("gprmc: unknown mode '%c'\n", np->mode));
355 		}
356 	}
357 #if NMEA_POS_IN_DESC
358 	nmea_degrees(np->time.desc, fld[3], *fld[4] == 'S' ? 1 : 0,
359 	    sizeof(np->time.desc));
360 	nmea_degrees(np->time.desc, fld[5], *fld[6] == 'W' ? 1 : 0,
361 	    sizeof(np->time.desc));
362 #endif
363 	switch (*fld[2]) {
364 	case 'A':	/* The GPS has a fix, (re)arm the timeout. */
365 		np->time.status = SENSOR_S_OK;
366 		np->signal.status = SENSOR_S_OK;
367 		break;
368 	case 'V':	/*
369 			 * The GPS indicates a warning status, do not add to
370 			 * the timeout, if the condition persist, the sensor
371 			 * will be degraded.  Signal the condition through
372 			 * the signal sensor.
373 			 */
374 		np->signal.status = SENSOR_S_WARN;
375 		break;
376 	}
377 
378 	if (jumped)
379 		np->time.status = SENSOR_S_WARN;
380 	if (np->time.status == SENSOR_S_OK)
381 		timeout_add_sec(&np->nmea_tout, TRUSTTIME);
382 	/*
383 	 * If tty timestamping is requested, but no PPS signal is present, set
384 	 * the sensor state to CRITICAL.
385 	 */
386 	if (np->no_pps)
387 		np->time.status = SENSOR_S_CRIT;
388 }
389 
390 #ifdef NMEA_POS_IN_DESC
391 /* format a nmea position in the form DDDMM.MMMM to DDDdMM.MMm */
392 void
393 nmea_degrees(char *dst, char *src, int neg, size_t len)
394 {
395 	size_t dlen, ppos, rlen;
396 	int n;
397 	char *p;
398 
399 	for (dlen = 0; *dst; dlen++)
400 		dst++;
401 
402 	while (*src == '0')
403 		++src;	/* skip leading zeroes */
404 
405 	for (p = src, ppos = 0; *p; ppos++)
406 		if (*p++ == '.')
407 			break;
408 
409 	if (*p == '\0')
410 		return;	/* no decimal point */
411 
412 	/*
413 	 * we need at least room for a comma, an optional '-', the src data up
414 	 * to the decimal point, the decimal point itself, two digits after
415 	 * it and some additional characters:  an optional leading '0' in case
416 	 * there a no degrees in src, the 'd' degrees indicator, the 'm'
417 	 * minutes indicator and the terminating NUL character.
418 	 */
419 	rlen = dlen + ppos + 7;
420 	if (neg)
421 		rlen++;
422 	if (ppos < 3)
423 		rlen++;
424 	if (len < rlen)
425 		return;		/* not enough room in dst */
426 
427 	*dst++ = ',';
428 	if (neg)
429 		*dst++ = '-';
430 
431 	if (ppos < 3)
432 		*dst++ = '0';
433 
434 	for (n = 0; *src && n + 2 < ppos; n++)
435 		*dst++ = *src++;
436 	*dst++ = 'd';
437 	if (ppos == 0)
438 		*dst++ = '0';
439 
440 	for (; *src && n < (ppos + 3); n++)
441 		*dst++ = *src++;
442 	*dst++ = 'm';
443 	*dst = '\0';
444 }
445 #endif
446 
447 /*
448  * Convert a NMEA 0183 formatted date string to seconds since the epoch.
449  * The string must be of the form DDMMYY.
450  * Return 0 on success, -1 if illegal characters are encountered.
451  */
452 int
453 nmea_date_to_nano(char *s, int64_t *nano)
454 {
455 	struct clock_ymdhms ymd;
456 	time_t secs;
457 	char *p;
458 	int n;
459 
460 	/* make sure the input contains only numbers and is six digits long */
461 	for (n = 0, p = s; n < 6 && *p && *p >= '0' && *p <= '9'; n++, p++)
462 		;
463 	if (n != 6 || (*p != '\0'))
464 		return -1;
465 
466 	ymd.dt_year = 2000 + (s[4] - '0') * 10 + (s[5] - '0');
467 	ymd.dt_mon = (s[2] - '0') * 10 + (s[3] - '0');
468 	ymd.dt_day = (s[0] - '0') * 10 + (s[1] - '0');
469 	ymd.dt_hour = ymd.dt_min = ymd.dt_sec = 0;
470 
471 	secs = clock_ymdhms_to_secs(&ymd);
472 	*nano = secs * 1000000000LL;
473 	return 0;
474 }
475 
476 /*
477  * Convert NMEA 0183 formatted time string to nanoseconds since midnight.
478  * The string must be of the form HHMMSS[.[sss]] (e.g. 143724 or 143723.615).
479  * Return 0 on success, -1 if illegal characters are encountered.
480  */
481 int
482 nmea_time_to_nano(char *s, int64_t *nano)
483 {
484 	long fac = 36000L, div = 6L, secs = 0L, frac = 0L;
485 	char ul = '2';
486 	int n;
487 
488 	for (n = 0, secs = 0; fac && *s && *s >= '0' && *s <= ul; s++, n++) {
489 		secs += (*s - '0') * fac;
490 		div = 16 - div;
491 		fac /= div;
492 		switch (n) {
493 		case 0:
494 			if (*s <= '1')
495 				ul = '9';
496 			else
497 				ul = '3';
498 			break;
499 		case 1:
500 		case 3:
501 			ul = '5';
502 			break;
503 		case 2:
504 		case 4:
505 			ul = '9';
506 			break;
507 		}
508 	}
509 	if (fac)
510 		return -1;
511 
512 	/* Handle the fractions of a second, up to a maximum of 6 digits. */
513 	div = 1L;
514 	if (*s == '.') {
515 		for (++s; div < 1000000 && *s && *s >= '0' && *s <= '9'; s++) {
516 			frac *= 10;
517 			frac += (*s - '0');
518 			div *= 10;
519 		}
520 	}
521 
522 	if (*s != '\0')
523 		return -1;
524 
525 	*nano = secs * 1000000000LL + (int64_t)frac * (1000000000 / div);
526 	return 0;
527 }
528 
529 /*
530  * Degrade the sensor state if we received no NMEA sentences for more than
531  * TRUSTTIME seconds.
532  */
533 void
534 nmea_timeout(void *xnp)
535 {
536 	struct nmea *np = xnp;
537 
538 	if (np->time.status == SENSOR_S_OK) {
539 		np->time.status = SENSOR_S_WARN;
540 		/*
541 		 * further degrade in TRUSTTIME seconds if no new valid NMEA
542 		 * sentences are received.
543 		 */
544 		timeout_add_sec(&np->nmea_tout, TRUSTTIME);
545 	} else
546 		np->time.status = SENSOR_S_CRIT;
547 }
548