root/kern/tty_nmea.c

DEFINITIONS

1. nmeaattach
2. nmeaopen
3. nmeaclose
4. nmeainput
5. nmea_scan
6. nmea_gprmc
7. nmea_date_to_nano
8. nmea_time_to_nano

    1 /*      $OpenBSD: tty_nmea.c,v 1.21 2007/03/22 16:55:31 deraadt Exp $ */
    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  */
   19 /* A tty line discipline to decode NMEA 0183 data to get the time. */
   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>
   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)
   39 int     nmeaopen(dev_t, struct tty *);
   40 int     nmeaclose(struct tty *, int);
   41 int     nmeainput(int, struct tty *);
   42 void    nmeaattach(int);
   44 #define NMEAMAX 82
   45 #define MAXFLDS 32
   47 int nmea_count; /* this is wrong, it should really be a SLIST */
   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 */
int64_t                 gap;            /* gap between two sentences */
   56 #ifdef NMEA_DEBUG
   57         int                     gapno;
   58 #endif
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 };
   66 /* NMEA decoding */
   67 void    nmea_scan(struct nmea *, struct tty *);
   68 void    nmea_gprmc(struct nmea *, struct tty *, char *fld[], int fldcnt);
   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);
   74 void
nmeaattach(int dummy)
   76 {
   77 }
   79 int
nmeaopen(dev_t dev, struct tty *tp)
   81 {
   82         struct proc *p = curproc;
   83         struct nmea *np;
   84         int error;
   86         if (tp->t_line == NMEADISC)
   87                 return ENODEV;
   88         if ((error = suser(p, 0)) != 0)
   89                 return error;
np = malloc(sizeof(struct nmea), M_DEVBUF, M_WAITOK);
bzero(np, sizeof(*np));
snprintf(np->timedev.xname, sizeof(np->timedev.xname), "nmea%d",
nmea_count++);
np->time.status = SENSOR_S_UNKNOWN;
np->time.type = SENSOR_TIMEDELTA;
np->time.flags = SENSOR_FINVALID;
sensor_attach(&np->timedev, &np->time);
np->sync = 1;
tp->t_sc = (caddr_t)np;
error = linesw[TTYDISC].l_open(dev, tp);
  102         if (error) {
free(np, M_DEVBUF);
tp->t_sc = NULL;
  105         } else
sensordev_install(&np->timedev);
  107         return error;
  108 }
  110 int
nmeaclose(struct tty *tp, int flags)
  112 {
  113         struct nmea *np = (struct nmea *)tp->t_sc;
tp->t_line = TTYDISC;   /* switch back to termios */
sensordev_deinstall(&np->timedev);
free(np, M_DEVBUF);
tp->t_sc = NULL;
nmea_count--;
  120         return linesw[TTYDISC].l_close(tp, flags);
  121 }
  123 /* collect NMEA sentence from tty */
  124 int
nmeainput(int c, struct tty *tp)
  126 {
  127         struct nmea *np = (struct nmea *)tp->t_sc;
  128         struct timespec ts;
int64_t gap;
  130         long tmin, tmax;
  132         switch (c) {
  133         case '$':
nanotime(&ts);
np->pos = np->sync = 0;
gap = (ts.tv_sec * 1000000000LL + ts.tv_nsec) -
  137                     (np->lts.tv_sec * 1000000000LL + np->lts.tv_nsec);
np->lts.tv_sec = ts.tv_sec;
np->lts.tv_nsec = ts.tv_nsec;
  142                 if (gap <= np->gap)
  143                         break;
np->ts.tv_sec = ts.tv_sec;
np->ts.tv_nsec = ts.tv_nsec;
  148 #ifdef NMEA_DEBUG
  149                 if (nmeadebug > 0) {
linesw[TTYDISC].l_rint('[', tp);
linesw[TTYDISC].l_rint('0' + np->gapno++, tp);
linesw[TTYDISC].l_rint(']', tp);
  153                 }
  154 #endif
np->gap = gap;
  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 |
TS_TSTAMPCTSSET | TS_TSTAMPCTSCLR)) {
tmax = lmax(np->ts.tv_sec, tp->t_tv.tv_sec);
tmin = lmin(np->ts.tv_sec, tp->t_tv.tv_sec);
  169                         if (tmax - tmin > 1)
np->no_pps = 1;
  171                         else {
np->ts.tv_sec = tp->t_tv.tv_sec;
np->ts.tv_nsec = tp->t_tv.tv_usec *
  174                                     1000L;
np->no_pps = 0;
  176                         }
  177                 }
  178                 break;
  179         case '\r':
  180         case '\n':
  181                 if (!np->sync) {
np->cbuf[np->pos] = '\0';
nmea_scan(np, tp);
np->sync = 1;
  185                 }
  186                 break;
  187         default:
  188                 if (!np->sync && np->pos < (NMEAMAX - 1))
