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