![[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|
Return to tty_nmea.c CVS log | Up to [local] / sys / kern |
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: }