1 /*
2 * refclock_gpsdjson.c - clock driver as GPSD JSON client
3 * Juergen Perlinger (perlinger@ntp.org)
4 * Feb 11, 2014 for the NTP project.
5 * The contents of 'html/copyright.html' apply.
6 *
7 * Heavily inspired by refclock_nmea.c
8 *
9 * Special thanks to Gary Miller and Hal Murray for their comments and
10 * ideas.
11 *
12 * Note: This will currently NOT work with Windows due to some
13 * limitations:
14 *
15 * - There is no GPSD for Windows. (There is an unofficial port to
16 * cygwin, but Windows is not officially supported.)
17 *
18 * - To work properly, this driver needs PPS and TPV/TOFF sentences
19 * from GPSD. I don't see how the cygwin port should deal with the
20 * PPS signal.
21 *
22 * - The device name matching must be done in a different way for
23 * Windows. (Can be done with COMxx matching, as done for NMEA.)
24 *
25 * Apart from those minor hickups, once GPSD has been fully ported to
26 * Windows, there's no reason why this should not work there ;-) If this
27 * is ever to happen at all is a different question.
28 *
29 * ---------------------------------------------------------------------
30 *
31 * This driver works slightly different from most others, as the PPS
32 * information (if available) is also coming from GPSD via the data
33 * connection. This makes using both the PPS data and the serial data
34 * easier, but OTOH it's not possible to use the ATOM driver to feed a
35 * raw PPS stream to the core of NTPD.
36 *
37 * To go around this, the driver can use a secondary clock unit
38 * (units>=128) that operate in tandem with the primary clock unit
39 * (unit%128). The primary clock unit does all the IO stuff and data
40 * decoding; if a a secondary unit is attached to a primary unit, this
41 * secondary unit is feed with the PPS samples only and can act as a PPS
42 * source to the clock selection.
43 *
44 * The drawback is that the primary unit must be present for the
45 * secondary unit to work.
46 *
47 * This design is a compromise to reduce the IO load for both NTPD and
48 * GPSD; it also ensures that data is transmitted and evaluated only
49 * once on the side of NTPD.
50 *
51 * ---------------------------------------------------------------------
52 *
53 * trouble shooting hints:
54 *
55 * Enable and check the clock stats. Check if there are bad replies;
56 * there should be none. If there are actually bad replies, then the
57 * driver cannot parse all JSON records from GPSD, and some record
58 * types are vital for the operation of the driver. This indicates a
59 * problem on the protocol level.
60 *
61 * When started on the command line with a debug level >= 2, the
62 * driver dumps the raw received data and the parser input to
63 * stdout. Since the debug level is global, NTPD starts to create a
64 * *lot* of output. It makes sense to pipe it through '(f)grep
65 * GPSD_JSON' before writing the result to disk.
66 *
67 * A bit less intrusive is using netcat or telnet to connect to GPSD
68 * and snoop what NTPD would get. If you try this, you have to send a
69 * WATCH command to GPSD:
70 *
71 * ?WATCH={"device":"/dev/gps0","enable":true,"json":true,"pps":true};<CRLF>
72 *
73 * should show you what GPSD has to say to NTPD. Replace "/dev/gps0"
74 * with the device link used by GPSD, if necessary.
75 */
76
77
78 #ifdef HAVE_CONFIG_H
79 #include <config.h>
80 #endif
81
82 #include "ntp_types.h"
83
84 #if defined(REFCLOCK) && defined(CLOCK_GPSDJSON) && !defined(SYS_WINNT)
85
86 /* =====================================================================
87 * Get the little JSMN library directly into our guts. Use the 'parent
88 * link' feature for maximum speed.
89 */
90 #define JSMN_PARENT_LINKS
91 #include "../libjsmn/jsmn.c"
92
93 /* =====================================================================
94 * JSON parsing stuff
95 */
96
97 #define JSMN_MAXTOK 350
98 #define INVALID_TOKEN (-1)
99
100 typedef struct json_ctx {
101 char * buf;
102 int ntok;
103 jsmntok_t tok[JSMN_MAXTOK];
104 } json_ctx;
105
106 typedef int tok_ref;
107
108 /* Not all targets have 'long long', and not all of them have 'strtoll'.
109 * Sigh. We roll our own integer number parser.
110 */
111 #ifdef HAVE_LONG_LONG
112 typedef signed long long int json_int;
113 typedef unsigned long long int json_uint;
114 #define JSON_INT_MAX LLONG_MAX
115 #define JSON_INT_MIN LLONG_MIN
116 #else
117 typedef signed long int json_int;
118 typedef unsigned long int json_uint;
119 #define JSON_INT_MAX LONG_MAX
120 #define JSON_INT_MIN LONG_MIN
121 #endif
122
123 /* =====================================================================
124 * header stuff we need
125 */
126
127 #include <netdb.h>
128 #include <unistd.h>
129 #include <fcntl.h>
130 #include <string.h>
131 #include <ctype.h>
132 #include <math.h>
133
134 #include <sys/types.h>
135 #include <sys/socket.h>
136 #include <sys/stat.h>
137 #include <netinet/tcp.h>
138
139 #if defined(HAVE_SYS_POLL_H)
140 # include <sys/poll.h>
141 #elif defined(HAVE_SYS_SELECT_H)
142 # include <sys/select.h>
143 #else
144 # error need poll() or select()
145 #endif
146
147 #include "ntpd.h"
148 #include "ntp_io.h"
149 #include "ntp_unixtime.h"
150 #include "ntp_refclock.h"
151 #include "ntp_stdlib.h"
152 #include "ntp_calendar.h"
153 #include "ntp_clockdev.h"
154 #include "timespecops.h"
155
156 /* get operation modes from mode word.
157
158 * + SERIAL (default) evaluates only serial time information ('STI') as
159 * provided by TPV and TOFF records. TPV evaluation suffers from a
160 * bigger jitter than TOFF, sine it does not contain the receive time
161 * from GPSD and therefore the receive time of NTPD must be
162 * substituted for it. The network latency makes this a second rate
163 * guess.
164 *
165 * If TOFF records are detected in the data stream, the timing
166 * information is gleaned from this record -- it contains the local
167 * receive time stamp from GPSD and therefore eliminates the
168 * transmission latency between GPSD and NTPD. The timing information
169 * from TPV is ignored once a TOFF is detected or expected.
170 *
171 * TPV is still used to check the fix status, so the driver can stop
172 * feeding samples when GPSD says that the time information is
173 * effectively unreliable.
174 *
175 * + STRICT means only feed clock samples when a valid STI/PPS pair is
176 * available. Combines the reference time from STI with the pulse time
177 * from PPS. Masks the serial data jitter as long PPS is available,
178 * but can rapidly deteriorate once PPS drops out.
179 *
180 * + AUTO tries to use STI/PPS pairs if available for some time, and if
181 * this fails for too long switches back to STI only until the PPS
182 * signal becomes available again. See the HTML docs for this driver
183 * about the gotchas and why this is not the default.
184 */
185 #define MODE_OP_MASK 0x03
186 #define MODE_OP_STI 0
187 #define MODE_OP_STRICT 1
188 #define MODE_OP_AUTO 2
189 #define MODE_OP_MAXVAL 2
190 #define MODE_OP_MODE(x) ((x) & MODE_OP_MASK)
191
192 #define PRECISION (-9) /* precision assumed (about 2 ms) */
193 #define PPS_PRECISION (-20) /* precision assumed (about 1 us) */
194 #define REFID "GPSD" /* reference id */
195 #define DESCRIPTION "GPSD JSON client clock" /* who we are */
196
197 #define MAX_PDU_LEN 8192 /* multi-GNSS reports can be HUGE */
198 #define TICKOVER_LOW 10
199 #define TICKOVER_HIGH 120
200 #define LOGTHROTTLE 3600
201
202 /* Primary channel PPS avilability dance:
203 * Every good PPS sample gets us a credit of PPS_INCCOUNT points, every
204 * bad/missing PPS sample costs us a debit of PPS_DECCOUNT points. When
205 * the account reaches the upper limit we change to a mode where only
206 * PPS-augmented samples are fed to the core; when the account drops to
207 * zero we switch to a mode where TPV-only timestamps are fed to the
208 * core.
209 * This reduces the chance of rapid alternation between raw and
210 * PPS-augmented time stamps.
211 */
212 #define PPS_MAXCOUNT 60 /* upper limit of account */
213 #define PPS_INCCOUNT 3 /* credit for good samples */
214 #define PPS_DECCOUNT 1 /* debit for bad samples */
215
216 /* The secondary (PPS) channel uses a different strategy to avoid old
217 * PPS samples in the median filter.
218 */
219 #define PPS2_MAXCOUNT 10
220
221 #ifndef BOOL
222 # define BOOL int
223 #endif
224 #ifndef TRUE
225 # define TRUE 1
226 #endif
227 #ifndef FALSE
228 # define FALSE 0
229 #endif
230
231 #define PROTO_VERSION(hi,lo) \
232 ((((uint32_t)(hi) << 16) & 0xFFFF0000u) | \
233 ((uint32_t)(lo) & 0x0FFFFu))
234
235 /* some local typedefs: The NTPD formatting style cries for short type
236 * names, and we provide them locally. Note:the suffix '_t' is reserved
237 * for the standard; I use a capital T instead.
238 */
239 typedef struct peer peerT;
240 typedef struct refclockproc clockprocT;
241 typedef struct addrinfo addrinfoT;
242
243 /* =====================================================================
244 * We use the same device name scheme as does the NMEA driver; since
245 * GPSD supports the same links, we can select devices by a fixed name.
246 */
247 static const char * s_dev_stem = "/dev/gps";
248
249 /* =====================================================================
250 * forward declarations for transfer vector and the vector itself
251 */
252
253 static void gpsd_init (void);
254 static int gpsd_start (int, peerT *);
255 static void gpsd_shutdown (int, peerT *);
256 static void gpsd_receive (struct recvbuf *);
257 static void gpsd_poll (int, peerT *);
258 static void gpsd_control (int, const struct refclockstat *,
259 struct refclockstat *, peerT *);
260 static void gpsd_timer (int, peerT *);
261
262 static int myasprintf(char**, char const*, ...) NTP_PRINTF(2, 3);
263
264 static void enter_opmode(peerT *peer, int mode);
265 static void leave_opmode(peerT *peer, int mode);
266
267 struct refclock refclock_gpsdjson = {
268 gpsd_start, /* start up driver */
269 gpsd_shutdown, /* shut down driver */
270 gpsd_poll, /* transmit poll message */
271 gpsd_control, /* fudge control */
272 gpsd_init, /* initialize driver */
273 noentry, /* buginfo */
274 gpsd_timer /* called once per second */
275 };
276
277 /* =====================================================================
278 * our local clock unit and data
279 */
280 struct gpsd_unit;
281 typedef struct gpsd_unit gpsd_unitT;
282
283 struct gpsd_unit {
284 /* links for sharing between master/slave units */
285 gpsd_unitT *next_unit;
286 size_t refcount;
287
288 /* data for the secondary PPS channel */
289 peerT *pps_peer;
290
291 /* unit and operation modes */
292 int unit;
293 int mode;
294 char *logname; /* cached name for log/print */
295 char * device; /* device name of unit */
296
297 /* current line protocol version */
298 uint32_t proto_version;
299
300 /* PPS time stamps primary + secondary channel */
301 l_fp pps_local; /* when we received the PPS message */
302 l_fp pps_stamp; /* related reference time */
303 l_fp pps_recvt; /* when GPSD detected the pulse */
304 l_fp pps_stamp2;/* related reference time (secondary) */
305 l_fp pps_recvt2;/* when GPSD detected the pulse (secondary)*/
306 int ppscount; /* PPS counter (primary unit) */
307 int ppscount2; /* PPS counter (secondary unit) */
308
309 /* TPV or TOFF serial time information */
310 l_fp sti_local; /* when we received the TPV/TOFF message */
311 l_fp sti_stamp; /* effective GPS time stamp */
312 l_fp sti_recvt; /* when GPSD got the fix */
313
314 /* precision estimates */
315 int16_t sti_prec; /* serial precision based on EPT */
316 int16_t pps_prec; /* PPS precision from GPSD or above */
317
318 /* fudge values for correction, mirrored as 'l_fp' */
319 l_fp pps_fudge; /* PPS fudge primary channel */
320 l_fp pps_fudge2; /* PPS fudge secondary channel */
321 l_fp sti_fudge; /* TPV/TOFF serial data fudge */
322
323 /* Flags to indicate available data */
324 int fl_nosync: 1; /* GPSD signals bad quality */
325 int fl_sti : 1; /* valid TPV/TOFF seen (have time) */
326 int fl_pps : 1; /* valid pulse seen */
327 int fl_pps2 : 1; /* valid pulse seen for PPS channel */
328 int fl_rawsti: 1; /* permit raw TPV/TOFF time stamps */
329 int fl_vers : 1; /* have protocol version */
330 int fl_watch : 1; /* watch reply seen */
331 /* protocol flags */
332 int pf_nsec : 1; /* have nanosec PPS info */
333 int pf_toff : 1; /* have TOFF record for timing */
334
335 /* admin stuff for sockets and device selection */
336 int fdt; /* current connecting socket */
337 addrinfoT * addr; /* next address to try */
338 u_int tickover; /* timeout countdown */
339 u_int tickpres; /* timeout preset */
340
341 /* tallies for the various events */
342 u_int tc_recv; /* received known records */
343 u_int tc_breply; /* bad replies / parsing errors */
344 u_int tc_nosync; /* TPV / sample cycles w/o fix */
345 u_int tc_sti_recv;/* received serial time info records */
346 u_int tc_sti_used;/* used --^-- */
347 u_int tc_pps_recv;/* received PPS timing info records */
348 u_int tc_pps_used;/* used --^-- */
349
350 /* log bloat throttle */
351 u_int logthrottle;/* seconds to next log slot */
352
353 /* The parse context for the current record */
354 json_ctx json_parse;
355
356 /* record assemby buffer and saved length */
357 int buflen;
358 char buffer[MAX_PDU_LEN];
359 };
360
361 /* =====================================================================
362 * static local helpers forward decls
363 */
364 static void gpsd_init_socket(peerT * const peer);
365 static void gpsd_test_socket(peerT * const peer);
366 static void gpsd_stop_socket(peerT * const peer);
367
368 static void gpsd_parse(peerT * const peer,
369 const l_fp * const rtime);
370 static BOOL convert_ascii_time(l_fp * fp, const char * gps_time);
371 static void save_ltc(clockprocT * const pp, const char * const tc);
372 static int syslogok(clockprocT * const pp, gpsd_unitT * const up);
373 static void log_data(peerT *peer, int level, const char *what,
374 const char *buf, size_t len);
375 static int16_t clamped_precision(int rawprec);
376
377 /* =====================================================================
378 * local / static stuff
379 */
380
381 static const char * const s_req_version =
382 "?VERSION;\r\n";
383
384 /* We keep a static list of network addresses for 'localhost:gpsd' or a
385 * fallback alias of it, and we try to connect to them in round-robin
386 * fashion. The service lookup is done during the driver init
387 * function to minmise the impact of 'getaddrinfo()'.
388 *
389 * Alas, the init function is called even if there are no clocks
390 * configured for this driver. So it makes sense to defer the logging of
391 * any errors or other notifications until the first clock unit is
392 * started -- otherwise there might be syslog entries from a driver that
393 * is not used at all.
394 */
395 static addrinfoT *s_gpsd_addr;
396 static gpsd_unitT *s_clock_units;
397
398 /* list of service/socket names we want to resolve against */
399 static const char * const s_svctab[][2] = {
400 { "localhost", "gpsd" },
401 { "localhost", "2947" },
402 { "127.0.0.1", "2947" },
403 { NULL, NULL }
404 };
405
406 /* list of address resolution errors and index of service entry that
407 * finally worked.
408 */
409 static int s_svcerr[sizeof(s_svctab)/sizeof(s_svctab[0])];
410 static int s_svcidx;
411
412 /* =====================================================================
413 * log throttling
414 */
415 static int/*BOOL*/
syslogok(clockprocT * const pp,gpsd_unitT * const up)416 syslogok(
417 clockprocT * const pp,
418 gpsd_unitT * const up)
419 {
420 int res = (0 != (pp->sloppyclockflag & CLK_FLAG3))
421 || (0 == up->logthrottle )
422 || (LOGTHROTTLE == up->logthrottle );
423 if (res)
424 up->logthrottle = LOGTHROTTLE;
425 return res;
426 }
427
428 /* =====================================================================
429 * the clock functions
430 */
431
432 /* ---------------------------------------------------------------------
433 * Init: This currently just gets the socket address for the GPS daemon
434 */
435 static void
gpsd_init(void)436 gpsd_init(void)
437 {
438 addrinfoT hints;
439 int rc, idx;
440
441 memset(s_svcerr, 0, sizeof(s_svcerr));
442 memset(&hints, 0, sizeof(hints));
443 hints.ai_family = AF_UNSPEC;
444 hints.ai_protocol = IPPROTO_TCP;
445 hints.ai_socktype = SOCK_STREAM;
446
447 for (idx = 0; s_svctab[idx][0] && !s_gpsd_addr; idx++) {
448 rc = getaddrinfo(s_svctab[idx][0], s_svctab[idx][1],
449 &hints, &s_gpsd_addr);
450 s_svcerr[idx] = rc;
451 if (0 == rc)
452 break;
453 s_gpsd_addr = NULL;
454 }
455 s_svcidx = idx;
456 }
457
458 /* ---------------------------------------------------------------------
459 * Init Check: flush pending log messages and check if we can proceed
460 */
461 static int/*BOOL*/
gpsd_init_check(void)462 gpsd_init_check(void)
463 {
464 int idx;
465
466 /* Check if there is something to log */
467 if (s_svcidx == 0)
468 return (s_gpsd_addr != NULL);
469
470 /* spool out the resolver errors */
471 for (idx = 0; idx < s_svcidx; ++idx) {
472 msyslog(LOG_WARNING,
473 "GPSD_JSON: failed to resolve '%s:%s', rc=%d (%s)",
474 s_svctab[idx][0], s_svctab[idx][1],
475 s_svcerr[idx], gai_strerror(s_svcerr[idx]));
476 }
477
478 /* check if it was fatal, or if we can proceed */
479 if (s_gpsd_addr == NULL)
480 msyslog(LOG_ERR, "%s",
481 "GPSD_JSON: failed to get socket address, giving up.");
482 else if (idx != 0)
483 msyslog(LOG_WARNING,
484 "GPSD_JSON: using '%s:%s' instead of '%s:%s'",
485 s_svctab[idx][0], s_svctab[idx][1],
486 s_svctab[0][0], s_svctab[0][1]);
487
488 /* make sure this gets logged only once and tell if we can
489 * proceed or not
490 */
491 s_svcidx = 0;
492 return (s_gpsd_addr != NULL);
493 }
494
495 /* ---------------------------------------------------------------------
496 * Start: allocate a unit pointer and set up the runtime data
497 */
498 static int
gpsd_start(int unit,peerT * peer)499 gpsd_start(
500 int unit,
501 peerT * peer)
502 {
503 clockprocT * const pp = peer->procptr;
504 gpsd_unitT * up;
505 gpsd_unitT ** uscan = &s_clock_units;
506 const char *tmpName;
507
508 struct stat sb;
509 char * devname = NULL;
510
511 /* check if we can proceed at all or if init failed */
512 if ( ! gpsd_init_check())
513 return FALSE;
514
515 /* search for matching unit */
516 while ((up = *uscan) != NULL && up->unit != (unit & 0x7F))
517 uscan = &up->next_unit;
518 if (up == NULL) {
519 /* alloc unit, add to list and increment use count ASAP. */
520 up = emalloc_zero(sizeof(*up));
521 *uscan = up;
522 ++up->refcount;
523
524 /* initialize the unit structure */
525 up->logname = estrdup(refnumtoa(&peer->srcadr));
526 up->unit = unit & 0x7F;
527 up->fdt = -1;
528 up->addr = s_gpsd_addr;
529 up->tickpres = TICKOVER_LOW;
530
531 /* Create the device name and check for a Character
532 * Device. It's assumed that GPSD was started with the
533 * same link, so the names match. (If this is not
534 * practicable, we will have to read the symlink, if
535 * any, so we can get the true device file.)
536 */
537 tmpName = clockdev_lookup(&peer->srcadr, 0);
538 if (NULL != tmpName) {
539 up->device = estrdup(tmpName);
540 } else if (-1 == myasprintf(&up->device, "%s%u", s_dev_stem, up->unit)) {
541 msyslog(LOG_ERR, "%s: clock device name too long",
542 up->logname);
543 goto dev_fail;
544 }
545 devname = up->device;
546 up->device = ntp_realpath(devname);
547 if (NULL == up->device) {
548 msyslog(LOG_ERR, "%s: '%s' has no absolute path",
549 up->logname, devname);
550 goto dev_fail;
551 }
552 free(devname);
553 devname = NULL;
554 if (-1 == lstat(up->device, &sb)) {
555 msyslog(LOG_ERR, "%s: '%s' not accessible",
556 up->logname, up->device);
557 goto dev_fail;
558 }
559 if (!S_ISCHR(sb.st_mode)) {
560 msyslog(LOG_ERR, "%s: '%s' is not a character device",
561 up->logname, up->device);
562 goto dev_fail;
563 }
564 } else {
565 /* All set up, just increment use count. */
566 ++up->refcount;
567 }
568
569 /* setup refclock processing */
570 pp->unitptr = (caddr_t)up;
571 pp->io.fd = -1;
572 pp->io.clock_recv = gpsd_receive;
573 pp->io.srcclock = peer;
574 pp->io.datalen = 0;
575 pp->a_lastcode[0] = '\0';
576 pp->lencode = 0;
577 pp->clockdesc = DESCRIPTION;
578 memcpy(&pp->refid, REFID, 4);
579
580 /* Initialize miscellaneous variables */
581 if (unit >= 128)
582 peer->precision = PPS_PRECISION;
583 else
584 peer->precision = PRECISION;
585
586 /* If the daemon name lookup failed, just give up now. */
587 if (NULL == up->addr) {
588 msyslog(LOG_ERR, "%s: no GPSD socket address, giving up",
589 up->logname);
590 goto dev_fail;
591 }
592
593 LOGIF(CLOCKINFO,
594 (LOG_NOTICE, "%s: startup, device is '%s'",
595 refnumtoa(&peer->srcadr), up->device));
596 up->mode = MODE_OP_MODE(peer->ttl);
597 if (up->mode > MODE_OP_MAXVAL)
598 up->mode = 0;
599 if (unit >= 128)
600 up->pps_peer = peer;
601 else
602 enter_opmode(peer, up->mode);
603 return TRUE;
604
605 dev_fail:
606 /* On failure, remove all UNIT ressources and declare defeat. */
607 free(devname);
608 INSIST (up);
609 if (!--up->refcount) {
610 *uscan = up->next_unit;
611 free(up->device);
612 free(up);
613 }
614
615 pp->unitptr = (caddr_t)NULL;
616 return FALSE;
617 }
618
619 /* ------------------------------------------------------------------ */
620
621 static void
gpsd_shutdown(int unit,peerT * peer)622 gpsd_shutdown(
623 int unit,
624 peerT * peer)
625 {
626 clockprocT * const pp = peer->procptr;
627 gpsd_unitT * const up = (gpsd_unitT *)pp->unitptr;
628 gpsd_unitT ** uscan = &s_clock_units;
629
630 UNUSED_ARG(unit);
631
632 /* The unit pointer might have been removed already. */
633 if (up == NULL)
634 return;
635
636 /* now check if we must close IO resources */
637 if (peer != up->pps_peer) {
638 if (-1 != pp->io.fd) {
639 DPRINTF(1, ("%s: closing clock, fd=%d\n",
640 up->logname, pp->io.fd));
641 io_closeclock(&pp->io);
642 pp->io.fd = -1;
643 }
644 if (up->fdt != -1)
645 close(up->fdt);
646 }
647 /* decrement use count and eventually remove this unit. */
648 if (!--up->refcount) {
649 /* unlink this unit */
650 while (*uscan != NULL)
651 if (*uscan == up)
652 *uscan = up->next_unit;
653 else
654 uscan = &(*uscan)->next_unit;
655 free(up->logname);
656 free(up->device);
657 free(up);
658 }
659 pp->unitptr = (caddr_t)NULL;
660 LOGIF(CLOCKINFO,
661 (LOG_NOTICE, "%s: shutdown", refnumtoa(&peer->srcadr)));
662 }
663
664 /* ------------------------------------------------------------------ */
665
666 static void
gpsd_receive(struct recvbuf * rbufp)667 gpsd_receive(
668 struct recvbuf * rbufp)
669 {
670 /* declare & init control structure ptrs */
671 peerT * const peer = rbufp->recv_peer;
672 clockprocT * const pp = peer->procptr;
673 gpsd_unitT * const up = (gpsd_unitT *)pp->unitptr;
674
675 const char *psrc, *esrc;
676 char *pdst, *edst, ch;
677
678 /* log the data stream, if this is enabled */
679 log_data(peer, 3, "recv", (const char*)rbufp->recv_buffer,
680 (size_t)rbufp->recv_length);
681
682
683 /* Since we're getting a raw stream data, we must assemble lines
684 * in our receive buffer. We can't use neither 'refclock_gtraw'
685 * not 'refclock_gtlin' here... We process chars until we reach
686 * an EoL (that is, line feed) but we truncate the message if it
687 * does not fit the buffer. GPSD might truncate messages, too,
688 * so dealing with truncated buffers is necessary anyway.
689 */
690 psrc = (const char*)rbufp->recv_buffer;
691 esrc = psrc + rbufp->recv_length;
692
693 pdst = up->buffer + up->buflen;
694 edst = up->buffer + sizeof(up->buffer) - 1; /* for trailing NUL */
695
696 while (psrc != esrc) {
697 ch = *psrc++;
698 if (ch == '\n') {
699 /* trim trailing whitespace & terminate buffer */
700 while (pdst != up->buffer && pdst[-1] <= ' ')
701 --pdst;
702 *pdst = '\0';
703 /* process data and reset buffer */
704 up->buflen = pdst - up->buffer;
705 gpsd_parse(peer, &rbufp->recv_time);
706 pdst = up->buffer;
707 } else if (pdst != edst) {
708 /* add next char, ignoring leading whitespace */
709 if (ch > ' ' || pdst != up->buffer)
710 *pdst++ = ch;
711 }
712 }
713 up->buflen = pdst - up->buffer;
714 up->tickover = TICKOVER_LOW;
715 }
716
717 /* ------------------------------------------------------------------ */
718
719 static void
poll_primary(peerT * const peer,clockprocT * const pp,gpsd_unitT * const up)720 poll_primary(
721 peerT * const peer ,
722 clockprocT * const pp ,
723 gpsd_unitT * const up )
724 {
725 if (pp->coderecv != pp->codeproc) {
726 /* all is well */
727 pp->lastref = pp->lastrec;
728 refclock_report(peer, CEVNT_NOMINAL);
729 refclock_receive(peer);
730 } else {
731 /* Not working properly, admit to it. If we have no
732 * connection to GPSD, declare the clock as faulty. If
733 * there were bad replies, this is handled as the major
734 * cause, and everything else is just a timeout.
735 */
736 peer->precision = PRECISION;
737 if (-1 == pp->io.fd)
738 refclock_report(peer, CEVNT_FAULT);
739 else if (0 != up->tc_breply)
740 refclock_report(peer, CEVNT_BADREPLY);
741 else
742 refclock_report(peer, CEVNT_TIMEOUT);
743 }
744
745 if (pp->sloppyclockflag & CLK_FLAG4)
746 mprintf_clock_stats(
747 &peer->srcadr,"%u %u %u %u %u %u %u",
748 up->tc_recv,
749 up->tc_breply, up->tc_nosync,
750 up->tc_sti_recv, up->tc_sti_used,
751 up->tc_pps_recv, up->tc_pps_used);
752
753 /* clear tallies for next round */
754 up->tc_breply = 0;
755 up->tc_recv = 0;
756 up->tc_nosync = 0;
757 up->tc_sti_recv = 0;
758 up->tc_sti_used = 0;
759 up->tc_pps_recv = 0;
760 up->tc_pps_used = 0;
761 }
762
763 static void
poll_secondary(peerT * const peer,clockprocT * const pp,gpsd_unitT * const up)764 poll_secondary(
765 peerT * const peer ,
766 clockprocT * const pp ,
767 gpsd_unitT * const up )
768 {
769 if (pp->coderecv != pp->codeproc) {
770 /* all is well */
771 pp->lastref = pp->lastrec;
772 refclock_report(peer, CEVNT_NOMINAL);
773 refclock_receive(peer);
774 } else {
775 peer->precision = PPS_PRECISION;
776 peer->flags &= ~FLAG_PPS;
777 refclock_report(peer, CEVNT_TIMEOUT);
778 }
779 }
780
781 static void
gpsd_poll(int unit,peerT * peer)782 gpsd_poll(
783 int unit,
784 peerT * peer)
785 {
786 clockprocT * const pp = peer->procptr;
787 gpsd_unitT * const up = (gpsd_unitT *)pp->unitptr;
788
789 ++pp->polls;
790 if (peer == up->pps_peer)
791 poll_secondary(peer, pp, up);
792 else
793 poll_primary(peer, pp, up);
794 }
795
796 /* ------------------------------------------------------------------ */
797
798 static void
gpsd_control(int unit,const struct refclockstat * in_st,struct refclockstat * out_st,peerT * peer)799 gpsd_control(
800 int unit,
801 const struct refclockstat * in_st,
802 struct refclockstat * out_st,
803 peerT * peer )
804 {
805 clockprocT * const pp = peer->procptr;
806 gpsd_unitT * const up = (gpsd_unitT *)pp->unitptr;
807
808 if (peer == up->pps_peer) {
809 DTOLFP(pp->fudgetime1, &up->pps_fudge2);
810 if ( ! (pp->sloppyclockflag & CLK_FLAG1))
811 peer->flags &= ~FLAG_PPS;
812 } else {
813 /* save preprocessed fudge times */
814 DTOLFP(pp->fudgetime1, &up->pps_fudge);
815 DTOLFP(pp->fudgetime2, &up->sti_fudge);
816
817 if (MODE_OP_MODE(up->mode ^ peer->ttl)) {
818 leave_opmode(peer, up->mode);
819 up->mode = MODE_OP_MODE(peer->ttl);
820 enter_opmode(peer, up->mode);
821 }
822 }
823 }
824
825 /* ------------------------------------------------------------------ */
826
827 static void
timer_primary(peerT * const peer,clockprocT * const pp,gpsd_unitT * const up)828 timer_primary(
829 peerT * const peer ,
830 clockprocT * const pp ,
831 gpsd_unitT * const up )
832 {
833 int rc;
834
835 /* This is used for timeout handling. Nothing that needs
836 * sub-second precison happens here, so receive/connect/retry
837 * timeouts are simply handled by a count down, and then we
838 * decide what to do by the socket values.
839 *
840 * Note that the timer stays at zero here, unless some of the
841 * functions set it to another value.
842 */
843 if (up->logthrottle)
844 --up->logthrottle;
845 if (up->tickover)
846 --up->tickover;
847 switch (up->tickover) {
848 case 4:
849 /* If we are connected to GPSD, try to get a live signal
850 * by querying the version. Otherwise just check the
851 * socket to become ready.
852 */
853 if (-1 != pp->io.fd) {
854 size_t rlen = strlen(s_req_version);
855 DPRINTF(2, ("%s: timer livecheck: '%s'\n",
856 up->logname, s_req_version));
857 log_data(peer, 2, "send", s_req_version, rlen);
858 rc = write(pp->io.fd, s_req_version, rlen);
859 (void)rc;
860 } else if (-1 != up->fdt) {
861 gpsd_test_socket(peer);
862 }
863 break;
864
865 case 0:
866 if (-1 != pp->io.fd)
867 gpsd_stop_socket(peer);
868 else if (-1 != up->fdt)
869 gpsd_test_socket(peer);
870 else if (NULL != s_gpsd_addr)
871 gpsd_init_socket(peer);
872 break;
873
874 default:
875 if (-1 == pp->io.fd && -1 != up->fdt)
876 gpsd_test_socket(peer);
877 }
878 }
879
880 static void
timer_secondary(peerT * const peer,clockprocT * const pp,gpsd_unitT * const up)881 timer_secondary(
882 peerT * const peer ,
883 clockprocT * const pp ,
884 gpsd_unitT * const up )
885 {
886 /* Reduce the count by one. Flush sample buffer and clear PPS
887 * flag when this happens.
888 */
889 up->ppscount2 = max(0, (up->ppscount2 - 1));
890 if (0 == up->ppscount2) {
891 if (pp->coderecv != pp->codeproc) {
892 refclock_report(peer, CEVNT_TIMEOUT);
893 pp->coderecv = pp->codeproc;
894 }
895 peer->flags &= ~FLAG_PPS;
896 }
897 }
898
899 static void
gpsd_timer(int unit,peerT * peer)900 gpsd_timer(
901 int unit,
902 peerT * peer)
903 {
904 clockprocT * const pp = peer->procptr;
905 gpsd_unitT * const up = (gpsd_unitT *)pp->unitptr;
906
907 if (peer == up->pps_peer)
908 timer_secondary(peer, pp, up);
909 else
910 timer_primary(peer, pp, up);
911 }
912
913 /* =====================================================================
914 * handle opmode switches
915 */
916
917 static void
enter_opmode(peerT * peer,int mode)918 enter_opmode(
919 peerT *peer,
920 int mode)
921 {
922 clockprocT * const pp = peer->procptr;
923 gpsd_unitT * const up = (gpsd_unitT *)pp->unitptr;
924
925 DPRINTF(1, ("%s: enter operation mode %d\n",
926 up->logname, MODE_OP_MODE(mode)));
927
928 if (MODE_OP_MODE(mode) == MODE_OP_AUTO) {
929 up->fl_rawsti = 0;
930 up->ppscount = PPS_MAXCOUNT / 2;
931 }
932 up->fl_pps = 0;
933 up->fl_sti = 0;
934 }
935
936 /* ------------------------------------------------------------------ */
937
938 static void
leave_opmode(peerT * peer,int mode)939 leave_opmode(
940 peerT *peer,
941 int mode)
942 {
943 clockprocT * const pp = peer->procptr;
944 gpsd_unitT * const up = (gpsd_unitT *)pp->unitptr;
945
946 DPRINTF(1, ("%s: leaving operation mode %d\n",
947 up->logname, MODE_OP_MODE(mode)));
948
949 if (MODE_OP_MODE(mode) == MODE_OP_AUTO) {
950 up->fl_rawsti = 0;
951 up->ppscount = 0;
952 }
953 up->fl_pps = 0;
954 up->fl_sti = 0;
955 }
956
957 /* =====================================================================
958 * operation mode specific evaluation
959 */
960
961 static void
add_clock_sample(peerT * const peer,clockprocT * const pp,l_fp stamp,l_fp recvt)962 add_clock_sample(
963 peerT * const peer ,
964 clockprocT * const pp ,
965 l_fp stamp,
966 l_fp recvt)
967 {
968 pp->lastref = stamp;
969 if (pp->coderecv == pp->codeproc)
970 refclock_report(peer, CEVNT_NOMINAL);
971 refclock_process_offset(pp, stamp, recvt, 0.0);
972 }
973
974 /* ------------------------------------------------------------------ */
975
976 static void
eval_strict(peerT * const peer,clockprocT * const pp,gpsd_unitT * const up)977 eval_strict(
978 peerT * const peer ,
979 clockprocT * const pp ,
980 gpsd_unitT * const up )
981 {
982 if (up->fl_sti && up->fl_pps) {
983 /* use TPV reference time + PPS receive time */
984 add_clock_sample(peer, pp, up->sti_stamp, up->pps_recvt);
985 peer->precision = up->pps_prec;
986 /* both packets consumed now... */
987 up->fl_pps = 0;
988 up->fl_sti = 0;
989 ++up->tc_sti_used;
990 }
991 }
992
993 /* ------------------------------------------------------------------ */
994 /* PPS processing for the secondary channel. GPSD provides us with full
995 * timing information, so there's no danger of PLL-locking to the wrong
996 * second. The belts and suspenders needed for the raw ATOM clock are
997 * unnecessary here.
998 */
999 static void
eval_pps_secondary(peerT * const peer,clockprocT * const pp,gpsd_unitT * const up)1000 eval_pps_secondary(
1001 peerT * const peer ,
1002 clockprocT * const pp ,
1003 gpsd_unitT * const up )
1004 {
1005 if (up->fl_pps2) {
1006 /* feed data */
1007 add_clock_sample(peer, pp, up->pps_stamp2, up->pps_recvt2);
1008 peer->precision = up->pps_prec;
1009 /* PPS peer flag logic */
1010 up->ppscount2 = min(PPS2_MAXCOUNT, (up->ppscount2 + 2));
1011 if ((PPS2_MAXCOUNT == up->ppscount2) &&
1012 (pp->sloppyclockflag & CLK_FLAG1) )
1013 peer->flags |= FLAG_PPS;
1014 /* mark time stamp as burned... */
1015 up->fl_pps2 = 0;
1016 ++up->tc_pps_used;
1017 }
1018 }
1019
1020 /* ------------------------------------------------------------------ */
1021
1022 static void
eval_serial(peerT * const peer,clockprocT * const pp,gpsd_unitT * const up)1023 eval_serial(
1024 peerT * const peer ,
1025 clockprocT * const pp ,
1026 gpsd_unitT * const up )
1027 {
1028 if (up->fl_sti) {
1029 add_clock_sample(peer, pp, up->sti_stamp, up->sti_recvt);
1030 peer->precision = up->sti_prec;
1031 /* mark time stamp as burned... */
1032 up->fl_sti = 0;
1033 ++up->tc_sti_used;
1034 }
1035 }
1036
1037 /* ------------------------------------------------------------------ */
1038 static void
eval_auto(peerT * const peer,clockprocT * const pp,gpsd_unitT * const up)1039 eval_auto(
1040 peerT * const peer ,
1041 clockprocT * const pp ,
1042 gpsd_unitT * const up )
1043 {
1044 /* If there's no TPV available, stop working here... */
1045 if (!up->fl_sti)
1046 return;
1047
1048 /* check how to handle STI+PPS: Can PPS be used to augment STI
1049 * (or vice versae), do we drop the sample because there is a
1050 * temporary missing PPS signal, or do we feed on STI time
1051 * stamps alone?
1052 *
1053 * Do a counter/threshold dance to decide how to proceed.
1054 */
1055 if (up->fl_pps) {
1056 up->ppscount = min(PPS_MAXCOUNT,
1057 (up->ppscount + PPS_INCCOUNT));
1058 if ((PPS_MAXCOUNT == up->ppscount) && up->fl_rawsti) {
1059 up->fl_rawsti = 0;
1060 msyslog(LOG_INFO,
1061 "%s: expect valid PPS from now",
1062 up->logname);
1063 }
1064 } else {
1065 up->ppscount = max(0, (up->ppscount - PPS_DECCOUNT));
1066 if ((0 == up->ppscount) && !up->fl_rawsti) {
1067 up->fl_rawsti = -1;
1068 msyslog(LOG_WARNING,
1069 "%s: use TPV alone from now",
1070 up->logname);
1071 }
1072 }
1073
1074 /* now eventually feed the sample */
1075 if (up->fl_rawsti)
1076 eval_serial(peer, pp, up);
1077 else
1078 eval_strict(peer, pp, up);
1079 }
1080
1081 /* =====================================================================
1082 * JSON parsing stuff
1083 */
1084
1085 /* ------------------------------------------------------------------ */
1086 /* Parse a decimal integer with a possible sign. Works like 'strtoll()'
1087 * or 'strtol()', but with a fixed base of 10 and without eating away
1088 * leading whitespace. For the error codes, the handling of the end
1089 * pointer and the return values see 'strtol()'.
1090 */
1091 static json_int
strtojint(const char * cp,char ** ep)1092 strtojint(
1093 const char *cp, char **ep)
1094 {
1095 json_uint accu, limit_lo, limit_hi;
1096 int flags; /* bit 0: overflow; bit 1: sign */
1097 const char * hold;
1098
1099 /* pointer union to circumvent a tricky/sticky const issue */
1100 union { const char * c; char * v; } vep;
1101
1102 /* store initial value of 'cp' -- see 'strtol()' */
1103 vep.c = cp;
1104
1105 /* Eat away an optional sign and set the limits accordingly: The
1106 * high limit is the maximum absolute value that can be returned,
1107 * and the low limit is the biggest value that does not cause an
1108 * overflow when multiplied with 10. Avoid negation overflows.
1109 */
1110 if (*cp == '-') {
1111 cp += 1;
1112 flags = 2;
1113 limit_hi = (json_uint)-(JSON_INT_MIN + 1) + 1;
1114 } else {
1115 cp += (*cp == '+');
1116 flags = 0;
1117 limit_hi = (json_uint)JSON_INT_MAX;
1118 }
1119 limit_lo = limit_hi / 10;
1120
1121 /* Now try to convert a sequence of digits. */
1122 hold = cp;
1123 accu = 0;
1124 while (isdigit(*(const u_char*)cp)) {
1125 flags |= (accu > limit_lo);
1126 accu = accu * 10 + (*(const u_char*)cp++ - '0');
1127 flags |= (accu > limit_hi);
1128 }
1129 /* Check for empty conversion (no digits seen). */
1130 if (hold != cp)
1131 vep.c = cp;
1132 else
1133 errno = EINVAL; /* accu is still zero */
1134 /* Check for range overflow */
1135 if (flags & 1) {
1136 errno = ERANGE;
1137 accu = limit_hi;
1138 }
1139 /* If possible, store back the end-of-conversion pointer */
1140 if (ep)
1141 *ep = vep.v;
1142 /* If negative, return the negated result if the accu is not
1143 * zero. Avoid negation overflows.
1144 */
1145 if ((flags & 2) && accu)
1146 return -(json_int)(accu - 1) - 1;
1147 else
1148 return (json_int)accu;
1149 }
1150
1151 /* ------------------------------------------------------------------ */
1152
1153 static tok_ref
json_token_skip(const json_ctx * ctx,tok_ref tid)1154 json_token_skip(
1155 const json_ctx * ctx,
1156 tok_ref tid)
1157 {
1158 if (tid >= 0 && tid < ctx->ntok) {
1159 int len = ctx->tok[tid].size;
1160 /* For arrays and objects, the size is the number of
1161 * ITEMS in the compound. Thats the number of objects in
1162 * the array, and the number of key/value pairs for
1163 * objects. In theory, the key must be a string, and we
1164 * could simply skip one token before skipping the
1165 * value, which can be anything. We're a bit paranoid
1166 * and lazy at the same time: We simply double the
1167 * number of tokens to skip and fall through into the
1168 * array processing when encountering an object.
1169 */
1170 switch (ctx->tok[tid].type) {
1171 case JSMN_OBJECT:
1172 len *= 2;
1173 /* FALLTHROUGH */
1174 case JSMN_ARRAY:
1175 for (++tid; len; --len)
1176 tid = json_token_skip(ctx, tid);
1177 break;
1178
1179 default:
1180 ++tid;
1181 break;
1182 }
1183 /* The next condition should never be true, but paranoia
1184 * prevails...
1185 */
1186 if (tid < 0 || tid > ctx->ntok)
1187 tid = ctx->ntok;
1188 }
1189 return tid;
1190 }
1191
1192 /* ------------------------------------------------------------------ */
1193
1194 static int
json_object_lookup(const json_ctx * ctx,tok_ref tid,const char * key,int what)1195 json_object_lookup(
1196 const json_ctx * ctx ,
1197 tok_ref tid ,
1198 const char * key ,
1199 int what)
1200 {
1201 int len;
1202
1203 if (tid < 0 || tid >= ctx->ntok ||
1204 ctx->tok[tid].type != JSMN_OBJECT)
1205 return INVALID_TOKEN;
1206
1207 len = ctx->tok[tid].size;
1208 for (++tid; len && tid+1 < ctx->ntok; --len) {
1209 if (ctx->tok[tid].type != JSMN_STRING) { /* Blooper! */
1210 tid = json_token_skip(ctx, tid); /* skip key */
1211 tid = json_token_skip(ctx, tid); /* skip val */
1212 } else if (strcmp(key, ctx->buf + ctx->tok[tid].start)) {
1213 tid = json_token_skip(ctx, tid+1); /* skip key+val */
1214 } else if (what < 0 || (u_int)what == ctx->tok[tid+1].type) {
1215 return tid + 1;
1216 } else {
1217 break;
1218 }
1219 /* if skipping ahead returned an error, bail out here. */
1220 if (tid < 0)
1221 break;
1222 }
1223 return INVALID_TOKEN;
1224 }
1225
1226 /* ------------------------------------------------------------------ */
1227
1228 static const char*
json_object_lookup_primitive(const json_ctx * ctx,tok_ref tid,const char * key)1229 json_object_lookup_primitive(
1230 const json_ctx * ctx,
1231 tok_ref tid,
1232 const char * key)
1233 {
1234 tid = json_object_lookup(ctx, tid, key, JSMN_PRIMITIVE);
1235 if (INVALID_TOKEN != tid)
1236 return ctx->buf + ctx->tok[tid].start;
1237 else
1238 return NULL;
1239 }
1240 /* ------------------------------------------------------------------ */
1241 /* look up a boolean value. This essentially returns a tribool:
1242 * 0->false, 1->true, (-1)->error/undefined
1243 */
1244 static int
json_object_lookup_bool(const json_ctx * ctx,tok_ref tid,const char * key)1245 json_object_lookup_bool(
1246 const json_ctx * ctx,
1247 tok_ref tid,
1248 const char * key)
1249 {
1250 const char *cp;
1251 cp = json_object_lookup_primitive(ctx, tid, key);
1252 switch ( cp ? *cp : '\0') {
1253 case 't': return 1;
1254 case 'f': return 0;
1255 default : return -1;
1256 }
1257 }
1258
1259 /* ------------------------------------------------------------------ */
1260
1261 static const char*
json_object_lookup_string(const json_ctx * ctx,tok_ref tid,const char * key)1262 json_object_lookup_string(
1263 const json_ctx * ctx,
1264 tok_ref tid,
1265 const char * key)
1266 {
1267 tid = json_object_lookup(ctx, tid, key, JSMN_STRING);
1268 if (INVALID_TOKEN != tid)
1269 return ctx->buf + ctx->tok[tid].start;
1270 return NULL;
1271 }
1272
1273 static const char*
json_object_lookup_string_default(const json_ctx * ctx,tok_ref tid,const char * key,const char * def)1274 json_object_lookup_string_default(
1275 const json_ctx * ctx,
1276 tok_ref tid,
1277 const char * key,
1278 const char * def)
1279 {
1280 tid = json_object_lookup(ctx, tid, key, JSMN_STRING);
1281 if (INVALID_TOKEN != tid)
1282 return ctx->buf + ctx->tok[tid].start;
1283 return def;
1284 }
1285
1286 /* ------------------------------------------------------------------ */
1287
1288 static json_int
json_object_lookup_int(const json_ctx * ctx,tok_ref tid,const char * key)1289 json_object_lookup_int(
1290 const json_ctx * ctx,
1291 tok_ref tid,
1292 const char * key)
1293 {
1294 json_int ret;
1295 const char * cp;
1296 char * ep;
1297
1298 cp = json_object_lookup_primitive(ctx, tid, key);
1299 if (NULL != cp) {
1300 ret = strtojint(cp, &ep);
1301 if (cp != ep && '\0' == *ep)
1302 return ret;
1303 } else {
1304 errno = EINVAL;
1305 }
1306 return 0;
1307 }
1308
1309 static json_int
json_object_lookup_int_default(const json_ctx * ctx,tok_ref tid,const char * key,json_int def)1310 json_object_lookup_int_default(
1311 const json_ctx * ctx,
1312 tok_ref tid,
1313 const char * key,
1314 json_int def)
1315 {
1316 json_int ret;
1317 const char * cp;
1318 char * ep;
1319
1320 cp = json_object_lookup_primitive(ctx, tid, key);
1321 if (NULL != cp) {
1322 ret = strtojint(cp, &ep);
1323 if (cp != ep && '\0' == *ep)
1324 return ret;
1325 }
1326 return def;
1327 }
1328
1329 /* ------------------------------------------------------------------ */
1330 #if 0 /* currently unused */
1331 static double
1332 json_object_lookup_float(
1333 const json_ctx * ctx,
1334 tok_ref tid,
1335 const char * key)
1336 {
1337 double ret;
1338 const char * cp;
1339 char * ep;
1340
1341 cp = json_object_lookup_primitive(ctx, tid, key);
1342 if (NULL != cp) {
1343 ret = strtod(cp, &ep);
1344 if (cp != ep && '\0' == *ep)
1345 return ret;
1346 } else {
1347 errno = EINVAL;
1348 }
1349 return 0.0;
1350 }
1351 #endif
1352
1353 static double
json_object_lookup_float_default(const json_ctx * ctx,tok_ref tid,const char * key,double def)1354 json_object_lookup_float_default(
1355 const json_ctx * ctx,
1356 tok_ref tid,
1357 const char * key,
1358 double def)
1359 {
1360 double ret;
1361 const char * cp;
1362 char * ep;
1363
1364 cp = json_object_lookup_primitive(ctx, tid, key);
1365 if (NULL != cp) {
1366 ret = strtod(cp, &ep);
1367 if (cp != ep && '\0' == *ep)
1368 return ret;
1369 }
1370 return def;
1371 }
1372
1373 /* ------------------------------------------------------------------ */
1374
1375 static BOOL
json_parse_record(json_ctx * ctx,char * buf,size_t len)1376 json_parse_record(
1377 json_ctx * ctx,
1378 char * buf,
1379 size_t len)
1380 {
1381 jsmn_parser jsm;
1382 int idx, rc;
1383
1384 jsmn_init(&jsm);
1385 rc = jsmn_parse(&jsm, buf, len, ctx->tok, JSMN_MAXTOK);
1386 if (rc <= 0)
1387 return FALSE;
1388 ctx->buf = buf;
1389 ctx->ntok = rc;
1390
1391 if (JSMN_OBJECT != ctx->tok[0].type)
1392 return FALSE; /* not object!?! */
1393
1394 /* Make all tokens NUL terminated by overwriting the
1395 * terminator symbol. Makes string compares and number parsing a
1396 * lot easier!
1397 */
1398 for (idx = 0; idx < ctx->ntok; ++idx)
1399 if (ctx->tok[idx].end > ctx->tok[idx].start)
1400 ctx->buf[ctx->tok[idx].end] = '\0';
1401 return TRUE;
1402 }
1403
1404
1405 /* =====================================================================
1406 * static local helpers
1407 */
1408 static BOOL
get_binary_time(l_fp * const dest,json_ctx * const jctx,const char * const time_name,const char * const frac_name,long fscale)1409 get_binary_time(
1410 l_fp * const dest ,
1411 json_ctx * const jctx ,
1412 const char * const time_name,
1413 const char * const frac_name,
1414 long fscale )
1415 {
1416 BOOL retv = FALSE;
1417 struct timespec ts;
1418
1419 errno = 0;
1420 ts.tv_sec = (time_t)json_object_lookup_int(jctx, 0, time_name);
1421 ts.tv_nsec = (long )json_object_lookup_int(jctx, 0, frac_name);
1422 if (0 == errno) {
1423 ts.tv_nsec *= fscale;
1424 *dest = tspec_stamp_to_lfp(ts);
1425 retv = TRUE;
1426 }
1427 return retv;
1428 }
1429
1430 /* ------------------------------------------------------------------ */
1431 /* Process a WATCH record
1432 *
1433 * Currently this is only used to recognise that the device is present
1434 * and that we're listed subscribers.
1435 */
1436 static void
process_watch(peerT * const peer,json_ctx * const jctx,const l_fp * const rtime)1437 process_watch(
1438 peerT * const peer ,
1439 json_ctx * const jctx ,
1440 const l_fp * const rtime)
1441 {
1442 clockprocT * const pp = peer->procptr;
1443 gpsd_unitT * const up = (gpsd_unitT *)pp->unitptr;
1444
1445 const char * path;
1446
1447 path = json_object_lookup_string(jctx, 0, "device");
1448 if (NULL == path || strcmp(path, up->device))
1449 return;
1450
1451 if (json_object_lookup_bool(jctx, 0, "enable") > 0 &&
1452 json_object_lookup_bool(jctx, 0, "json" ) > 0 )
1453 up->fl_watch = -1;
1454 else
1455 up->fl_watch = 0;
1456 DPRINTF(2, ("%s: process_watch, enabled=%d\n",
1457 up->logname, (up->fl_watch & 1)));
1458 }
1459
1460 /* ------------------------------------------------------------------ */
1461
1462 static void
process_version(peerT * const peer,json_ctx * const jctx,const l_fp * const rtime)1463 process_version(
1464 peerT * const peer ,
1465 json_ctx * const jctx ,
1466 const l_fp * const rtime)
1467 {
1468 clockprocT * const pp = peer->procptr;
1469 gpsd_unitT * const up = (gpsd_unitT *)pp->unitptr;
1470
1471 int len;
1472 char * buf;
1473 const char *revision;
1474 const char *release;
1475 uint16_t pvhi, pvlo;
1476
1477 /* get protocol version number */
1478 revision = json_object_lookup_string_default(
1479 jctx, 0, "rev", "(unknown)");
1480 release = json_object_lookup_string_default(
1481 jctx, 0, "release", "(unknown)");
1482 errno = 0;
1483 pvhi = (uint16_t)json_object_lookup_int(jctx, 0, "proto_major");
1484 pvlo = (uint16_t)json_object_lookup_int(jctx, 0, "proto_minor");
1485
1486 if (0 == errno) {
1487 if ( ! up->fl_vers)
1488 msyslog(LOG_INFO,
1489 "%s: GPSD revision=%s release=%s protocol=%u.%u",
1490 up->logname, revision, release,
1491 pvhi, pvlo);
1492 up->proto_version = PROTO_VERSION(pvhi, pvlo);
1493 up->fl_vers = -1;
1494 } else {
1495 if (syslogok(pp, up))
1496 msyslog(LOG_INFO,
1497 "%s: could not evaluate version data",
1498 up->logname);
1499 return;
1500 }
1501 /* With the 3.9 GPSD protocol, '*_musec' vanished from the PPS
1502 * record and was replace by '*_nsec'.
1503 */
1504 up->pf_nsec = -(up->proto_version >= PROTO_VERSION(3,9));
1505
1506 /* With the 3.10 protocol we can get TOFF records for better
1507 * timing information.
1508 */
1509 up->pf_toff = -(up->proto_version >= PROTO_VERSION(3,10));
1510
1511 /* request watch for our GPS device if not yet watched.
1512 *
1513 * The version string is also sent as a life signal, if we have
1514 * seen useable data. So if we're already watching the device,
1515 * skip the request.
1516 *
1517 * Reuse the input buffer, which is no longer needed in the
1518 * current cycle. Also assume that we can write the watch
1519 * request in one sweep into the socket; since we do not do
1520 * output otherwise, this should always work. (Unless the
1521 * TCP/IP window size gets lower than the length of the
1522 * request. We handle that when it happens.)
1523 */
1524 if (up->fl_watch)
1525 return;
1526
1527 /* The logon string is actually the ?WATCH command of GPSD,
1528 * using JSON data and selecting the GPS device name we created
1529 * from our unit number. We have an old and a newer version that
1530 * request PPS (and TOFF) transmission.
1531 */
1532 snprintf(up->buffer, sizeof(up->buffer),
1533 "?WATCH={\"device\":\"%s\",\"enable\":true,\"json\":true%s};\r\n",
1534 up->device, (up->pf_toff ? ",\"pps\":true" : ""));
1535 buf = up->buffer;
1536 len = strlen(buf);
1537 log_data(peer, 2, "send", buf, len);
1538 if (len != write(pp->io.fd, buf, len) && (syslogok(pp, up))) {
1539 /* Note: if the server fails to read our request, the
1540 * resulting data timeout will take care of the
1541 * connection!
1542 */
1543 msyslog(LOG_ERR, "%s: failed to write watch request (%m)",
1544 up->logname);
1545 }
1546 }
1547
1548 /* ------------------------------------------------------------------ */
1549
1550 static void
process_tpv(peerT * const peer,json_ctx * const jctx,const l_fp * const rtime)1551 process_tpv(
1552 peerT * const peer ,
1553 json_ctx * const jctx ,
1554 const l_fp * const rtime)
1555 {
1556 clockprocT * const pp = peer->procptr;
1557 gpsd_unitT * const up = (gpsd_unitT *)pp->unitptr;
1558
1559 const char * gps_time;
1560 int gps_mode;
1561 double ept;
1562 int xlog2;
1563
1564 gps_mode = (int)json_object_lookup_int_default(
1565 jctx, 0, "mode", 0);
1566
1567 gps_time = json_object_lookup_string(
1568 jctx, 0, "time");
1569
1570 /* accept time stamps only in 2d or 3d fix */
1571 if (gps_mode < 2 || NULL == gps_time) {
1572 /* receiver has no fix; tell about and avoid stale data */
1573 if ( ! up->pf_toff)
1574 ++up->tc_sti_recv;
1575 ++up->tc_nosync;
1576 up->fl_sti = 0;
1577 up->fl_pps = 0;
1578 up->fl_nosync = -1;
1579 return;
1580 }
1581 up->fl_nosync = 0;
1582
1583 /* convert clock and set resulting ref time, but only if the
1584 * TOFF sentence is *not* available
1585 */
1586 if ( ! up->pf_toff) {
1587 ++up->tc_sti_recv;
1588 /* save last time code to clock data */
1589 save_ltc(pp, gps_time);
1590 /* now parse the time string */
1591 if (convert_ascii_time(&up->sti_stamp, gps_time)) {
1592 DPRINTF(2, ("%s: process_tpv, stamp='%s',"
1593 " recvt='%s' mode=%u\n",
1594 up->logname,
1595 gmprettydate(&up->sti_stamp),
1596 gmprettydate(&up->sti_recvt),
1597 gps_mode));
1598
1599 /* have to use local receive time as substitute
1600 * for the real receive time: TPV does not tell
1601 * us.
1602 */
1603 up->sti_local = *rtime;
1604 up->sti_recvt = *rtime;
1605 L_SUB(&up->sti_recvt, &up->sti_fudge);
1606 up->fl_sti = -1;
1607 } else {
1608 ++up->tc_breply;
1609 up->fl_sti = 0;
1610 }
1611 }
1612
1613 /* Set the precision from the GPSD data
1614 * Use the ETP field for an estimation of the precision of the
1615 * serial data. If ETP is not available, use the default serial
1616 * data presion instead. (Note: The PPS branch has a different
1617 * precision estimation, since it gets the proper value directly
1618 * from GPSD!)
1619 */
1620 ept = json_object_lookup_float_default(jctx, 0, "ept", 2.0e-3);
1621 ept = frexp(fabs(ept)*0.70710678, &xlog2); /* ~ sqrt(0.5) */
1622 if (ept < 0.25)
1623 xlog2 = INT_MIN;
1624 if (ept > 2.0)
1625 xlog2 = INT_MAX;
1626 up->sti_prec = clamped_precision(xlog2);
1627 }
1628
1629 /* ------------------------------------------------------------------ */
1630
1631 static void
process_pps(peerT * const peer,json_ctx * const jctx,const l_fp * const rtime)1632 process_pps(
1633 peerT * const peer ,
1634 json_ctx * const jctx ,
1635 const l_fp * const rtime)
1636 {
1637 clockprocT * const pp = peer->procptr;
1638 gpsd_unitT * const up = (gpsd_unitT *)pp->unitptr;
1639
1640 int xlog2;
1641
1642 ++up->tc_pps_recv;
1643
1644 /* Bail out if there's indication that time sync is bad or
1645 * if we're explicitely requested to ignore PPS data.
1646 */
1647 if (up->fl_nosync)
1648 return;
1649
1650 up->pps_local = *rtime;
1651 /* Now grab the time values. 'clock_*' is the event time of the
1652 * pulse measured on the local system clock; 'real_*' is the GPS
1653 * reference time GPSD associated with the pulse.
1654 */
1655 if (up->pf_nsec) {
1656 if ( ! get_binary_time(&up->pps_recvt2, jctx,
1657 "clock_sec", "clock_nsec", 1))
1658 goto fail;
1659 if ( ! get_binary_time(&up->pps_stamp2, jctx,
1660 "real_sec", "real_nsec", 1))
1661 goto fail;
1662 } else {
1663 if ( ! get_binary_time(&up->pps_recvt2, jctx,
1664 "clock_sec", "clock_musec", 1000))
1665 goto fail;
1666 if ( ! get_binary_time(&up->pps_stamp2, jctx,
1667 "real_sec", "real_musec", 1000))
1668 goto fail;
1669 }
1670
1671 /* Try to read the precision field from the PPS record. If it's
1672 * not there, take the precision from the serial data.
1673 */
1674 xlog2 = json_object_lookup_int_default(
1675 jctx, 0, "precision", up->sti_prec);
1676 up->pps_prec = clamped_precision(xlog2);
1677
1678 /* Get fudged receive times for primary & secondary unit */
1679 up->pps_recvt = up->pps_recvt2;
1680 L_SUB(&up->pps_recvt , &up->pps_fudge );
1681 L_SUB(&up->pps_recvt2, &up->pps_fudge2);
1682 pp->lastrec = up->pps_recvt;
1683
1684 /* Map to nearest full second as reference time stamp for the
1685 * primary channel. Sanity checks are done in evaluation step.
1686 */
1687 up->pps_stamp = up->pps_recvt;
1688 L_ADDUF(&up->pps_stamp, 0x80000000u);
1689 up->pps_stamp.l_uf = 0;
1690
1691 if (NULL != up->pps_peer)
1692 save_ltc(up->pps_peer->procptr,
1693 gmprettydate(&up->pps_stamp2));
1694 DPRINTF(2, ("%s: PPS record processed,"
1695 " stamp='%s', recvt='%s'\n",
1696 up->logname,
1697 gmprettydate(&up->pps_stamp2),
1698 gmprettydate(&up->pps_recvt2)));
1699
1700 up->fl_pps = (0 != (pp->sloppyclockflag & CLK_FLAG2)) - 1;
1701 up->fl_pps2 = -1;
1702 return;
1703
1704 fail:
1705 DPRINTF(1, ("%s: PPS record processing FAILED\n",
1706 up->logname));
1707 ++up->tc_breply;
1708 }
1709
1710 /* ------------------------------------------------------------------ */
1711
1712 static void
process_toff(peerT * const peer,json_ctx * const jctx,const l_fp * const rtime)1713 process_toff(
1714 peerT * const peer ,
1715 json_ctx * const jctx ,
1716 const l_fp * const rtime)
1717 {
1718 clockprocT * const pp = peer->procptr;
1719 gpsd_unitT * const up = (gpsd_unitT *)pp->unitptr;
1720
1721 ++up->tc_sti_recv;
1722
1723 /* remember this! */
1724 up->pf_toff = -1;
1725
1726 /* bail out if there's indication that time sync is bad */
1727 if (up->fl_nosync)
1728 return;
1729
1730 if ( ! get_binary_time(&up->sti_recvt, jctx,
1731 "clock_sec", "clock_nsec", 1))
1732 goto fail;
1733 if ( ! get_binary_time(&up->sti_stamp, jctx,
1734 "real_sec", "real_nsec", 1))
1735 goto fail;
1736 L_SUB(&up->sti_recvt, &up->sti_fudge);
1737 up->sti_local = *rtime;
1738 up->fl_sti = -1;
1739
1740 save_ltc(pp, gmprettydate(&up->sti_stamp));
1741 DPRINTF(2, ("%s: TOFF record processed,"
1742 " stamp='%s', recvt='%s'\n",
1743 up->logname,
1744 gmprettydate(&up->sti_stamp),
1745 gmprettydate(&up->sti_recvt)));
1746 return;
1747
1748 fail:
1749 DPRINTF(1, ("%s: TOFF record processing FAILED\n",
1750 up->logname));
1751 ++up->tc_breply;
1752 }
1753
1754 /* ------------------------------------------------------------------ */
1755
1756 static void
gpsd_parse(peerT * const peer,const l_fp * const rtime)1757 gpsd_parse(
1758 peerT * const peer ,
1759 const l_fp * const rtime)
1760 {
1761 clockprocT * const pp = peer->procptr;
1762 gpsd_unitT * const up = (gpsd_unitT *)pp->unitptr;
1763
1764 const char * clsid;
1765
1766 DPRINTF(2, ("%s: gpsd_parse: time %s '%.*s'\n",
1767 up->logname, ulfptoa(rtime, 6),
1768 up->buflen, up->buffer));
1769
1770 /* See if we can grab anything potentially useful. JSMN does not
1771 * need a trailing NUL, but it needs the number of bytes to
1772 * process. */
1773 if (!json_parse_record(&up->json_parse, up->buffer, up->buflen)) {
1774 ++up->tc_breply;
1775 return;
1776 }
1777
1778 /* Now dispatch over the objects we know */
1779 clsid = json_object_lookup_string(&up->json_parse, 0, "class");
1780 if (NULL == clsid) {
1781 ++up->tc_breply;
1782 return;
1783 }
1784
1785 if (!strcmp("TPV", clsid))
1786 process_tpv(peer, &up->json_parse, rtime);
1787 else if (!strcmp("PPS", clsid))
1788 process_pps(peer, &up->json_parse, rtime);
1789 else if (!strcmp("TOFF", clsid))
1790 process_toff(peer, &up->json_parse, rtime);
1791 else if (!strcmp("VERSION", clsid))
1792 process_version(peer, &up->json_parse, rtime);
1793 else if (!strcmp("WATCH", clsid))
1794 process_watch(peer, &up->json_parse, rtime);
1795 else
1796 return; /* nothing we know about... */
1797 ++up->tc_recv;
1798
1799 /* if possible, feed the PPS side channel */
1800 if (up->pps_peer)
1801 eval_pps_secondary(
1802 up->pps_peer, up->pps_peer->procptr, up);
1803
1804 /* check PPS vs. STI receive times:
1805 * If STI is before PPS, then clearly the STI is too old. If PPS
1806 * is before STI by more than one second, then PPS is too old.
1807 * Weed out stale time stamps & flags.
1808 */
1809 if (up->fl_pps && up->fl_sti) {
1810 l_fp diff;
1811 diff = up->sti_local;
1812 L_SUB(&diff, &up->pps_local);
1813 if (diff.l_i > 0)
1814 up->fl_pps = 0; /* pps too old */
1815 else if (diff.l_i < 0)
1816 up->fl_sti = 0; /* serial data too old */
1817 }
1818
1819 /* dispatch to the mode-dependent processing functions */
1820 switch (up->mode) {
1821 default:
1822 case MODE_OP_STI:
1823 eval_serial(peer, pp, up);
1824 break;
1825
1826 case MODE_OP_STRICT:
1827 eval_strict(peer, pp, up);
1828 break;
1829
1830 case MODE_OP_AUTO:
1831 eval_auto(peer, pp, up);
1832 break;
1833 }
1834 }
1835
1836 /* ------------------------------------------------------------------ */
1837
1838 static void
gpsd_stop_socket(peerT * const peer)1839 gpsd_stop_socket(
1840 peerT * const peer)
1841 {
1842 clockprocT * const pp = peer->procptr;
1843 gpsd_unitT * const up = (gpsd_unitT *)pp->unitptr;
1844
1845 if (-1 != pp->io.fd) {
1846 if (syslogok(pp, up))
1847 msyslog(LOG_INFO,
1848 "%s: closing socket to GPSD, fd=%d",
1849 up->logname, pp->io.fd);
1850 else
1851 DPRINTF(1, ("%s: closing socket to GPSD, fd=%d\n",
1852 up->logname, pp->io.fd));
1853 io_closeclock(&pp->io);
1854 pp->io.fd = -1;
1855 }
1856 up->tickover = up->tickpres;
1857 up->tickpres = min(up->tickpres + 5, TICKOVER_HIGH);
1858 up->fl_vers = 0;
1859 up->fl_sti = 0;
1860 up->fl_pps = 0;
1861 up->fl_watch = 0;
1862 }
1863
1864 /* ------------------------------------------------------------------ */
1865
1866 static void
gpsd_init_socket(peerT * const peer)1867 gpsd_init_socket(
1868 peerT * const peer)
1869 {
1870 clockprocT * const pp = peer->procptr;
1871 gpsd_unitT * const up = (gpsd_unitT *)pp->unitptr;
1872 addrinfoT * ai;
1873 int rc;
1874 int ov;
1875
1876 /* draw next address to try */
1877 if (NULL == up->addr)
1878 up->addr = s_gpsd_addr;
1879 ai = up->addr;
1880 up->addr = ai->ai_next;
1881
1882 /* try to create a matching socket */
1883 up->fdt = socket(
1884 ai->ai_family, ai->ai_socktype, ai->ai_protocol);
1885 if (-1 == up->fdt) {
1886 if (syslogok(pp, up))
1887 msyslog(LOG_ERR,
1888 "%s: cannot create GPSD socket: %m",
1889 up->logname);
1890 goto no_socket;
1891 }
1892
1893 /* Make sure the socket is non-blocking. Connect/reconnect and
1894 * IO happen in an event-driven environment, and synchronous
1895 * operations wreak havoc on that.
1896 */
1897 rc = fcntl(up->fdt, F_SETFL, O_NONBLOCK, 1);
1898 if (-1 == rc) {
1899 if (syslogok(pp, up))
1900 msyslog(LOG_ERR,
1901 "%s: cannot set GPSD socket to non-blocking: %m",
1902 up->logname);
1903 goto no_socket;
1904 }
1905 /* Disable nagling. The way both GPSD and NTPD handle the
1906 * protocol makes it record-oriented, and in most cases
1907 * complete records (JSON serialised objects) will be sent in
1908 * one sweep. Nagling gives not much advantage but adds another
1909 * delay, which can worsen the situation for some packets.
1910 */
1911 ov = 1;
1912 rc = setsockopt(up->fdt, IPPROTO_TCP, TCP_NODELAY,
1913 (void *)&ov, sizeof(ov));
1914 if (-1 == rc) {
1915 if (syslogok(pp, up))
1916 msyslog(LOG_INFO,
1917 "%s: cannot disable TCP nagle: %m",
1918 up->logname);
1919 }
1920
1921 /* Start a non-blocking connect. There might be a synchronous
1922 * connection result we have to handle.
1923 */
1924 rc = connect(up->fdt, ai->ai_addr, ai->ai_addrlen);
1925 if (-1 == rc) {
1926 if (errno == EINPROGRESS) {
1927 DPRINTF(1, ("%s: async connect pending, fd=%d\n",
1928 up->logname, up->fdt));
1929 return;
1930 }
1931
1932 if (syslogok(pp, up))
1933 msyslog(LOG_ERR,
1934 "%s: cannot connect GPSD socket: %m",
1935 up->logname);
1936 goto no_socket;
1937 }
1938
1939 /* We had a successful synchronous connect, so we add the
1940 * refclock processing ASAP. We still have to wait for the
1941 * version string and apply the watch command later on, but we
1942 * might as well get the show on the road now.
1943 */
1944 DPRINTF(1, ("%s: new socket connection, fd=%d\n",
1945 up->logname, up->fdt));
1946
1947 pp->io.fd = up->fdt;
1948 up->fdt = -1;
1949 if (0 == io_addclock(&pp->io)) {
1950 if (syslogok(pp, up))
1951 msyslog(LOG_ERR,
1952 "%s: failed to register with I/O engine",
1953 up->logname);
1954 goto no_socket;
1955 }
1956
1957 return;
1958
1959 no_socket:
1960 if (-1 != pp->io.fd)
1961 close(pp->io.fd);
1962 if (-1 != up->fdt)
1963 close(up->fdt);
1964 pp->io.fd = -1;
1965 up->fdt = -1;
1966 up->tickover = up->tickpres;
1967 up->tickpres = min(up->tickpres + 5, TICKOVER_HIGH);
1968 }
1969
1970 /* ------------------------------------------------------------------ */
1971
1972 static void
gpsd_test_socket(peerT * const peer)1973 gpsd_test_socket(
1974 peerT * const peer)
1975 {
1976 clockprocT * const pp = peer->procptr;
1977 gpsd_unitT * const up = (gpsd_unitT *)pp->unitptr;
1978
1979 int ec, rc;
1980 socklen_t lc;
1981
1982 /* Check if the non-blocking connect was finished by testing the
1983 * socket for writeability. Use the 'poll()' API if available
1984 * and 'select()' otherwise.
1985 */
1986 DPRINTF(2, ("%s: check connect, fd=%d\n",
1987 up->logname, up->fdt));
1988
1989 #if defined(HAVE_SYS_POLL_H)
1990 {
1991 struct pollfd pfd;
1992
1993 pfd.events = POLLOUT;
1994 pfd.fd = up->fdt;
1995 rc = poll(&pfd, 1, 0);
1996 if (1 != rc || !(pfd.revents & POLLOUT))
1997 return;
1998 }
1999 #elif defined(HAVE_SYS_SELECT_H)
2000 {
2001 struct timeval tout;
2002 fd_set wset;
2003
2004 memset(&tout, 0, sizeof(tout));
2005 FD_ZERO(&wset);
2006 FD_SET(up->fdt, &wset);
2007 rc = select(up->fdt+1, NULL, &wset, NULL, &tout);
2008 if (0 == rc || !(FD_ISSET(up->fdt, &wset)))
2009 return;
2010 }
2011 #else
2012 # error Blooper! That should have been found earlier!
2013 #endif
2014
2015 /* next timeout is a full one... */
2016 up->tickover = TICKOVER_LOW;
2017
2018 /* check for socket error */
2019 ec = 0;
2020 lc = sizeof(ec);
2021 rc = getsockopt(up->fdt, SOL_SOCKET, SO_ERROR, (void *)&ec, &lc);
2022 if (-1 == rc || 0 != ec) {
2023 const char *errtxt;
2024 if (0 == ec)
2025 ec = errno;
2026 errtxt = strerror(ec);
2027 if (syslogok(pp, up))
2028 msyslog(LOG_ERR,
2029 "%s: async connect to GPSD failed,"
2030 " fd=%d, ec=%d(%s)",
2031 up->logname, up->fdt, ec, errtxt);
2032 else
2033 DPRINTF(1, ("%s: async connect to GPSD failed,"
2034 " fd=%d, ec=%d(%s)\n",
2035 up->logname, up->fdt, ec, errtxt));
2036 goto no_socket;
2037 } else {
2038 DPRINTF(1, ("%s: async connect to GPSD succeeded, fd=%d\n",
2039 up->logname, up->fdt));
2040 }
2041
2042 /* swap socket FDs, and make sure the clock was added */
2043 pp->io.fd = up->fdt;
2044 up->fdt = -1;
2045 if (0 == io_addclock(&pp->io)) {
2046 if (syslogok(pp, up))
2047 msyslog(LOG_ERR,
2048 "%s: failed to register with I/O engine",
2049 up->logname);
2050 goto no_socket;
2051 }
2052 return;
2053
2054 no_socket:
2055 if (-1 != up->fdt) {
2056 DPRINTF(1, ("%s: closing socket, fd=%d\n",
2057 up->logname, up->fdt));
2058 close(up->fdt);
2059 }
2060 up->fdt = -1;
2061 up->tickover = up->tickpres;
2062 up->tickpres = min(up->tickpres + 5, TICKOVER_HIGH);
2063 }
2064
2065 /* =====================================================================
2066 * helper stuff
2067 */
2068
2069 /* -------------------------------------------------------------------
2070 * store a properly clamped precision value
2071 */
2072 static int16_t
clamped_precision(int rawprec)2073 clamped_precision(
2074 int rawprec)
2075 {
2076 if (rawprec > 0)
2077 rawprec = 0;
2078 if (rawprec < -32)
2079 rawprec = -32;
2080 return (int16_t)rawprec;
2081 }
2082
2083 /* -------------------------------------------------------------------
2084 * Convert a GPSD timestamp (ISO8601 Format) to an l_fp
2085 */
2086 static BOOL
convert_ascii_time(l_fp * fp,const char * gps_time)2087 convert_ascii_time(
2088 l_fp * fp ,
2089 const char * gps_time)
2090 {
2091 char *ep;
2092 struct tm gd;
2093 struct timespec ts;
2094 uint32_t dw;
2095
2096 /* Use 'strptime' to take the brunt of the work, then parse
2097 * the fractional part manually, starting with a digit weight of
2098 * 10^8 nanoseconds.
2099 */
2100 ts.tv_nsec = 0;
2101 ep = strptime(gps_time, "%Y-%m-%dT%H:%M:%S", &gd);
2102 if (NULL == ep)
2103 return FALSE; /* could not parse the mandatory stuff! */
2104 if (*ep == '.') {
2105 dw = 100000000u;
2106 while (isdigit(*(u_char*)++ep)) {
2107 ts.tv_nsec += (*(u_char*)ep - '0') * dw;
2108 dw /= 10u;
2109 }
2110 }
2111 if (ep[0] != 'Z' || ep[1] != '\0')
2112 return FALSE; /* trailing garbage */
2113
2114 /* Now convert the whole thing into a 'l_fp'. We do not use
2115 * 'mkgmtime()' since its not standard and going through the
2116 * calendar routines is not much effort, either.
2117 */
2118 ts.tv_sec = (ntpcal_tm_to_rd(&gd) - DAY_NTP_STARTS) * SECSPERDAY
2119 + ntpcal_tm_to_daysec(&gd);
2120 *fp = tspec_intv_to_lfp(ts);
2121
2122 return TRUE;
2123 }
2124
2125 /* -------------------------------------------------------------------
2126 * Save the last timecode string, making sure it's properly truncated
2127 * if necessary and NUL terminated in any case.
2128 */
2129 static void
save_ltc(clockprocT * const pp,const char * const tc)2130 save_ltc(
2131 clockprocT * const pp,
2132 const char * const tc)
2133 {
2134 size_t len = 0;
2135
2136 if (tc) {
2137 len = strlen(tc);
2138 if (len >= sizeof(pp->a_lastcode))
2139 len = sizeof(pp->a_lastcode) - 1;
2140 memcpy(pp->a_lastcode, tc, len);
2141 }
2142 pp->lencode = (u_short)len;
2143 pp->a_lastcode[len] = '\0';
2144 }
2145
2146 /* -------------------------------------------------------------------
2147 * asprintf replacement... it's not available everywhere...
2148 */
2149 static int
myasprintf(char ** spp,char const * fmt,...)2150 myasprintf(
2151 char ** spp,
2152 char const * fmt,
2153 ... )
2154 {
2155 size_t alen, plen;
2156
2157 alen = 32;
2158 *spp = NULL;
2159 do {
2160 va_list va;
2161
2162 alen += alen;
2163 free(*spp);
2164 *spp = (char*)malloc(alen);
2165 if (NULL == *spp)
2166 return -1;
2167
2168 va_start(va, fmt);
2169 plen = (size_t)vsnprintf(*spp, alen, fmt, va);
2170 va_end(va);
2171 } while (plen >= alen);
2172
2173 return (int)plen;
2174 }
2175
2176 /* -------------------------------------------------------------------
2177 * dump a raw data buffer
2178 */
2179
2180 static char *
add_string(char * dp,char * ep,const char * sp)2181 add_string(
2182 char *dp,
2183 char *ep,
2184 const char *sp)
2185 {
2186 while (dp != ep && *sp)
2187 *dp++ = *sp++;
2188 return dp;
2189 }
2190
2191 static void
log_data(peerT * peer,int level,const char * what,const char * buf,size_t len)2192 log_data(
2193 peerT *peer,
2194 int level,
2195 const char *what,
2196 const char *buf ,
2197 size_t len )
2198 {
2199 /* we're running single threaded with regards to the clocks. */
2200 static char s_lbuf[2048];
2201
2202 clockprocT * const pp = peer->procptr;
2203 gpsd_unitT * const up = (gpsd_unitT *)pp->unitptr;
2204
2205 if (debug >= level) {
2206 const char *sptr = buf;
2207 const char *stop = buf + len;
2208 char *dptr = s_lbuf;
2209 char *dtop = s_lbuf + sizeof(s_lbuf) - 1; /* for NUL */
2210
2211 while (sptr != stop && dptr != dtop) {
2212 u_char uch = (u_char)*sptr++;
2213 if (uch == '\\') {
2214 dptr = add_string(dptr, dtop, "\\\\");
2215 } else if (isprint(uch)) {
2216 *dptr++ = (char)uch;
2217 } else {
2218 char fbuf[6];
2219 snprintf(fbuf, sizeof(fbuf), "\\%03o", uch);
2220 dptr = add_string(dptr, dtop, fbuf);
2221 }
2222 }
2223 *dptr = '\0';
2224 mprintf("%s[%s]: '%s'\n", up->logname, what, s_lbuf);
2225 }
2226 }
2227
2228
2229 #else
2230 NONEMPTY_TRANSLATION_UNIT
2231 #endif /* REFCLOCK && CLOCK_GPSDJSON */
2232