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