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