np->cbuf[np->pos++] = c;
  190                 break;
  191         }
  192         /* pass data to termios */
  193         return linesw[TTYDISC].l_rint(c, tp);
  194 }
  196 /* Scan the NMEA sentence just received */
  197 void
nmea_scan(struct nmea *np, struct tty *tp)
  199 {
  200         int fldcnt = 0, cksum = 0, msgcksum, n;
  201         char *fld[MAXFLDS], *cs;
  203         /* split into fields and calculate the checksum */
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 '*':
np->cbuf[n] = '\0';
cs = &np->cbuf[n + 1];
  210                         break;
  211                 case ',':
  212                         if (fldcnt < MAXFLDS) {
cksum ^= np->cbuf[n];
np->cbuf[n] = '\0';
fld[fldcnt++] = &np->cbuf[n + 1];
  216                         } else {
DPRINTF(("nr of fields in %s sentence exceeds "
  218                                     "maximum of %d\n", fld[0], MAXFLDS));
  219                                 return;
  220                         }
  221                         break;
  222                 default:
cksum ^= np->cbuf[n];
  224                 }
  225         }
  227         /* if we have a checksum, verify it */
  228         if (cs != NULL) {
msgcksum = 0;
  230                 while (*cs) {
  231                         if ((*cs >= '0' && *cs <= '9') ||
  232                             (*cs >= 'A' && *cs <= 'F')) {
  233                                 if (msgcksum)
msgcksum <<= 4;
  235                                 if (*cs >= '0' && *cs<= '9')
msgcksum += *cs - '0';
  237                                 else if (*cs >= 'A' && *cs <= 'F')
msgcksum += 10 + *cs - 'A';
cs++;
  240                         } else {
DPRINTF(("bad char %c in checksum\n", *cs));
  242                                 return;
  243                         }
  244                 }
  245                 if (msgcksum != cksum) {
DPRINTF(("checksum mismatch\n"));
  247                         return;
  248                 }
  249         }
  251         /* check message type */
  252         if (!strcmp(fld[0], "GPRMC"))
nmea_gprmc(np, tp, fld, fldcnt);
  254 }
  256 /* Decode the recommended minimum specific GPS/TRANSIT data */
  257 void
