Annotation of sys/kern/tty_nmea.c, Revision 1.1
1.1 ! nbrk 1: /* $OpenBSD: tty_nmea.c,v 1.21 2007/03/22 16:55:31 deraadt Exp $ */
! 2:
! 3: /*
! 4: * Copyright (c) 2006, 2007 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/queue.h>
! 24: #include <sys/proc.h>
! 25: #include <sys/malloc.h>
! 26: #include <sys/sensors.h>
! 27: #include <sys/tty.h>
! 28: #include <sys/conf.h>
! 29: #include <sys/time.h>
! 30:
! 31: #ifdef NMEA_DEBUG
! 32: #define DPRINTFN(n, x) do { if (nmeadebug > (n)) printf x; } while (0)
! 33: int nmeadebug = 0;
! 34: #else
! 35: #define DPRINTFN(n, x)
! 36: #endif
! 37: #define DPRINTF(x) DPRINTFN(0, x)
! 38:
! 39: int nmeaopen(dev_t, struct tty *);
! 40: int nmeaclose(struct tty *, int);
! 41: int nmeainput(int, struct tty *);
! 42: void nmeaattach(int);
! 43:
! 44: #define NMEAMAX 82
! 45: #define MAXFLDS 32
! 46:
! 47: int nmea_count; /* this is wrong, it should really be a SLIST */
! 48:
! 49: struct nmea {
! 50: char cbuf[NMEAMAX]; /* receive buffer */
! 51: struct ksensor time; /* the timedelta sensor */
! 52: struct ksensordev timedev;
! 53: struct timespec ts; /* current timestamp */
! 54: struct timespec lts; /* timestamp of last '$' seen */
! 55: int64_t gap; /* gap between two sentences */
! 56: #ifdef NMEA_DEBUG
! 57: int gapno;
! 58: #endif
! 59: int64_t last; /* last time rcvd */
! 60: int sync; /* if 1, waiting for '$' */
! 61: int pos; /* positon in rcv buffer */
! 62: int no_pps; /* no PPS although requested */
! 63: char mode; /* GPS mode */
! 64: };
! 65:
! 66: /* NMEA decoding */
! 67: void nmea_scan(struct nmea *, struct tty *);
! 68: void nmea_gprmc(struct nmea *, struct tty *, char *fld[], int fldcnt);
! 69:
! 70: /* date and time conversion */
! 71: int nmea_date_to_nano(char *s, int64_t *nano);
! 72: int nmea_time_to_nano(char *s, int64_t *nano);
! 73:
! 74: void
! 75: nmeaattach(int dummy)
! 76: {
! 77: }
! 78:
! 79: int
! 80: nmeaopen(dev_t dev, struct tty *tp)
! 81: {
! 82: struct proc *p = curproc;
! 83: struct nmea *np;
! 84: int error;
! 85:
! 86: if (tp->t_line == NMEADISC)
! 87: return ENODEV;
! 88: if ((error = suser(p, 0)) != 0)
! 89: return error;
! 90: np = malloc(sizeof(struct nmea), M_DEVBUF, M_WAITOK);
! 91: bzero(np, sizeof(*np));
! 92: snprintf(np->timedev.xname, sizeof(np->timedev.xname), "nmea%d",
! 93: nmea_count++);
! 94: np->time.status = SENSOR_S_UNKNOWN;
! 95: np->time.type = SENSOR_TIMEDELTA;
! 96: np->time.flags = SENSOR_FINVALID;
! 97: sensor_attach(&np->timedev, &np->time);
! 98: np->sync = 1;
! 99: tp->t_sc = (caddr_t)np;
! 100:
! 101: error = linesw[TTYDISC].l_open(dev, tp);
! 102: if (error) {
! 103: free(np, M_DEVBUF);
! 104: tp->t_sc = NULL;
! 105: } else
! 106: sensordev_install(&np->timedev);
! 107: return error;
! 108: }
! 109:
! 110: int
! 111: nmeaclose(struct tty *tp, int flags)
! 112: {
! 113: struct nmea *np = (struct nmea *)tp->t_sc;
! 114:
! 115: tp->t_line = TTYDISC; /* switch back to termios */
! 116: sensordev_deinstall(&np->timedev);
! 117: free(np, M_DEVBUF);
! 118: tp->t_sc = NULL;
! 119: nmea_count--;
! 120: return linesw[TTYDISC].l_close(tp, flags);
! 121: }
! 122:
! 123: /* collect NMEA sentence from tty */
! 124: int
! 125: nmeainput(int c, struct tty *tp)
! 126: {
! 127: struct nmea *np = (struct nmea *)tp->t_sc;
! 128: struct timespec ts;
! 129: int64_t gap;
! 130: long tmin, tmax;
! 131:
! 132: switch (c) {
! 133: case '$':
! 134: nanotime(&ts);
! 135: np->pos = np->sync = 0;
! 136: gap = (ts.tv_sec * 1000000000LL + ts.tv_nsec) -
! 137: (np->lts.tv_sec * 1000000000LL + np->lts.tv_nsec);
! 138:
! 139: np->lts.tv_sec = ts.tv_sec;
! 140: np->lts.tv_nsec = ts.tv_nsec;
! 141:
! 142: if (gap <= np->gap)
! 143: break;
! 144:
! 145: np->ts.tv_sec = ts.tv_sec;
! 146: np->ts.tv_nsec = ts.tv_nsec;
! 147:
! 148: #ifdef NMEA_DEBUG
! 149: if (nmeadebug > 0) {
! 150: linesw[TTYDISC].l_rint('[', tp);
! 151: linesw[TTYDISC].l_rint('0' + np->gapno++, tp);
! 152: linesw[TTYDISC].l_rint(']', tp);
! 153: }
! 154: #endif
! 155: np->gap = gap;
! 156:
! 157: /*
! 158: * If a tty timestamp is available, make sure its value is
! 159: * reasonable by comparing against the timestamp just taken.
! 160: * If they differ by more than 2 seconds, assume no PPS signal
! 161: * is present, note the fact, and keep using the timestamp
! 162: * value. When this happens, the sensor state is set to
! 163: * CRITICAL later when the GPRMC sentence is decoded.
! 164: */
! 165: if (tp->t_flags & (TS_TSTAMPDCDSET | TS_TSTAMPDCDCLR |
! 166: TS_TSTAMPCTSSET | TS_TSTAMPCTSCLR)) {
! 167: tmax = lmax(np->ts.tv_sec, tp->t_tv.tv_sec);
! 168: tmin = lmin(np->ts.tv_sec, tp->t_tv.tv_sec);
! 169: if (tmax - tmin > 1)
! 170: np->no_pps = 1;
! 171: else {
! 172: np->ts.tv_sec = tp->t_tv.tv_sec;
! 173: np->ts.tv_nsec = tp->t_tv.tv_usec *
! 174: 1000L;
! 175: np->no_pps = 0;
! 176: }
! 177: }
! 178: break;
! 179: case '\r':
! 180: case '\n':
! 181: if (!np->sync) {
! 182: np->cbuf[np->pos] = '\0';
! 183: nmea_scan(np, tp);
! 184: np->sync = 1;
! 185: }
! 186: break;
! 187: default:
! 188: if (!np->sync && np->pos < (NMEAMAX - 1))
! 189: np->cbuf[np->pos++] = c;
! 190: break;
! 191: }
! 192: /* pass data to termios */
! 193: return linesw[TTYDISC].l_rint(c, tp);
! 194: }
! 195:
! 196: /* Scan the NMEA sentence just received */
! 197: void
! 198: nmea_scan(struct nmea *np, struct tty *tp)
! 199: {
! 200: int fldcnt = 0, cksum = 0, msgcksum, n;
! 201: char *fld[MAXFLDS], *cs;
! 202:
! 203: /* split into fields and calculate the checksum */
! 204: fld[fldcnt++] = &np->cbuf[0]; /* message type */
! 205: for (cs = NULL, n = 0; n < np->pos && cs == NULL; n++) {
! 206: switch (np->cbuf[n]) {
! 207: case '*':
! 208: np->cbuf[n] = '\0';
! 209: cs = &np->cbuf[n + 1];
! 210: break;
! 211: case ',':
! 212: if (fldcnt < MAXFLDS) {
! 213: cksum ^= np->cbuf[n];
! 214: np->cbuf[n] = '\0';
! 215: fld[fldcnt++] = &np->cbuf[n + 1];
! 216: } else {
! 217: DPRINTF(("nr of fields in %s sentence exceeds "
! 218: "maximum of %d\n", fld[0], MAXFLDS));
! 219: return;
! 220: }
! 221: break;
! 222: default:
! 223: cksum ^= np->cbuf[n];
! 224: }
! 225: }
! 226:
! 227: /* if we have a checksum, verify it */
! 228: if (cs != NULL) {
! 229: msgcksum = 0;
! 230: while (*cs) {
! 231: if ((*cs >= '0' && *cs <= '9') ||
! 232: (*cs >= 'A' && *cs <= 'F')) {
! 233: if (msgcksum)
! 234: msgcksum <<= 4;
! 235: if (*cs >= '0' && *cs<= '9')
! 236: msgcksum += *cs - '0';
! 237: else if (*cs >= 'A' && *cs <= 'F')
! 238: msgcksum += 10 + *cs - 'A';
! 239: cs++;
! 240: } else {
! 241: DPRINTF(("bad char %c in checksum\n", *cs));
! 242: return;
! 243: }
! 244: }
! 245: if (msgcksum != cksum) {
! 246: DPRINTF(("checksum mismatch\n"));
! 247: return;
! 248: }
! 249: }
! 250:
! 251: /* check message type */
! 252: if (!strcmp(fld[0], "GPRMC"))
! 253: nmea_gprmc(np, tp, fld, fldcnt);
! 254: }
! 255:
! 256: /* Decode the recommended minimum specific GPS/TRANSIT data */
! 257: void
! 258: nmea_gprmc(struct nmea *np, struct tty *tp, char *fld[], int fldcnt)
! 259: {
! 260: int64_t date_nano, time_nano, nmea_now;
! 261:
! 262: if (fldcnt != 12 && fldcnt != 13) {
! 263: DPRINTF(("gprmc: field count mismatch, %d\n", fldcnt));
! 264: return;
! 265: }
! 266: if (nmea_time_to_nano(fld[1], &time_nano)) {
! 267: DPRINTF(("gprmc: illegal time, %s\n", fld[1]));
! 268: return;
! 269: }
! 270: if (nmea_date_to_nano(fld[9], &date_nano)) {
! 271: DPRINTF(("gprmc: illegal date, %s\n", fld[9]));
! 272: return;
! 273: }
! 274: nmea_now = date_nano + time_nano;
! 275: if (nmea_now <= np->last) {
! 276: DPRINTF(("gprmc: time not monotonically increasing\n"));
! 277: return;
! 278: }
! 279: np->last = nmea_now;
! 280: np->gap = 0LL;
! 281: #ifdef NMEA_DEBUG
! 282: np->gapno = 0;
! 283: if (nmeadebug > 0) {
! 284: linesw[TTYDISC].l_rint('[', tp);
! 285: linesw[TTYDISC].l_rint('C', tp);
! 286: linesw[TTYDISC].l_rint(']', tp);
! 287: }
! 288: #endif
! 289:
! 290: np->time.value = np->ts.tv_sec * 1000000000LL +
! 291: np->ts.tv_nsec - nmea_now;
! 292: np->time.tv.tv_sec = np->ts.tv_sec;
! 293: np->time.tv.tv_usec = np->ts.tv_nsec / 1000L;
! 294: if (np->time.status == SENSOR_S_UNKNOWN) {
! 295: np->time.status = SENSOR_S_OK;
! 296: np->time.flags &= ~SENSOR_FINVALID;
! 297: if (fldcnt != 13)
! 298: strlcpy(np->time.desc, "GPS", sizeof(np->time.desc));
! 299: }
! 300: if (fldcnt == 13 && *fld[12] != np->mode) {
! 301: np->mode = *fld[12];
! 302: switch (np->mode) {
! 303: case 'S':
! 304: strlcpy(np->time.desc, "GPS simulated",
! 305: sizeof(np->time.desc));
! 306: break;
! 307: case 'E':
! 308: strlcpy(np->time.desc, "GPS estimated",
! 309: sizeof(np->time.desc));
! 310: break;
! 311: case 'A':
! 312: strlcpy(np->time.desc, "GPS autonomous",
! 313: sizeof(np->time.desc));
! 314: break;
! 315: case 'D':
! 316: strlcpy(np->time.desc, "GPS differential",
! 317: sizeof(np->time.desc));
! 318: break;
! 319: case 'N':
! 320: strlcpy(np->time.desc, "GPS not valid",
! 321: sizeof(np->time.desc));
! 322: break;
! 323: default:
! 324: strlcpy(np->time.desc, "GPS unknown",
! 325: sizeof(np->time.desc));
! 326: DPRINTF(("gprmc: unknown mode '%c'\n", np->mode));
! 327: }
! 328: }
! 329: switch (*fld[2]) {
! 330: case 'A':
! 331: np->time.status = SENSOR_S_OK;
! 332: break;
! 333: case 'V':
! 334: np->time.status = SENSOR_S_WARN;
! 335: break;
! 336: default:
! 337: DPRINTF(("gprmc: unknown warning indication\n"));
! 338: }
! 339:
! 340: /*
! 341: * If tty timestamping is requested, but not PPS signal is present, set
! 342: * the sensor state to CRITICAL.
! 343: */
! 344: if (np->no_pps)
! 345: np->time.status = SENSOR_S_CRIT;
! 346: }
! 347:
! 348: /*
! 349: * Convert a NMEA 0183 formatted date string to seconds since the epoch.
! 350: * The string must be of the form DDMMYY.
! 351: * Return 0 on success, -1 if illegal characters are encountered.
! 352: */
! 353: int
! 354: nmea_date_to_nano(char *s, int64_t *nano)
! 355: {
! 356: struct clock_ymdhms ymd;
! 357: time_t secs;
! 358: char *p;
! 359: int n;
! 360:
! 361: /* make sure the input contains only numbers and is six digits long */
! 362: for (n = 0, p = s; n < 6 && *p && *p >= '0' && *p <= '9'; n++, p++)
! 363: ;
! 364: if (n != 6 || (*p != '\0'))
! 365: return -1;
! 366:
! 367: ymd.dt_year = 2000 + (s[4] - '0') * 10 + (s[5] - '0');
! 368: ymd.dt_mon = (s[2] - '0') * 10 + (s[3] - '0');
! 369: ymd.dt_day = (s[0] - '0') * 10 + (s[1] - '0');
! 370: ymd.dt_hour = ymd.dt_min = ymd.dt_sec = 0;
! 371:
! 372: secs = clock_ymdhms_to_secs(&ymd);
! 373: *nano = secs * 1000000000LL;
! 374: return 0;
! 375: }
! 376:
! 377: /*
! 378: * Convert NMEA 0183 formatted time string to nanoseconds since midnight.
! 379: * The string must be of the form HHMMSS[.[sss]] (e.g. 143724 or 143723.615).
! 380: * Return 0 on success, -1 if illegal characters are encountered.
! 381: */
! 382: int
! 383: nmea_time_to_nano(char *s, int64_t *nano)
! 384: {
! 385: long fac = 36000L, div = 6L, secs = 0L, frac = 0L;
! 386: char ul = '2';
! 387: int n;
! 388:
! 389: for (n = 0, secs = 0; fac && *s && *s >= '0' && *s <= ul; s++, n++) {
! 390: secs += (*s - '0') * fac;
! 391: div = 16 - div;
! 392: fac /= div;
! 393: switch (n) {
! 394: case 0:
! 395: if (*s <= '1')
! 396: ul = '9';
! 397: else
! 398: ul = '3';
! 399: break;
! 400: case 1:
! 401: case 3:
! 402: ul = '5';
! 403: break;
! 404: case 2:
! 405: case 4:
! 406: ul = '9';
! 407: break;
! 408: }
! 409: }
! 410: if (fac)
! 411: return -1;
! 412:
! 413: /* Handle the fractions of a second, up to a maximum of 6 digits. */
! 414: div = 1L;
! 415: if (*s == '.') {
! 416: for (++s; div < 1000000 && *s && *s >= '0' && *s <= '9'; s++) {
! 417: frac *= 10;
! 418: frac += (*s - '0');
! 419: div *= 10;
! 420: }
! 421: }
! 422:
! 423: if (*s != '\0')
! 424: return -1;
! 425:
! 426: *nano = secs * 1000000000LL + (int64_t)frac * (1000000000 / div);
! 427: return 0;
! 428: }
CVSweb
![[BACK]](/icons/back.gif){kind=icon}
Return to tty_nmea.c CVS log ![[TXT]](/icons/text.gif){kind=icon}
![[DIR]](/icons/dir.gif){kind=icon}
Up to [local] / sys / kern