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 * Note: This will currently NOT work with Windows due to some 10 * limitations: 11 * 12 * - There is no GPSD for Windows. (There is an unofficial port to 13 * cygwin, but Windows is not officially supported.) 14 * 15 * - To work properly, this driver needs PPS and TPV sentences from 16 * GPSD. I don't see how the cygwin port should deal with that. 17 * 18 * - The device name matching must be done in a different way for 19 * Windows. (Can be done with COMxx matching, as done for NMEA.) 20 * 21 * Apart from those minor hickups, once GPSD has been fully ported to 22 * Windows, there's no reason why this should not work there ;-) 23 */ 24 25 #ifdef HAVE_CONFIG_H 26 #include <config.h> 27 #endif 28 29 #include "ntp_types.h" 30 31 #if defined(REFCLOCK) && defined(CLOCK_GPSDJSON) && !defined(SYS_WINNT) 32 33 /* ===================================================================== 34 * get the little JSMN library directly into our guts 35 */ 36 #include "../libjsmn/jsmn.c" 37 38 /* ===================================================================== 39 * header stuff we need 40 */ 41 42 #include <netdb.h> 43 #include <unistd.h> 44 #include <fcntl.h> 45 #include <string.h> 46 #include <ctype.h> 47 48 #include <sys/types.h> 49 #include <sys/socket.h> 50 #include <sys/stat.h> 51 #include <netinet/tcp.h> 52 53 #if defined(HAVE_SYS_POLL_H) 54 # include <sys/poll.h> 55 #elif defined(HAVE_SYS_SELECT_H) 56 # include <sys/select.h> 57 #else 58 # error need poll() or select() 59 #endif 60 61 #include "ntpd.h" 62 #include "ntp_io.h" 63 #include "ntp_unixtime.h" 64 #include "ntp_refclock.h" 65 #include "ntp_stdlib.h" 66 #include "ntp_calendar.h" 67 #include "timespecops.h" 68 69 #define PRECISION (-9) /* precision assumed (about 2 ms) */ 70 #define PPS_PRECISION (-20) /* precision assumed (about 1 us) */ 71 #define REFID "GPSD" /* reference id */ 72 #define DESCRIPTION "GPSD JSON client clock" /* who we are */ 73 74 #define MAX_PDU_LEN 1600 75 #define TICKOVER_LOW 10 76 #define TICKOVER_HIGH 120 77 #define LOGTHROTTLE 3600 78 79 #define PPS_MAXCOUNT 30 80 #define PPS_HIWAT 20 81 #define PPS_LOWAT 10 82 83 #ifndef BOOL 84 # define BOOL int 85 #endif 86 #ifndef TRUE 87 # define TRUE 1 88 #endif 89 #ifndef FALSE 90 # define FALSE 0 91 #endif 92 93 /* some local typedefs : The NTPD formatting style cries for short type 94 * names, and we provide them locally. Note:the suffix '_t' is reserved 95 * for the standard; I use a capital T instead. 96 */ 97 typedef struct peer peerT; 98 typedef struct refclockproc clockprocT; 99 typedef struct addrinfo addrinfoT; 100 101 /* ===================================================================== 102 * We use the same device name scheme as does the NMEA driver; since 103 * GPSD supports the same links, we can select devices by a fixed name. 104 */ 105 static const char * s_dev_stem = "/dev/gps"; 106 107 /* ===================================================================== 108 * forward declarations for transfer vector and the vector itself 109 */ 110 111 static void gpsd_init (void); 112 static int gpsd_start (int, peerT *); 113 static void gpsd_shutdown (int, peerT *); 114 static void gpsd_receive (struct recvbuf *); 115 static void gpsd_poll (int, peerT *); 116 static void gpsd_control (int, const struct refclockstat *, 117 struct refclockstat *, peerT *); 118 static void gpsd_timer (int, peerT *); 119 static void gpsd_clockstats (int, peerT *); 120 121 static int myasprintf(char**, char const*, ...); 122 123 struct refclock refclock_gpsdjson = { 124 gpsd_start, /* start up driver */ 125 gpsd_shutdown, /* shut down driver */ 126 gpsd_poll, /* transmit poll message */ 127 gpsd_control, /* fudge control */ 128 gpsd_init, /* initialize driver */ 129 noentry, /* buginfo */ 130 gpsd_timer /* called once per second */ 131 }; 132 133 /* ===================================================================== 134 * our local clock unit and data 135 */ 136 typedef struct gpsd_unit { 137 int unit; 138 /* current line protocol version */ 139 uint16_t proto_major; 140 uint16_t proto_minor; 141 142 /* PPS time stamps */ 143 l_fp pps_local; /* when we received the PPS message */ 144 l_fp pps_stamp; /* related reference time */ 145 l_fp pps_recvt; /* when GPSD detected the pulse */ 146 147 /* TPV (GPS data) time stamps */ 148 l_fp tpv_local; /* when we received the TPV message */ 149 l_fp tpv_stamp; /* effective GPS time stamp */ 150 l_fp tpv_recvt; /* when GPSD got the fix */ 151 152 /* fudge values for correction, mirrored as 'l_fp' */ 153 l_fp pps_fudge; 154 l_fp tpv_fudge; 155 156 /* Flags to indicate available data */ 157 int fl_tpv : 1; /* valid TPV seen (have time) */ 158 int fl_pps : 1; /* valid pulse seen */ 159 int fl_vers : 1; /* have protocol version */ 160 int fl_watch : 1; /* watch reply seen */ 161 int fl_nsec : 1; /* have nanosec PPS info */ 162 163 /* admin stuff for sockets and device selection */ 164 int fdt; /* current connecting socket */ 165 addrinfoT * addr; /* next address to try */ 166 u_int tickover; /* timeout countdown */ 167 u_int tickpres; /* timeout preset */ 168 u_int ppscount; /* PPS mode up/down count */ 169 char * device; /* device name of unit */ 170 171 /* tallies for the various events */ 172 u_int tc_good; /* good samples received */ 173 u_int tc_btime; /* bad time stamps */ 174 u_int tc_bdate; /* bad date strings */ 175 u_int tc_breply; /* bad replies */ 176 u_int tc_recv; /* received known records */ 177 178 /* log bloat throttle */ 179 u_int logthrottle;/* seconds to next log slot */ 180 181 /* record assemby buffer and saved length */ 182 int buflen; 183 char buffer[MAX_PDU_LEN]; 184 } gpsd_unitT; 185 186 /* ===================================================================== 187 * static local helpers forward decls 188 */ 189 static void gpsd_init_socket(peerT * const peer); 190 static void gpsd_test_socket(peerT * const peer); 191 static void gpsd_stop_socket(peerT * const peer); 192 193 static void gpsd_parse(peerT * const peer, 194 const l_fp * const rtime); 195 static BOOL convert_ascii_time(l_fp * fp, const char * gps_time); 196 static void save_ltc(clockprocT * const pp, const char * const tc); 197 static int syslogok(clockprocT * const pp, gpsd_unitT * const up); 198 199 /* ===================================================================== 200 * local / static stuff 201 */ 202 203 /* The logon string is actually the ?WATCH command of GPSD, using JSON 204 * data and selecting the GPS device name we created from our unit 205 * number. [Note: This is a format string!] 206 */ 207 #define s_logon \ 208 "?WATCH={\"enable\":true,\"json\":true,\"device\":\"%s\"};\r\n" 209 210 /* We keep a static list of network addresses for 'localhost:gpsd', and 211 * we try to connect to them in round-robin fashion. 212 */ 213 static addrinfoT * s_gpsd_addr; 214 215 /* ===================================================================== 216 * log throttling 217 */ 218 static int/*BOOL*/ 219 syslogok( 220 clockprocT * const pp, 221 gpsd_unitT * const up) 222 { 223 int res = (0 != (pp->sloppyclockflag & CLK_FLAG3)) 224 || (0 == up->logthrottle ) 225 || (LOGTHROTTLE == up->logthrottle ); 226 if (res) 227 up->logthrottle = LOGTHROTTLE; 228 return res; 229 } 230 231 /* ===================================================================== 232 * the clock functions 233 */ 234 235 /* --------------------------------------------------------------------- 236 * Init: This currently just gets the socket address for the GPS daemon 237 */ 238 static void 239 gpsd_init(void) 240 { 241 addrinfoT hints; 242 243 memset(&hints, 0, sizeof(hints)); 244 hints.ai_family = AF_UNSPEC; 245 hints.ai_protocol = IPPROTO_TCP; 246 hints.ai_socktype = SOCK_STREAM; 247 248 /* just take the first configured address of localhost... */ 249 if (getaddrinfo("localhost", "gpsd", &hints, &s_gpsd_addr)) 250 s_gpsd_addr = NULL; 251 } 252 253 /* --------------------------------------------------------------------- 254 * Start: allocate a unit pointer and set up the runtime data 255 */ 256 257 static int 258 gpsd_start( 259 int unit, 260 peerT * peer) 261 { 262 clockprocT * const pp = peer->procptr; 263 gpsd_unitT * const up = emalloc_zero(sizeof(*up)); 264 265 struct stat sb; 266 267 /* initialize the unit structure */ 268 up->fdt = -1; 269 up->addr = s_gpsd_addr; 270 up->tickpres = TICKOVER_LOW; 271 272 /* setup refclock processing */ 273 up->unit = unit; 274 pp->unitptr = (caddr_t)up; 275 pp->io.fd = -1; 276 pp->io.clock_recv = gpsd_receive; 277 pp->io.srcclock = peer; 278 pp->io.datalen = 0; 279 pp->a_lastcode[0] = '\0'; 280 pp->lencode = 0; 281 pp->clockdesc = DESCRIPTION; 282 memcpy(&pp->refid, REFID, 4); 283 284 /* Initialize miscellaneous variables */ 285 peer->precision = PRECISION; 286 287 /* Create the device name and check for a Character Device. It's 288 * assumed that GPSD was started with the same link, so the 289 * names match. (If this is not practicable, we will have to 290 * read the symlink, if any, so we can get the true device 291 * file.) 292 */ 293 if (-1 == myasprintf(&up->device, "%s%u", s_dev_stem, unit)) { 294 msyslog(LOG_ERR, "%s clock device name too long", 295 refnumtoa(&peer->srcadr)); 296 goto dev_fail; 297 } 298 if (-1 == stat(up->device, &sb) || !S_ISCHR(sb.st_mode)) { 299 msyslog(LOG_ERR, "%s: '%s' is not a character device", 300 refnumtoa(&peer->srcadr), up->device); 301 goto dev_fail; 302 } 303 LOGIF(CLOCKINFO, 304 (LOG_NOTICE, "%s: startup, device is '%s'", 305 refnumtoa(&peer->srcadr), up->device)); 306 return TRUE; 307 308 dev_fail: 309 /* On failure, remove all UNIT ressources and declare defeat. */ 310 311 INSIST (up); 312 free(up->device); 313 free(up); 314 315 pp->unitptr = (caddr_t)NULL; 316 return FALSE; 317 } 318 319 /* ------------------------------------------------------------------ */ 320 321 static void 322 gpsd_shutdown( 323 int unit, 324 peerT * peer) 325 { 326 clockprocT * const pp = peer->procptr; 327 gpsd_unitT * const up = (gpsd_unitT *)pp->unitptr; 328 329 UNUSED_ARG(unit); 330 331 if (up) { 332 free(up->device); 333 free(up); 334 } 335 pp->unitptr = (caddr_t)NULL; 336 if (-1 != pp->io.fd) 337 io_closeclock(&pp->io); 338 pp->io.fd = -1; 339 LOGIF(CLOCKINFO, 340 (LOG_NOTICE, "%s: shutdown", refnumtoa(&peer->srcadr))); 341 } 342 343 /* ------------------------------------------------------------------ */ 344 345 static void 346 gpsd_receive( 347 struct recvbuf * rbufp) 348 { 349 /* declare & init control structure ptrs */ 350 peerT * const peer = rbufp->recv_peer; 351 clockprocT * const pp = peer->procptr; 352 gpsd_unitT * const up = (gpsd_unitT *)pp->unitptr; 353 354 const char *psrc, *esrc; 355 char *pdst, *edst, ch; 356 357 /* Since we're getting a raw stream data, we must assemble lines 358 * in our receive buffer. We can't use neither 'refclock_gtraw' 359 * not 'refclock_gtlin' here... We process chars until we reach 360 * an EoL (that is, line feed) but we truncate the message if it 361 * does not fit the buffer. GPSD might truncate messages, too, 362 * so dealing with truncated buffers is necessary anyway. 363 */ 364 psrc = (const char*)rbufp->recv_buffer; 365 esrc = psrc + rbufp->recv_length; 366 367 pdst = up->buffer + up->buflen; 368 edst = pdst + sizeof(up->buffer) - 1; /* for trailing NUL */ 369 370 while (psrc != esrc) { 371 ch = *psrc++; 372 if (ch == '\n') { 373 /* trim trailing whitespace & terminate buffer */ 374 while (pdst != up->buffer && pdst[-1] <= ' ') 375 --pdst; 376 *pdst = '\0'; 377 /* process data and reset buffer */ 378 gpsd_parse(peer, &rbufp->recv_time); 379 pdst = up->buffer; 380 } else if (pdst != edst) { 381 /* add next char, ignoring leading whitespace */ 382 if (ch > ' ' || pdst != up->buffer) 383 *pdst++ = ch; 384 } 385 } 386 up->buflen = pdst - up->buffer; 387 up->tickover = TICKOVER_LOW; 388 } 389 390 /* ------------------------------------------------------------------ */ 391 392 static void 393 gpsd_poll( 394 int unit, 395 peerT * peer) 396 { 397 clockprocT * const pp = peer->procptr; 398 gpsd_unitT * const up = (gpsd_unitT *)pp->unitptr; 399 u_int tc_max; 400 401 ++pp->polls; 402 403 /* find the dominant error */ 404 tc_max = max(up->tc_btime, up->tc_bdate); 405 tc_max = max(tc_max, up->tc_breply); 406 407 if (pp->coderecv != pp->codeproc) { 408 /* all is well */ 409 pp->lastref = pp->lastrec; 410 refclock_receive(peer); 411 } else { 412 /* not working properly, admit to it */ 413 peer->flags &= ~FLAG_PPS; 414 peer->precision = PRECISION; 415 416 if (-1 == pp->io.fd) { 417 /* not connected to GPSD: clearly not working! */ 418 refclock_report(peer, CEVNT_FAULT); 419 } else if (tc_max == up->tc_breply) { 420 refclock_report(peer, CEVNT_BADREPLY); 421 } else if (tc_max == up->tc_btime) { 422 refclock_report(peer, CEVNT_BADTIME); 423 } else if (tc_max == up->tc_bdate) { 424 refclock_report(peer, CEVNT_BADDATE); 425 } else { 426 refclock_report(peer, CEVNT_TIMEOUT); 427 } 428 } 429 430 if (pp->sloppyclockflag & CLK_FLAG4) 431 gpsd_clockstats(unit, peer); 432 433 /* clear tallies for next round */ 434 up->tc_good = up->tc_btime = up->tc_bdate = 435 up->tc_breply = up->tc_recv = 0; 436 } 437 438 /* ------------------------------------------------------------------ */ 439 440 static void 441 gpsd_control( 442 int unit, 443 const struct refclockstat * in_st, 444 struct refclockstat * out_st, 445 peerT * peer ) 446 { 447 clockprocT * const pp = peer->procptr; 448 gpsd_unitT * const up = (gpsd_unitT *)pp->unitptr; 449 450 /* save preprocessed fudge times */ 451 DTOLFP(pp->fudgetime1, &up->pps_fudge); 452 DTOLFP(pp->fudgetime2, &up->tpv_fudge); 453 } 454 455 /* ------------------------------------------------------------------ */ 456 457 static void 458 gpsd_timer( 459 int unit, 460 peerT * peer) 461 { 462 static const char query[] = "?VERSION;"; 463 464 clockprocT * const pp = peer->procptr; 465 gpsd_unitT * const up = (gpsd_unitT *)pp->unitptr; 466 int rc; 467 468 /* This is used for timeout handling. Nothing that needs 469 * sub-second precison happens here, so receive/connect/retry 470 * timeouts are simply handled by a count down, and then we 471 * decide what to do by the socket values. 472 * 473 * Note that the timer stays at zero here, unless some of the 474 * functions set it to another value. 475 */ 476 if (up->logthrottle) 477 --up->logthrottle; 478 if (up->tickover) 479 --up->tickover; 480 switch (up->tickover) { 481 case 4: 482 /* try to get a live signal 483 * If the device is not yet present, we will most likely 484 * get an error. We put out a new version request, 485 * because the reply will initiate a new watch request 486 * cycle. 487 */ 488 if (-1 != pp->io.fd) { 489 if ( ! up->fl_watch) { 490 DPRINTF(2, ("GPSD_JSON(%d): timer livecheck: '%s'\n", 491 up->unit, query)); 492 rc = write(pp->io.fd, 493 query, sizeof(query)); 494 (void)rc; 495 } 496 } else if (-1 != up->fdt) { 497 gpsd_test_socket(peer); 498 } 499 break; 500 501 case 0: 502 if (-1 != pp->io.fd) 503 gpsd_stop_socket(peer); 504 else if (-1 != up->fdt) 505 gpsd_test_socket(peer); 506 else if (NULL != s_gpsd_addr) 507 gpsd_init_socket(peer); 508 break; 509 510 default: 511 if (-1 == pp->io.fd && -1 != up->fdt) 512 gpsd_test_socket(peer); 513 } 514 515 if (up->ppscount > PPS_HIWAT && !(peer->flags & FLAG_PPS)) 516 peer->flags |= FLAG_PPS; 517 if (up->ppscount < PPS_LOWAT && (peer->flags & FLAG_PPS)) 518 peer->flags &= ~FLAG_PPS; 519 } 520 521 /* ===================================================================== 522 * JSON parsing stuff 523 */ 524 525 #define JSMN_MAXTOK 100 526 #define INVALID_TOKEN (-1) 527 528 typedef struct json_ctx { 529 char * buf; 530 int ntok; 531 jsmntok_t tok[JSMN_MAXTOK]; 532 } json_ctx; 533 534 typedef int tok_ref; 535 536 #ifdef HAVE_LONG_LONG 537 typedef long long json_int; 538 #define JSON_STRING_TO_INT strtoll 539 #else 540 typedef long json_int; 541 #define JSON_STRING_TO_INT strtol 542 #endif 543 544 /* ------------------------------------------------------------------ */ 545 546 static tok_ref 547 json_token_skip( 548 const json_ctx * ctx, 549 tok_ref tid) 550 { 551 int len; 552 len = ctx->tok[tid].size; 553 for (++tid; len; --len) 554 if (tid < ctx->ntok) 555 tid = json_token_skip(ctx, tid); 556 else 557 break; 558 if (tid > ctx->ntok) 559 tid = ctx->ntok; 560 return tid; 561 } 562 563 /* ------------------------------------------------------------------ */ 564 565 static int 566 json_object_lookup( 567 const json_ctx * ctx, 568 tok_ref tid, 569 const char * key) 570 { 571 int len; 572 573 if (tid >= ctx->ntok || ctx->tok[tid].type != JSMN_OBJECT) 574 return INVALID_TOKEN; 575 len = ctx->ntok - tid - 1; 576 if (len > ctx->tok[tid].size) 577 len = ctx->tok[tid].size; 578 for (tid += 1; len > 1; len-=2) { 579 if (ctx->tok[tid].type != JSMN_STRING) 580 continue; /* hmmm... that's an error, strictly speaking */ 581 if (!strcmp(key, ctx->buf + ctx->tok[tid].start)) 582 return tid + 1; 583 tid = json_token_skip(ctx, tid + 1); 584 } 585 return INVALID_TOKEN; 586 } 587 588 /* ------------------------------------------------------------------ */ 589 590 #if 0 /* currently unused */ 591 static const char* 592 json_object_lookup_string( 593 const json_ctx * ctx, 594 tok_ref tid, 595 const char * key) 596 { 597 tok_ref val_ref; 598 val_ref = json_object_lookup(ctx, tid, key); 599 if (INVALID_TOKEN == val_ref || 600 JSMN_STRING != ctx->tok[val_ref].type ) 601 goto cvt_error; 602 return ctx->buf + ctx->tok[val_ref].start; 603 604 cvt_error: 605 errno = EINVAL; 606 return NULL; 607 } 608 #endif 609 610 static const char* 611 json_object_lookup_string_default( 612 const json_ctx * ctx, 613 tok_ref tid, 614 const char * key, 615 const char * def) 616 { 617 tok_ref val_ref; 618 val_ref = json_object_lookup(ctx, tid, key); 619 if (INVALID_TOKEN == val_ref || 620 JSMN_STRING != ctx->tok[val_ref].type ) 621 return def; 622 return ctx->buf + ctx->tok[val_ref].start; 623 } 624 625 /* ------------------------------------------------------------------ */ 626 627 static json_int 628 json_object_lookup_int( 629 const json_ctx * ctx, 630 tok_ref tid, 631 const char * key) 632 { 633 json_int ret; 634 tok_ref val_ref; 635 char * ep; 636 637 val_ref = json_object_lookup(ctx, tid, key); 638 if (INVALID_TOKEN == val_ref || 639 JSMN_PRIMITIVE != ctx->tok[val_ref].type ) 640 goto cvt_error; 641 ret = JSON_STRING_TO_INT( 642 ctx->buf + ctx->tok[val_ref].start, &ep, 10); 643 if (*ep) 644 goto cvt_error; 645 return ret; 646 647 cvt_error: 648 errno = EINVAL; 649 return 0; 650 } 651 652 static json_int 653 json_object_lookup_int_default( 654 const json_ctx * ctx, 655 tok_ref tid, 656 const char * key, 657 json_int def) 658 { 659 json_int retv; 660 int esave; 661 662 esave = errno; 663 errno = 0; 664 retv = json_object_lookup_int(ctx, tid, key); 665 if (0 != errno) 666 retv = def; 667 errno = esave; 668 return retv; 669 } 670 671 /* ------------------------------------------------------------------ */ 672 673 static double 674 json_object_lookup_float( 675 const json_ctx * ctx, 676 tok_ref tid, 677 const char * key) 678 { 679 double ret; 680 tok_ref val_ref; 681 char * ep; 682 683 val_ref = json_object_lookup(ctx, tid, key); 684 if (INVALID_TOKEN == val_ref || 685 JSMN_PRIMITIVE != ctx->tok[val_ref].type ) 686 goto cvt_error; 687 ret = strtod(ctx->buf + ctx->tok[val_ref].start, &ep); 688 if (*ep) 689 goto cvt_error; 690 return ret; 691 692 cvt_error: 693 errno = EINVAL; 694 return 0.0; 695 } 696 697 static double 698 json_object_lookup_float_default( 699 const json_ctx * ctx, 700 tok_ref tid, 701 const char * key, 702 double def) 703 { 704 double retv; 705 int esave; 706 707 esave = errno; 708 errno = 0; 709 retv = json_object_lookup_float(ctx, tid, key); 710 if (0 != errno) 711 retv = def; 712 errno = esave; 713 return retv; 714 } 715 716 /* ------------------------------------------------------------------ */ 717 718 static BOOL 719 json_parse_record( 720 json_ctx * ctx, 721 char * buf) 722 { 723 jsmn_parser jsm; 724 int idx, rc; 725 726 jsmn_init(&jsm); 727 rc = jsmn_parse(&jsm, buf, ctx->tok, JSMN_MAXTOK); 728 ctx->buf = buf; 729 ctx->ntok = jsm.toknext; 730 731 /* Make all tokens NUL terminated by overwriting the 732 * terminator symbol 733 */ 734 for (idx = 0; idx < jsm.toknext; ++idx) 735 if (ctx->tok[idx].end > ctx->tok[idx].start) 736 ctx->buf[ctx->tok[idx].end] = '\0'; 737 738 if (JSMN_ERROR_PART != rc && 739 JSMN_ERROR_NOMEM != rc && 740 JSMN_SUCCESS != rc ) 741 return FALSE; /* not parseable - bail out */ 742 743 if (0 >= jsm.toknext || JSMN_OBJECT != ctx->tok[0].type) 744 return FALSE; /* not object or no data!?! */ 745 746 return TRUE; 747 } 748 749 750 /* ===================================================================== 751 * static local helpers 752 */ 753 754 /* ------------------------------------------------------------------ */ 755 /* Process a WATCH record 756 * 757 * Currently this is only used to recognise that the device is present 758 * and that we're listed subscribers. 759 */ 760 static void 761 process_watch( 762 peerT * const peer , 763 json_ctx * const jctx , 764 const l_fp * const rtime) 765 { 766 clockprocT * const pp = peer->procptr; 767 gpsd_unitT * const up = (gpsd_unitT *)pp->unitptr; 768 769 up->fl_watch = -1; 770 } 771 772 /* ------------------------------------------------------------------ */ 773 774 static void 775 process_version( 776 peerT * const peer , 777 json_ctx * const jctx , 778 const l_fp * const rtime) 779 { 780 clockprocT * const pp = peer->procptr; 781 gpsd_unitT * const up = (gpsd_unitT *)pp->unitptr; 782 783 int len; 784 char * buf; 785 const char *revision; 786 const char *release; 787 788 /* get protocol version number */ 789 revision = json_object_lookup_string_default( 790 jctx, 0, "rev", "(unknown)"); 791 release = json_object_lookup_string_default( 792 jctx, 0, "release", "(unknown)"); 793 errno = 0; 794 up->proto_major = (uint16_t)json_object_lookup_int( 795 jctx, 0, "proto_major"); 796 up->proto_minor = (uint16_t)json_object_lookup_int( 797 jctx, 0, "proto_minor"); 798 if (0 == errno) { 799 up->fl_vers = -1; 800 if (syslogok(pp, up)) 801 msyslog(LOG_INFO, 802 "%s: GPSD revision=%s release=%s protocol=%u.%u", 803 refnumtoa(&peer->srcadr), 804 revision, release, 805 up->proto_major, up->proto_minor); 806 } 807 808 /* With the 3.9 GPSD protocol, '*_musec' vanished and was 809 * replace by '*_nsec'. Dispatch properly. 810 */ 811 if ( up->proto_major > 3 || 812 (up->proto_major == 3 && up->proto_minor >= 9)) 813 up->fl_nsec = -1; 814 else 815 up->fl_nsec = 0; 816 817 /*TODO: validate protocol version! */ 818 819 /* request watch for our GPS device 820 * Reuse the input buffer, which is no longer needed in the 821 * current cycle. Also assume that we can write the watch 822 * request in one sweep into the socket; since we do not do 823 * output otherwise, this should always work. (Unless the 824 * TCP/IP window size gets lower than the length of the 825 * request. We handle that when it happens.) 826 */ 827 snprintf(up->buffer, sizeof(up->buffer), 828 s_logon, up->device); 829 buf = up->buffer; 830 len = strlen(buf); 831 if (len != write(pp->io.fd, buf, len)) { 832 /*Note: if the server fails to read our request, the 833 * resulting data timeout will take care of the 834 * connection! 835 */ 836 if (syslogok(pp, up)) 837 msyslog(LOG_ERR, 838 "%s: failed to write watch request (%m)", 839 refnumtoa(&peer->srcadr)); 840 } 841 } 842 843 /* ------------------------------------------------------------------ */ 844 845 static void 846 process_tpv( 847 peerT * const peer , 848 json_ctx * const jctx , 849 const l_fp * const rtime) 850 { 851 clockprocT * const pp = peer->procptr; 852 gpsd_unitT * const up = (gpsd_unitT *)pp->unitptr; 853 854 const char * gps_time; 855 int gps_mode; 856 double ept, epp, epx, epy, epv; 857 int xlog2; 858 859 gps_mode = (int)json_object_lookup_int_default( 860 jctx, 0, "mode", 0); 861 862 gps_time = json_object_lookup_string_default( 863 jctx, 0, "time", NULL); 864 865 if (gps_mode < 1 || NULL == gps_time) { 866 /* receiver has no fix; tell about and avoid stale data */ 867 up->tc_breply += 1; 868 up->fl_tpv = 0; 869 up->fl_pps = 0; 870 return; 871 } 872 873 /* save last time code to clock data */ 874 save_ltc(pp, gps_time); 875 876 /* convert clock and set resulting ref time */ 877 if (convert_ascii_time(&up->tpv_stamp, gps_time)) { 878 DPRINTF(2, ("GPSD_JSON(%d): process_tpv, stamp='%s', recvt='%s' mode=%u\n", 879 up->unit, 880 gmprettydate(&up->tpv_stamp), 881 gmprettydate(&up->tpv_recvt), 882 gps_mode)); 883 884 up->tpv_local = *rtime; 885 up->tpv_recvt = *rtime;/*TODO: hack until we get it remote from GPSD */ 886 L_SUB(&up->tpv_recvt, &up->tpv_fudge); 887 up->fl_tpv = -1; 888 } else { 889 up->tc_btime += 1; 890 up->fl_tpv = 0; 891 } 892 893 /* Set the precision from the GPSD data 894 * 895 * Since EPT has some issues, we use EPT and a home-brewed error 896 * estimation base on a sphere derived from EPX/Y/V and the 897 * speed of light. Use the better one of those two. 898 */ 899 ept = json_object_lookup_float_default(jctx, 0, "ept", 1.0); 900 epx = json_object_lookup_float_default(jctx, 0, "epx", 1000.0); 901 epy = json_object_lookup_float_default(jctx, 0, "epy", 1000.0); 902 if (1 == gps_mode) { 903 /* 2d-fix: extend bounding rectangle to cuboid */ 904 epv = max(epx, epy); 905 } else { 906 /* 3d-fix: get bounding cuboid */ 907 epv = json_object_lookup_float_default( 908 jctx, 0, "epv", 1000.0); 909 } 910 911 /* get diameter of enclosing sphere of bounding cuboid as spatial 912 * error, then divide spatial error by speed of light to get 913 * another time error estimate. Add extra 100 meters as 914 * optimistic lower bound. Then use the better one of the two 915 * estimations. 916 */ 917 epp = 2.0 * sqrt(epx*epx + epy*epy + epv*epv); 918 epp = (epp + 100.0) / 299792458.0; 919 920 ept = min(ept, epp ); 921 ept = min(ept, 0.5 ); 922 ept = max(ept, 1.0-9); 923 ept = frexp(ept, &xlog2); 924 925 peer->precision = xlog2; 926 } 927 928 /* ------------------------------------------------------------------ */ 929 930 static void 931 process_pps( 932 peerT * const peer , 933 json_ctx * const jctx , 934 const l_fp * const rtime) 935 { 936 clockprocT * const pp = peer->procptr; 937 gpsd_unitT * const up = (gpsd_unitT *)pp->unitptr; 938 939 struct timespec ts; 940 941 errno = 0; 942 ts.tv_sec = (time_t)json_object_lookup_int( 943 jctx, 0, "clock_sec"); 944 if (up->fl_nsec) 945 ts.tv_nsec = json_object_lookup_int( 946 jctx, 0, "clock_nsec"); 947 else 948 ts.tv_nsec = json_object_lookup_int( 949 jctx, 0, "clock_musec") * 1000; 950 951 if (0 != errno) 952 goto fail; 953 954 up->pps_local = *rtime; 955 /* get fudged receive time */ 956 up->pps_recvt = tspec_stamp_to_lfp(ts); 957 L_SUB(&up->pps_recvt, &up->pps_fudge); 958 959 /* map to next full second as reference time stamp */ 960 up->pps_stamp = up->pps_recvt; 961 L_ADDUF(&up->pps_stamp, 0x80000000u); 962 up->pps_stamp.l_uf = 0; 963 964 pp->lastrec = up->pps_stamp; 965 966 DPRINTF(2, ("GPSD_JSON(%d): process_pps, stamp='%s', recvt='%s'\n", 967 up->unit, 968 gmprettydate(&up->pps_stamp), 969 gmprettydate(&up->pps_recvt))); 970 971 /* When we have a time pulse, clear the TPV flag: the 972 * PPS is only valid for the >NEXT< TPV value! 973 */ 974 up->fl_pps = -1; 975 up->fl_tpv = 0; 976 return; 977 978 fail: 979 DPRINTF(2, ("GPSD_JSON(%d): process_pps FAILED, nsec=%d stamp='%s', recvt='%s'\n", 980 up->unit, up->fl_nsec, 981 gmprettydate(&up->pps_stamp), 982 gmprettydate(&up->pps_recvt))); 983 up->tc_breply += 1; 984 } 985 986 /* ------------------------------------------------------------------ */ 987 988 static void 989 gpsd_parse( 990 peerT * const peer , 991 const l_fp * const rtime) 992 { 993 clockprocT * const pp = peer->procptr; 994 gpsd_unitT * const up = (gpsd_unitT *)pp->unitptr; 995 996 json_ctx jctx; 997 const char * clsid; 998 l_fp tmpfp; 999 1000 DPRINTF(2, ("GPSD_JSON(%d): gpsd_parse: time %s '%s'\n", 1001 up->unit, ulfptoa(rtime, 6), up->buffer)); 1002 1003 /* See if we can grab anything potentially useful */ 1004 if (!json_parse_record(&jctx, up->buffer)) 1005 return; 1006 1007 /* Now dispatch over the objects we know */ 1008 clsid = json_object_lookup_string_default( 1009 &jctx, 0, "class", "-bad-repy-"); 1010 1011 up->tc_recv += 1; 1012 if (!strcmp("VERSION", clsid)) 1013 process_version(peer, &jctx, rtime); 1014 else if (!strcmp("TPV", clsid)) 1015 process_tpv(peer, &jctx, rtime); 1016 else if (!strcmp("PPS", clsid)) 1017 process_pps(peer, &jctx, rtime); 1018 else if (!strcmp("WATCH", clsid)) 1019 process_watch(peer, &jctx, rtime); 1020 else 1021 return; /* nothing we know about... */ 1022 1023 /* now aggregate TPV and PPS -- no PPS? just use TPV...*/ 1024 if (up->fl_tpv) { 1025 /* TODO: also check remote receive time stamps */ 1026 tmpfp = up->tpv_local; 1027 L_SUB(&tmpfp, &up->pps_local); 1028 1029 if (up->fl_pps && 0 == tmpfp.l_ui) { 1030 refclock_process_offset( 1031 pp, up->tpv_stamp, up->pps_recvt, 0.0); 1032 if (up->ppscount < PPS_MAXCOUNT) 1033 up->ppscount += 1; 1034 } else { 1035 refclock_process_offset( 1036 pp, up->tpv_stamp, up->tpv_recvt, 0.0); 1037 if (up->ppscount > 0) 1038 up->ppscount -= 1; 1039 } 1040 up->fl_pps = 0; 1041 up->fl_tpv = 0; 1042 up->tc_good += 1; 1043 } 1044 } 1045 1046 /* ------------------------------------------------------------------ */ 1047 1048 static void 1049 gpsd_stop_socket( 1050 peerT * const peer) 1051 { 1052 clockprocT * const pp = peer->procptr; 1053 gpsd_unitT * const up = (gpsd_unitT *)pp->unitptr; 1054 1055 if (-1 != pp->io.fd) 1056 io_closeclock(&pp->io); 1057 pp->io.fd = -1; 1058 if (syslogok(pp, up)) 1059 msyslog(LOG_INFO, 1060 "%s: closing socket to GPSD", 1061 refnumtoa(&peer->srcadr)); 1062 up->tickover = up->tickpres; 1063 up->tickpres = min(up->tickpres + 5, TICKOVER_HIGH); 1064 up->fl_vers = 0; 1065 up->fl_tpv = 0; 1066 up->fl_pps = 0; 1067 up->fl_watch = 0; 1068 } 1069 1070 /* ------------------------------------------------------------------ */ 1071 1072 static void 1073 gpsd_init_socket( 1074 peerT * const peer) 1075 { 1076 clockprocT * const pp = peer->procptr; 1077 gpsd_unitT * const up = (gpsd_unitT *)pp->unitptr; 1078 addrinfoT * ai; 1079 int rc; 1080 int ov; 1081 1082 /* draw next address to try */ 1083 if (NULL == up->addr) 1084 up->addr = s_gpsd_addr; 1085 ai = up->addr; 1086 up->addr = ai->ai_next; 1087 1088 /* try to create a matching socket */ 1089 up->fdt = socket( 1090 ai->ai_family, ai->ai_socktype, ai->ai_protocol); 1091 if (-1 == up->fdt) { 1092 if (syslogok(pp, up)) 1093 msyslog(LOG_ERR, 1094 "%s: cannot create GPSD socket: %m", 1095 refnumtoa(&peer->srcadr)); 1096 goto no_socket; 1097 } 1098 1099 /* make sure the socket is non-blocking */ 1100 rc = fcntl(up->fdt, F_SETFL, O_NONBLOCK, 1); 1101 if (-1 == rc) { 1102 if (syslogok(pp, up)) 1103 msyslog(LOG_ERR, 1104 "%s: cannot set GPSD socket to non-blocking: %m", 1105 refnumtoa(&peer->srcadr)); 1106 goto no_socket; 1107 } 1108 /* disable nagling */ 1109 ov = 1; 1110 rc = setsockopt(up->fdt, IPPROTO_TCP, TCP_NODELAY, 1111 (char*)&ov, sizeof(ov)); 1112 if (-1 == rc) { 1113 if (syslogok(pp, up)) 1114 msyslog(LOG_INFO, 1115 "%s: cannot disable TCP nagle: %m", 1116 refnumtoa(&peer->srcadr)); 1117 } 1118 1119 /* start a non-blocking connect */ 1120 rc = connect(up->fdt, ai->ai_addr, ai->ai_addrlen); 1121 if (-1 == rc && errno != EINPROGRESS) { 1122 if (syslogok(pp, up)) 1123 msyslog(LOG_ERR, 1124 "%s: cannot connect GPSD socket: %m", 1125 refnumtoa(&peer->srcadr)); 1126 goto no_socket; 1127 } 1128 1129 return; 1130 1131 no_socket: 1132 if (-1 != up->fdt) 1133 close(up->fdt); 1134 up->fdt = -1; 1135 up->tickover = up->tickpres; 1136 up->tickpres = min(up->tickpres + 5, TICKOVER_HIGH); 1137 } 1138 1139 /* ------------------------------------------------------------------ */ 1140 1141 static void 1142 gpsd_test_socket( 1143 peerT * const peer) 1144 { 1145 clockprocT * const pp = peer->procptr; 1146 gpsd_unitT * const up = (gpsd_unitT *)pp->unitptr; 1147 1148 int ec, rc; 1149 socklen_t lc; 1150 1151 /* Check if the non-blocking connect was finished by testing the 1152 * socket for writeability. Use the 'poll()' API if available 1153 * and 'select()' otherwise. 1154 */ 1155 DPRINTF(2, ("GPSD_JSON(%d): check connect, fd=%d\n", 1156 up->unit, up->fdt)); 1157 1158 #if defined(HAVE_SYS_POLL_H) 1159 { 1160 struct pollfd pfd; 1161 1162 pfd.events = POLLOUT; 1163 pfd.fd = up->fdt; 1164 rc = poll(&pfd, 1, 0); 1165 if (1 != rc || !(pfd.revents & POLLOUT)) 1166 return; 1167 } 1168 #elif defined(HAVE_SYS_SELECT_H) 1169 { 1170 struct timeval tout; 1171 fd_set wset; 1172 1173 memset(&tout, 0, sizeof(tout)); 1174 FD_ZERO(&wset); 1175 FD_SET(up->fdt, &wset); 1176 rc = select(up->fdt+1, NULL, &wset, NULL, &tout); 1177 if (0 == rc || !(FD_ISSET(up->fdt, &wset))) 1178 return; 1179 } 1180 #else 1181 # error Blooper! That should have been found earlier! 1182 #endif 1183 1184 /* next timeout is a full one... */ 1185 up->tickover = TICKOVER_LOW; 1186 1187 /* check for socket error */ 1188 ec = 0; 1189 lc = sizeof(ec); 1190 rc = getsockopt(up->fdt, SOL_SOCKET, SO_ERROR, &ec, &lc); 1191 DPRINTF(1, ("GPSD_JSON(%d): connect finshed, fd=%d, ec=%d(%s)\n", 1192 up->unit, up->fdt, ec, strerror(ec))); 1193 if (-1 == rc || 0 != ec) { 1194 errno = ec; 1195 if (syslogok(pp, up)) 1196 msyslog(LOG_ERR, 1197 "%s: (async)cannot connect GPSD socket: %m", 1198 refnumtoa(&peer->srcadr)); 1199 goto no_socket; 1200 } 1201 /* swap socket FDs, and make sure the clock was added */ 1202 pp->io.fd = up->fdt; 1203 up->fdt = -1; 1204 if (0 == io_addclock(&pp->io)) { 1205 if (syslogok(pp, up)) 1206 msyslog(LOG_ERR, 1207 "%s: failed to register with I/O engine", 1208 refnumtoa(&peer->srcadr)); 1209 goto no_socket; 1210 } 1211 return; 1212 1213 no_socket: 1214 if (-1 != up->fdt) 1215 close(up->fdt); 1216 up->fdt = -1; 1217 up->tickover = up->tickpres; 1218 up->tickpres = min(up->tickpres + 5, TICKOVER_HIGH); 1219 } 1220 1221 /* ===================================================================== 1222 * helper stuff 1223 */ 1224 1225 /* 1226 * shm_clockstats - dump and reset counters 1227 */ 1228 static void 1229 gpsd_clockstats( 1230 int unit, 1231 peerT * const peer 1232 ) 1233 { 1234 clockprocT * const pp = peer->procptr; 1235 gpsd_unitT * const up = (gpsd_unitT *)pp->unitptr; 1236 1237 char logbuf[128]; 1238 unsigned int llen; 1239 1240 /* if snprintf() returns a negative values on errors (some older 1241 * ones do) make sure we are NUL terminated. Using an unsigned 1242 * result does the trick. 1243 */ 1244 llen = snprintf(logbuf, sizeof(logbuf), 1245 "good=%-3u badtime=%-3u baddate=%-3u badreply=%-3u recv=%-3u", 1246 up->tc_good, up->tc_btime, up->tc_bdate, 1247 up->tc_breply, up->tc_recv); 1248 logbuf[min(llen, sizeof(logbuf)-1)] = '\0'; 1249 record_clock_stats(&peer->srcadr, logbuf); 1250 } 1251 1252 /* ------------------------------------------------------------------- 1253 * Convert a GPSD timestam (ISO8601 Format) to an l_fp 1254 */ 1255 static BOOL 1256 convert_ascii_time( 1257 l_fp * fp , 1258 const char * gps_time) 1259 { 1260 char *ep; 1261 struct tm gd; 1262 struct timespec ts; 1263 long dw; 1264 1265 /* Use 'strptime' to take the brunt of the work, then parse 1266 * the fractional part manually, starting with a digit weight of 1267 * 10^8 nanoseconds. 1268 */ 1269 ts.tv_nsec = 0; 1270 ep = strptime(gps_time, "%Y-%m-%dT%H:%M:%S", &gd); 1271 if (*ep == '.') { 1272 dw = 100000000; 1273 while (isdigit((unsigned char)*++ep)) { 1274 ts.tv_nsec += (*ep - '0') * dw; 1275 dw /= 10; 1276 } 1277 } 1278 if (ep[0] != 'Z' || ep[1] != '\0') 1279 return FALSE; 1280 1281 /* now convert the whole thing into a 'l_fp' */ 1282 ts.tv_sec = (ntpcal_tm_to_rd(&gd) - DAY_NTP_STARTS) * SECSPERDAY 1283 + ntpcal_tm_to_daysec(&gd); 1284 *fp = tspec_intv_to_lfp(ts); 1285 1286 return TRUE; 1287 } 1288 1289 /* ------------------------------------------------------------------- 1290 * Save the last timecode string, making sure it's properly truncated 1291 * if necessary and NUL terminated in any case. 1292 */ 1293 static void 1294 save_ltc( 1295 clockprocT * const pp, 1296 const char * const tc) 1297 { 1298 size_t len; 1299 1300 len = (tc) ? strlen(tc) : 0; 1301 if (len >= sizeof(pp->a_lastcode)) 1302 len = sizeof(pp->a_lastcode) - 1; 1303 pp->lencode = (u_short)len; 1304 memcpy(pp->a_lastcode, tc, len); 1305 pp->a_lastcode[len] = '\0'; 1306 } 1307 1308 /* 1309 * ------------------------------------------------------------------- 1310 * asprintf replacement... it's not available everywhere... 1311 */ 1312 static int 1313 myasprintf( 1314 char ** spp, 1315 char const * fmt, 1316 ... ) 1317 { 1318 size_t alen, plen; 1319 1320 alen = 32; 1321 *spp = NULL; 1322 do { 1323 va_list va; 1324 1325 alen += alen; 1326 free(*spp); 1327 *spp = (char*)malloc(alen); 1328 if (NULL == *spp) 1329 return -1; 1330 1331 va_start(va, fmt); 1332 plen = (size_t)vsnprintf(*spp, alen, fmt, va); 1333 va_end(va); 1334 } while (plen >= alen); 1335 1336 return (int)plen; 1337 } 1338 1339 #else 1340 NONEMPTY_TRANSLATION_UNIT 1341 #endif /* REFCLOCK && CLOCK_GPSDJSON */ 1342