nmea_gprmc(struct nmea *np, struct tty *tp, char *fld[], int fldcnt)
  259 {
int64_t date_nano, time_nano, nmea_now;
  262         if (fldcnt != 12 && fldcnt != 13) {
DPRINTF(("gprmc: field count mismatch, %d\n", fldcnt));
  264                 return;
  265         }
  266         if (nmea_time_to_nano(fld[1], &time_nano)) {
DPRINTF(("gprmc: illegal time, %s\n", fld[1]));
  268                 return;
  269         }
  270         if (nmea_date_to_nano(fld[9], &date_nano)) {
DPRINTF(("gprmc: illegal date, %s\n", fld[9]));
  272                 return;
  273         }
nmea_now = date_nano + time_nano;
  275         if (nmea_now <= np->last) {
DPRINTF(("gprmc: time not monotonically increasing\n"));
  277                 return;
  278         }
np->last = nmea_now;
np->gap = 0LL;
  281 #ifdef NMEA_DEBUG
np->gapno = 0;
  283         if (nmeadebug > 0) {
linesw[TTYDISC].l_rint('[', tp);
linesw[TTYDISC].l_rint('C', tp);
linesw[TTYDISC].l_rint(']', tp);
  287         }
  288 #endif
np->time.value = np->ts.tv_sec * 1000000000LL +
np->ts.tv_nsec - nmea_now;
np->time.tv.tv_sec = np->ts.tv_sec;
np->time.tv.tv_usec = np->ts.tv_nsec / 1000L;
  294         if (np->time.status == SENSOR_S_UNKNOWN) {
np->time.status = SENSOR_S_OK;
np->time.flags &= ~SENSOR_FINVALID;
  297                 if (fldcnt != 13)
strlcpy(np->time.desc, "GPS", sizeof(np->time.desc));
  299         }
  300         if (fldcnt == 13 && *fld[12] != np->mode) {
np->mode = *fld[12];
  302                 switch (np->mode) {
  303                 case 'S':
strlcpy(np->time.desc, "GPS simulated",
  305                             sizeof(np->time.desc));
  306                         break;
  307                 case 'E':
strlcpy(np->time.desc, "GPS estimated",
  309                             sizeof(np->time.desc));
  310                         break;
  311                 case 'A':
strlcpy(np->time.desc, "GPS autonomous",
  313                             sizeof(np->time.desc));
  314                         break;
  315                 case 'D':
strlcpy(np->time.desc, "GPS differential",
  317                             sizeof(np->time.desc));
  318                         break;
  319                 case 'N':
strlcpy(np->time.desc, "GPS not valid",
  321                             sizeof(np->time.desc));
  322                         break;
  323                 default:
strlcpy(np->time.desc, "GPS unknown",
  325                             sizeof(np->time.desc));
DPRINTF(("gprmc: unknown mode '%c'\n", np->mode));
  327                 }
  328         }
  329         switch (*fld[2]) {
  330         case 'A':
np->time.status = SENSOR_S_OK;
  332                 break;
  333         case 'V':
np->time.status = SENSOR_S_WARN;
  335                 break;
  336         default:
DPRINTF(("gprmc: unknown warning indication\n"));
  338         }
  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)
np->time.status = SENSOR_S_CRIT;
  346 }
  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
nmea_date_to_nano(char *s, int64_t *nano)
  355 {
  356         struct clock_ymdhms ymd;
time_t secs;
  358         char *p;
  359         int n;
  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;
ymd.dt_year = 2000 + (s[4] - '0') * 10 + (s[5] - '0');
ymd.dt_mon = (s[2] - '0') * 10 + (s[3] - '0');
ymd.dt_day = (s[0] - '0') * 10 + (s[1] - '0');
ymd.dt_hour = ymd.dt_min = ymd.dt_sec = 0;
secs = clock_ymdhms_to_secs(&ymd);
  373         *nano = secs * 1000000000LL;
  374         return 0;
  375 }
  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
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;
  389         for (n = 0, secs = 0; fac && *s && *s >= '0' && *s <= ul; s++, n++) {
secs += (*s - '0') * fac;
div = 16 - div;
fac /= div;
  393                 switch (n) {
  394                 case 0:
  395                         if (*s <= '1')
ul = '9';
  397                         else
ul = '3';
  399                         break;
  400                 case 1:
  401                 case 3:
ul = '5';
  403                         break;
  404                 case 2:
  405                 case 4:
ul = '9';
  407                         break;
  408                 }
  409         }
  410         if (fac)
  411                 return -1;
  413         /* Handle the fractions of a second, up to a maximum of 6 digits. */
div = 1L;
  415         if (*s == '.') {
  416                 for (++s; div < 1000000 && *s && *s >= '0' && *s <= '9'; s++) {
frac *= 10;
frac += (*s - '0');
div *= 10;
  420                 }
  421         }
  423         if (*s != '\0')
  424                 return -1;
  426         *nano = secs * 1000000000LL + (int64_t)frac * (1000000000 / div);
  427         return 0;
  428 }