net_ntrip.c

   1 /* net_ntrip.c -- gather and dispatch DGNSS data from Ntrip broadcasters
   2  *
   3  * This file is Copyright (c) 2010 by the GPSD project
   4  * BSD terms apply: see the file COPYING in the distribution root for details.
   5  */
   6 #include <stdlib.h>
   7 #include "gpsd_config.h"
   8 #include <sys/types.h>
   9 #ifndef S_SPLINT_S
  10 #ifdef HAVE_SYS_SOCKET_H
  11 #include <sys/socket.h>
  12 #else
  13 #define AF_UNSPEC 0
  14 #endif /* HAVE_SYS_SOCKET_H */
  15 #include <unistd.h>
  16 #endif /* S_SPLINT_S */
  17 #include <sys/time.h>
  18 #include <stdio.h>
  19 #include <math.h>
  20 #ifndef S_SPLINT_S
  21 #ifdef HAVE_NETDB_H
  22 #include <netdb.h>
  23 #endif
  24 #endif /* S_SPLINT_S */
  25 #include <string.h>
  26 #include <errno.h>
  27 #include <fcntl.h>
  28
  29 #include "gpsd.h"
  30 #include "bsd-base64.h"
  31
  32 struct ntrip_stream_t
  33 {
  34     char mountpoint[101];
  35     enum
  36     {
  37         fmt_rtcm2,
  38         fmt_rtcm2_0,
  39         fmt_rtcm2_1,
  40         fmt_rtcm2_2,
  41         fmt_rtcm2_3,
  42         fmt_rtcm3,
  43         fmt_unknown
  44     } format;
  45     int carrier;
  46     double latitude;
  47     double longitude;
  48     int nmea;
  49     enum
  50     { cmp_enc_none, cmp_enc_unknown } compr_encryp;
  51     enum
  52     { auth_none, auth_basic, auth_digest, auth_unknown } authentication;
  53     int fee;
  54     int bitrate;
  55 };
  56
  57 #define NTRIP_SOURCETABLE       "SOURCETABLE 200 OK\r\n"
  58 #define NTRIP_ENDSOURCETABLE    "ENDSOURCETABLE"
  59 #define NTRIP_CAS               "CAS;"
  60 #define NTRIP_NET               "NET;"
  61 #define NTRIP_STR               "STR;"
  62 #define NTRIP_BR                "\r\n"
  63 #define NTRIP_QSC               "\";\""
  64 #define NTRIP_ICY               "ICY 200 OK"
  65 #define NTRIP_UNAUTH            "401 Unauthorized"
  66
  67 static struct ntrip_stream_t ntrip_stream;
  68
  69 /*@ -temptrans -mustfreefresh @*/
  70 static /*@null@*/ char *ntrip_field_iterate( /*@null@ */ char *start,
  71                                              /*@null@*/ char *prev,
  72                                              const char *eol)
  73 {
  74     char *s, *t, *u;
  75
  76     if (start)
  77         s = start;
  78     else {
  79         if (!prev)
  80             return NULL;
  81         s = prev + strlen(prev) + 1;
  82         if (s >= eol)
  83             return NULL;
  84     }
  85
  86     /* ignore any quoted ; chars as they are part of the field content */
  87     t = s;
  88     while ((u = strstr(t, NTRIP_QSC)))
  89         t = u + strlen(NTRIP_QSC);
  90
  91     if ((t = strstr(t, ";")))
  92         *t = '\0';
  93
  94     gpsd_report(LOG_RAW, "Next Ntrip source table field %s\n", s);
  95
  96     return s;
  97 }
  98
  99 /*@ +temptrans +mustfreefresh @*/
 100
 101 /*@ -mustfreefresh @*/
 102 static void ntrip_str_parse(char *str, size_t len,
 103                             /*@out@*/ struct ntrip_stream_t *hold)
 104 {
 105     char *s, *eol = str + len;
 106
 107     memset(hold, 0, sizeof(*hold));
 108
 109     /* <mountpoint> */
 110     if ((s = ntrip_field_iterate(str, NULL, eol)))
 111         strncpy(hold->mountpoint, s, sizeof(hold->mountpoint) - 1);
 112     /* <identifier> */
 113     s = ntrip_field_iterate(NULL, s, eol);
 114     /* <format> */
 115     if ((s = ntrip_field_iterate(NULL, s, eol))) {
 116         if (strcasecmp("RTCM 2", s) == 0)
 117             hold->format = fmt_rtcm2;
 118         else if (strcasecmp("RTCM 2.0", s) == 0)
 119             hold->format = fmt_rtcm2_0;
 120         else if (strcasecmp("RTCM 2.1", s) == 0)
 121             hold->format = fmt_rtcm2_1;
 122         else if (strcasecmp("RTCM 2.2", s) == 0)
 123             hold->format = fmt_rtcm2_2;
 124         else if (strcasecmp("RTCM 2.3", s) == 0)
 125             hold->format = fmt_rtcm2_3;
 126         else if (strcasecmp("RTCM 3.0", s) == 0)
 127             hold->format = fmt_rtcm3;
 128         else
 129             hold->format = fmt_unknown;
 130     }
 131     /* <format-details> */
 132     s = ntrip_field_iterate(NULL, s, eol);
 133     /* <carrier> */
 134     if ((s = ntrip_field_iterate(NULL, s, eol)))
 135         (void)sscanf(s, "%d", &hold->carrier);
 136     /* <nav-system> */
 137     s = ntrip_field_iterate(NULL, s, eol);
 138     /* <network> */
 139     s = ntrip_field_iterate(NULL, s, eol);
 140     /* <country> */
 141     s = ntrip_field_iterate(NULL, s, eol);
 142     /* <latitude> */
 143     hold->latitude = NAN;
 144     if ((s = ntrip_field_iterate(NULL, s, eol)))
 145         (void)sscanf(s, "%lf", &hold->latitude);
 146     /* <longitude> */
 147     hold->longitude = NAN;
 148     if ((s = ntrip_field_iterate(NULL, s, eol)))
 149         (void)sscanf(s, "%lf", &hold->longitude);
 150     /* <nmea> */
 151     if ((s = ntrip_field_iterate(NULL, s, eol))) {
 152         (void)sscanf(s, "%d", &hold->nmea);
 153     }
 154     /* <solution> */
 155     s = ntrip_field_iterate(NULL, s, eol);
 156     /* <generator> */
 157     s = ntrip_field_iterate(NULL, s, eol);
 158     /* <compr-encryp> */
 159     if ((s = ntrip_field_iterate(NULL, s, eol))) {
 160         if (strcasecmp("none", s) == 0)
 161             hold->compr_encryp = cmp_enc_none;
 162         else
 163             hold->compr_encryp = cmp_enc_unknown;
 164     }
 165     /* <authentication> */
 166     if ((s = ntrip_field_iterate(NULL, s, eol))) {
 167         if (strcasecmp("N", s) == 0)
 168             hold->authentication = auth_none;
 169         else if (strcasecmp("B", s) == 0)
 170             hold->authentication = auth_basic;
 171         else if (strcasecmp("D", s) == 0)
 172             hold->authentication = auth_digest;
 173         else
 174             hold->authentication = auth_unknown;
 175     }
 176     /* <fee> */
 177     if ((s = ntrip_field_iterate(NULL, s, eol))) {
 178         (void)sscanf(s, "%d", &hold->fee);
 179     }
 180     /* <bitrate> */
 181     if ((s = ntrip_field_iterate(NULL, s, eol))) {
 182         (void)sscanf(s, "%d", &hold->bitrate);
 183     }
 184     /* ...<misc> */
 185     while ((s = ntrip_field_iterate(NULL, s, eol)));
 186 }
 187
 188 /*@ +mustfreefresh @*/
 189
 190 static int ntrip_sourcetable_parse(int fd, char *buf, ssize_t blen,
 191                                    const char *stream,
 192                                    struct ntrip_stream_t *keep)
 193 {
 194     struct ntrip_stream_t hold;
 195     ssize_t llen, len = 0;
 196     char *line;
 197     bool srctbl = false;
 198     bool match = false;
 199
 200     for (;;) {
 201         char *eol;
 202         ssize_t rlen;
 203
 204         memset(&buf[len], 0, (size_t) (blen - len));
 205
 206         if ((rlen = recv(fd, &buf[len], (size_t) (blen - 1 - len), 0)) == -1) {
 207             if (errno == EINTR)
 208                 continue;
 209             return -1;
 210         }
 211         if (rlen == 0)
 212             continue;
 213
 214         line = buf;
 215         rlen = len += rlen;
 216
 217         gpsd_report(LOG_RAW, "Ntrip source table buffer %s\n", buf);
 218
 219         if (!srctbl) {
 220             /* parse SOURCETABLE */
 221             if (strncmp(line, NTRIP_SOURCETABLE, strlen(NTRIP_SOURCETABLE)) ==
 222                 0) {
 223                 srctbl = true;
 224                 llen = (ssize_t) strlen(NTRIP_SOURCETABLE);
 225                 line += llen;
 226                 len -= llen;
 227             } else {
 228                 gpsd_report(LOG_WARN, "Received unexpexted Ntrip reply %s.\n",
 229                             buf);
 230                 return -1;
 231             }
 232         }
 233         if (!srctbl)
 234             return -1;
 235
 236         while (len > 0) {
 237             /* parse ENDSOURCETABLE */
 238             if (strncmp
 239                 (line, NTRIP_ENDSOURCETABLE,
 240                  strlen(NTRIP_ENDSOURCETABLE)) == 0)
 241                 goto done;
 242
 243             if (!(eol = strstr(line, NTRIP_BR)))
 244                 break;
 245
 246             gpsd_report(LOG_IO, "next Ntrip source table line %s\n", line);
 247
 248             *eol = '\0';
 249             llen = (ssize_t) (eol - line);
 250
 251             /* todo: parse headers */
 252
 253             /* parse STR */
 254             if (strncmp(line, NTRIP_STR, strlen(NTRIP_STR)) == 0) {
 255                 ntrip_str_parse(line + strlen(NTRIP_STR),
 256                                 (size_t) (llen - strlen(NTRIP_STR)), &hold);
 257                 if (stream != NULL && strcmp(stream, hold.mountpoint) == 0) {
 258                     /* todo: support for RTCM 3.0, SBAS (WAAS, EGNOS), ... */
 259                     if (hold.format == fmt_unknown) {
 260                         gpsd_report(LOG_ERROR,
 261                                     "Ntrip stream %s format not supported\n",
 262                                     line);
 263                         return -1;
 264                     }
 265                     /* todo: support encryption and compression algorithms */
 266                     if (hold.compr_encryp != cmp_enc_none) {
 267                         gpsd_report(LOG_ERROR,
 268                                     "Ntrip stream %s compression/encryption algorithm not supported\n",
 269                                     line);
 270                         return -1;
 271                     }
 272                     /* todo: support digest authentication */
 273                     if (hold.authentication != auth_none
 274                         && hold.authentication != auth_basic) {
 275                         gpsd_report(LOG_ERROR,
 276                                     "Ntrip stream %s authentication method not supported\n",
 277                                     line);
 278                         return -1;
 279                     }
 280                     memcpy(keep, &hold, sizeof(hold));
 281                     match = true;
 282                 }
 283                 /* todo: compare stream location to own location to
 284                  * find nearest stream if user hasn't provided one */
 285             }
 286             /* todo: parse CAS */
 287             /* else if (strncmp(line, NTRIP_CAS, strlen(NTRIP_CAS))==0); */
 288
 289             /* todo: parse NET */
 290             /* else if (strncmp(line, NTRIP_NET, strlen(NTRIP_NET))==0); */
 291
 292             llen += strlen(NTRIP_BR);
 293             line += llen;
 294             len -= llen;
 295             gpsd_report(LOG_RAW,
 296                         "Remaining Ntrip source table buffer %zd %s\n", len,
 297                         line);
 298         }
 299         /* message too big to fit into buffer */
 300         if (len == blen - 1)
 301             return -1;
 302
 303         if (len > 0)
 304             memcpy(buf, &buf[rlen - len], (size_t) len);
 305     }
 306
 307   done:
 308     return match ? 0 : -1;
 309 }
 310
 311 static int ntrip_stream_probe(const char *caster,
 312                               const char *port,
 313                               const char *stream, struct ntrip_stream_t *keep)
 314 {
 315     int ret;
 316     socket_t dsock;
 317     char buf[BUFSIZ];
 318
 319     if ((dsock = netlib_connectsock(AF_UNSPEC, caster, port, "tcp")) == -1) {
 320         printf("ntrip stream connect error %d\n", dsock);
 321         return -1;
 322     }
 323     (void)snprintf(buf, sizeof(buf),
 324                    "GET / HTTP/1.1\r\n"
 325                    "User-Agent: NTRIP gpsd/%s\r\n"
 326                    "Connection: close\r\n" "\r\n", VERSION);
 327     if (write(dsock, buf, strlen(buf)) != (ssize_t) strlen(buf)) {
 328         printf("ntrip stream write error %d\n", dsock);
 329         return -1;
 330     }
 331     ret =
 332         ntrip_sourcetable_parse(dsock, buf, (ssize_t) sizeof(buf), stream,
 333                                 keep);
 334     (void)close(dsock);
 335     return ret;
 336 }
 337
 338 static int ntrip_auth_encode(const struct ntrip_stream_t *stream,
 339                              const char *auth,
 340                              /*@out@*/ char buf[],
 341                              size_t size)
 342 {
 343     memset(buf, 0, size);
 344     if (stream->authentication == auth_none)
 345         return 0;
 346     else if (stream->authentication == auth_basic) {
 347         char authenc[64];
 348         if (!auth)
 349             return -1;
 350         memset(authenc, 0, sizeof(authenc));
 351         if (b64_ntop
 352             ((unsigned char *)auth, strlen(auth), authenc,
 353              sizeof(authenc) - 1) < 0)
 354             return -1;
 355         (void)snprintf(buf, size - 1, "Authorization: Basic %s\r\n", authenc);
 356     } else {
 357         /* todo: support digest authentication */
 358     }
 359     return 0;
 360 }
 361
 362 /* *INDENT-OFF* */
 363 /*@ -nullpass @*//* work around a splint bug */
 364 static int ntrip_stream_open(const char *caster, const char *port,
 365                              /*@null@*/ const char *auth,
 366                              struct gps_context_t *context,
 367                              struct ntrip_stream_t *stream)
 368 {
 369     char buf[BUFSIZ];
 370     char authstr[128];
 371     int opts;
 372
 373     if (ntrip_auth_encode(stream, auth, authstr, sizeof(authstr)) < 0) {
 374         gpsd_report(LOG_ERROR, "User-ID and password needed for %s:%s/%s\n",
 375                     caster, port, stream->mountpoint);
 376         return -1;
 377     }
 378     if ((context->dsock =
 379          netlib_connectsock(AF_UNSPEC, caster, port, "tcp")) < 0)
 380         return -1;
 381
 382     (void)snprintf(buf, sizeof(buf),
 383                    "GET /%s HTTP/1.1\r\n"
 384                    "User-Agent: NTRIP gpsd/%s\r\n"
 385                    "Accept: rtk/rtcm, dgps/rtcm\r\n"
 386                    "%s"
 387                    "Connection: close\r\n"
 388                    "\r\n", stream->mountpoint, VERSION, authstr);
 389     if (write(context->dsock, buf, strlen(buf)) != (ssize_t) strlen(buf)) {
 390         printf("ntrip stream write error on %d\n", context->dsock);
 391         return -1;
 392     }
 393
 394     memset(buf, 0, sizeof(buf));
 395     if (read(context->dsock, buf, sizeof(buf) - 1) == -1)
 396         goto close;
 397
 398     /* parse 401 Unauthorized */
 399     if (strstr(buf, NTRIP_UNAUTH)) {
 400         gpsd_report(LOG_ERROR,
 401                     "%s not authorized for Ntrip stream %s:%s/%s\n", auth,
 402                     caster, port, stream->mountpoint);
 403         goto close;
 404     }
 405     /* parse SOURCETABLE */
 406     if (strstr(buf, NTRIP_SOURCETABLE)) {
 407         gpsd_report(LOG_ERROR,
 408                     "Broadcaster doesn't recognize Ntrip stream %s:%s/%s\n",
 409                     caster, port, stream->mountpoint);
 410         goto close;
 411     }
 412     /* parse ICY 200 OK */
 413     if (!strstr(buf, NTRIP_ICY)) {
 414         gpsd_report(LOG_ERROR,
 415                     "Unknown reply %s from Ntrip service %s:%s/%s\n", buf,
 416                     caster, port, stream->mountpoint);
 417         goto close;
 418     }
 419     opts = fcntl(context->dsock, F_GETFL);
 420
 421     if (opts >= 0)
 422         (void)fcntl(context->dsock, F_SETFL, opts | O_NONBLOCK);
 423
 424     context->netgnss_service = netgnss_ntrip;
 425 #ifndef S_SPLINT_S
 426     context->netgnss_privdata = stream;
 427 #endif
 428     return context->dsock;
 429   close:
 430     (void)close(context->dsock);
 431     return -1;
 432 }
 433 /* *INDENT-ON* */
 434
 435 /*@ +nullpass @*/
 436
 437 /*@ -branchstate @*/
 438 int ntrip_open(struct gps_context_t *context, char *caster)
 439 /* open a connection to a Ntrip broadcaster */
 440 {
 441     char *amp, *colon, *slash;
 442     char *auth = NULL;
 443     char *port = NULL;
 444     char *stream = NULL;
 445     int ret;
 446
 447     /*@ -boolops @*/
 448     if ((amp = strchr(caster, '@')) != NULL) {
 449         if (((colon = strchr(caster, ':')) != NULL) && colon < amp) {
 450             auth = caster;
 451             *amp = '\0';
 452             caster = amp + 1;
 453         } else {
 454             gpsd_report(LOG_ERROR,
 455                         "can't extract user-ID and password from %s\n",
 456                         caster);
 457             return -1;
 458         }
 459     }
 460     /*@ +boolops @*/
 461     if ((slash = strchr(caster, '/')) != NULL) {
 462         *slash = '\0';
 463         stream = slash + 1;
 464     } else {
 465         /* todo: add autoconnect like in dgpsip.c */
 466         gpsd_report(LOG_ERROR, "can't extract Ntrip stream from %s\n",
 467                     caster);
 468         return -1;
 469     }
 470     if ((colon = strchr(caster, ':')) != NULL) {
 471         port = colon + 1;
 472         *colon = '\0';
 473     }
 474     if (!port) {
 475         port = "rtcm-sc104";
 476         if (!getservbyname(port, "tcp"))
 477             port = DEFAULT_RTCM_PORT;
 478     }
 479     if (ntrip_stream_probe(caster, port, stream, &ntrip_stream)) {
 480         gpsd_report(LOG_ERROR,
 481                     "unable to probe for data about stream %s:%s/%s\n",
 482                     caster, port, stream);
 483         return -1;
 484     }
 485     ret = ntrip_stream_open(caster, port, auth, context, &ntrip_stream);
 486     if (ret >= 0)
 487         gpsd_report(LOG_PROG,
 488                     "connection to Ntrip broadcaster %s established.\n",
 489                     caster);
 490     else
 491         gpsd_report(LOG_ERROR, "can't connect to Ntrip stream %s:%s/%s.\n",
 492                     caster, port, stream);
 493     return ret;
 494 }
 495
 496 /*@ +branchstate @*/
 497
 498 void ntrip_report(struct gps_device_t *session)
 499 /* may be time to ship a usage report to the Ntrip caster */
 500 {
 501     struct ntrip_stream_t *stream = session->context->netgnss_privdata;
 502     /*
 503      * 10 is an arbitrary number, the point is to have gotten several good
 504      * fixes before reporting usage to our Ntrip caster.
 505      */
 506     if (stream != NULL && stream->nmea != 0
 507         && session->context->fixcnt > 10 && !session->context->sentdgps) {
 508         session->context->sentdgps = true;
 509         if (session->context->dsock > -1) {
 510             char buf[BUFSIZ];
 511             gpsd_position_fix_dump(session, buf, sizeof(buf));
 512             if (write(session->context->dsock, buf, strlen(buf)) ==
 513                 (ssize_t) strlen(buf))
 514                 gpsd_report(LOG_IO, "=> dgps %s\n", buf);
 515             else
 516                 gpsd_report(LOG_IO, "ntrip report write failed\n");
 517         }
 518     }
 519 }