refclock_gpsdjson.c revision 330106
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 100typedef struct json_ctx { 101 char * buf; 102 int ntok; 103 jsmntok_t tok[JSMN_MAXTOK]; 104} json_ctx; 105 106typedef 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 112typedef signed long long int json_int; 113typedef unsigned long long int json_uint; 114#define JSON_INT_MAX LLONG_MAX 115#define JSON_INT_MIN LLONG_MIN 116#else 117typedef signed long int json_int; 118typedef 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 */ 238typedef struct peer peerT; 239typedef struct refclockproc clockprocT; 240typedef 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 */ 246static const char * s_dev_stem = "/dev/gps"; 247 248/* ===================================================================== 249 * forward declarations for transfer vector and the vector itself 250 */ 251 252static void gpsd_init (void); 253static int gpsd_start (int, peerT *); 254static void gpsd_shutdown (int, peerT *); 255static void gpsd_receive (struct recvbuf *); 256static void gpsd_poll (int, peerT *); 257static void gpsd_control (int, const struct refclockstat *, 258 struct refclockstat *, peerT *); 259static void gpsd_timer (int, peerT *); 260 261static int myasprintf(char**, char const*, ...) NTP_PRINTF(2, 3); 262 263static void enter_opmode(peerT *peer, int mode); 264static void leave_opmode(peerT *peer, int mode); 265 266struct 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 */ 279struct gpsd_unit; 280typedef struct gpsd_unit gpsd_unitT; 281 282struct 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 */ 363static void gpsd_init_socket(peerT * const peer); 364static void gpsd_test_socket(peerT * const peer); 365static void gpsd_stop_socket(peerT * const peer); 366 367static void gpsd_parse(peerT * const peer, 368 const l_fp * const rtime); 369static BOOL convert_ascii_time(l_fp * fp, const char * gps_time); 370static void save_ltc(clockprocT * const pp, const char * const tc); 371static int syslogok(clockprocT * const pp, gpsd_unitT * const up); 372static void log_data(peerT *peer, const char *what, 373 const char *buf, size_t len); 374static int16_t clamped_precision(int rawprec); 375 376/* ===================================================================== 377 * local / static stuff 378 */ 379 380static const char * const s_req_version = 381 "?VERSION;\r\n"; 382 383/* We keep a static list of network addresses for 'localhost:gpsd' or a 384 * fallback alias of it, and we try to connect to them in round-robin 385 * fashion. The service lookup is done during the driver init 386 * function to minmise the impact of 'getaddrinfo()'. 387 * 388 * Alas, the init function is called even if there are no clocks 389 * configured for this driver. So it makes sense to defer the logging of 390 * any errors or other notifications until the first clock unit is 391 * started -- otherwise there might be syslog entries from a driver that 392 * is not used at all. 393 */ 394static addrinfoT *s_gpsd_addr; 395static gpsd_unitT *s_clock_units; 396 397/* list of service/socket names we want to resolve against */ 398static const char * const s_svctab[][2] = { 399 { "localhost", "gpsd" }, 400 { "localhost", "2947" }, 401 { "127.0.0.1", "2947" }, 402 { NULL, NULL } 403}; 404 405/* list of address resolution errors and index of service entry that 406 * finally worked. 407 */ 408static int s_svcerr[sizeof(s_svctab)/sizeof(s_svctab[0])]; 409static int s_svcidx; 410 411/* ===================================================================== 412 * log throttling 413 */ 414static int/*BOOL*/ 415syslogok( 416 clockprocT * const pp, 417 gpsd_unitT * const up) 418{ 419 int res = (0 != (pp->sloppyclockflag & CLK_FLAG3)) 420 || (0 == up->logthrottle ) 421 || (LOGTHROTTLE == up->logthrottle ); 422 if (res) 423 up->logthrottle = LOGTHROTTLE; 424 return res; 425} 426 427/* ===================================================================== 428 * the clock functions 429 */ 430 431/* --------------------------------------------------------------------- 432 * Init: This currently just gets the socket address for the GPS daemon 433 */ 434static void 435gpsd_init(void) 436{ 437 addrinfoT hints; 438 int rc, idx; 439 440 memset(s_svcerr, 0, sizeof(s_svcerr)); 441 memset(&hints, 0, sizeof(hints)); 442 hints.ai_family = AF_UNSPEC; 443 hints.ai_protocol = IPPROTO_TCP; 444 hints.ai_socktype = SOCK_STREAM; 445 446 for (idx = 0; s_svctab[idx][0] && !s_gpsd_addr; idx++) { 447 rc = getaddrinfo(s_svctab[idx][0], s_svctab[idx][1], 448 &hints, &s_gpsd_addr); 449 s_svcerr[idx] = rc; 450 if (0 == rc) 451 break; 452 s_gpsd_addr = NULL; 453 } 454 s_svcidx = idx; 455} 456 457/* --------------------------------------------------------------------- 458 * Init Check: flush pending log messages and check if we can proceed 459 */ 460static int/*BOOL*/ 461gpsd_init_check(void) 462{ 463 int idx; 464 465 /* Check if there is something to log */ 466 if (s_svcidx == 0) 467 return (s_gpsd_addr != NULL); 468 469 /* spool out the resolver errors */ 470 for (idx = 0; idx < s_svcidx; ++idx) { 471 msyslog(LOG_WARNING, 472 "GPSD_JSON: failed to resolve '%s:%s', rc=%d (%s)", 473 s_svctab[idx][0], s_svctab[idx][1], 474 s_svcerr[idx], gai_strerror(s_svcerr[idx])); 475 } 476 477 /* check if it was fatal, or if we can proceed */ 478 if (s_gpsd_addr == NULL) 479 msyslog(LOG_ERR, "%s", 480 "GPSD_JSON: failed to get socket address, giving up."); 481 else if (idx != 0) 482 msyslog(LOG_WARNING, 483 "GPSD_JSON: using '%s:%s' instead of '%s:%s'", 484 s_svctab[idx][0], s_svctab[idx][1], 485 s_svctab[0][0], s_svctab[0][1]); 486 487 /* make sure this gets logged only once and tell if we can 488 * proceed or not 489 */ 490 s_svcidx = 0; 491 return (s_gpsd_addr != NULL); 492} 493 494/* --------------------------------------------------------------------- 495 * Start: allocate a unit pointer and set up the runtime data 496 */ 497static int 498gpsd_start( 499 int unit, 500 peerT * peer) 501{ 502 clockprocT * const pp = peer->procptr; 503 gpsd_unitT * up; 504 gpsd_unitT ** uscan = &s_clock_units; 505 506 struct stat sb; 507 508 /* check if we can proceed at all or if init failed */ 509 if ( ! gpsd_init_check()) 510 return FALSE; 511 512 /* search for matching unit */ 513 while ((up = *uscan) != NULL && up->unit != (unit & 0x7F)) 514 uscan = &up->next_unit; 515 if (up == NULL) { 516 /* alloc unit, add to list and increment use count ASAP. */ 517 up = emalloc_zero(sizeof(*up)); 518 *uscan = up; 519 ++up->refcount; 520 521 /* initialize the unit structure */ 522 up->logname = estrdup(refnumtoa(&peer->srcadr)); 523 up->unit = unit & 0x7F; 524 up->fdt = -1; 525 up->addr = s_gpsd_addr; 526 up->tickpres = TICKOVER_LOW; 527 528 /* Create the device name and check for a Character 529 * Device. It's assumed that GPSD was started with the 530 * same link, so the names match. (If this is not 531 * practicable, we will have to read the symlink, if 532 * any, so we can get the true device file.) 533 */ 534 if (-1 == myasprintf(&up->device, "%s%u", 535 s_dev_stem, up->unit)) { 536 msyslog(LOG_ERR, "%s: clock device name too long", 537 up->logname); 538 goto dev_fail; 539 } 540 if (-1 == stat(up->device, &sb) || !S_ISCHR(sb.st_mode)) { 541 msyslog(LOG_ERR, "%s: '%s' is not a character device", 542 up->logname, up->device); 543 goto dev_fail; 544 } 545 } else { 546 /* All set up, just increment use count. */ 547 ++up->refcount; 548 } 549 550 /* setup refclock processing */ 551 pp->unitptr = (caddr_t)up; 552 pp->io.fd = -1; 553 pp->io.clock_recv = gpsd_receive; 554 pp->io.srcclock = peer; 555 pp->io.datalen = 0; 556 pp->a_lastcode[0] = '\0'; 557 pp->lencode = 0; 558 pp->clockdesc = DESCRIPTION; 559 memcpy(&pp->refid, REFID, 4); 560 561 /* Initialize miscellaneous variables */ 562 if (unit >= 128) 563 peer->precision = PPS_PRECISION; 564 else 565 peer->precision = PRECISION; 566 567 /* If the daemon name lookup failed, just give up now. */ 568 if (NULL == up->addr) { 569 msyslog(LOG_ERR, "%s: no GPSD socket address, giving up", 570 up->logname); 571 goto dev_fail; 572 } 573 574 LOGIF(CLOCKINFO, 575 (LOG_NOTICE, "%s: startup, device is '%s'", 576 refnumtoa(&peer->srcadr), up->device)); 577 up->mode = MODE_OP_MODE(peer->ttl); 578 if (up->mode > MODE_OP_MAXVAL) 579 up->mode = 0; 580 if (unit >= 128) 581 up->pps_peer = peer; 582 else 583 enter_opmode(peer, up->mode); 584 return TRUE; 585 586dev_fail: 587 /* On failure, remove all UNIT ressources and declare defeat. */ 588 589 INSIST (up); 590 if (!--up->refcount) { 591 *uscan = up->next_unit; 592 free(up->device); 593 free(up); 594 } 595 596 pp->unitptr = (caddr_t)NULL; 597 return FALSE; 598} 599 600/* ------------------------------------------------------------------ */ 601 602static void 603gpsd_shutdown( 604 int unit, 605 peerT * peer) 606{ 607 clockprocT * const pp = peer->procptr; 608 gpsd_unitT * const up = (gpsd_unitT *)pp->unitptr; 609 gpsd_unitT ** uscan = &s_clock_units; 610 611 UNUSED_ARG(unit); 612 613 /* The unit pointer might have been removed already. */ 614 if (up == NULL) 615 return; 616 617 /* now check if we must close IO resources */ 618 if (peer != up->pps_peer) { 619 if (-1 != pp->io.fd) { 620 DPRINTF(1, ("%s: closing clock, fd=%d\n", 621 up->logname, pp->io.fd)); 622 io_closeclock(&pp->io); 623 pp->io.fd = -1; 624 } 625 if (up->fdt != -1) 626 close(up->fdt); 627 } 628 /* decrement use count and eventually remove this unit. */ 629 if (!--up->refcount) { 630 /* unlink this unit */ 631 while (*uscan != NULL) 632 if (*uscan == up) 633 *uscan = up->next_unit; 634 else 635 uscan = &(*uscan)->next_unit; 636 free(up->logname); 637 free(up->device); 638 free(up); 639 } 640 pp->unitptr = (caddr_t)NULL; 641 LOGIF(CLOCKINFO, 642 (LOG_NOTICE, "%s: shutdown", refnumtoa(&peer->srcadr))); 643} 644 645/* ------------------------------------------------------------------ */ 646 647static void 648gpsd_receive( 649 struct recvbuf * rbufp) 650{ 651 /* declare & init control structure ptrs */ 652 peerT * const peer = rbufp->recv_peer; 653 clockprocT * const pp = peer->procptr; 654 gpsd_unitT * const up = (gpsd_unitT *)pp->unitptr; 655 656 const char *psrc, *esrc; 657 char *pdst, *edst, ch; 658 659 /* log the data stream, if this is enabled */ 660 log_data(peer, "recv", (const char*)rbufp->recv_buffer, 661 (size_t)rbufp->recv_length); 662 663 664 /* Since we're getting a raw stream data, we must assemble lines 665 * in our receive buffer. We can't use neither 'refclock_gtraw' 666 * not 'refclock_gtlin' here... We process chars until we reach 667 * an EoL (that is, line feed) but we truncate the message if it 668 * does not fit the buffer. GPSD might truncate messages, too, 669 * so dealing with truncated buffers is necessary anyway. 670 */ 671 psrc = (const char*)rbufp->recv_buffer; 672 esrc = psrc + rbufp->recv_length; 673 674 pdst = up->buffer + up->buflen; 675 edst = pdst + sizeof(up->buffer) - 1; /* for trailing NUL */ 676 677 while (psrc != esrc) { 678 ch = *psrc++; 679 if (ch == '\n') { 680 /* trim trailing whitespace & terminate buffer */ 681 while (pdst != up->buffer && pdst[-1] <= ' ') 682 --pdst; 683 *pdst = '\0'; 684 /* process data and reset buffer */ 685 up->buflen = pdst - up->buffer; 686 gpsd_parse(peer, &rbufp->recv_time); 687 pdst = up->buffer; 688 } else if (pdst != edst) { 689 /* add next char, ignoring leading whitespace */ 690 if (ch > ' ' || pdst != up->buffer) 691 *pdst++ = ch; 692 } 693 } 694 up->buflen = pdst - up->buffer; 695 up->tickover = TICKOVER_LOW; 696} 697 698/* ------------------------------------------------------------------ */ 699 700static void 701poll_primary( 702 peerT * const peer , 703 clockprocT * const pp , 704 gpsd_unitT * const up ) 705{ 706 if (pp->coderecv != pp->codeproc) { 707 /* all is well */ 708 pp->lastref = pp->lastrec; 709 refclock_report(peer, CEVNT_NOMINAL); 710 refclock_receive(peer); 711 } else { 712 /* Not working properly, admit to it. If we have no 713 * connection to GPSD, declare the clock as faulty. If 714 * there were bad replies, this is handled as the major 715 * cause, and everything else is just a timeout. 716 */ 717 peer->precision = PRECISION; 718 if (-1 == pp->io.fd) 719 refclock_report(peer, CEVNT_FAULT); 720 else if (0 != up->tc_breply) 721 refclock_report(peer, CEVNT_BADREPLY); 722 else 723 refclock_report(peer, CEVNT_TIMEOUT); 724 } 725 726 if (pp->sloppyclockflag & CLK_FLAG4) 727 mprintf_clock_stats( 728 &peer->srcadr,"%u %u %u %u %u %u %u", 729 up->tc_recv, 730 up->tc_breply, up->tc_nosync, 731 up->tc_sti_recv, up->tc_sti_used, 732 up->tc_pps_recv, up->tc_pps_used); 733 734 /* clear tallies for next round */ 735 up->tc_breply = 0; 736 up->tc_recv = 0; 737 up->tc_nosync = 0; 738 up->tc_sti_recv = 0; 739 up->tc_sti_used = 0; 740 up->tc_pps_recv = 0; 741 up->tc_pps_used = 0; 742} 743 744static void 745poll_secondary( 746 peerT * const peer , 747 clockprocT * const pp , 748 gpsd_unitT * const up ) 749{ 750 if (pp->coderecv != pp->codeproc) { 751 /* all is well */ 752 pp->lastref = pp->lastrec; 753 refclock_report(peer, CEVNT_NOMINAL); 754 refclock_receive(peer); 755 } else { 756 peer->precision = PPS_PRECISION; 757 peer->flags &= ~FLAG_PPS; 758 refclock_report(peer, CEVNT_TIMEOUT); 759 } 760} 761 762static void 763gpsd_poll( 764 int unit, 765 peerT * peer) 766{ 767 clockprocT * const pp = peer->procptr; 768 gpsd_unitT * const up = (gpsd_unitT *)pp->unitptr; 769 770 ++pp->polls; 771 if (peer == up->pps_peer) 772 poll_secondary(peer, pp, up); 773 else 774 poll_primary(peer, pp, up); 775} 776 777/* ------------------------------------------------------------------ */ 778 779static void 780gpsd_control( 781 int unit, 782 const struct refclockstat * in_st, 783 struct refclockstat * out_st, 784 peerT * peer ) 785{ 786 clockprocT * const pp = peer->procptr; 787 gpsd_unitT * const up = (gpsd_unitT *)pp->unitptr; 788 789 if (peer == up->pps_peer) { 790 DTOLFP(pp->fudgetime1, &up->pps_fudge2); 791 if ( ! (pp->sloppyclockflag & CLK_FLAG1)) 792 peer->flags &= ~FLAG_PPS; 793 } else { 794 /* save preprocessed fudge times */ 795 DTOLFP(pp->fudgetime1, &up->pps_fudge); 796 DTOLFP(pp->fudgetime2, &up->sti_fudge); 797 798 if (MODE_OP_MODE(up->mode ^ peer->ttl)) { 799 leave_opmode(peer, up->mode); 800 up->mode = MODE_OP_MODE(peer->ttl); 801 enter_opmode(peer, up->mode); 802 } 803 } 804 } 805 806/* ------------------------------------------------------------------ */ 807 808static void 809timer_primary( 810 peerT * const peer , 811 clockprocT * const pp , 812 gpsd_unitT * const up ) 813{ 814 int rc; 815 816 /* This is used for timeout handling. Nothing that needs 817 * sub-second precison happens here, so receive/connect/retry 818 * timeouts are simply handled by a count down, and then we 819 * decide what to do by the socket values. 820 * 821 * Note that the timer stays at zero here, unless some of the 822 * functions set it to another value. 823 */ 824 if (up->logthrottle) 825 --up->logthrottle; 826 if (up->tickover) 827 --up->tickover; 828 switch (up->tickover) { 829 case 4: 830 /* If we are connected to GPSD, try to get a live signal 831 * by querying the version. Otherwise just check the 832 * socket to become ready. 833 */ 834 if (-1 != pp->io.fd) { 835 size_t rlen = strlen(s_req_version); 836 DPRINTF(2, ("%s: timer livecheck: '%s'\n", 837 up->logname, s_req_version)); 838 log_data(peer, "send", s_req_version, rlen); 839 rc = write(pp->io.fd, s_req_version, rlen); 840 (void)rc; 841 } else if (-1 != up->fdt) { 842 gpsd_test_socket(peer); 843 } 844 break; 845 846 case 0: 847 if (-1 != pp->io.fd) 848 gpsd_stop_socket(peer); 849 else if (-1 != up->fdt) 850 gpsd_test_socket(peer); 851 else if (NULL != s_gpsd_addr) 852 gpsd_init_socket(peer); 853 break; 854 855 default: 856 if (-1 == pp->io.fd && -1 != up->fdt) 857 gpsd_test_socket(peer); 858 } 859} 860 861static void 862timer_secondary( 863 peerT * const peer , 864 clockprocT * const pp , 865 gpsd_unitT * const up ) 866{ 867 /* Reduce the count by one. Flush sample buffer and clear PPS 868 * flag when this happens. 869 */ 870 up->ppscount2 = max(0, (up->ppscount2 - 1)); 871 if (0 == up->ppscount2) { 872 if (pp->coderecv != pp->codeproc) { 873 refclock_report(peer, CEVNT_TIMEOUT); 874 pp->coderecv = pp->codeproc; 875 } 876 peer->flags &= ~FLAG_PPS; 877 } 878} 879 880static void 881gpsd_timer( 882 int unit, 883 peerT * peer) 884{ 885 clockprocT * const pp = peer->procptr; 886 gpsd_unitT * const up = (gpsd_unitT *)pp->unitptr; 887 888 if (peer == up->pps_peer) 889 timer_secondary(peer, pp, up); 890 else 891 timer_primary(peer, pp, up); 892} 893 894/* ===================================================================== 895 * handle opmode switches 896 */ 897 898static void 899enter_opmode( 900 peerT *peer, 901 int mode) 902{ 903 clockprocT * const pp = peer->procptr; 904 gpsd_unitT * const up = (gpsd_unitT *)pp->unitptr; 905 906 DPRINTF(1, ("%s: enter operation mode %d\n", 907 up->logname, MODE_OP_MODE(mode))); 908 909 if (MODE_OP_MODE(mode) == MODE_OP_AUTO) { 910 up->fl_rawsti = 0; 911 up->ppscount = PPS_MAXCOUNT / 2; 912 } 913 up->fl_pps = 0; 914 up->fl_sti = 0; 915} 916 917/* ------------------------------------------------------------------ */ 918 919static void 920leave_opmode( 921 peerT *peer, 922 int mode) 923{ 924 clockprocT * const pp = peer->procptr; 925 gpsd_unitT * const up = (gpsd_unitT *)pp->unitptr; 926 927 DPRINTF(1, ("%s: leaving operation mode %d\n", 928 up->logname, MODE_OP_MODE(mode))); 929 930 if (MODE_OP_MODE(mode) == MODE_OP_AUTO) { 931 up->fl_rawsti = 0; 932 up->ppscount = 0; 933 } 934 up->fl_pps = 0; 935 up->fl_sti = 0; 936} 937 938/* ===================================================================== 939 * operation mode specific evaluation 940 */ 941 942static void 943add_clock_sample( 944 peerT * const peer , 945 clockprocT * const pp , 946 l_fp stamp, 947 l_fp recvt) 948{ 949 pp->lastref = stamp; 950 if (pp->coderecv == pp->codeproc) 951 refclock_report(peer, CEVNT_NOMINAL); 952 refclock_process_offset(pp, stamp, recvt, pp->fudgetime1); 953} 954 955/* ------------------------------------------------------------------ */ 956 957static void 958eval_strict( 959 peerT * const peer , 960 clockprocT * const pp , 961 gpsd_unitT * const up ) 962{ 963 if (up->fl_sti && up->fl_pps) { 964 /* use TPV reference time + PPS receive time */ 965 add_clock_sample(peer, pp, up->sti_stamp, up->pps_recvt); 966 peer->precision = up->pps_prec; 967 /* both packets consumed now... */ 968 up->fl_pps = 0; 969 up->fl_sti = 0; 970 ++up->tc_sti_used; 971 } 972} 973 974/* ------------------------------------------------------------------ */ 975/* PPS processing for the secondary channel. GPSD provides us with full 976 * timing information, so there's no danger of PLL-locking to the wrong 977 * second. The belts and suspenders needed for the raw ATOM clock are 978 * unnecessary here. 979 */ 980static void 981eval_pps_secondary( 982 peerT * const peer , 983 clockprocT * const pp , 984 gpsd_unitT * const up ) 985{ 986 if (up->fl_pps2) { 987 /* feed data */ 988 add_clock_sample(peer, pp, up->pps_stamp2, up->pps_recvt2); 989 peer->precision = up->pps_prec; 990 /* PPS peer flag logic */ 991 up->ppscount2 = min(PPS2_MAXCOUNT, (up->ppscount2 + 2)); 992 if ((PPS2_MAXCOUNT == up->ppscount2) && 993 (pp->sloppyclockflag & CLK_FLAG1) ) 994 peer->flags |= FLAG_PPS; 995 /* mark time stamp as burned... */ 996 up->fl_pps2 = 0; 997 ++up->tc_pps_used; 998 } 999} 1000 1001/* ------------------------------------------------------------------ */ 1002 1003static void 1004eval_serial( 1005 peerT * const peer , 1006 clockprocT * const pp , 1007 gpsd_unitT * const up ) 1008{ 1009 if (up->fl_sti) { 1010 add_clock_sample(peer, pp, up->sti_stamp, up->sti_recvt); 1011 peer->precision = up->sti_prec; 1012 /* mark time stamp as burned... */ 1013 up->fl_sti = 0; 1014 ++up->tc_sti_used; 1015 } 1016} 1017 1018/* ------------------------------------------------------------------ */ 1019static void 1020eval_auto( 1021 peerT * const peer , 1022 clockprocT * const pp , 1023 gpsd_unitT * const up ) 1024{ 1025 /* If there's no TPV available, stop working here... */ 1026 if (!up->fl_sti) 1027 return; 1028 1029 /* check how to handle STI+PPS: Can PPS be used to augment STI 1030 * (or vice versae), do we drop the sample because there is a 1031 * temporary missing PPS signal, or do we feed on STI time 1032 * stamps alone? 1033 * 1034 * Do a counter/threshold dance to decide how to proceed. 1035 */ 1036 if (up->fl_pps) { 1037 up->ppscount = min(PPS_MAXCOUNT, 1038 (up->ppscount + PPS_INCCOUNT)); 1039 if ((PPS_MAXCOUNT == up->ppscount) && up->fl_rawsti) { 1040 up->fl_rawsti = 0; 1041 msyslog(LOG_INFO, 1042 "%s: expect valid PPS from now", 1043 up->logname); 1044 } 1045 } else { 1046 up->ppscount = max(0, (up->ppscount - PPS_DECCOUNT)); 1047 if ((0 == up->ppscount) && !up->fl_rawsti) { 1048 up->fl_rawsti = -1; 1049 msyslog(LOG_WARNING, 1050 "%s: use TPV alone from now", 1051 up->logname); 1052 } 1053 } 1054 1055 /* now eventually feed the sample */ 1056 if (up->fl_rawsti) 1057 eval_serial(peer, pp, up); 1058 else 1059 eval_strict(peer, pp, up); 1060} 1061 1062/* ===================================================================== 1063 * JSON parsing stuff 1064 */ 1065 1066/* ------------------------------------------------------------------ */ 1067/* Parse a decimal integer with a possible sign. Works like 'strtoll()' 1068 * or 'strtol()', but with a fixed base of 10 and without eating away 1069 * leading whitespace. For the error codes, the handling of the end 1070 * pointer and the return values see 'strtol()'. 1071 */ 1072static json_int 1073strtojint( 1074 const char *cp, char **ep) 1075{ 1076 json_uint accu, limit_lo, limit_hi; 1077 int flags; /* bit 0: overflow; bit 1: sign */ 1078 const char * hold; 1079 1080 /* pointer union to circumvent a tricky/sticky const issue */ 1081 union { const char * c; char * v; } vep; 1082 1083 /* store initial value of 'cp' -- see 'strtol()' */ 1084 vep.c = cp; 1085 1086 /* Eat away an optional sign and set the limits accordingly: The 1087 * high limit is the maximum absolute value that can be returned, 1088 * and the low limit is the biggest value that does not cause an 1089 * overflow when multiplied with 10. Avoid negation overflows. 1090 */ 1091 if (*cp == '-') { 1092 cp += 1; 1093 flags = 2; 1094 limit_hi = (json_uint)-(JSON_INT_MIN + 1) + 1; 1095 } else { 1096 cp += (*cp == '+'); 1097 flags = 0; 1098 limit_hi = (json_uint)JSON_INT_MAX; 1099 } 1100 limit_lo = limit_hi / 10; 1101 1102 /* Now try to convert a sequence of digits. */ 1103 hold = cp; 1104 accu = 0; 1105 while (isdigit(*(const u_char*)cp)) { 1106 flags |= (accu > limit_lo); 1107 accu = accu * 10 + (*(const u_char*)cp++ - '0'); 1108 flags |= (accu > limit_hi); 1109 } 1110 /* Check for empty conversion (no digits seen). */ 1111 if (hold != cp) 1112 vep.c = cp; 1113 else 1114 errno = EINVAL; /* accu is still zero */ 1115 /* Check for range overflow */ 1116 if (flags & 1) { 1117 errno = ERANGE; 1118 accu = limit_hi; 1119 } 1120 /* If possible, store back the end-of-conversion pointer */ 1121 if (ep) 1122 *ep = vep.v; 1123 /* If negative, return the negated result if the accu is not 1124 * zero. Avoid negation overflows. 1125 */ 1126 if ((flags & 2) && accu) 1127 return -(json_int)(accu - 1) - 1; 1128 else 1129 return (json_int)accu; 1130} 1131 1132/* ------------------------------------------------------------------ */ 1133 1134static tok_ref 1135json_token_skip( 1136 const json_ctx * ctx, 1137 tok_ref tid) 1138{ 1139 if (tid >= 0 && (u_int)tid < ctx->ntok) { 1140 int len = ctx->tok[tid].size; 1141 /* For arrays and objects, the size is the number of 1142 * ITEMS in the compound. Thats the number of objects in 1143 * the array, and the number of key/value pairs for 1144 * objects. In theory, the key must be a string, and we 1145 * could simply skip one token before skipping the 1146 * value, which can be anything. We're a bit paranoid 1147 * and lazy at the same time: We simply double the 1148 * number of tokens to skip and fall through into the 1149 * array processing when encountering an object. 1150 */ 1151 switch (ctx->tok[tid].type) { 1152 case JSMN_OBJECT: 1153 len *= 2; 1154 /* FALLTHROUGH */ 1155 case JSMN_ARRAY: 1156 for (++tid; len; --len) 1157 tid = json_token_skip(ctx, tid); 1158 break; 1159 1160 default: 1161 ++tid; 1162 break; 1163 } 1164 /* The next condition should never be true, but paranoia 1165 * prevails... 1166 */ 1167 if (tid < 0 || (u_int)tid > ctx->ntok) 1168 tid = ctx->ntok; 1169 } 1170 return tid; 1171} 1172 1173/* ------------------------------------------------------------------ */ 1174 1175static int 1176json_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 || (u_int)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 1209static const char* 1210json_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 */ 1225static int 1226json_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 1242static const char* 1243json_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 1254static const char* 1255json_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 1269static json_int 1270json_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 1290static json_int 1291json_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 */ 1312static double 1313json_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 1334static double 1335json_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 1356static BOOL 1357json_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 */ 1389static BOOL 1390get_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 */ 1417static void 1418process_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 1443static void 1444process_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, 1509 * using JSON data and selecting the GPS device name we created 1510 * from our unit number. We have an old a newer version that 1511 * request PPS (and TOFF) 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 1531static void 1532process_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 1612static void 1613process_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 1693static void 1694process_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 1737static void 1738gpsd_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 1819static void 1820gpsd_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 1847static void 1848gpsd_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 (void *)&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 1953static void 1954gpsd_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, (void *)&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 */ 2053static int16_t 2054clamped_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 */ 2067static BOOL 2068convert_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 */ 2110static void 2111save_ltc( 2112 clockprocT * const pp, 2113 const char * const tc) 2114{ 2115 size_t len = 0; 2116 2117 if (tc) { 2118 len = strlen(tc); 2119 if (len >= sizeof(pp->a_lastcode)) 2120 len = sizeof(pp->a_lastcode) - 1; 2121 memcpy(pp->a_lastcode, tc, len); 2122 } 2123 pp->lencode = (u_short)len; 2124 pp->a_lastcode[len] = '\0'; 2125} 2126 2127/* ------------------------------------------------------------------- 2128 * asprintf replacement... it's not available everywhere... 2129 */ 2130static int 2131myasprintf( 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 2161static char * 2162add_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 2172static void 2173log_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 2209NONEMPTY_TRANSLATION_UNIT 2210#endif /* REFCLOCK && CLOCK_GPSDJSON */ 2211