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