refclock_gpsdjson.c [plain text]
#ifdef HAVE_CONFIG_H
#include <config.h>
#endif
#include "ntp_types.h"
#if defined(REFCLOCK) && defined(CLOCK_GPSDJSON) && !defined(SYS_WINNT)
#define JSMN_PARENT_LINKS
#include "../libjsmn/jsmn.c"
#define JSMN_MAXTOK 350
#define INVALID_TOKEN (-1)
typedef struct json_ctx {
char * buf;
int ntok;
jsmntok_t tok[JSMN_MAXTOK];
} json_ctx;
typedef int tok_ref;
#ifdef HAVE_LONG_LONG
typedef signed long long int json_int;
typedef unsigned long long int json_uint;
#define JSON_INT_MAX LLONG_MAX
#define JSON_INT_MIN LLONG_MIN
#else
typedef signed long int json_int;
typedef unsigned long int json_uint;
#define JSON_INT_MAX LONG_MAX
#define JSON_INT_MIN LONG_MIN
#endif
#include <netdb.h>
#include <unistd.h>
#include <fcntl.h>
#include <string.h>
#include <ctype.h>
#include <math.h>
#include <sys/types.h>
#include <sys/socket.h>
#include <sys/stat.h>
#include <netinet/tcp.h>
#if defined(HAVE_SYS_POLL_H)
# include <sys/poll.h>
#elif defined(HAVE_SYS_SELECT_H)
# include <sys/select.h>
#else
# error need poll() or select()
#endif
#include "ntpd.h"
#include "ntp_io.h"
#include "ntp_unixtime.h"
#include "ntp_refclock.h"
#include "ntp_stdlib.h"
#include "ntp_calendar.h"
#include "timespecops.h"
#define MODE_OP_MASK 0x03
#define MODE_OP_STI 0
#define MODE_OP_STRICT 1
#define MODE_OP_AUTO 2
#define MODE_OP_MAXVAL 2
#define MODE_OP_MODE(x) ((x) & MODE_OP_MASK)
#define PRECISION (-9)
#define PPS_PRECISION (-20)
#define REFID "GPSD"
#define DESCRIPTION "GPSD JSON client clock"
#define MAX_PDU_LEN 1600
#define TICKOVER_LOW 10
#define TICKOVER_HIGH 120
#define LOGTHROTTLE 3600
#define PPS_MAXCOUNT 60
#define PPS_INCCOUNT 3
#define PPS_DECCOUNT 1
#define PPS2_MAXCOUNT 10
#ifndef BOOL
# define BOOL int
#endif
#ifndef TRUE
# define TRUE 1
#endif
#ifndef FALSE
# define FALSE 0
#endif
#define PROTO_VERSION(hi,lo) \
((((uint32_t)(hi) << 16) & 0xFFFF0000u) | \
((uint32_t)(lo) & 0x0FFFFu))
typedef struct peer peerT;
typedef struct refclockproc clockprocT;
typedef struct addrinfo addrinfoT;
static const char * s_dev_stem = "/dev/gps";
static void gpsd_init (void);
static int gpsd_start (int, peerT *);
static void gpsd_shutdown (int, peerT *);
static void gpsd_receive (struct recvbuf *);
static void gpsd_poll (int, peerT *);
static void gpsd_control (int, const struct refclockstat *,
struct refclockstat *, peerT *);
static void gpsd_timer (int, peerT *);
static int myasprintf(char**, char const*, ...) NTP_PRINTF(2, 3);
static void enter_opmode(peerT *peer, int mode);
static void leave_opmode(peerT *peer, int mode);
struct refclock refclock_gpsdjson = {
gpsd_start,
gpsd_shutdown,
gpsd_poll,
gpsd_control,
gpsd_init,
noentry,
gpsd_timer
};
struct gpsd_unit;
typedef struct gpsd_unit gpsd_unitT;
struct gpsd_unit {
gpsd_unitT *next_unit;
size_t refcount;
peerT *pps_peer;
int unit;
int mode;
char *logname;
char * device;
uint32_t proto_version;
l_fp pps_local;
l_fp pps_stamp;
l_fp pps_recvt;
l_fp pps_stamp2;
l_fp pps_recvt2;
int ppscount;
int ppscount2;
l_fp sti_local;
l_fp sti_stamp;
l_fp sti_recvt;
int16_t sti_prec;
int16_t pps_prec;
l_fp pps_fudge;
l_fp pps_fudge2;
l_fp sti_fudge;
int fl_nosync: 1;
int fl_sti : 1;
int fl_pps : 1;
int fl_pps2 : 1;
int fl_rawsti: 1;
int fl_vers : 1;
int fl_watch : 1;
int pf_nsec : 1;
int pf_toff : 1;
int fdt;
addrinfoT * addr;
u_int tickover;
u_int tickpres;
u_int tc_recv;
u_int tc_breply;
u_int tc_nosync;
u_int tc_sti_recv;
u_int tc_sti_used;
u_int tc_pps_recv;
u_int tc_pps_used;
u_int logthrottle;
json_ctx json_parse;
int buflen;
char buffer[MAX_PDU_LEN];
};
static void gpsd_init_socket(peerT * const peer);
static void gpsd_test_socket(peerT * const peer);
static void gpsd_stop_socket(peerT * const peer);
static void gpsd_parse(peerT * const peer,
const l_fp * const rtime);
static BOOL convert_ascii_time(l_fp * fp, const char * gps_time);
static void save_ltc(clockprocT * const pp, const char * const tc);
static int syslogok(clockprocT * const pp, gpsd_unitT * const up);
static void log_data(peerT *peer, const char *what,
const char *buf, size_t len);
static int16_t clamped_precision(int rawprec);
static const char * const s_req_version =
"?VERSION;\r\n";
static addrinfoT *s_gpsd_addr;
static gpsd_unitT *s_clock_units;
static const char * const s_svctab[][2] = {
{ "localhost", "gpsd" },
{ "localhost", "2947" },
{ "127.0.0.1", "2947" },
{ NULL, NULL }
};
static int s_svcerr[sizeof(s_svctab)/sizeof(s_svctab[0])];
static int s_svcidx;
static int
syslogok(
clockprocT * const pp,
gpsd_unitT * const up)
{
int res = (0 != (pp->sloppyclockflag & CLK_FLAG3))
|| (0 == up->logthrottle )
|| (LOGTHROTTLE == up->logthrottle );
if (res)
up->logthrottle = LOGTHROTTLE;
return res;
}
static void
gpsd_init(void)
{
addrinfoT hints;
int rc, idx;
memset(s_svcerr, 0, sizeof(s_svcerr));
memset(&hints, 0, sizeof(hints));
hints.ai_family = AF_UNSPEC;
hints.ai_protocol = IPPROTO_TCP;
hints.ai_socktype = SOCK_STREAM;
for (idx = 0; s_svctab[idx][0] && !s_gpsd_addr; idx++) {
rc = getaddrinfo(s_svctab[idx][0], s_svctab[idx][1],
&hints, &s_gpsd_addr);
s_svcerr[idx] = rc;
if (0 == rc)
break;
s_gpsd_addr = NULL;
}
s_svcidx = idx;
}
static int
gpsd_init_check(void)
{
int idx;
if (s_svcidx == 0)
return (s_gpsd_addr != NULL);
for (idx = 0; idx < s_svcidx; ++idx) {
msyslog(LOG_WARNING,
"GPSD_JSON: failed to resolve '%s:%s', rc=%d (%s)",
s_svctab[idx][0], s_svctab[idx][1],
s_svcerr[idx], gai_strerror(s_svcerr[idx]));
}
if (s_gpsd_addr == NULL)
msyslog(LOG_ERR, "%s",
"GPSD_JSON: failed to get socket address, giving up.");
else if (idx != 0)
msyslog(LOG_WARNING,
"GPSD_JSON: using '%s:%s' instead of '%s:%s'",
s_svctab[idx][0], s_svctab[idx][1],
s_svctab[0][0], s_svctab[0][1]);
s_svcidx = 0;
return (s_gpsd_addr != NULL);
}
static int
gpsd_start(
int unit,
peerT * peer)
{
clockprocT * const pp = peer->procptr;
gpsd_unitT * up;
gpsd_unitT ** uscan = &s_clock_units;
struct stat sb;
if ( ! gpsd_init_check())
return FALSE;
while ((up = *uscan) != NULL && up->unit != (unit & 0x7F))
uscan = &up->next_unit;
if (up == NULL) {
up = emalloc_zero(sizeof(*up));
*uscan = up;
++up->refcount;
up->logname = estrdup(refnumtoa(&peer->srcadr));
up->unit = unit & 0x7F;
up->fdt = -1;
up->addr = s_gpsd_addr;
up->tickpres = TICKOVER_LOW;
if (-1 == myasprintf(&up->device, "%s%u",
s_dev_stem, up->unit)) {
msyslog(LOG_ERR, "%s: clock device name too long",
up->logname);
goto dev_fail;
}
if (-1 == stat(up->device, &sb) || !S_ISCHR(sb.st_mode)) {
msyslog(LOG_ERR, "%s: '%s' is not a character device",
up->logname, up->device);
goto dev_fail;
}
} else {
++up->refcount;
}
pp->unitptr = (caddr_t)up;
pp->io.fd = -1;
pp->io.clock_recv = gpsd_receive;
pp->io.srcclock = peer;
pp->io.datalen = 0;
pp->a_lastcode[0] = '\0';
pp->lencode = 0;
pp->clockdesc = DESCRIPTION;
memcpy(&pp->refid, REFID, 4);
if (unit >= 128)
peer->precision = PPS_PRECISION;
else
peer->precision = PRECISION;
if (NULL == up->addr) {
msyslog(LOG_ERR, "%s: no GPSD socket address, giving up",
up->logname);
goto dev_fail;
}
LOGIF(CLOCKINFO,
(LOG_NOTICE, "%s: startup, device is '%s'",
refnumtoa(&peer->srcadr), up->device));
up->mode = MODE_OP_MODE(peer->ttl);
if (up->mode > MODE_OP_MAXVAL)
up->mode = 0;
if (unit >= 128)
up->pps_peer = peer;
else
enter_opmode(peer, up->mode);
return TRUE;
dev_fail:
INSIST (up);
if (!--up->refcount) {
*uscan = up->next_unit;
free(up->device);
free(up);
}
pp->unitptr = (caddr_t)NULL;
return FALSE;
}
static void
gpsd_shutdown(
int unit,
peerT * peer)
{
clockprocT * const pp = peer->procptr;
gpsd_unitT * const up = (gpsd_unitT *)pp->unitptr;
gpsd_unitT ** uscan = &s_clock_units;
UNUSED_ARG(unit);
if (up == NULL)
return;
if (peer != up->pps_peer) {
if (-1 != pp->io.fd) {
DPRINTF(1, ("%s: closing clock, fd=%d\n",
up->logname, pp->io.fd));
io_closeclock(&pp->io);
pp->io.fd = -1;
}
if (up->fdt != -1)
close(up->fdt);
}
if (!--up->refcount) {
while (*uscan != NULL)
if (*uscan == up)
*uscan = up->next_unit;
else
uscan = &(*uscan)->next_unit;
free(up->logname);
free(up->device);
free(up);
}
pp->unitptr = (caddr_t)NULL;
LOGIF(CLOCKINFO,
(LOG_NOTICE, "%s: shutdown", refnumtoa(&peer->srcadr)));
}
static void
gpsd_receive(
struct recvbuf * rbufp)
{
peerT * const peer = rbufp->recv_peer;
clockprocT * const pp = peer->procptr;
gpsd_unitT * const up = (gpsd_unitT *)pp->unitptr;
const char *psrc, *esrc;
char *pdst, *edst, ch;
log_data(peer, "recv", (const char*)rbufp->recv_buffer,
(size_t)rbufp->recv_length);
psrc = (const char*)rbufp->recv_buffer;
esrc = psrc + rbufp->recv_length;
pdst = up->buffer + up->buflen;
edst = pdst + sizeof(up->buffer) - 1;
while (psrc != esrc) {
ch = *psrc++;
if (ch == '\n') {
while (pdst != up->buffer && pdst[-1] <= ' ')
--pdst;
*pdst = '\0';
up->buflen = pdst - up->buffer;
gpsd_parse(peer, &rbufp->recv_time);
pdst = up->buffer;
} else if (pdst != edst) {
if (ch > ' ' || pdst != up->buffer)
*pdst++ = ch;
}
}
up->buflen = pdst - up->buffer;
up->tickover = TICKOVER_LOW;
}
static void
poll_primary(
peerT * const peer ,
clockprocT * const pp ,
gpsd_unitT * const up )
{
if (pp->coderecv != pp->codeproc) {
pp->lastref = pp->lastrec;
refclock_report(peer, CEVNT_NOMINAL);
refclock_receive(peer);
} else {
peer->precision = PRECISION;
if (-1 == pp->io.fd)
refclock_report(peer, CEVNT_FAULT);
else if (0 != up->tc_breply)
refclock_report(peer, CEVNT_BADREPLY);
else
refclock_report(peer, CEVNT_TIMEOUT);
}
if (pp->sloppyclockflag & CLK_FLAG4)
mprintf_clock_stats(
&peer->srcadr,"%u %u %u %u %u %u %u",
up->tc_recv,
up->tc_breply, up->tc_nosync,
up->tc_sti_recv, up->tc_sti_used,
up->tc_pps_recv, up->tc_pps_used);
up->tc_breply = 0;
up->tc_recv = 0;
up->tc_nosync = 0;
up->tc_sti_recv = 0;
up->tc_sti_used = 0;
up->tc_pps_recv = 0;
up->tc_pps_used = 0;
}
static void
poll_secondary(
peerT * const peer ,
clockprocT * const pp ,
gpsd_unitT * const up )
{
if (pp->coderecv != pp->codeproc) {
pp->lastref = pp->lastrec;
refclock_report(peer, CEVNT_NOMINAL);
refclock_receive(peer);
} else {
peer->precision = PPS_PRECISION;
peer->flags &= ~FLAG_PPS;
refclock_report(peer, CEVNT_TIMEOUT);
}
}
static void
gpsd_poll(
int unit,
peerT * peer)
{
clockprocT * const pp = peer->procptr;
gpsd_unitT * const up = (gpsd_unitT *)pp->unitptr;
++pp->polls;
if (peer == up->pps_peer)
poll_secondary(peer, pp, up);
else
poll_primary(peer, pp, up);
}
static void
gpsd_control(
int unit,
const struct refclockstat * in_st,
struct refclockstat * out_st,
peerT * peer )
{
clockprocT * const pp = peer->procptr;
gpsd_unitT * const up = (gpsd_unitT *)pp->unitptr;
if (peer == up->pps_peer) {
DTOLFP(pp->fudgetime1, &up->pps_fudge2);
if ( ! (pp->sloppyclockflag & CLK_FLAG1))
peer->flags &= ~FLAG_PPS;
} else {
DTOLFP(pp->fudgetime1, &up->pps_fudge);
DTOLFP(pp->fudgetime2, &up->sti_fudge);
if (MODE_OP_MODE(up->mode ^ peer->ttl)) {
leave_opmode(peer, up->mode);
up->mode = MODE_OP_MODE(peer->ttl);
enter_opmode(peer, up->mode);
}
}
}
static void
timer_primary(
peerT * const peer ,
clockprocT * const pp ,
gpsd_unitT * const up )
{
int rc;
if (up->logthrottle)
--up->logthrottle;
if (up->tickover)
--up->tickover;
switch (up->tickover) {
case 4:
if (-1 != pp->io.fd) {
size_t rlen = strlen(s_req_version);
DPRINTF(2, ("%s: timer livecheck: '%s'\n",
up->logname, s_req_version));
log_data(peer, "send", s_req_version, rlen);
rc = write(pp->io.fd, s_req_version, rlen);
(void)rc;
} else if (-1 != up->fdt) {
gpsd_test_socket(peer);
}
break;
case 0:
if (-1 != pp->io.fd)
gpsd_stop_socket(peer);
else if (-1 != up->fdt)
gpsd_test_socket(peer);
else if (NULL != s_gpsd_addr)
gpsd_init_socket(peer);
break;
default:
if (-1 == pp->io.fd && -1 != up->fdt)
gpsd_test_socket(peer);
}
}
static void
timer_secondary(
peerT * const peer ,
clockprocT * const pp ,
gpsd_unitT * const up )
{
up->ppscount2 = max(0, (up->ppscount2 - 1));
if (0 == up->ppscount2) {
if (pp->coderecv != pp->codeproc) {
refclock_report(peer, CEVNT_TIMEOUT);
pp->coderecv = pp->codeproc;
}
peer->flags &= ~FLAG_PPS;
}
}
static void
gpsd_timer(
int unit,
peerT * peer)
{
clockprocT * const pp = peer->procptr;
gpsd_unitT * const up = (gpsd_unitT *)pp->unitptr;
if (peer == up->pps_peer)
timer_secondary(peer, pp, up);
else
timer_primary(peer, pp, up);
}
static void
enter_opmode(
peerT *peer,
int mode)
{
clockprocT * const pp = peer->procptr;
gpsd_unitT * const up = (gpsd_unitT *)pp->unitptr;
DPRINTF(1, ("%s: enter operation mode %d\n",
up->logname, MODE_OP_MODE(mode)));
if (MODE_OP_MODE(mode) == MODE_OP_AUTO) {
up->fl_rawsti = 0;
up->ppscount = PPS_MAXCOUNT / 2;
}
up->fl_pps = 0;
up->fl_sti = 0;
}
static void
leave_opmode(
peerT *peer,
int mode)
{
clockprocT * const pp = peer->procptr;
gpsd_unitT * const up = (gpsd_unitT *)pp->unitptr;
DPRINTF(1, ("%s: leaving operation mode %d\n",
up->logname, MODE_OP_MODE(mode)));
if (MODE_OP_MODE(mode) == MODE_OP_AUTO) {
up->fl_rawsti = 0;
up->ppscount = 0;
}
up->fl_pps = 0;
up->fl_sti = 0;
}
static void
add_clock_sample(
peerT * const peer ,
clockprocT * const pp ,
l_fp stamp,
l_fp recvt)
{
pp->lastref = stamp;
if (pp->coderecv == pp->codeproc)
refclock_report(peer, CEVNT_NOMINAL);
refclock_process_offset(pp, stamp, recvt, pp->fudgetime1);
}
static void
eval_strict(
peerT * const peer ,
clockprocT * const pp ,
gpsd_unitT * const up )
{
if (up->fl_sti && up->fl_pps) {
add_clock_sample(peer, pp, up->sti_stamp, up->pps_recvt);
peer->precision = up->pps_prec;
up->fl_pps = 0;
up->fl_sti = 0;
++up->tc_sti_used;
}
}
static void
eval_pps_secondary(
peerT * const peer ,
clockprocT * const pp ,
gpsd_unitT * const up )
{
if (up->fl_pps2) {
add_clock_sample(peer, pp, up->pps_stamp2, up->pps_recvt2);
peer->precision = up->pps_prec;
up->ppscount2 = min(PPS2_MAXCOUNT, (up->ppscount2 + 2));
if ((PPS2_MAXCOUNT == up->ppscount2) &&
(pp->sloppyclockflag & CLK_FLAG1) )
peer->flags |= FLAG_PPS;
up->fl_pps2 = 0;
++up->tc_pps_used;
}
}
static void
eval_serial(
peerT * const peer ,
clockprocT * const pp ,
gpsd_unitT * const up )
{
if (up->fl_sti) {
add_clock_sample(peer, pp, up->sti_stamp, up->sti_recvt);
peer->precision = up->sti_prec;
up->fl_sti = 0;
++up->tc_sti_used;
}
}
static void
eval_auto(
peerT * const peer ,
clockprocT * const pp ,
gpsd_unitT * const up )
{
if (!up->fl_sti)
return;
if (up->fl_pps) {
up->ppscount = min(PPS_MAXCOUNT,
(up->ppscount + PPS_INCCOUNT));
if ((PPS_MAXCOUNT == up->ppscount) && up->fl_rawsti) {
up->fl_rawsti = 0;
msyslog(LOG_INFO,
"%s: expect valid PPS from now",
up->logname);
}
} else {
up->ppscount = max(0, (up->ppscount - PPS_DECCOUNT));
if ((0 == up->ppscount) && !up->fl_rawsti) {
up->fl_rawsti = -1;
msyslog(LOG_WARNING,
"%s: use TPV alone from now",
up->logname);
}
}
if (up->fl_rawsti)
eval_serial(peer, pp, up);
else
eval_strict(peer, pp, up);
}
static json_int
strtojint(
const char *cp, char **ep)
{
json_uint accu, limit_lo, limit_hi;
int flags;
const char * hold;
union { const char * c; char * v; } vep;
vep.c = cp;
if (*cp == '-') {
cp += 1;
flags = 2;
limit_hi = (json_uint)-(JSON_INT_MIN + 1) + 1;
} else {
cp += (*cp == '+');
flags = 0;
limit_hi = (json_uint)JSON_INT_MAX;
}
limit_lo = limit_hi / 10;
hold = cp;
accu = 0;
while (isdigit(*(const u_char*)cp)) {
flags |= (accu > limit_lo);
accu = accu * 10 + (*(const u_char*)cp++ - '0');
flags |= (accu > limit_hi);
}
if (hold != cp)
vep.c = cp;
else
errno = EINVAL;
if (flags & 1) {
errno = ERANGE;
accu = limit_hi;
}
if (ep)
*ep = vep.v;
if ((flags & 2) && accu)
return -(json_int)(accu - 1) - 1;
else
return (json_int)accu;
}
static tok_ref
json_token_skip(
const json_ctx * ctx,
tok_ref tid)
{
if (tid >= 0 && (u_int)tid < ctx->ntok) {
int len = ctx->tok[tid].size;
switch (ctx->tok[tid].type) {
case JSMN_OBJECT:
len *= 2;
case JSMN_ARRAY:
for (++tid; len; --len)
tid = json_token_skip(ctx, tid);
break;
default:
++tid;
break;
}
if (tid < 0 || (u_int)tid > ctx->ntok)
tid = ctx->ntok;
}
return tid;
}
static int
json_object_lookup(
const json_ctx * ctx ,
tok_ref tid ,
const char * key ,
int what)
{
int len;
if (tid < 0 || tid >= ctx->ntok ||
ctx->tok[tid].type != JSMN_OBJECT)
return INVALID_TOKEN;
len = ctx->tok[tid].size;
for (++tid; len && tid+1 < ctx->ntok; --len) {
if (ctx->tok[tid].type != JSMN_STRING) {
tid = json_token_skip(ctx, tid);
tid = json_token_skip(ctx, tid);
} else if (strcmp(key, ctx->buf + ctx->tok[tid].start)) {
tid = json_token_skip(ctx, tid+1);
} else if (what < 0 || (u_int)what == ctx->tok[tid+1].type) {
return tid + 1;
} else {
break;
}
if (tid < 0)
break;
}
return INVALID_TOKEN;
}
static const char*
json_object_lookup_primitive(
const json_ctx * ctx,
tok_ref tid,
const char * key)
{
tid = json_object_lookup(ctx, tid, key, JSMN_PRIMITIVE);
if (INVALID_TOKEN != tid)
return ctx->buf + ctx->tok[tid].start;
else
return NULL;
}
static int
json_object_lookup_bool(
const json_ctx * ctx,
tok_ref tid,
const char * key)
{
const char *cp;
cp = json_object_lookup_primitive(ctx, tid, key);
switch ( cp ? *cp : '\0') {
case 't': return 1;
case 'f': return 0;
default : return -1;
}
}
static const char*
json_object_lookup_string(
const json_ctx * ctx,
tok_ref tid,
const char * key)
{
tid = json_object_lookup(ctx, tid, key, JSMN_STRING);
if (INVALID_TOKEN != tid)
return ctx->buf + ctx->tok[tid].start;
return NULL;
}
static const char*
json_object_lookup_string_default(
const json_ctx * ctx,
tok_ref tid,
const char * key,
const char * def)
{
tid = json_object_lookup(ctx, tid, key, JSMN_STRING);
if (INVALID_TOKEN != tid)
return ctx->buf + ctx->tok[tid].start;
return def;
}
static json_int
json_object_lookup_int(
const json_ctx * ctx,
tok_ref tid,
const char * key)
{
json_int ret;
const char * cp;
char * ep;
cp = json_object_lookup_primitive(ctx, tid, key);
if (NULL != cp) {
ret = strtojint(cp, &ep);
if (cp != ep && '\0' == *ep)
return ret;
} else {
errno = EINVAL;
}
return 0;
}
static json_int
json_object_lookup_int_default(
const json_ctx * ctx,
tok_ref tid,
const char * key,
json_int def)
{
json_int ret;
const char * cp;
char * ep;
cp = json_object_lookup_primitive(ctx, tid, key);
if (NULL != cp) {
ret = strtojint(cp, &ep);
if (cp != ep && '\0' == *ep)
return ret;
}
return def;
}
#if 0
static double
json_object_lookup_float(
const json_ctx * ctx,
tok_ref tid,
const char * key)
{
double ret;
const char * cp;
char * ep;
cp = json_object_lookup_primitive(ctx, tid, key);
if (NULL != cp) {
ret = strtod(cp, &ep);
if (cp != ep && '\0' == *ep)
return ret;
} else {
errno = EINVAL;
}
return 0.0;
}
#endif
static double
json_object_lookup_float_default(
const json_ctx * ctx,
tok_ref tid,
const char * key,
double def)
{
double ret;
const char * cp;
char * ep;
cp = json_object_lookup_primitive(ctx, tid, key);
if (NULL != cp) {
ret = strtod(cp, &ep);
if (cp != ep && '\0' == *ep)
return ret;
}
return def;
}
static BOOL
json_parse_record(
json_ctx * ctx,
char * buf,
size_t len)
{
jsmn_parser jsm;
int idx, rc;
jsmn_init(&jsm);
rc = jsmn_parse(&jsm, buf, len, ctx->tok, JSMN_MAXTOK);
if (rc <= 0)
return FALSE;
ctx->buf = buf;
ctx->ntok = rc;
if (JSMN_OBJECT != ctx->tok[0].type)
return FALSE;
for (idx = 0; idx < ctx->ntok; ++idx)
if (ctx->tok[idx].end > ctx->tok[idx].start)
ctx->buf[ctx->tok[idx].end] = '\0';
return TRUE;
}
static BOOL
get_binary_time(
l_fp * const dest ,
json_ctx * const jctx ,
const char * const time_name,
const char * const frac_name,
long fscale )
{
BOOL retv = FALSE;
struct timespec ts;
errno = 0;
ts.tv_sec = (time_t)json_object_lookup_int(jctx, 0, time_name);
ts.tv_nsec = (long )json_object_lookup_int(jctx, 0, frac_name);
if (0 == errno) {
ts.tv_nsec *= fscale;
*dest = tspec_stamp_to_lfp(ts);
retv = TRUE;
}
return retv;
}
static void
process_watch(
peerT * const peer ,
json_ctx * const jctx ,
const l_fp * const rtime)
{
clockprocT * const pp = peer->procptr;
gpsd_unitT * const up = (gpsd_unitT *)pp->unitptr;
const char * path;
path = json_object_lookup_string(jctx, 0, "device");
if (NULL == path || strcmp(path, up->device))
return;
if (json_object_lookup_bool(jctx, 0, "enable") > 0 &&
json_object_lookup_bool(jctx, 0, "json" ) > 0 )
up->fl_watch = -1;
else
up->fl_watch = 0;
DPRINTF(2, ("%s: process_watch, enabled=%d\n",
up->logname, (up->fl_watch & 1)));
}
static void
process_version(
peerT * const peer ,
json_ctx * const jctx ,
const l_fp * const rtime)
{
clockprocT * const pp = peer->procptr;
gpsd_unitT * const up = (gpsd_unitT *)pp->unitptr;
int len;
char * buf;
const char *revision;
const char *release;
uint16_t pvhi, pvlo;
revision = json_object_lookup_string_default(
jctx, 0, "rev", "(unknown)");
release = json_object_lookup_string_default(
jctx, 0, "release", "(unknown)");
errno = 0;
pvhi = (uint16_t)json_object_lookup_int(jctx, 0, "proto_major");
pvlo = (uint16_t)json_object_lookup_int(jctx, 0, "proto_minor");
if (0 == errno) {
if ( ! up->fl_vers)
msyslog(LOG_INFO,
"%s: GPSD revision=%s release=%s protocol=%u.%u",
up->logname, revision, release,
pvhi, pvlo);
up->proto_version = PROTO_VERSION(pvhi, pvlo);
up->fl_vers = -1;
} else {
if (syslogok(pp, up))
msyslog(LOG_INFO,
"%s: could not evaluate version data",
up->logname);
return;
}
up->pf_nsec = -(up->proto_version >= PROTO_VERSION(3,9));
up->pf_toff = -(up->proto_version >= PROTO_VERSION(3,10));
if (up->fl_watch)
return;
snprintf(up->buffer, sizeof(up->buffer),
"?WATCH={\"device\":\"%s\",\"enable\":true,\"json\":true%s};\r\n",
up->device, (up->pf_toff ? ",\"pps\":true" : ""));
buf = up->buffer;
len = strlen(buf);
log_data(peer, "send", buf, len);
if (len != write(pp->io.fd, buf, len) && (syslogok(pp, up))) {
msyslog(LOG_ERR, "%s: failed to write watch request (%m)",
up->logname);
}
}
static void
process_tpv(
peerT * const peer ,
json_ctx * const jctx ,
const l_fp * const rtime)
{
clockprocT * const pp = peer->procptr;
gpsd_unitT * const up = (gpsd_unitT *)pp->unitptr;
const char * gps_time;
int gps_mode;
double ept;
int xlog2;
gps_mode = (int)json_object_lookup_int_default(
jctx, 0, "mode", 0);
gps_time = json_object_lookup_string(
jctx, 0, "time");
if (gps_mode < 2 || NULL == gps_time) {
if ( ! up->pf_toff)
++up->tc_sti_recv;
++up->tc_nosync;
up->fl_sti = 0;
up->fl_pps = 0;
up->fl_nosync = -1;
return;
}
up->fl_nosync = 0;
if ( ! up->pf_toff) {
++up->tc_sti_recv;
save_ltc(pp, gps_time);
if (convert_ascii_time(&up->sti_stamp, gps_time)) {
DPRINTF(2, ("%s: process_tpv, stamp='%s',"
" recvt='%s' mode=%u\n",
up->logname,
gmprettydate(&up->sti_stamp),
gmprettydate(&up->sti_recvt),
gps_mode));
up->sti_local = *rtime;
up->sti_recvt = *rtime;
L_SUB(&up->sti_recvt, &up->sti_fudge);
up->fl_sti = -1;
} else {
++up->tc_breply;
up->fl_sti = 0;
}
}
ept = json_object_lookup_float_default(jctx, 0, "ept", 2.0e-3);
ept = frexp(fabs(ept)*0.70710678, &xlog2);
if (ept < 0.25)
xlog2 = INT_MIN;
if (ept > 2.0)
xlog2 = INT_MAX;
up->sti_prec = clamped_precision(xlog2);
}
static void
process_pps(
peerT * const peer ,
json_ctx * const jctx ,
const l_fp * const rtime)
{
clockprocT * const pp = peer->procptr;
gpsd_unitT * const up = (gpsd_unitT *)pp->unitptr;
int xlog2;
++up->tc_pps_recv;
if (up->fl_nosync)
return;
up->pps_local = *rtime;
if (up->pf_nsec) {
if ( ! get_binary_time(&up->pps_recvt2, jctx,
"clock_sec", "clock_nsec", 1))
goto fail;
if ( ! get_binary_time(&up->pps_stamp2, jctx,
"real_sec", "real_nsec", 1))
goto fail;
} else {
if ( ! get_binary_time(&up->pps_recvt2, jctx,
"clock_sec", "clock_musec", 1000))
goto fail;
if ( ! get_binary_time(&up->pps_stamp2, jctx,
"real_sec", "real_musec", 1000))
goto fail;
}
xlog2 = json_object_lookup_int_default(
jctx, 0, "precision", up->sti_prec);
up->pps_prec = clamped_precision(xlog2);
up->pps_recvt = up->pps_recvt2;
L_SUB(&up->pps_recvt , &up->pps_fudge );
L_SUB(&up->pps_recvt2, &up->pps_fudge2);
pp->lastrec = up->pps_recvt;
up->pps_stamp = up->pps_recvt;
L_ADDUF(&up->pps_stamp, 0x80000000u);
up->pps_stamp.l_uf = 0;
if (NULL != up->pps_peer)
save_ltc(up->pps_peer->procptr,
gmprettydate(&up->pps_stamp2));
DPRINTF(2, ("%s: PPS record processed,"
" stamp='%s', recvt='%s'\n",
up->logname,
gmprettydate(&up->pps_stamp2),
gmprettydate(&up->pps_recvt2)));
up->fl_pps = (0 != (pp->sloppyclockflag & CLK_FLAG2)) - 1;
up->fl_pps2 = -1;
return;
fail:
DPRINTF(1, ("%s: PPS record processing FAILED\n",
up->logname));
++up->tc_breply;
}
static void
process_toff(
peerT * const peer ,
json_ctx * const jctx ,
const l_fp * const rtime)
{
clockprocT * const pp = peer->procptr;
gpsd_unitT * const up = (gpsd_unitT *)pp->unitptr;
++up->tc_sti_recv;
up->pf_toff = -1;
if (up->fl_nosync)
return;
if ( ! get_binary_time(&up->sti_recvt, jctx,
"clock_sec", "clock_nsec", 1))
goto fail;
if ( ! get_binary_time(&up->sti_stamp, jctx,
"real_sec", "real_nsec", 1))
goto fail;
L_SUB(&up->sti_recvt, &up->sti_fudge);
up->sti_local = *rtime;
up->fl_sti = -1;
save_ltc(pp, gmprettydate(&up->sti_stamp));
DPRINTF(2, ("%s: TOFF record processed,"
" stamp='%s', recvt='%s'\n",
up->logname,
gmprettydate(&up->sti_stamp),
gmprettydate(&up->sti_recvt)));
return;
fail:
DPRINTF(1, ("%s: TOFF record processing FAILED\n",
up->logname));
++up->tc_breply;
}
static void
gpsd_parse(
peerT * const peer ,
const l_fp * const rtime)
{
clockprocT * const pp = peer->procptr;
gpsd_unitT * const up = (gpsd_unitT *)pp->unitptr;
const char * clsid;
DPRINTF(2, ("%s: gpsd_parse: time %s '%.*s'\n",
up->logname, ulfptoa(rtime, 6),
up->buflen, up->buffer));
if (!json_parse_record(&up->json_parse, up->buffer, up->buflen)) {
++up->tc_breply;
return;
}
clsid = json_object_lookup_string(&up->json_parse, 0, "class");
if (NULL == clsid) {
++up->tc_breply;
return;
}
if (!strcmp("TPV", clsid))
process_tpv(peer, &up->json_parse, rtime);
else if (!strcmp("PPS", clsid))
process_pps(peer, &up->json_parse, rtime);
else if (!strcmp("TOFF", clsid))
process_toff(peer, &up->json_parse, rtime);
else if (!strcmp("VERSION", clsid))
process_version(peer, &up->json_parse, rtime);
else if (!strcmp("WATCH", clsid))
process_watch(peer, &up->json_parse, rtime);
else
return;
++up->tc_recv;
if (up->pps_peer)
eval_pps_secondary(
up->pps_peer, up->pps_peer->procptr, up);
if (up->fl_pps && up->fl_sti) {
l_fp diff;
diff = up->sti_local;
L_SUB(&diff, &up->pps_local);
if (diff.l_i > 0)
up->fl_pps = 0;
else if (diff.l_i < 0)
up->fl_sti = 0;
}
switch (up->mode) {
default:
case MODE_OP_STI:
eval_serial(peer, pp, up);
break;
case MODE_OP_STRICT:
eval_strict(peer, pp, up);
break;
case MODE_OP_AUTO:
eval_auto(peer, pp, up);
break;
}
}
static void
gpsd_stop_socket(
peerT * const peer)
{
clockprocT * const pp = peer->procptr;
gpsd_unitT * const up = (gpsd_unitT *)pp->unitptr;
if (-1 != pp->io.fd) {
if (syslogok(pp, up))
msyslog(LOG_INFO,
"%s: closing socket to GPSD, fd=%d",
up->logname, pp->io.fd);
else
DPRINTF(1, ("%s: closing socket to GPSD, fd=%d\n",
up->logname, pp->io.fd));
io_closeclock(&pp->io);
pp->io.fd = -1;
}
up->tickover = up->tickpres;
up->tickpres = min(up->tickpres + 5, TICKOVER_HIGH);
up->fl_vers = 0;
up->fl_sti = 0;
up->fl_pps = 0;
up->fl_watch = 0;
}
static void
gpsd_init_socket(
peerT * const peer)
{
clockprocT * const pp = peer->procptr;
gpsd_unitT * const up = (gpsd_unitT *)pp->unitptr;
addrinfoT * ai;
int rc;
int ov;
if (NULL == up->addr)
up->addr = s_gpsd_addr;
ai = up->addr;
up->addr = ai->ai_next;
up->fdt = socket(
ai->ai_family, ai->ai_socktype, ai->ai_protocol);
if (-1 == up->fdt) {
if (syslogok(pp, up))
msyslog(LOG_ERR,
"%s: cannot create GPSD socket: %m",
up->logname);
goto no_socket;
}
rc = fcntl(up->fdt, F_SETFL, O_NONBLOCK, 1);
if (-1 == rc) {
if (syslogok(pp, up))
msyslog(LOG_ERR,
"%s: cannot set GPSD socket to non-blocking: %m",
up->logname);
goto no_socket;
}
ov = 1;
rc = setsockopt(up->fdt, IPPROTO_TCP, TCP_NODELAY,
(char*)&ov, sizeof(ov));
if (-1 == rc) {
if (syslogok(pp, up))
msyslog(LOG_INFO,
"%s: cannot disable TCP nagle: %m",
up->logname);
}
rc = connect(up->fdt, ai->ai_addr, ai->ai_addrlen);
if (-1 == rc) {
if (errno == EINPROGRESS) {
DPRINTF(1, ("%s: async connect pending, fd=%d\n",
up->logname, up->fdt));
return;
}
if (syslogok(pp, up))
msyslog(LOG_ERR,
"%s: cannot connect GPSD socket: %m",
up->logname);
goto no_socket;
}
DPRINTF(1, ("%s: new socket connection, fd=%d\n",
up->logname, up->fdt));
pp->io.fd = up->fdt;
up->fdt = -1;
if (0 == io_addclock(&pp->io)) {
if (syslogok(pp, up))
msyslog(LOG_ERR,
"%s: failed to register with I/O engine",
up->logname);
goto no_socket;
}
return;
no_socket:
if (-1 != pp->io.fd)
close(pp->io.fd);
if (-1 != up->fdt)
close(up->fdt);
pp->io.fd = -1;
up->fdt = -1;
up->tickover = up->tickpres;
up->tickpres = min(up->tickpres + 5, TICKOVER_HIGH);
}
static void
gpsd_test_socket(
peerT * const peer)
{
clockprocT * const pp = peer->procptr;
gpsd_unitT * const up = (gpsd_unitT *)pp->unitptr;
int ec, rc;
socklen_t lc;
DPRINTF(2, ("%s: check connect, fd=%d\n",
up->logname, up->fdt));
#if defined(HAVE_SYS_POLL_H)
{
struct pollfd pfd;
pfd.events = POLLOUT;
pfd.fd = up->fdt;
rc = poll(&pfd, 1, 0);
if (1 != rc || !(pfd.revents & POLLOUT))
return;
}
#elif defined(HAVE_SYS_SELECT_H)
{
struct timeval tout;
fd_set wset;
memset(&tout, 0, sizeof(tout));
FD_ZERO(&wset);
FD_SET(up->fdt, &wset);
rc = select(up->fdt+1, NULL, &wset, NULL, &tout);
if (0 == rc || !(FD_ISSET(up->fdt, &wset)))
return;
}
#else
# error Blooper! That should have been found earlier!
#endif
up->tickover = TICKOVER_LOW;
ec = 0;
lc = sizeof(ec);
rc = getsockopt(up->fdt, SOL_SOCKET, SO_ERROR, &ec, &lc);
if (-1 == rc || 0 != ec) {
const char *errtxt;
if (0 == ec)
ec = errno;
errtxt = strerror(ec);
if (syslogok(pp, up))
msyslog(LOG_ERR,
"%s: async connect to GPSD failed,"
" fd=%d, ec=%d(%s)",
up->logname, up->fdt, ec, errtxt);
else
DPRINTF(1, ("%s: async connect to GPSD failed,"
" fd=%d, ec=%d(%s)\n",
up->logname, up->fdt, ec, errtxt));
goto no_socket;
} else {
DPRINTF(1, ("%s: async connect to GPSD succeeded, fd=%d\n",
up->logname, up->fdt));
}
pp->io.fd = up->fdt;
up->fdt = -1;
if (0 == io_addclock(&pp->io)) {
if (syslogok(pp, up))
msyslog(LOG_ERR,
"%s: failed to register with I/O engine",
up->logname);
goto no_socket;
}
return;
no_socket:
if (-1 != up->fdt) {
DPRINTF(1, ("%s: closing socket, fd=%d\n",
up->logname, up->fdt));
close(up->fdt);
}
up->fdt = -1;
up->tickover = up->tickpres;
up->tickpres = min(up->tickpres + 5, TICKOVER_HIGH);
}
static int16_t
clamped_precision(
int rawprec)
{
if (rawprec > 0)
rawprec = 0;
if (rawprec < -32)
rawprec = -32;
return (int16_t)rawprec;
}
static BOOL
convert_ascii_time(
l_fp * fp ,
const char * gps_time)
{
char *ep;
struct tm gd;
struct timespec ts;
uint32_t dw;
ts.tv_nsec = 0;
ep = strptime(gps_time, "%Y-%m-%dT%H:%M:%S", &gd);
if (NULL == ep)
return FALSE;
if (*ep == '.') {
dw = 100000000u;
while (isdigit(*(u_char*)++ep)) {
ts.tv_nsec += (*(u_char*)ep - '0') * dw;
dw /= 10u;
}
}
if (ep[0] != 'Z' || ep[1] != '\0')
return FALSE;
ts.tv_sec = (ntpcal_tm_to_rd(&gd) - DAY_NTP_STARTS) * SECSPERDAY
+ ntpcal_tm_to_daysec(&gd);
*fp = tspec_intv_to_lfp(ts);
return TRUE;
}
static void
save_ltc(
clockprocT * const pp,
const char * const tc)
{
size_t len = 0;
if (tc) {
len = strlen(tc);
if (len >= sizeof(pp->a_lastcode))
len = sizeof(pp->a_lastcode) - 1;
memcpy(pp->a_lastcode, tc, len);
}
pp->lencode = (u_short)len;
pp->a_lastcode[len] = '\0';
}
static int
myasprintf(
char ** spp,
char const * fmt,
... )
{
size_t alen, plen;
alen = 32;
*spp = NULL;
do {
va_list va;
alen += alen;
free(*spp);
*spp = (char*)malloc(alen);
if (NULL == *spp)
return -1;
va_start(va, fmt);
plen = (size_t)vsnprintf(*spp, alen, fmt, va);
va_end(va);
} while (plen >= alen);
return (int)plen;
}
static char *
add_string(
char *dp,
char *ep,
const char *sp)
{
while (dp != ep && *sp)
*dp++ = *sp++;
return dp;
}
static void
log_data(
peerT *peer,
const char *what,
const char *buf ,
size_t len )
{
static char s_lbuf[2048];
clockprocT * const pp = peer->procptr;
gpsd_unitT * const up = (gpsd_unitT *)pp->unitptr;
if (debug > 1) {
const char *sptr = buf;
const char *stop = buf + len;
char *dptr = s_lbuf;
char *dtop = s_lbuf + sizeof(s_lbuf) - 1;
while (sptr != stop && dptr != dtop) {
u_char uch = (u_char)*sptr++;
if (uch == '\\') {
dptr = add_string(dptr, dtop, "\\\\");
} else if (isprint(uch)) {
*dptr++ = (char)uch;
} else {
char fbuf[6];
snprintf(fbuf, sizeof(fbuf), "\\%03o", uch);
dptr = add_string(dptr, dtop, fbuf);
}
}
*dptr = '\0';
mprintf("%s[%s]: '%s'\n", up->logname, what, s_lbuf);
}
}
#else
NONEMPTY_TRANSLATION_UNIT
#endif