xref: /openbsd-src/sys/kern/tty_nmea.c (revision 2b0358df1d88d06ef4139321dd05bd5e05d91eaf) 
1 /*	$OpenBSD: tty_nmea.c,v 1.33 2008/12/25 21:25:55 stevesk 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 	np->time.value = 0LL;
111 	sensor_attach(&np->timedev, &np->time);
112 
113 	np->signal.type = SENSOR_PERCENT;
114 	np->signal.status = SENSOR_S_UNKNOWN;
115 	np->signal.value = 100000LL;
116 	np->signal.flags = 0;
117 	strlcpy(np->signal.desc, "Signal", sizeof(np->signal.desc));
118 	sensor_attach(&np->timedev, &np->signal);
119 
120 	np->sync = 1;
121 	tp->t_sc = (caddr_t)np;
122 
123 	error = linesw[TTYDISC].l_open(dev, tp);
124 	if (error) {
125 		free(np, M_DEVBUF);
126 		tp->t_sc = NULL;
127 	} else {
128 		sensordev_install(&np->timedev);
129 		timeout_set(&np->nmea_tout, nmea_timeout, np);
130 	}
131 	return error;
132 }
133 
134 int
135 nmeaclose(struct tty *tp, int flags)
136 {
137 	struct nmea *np = (struct nmea *)tp->t_sc;
138 
139 	tp->t_line = TTYDISC;	/* switch back to termios */
140 	timeout_del(&np->nmea_tout);
141 	sensordev_deinstall(&np->timedev);
142 	free(np, M_DEVBUF);
143 	tp->t_sc = NULL;
144 	nmea_count--;
145 	if (nmea_count == 0)
146 		nmea_nxid = 0;
147 	return linesw[TTYDISC].l_close(tp, flags);
148 }
149 
150 /* Collect NMEA sentences from the tty. */
151 int
152 nmeainput(int c, struct tty *tp)
153 {
154 	struct nmea *np = (struct nmea *)tp->t_sc;
155 	struct timespec ts;
156 	int64_t gap;
157 	long tmin, tmax;
158 
159 	switch (c) {
160 	case '$':
161 		nanotime(&ts);
162 		np->pos = np->sync = 0;
163 		gap = (ts.tv_sec * 1000000000LL + ts.tv_nsec) -
164 		    (np->lts.tv_sec * 1000000000LL + np->lts.tv_nsec);
165 
166 		np->lts.tv_sec = ts.tv_sec;
167 		np->lts.tv_nsec = ts.tv_nsec;
168 
169 		if (gap <= np->gap)
170 			break;
171 
172 		np->ts.tv_sec = ts.tv_sec;
173 		np->ts.tv_nsec = ts.tv_nsec;
174 
175 #ifdef NMEA_DEBUG
176 		if (nmeadebug > 0) {
177 			linesw[TTYDISC].l_rint('[', tp);
178 			linesw[TTYDISC].l_rint('0' + np->gapno++, tp);
179 			linesw[TTYDISC].l_rint(']', tp);
180 		}
181 #endif
182 		np->gap = gap;
183 
184 		/*
185 		 * If a tty timestamp is available, make sure its value is
186 		 * reasonable by comparing against the timestamp just taken.
187 		 * If they differ by more than 2 seconds, assume no PPS signal
188 		 * is present, note the fact, and keep using the timestamp
189 		 * value.  When this happens, the sensor state is set to
190 		 * CRITICAL later when the GPRMC sentence is decoded.
191 		 */
192 		if (tp->t_flags & (TS_TSTAMPDCDSET | TS_TSTAMPDCDCLR |
193 		    TS_TSTAMPCTSSET | TS_TSTAMPCTSCLR)) {
194 			tmax = lmax(np->ts.tv_sec, tp->t_tv.tv_sec);
195 			tmin = lmin(np->ts.tv_sec, tp->t_tv.tv_sec);
196 			if (tmax - tmin > 1)
197 				np->no_pps = 1;
198 			else {
199 				np->ts.tv_sec = tp->t_tv.tv_sec;
200 				np->ts.tv_nsec = tp->t_tv.tv_usec *
201 				    1000L;
202 				np->no_pps = 0;
203 			}
204 		}
205 		break;
206 	case '\r':
207 	case '\n':
208 		if (!np->sync) {
209 			np->cbuf[np->pos] = '\0';
210 			nmea_scan(np, tp);
211 			np->sync = 1;
212 		}
213 		break;
214 	default:
215 		if (!np->sync && np->pos < (NMEAMAX - 1))
216 			np->cbuf[np->pos++] = c;
217 		break;
218 	}
219 	/* pass data to termios */
220 	return linesw[TTYDISC].l_rint(c, tp);
221 }
222 
223 /* Scan the NMEA sentence just received. */
224 void
225 nmea_scan(struct nmea *np, struct tty *tp)
226 {
227 	int fldcnt = 0, cksum = 0, msgcksum, n;
228 	char *fld[MAXFLDS], *cs;
229 
230 	/* split into fields and calculate the checksum */
231 	fld[fldcnt++] = &np->cbuf[0];	/* message type */
232 	for (cs = NULL, n = 0; n < np->pos && cs == NULL; n++) {
233 		switch (np->cbuf[n]) {
234 		case '*':
235 			np->cbuf[n] = '\0';
236 			cs = &np->cbuf[n + 1];
237 			break;
238 		case ',':
239 			if (fldcnt < MAXFLDS) {
240 				cksum ^= np->cbuf[n];
241 				np->cbuf[n] = '\0';
242 				fld[fldcnt++] = &np->cbuf[n + 1];
243 			} else {
244 				DPRINTF(("nr of fields in %s sentence exceeds "
245 				    "maximum of %d\n", fld[0], MAXFLDS));
246 				return;
247 			}
248 			break;
249 		default:
250 			cksum ^= np->cbuf[n];
251 		}
252 	}
253 
254 	/* we only look at the GPRMC message */
255 	if (strcmp(fld[0], "GPRMC"))
256 		return;
257 
258 	/* if we have a checksum, verify it */
259 	if (cs != NULL) {
260 		msgcksum = 0;
261 		while (*cs) {
262 			if ((*cs >= '0' && *cs <= '9') ||
263 			    (*cs >= 'A' && *cs <= 'F')) {
264 				if (msgcksum)
265 					msgcksum <<= 4;
266 				if (*cs >= '0' && *cs<= '9')
267 					msgcksum += *cs - '0';
268 				else if (*cs >= 'A' && *cs <= 'F')
269 					msgcksum += 10 + *cs - 'A';
270 				cs++;
271 			} else {
272 				DPRINTF(("bad char %c in checksum\n", *cs));
273 				return;
274 			}
275 		}
276 		if (msgcksum != cksum) {
277 			DPRINTF(("checksum mismatch\n"));
278 			return;
279 		}
280 	}
281 	nmea_gprmc(np, tp, fld, fldcnt);
282 }
283 
284 /* Decode the recommended minimum specific GPS/TRANSIT data. */
285 void
286 nmea_gprmc(struct nmea *np, struct tty *tp, char *fld[], int fldcnt)
287 {
288 	int64_t date_nano, time_nano, nmea_now;
289 
290 	if (fldcnt != 12 && fldcnt != 13) {
291 		DPRINTF(("gprmc: field count mismatch, %d\n", fldcnt));
292 		return;
293 	}
294 	if (nmea_time_to_nano(fld[1], &time_nano)) {
295 		DPRINTF(("gprmc: illegal time, %s\n", fld[1]));
296 		return;
297 	}
298 	if (nmea_date_to_nano(fld[9], &date_nano)) {
299 		DPRINTF(("gprmc: illegal date, %s\n", fld[9]));
300 		return;
301 	}
302 	nmea_now = date_nano + time_nano;
303 	if (nmea_now <= np->last) {
304 		DPRINTF(("gprmc: time not monotonically increasing\n"));
305 		return;
306 	}
307 	np->last = nmea_now;
308 	np->gap = 0LL;
309 #ifdef NMEA_DEBUG
310 	if (np->time.status == SENSOR_S_UNKNOWN) {
311 		np->time.status = SENSOR_S_OK;
312 		timeout_add_sec(&np->nmea_tout, TRUSTTIME);
313 	}
314 	np->gapno = 0;
315 	if (nmeadebug > 0) {
316 		linesw[TTYDISC].l_rint('[', tp);
317 		linesw[TTYDISC].l_rint('C', tp);
318 		linesw[TTYDISC].l_rint(']', tp);
319 	}
320 #endif
321 
322 	np->time.value = np->ts.tv_sec * 1000000000LL +
323 	    np->ts.tv_nsec - nmea_now;
324 	np->time.tv.tv_sec = np->ts.tv_sec;
325 	np->time.tv.tv_usec = np->ts.tv_nsec / 1000L;
326 
327 	if (fldcnt != 13)
328 		strlcpy(np->time.desc, "GPS", sizeof(np->time.desc));
329 	else if (fldcnt == 13 && *fld[12] != np->mode) {
330 		np->mode = *fld[12];
331 		switch (np->mode) {
332 		case 'S':
333 			strlcpy(np->time.desc, "GPS simulated",
334 			    sizeof(np->time.desc));
335 			break;
336 		case 'E':
337 			strlcpy(np->time.desc, "GPS estimated",
338 			    sizeof(np->time.desc));
339 			break;
340 		case 'A':
341 			strlcpy(np->time.desc, "GPS autonomous",
342 			    sizeof(np->time.desc));
343 			break;
344 		case 'D':
345 			strlcpy(np->time.desc, "GPS differential",
346 			    sizeof(np->time.desc));
347 			break;
348 		case 'N':
349 			strlcpy(np->time.desc, "GPS invalid",
350 			    sizeof(np->time.desc));
351 			break;
352 		default:
353 			strlcpy(np->time.desc, "GPS unknown",
354 			    sizeof(np->time.desc));
355 			DPRINTF(("gprmc: unknown mode '%c'\n", np->mode));
356 		}
357 	}
358 #if NMEA_POS_IN_DESC
359 	nmea_degrees(np->time.desc, fld[3], *fld[4] == 'S' ? 1 : 0,
360 	    sizeof(np->time.desc));
361 	nmea_degrees(np->time.desc, fld[5], *fld[6] == 'W' ? 1 : 0,
362 	    sizeof(np->time.desc));
363 #endif
364 	switch (*fld[2]) {
365 	case 'A':	/* The GPS has a fix, (re)arm the timeout. */
366 		np->time.status = SENSOR_S_OK;
367 		np->signal.status = SENSOR_S_OK;
368 		timeout_add_sec(&np->nmea_tout, TRUSTTIME);
369 		break;
370 	case 'V':	/*
371 			 * The GPS indicates a warning status, do not add to
372 			 * the timeout, if the condition persist, the sensor
373 			 * will be degraded.  Signal the condition through
374 			 * the signal sensor.
375 			 */
376 		np->signal.status = SENSOR_S_WARN;
377 		break;
378 	}
379 
380 	/*
381 	 * If tty timestamping is requested, but no PPS signal is present, set
382 	 * the sensor state to CRITICAL.
383 	 */
384 	if (np->no_pps)
385 		np->time.status = SENSOR_S_CRIT;
386 }
387 
388 #ifdef NMEA_POS_IN_DESC
389 /* format a nmea position in the form DDDMM.MMMM to DDDdMM.MMm */
390 void
391 nmea_degrees(char *dst, char *src, int neg, size_t len)
392 {
393 	size_t dlen, ppos, rlen;
394 	int n;
395 	char *p;
396 
397 	for (dlen = 0; *dst; dlen++)
398 		dst++;
399 
400 	while (*src == '0')
401 		++src;	/* skip leading zeroes */
402 
403 	for (p = src, ppos = 0; *p; ppos++)
404 		if (*p++ == '.')
405 			break;
406 
407 	if (*p == '\0')
408 		return;	/* no decimal point */
409 
410 	/*
411 	 * we need at least room for a comma, an optional '-', the src data up
412 	 * to the decimal point, the decimal point itself, two digits after
413 	 * it and some additional characters:  an optional leading '0' in case
414 	 * there a no degrees in src, the 'd' degrees indicator, the 'm'
415 	 * minutes indicator and the terminating NUL character.
416 	 */
417 	rlen = dlen + ppos + 7;
418 	if (neg)
419 		rlen++;
420 	if (ppos < 3)
421 		rlen++;
422 	if (len < rlen)
423 		return;		/* not enough room in dst */
424 
425 	*dst++ = ',';
426 	if (neg)
427 		*dst++ = '-';
428 
429 	if (ppos < 3)
430 		*dst++ = '0';
431 
432 	for (n = 0; *src && n + 2 < ppos; n++)
433 		*dst++ = *src++;
434 	*dst++ = 'd';
435 	if (ppos == 0)
436 		*dst++ = '0';
437 
438 	for (; *src && n < (ppos + 3); n++)
439 		*dst++ = *src++;
440 	*dst++ = 'm';
441 	*dst = '\0';
442 }
443 #endif
444 
445 /*
446  * Convert a NMEA 0183 formatted date string to seconds since the epoch.
447  * The string must be of the form DDMMYY.
448  * Return 0 on success, -1 if illegal characters are encountered.
449  */
450 int
451 nmea_date_to_nano(char *s, int64_t *nano)
452 {
453 	struct clock_ymdhms ymd;
454 	time_t secs;
455 	char *p;
456 	int n;
457 
458 	/* make sure the input contains only numbers and is six digits long */
459 	for (n = 0, p = s; n < 6 && *p && *p >= '0' && *p <= '9'; n++, p++)
460 		;
461 	if (n != 6 || (*p != '\0'))
462 		return -1;
463 
464 	ymd.dt_year = 2000 + (s[4] - '0') * 10 + (s[5] - '0');
465 	ymd.dt_mon = (s[2] - '0') * 10 + (s[3] - '0');
466 	ymd.dt_day = (s[0] - '0') * 10 + (s[1] - '0');
467 	ymd.dt_hour = ymd.dt_min = ymd.dt_sec = 0;
468 
469 	secs = clock_ymdhms_to_secs(&ymd);
470 	*nano = secs * 1000000000LL;
471 	return 0;
472 }
473 
474 /*
475  * Convert NMEA 0183 formatted time string to nanoseconds since midnight.
476  * The string must be of the form HHMMSS[.[sss]] (e.g. 143724 or 143723.615).
477  * Return 0 on success, -1 if illegal characters are encountered.
478  */
479 int
480 nmea_time_to_nano(char *s, int64_t *nano)
481 {
482 	long fac = 36000L, div = 6L, secs = 0L, frac = 0L;
483 	char ul = '2';
484 	int n;
485 
486 	for (n = 0, secs = 0; fac && *s && *s >= '0' && *s <= ul; s++, n++) {
487 		secs += (*s - '0') * fac;
488 		div = 16 - div;
489 		fac /= div;
490 		switch (n) {
491 		case 0:
492 			if (*s <= '1')
493 				ul = '9';
494 			else
495 				ul = '3';
496 			break;
497 		case 1:
498 		case 3:
499 			ul = '5';
500 			break;
501 		case 2:
502 		case 4:
503 			ul = '9';
504 			break;
505 		}
506 	}
507 	if (fac)
508 		return -1;
509 
510 	/* Handle the fractions of a second, up to a maximum of 6 digits. */
511 	div = 1L;
512 	if (*s == '.') {
513 		for (++s; div < 1000000 && *s && *s >= '0' && *s <= '9'; s++) {
514 			frac *= 10;
515 			frac += (*s - '0');
516 			div *= 10;
517 		}
518 	}
519 
520 	if (*s != '\0')
521 		return -1;
522 
523 	*nano = secs * 1000000000LL + (int64_t)frac * (1000000000 / div);
524 	return 0;
525 }
526 
527 /*
528  * Degrade the sensor state if we received no NMEA sentences for more than
529  * TRUSTTIME seconds.
530  */
531 void
532 nmea_timeout(void *xnp)
533 {
534 	struct nmea *np = xnp;
535 
536 	if (np->time.status == SENSOR_S_OK) {
537 		np->time.status = SENSOR_S_WARN;
538 		/*
539 		 * further degrade in TRUSTTIME seconds if no new valid NMEA
540 		 * sentences are received.
541 		 */
542 		timeout_add_sec(&np->nmea_tout, TRUSTTIME);
543 	} else
544 		np->time.status = SENSOR_S_CRIT;
545 }
546