splint cleanup

1 files changed, 473 insertions, 0 deletions

diff --git a/ntrip.c b/ntrip.c
new file mode 100644
index 00000000..9d89c4e8
--- /dev/null
+++ b/ntrip.c
@@ -0,0 +1,473 @@
+/* ntrip.c -- gather and dispatch DGNSS data from Ntrip broadcasters */
+#include <unistd.h>
+#include <stdlib.h>
+#include <stdio.h>
+#include <math.h>
+#include <netdb.h>
+#include <sys/socket.h>
+#include <sys/time.h>
+#include <string.h>
+#include <errno.h>
+#include <fcntl.h>
+#include "gpsd.h"
+#include "bsd-base64.h"
+
+struct ntrip_stream_t {
+    char mountpoint[101];
+    enum { fmt_rtcm2, fmt_rtcm2_0, fmt_rtcm2_1, fmt_rtcm2_2, fmt_rtcm2_3, fmt_unknown } format;
+    int carrier;
+    double latitude;
+    double longitude;
+    int nmea;
+    enum { cmp_enc_none, cmp_enc_unknown } compr_encryp;
+    enum { auth_none, auth_basic, auth_digest, auth_unknown } authentication;
+    int fee;
+    int bitrate;
+};
+
+#define NTRIP_SOURCETABLE	"SOURCETABLE 200 OK\r\n"
+#define NTRIP_ENDSOURCETABLE	"ENDSOURCETABLE"
+#define NTRIP_CAS		"CAS;"
+#define NTRIP_NET		"NET;"
+#define NTRIP_STR		"STR;"
+#define NTRIP_BR		"\r\n"
+#define NTRIP_QSC		"\";\""
+#define NTRIP_ICY		"ICY 200 OK"
+#define NTRIP_UNAUTH		"401 Unauthorized"
+
+/*@ -fullinitblock @*/
+static struct ntrip_stream_t ntrip_stream = {
+    .longitude = NAN,
+    .latitude  = NAN
+};
+/*@ +fullinitblock @*/
+
+/*@ -temptrans -mustfreefresh @*/
+static char *ntrip_field_iterate(char *start, /*@null@*/char *prev, const char *eol)
+{
+    char *s, *t, *u;
+
+    if (start)
+	    s = start;
+    else {
+	if (!prev)
+	    return NULL;
+	s = prev + strlen(prev) + 1;
+	if (s >= eol)
+	    return NULL;
+    }
+
+    /* ignore any quoted ; chars as they are part of the field content */
+    t = s;
+    while ((u = strstr(t, NTRIP_QSC)))
+	t = u + strlen(NTRIP_QSC);
+
+    if ((t = strstr(t, ";")))
+	*t = '\0';
+
+    gpsd_report(5, "Next Ntrip source table field %s\n", s);	
+
+    return s;
+}
+/*@ +temptrans +mustfreefresh @*/
+
+/*@ -mustfreefresh @*/
+static void ntrip_str_parse(char *str, size_t len,
+			    /*@out@*/struct ntrip_stream_t *hold)
+{
+    char *s, *eol = str + len;
+
+    memset(hold, 0, sizeof(*hold));
+
+    /* <mountpoint> */
+    if ((s = ntrip_field_iterate(str, NULL, eol)))
+	strncpy(hold->mountpoint, s, sizeof(hold->mountpoint) - 1);
+    /* <identifier> */
+    s = ntrip_field_iterate(NULL, s, eol);
+    /* <format> */
+    if ((s = ntrip_field_iterate(NULL, s, eol))) {
+	if (strcasecmp("RTCM 2", s)==0)
+	    hold->format = fmt_rtcm2;
+	else if (strcasecmp("RTCM 2.0", s)==0)
+	    hold->format = fmt_rtcm2_0;
+	else if (strcasecmp("RTCM 2.1", s)==0)
+	    hold->format = fmt_rtcm2_1;
+	else if (strcasecmp("RTCM 2.2", s)==0)
+	    hold->format = fmt_rtcm2_2;
+	else if (strcasecmp("RTCM 2.3", s)==0)
+	    hold->format = fmt_rtcm2_3;
+	else
+	    hold->format = fmt_unknown;
+    }
+    /* <format-details> */
+    s = ntrip_field_iterate(NULL, s, eol);
+    /* <carrier> */
+    if ((s = ntrip_field_iterate(NULL, s, eol)))
+	(void)sscanf(s, "%d", &hold->carrier);
+    /* <nav-system> */
+    s = ntrip_field_iterate(NULL, s, eol);
+    /* <network> */
+    s = ntrip_field_iterate(NULL, s, eol);
+    /* <country> */
+    s = ntrip_field_iterate(NULL, s, eol);
+    /* <latitude> */
+    hold->latitude = NAN;
+    if ((s = ntrip_field_iterate(NULL, s, eol)))
+	(void)sscanf(s, "%lf", &hold->latitude);
+    /* <longitude> */
+    hold->longitude = NAN;
+    if ((s = ntrip_field_iterate(NULL, s, eol)))
+	(void)sscanf(s, "%lf", &hold->longitude);
+    /* <nmea> */
+    if ((s = ntrip_field_iterate(NULL, s, eol))) {
+	(void)sscanf(s, "%d", &hold->nmea);
+    }
+    /* <solution> */
+    s = ntrip_field_iterate(NULL, s, eol);
+    /* <generator> */
+    s = ntrip_field_iterate(NULL, s, eol);
+    /* <compr-encryp> */
+    if ((s = ntrip_field_iterate(NULL, s, eol))) {
+	if (strcasecmp("none", s)==0)
+	    hold->compr_encryp = cmp_enc_none;
+	else
+	    hold->compr_encryp = cmp_enc_unknown;
+    }
+    /* <authentication> */
+    if ((s = ntrip_field_iterate(NULL, s, eol))) {
+	if (strcasecmp("N", s)==0)
+	    hold->authentication = auth_none;
+	else if (strcasecmp("B", s)==0)
+	    hold->authentication = auth_basic;
+	else if (strcasecmp("D", s)==0)
+	    hold->authentication = auth_digest;
+	else
+	    hold->authentication = auth_unknown;
+    }
+    /* <fee> */
+    if ((s = ntrip_field_iterate(NULL, s, eol))) {
+	(void)sscanf(s, "%d", &hold->fee);
+    }
+    /* <bitrate> */
+    if ((s = ntrip_field_iterate(NULL, s, eol))) {
+	(void)sscanf(s, "%d", &hold->bitrate);
+    }
+    /* ...<misc> */
+    while ((s = ntrip_field_iterate(NULL, s, eol)));
+}
+/*@ +mustfreefresh @*/
+
+static int ntrip_sourcetable_parse(int fd, char *buf, ssize_t blen,
+				   const char *stream,
+				   struct ntrip_stream_t *keep)
+{
+    struct ntrip_stream_t hold;
+    ssize_t llen, len = 0;
+    char *line;
+    bool srctbl = false;
+    bool match = false;
+
+    for (;;) {
+	char *eol;
+	ssize_t rlen;
+
+	memset(&buf[len], 0, (size_t)(blen - len));
+
+	if ((rlen = recv(fd, &buf[len], (size_t)(blen - 1 - len), 0)) < 0) {
+	    if (errno == EINTR)
+		continue;
+	    return -1;
+	}
+	if (rlen == 0)
+	    continue;
+
+	line = buf;
+	rlen = len += rlen;
+
+	gpsd_report(5, "Ntrip source table buffer %s\n", buf);	
+	
+	if (!srctbl) {
+	    /* parse SOURCETABLE */
+	    if (strncmp(line,NTRIP_SOURCETABLE,strlen(NTRIP_SOURCETABLE))==0) {
+		srctbl = true;
+		llen = (ssize_t)strlen(NTRIP_SOURCETABLE);
+		line += llen;
+		len -= llen;
+	    } else {
+		gpsd_report(1, "Received unexpexted Ntrip reply %s.\n", buf);
+		return -1;
+	    }
+	}
+	if (!srctbl)
+		return -1;
+
+	while (len > 0) {
+	    /* parse ENDSOURCETABLE */
+	    if (strncmp(line, NTRIP_ENDSOURCETABLE, strlen(NTRIP_ENDSOURCETABLE))==0)
+		return match ? 0 : -1;
+
+	    if (!(eol = strstr(line, NTRIP_BR)))
+		break;
+
+	    gpsd_report(4, "next Ntrip source table line %s\n", line);	
+
+	    *eol = '\0';
+	    llen = (ssize_t)(eol - line);
+
+	    /* todo: parse headers */
+
+	    /* parse STR */
+	    if (strncmp(line, NTRIP_STR, strlen(NTRIP_STR))==0) {
+		ntrip_str_parse(line + strlen(NTRIP_STR), (size_t)(llen - strlen(NTRIP_STR)), &hold);
+		if (stream!=NULL && strcmp(stream, hold.mountpoint)==0) {
+		    /* todo: support for RTCM 3.0, SBAS (WAAS, EGNOS), ... */
+		    if (hold.format == fmt_unknown) {
+			gpsd_report(1,
+				    "Ntrip stream %s format not supported\n",
+				    line);	
+			return -1;
+		    }
+		    /* todo: support encryption and compression algorithms */
+		    if (hold.compr_encryp != cmp_enc_none) {
+			gpsd_report(1,
+				    "Ntrip stream %s compression/encryption algorithm not supported\n",
+				    line);	
+			return -1;
+		    }
+		    /* todo: support digest authentication */
+		    if (hold.authentication != auth_none 
+			&& hold.authentication != auth_basic) {
+			gpsd_report(1,
+				    "Ntrip stream %s authentication method not supported\n",
+				    line);	
+			return -1;
+		    }
+		    memcpy(keep, &hold, sizeof(hold));
+		    match = true;
+		}
+		/* todo: compare stream location to own location to
+		   find nearest stream if user hasn't provided one */
+	    }
+	    /* todo: parse CAS */
+	    /* else if (strncmp(line, NTRIP_CAS, strlen(NTRIP_CAS))==0); */
+
+	    /* todo: parse NET */
+	    /* else if (strncmp(line, NTRIP_NET, strlen(NTRIP_NET))==0); */
+
+	    llen += strlen(NTRIP_BR);
+	    line += llen;
+	    len -= llen;
+	    gpsd_report(5, "Remaining Ntrip source table buffer %d %s\n", len, line);
+	}
+	/* message too big to fit into buffer */
+	if (len == blen - 1)
+	    return -1;
+
+	if (len > 0)
+	    memcpy(buf, &buf[rlen-len], (size_t)len);
+    }
+
+    return (int)len;
+}
+
+static int ntrip_stream_probe(const char *caster,
+			      const char *port,
+			      const char *stream,
+			      struct ntrip_stream_t *keep)
+{
+    int ret;
+    int dsock;
+    char buf[BUFSIZ];
+
+    if ((dsock = netlib_connectsock(caster, port, "tcp")) < 0) {
+	    printf("error %d\n", dsock);
+	    return -1;
+    }
+    (void)snprintf(buf, sizeof(buf),
+		   "GET / HTTP/1.1\r\n"
+		   "User-Agent: NTRIP gpsd/%s\r\n"
+		   "Connection: close\r\n"
+		   "\r\n",
+		   VERSION);
+    (void)write(dsock, buf, strlen(buf));
+    ret = ntrip_sourcetable_parse(dsock, buf, (ssize_t)sizeof(buf), stream, keep);
+    (void)close(dsock);
+    return ret;
+}
+
+static int ntrip_auth_encode(const struct ntrip_stream_t *stream,
+			     const char *auth, 
+			     /*@out@*/char buf[], 
+			     size_t size)
+{
+    memset(buf, 0, size);
+    if (stream->authentication == auth_none)
+	return 0;
+    else if (stream->authentication == auth_basic) {
+	char authenc[64];
+	if (!auth)
+	    return -1;
+	memset(authenc, 0, sizeof(authenc));
+	if (b64_ntop((u_char *) auth, strlen(auth), authenc, sizeof(authenc) - 1) < 0)
+	    return -1;
+	(void)snprintf(buf, size - 1, "Authorization: Basic %s\r\n", authenc);
+    } else {
+	/* todo: support digest authentication */
+    }
+    return 0;
+}
+
+/*@ -nullpass @*/ /* work around a splint bug */
+static int ntrip_stream_open(const char *caster,
+			     const char *port,
+			     const char *auth,
+			     struct gps_context_t *context, 
+			     struct ntrip_stream_t *stream)
+{
+    char buf[BUFSIZ];
+    char authstr[128];
+    int opts;
+
+    if (ntrip_auth_encode(stream, auth, authstr, sizeof(authstr)) < 0) {
+	gpsd_report(1, "User-ID and password needed for %s:%s/%s\n",
+		    caster, port, stream->mountpoint);	
+	return -1;
+    }
+    if ((context->dsock = netlib_connectsock(caster, port, "tcp")) < 0)
+	return -1;
+
+    (void)snprintf(buf, sizeof(buf),
+		   "GET /%s HTTP/1.1\r\n"
+		   "User-Agent: NTRIP gpsd/%s\r\n"
+		   "Accept: rtk/rtcm, dgps/rtcm\r\n"
+		   "%s"
+		   "Connection: close\r\n"
+		   "\r\n",
+		   stream->mountpoint, VERSION, authstr);
+    (void)write(context->dsock, buf, strlen(buf));
+
+    memset(buf, 0, sizeof(buf));
+    if (read(context->dsock, buf, sizeof(buf) - 1) < 0)
+	goto close;
+
+    /* parse 401 Unauthorized */
+    if (strstr(buf, NTRIP_UNAUTH)) {
+	gpsd_report(1, "%s not authorized for Ntrip stream %s:%s/%s\n",
+		    auth, caster, port, stream->mountpoint);	
+	goto close;
+    }
+    /* parse SOURCETABLE */
+    if (strstr(buf, NTRIP_SOURCETABLE)) {
+	gpsd_report(1, "Broadcaster doesn't recognize Ntrip stream %s:%s/%s\n",
+		    caster, port, stream->mountpoint);	
+	goto close;
+    }
+    /* parse ICY 200 OK */
+    if (!strstr(buf, NTRIP_ICY)) {
+	gpsd_report(1, "Unknown reply %s from Ntrip service %s:%s/%s\n",
+		    buf, caster, port, stream->mountpoint);	
+	goto close;
+    }
+    opts = fcntl(context->dsock, F_GETFL);
+
+    if (opts >= 0)
+	(void)fcntl(context->dsock, F_SETFL, opts | O_NONBLOCK);
+
+    context->dgnss_service = dgnss_ntrip;
+#ifndef S_SPLINT_S
+    context->dgnss_privdata = stream;
+#endif
+    return context->dsock;
+close:
+    (void)close(context->dsock);
+    return -1;
+}
+/*@ +nullpass @*/
+
+/*@ -branchstate @*/
+int ntrip_open(struct gps_context_t *context, char *caster)
+/* open a connection to a Ntrip broadcaster */
+{
+    char *amp, *colon, *slash;
+    char *auth = NULL;
+    char *port = NULL;
+    char *stream = NULL;
+    int ret;
+
+    /*@ -boolops @*/
+    if ((amp = strchr(caster, '@'))) {
+	if ((colon = strchr(caster, ':')) &&  colon < amp) { 
+	    auth = caster;
+	    *amp = '\0';
+	    caster = amp + 1;
+	} else {
+	    gpsd_report(1, "can't extract user-ID and password from %s\n",
+			caster);
+	    return -1;
+	}
+    }
+    /*@ +boolops @*/
+    if ((slash = strchr(caster, '/'))) {
+	*slash = '\0';
+	stream = slash + 1;
+    } else {
+	/* todo: add autoconnect like in dgpsip.c */
+	gpsd_report(1, "can't extract Ntrip stream from %s\n", caster);
+	return -1;
+    }
+    if ((colon = strchr(caster, ':'))) {
+	port = colon + 1;
+	*colon = '\0';
+    }
+    if (!port) {
+	port = "rtcm-sc104";
+	if (!getservbyname(port, "tcp"))
+	    port = DEFAULT_RTCM_PORT;
+    }
+    if (ntrip_stream_probe(caster, port, stream, &ntrip_stream)) {
+	gpsd_report(1, "unable to probe for data about stream %s:%s/%s\n",
+		    caster, port, stream);
+	return -1;
+    }
+    ret = ntrip_stream_open(caster, port, auth, context, &ntrip_stream);
+    if (ret >= 0)
+	gpsd_report(1,"connection to Ntrip broadcaster %s established.\n",
+		    caster);
+    else
+	gpsd_report(1, "can't connect to Ntrip stream %s:%s/%s.\n",
+		    caster, port, stream);
+    return ret;
+}
+/*@ +branchstate @*/
+
+void ntrip_poll(struct gps_context_t *context)
+/* poll the NTRIP server for a correction report */
+{
+    if (context->dsock > -1) {
+	context->rtcmbytes = read(context->dsock, context->rtcmbuf, sizeof(context->rtcmbuf));
+	if (context->rtcmbytes < 0 && errno != EAGAIN)
+	    gpsd_report(1, "Read from rtcm source failed\n");
+	else
+	    context->rtcmtime = timestamp();
+    }
+}
+
+void ntrip_report(struct gps_device_t *session)
+/* may be time to ship a usage report to the Ntrip caster */
+{
+    struct ntrip_stream_t *stream = session->context->dgnss_privdata;
+    /*
+     * 10 is an arbitrary number, the point is to have gotten several good
+     * fixes before reporting usage to our Ntrip caster.
+     */
+    if (stream!=NULL && stream->nmea!=0
+	&& session->context->fixcnt > 10 && !session->context->sentdgps) {
+	session->context->sentdgps = true;
+	if (session->context->dsock > -1) {
+	    char buf[BUFSIZ];
+	    gpsd_position_fix_dump(session, buf, sizeof(buf));
+	    (void)write(session->context->dsock, buf, strlen(buf));
+	    gpsd_report(2, "=> dgps %s", buf);
+	}
+    }
+}