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