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