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