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