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