contrib/ntp/ntpd/refclock_gpsdjson.c

   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 */