1 /* 2 * refclock_gpsdjson.c - clock driver as GPSD JSON client 3 * Juergen Perlinger (perlinger@ntp.org) 4 * Feb 11, 2014 for the NTP project. 5 * The contents of 'html/copyright.html' apply. 6 * 7 * Heavily inspired by refclock_nmea.c 8 * 9 * Special thanks to Gary Miller and Hal Murray for their comments and 10 * ideas. 11 * 12 * Note: This will currently NOT work with Windows due to some 13 * limitations: 14 * 15 * - There is no GPSD for Windows. (There is an unofficial port to 16 * cygwin, but Windows is not officially supported.) 17 * 18 * - To work properly, this driver needs PPS and TPV/TOFF sentences 19 * from GPSD. I don't see how the cygwin port should deal with the 20 * PPS signal. 21 * 22 * - The device name matching must be done in a different way for 23 * Windows. (Can be done with COMxx matching, as done for NMEA.) 24 * 25 * Apart from those minor hickups, once GPSD has been fully ported to 26 * Windows, there's no reason why this should not work there ;-) If this 27 * is ever to happen at all is a different question. 28 * 29 * --------------------------------------------------------------------- 30 * 31 * This driver works slightly different from most others, as the PPS 32 * information (if available) is also coming from GPSD via the data 33 * connection. This makes using both the PPS data and the serial data 34 * easier, but OTOH it's not possible to use the ATOM driver to feed a 35 * raw PPS stream to the core of NTPD. 36 * 37 * To go around this, the driver can use a secondary clock unit 38 * (units>=128) that operate in tandem with the primary clock unit 39 * (unit%128). The primary clock unit does all the IO stuff and data 40 * decoding; if a a secondary unit is attached to a primary unit, this 41 * secondary unit is feed with the PPS samples only and can act as a PPS 42 * source to the clock selection. 43 * 44 * The drawback is that the primary unit must be present for the 45 * secondary unit to work. 46 * 47 * This design is a compromise to reduce the IO load for both NTPD and 48 * GPSD; it also ensures that data is transmitted and evaluated only 49 * once on the side of NTPD. 50 * 51 * --------------------------------------------------------------------- 52 * 53 * trouble shooting hints: 54 * 55 * Enable and check the clock stats. Check if there are bad replies; 56 * there should be none. If there are actually bad replies, then the 57 * driver cannot parse all JSON records from GPSD, and some record 58 * types are vital for the operation of the driver. This indicates a 59 * problem on the protocol level. 60 * 61 * When started on the command line with a debug level >= 2, the 62 * driver dumps the raw received data and the parser input to 63 * stdout. Since the debug level is global, NTPD starts to create a 64 * *lot* of output. It makes sense to pipe it through '(f)grep 65 * GPSD_JSON' before writing the result to disk. 66 * 67 * A bit less intrusive is using netcat or telnet to connect to GPSD 68 * and snoop what NTPD would get. If you try this, you have to send a 69 * WATCH command to GPSD: 70 * 71 * ?WATCH={"device":"/dev/gps0","enable":true,"json":true,"pps":true};<CRLF> 72 * 73 * should show you what GPSD has to say to NTPD. Replace "/dev/gps0" 74 * with the device link used by GPSD, if necessary. 75 */ 76 77 78 #ifdef HAVE_CONFIG_H 79 #include <config.h> 80 #endif 81 82 #include "ntp_types.h" 83 84 #if defined(REFCLOCK) && defined(CLOCK_GPSDJSON) && !defined(SYS_WINNT) 85 86 /* ===================================================================== 87 * Get the little JSMN library directly into our guts. Use the 'parent 88 * link' feature for maximum speed. 89 */ 90 #define JSMN_PARENT_LINKS 91 #include "../libjsmn/jsmn.c" 92 93 /* ===================================================================== 94 * JSON parsing stuff 95 */ 96 97 #define JSMN_MAXTOK 350 98 #define INVALID_TOKEN (-1) 99 100 typedef struct json_ctx { 101 char * buf; 102 int ntok; 103 jsmntok_t tok[JSMN_MAXTOK]; 104 } json_ctx; 105 106 typedef int tok_ref; 107 108 /* Not all targets have 'long long', and not all of them have 'strtoll'. 109 * Sigh. We roll our own integer number parser. 110 */ 111 #ifdef HAVE_LONG_LONG 112 typedef signed long long int json_int; 113 typedef unsigned long long int json_uint; 114 #define JSON_INT_MAX LLONG_MAX 115 #define JSON_INT_MIN LLONG_MIN 116 #else 117 typedef signed long int json_int; 118 typedef unsigned long int json_uint; 119 #define JSON_INT_MAX LONG_MAX 120 #define JSON_INT_MIN LONG_MIN 121 #endif 122 123 /* ===================================================================== 124 * header stuff we need 125 */ 126 127 #include <netdb.h> 128 #include <unistd.h> 129 #include <fcntl.h> 130 #include <string.h> 131 #include <ctype.h> 132 #include <math.h> 133 134 #include <sys/types.h> 135 #include <sys/socket.h> 136 #include <sys/stat.h> 137 #include <netinet/tcp.h> 138 139 #if defined(HAVE_SYS_POLL_H) 140 # include <sys/poll.h> 141 #elif defined(HAVE_SYS_SELECT_H) 142 # include <sys/select.h> 143 #else 144 # error need poll() or select() 145 #endif 146 147 #include "ntpd.h" 148 #include "ntp_io.h" 149 #include "ntp_unixtime.h" 150 #include "ntp_refclock.h" 151 #include "ntp_stdlib.h" 152 #include "ntp_calendar.h" 153 #include "timespecops.h" 154 155 /* get operation modes from mode word. 156 157 * + SERIAL (default) evaluates only serial time information ('STI') as 158 * provided by TPV and TOFF records. TPV evaluation suffers from a 159 * bigger jitter than TOFF, sine it does not contain the receive time 160 * from GPSD and therefore the receive time of NTPD must be 161 * substituted for it. The network latency makes this a second rate 162 * guess. 163 * 164 * If TOFF records are detected in the data stream, the timing 165 * information is gleaned from this record -- it contains the local 166 * receive time stamp from GPSD and therefore eliminates the 167 * transmission latency between GPSD and NTPD. The timing information 168 * from TPV is ignored once a TOFF is detected or expected. 169 * 170 * TPV is still used to check the fix status, so the driver can stop 171 * feeding samples when GPSD says that the time information is 172 * effectively unreliable. 173 * 174 * + STRICT means only feed clock samples when a valid STI/PPS pair is 175 * available. Combines the reference time from STI with the pulse time 176 * from PPS. Masks the serial data jitter as long PPS is available, 177 * but can rapidly deteriorate once PPS drops out. 178 * 179 * + AUTO tries to use STI/PPS pairs if available for some time, and if 180 * this fails for too long switches back to STI only until the PPS 181 * signal becomes available again. See the HTML docs for this driver 182 * about the gotchas and why this is not the default. 183 */ 184 #define MODE_OP_MASK 0x03 185 #define MODE_OP_STI 0 186 #define MODE_OP_STRICT 1 187 #define MODE_OP_AUTO 2 188 #define MODE_OP_MAXVAL 2 189 #define MODE_OP_MODE(x) ((x) & MODE_OP_MASK) 190 191 #define PRECISION (-9) /* precision assumed (about 2 ms) */ 192 #define PPS_PRECISION (-20) /* precision assumed (about 1 us) */ 193 #define REFID "GPSD" /* reference id */ 194 #define DESCRIPTION "GPSD JSON client clock" /* who we are */ 195 196 #define MAX_PDU_LEN 1600 197 #define TICKOVER_LOW 10 198 #define TICKOVER_HIGH 120 199 #define LOGTHROTTLE 3600 200 201 /* Primary channel PPS avilability dance: 202 * Every good PPS sample gets us a credit of PPS_INCCOUNT points, every 203 * bad/missing PPS sample costs us a debit of PPS_DECCOUNT points. When 204 * the account reaches the upper limit we change to a mode where only 205 * PPS-augmented samples are fed to the core; when the account drops to 206 * zero we switch to a mode where TPV-only timestamps are fed to the 207 * core. 208 * This reduces the chance of rapid alternation between raw and 209 * PPS-augmented time stamps. 210 */ 211 #define PPS_MAXCOUNT 60 /* upper limit of account */ 212 #define PPS_INCCOUNT 3 /* credit for good samples */ 213 #define PPS_DECCOUNT 1 /* debit for bad samples */ 214 215 /* The secondary (PPS) channel uses a different strategy to avoid old 216 * PPS samples in the median filter. 217 */ 218 #define PPS2_MAXCOUNT 10 219 220 #ifndef BOOL 221 # define BOOL int 222 #endif 223 #ifndef TRUE 224 # define TRUE 1 225 #endif 226 #ifndef FALSE 227 # define FALSE 0 228 #endif 229 230 #define PROTO_VERSION(hi,lo) \ 231 ((((uint32_t)(hi) << 16) & 0xFFFF0000u) | \ 232 ((uint32_t)(lo) & 0x0FFFFu)) 233 234 /* some local typedefs: The NTPD formatting style cries for short type 235 * names, and we provide them locally. Note:the suffix '_t' is reserved 236 * for the standard; I use a capital T instead. 237 */ 238 typedef struct peer peerT; 239 typedef struct refclockproc clockprocT; 240 typedef struct addrinfo addrinfoT; 241 242 /* ===================================================================== 243 * We use the same device name scheme as does the NMEA driver; since 244 * GPSD supports the same links, we can select devices by a fixed name. 245 */ 246 static const char * s_dev_stem = "/dev/gps"; 247 248 /* ===================================================================== 249 * forward declarations for transfer vector and the vector itself 250 */ 251 252 static void gpsd_init (void); 253 static int gpsd_start (int, peerT *); 254 static void gpsd_shutdown (int, peerT *); 255 static void gpsd_receive (struct recvbuf *); 256 static void gpsd_poll (int, peerT *); 257 static void gpsd_control (int, const struct refclockstat *, 258 struct refclockstat *, peerT *); 259 static void gpsd_timer (int, peerT *); 260 261 static int myasprintf(char**, char const*, ...) NTP_PRINTF(2, 3); 262 263 static void enter_opmode(peerT *peer, int mode); 264 static void leave_opmode(peerT *peer, int mode); 265 266 struct refclock refclock_gpsdjson = { 267 gpsd_start, /* start up driver */ 268 gpsd_shutdown, /* shut down driver */ 269 gpsd_poll, /* transmit poll message */ 270 gpsd_control, /* fudge control */ 271 gpsd_init, /* initialize driver */ 272 noentry, /* buginfo */ 273 gpsd_timer /* called once per second */ 274 }; 275 276 /* ===================================================================== 277 * our local clock unit and data 278 */ 279 struct gpsd_unit; 280 typedef struct gpsd_unit gpsd_unitT; 281 282 struct gpsd_unit { 283 /* links for sharing between master/slave units */ 284 gpsd_unitT *next_unit; 285 size_t refcount; 286 287 /* data for the secondary PPS channel */ 288 peerT *pps_peer; 289 290 /* unit and operation modes */ 291 int unit; 292 int mode; 293 char *logname; /* cached name for log/print */ 294 char * device; /* device name of unit */ 295 296 /* current line protocol version */ 297 uint32_t proto_version; 298 299 /* PPS time stamps primary + secondary channel */ 300 l_fp pps_local; /* when we received the PPS message */ 301 l_fp pps_stamp; /* related reference time */ 302 l_fp pps_recvt; /* when GPSD detected the pulse */ 303 l_fp pps_stamp2;/* related reference time (secondary) */ 304 l_fp pps_recvt2;/* when GPSD detected the pulse (secondary)*/ 305 int ppscount; /* PPS counter (primary unit) */ 306 int ppscount2; /* PPS counter (secondary unit) */ 307 308 /* TPV or TOFF serial time information */ 309 l_fp sti_local; /* when we received the TPV/TOFF message */ 310 l_fp sti_stamp; /* effective GPS time stamp */ 311 l_fp sti_recvt; /* when GPSD got the fix */ 312 313 /* precision estimates */ 314 int16_t sti_prec; /* serial precision based on EPT */ 315 int16_t pps_prec; /* PPS precision from GPSD or above */ 316 317 /* fudge values for correction, mirrored as 'l_fp' */ 318 l_fp pps_fudge; /* PPS fudge primary channel */ 319 l_fp pps_fudge2; /* PPS fudge secondary channel */ 320 l_fp sti_fudge; /* TPV/TOFF serial data fudge */ 321 322 /* Flags to indicate available data */ 323 int fl_nosync: 1; /* GPSD signals bad quality */ 324 int fl_sti : 1; /* valid TPV/TOFF seen (have time) */ 325 int fl_pps : 1; /* valid pulse seen */ 326 int fl_pps2 : 1; /* valid pulse seen for PPS channel */ 327 int fl_rawsti: 1; /* permit raw TPV/TOFF time stamps */ 328 int fl_vers : 1; /* have protocol version */ 329 int fl_watch : 1; /* watch reply seen */ 330 /* protocol flags */ 331 int pf_nsec : 1; /* have nanosec PPS info */ 332 int pf_toff : 1; /* have TOFF record for timing */ 333 334 /* admin stuff for sockets and device selection */ 335 int fdt; /* current connecting socket */ 336 addrinfoT * addr; /* next address to try */ 337 u_int tickover; /* timeout countdown */ 338 u_int tickpres; /* timeout preset */ 339 340 /* tallies for the various events */ 341 u_int tc_recv; /* received known records */ 342 u_int tc_breply; /* bad replies / parsing errors */ 343 u_int tc_nosync; /* TPV / sample cycles w/o fix */ 344 u_int tc_sti_recv;/* received serial time info records */ 345 u_int tc_sti_used;/* used --^-- */ 346 u_int tc_pps_recv;/* received PPS timing info records */ 347 u_int tc_pps_used;/* used --^-- */ 348 349 /* log bloat throttle */ 350 u_int logthrottle;/* seconds to next log slot */ 351 352 /* The parse context for the current record */ 353 json_ctx json_parse; 354 355 /* record assemby buffer and saved length */ 356 int buflen; 357 char buffer[MAX_PDU_LEN]; 358 }; 359 360 /* ===================================================================== 361 * static local helpers forward decls 362 */ 363 static void gpsd_init_socket(peerT * const peer); 364 static void gpsd_test_socket(peerT * const peer); 365 static void gpsd_stop_socket(peerT * const peer); 366 367 static void gpsd_parse(peerT * const peer, 368 const l_fp * const rtime); 369 static BOOL convert_ascii_time(l_fp * fp, const char * gps_time); 370 static void save_ltc(clockprocT * const pp, const char * const tc); 371 static int syslogok(clockprocT * const pp, gpsd_unitT * const up); 372 static void log_data(peerT *peer, const char *what, 373 const char *buf, size_t len); 374 static int16_t clamped_precision(int rawprec); 375 376 /* ===================================================================== 377 * local / static stuff 378 */ 379 380 /* The logon string is actually the ?WATCH command of GPSD, using JSON 381 * data and selecting the GPS device name we created from our unit 382 * number. We have an old a newer version that request PPS (and TOFF) 383 * transmission. 384 * Note: These are actually format strings! 385 */ 386 static const char * const s_req_watch[2] = { 387 "?WATCH={\"device\":\"%s\",\"enable\":true,\"json\":true};\r\n", 388 "?WATCH={\"device\":\"%s\",\"enable\":true,\"json\":true,\"pps\":true};\r\n" 389 }; 390 391 static const char * const s_req_version = 392 "?VERSION;\r\n"; 393 394 /* We keep a static list of network addresses for 'localhost:gpsd' or a 395 * fallback alias of it, and we try to connect to them in round-robin 396 * fashion. The service lookup is done during the driver init 397 * function to minmise the impact of 'getaddrinfo()'. 398 * 399 * Alas, the init function is called even if there are no clocks 400 * configured for this driver. So it makes sense to defer the logging of 401 * any errors or other notifications until the first clock unit is 402 * started -- otherwise there might be syslog entries from a driver that 403 * is not used at all. 404 */ 405 static addrinfoT *s_gpsd_addr; 406 static gpsd_unitT *s_clock_units; 407 408 /* list of service/socket names we want to resolve against */ 409 static const char * const s_svctab[][2] = { 410 { "localhost", "gpsd" }, 411 { "localhost", "2947" }, 412 { "127.0.0.1", "2947" }, 413 { NULL, NULL } 414 }; 415 416 /* list of address resolution errors and index of service entry that 417 * finally worked. 418 */ 419 static int s_svcerr[sizeof(s_svctab)/sizeof(s_svctab[0])]; 420 static int s_svcidx; 421 422 /* ===================================================================== 423 * log throttling 424 */ 425 static int/*BOOL*/ 426 syslogok( 427 clockprocT * const pp, 428 gpsd_unitT * const up) 429 { 430 int res = (0 != (pp->sloppyclockflag & CLK_FLAG3)) 431 || (0 == up->logthrottle ) 432 || (LOGTHROTTLE == up->logthrottle ); 433 if (res) 434 up->logthrottle = LOGTHROTTLE; 435 return res; 436 } 437 438 /* ===================================================================== 439 * the clock functions 440 */ 441 442 /* --------------------------------------------------------------------- 443 * Init: This currently just gets the socket address for the GPS daemon 444 */ 445 static void 446 gpsd_init(void) 447 { 448 addrinfoT hints; 449 int rc, idx; 450 451 memset(s_svcerr, 0, sizeof(s_svcerr)); 452 memset(&hints, 0, sizeof(hints)); 453 hints.ai_family = AF_UNSPEC; 454 hints.ai_protocol = IPPROTO_TCP; 455 hints.ai_socktype = SOCK_STREAM; 456 457 for (idx = 0; s_svctab[idx][0] && !s_gpsd_addr; idx++) { 458 rc = getaddrinfo(s_svctab[idx][0], s_svctab[idx][1], 459 &hints, &s_gpsd_addr); 460 s_svcerr[idx] = rc; 461 if (0 == rc) 462 break; 463 s_gpsd_addr = NULL; 464 } 465 s_svcidx = idx; 466 } 467 468 /* --------------------------------------------------------------------- 469 * Init Check: flush pending log messages and check if we can proceed 470 */ 471 static int/*BOOL*/ 472 gpsd_init_check(void) 473 { 474 int idx; 475 476 /* Check if there is something to log */ 477 if (s_svcidx == 0) 478 return (s_gpsd_addr != NULL); 479 480 /* spool out the resolver errors */ 481 for (idx = 0; idx < s_svcidx; ++idx) { 482 msyslog(LOG_WARNING, 483 "GPSD_JSON: failed to resolve '%s:%s', rc=%d (%s)", 484 s_svctab[idx][0], s_svctab[idx][1], 485 s_svcerr[idx], gai_strerror(s_svcerr[idx])); 486 } 487 488 /* check if it was fatal, or if we can proceed */ 489 if (s_gpsd_addr == NULL) 490 msyslog(LOG_ERR, "%s", 491 "GPSD_JSON: failed to get socket address, giving up."); 492 else if (idx != 0) 493 msyslog(LOG_WARNING, 494 "GPSD_JSON: using '%s:%s' instead of '%s:%s'", 495 s_svctab[idx][0], s_svctab[idx][1], 496 s_svctab[0][0], s_svctab[0][1]); 497 498 /* make sure this gets logged only once and tell if we can 499 * proceed or not 500 */ 501 s_svcidx = 0; 502 return (s_gpsd_addr != NULL); 503 } 504 505 /* --------------------------------------------------------------------- 506 * Start: allocate a unit pointer and set up the runtime data 507 */ 508 static int 509 gpsd_start( 510 int unit, 511 peerT * peer) 512 { 513 clockprocT * const pp = peer->procptr; 514 gpsd_unitT * up; 515 gpsd_unitT ** uscan = &s_clock_units; 516 517 struct stat sb; 518 519 /* check if we can proceed at all or if init failed */ 520 if ( ! gpsd_init_check()) 521 return FALSE; 522 523 /* search for matching unit */ 524 while ((up = *uscan) != NULL && up->unit != (unit & 0x7F)) 525 uscan = &up->next_unit; 526 if (up == NULL) { 527 /* alloc unit, add to list and increment use count ASAP. */ 528 up = emalloc_zero(sizeof(*up)); 529 *uscan = up; 530 ++up->refcount; 531 532 /* initialize the unit structure */ 533 up->logname = estrdup(refnumtoa(&peer->srcadr)); 534 up->unit = unit & 0x7F; 535 up->fdt = -1; 536 up->addr = s_gpsd_addr; 537 up->tickpres = TICKOVER_LOW; 538 539 /* Create the device name and check for a Character 540 * Device. It's assumed that GPSD was started with the 541 * same link, so the names match. (If this is not 542 * practicable, we will have to read the symlink, if 543 * any, so we can get the true device file.) 544 */ 545 if (-1 == myasprintf(&up->device, "%s%u", 546 s_dev_stem, up->unit)) { 547 msyslog(LOG_ERR, "%s: clock device name too long", 548 up->logname); 549 goto dev_fail; 550 } 551 if (-1 == stat(up->device, &sb) || !S_ISCHR(sb.st_mode)) { 552 msyslog(LOG_ERR, "%s: '%s' is not a character device", 553 up->logname, up->device); 554 goto dev_fail; 555 } 556 } else { 557 /* All set up, just increment use count. */ 558 ++up->refcount; 559 } 560 561 /* setup refclock processing */ 562 pp->unitptr = (caddr_t)up; 563 pp->io.fd = -1; 564 pp->io.clock_recv = gpsd_receive; 565 pp->io.srcclock = peer; 566 pp->io.datalen = 0; 567 pp->a_lastcode[0] = '\0'; 568 pp->lencode = 0; 569 pp->clockdesc = DESCRIPTION; 570 memcpy(&pp->refid, REFID, 4); 571 572 /* Initialize miscellaneous variables */ 573 if (unit >= 128) 574 peer->precision = PPS_PRECISION; 575 else 576 peer->precision = PRECISION; 577 578 /* If the daemon name lookup failed, just give up now. */ 579 if (NULL == up->addr) { 580 msyslog(LOG_ERR, "%s: no GPSD socket address, giving up", 581 up->logname); 582 goto dev_fail; 583 } 584 585 LOGIF(CLOCKINFO, 586 (LOG_NOTICE, "%s: startup, device is '%s'", 587 refnumtoa(&peer->srcadr), up->device)); 588 up->mode = MODE_OP_MODE(peer->ttl); 589 if (up->mode > MODE_OP_MAXVAL) 590 up->mode = 0; 591 if (unit >= 128) 592 up->pps_peer = peer; 593 else 594 enter_opmode(peer, up->mode); 595 return TRUE; 596 597 dev_fail: 598 /* On failure, remove all UNIT ressources and declare defeat. */ 599 600 INSIST (up); 601 if (!--up->refcount) { 602 *uscan = up->next_unit; 603 free(up->device); 604 free(up); 605 } 606 607 pp->unitptr = (caddr_t)NULL; 608 return FALSE; 609 } 610 611 /* ------------------------------------------------------------------ */ 612 613 static void 614 gpsd_shutdown( 615 int unit, 616 peerT * peer) 617 { 618 clockprocT * const pp = peer->procptr; 619 gpsd_unitT * const up = (gpsd_unitT *)pp->unitptr; 620 gpsd_unitT ** uscan = &s_clock_units; 621 622 UNUSED_ARG(unit); 623 624 /* The unit pointer might have been removed already. */ 625 if (up == NULL) 626 return; 627 628 /* now check if we must close IO resources */ 629 if (peer != up->pps_peer) { 630 if (-1 != pp->io.fd) { 631 DPRINTF(1, ("%s: closing clock, fd=%d\n", 632 up->logname, pp->io.fd)); 633 io_closeclock(&pp->io); 634 pp->io.fd = -1; 635 } 636 if (up->fdt != -1) 637 close(up->fdt); 638 } 639 /* decrement use count and eventually remove this unit. */ 640 if (!--up->refcount) { 641 /* unlink this unit */ 642 while (*uscan != NULL) 643 if (*uscan == up) 644 *uscan = up->next_unit; 645 else 646 uscan = &(*uscan)->next_unit; 647 free(up->logname); 648 free(up->device); 649 free(up); 650 } 651 pp->unitptr = (caddr_t)NULL; 652 LOGIF(CLOCKINFO, 653 (LOG_NOTICE, "%s: shutdown", refnumtoa(&peer->srcadr))); 654 } 655 656 /* ------------------------------------------------------------------ */ 657 658 static void 659 gpsd_receive( 660 struct recvbuf * rbufp) 661 { 662 /* declare & init control structure ptrs */ 663 peerT * const peer = rbufp->recv_peer; 664 clockprocT * const pp = peer->procptr; 665 gpsd_unitT * const up = (gpsd_unitT *)pp->unitptr; 666 667 const char *psrc, *esrc; 668 char *pdst, *edst, ch; 669 670 /* log the data stream, if this is enabled */ 671 log_data(peer, "recv", (const char*)rbufp->recv_buffer, 672 (size_t)rbufp->recv_length); 673 674 675 /* Since we're getting a raw stream data, we must assemble lines 676 * in our receive buffer. We can't use neither 'refclock_gtraw' 677 * not 'refclock_gtlin' here... We process chars until we reach 678 * an EoL (that is, line feed) but we truncate the message if it 679 * does not fit the buffer. GPSD might truncate messages, too, 680 * so dealing with truncated buffers is necessary anyway. 681 */ 682 psrc = (const char*)rbufp->recv_buffer; 683 esrc = psrc + rbufp->recv_length; 684 685 pdst = up->buffer + up->buflen; 686 edst = pdst + sizeof(up->buffer) - 1; /* for trailing NUL */ 687 688 while (psrc != esrc) { 689 ch = *psrc++; 690 if (ch == '\n') { 691 /* trim trailing whitespace & terminate buffer */ 692 while (pdst != up->buffer && pdst[-1] <= ' ') 693 --pdst; 694 *pdst = '\0'; 695 /* process data and reset buffer */ 696 up->buflen = pdst - up->buffer; 697 gpsd_parse(peer, &rbufp->recv_time); 698 pdst = up->buffer; 699 } else if (pdst != edst) { 700 /* add next char, ignoring leading whitespace */ 701 if (ch > ' ' || pdst != up->buffer) 702 *pdst++ = ch; 703 } 704 } 705 up->buflen = pdst - up->buffer; 706 up->tickover = TICKOVER_LOW; 707 } 708 709 /* ------------------------------------------------------------------ */ 710 711 static void 712 poll_primary( 713 peerT * const peer , 714 clockprocT * const pp , 715 gpsd_unitT * const up ) 716 { 717 if (pp->coderecv != pp->codeproc) { 718 /* all is well */ 719 pp->lastref = pp->lastrec; 720 refclock_report(peer, CEVNT_NOMINAL); 721 refclock_receive(peer); 722 } else { 723 /* Not working properly, admit to it. If we have no 724 * connection to GPSD, declare the clock as faulty. If 725 * there were bad replies, this is handled as the major 726 * cause, and everything else is just a timeout. 727 */ 728 peer->precision = PRECISION; 729 if (-1 == pp->io.fd) 730 refclock_report(peer, CEVNT_FAULT); 731 else if (0 != up->tc_breply) 732 refclock_report(peer, CEVNT_BADREPLY); 733 else 734 refclock_report(peer, CEVNT_TIMEOUT); 735 } 736 737 if (pp->sloppyclockflag & CLK_FLAG4) 738 mprintf_clock_stats( 739 &peer->srcadr,"%u %u %u %u %u %u %u", 740 up->tc_recv, 741 up->tc_breply, up->tc_nosync, 742 up->tc_sti_recv, up->tc_sti_used, 743 up->tc_pps_recv, up->tc_pps_used); 744 745 /* clear tallies for next round */ 746 up->tc_breply = 0; 747 up->tc_recv = 0; 748 up->tc_nosync = 0; 749 up->tc_sti_recv = 0; 750 up->tc_sti_used = 0; 751 up->tc_pps_recv = 0; 752 up->tc_pps_used = 0; 753 } 754 755 static void 756 poll_secondary( 757 peerT * const peer , 758 clockprocT * const pp , 759 gpsd_unitT * const up ) 760 { 761 if (pp->coderecv != pp->codeproc) { 762 /* all is well */ 763 pp->lastref = pp->lastrec; 764 refclock_report(peer, CEVNT_NOMINAL); 765 refclock_receive(peer); 766 } else { 767 peer->precision = PPS_PRECISION; 768 peer->flags &= ~FLAG_PPS; 769 refclock_report(peer, CEVNT_TIMEOUT); 770 } 771 } 772 773 static void 774 gpsd_poll( 775 int unit, 776 peerT * peer) 777 { 778 clockprocT * const pp = peer->procptr; 779 gpsd_unitT * const up = (gpsd_unitT *)pp->unitptr; 780 781 ++pp->polls; 782 if (peer == up->pps_peer) 783 poll_secondary(peer, pp, up); 784 else 785 poll_primary(peer, pp, up); 786 } 787 788 /* ------------------------------------------------------------------ */ 789 790 static void 791 gpsd_control( 792 int unit, 793 const struct refclockstat * in_st, 794 struct refclockstat * out_st, 795 peerT * peer ) 796 { 797 clockprocT * const pp = peer->procptr; 798 gpsd_unitT * const up = (gpsd_unitT *)pp->unitptr; 799 800 if (peer == up->pps_peer) { 801 DTOLFP(pp->fudgetime1, &up->pps_fudge2); 802 if ( ! (pp->sloppyclockflag & CLK_FLAG1)) 803 peer->flags &= ~FLAG_PPS; 804 } else { 805 /* save preprocessed fudge times */ 806 DTOLFP(pp->fudgetime1, &up->pps_fudge); 807 DTOLFP(pp->fudgetime2, &up->sti_fudge); 808 809 if (MODE_OP_MODE(up->mode ^ peer->ttl)) { 810 leave_opmode(peer, up->mode); 811 up->mode = MODE_OP_MODE(peer->ttl); 812 enter_opmode(peer, up->mode); 813 } 814 } 815 } 816 817 /* ------------------------------------------------------------------ */ 818 819 static void 820 timer_primary( 821 peerT * const peer , 822 clockprocT * const pp , 823 gpsd_unitT * const up ) 824 { 825 int rc; 826 827 /* This is used for timeout handling. Nothing that needs 828 * sub-second precison happens here, so receive/connect/retry 829 * timeouts are simply handled by a count down, and then we 830 * decide what to do by the socket values. 831 * 832 * Note that the timer stays at zero here, unless some of the 833 * functions set it to another value. 834 */ 835 if (up->logthrottle) 836 --up->logthrottle; 837 if (up->tickover) 838 --up->tickover; 839 switch (up->tickover) { 840 case 4: 841 /* If we are connected to GPSD, try to get a live signal 842 * by querying the version. Otherwise just check the 843 * socket to become ready. 844 */ 845 if (-1 != pp->io.fd) { 846 size_t rlen = strlen(s_req_version); 847 DPRINTF(2, ("%s: timer livecheck: '%s'\n", 848 up->logname, s_req_version)); 849 log_data(peer, "send", s_req_version, rlen); 850 rc = write(pp->io.fd, s_req_version, rlen); 851 (void)rc; 852 } else if (-1 != up->fdt) { 853 gpsd_test_socket(peer); 854 } 855 break; 856 857 case 0: 858 if (-1 != pp->io.fd) 859 gpsd_stop_socket(peer); 860 else if (-1 != up->fdt) 861 gpsd_test_socket(peer); 862 else if (NULL != s_gpsd_addr) 863 gpsd_init_socket(peer); 864 break; 865 866 default: 867 if (-1 == pp->io.fd && -1 != up->fdt) 868 gpsd_test_socket(peer); 869 } 870 } 871 872 static void 873 timer_secondary( 874 peerT * const peer , 875 clockprocT * const pp , 876 gpsd_unitT * const up ) 877 { 878 /* Reduce the count by one. Flush sample buffer and clear PPS 879 * flag when this happens. 880 */ 881 up->ppscount2 = max(0, (up->ppscount2 - 1)); 882 if (0 == up->ppscount2) { 883 if (pp->coderecv != pp->codeproc) { 884 refclock_report(peer, CEVNT_TIMEOUT); 885 pp->coderecv = pp->codeproc; 886 } 887 peer->flags &= ~FLAG_PPS; 888 } 889 } 890 891 static void 892 gpsd_timer( 893 int unit, 894 peerT * peer) 895 { 896 clockprocT * const pp = peer->procptr; 897 gpsd_unitT * const up = (gpsd_unitT *)pp->unitptr; 898 899 if (peer == up->pps_peer) 900 timer_secondary(peer, pp, up); 901 else 902 timer_primary(peer, pp, up); 903 } 904 905 /* ===================================================================== 906 * handle opmode switches 907 */ 908 909 static void 910 enter_opmode( 911 peerT *peer, 912 int mode) 913 { 914 clockprocT * const pp = peer->procptr; 915 gpsd_unitT * const up = (gpsd_unitT *)pp->unitptr; 916 917 DPRINTF(1, ("%s: enter operation mode %d\n", 918 up->logname, MODE_OP_MODE(mode))); 919 920 if (MODE_OP_MODE(mode) == MODE_OP_AUTO) { 921 up->fl_rawsti = 0; 922 up->ppscount = PPS_MAXCOUNT / 2; 923 } 924 up->fl_pps = 0; 925 up->fl_sti = 0; 926 } 927 928 /* ------------------------------------------------------------------ */ 929 930 static void 931 leave_opmode( 932 peerT *peer, 933 int mode) 934 { 935 clockprocT * const pp = peer->procptr; 936 gpsd_unitT * const up = (gpsd_unitT *)pp->unitptr; 937 938 DPRINTF(1, ("%s: leaving operation mode %d\n", 939 up->logname, MODE_OP_MODE(mode))); 940 941 if (MODE_OP_MODE(mode) == MODE_OP_AUTO) { 942 up->fl_rawsti = 0; 943 up->ppscount = 0; 944 } 945 up->fl_pps = 0; 946 up->fl_sti = 0; 947 } 948 949 /* ===================================================================== 950 * operation mode specific evaluation 951 */ 952 953 static void 954 add_clock_sample( 955 peerT * const peer , 956 clockprocT * const pp , 957 l_fp stamp, 958 l_fp recvt) 959 { 960 pp->lastref = stamp; 961 if (pp->coderecv == pp->codeproc) 962 refclock_report(peer, CEVNT_NOMINAL); 963 refclock_process_offset(pp, stamp, recvt, 0.0); 964 } 965 966 /* ------------------------------------------------------------------ */ 967 968 static void 969 eval_strict( 970 peerT * const peer , 971 clockprocT * const pp , 972 gpsd_unitT * const up ) 973 { 974 if (up->fl_sti && up->fl_pps) { 975 /* use TPV reference time + PPS receive time */ 976 add_clock_sample(peer, pp, up->sti_stamp, up->pps_recvt); 977 peer->precision = up->pps_prec; 978 /* both packets consumed now... */ 979 up->fl_pps = 0; 980 up->fl_sti = 0; 981 ++up->tc_sti_used; 982 } 983 } 984 985 /* ------------------------------------------------------------------ */ 986 /* PPS processing for the secondary channel. GPSD provides us with full 987 * timing information, so there's no danger of PLL-locking to the wrong 988 * second. The belts and suspenders needed for the raw ATOM clock are 989 * unnecessary here. 990 */ 991 static void 992 eval_pps_secondary( 993 peerT * const peer , 994 clockprocT * const pp , 995 gpsd_unitT * const up ) 996 { 997 if (up->fl_pps2) { 998 /* feed data */ 999 add_clock_sample(peer, pp, up->pps_stamp2, up->pps_recvt2); 1000 peer->precision = up->pps_prec; 1001 /* PPS peer flag logic */ 1002 up->ppscount2 = min(PPS2_MAXCOUNT, (up->ppscount2 + 2)); 1003 if ((PPS2_MAXCOUNT == up->ppscount2) && 1004 (pp->sloppyclockflag & CLK_FLAG1) ) 1005 peer->flags |= FLAG_PPS; 1006 /* mark time stamp as burned... */ 1007 up->fl_pps2 = 0; 1008 ++up->tc_pps_used; 1009 } 1010 } 1011 1012 /* ------------------------------------------------------------------ */ 1013 1014 static void 1015 eval_serial( 1016 peerT * const peer , 1017 clockprocT * const pp , 1018 gpsd_unitT * const up ) 1019 { 1020 if (up->fl_sti) { 1021 add_clock_sample(peer, pp, up->sti_stamp, up->sti_recvt); 1022 peer->precision = up->sti_prec; 1023 /* mark time stamp as burned... */ 1024 up->fl_sti = 0; 1025 ++up->tc_sti_used; 1026 } 1027 } 1028 1029 /* ------------------------------------------------------------------ */ 1030 static void 1031 eval_auto( 1032 peerT * const peer , 1033 clockprocT * const pp , 1034 gpsd_unitT * const up ) 1035 { 1036 /* If there's no TPV available, stop working here... */ 1037 if (!up->fl_sti) 1038 return; 1039 1040 /* check how to handle STI+PPS: Can PPS be used to augment STI 1041 * (or vice versae), do we drop the sample because there is a 1042 * temporary missing PPS signal, or do we feed on STI time 1043 * stamps alone? 1044 * 1045 * Do a counter/threshold dance to decide how to proceed. 1046 */ 1047 if (up->fl_pps) { 1048 up->ppscount = min(PPS_MAXCOUNT, 1049 (up->ppscount + PPS_INCCOUNT)); 1050 if ((PPS_MAXCOUNT == up->ppscount) && up->fl_rawsti) { 1051 up->fl_rawsti = 0; 1052 msyslog(LOG_INFO, 1053 "%s: expect valid PPS from now", 1054 up->logname); 1055 } 1056 } else { 1057 up->ppscount = max(0, (up->ppscount - PPS_DECCOUNT)); 1058 if ((0 == up->ppscount) && !up->fl_rawsti) { 1059 up->fl_rawsti = -1; 1060 msyslog(LOG_WARNING, 1061 "%s: use TPV alone from now", 1062 up->logname); 1063 } 1064 } 1065 1066 /* now eventually feed the sample */ 1067 if (up->fl_rawsti) 1068 eval_serial(peer, pp, up); 1069 else 1070 eval_strict(peer, pp, up); 1071 } 1072 1073 /* ===================================================================== 1074 * JSON parsing stuff 1075 */ 1076 1077 /* ------------------------------------------------------------------ */ 1078 /* Parse a decimal integer with a possible sign. Works like 'strtoll()' 1079 * or 'strtol()', but with a fixed base of 10 and without eating away 1080 * leading whitespace. For the error codes, the handling of the end 1081 * pointer and the return values see 'strtol()'. 1082 */ 1083 static json_int 1084 strtojint( 1085 const char *cp, char **ep) 1086 { 1087 json_uint accu, limit_lo, limit_hi; 1088 int flags; /* bit 0: overflow; bit 1: sign */ 1089 const char * hold; 1090 1091 /* pointer union to circumvent a tricky/sticky const issue */ 1092 union { const char * c; char * v; } vep; 1093 1094 /* store initial value of 'cp' -- see 'strtol()' */ 1095 vep.c = cp; 1096 1097 /* Eat away an optional sign and set the limits accordingly: The 1098 * high limit is the maximum absolute value that can be returned, 1099 * and the low limit is the biggest value that does not cause an 1100 * overflow when multiplied with 10. Avoid negation overflows. 1101 */ 1102 if (*cp == '-') { 1103 cp += 1; 1104 flags = 2; 1105 limit_hi = (json_uint)-(JSON_INT_MIN + 1) + 1; 1106 } else { 1107 cp += (*cp == '+'); 1108 flags = 0; 1109 limit_hi = (json_uint)JSON_INT_MAX; 1110 } 1111 limit_lo = limit_hi / 10; 1112 1113 /* Now try to convert a sequence of digits. */ 1114 hold = cp; 1115 accu = 0; 1116 while (isdigit(*(const u_char*)cp)) { 1117 flags |= (accu > limit_lo); 1118 accu = accu * 10 + (*(const u_char*)cp++ - '0'); 1119 flags |= (accu > limit_hi); 1120 } 1121 /* Check for empty conversion (no digits seen). */ 1122 if (hold != cp) 1123 vep.c = cp; 1124 else 1125 errno = EINVAL; /* accu is still zero */ 1126 /* Check for range overflow */ 1127 if (flags & 1) { 1128 errno = ERANGE; 1129 accu = limit_hi; 1130 } 1131 /* If possible, store back the end-of-conversion pointer */ 1132 if (ep) 1133 *ep = vep.v; 1134 /* If negative, return the negated result if the accu is not 1135 * zero. Avoid negation overflows. 1136 */ 1137 if ((flags & 2) && accu) 1138 return -(json_int)(accu - 1) - 1; 1139 else 1140 return (json_int)accu; 1141 } 1142 1143 /* ------------------------------------------------------------------ */ 1144 1145 static tok_ref 1146 json_token_skip( 1147 const json_ctx * ctx, 1148 tok_ref tid) 1149 { 1150 if (tid >= 0 && tid < ctx->ntok) { 1151 int len = ctx->tok[tid].size; 1152 /* For arrays and objects, the size is the number of 1153 * ITEMS in the compound. Thats the number of objects in 1154 * the array, and the number of key/value pairs for 1155 * objects. In theory, the key must be a string, and we 1156 * could simply skip one token before skipping the 1157 * value, which can be anything. We're a bit paranoid 1158 * and lazy at the same time: We simply double the 1159 * number of tokens to skip and fall through into the 1160 * array processing when encountering an object. 1161 */ 1162 switch (ctx->tok[tid].type) { 1163 case JSMN_OBJECT: 1164 len *= 2; 1165 /* FALLTHROUGH */ 1166 case JSMN_ARRAY: 1167 for (++tid; len; --len) 1168 tid = json_token_skip(ctx, tid); 1169 break; 1170 1171 default: 1172 ++tid; 1173 break; 1174 } 1175 if (tid > ctx->ntok) /* Impossible? Paranoia rulez. */ 1176 tid = ctx->ntok; 1177 } 1178 return tid; 1179 } 1180 1181 /* ------------------------------------------------------------------ */ 1182 1183 static int 1184 json_object_lookup( 1185 const json_ctx * ctx , 1186 tok_ref tid , 1187 const char * key , 1188 int what) 1189 { 1190 int len; 1191 1192 if (tid < 0 || tid >= ctx->ntok || 1193 ctx->tok[tid].type != JSMN_OBJECT) 1194 return INVALID_TOKEN; 1195 1196 len = ctx->tok[tid].size; 1197 for (++tid; len && tid+1 < ctx->ntok; --len) { 1198 if (ctx->tok[tid].type != JSMN_STRING) { /* Blooper! */ 1199 tid = json_token_skip(ctx, tid); /* skip key */ 1200 tid = json_token_skip(ctx, tid); /* skip val */ 1201 } else if (strcmp(key, ctx->buf + ctx->tok[tid].start)) { 1202 tid = json_token_skip(ctx, tid+1); /* skip key+val */ 1203 } else if (what < 0 || what == ctx->tok[tid+1].type) { 1204 return tid + 1; 1205 } else { 1206 break; 1207 } 1208 /* if skipping ahead returned an error, bail out here. */ 1209 if (tid < 0) 1210 break; 1211 } 1212 return INVALID_TOKEN; 1213 } 1214 1215 /* ------------------------------------------------------------------ */ 1216 1217 static const char* 1218 json_object_lookup_primitive( 1219 const json_ctx * ctx, 1220 tok_ref tid, 1221 const char * key) 1222 { 1223 tid = json_object_lookup(ctx, tid, key, JSMN_PRIMITIVE); 1224 if (INVALID_TOKEN != tid) 1225 return ctx->buf + ctx->tok[tid].start; 1226 else 1227 return NULL; 1228 } 1229 /* ------------------------------------------------------------------ */ 1230 /* look up a boolean value. This essentially returns a tribool: 1231 * 0->false, 1->true, (-1)->error/undefined 1232 */ 1233 static int 1234 json_object_lookup_bool( 1235 const json_ctx * ctx, 1236 tok_ref tid, 1237 const char * key) 1238 { 1239 const char *cp; 1240 cp = json_object_lookup_primitive(ctx, tid, key); 1241 switch ( cp ? *cp : '\0') { 1242 case 't': return 1; 1243 case 'f': return 0; 1244 default : return -1; 1245 } 1246 } 1247 1248 /* ------------------------------------------------------------------ */ 1249 1250 static const char* 1251 json_object_lookup_string( 1252 const json_ctx * ctx, 1253 tok_ref tid, 1254 const char * key) 1255 { 1256 tid = json_object_lookup(ctx, tid, key, JSMN_STRING); 1257 if (INVALID_TOKEN != tid) 1258 return ctx->buf + ctx->tok[tid].start; 1259 return NULL; 1260 } 1261 1262 static const char* 1263 json_object_lookup_string_default( 1264 const json_ctx * ctx, 1265 tok_ref tid, 1266 const char * key, 1267 const char * def) 1268 { 1269 tid = json_object_lookup(ctx, tid, key, JSMN_STRING); 1270 if (INVALID_TOKEN != tid) 1271 return ctx->buf + ctx->tok[tid].start; 1272 return def; 1273 } 1274 1275 /* ------------------------------------------------------------------ */ 1276 1277 static json_int 1278 json_object_lookup_int( 1279 const json_ctx * ctx, 1280 tok_ref tid, 1281 const char * key) 1282 { 1283 json_int ret; 1284 const char * cp; 1285 char * ep; 1286 1287 cp = json_object_lookup_primitive(ctx, tid, key); 1288 if (NULL != cp) { 1289 ret = strtojint(cp, &ep); 1290 if (cp != ep && '\0' == *ep) 1291 return ret; 1292 } else { 1293 errno = EINVAL; 1294 } 1295 return 0; 1296 } 1297 1298 static json_int 1299 json_object_lookup_int_default( 1300 const json_ctx * ctx, 1301 tok_ref tid, 1302 const char * key, 1303 json_int def) 1304 { 1305 json_int ret; 1306 const char * cp; 1307 char * ep; 1308 1309 cp = json_object_lookup_primitive(ctx, tid, key); 1310 if (NULL != cp) { 1311 ret = strtojint(cp, &ep); 1312 if (cp != ep && '\0' == *ep) 1313 return ret; 1314 } 1315 return def; 1316 } 1317 1318 /* ------------------------------------------------------------------ */ 1319 #if 0 /* currently unused */ 1320 static double 1321 json_object_lookup_float( 1322 const json_ctx * ctx, 1323 tok_ref tid, 1324 const char * key) 1325 { 1326 double ret; 1327 const char * cp; 1328 char * ep; 1329 1330 cp = json_object_lookup_primitive(ctx, tid, key); 1331 if (NULL != cp) { 1332 ret = strtod(cp, &ep); 1333 if (cp != ep && '\0' == *ep) 1334 return ret; 1335 } else { 1336 errno = EINVAL; 1337 } 1338 return 0.0; 1339 } 1340 #endif 1341 1342 static double 1343 json_object_lookup_float_default( 1344 const json_ctx * ctx, 1345 tok_ref tid, 1346 const char * key, 1347 double def) 1348 { 1349 double ret; 1350 const char * cp; 1351 char * ep; 1352 1353 cp = json_object_lookup_primitive(ctx, tid, key); 1354 if (NULL != cp) { 1355 ret = strtod(cp, &ep); 1356 if (cp != ep && '\0' == *ep) 1357 return ret; 1358 } 1359 return def; 1360 } 1361 1362 /* ------------------------------------------------------------------ */ 1363 1364 static BOOL 1365 json_parse_record( 1366 json_ctx * ctx, 1367 char * buf, 1368 size_t len) 1369 { 1370 jsmn_parser jsm; 1371 int idx, rc; 1372 1373 jsmn_init(&jsm); 1374 rc = jsmn_parse(&jsm, buf, len, ctx->tok, JSMN_MAXTOK); 1375 if (rc <= 0) 1376 return FALSE; 1377 ctx->buf = buf; 1378 ctx->ntok = rc; 1379 1380 if (JSMN_OBJECT != ctx->tok[0].type) 1381 return FALSE; /* not object!?! */ 1382 1383 /* Make all tokens NUL terminated by overwriting the 1384 * terminator symbol. Makes string compares and number parsing a 1385 * lot easier! 1386 */ 1387 for (idx = 0; idx < ctx->ntok; ++idx) 1388 if (ctx->tok[idx].end > ctx->tok[idx].start) 1389 ctx->buf[ctx->tok[idx].end] = '\0'; 1390 return TRUE; 1391 } 1392 1393 1394 /* ===================================================================== 1395 * static local helpers 1396 */ 1397 static BOOL 1398 get_binary_time( 1399 l_fp * const dest , 1400 json_ctx * const jctx , 1401 const char * const time_name, 1402 const char * const frac_name, 1403 long fscale ) 1404 { 1405 BOOL retv = FALSE; 1406 struct timespec ts; 1407 1408 errno = 0; 1409 ts.tv_sec = (time_t)json_object_lookup_int(jctx, 0, time_name); 1410 ts.tv_nsec = (long )json_object_lookup_int(jctx, 0, frac_name); 1411 if (0 == errno) { 1412 ts.tv_nsec *= fscale; 1413 *dest = tspec_stamp_to_lfp(ts); 1414 retv = TRUE; 1415 } 1416 return retv; 1417 } 1418 1419 /* ------------------------------------------------------------------ */ 1420 /* Process a WATCH record 1421 * 1422 * Currently this is only used to recognise that the device is present 1423 * and that we're listed subscribers. 1424 */ 1425 static void 1426 process_watch( 1427 peerT * const peer , 1428 json_ctx * const jctx , 1429 const l_fp * const rtime) 1430 { 1431 clockprocT * const pp = peer->procptr; 1432 gpsd_unitT * const up = (gpsd_unitT *)pp->unitptr; 1433 1434 const char * path; 1435 1436 path = json_object_lookup_string(jctx, 0, "device"); 1437 if (NULL == path || strcmp(path, up->device)) 1438 return; 1439 1440 if (json_object_lookup_bool(jctx, 0, "enable") > 0 && 1441 json_object_lookup_bool(jctx, 0, "json" ) > 0 ) 1442 up->fl_watch = -1; 1443 else 1444 up->fl_watch = 0; 1445 DPRINTF(2, ("%s: process_watch, enabled=%d\n", 1446 up->logname, (up->fl_watch & 1))); 1447 } 1448 1449 /* ------------------------------------------------------------------ */ 1450 1451 static void 1452 process_version( 1453 peerT * const peer , 1454 json_ctx * const jctx , 1455 const l_fp * const rtime) 1456 { 1457 clockprocT * const pp = peer->procptr; 1458 gpsd_unitT * const up = (gpsd_unitT *)pp->unitptr; 1459 1460 int len; 1461 char * buf; 1462 const char *revision; 1463 const char *release; 1464 uint16_t pvhi, pvlo; 1465 1466 /* get protocol version number */ 1467 revision = json_object_lookup_string_default( 1468 jctx, 0, "rev", "(unknown)"); 1469 release = json_object_lookup_string_default( 1470 jctx, 0, "release", "(unknown)"); 1471 errno = 0; 1472 pvhi = (uint16_t)json_object_lookup_int(jctx, 0, "proto_major"); 1473 pvlo = (uint16_t)json_object_lookup_int(jctx, 0, "proto_minor"); 1474 1475 if (0 == errno) { 1476 if ( ! up->fl_vers) 1477 msyslog(LOG_INFO, 1478 "%s: GPSD revision=%s release=%s protocol=%u.%u", 1479 up->logname, revision, release, 1480 pvhi, pvlo); 1481 up->proto_version = PROTO_VERSION(pvhi, pvlo); 1482 up->fl_vers = -1; 1483 } else { 1484 if (syslogok(pp, up)) 1485 msyslog(LOG_INFO, 1486 "%s: could not evaluate version data", 1487 up->logname); 1488 return; 1489 } 1490 /* With the 3.9 GPSD protocol, '*_musec' vanished from the PPS 1491 * record and was replace by '*_nsec'. 1492 */ 1493 up->pf_nsec = -(up->proto_version >= PROTO_VERSION(3,9)); 1494 1495 /* With the 3.10 protocol we can get TOFF records for better 1496 * timing information. 1497 */ 1498 up->pf_toff = -(up->proto_version >= PROTO_VERSION(3,10)); 1499 1500 /* request watch for our GPS device if not yet watched. 1501 * 1502 * The version string is also sent as a life signal, if we have 1503 * seen useable data. So if we're already watching the device, 1504 * skip the request. 1505 * 1506 * Reuse the input buffer, which is no longer needed in the 1507 * current cycle. Also assume that we can write the watch 1508 * request in one sweep into the socket; since we do not do 1509 * output otherwise, this should always work. (Unless the 1510 * TCP/IP window size gets lower than the length of the 1511 * request. We handle that when it happens.) 1512 */ 1513 if (up->fl_watch) 1514 return; 1515 1516 snprintf(up->buffer, sizeof(up->buffer), 1517 s_req_watch[up->pf_toff != 0], up->device); 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 (char*)&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, &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; 2118 2119 len = (tc) ? strlen(tc) : 0; 2120 if (len >= sizeof(pp->a_lastcode)) 2121 len = sizeof(pp->a_lastcode) - 1; 2122 pp->lencode = (u_short)len; 2123 memcpy(pp->a_lastcode, tc, len); 2124 pp->a_lastcode[len] = '\0'; 2125 } 2126 2127 /* ------------------------------------------------------------------- 2128 * asprintf replacement... it's not available everywhere... 2129 */ 2130 static int 2131 myasprintf( 2132 char ** spp, 2133 char const * fmt, 2134 ... ) 2135 { 2136 size_t alen, plen; 2137 2138 alen = 32; 2139 *spp = NULL; 2140 do { 2141 va_list va; 2142 2143 alen += alen; 2144 free(*spp); 2145 *spp = (char*)malloc(alen); 2146 if (NULL == *spp) 2147 return -1; 2148 2149 va_start(va, fmt); 2150 plen = (size_t)vsnprintf(*spp, alen, fmt, va); 2151 va_end(va); 2152 } while (plen >= alen); 2153 2154 return (int)plen; 2155 } 2156 2157 /* ------------------------------------------------------------------- 2158 * dump a raw data buffer 2159 */ 2160 2161 static char * 2162 add_string( 2163 char *dp, 2164 char *ep, 2165 const char *sp) 2166 { 2167 while (dp != ep && *sp) 2168 *dp++ = *sp++; 2169 return dp; 2170 } 2171 2172 static void 2173 log_data( 2174 peerT *peer, 2175 const char *what, 2176 const char *buf , 2177 size_t len ) 2178 { 2179 /* we're running single threaded with regards to the clocks. */ 2180 static char s_lbuf[2048]; 2181 2182 clockprocT * const pp = peer->procptr; 2183 gpsd_unitT * const up = (gpsd_unitT *)pp->unitptr; 2184 2185 if (debug > 1) { 2186 const char *sptr = buf; 2187 const char *stop = buf + len; 2188 char *dptr = s_lbuf; 2189 char *dtop = s_lbuf + sizeof(s_lbuf) - 1; /* for NUL */ 2190 2191 while (sptr != stop && dptr != dtop) { 2192 u_char uch = (u_char)*sptr++; 2193 if (uch == '\\') { 2194 dptr = add_string(dptr, dtop, "\\\\"); 2195 } else if (isprint(uch)) { 2196 *dptr++ = (char)uch; 2197 } else { 2198 char fbuf[6]; 2199 snprintf(fbuf, sizeof(fbuf), "\\%03o", uch); 2200 dptr = add_string(dptr, dtop, fbuf); 2201 } 2202 } 2203 *dptr = '\0'; 2204 mprintf("%s[%s]: '%s'\n", up->logname, what, s_lbuf); 2205 } 2206 } 2207 2208 #else 2209 NONEMPTY_TRANSLATION_UNIT 2210 #endif /* REFCLOCK && CLOCK_GPSDJSON */ 2211