xref: /openbsd-src/sys/kern/tty_nmea.c (revision a28daedfc357b214be5c701aa8ba8adb29a7f1c2) 
1 /*	$OpenBSD: tty_nmea.c,v 1.34 2009/04/26 02:25:36 cnst 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 
288 	if (fldcnt != 12 && fldcnt != 13) {
289 		DPRINTF(("gprmc: field count mismatch, %d\n", fldcnt));
290 		return;
291 	}
292 	if (nmea_time_to_nano(fld[1], &time_nano)) {
293 		DPRINTF(("gprmc: illegal time, %s\n", fld[1]));
294 		return;
295 	}
296 	if (nmea_date_to_nano(fld[9], &date_nano)) {
297 		DPRINTF(("gprmc: illegal date, %s\n", fld[9]));
298 		return;
299 	}
300 	nmea_now = date_nano + time_nano;
301 	if (nmea_now <= np->last) {
302 		DPRINTF(("gprmc: time not monotonically increasing\n"));
303 		return;
304 	}
305 	np->last = nmea_now;
306 	np->gap = 0LL;
307 #ifdef NMEA_DEBUG
308 	if (np->time.status == SENSOR_S_UNKNOWN) {
309 		np->time.status = SENSOR_S_OK;
310 		timeout_add_sec(&np->nmea_tout, TRUSTTIME);
311 	}
312 	np->gapno = 0;
313 	if (nmeadebug > 0) {
314 		linesw[TTYDISC].l_rint('[', tp);
315 		linesw[TTYDISC].l_rint('C', tp);
316 		linesw[TTYDISC].l_rint(']', tp);
317 	}
318 #endif
319 
320 	np->time.value = np->ts.tv_sec * 1000000000LL +
321 	    np->ts.tv_nsec - nmea_now;
322 	np->time.tv.tv_sec = np->ts.tv_sec;
323 	np->time.tv.tv_usec = np->ts.tv_nsec / 1000L;
324 
325 	if (fldcnt != 13)
326 		strlcpy(np->time.desc, "GPS", sizeof(np->time.desc));
327 	else if (fldcnt == 13 && *fld[12] != np->mode) {
328 		np->mode = *fld[12];
329 		switch (np->mode) {
330 		case 'S':
331 			strlcpy(np->time.desc, "GPS simulated",
332 			    sizeof(np->time.desc));
333 			break;
334 		case 'E':
335 			strlcpy(np->time.desc, "GPS estimated",
336 			    sizeof(np->time.desc));
337 			break;
338 		case 'A':
339 			strlcpy(np->time.desc, "GPS autonomous",
340 			    sizeof(np->time.desc));
341 			break;
342 		case 'D':
343 			strlcpy(np->time.desc, "GPS differential",
344 			    sizeof(np->time.desc));
345 			break;
346 		case 'N':
347 			strlcpy(np->time.desc, "GPS invalid",
348 			    sizeof(np->time.desc));
349 			break;
350 		default:
351 			strlcpy(np->time.desc, "GPS unknown",
352 			    sizeof(np->time.desc));
353 			DPRINTF(("gprmc: unknown mode '%c'\n", np->mode));
354 		}
355 	}
356 #if NMEA_POS_IN_DESC
357 	nmea_degrees(np->time.desc, fld[3], *fld[4] == 'S' ? 1 : 0,
358 	    sizeof(np->time.desc));
359 	nmea_degrees(np->time.desc, fld[5], *fld[6] == 'W' ? 1 : 0,
360 	    sizeof(np->time.desc));
361 #endif
362 	switch (*fld[2]) {
363 	case 'A':	/* The GPS has a fix, (re)arm the timeout. */
364 		np->time.status = SENSOR_S_OK;
365 		np->signal.status = SENSOR_S_OK;
366 		timeout_add_sec(&np->nmea_tout, TRUSTTIME);
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 	/*
379 	 * If tty timestamping is requested, but no PPS signal is present, set
380 	 * the sensor state to CRITICAL.
381 	 */
382 	if (np->no_pps)
383 		np->time.status = SENSOR_S_CRIT;
384 }
385 
386 #ifdef NMEA_POS_IN_DESC
387 /* format a nmea position in the form DDDMM.MMMM to DDDdMM.MMm */
388 void
389 nmea_degrees(char *dst, char *src, int neg, size_t len)
390 {
391 	size_t dlen, ppos, rlen;
392 	int n;
393 	char *p;
394 
395 	for (dlen = 0; *dst; dlen++)
396 		dst++;
397 
398 	while (*src == '0')
399 		++src;	/* skip leading zeroes */
400 
401 	for (p = src, ppos = 0; *p; ppos++)
402 		if (*p++ == '.')
403 			break;
404 
405 	if (*p == '\0')
406 		return;	/* no decimal point */
407 
408 	/*
409 	 * we need at least room for a comma, an optional '-', the src data up
410 	 * to the decimal point, the decimal point itself, two digits after
411 	 * it and some additional characters:  an optional leading '0' in case
412 	 * there a no degrees in src, the 'd' degrees indicator, the 'm'
413 	 * minutes indicator and the terminating NUL character.
414 	 */
415 	rlen = dlen + ppos + 7;
416 	if (neg)
417 		rlen++;
418 	if (ppos < 3)
419 		rlen++;
420 	if (len < rlen)
421 		return;		/* not enough room in dst */
422 
423 	*dst++ = ',';
424 	if (neg)
425 		*dst++ = '-';
426 
427 	if (ppos < 3)
428 		*dst++ = '0';
429 
430 	for (n = 0; *src && n + 2 < ppos; n++)
431 		*dst++ = *src++;
432 	*dst++ = 'd';
433 	if (ppos == 0)
434 		*dst++ = '0';
435 
436 	for (; *src && n < (ppos + 3); n++)
437 		*dst++ = *src++;
438 	*dst++ = 'm';
439 	*dst = '\0';
440 }
441 #endif
442 
443 /*
444  * Convert a NMEA 0183 formatted date string to seconds since the epoch.
445  * The string must be of the form DDMMYY.
446  * Return 0 on success, -1 if illegal characters are encountered.
447  */
448 int
449 nmea_date_to_nano(char *s, int64_t *nano)
450 {
451 	struct clock_ymdhms ymd;
452 	time_t secs;
453 	char *p;
454 	int n;
455 
456 	/* make sure the input contains only numbers and is six digits long */
457 	for (n = 0, p = s; n < 6 && *p && *p >= '0' && *p <= '9'; n++, p++)
458 		;
459 	if (n != 6 || (*p != '\0'))
460 		return -1;
461 
462 	ymd.dt_year = 2000 + (s[4] - '0') * 10 + (s[5] - '0');
463 	ymd.dt_mon = (s[2] - '0') * 10 + (s[3] - '0');
464 	ymd.dt_day = (s[0] - '0') * 10 + (s[1] - '0');
465 	ymd.dt_hour = ymd.dt_min = ymd.dt_sec = 0;
466 
467 	secs = clock_ymdhms_to_secs(&ymd);
468 	*nano = secs * 1000000000LL;
469 	return 0;
470 }
471 
472 /*
473  * Convert NMEA 0183 formatted time string to nanoseconds since midnight.
474  * The string must be of the form HHMMSS[.[sss]] (e.g. 143724 or 143723.615).
475  * Return 0 on success, -1 if illegal characters are encountered.
476  */
477 int
478 nmea_time_to_nano(char *s, int64_t *nano)
479 {
480 	long fac = 36000L, div = 6L, secs = 0L, frac = 0L;
481 	char ul = '2';
482 	int n;
483 
484 	for (n = 0, secs = 0; fac && *s && *s >= '0' && *s <= ul; s++, n++) {
485 		secs += (*s - '0') * fac;
486 		div = 16 - div;
487 		fac /= div;
488 		switch (n) {
489 		case 0:
490 			if (*s <= '1')
491 				ul = '9';
492 			else
493 				ul = '3';
494 			break;
495 		case 1:
496 		case 3:
497 			ul = '5';
498 			break;
499 		case 2:
500 		case 4:
501 			ul = '9';
502 			break;
503 		}
504 	}
505 	if (fac)
506 		return -1;
507 
508 	/* Handle the fractions of a second, up to a maximum of 6 digits. */
509 	div = 1L;
510 	if (*s == '.') {
511 		for (++s; div < 1000000 && *s && *s >= '0' && *s <= '9'; s++) {
512 			frac *= 10;
513 			frac += (*s - '0');
514 			div *= 10;
515 		}
516 	}
517 
518 	if (*s != '\0')
519 		return -1;
520 
521 	*nano = secs * 1000000000LL + (int64_t)frac * (1000000000 / div);
522 	return 0;
523 }
524 
525 /*
526  * Degrade the sensor state if we received no NMEA sentences for more than
527  * TRUSTTIME seconds.
528  */
529 void
530 nmea_timeout(void *xnp)
531 {
532 	struct nmea *np = xnp;
533 
534 	if (np->time.status == SENSOR_S_OK) {
535 		np->time.status = SENSOR_S_WARN;
536 		/*
537 		 * further degrade in TRUSTTIME seconds if no new valid NMEA
538 		 * sentences are received.
539 		 */
540 		timeout_add_sec(&np->nmea_tout, TRUSTTIME);
541 	} else
542 		np->time.status = SENSOR_S_CRIT;
543 }
544