1 /* net_ntrip.c -- gather and dispatch DGNSS data from Ntrip broadcasters
2  *
3  * This file is Copyright (c) 2010-2018 by the GPSD project
4  * SPDX-License-Identifier: BSD-2-clause
5  */
6 
7 #include "gpsd_config.h"  /* must be before all includes */
8 
9 #include <errno.h>
10 #include <fcntl.h>
11 #include <math.h>
12 #include <netdb.h>
13 #include <stdbool.h>
14 #include <stdio.h>
15 #include <stdlib.h>
16 #include <string.h>
17 #include <strings.h>
18 #include <sys/socket.h>
19 #include <sys/stat.h>
20 #include <sys/types.h>
21 #include <unistd.h>
22 
23 #include "gpsd.h"
24 #include "strfuncs.h"
25 
26 #define NTRIP_SOURCETABLE       "SOURCETABLE 200 OK\r\n"
27 #define NTRIP_ENDSOURCETABLE    "ENDSOURCETABLE"
28 #define NTRIP_CAS               "CAS;"
29 #define NTRIP_NET               "NET;"
30 #define NTRIP_STR               "STR;"
31 #define NTRIP_BR                "\r\n"
32 #define NTRIP_QSC               "\";\""
33 #define NTRIP_ICY               "ICY 200 OK"
34 #define NTRIP_UNAUTH            "401 Unauthorized"
35 
ntrip_field_iterate(char * start,char * prev,const char * eol,const struct gpsd_errout_t * errout)36 static char *ntrip_field_iterate(char *start,
37                                  char *prev,
38                                  const char *eol,
39                                  const struct gpsd_errout_t *errout)
40 {
41     char *s, *t, *u;
42 
43     if (start)
44         s = start;
45     else {
46         if (!prev)
47             return NULL;
48         s = prev + strlen(prev) + 1;
49         if (s >= eol)
50             return NULL;
51     }
52 
53     /* ignore any quoted ; chars as they are part of the field content */
54     t = s;
55     while ((u = strstr(t, NTRIP_QSC)))
56         t = u + strlen(NTRIP_QSC);
57 
58     if ((t = strstr(t, ";")))
59         *t = '\0';
60 
61     GPSD_LOG(LOG_RAW, errout, "Next Ntrip source table field %s\n", s);
62 
63     return s;
64 }
65 
66 
ntrip_str_parse(char * str,size_t len,struct ntrip_stream_t * hold,const struct gpsd_errout_t * errout)67 static void ntrip_str_parse(char *str, size_t len,
68                             struct ntrip_stream_t *hold,
69                             const struct gpsd_errout_t *errout)
70 {
71     char *s, *eol = str + len;
72 
73     memset(hold, 0, sizeof(*hold));
74 
75     /* <mountpoint> */
76     if ((s = ntrip_field_iterate(str, NULL, eol, errout)))
77         (void)strlcpy(hold->mountpoint, s, sizeof(hold->mountpoint));
78     /* <identifier> */
79     s = ntrip_field_iterate(NULL, s, eol, errout);
80     /* <format> */
81     if ((s = ntrip_field_iterate(NULL, s, eol, errout))) {
82 	if ((strcasecmp("RTCM 2", s) == 0) ||
83             (strcasecmp("RTCM2", s) == 0))
84             hold->format = fmt_rtcm2;
85         else if (strcasecmp("RTCM 2.0", s) == 0)
86             hold->format = fmt_rtcm2_0;
87         else if (strcasecmp("RTCM 2.1", s) == 0)
88             hold->format = fmt_rtcm2_1;
89         else if ((strcasecmp("RTCM 2.2", s) == 0) ||
90                  (strcasecmp("RTCM22", s) == 0))
91             hold->format = fmt_rtcm2_2;
92         else if ((strcasecmp("RTCM2.3", s) == 0) ||
93                  (strcasecmp("RTCM 2.3", s) == 0))
94             hold->format = fmt_rtcm2_3;
95         /* required for the SAPOS derver in Gemany, confirmed as RTCM2.3 */
96         else if (strcasecmp("RTCM1_", s) == 0)
97             hold->format = fmt_rtcm2_3;
98         else if ((strcasecmp("RTCM 3", s) == 0) ||
99                  (strcasecmp("RTCM 3.0", s) == 0) ||
100                  (strcasecmp("RTCM3.0", s) == 0) ||
101                  (strcasecmp("RTCM3", s) == 0))
102             hold->format = fmt_rtcm3_0;
103         else if ((strcasecmp("RTCM3.1", s) == 0) ||
104                  (strcasecmp("RTCM 3.1", s) == 0))
105             hold->format = fmt_rtcm3_1;
106         else if ((strcasecmp("RTCM 3.2", s) == 0) ||
107                  (strcasecmp("RTCM32", s) == 0))
108             hold->format = fmt_rtcm3_2;
109         else if (strcasecmp("RTCM 3.3", s) == 0)
110             hold->format = fmt_rtcm3_3;
111         else
112             hold->format = fmt_unknown;
113     }
114     /* <format-details> */
115     s = ntrip_field_iterate(NULL, s, eol, errout);
116     /* <carrier> */
117     if ((s = ntrip_field_iterate(NULL, s, eol, errout)))
118         hold->carrier = atoi(s);
119     /* <nav-system> */
120     s = ntrip_field_iterate(NULL, s, eol, errout);
121     /* <network> */
122     s = ntrip_field_iterate(NULL, s, eol, errout);
123     /* <country> */
124     s = ntrip_field_iterate(NULL, s, eol, errout);
125     /* <latitude> */
126     hold->latitude = NAN;
127     if ((s = ntrip_field_iterate(NULL, s, eol, errout)))
128         hold->latitude = safe_atof(s);
129     /* <longitude> */
130     hold->longitude = NAN;
131     if ((s = ntrip_field_iterate(NULL, s, eol, errout)))
132         hold->longitude = safe_atof(s);
133     /* <nmea> */
134     if ((s = ntrip_field_iterate(NULL, s, eol, errout))) {
135         hold->nmea = atoi(s);
136     }
137     /* <solution> */
138     s = ntrip_field_iterate(NULL, s, eol, errout);
139     /* <generator> */
140     s = ntrip_field_iterate(NULL, s, eol, errout);
141     /* <compr-encryp> */
142     if ((s = ntrip_field_iterate(NULL, s, eol, errout))) {
143         if (strcasecmp("none", s) == 0)
144             hold->compr_encryp = cmp_enc_none;
145         else
146             hold->compr_encryp = cmp_enc_unknown;
147     }
148     /* <authentication> */
149     if ((s = ntrip_field_iterate(NULL, s, eol, errout))) {
150         if (strcasecmp("N", s) == 0)
151             hold->authentication = auth_none;
152         else if (strcasecmp("B", s) == 0)
153             hold->authentication = auth_basic;
154         else if (strcasecmp("D", s) == 0)
155             hold->authentication = auth_digest;
156         else
157             hold->authentication = auth_unknown;
158     }
159     /* <fee> */
160     if ((s = ntrip_field_iterate(NULL, s, eol, errout))) {
161         hold->fee = atoi(s);
162     }
163     /* <bitrate> */
164     if ((s = ntrip_field_iterate(NULL, s, eol, errout))) {
165         hold->bitrate = atoi(s);
166     }
167     /* ...<misc> */
168     while ((s = ntrip_field_iterate(NULL, s, eol, errout)));
169 }
170 
ntrip_sourcetable_parse(struct gps_device_t * device)171 static int ntrip_sourcetable_parse(struct gps_device_t *device)
172 {
173     struct ntrip_stream_t hold;
174     ssize_t llen, len = 0;
175     char *line;
176     bool sourcetable = false;
177     bool match = false;
178     char buf[BUFSIZ];
179     size_t blen = sizeof(buf);
180     int fd = device->gpsdata.gps_fd;
181 
182     for (;;) {
183         char *eol;
184         ssize_t rlen;
185 
186         memset(&buf[len], 0, (size_t) (blen - len));
187         rlen = read(fd, &buf[len], (size_t) (blen - 1 - len));
188         if (rlen == -1) {
189             if (errno == EINTR) {
190                 continue;
191             }
192             if (sourcetable && !match && errno == EAGAIN) {
193                 /* we have not yet found a match, but there currently
194                  * is no more data */
195                 return 0;
196             }
197             if (match) {
198                 return 1;
199             }
200             GPSD_LOG(LOG_ERROR, &device->context->errout,
201                      "ntrip stream read error %d on fd %d\n",
202                      errno, fd);
203             return -1;
204         } else if (rlen == 0) { // server closed the connection
205             GPSD_LOG(LOG_ERROR, &device->context->errout,
206                      "ntrip stream unexpected close %d on fd %d "
207                      "during sourcetable read\n",
208                      errno, fd);
209             return -1;
210         }
211 
212         line = buf;
213         rlen = len += rlen;
214 
215         GPSD_LOG(LOG_RAW, &device->context->errout,
216                  "Ntrip source table buffer %s\n", buf);
217 
218         sourcetable = device->ntrip.sourcetable_parse;
219         if (!sourcetable) {
220             /* parse SOURCETABLE */
221             if (str_starts_with(line, NTRIP_SOURCETABLE)) {
222                 sourcetable = true;
223                 device->ntrip.sourcetable_parse = true;
224                 llen = (ssize_t) strlen(NTRIP_SOURCETABLE);
225                 line += llen;
226                 len -= llen;
227             } else {
228                 GPSD_LOG(LOG_WARN, &device->context->errout,
229                          "Received unexpexted Ntrip reply %s.\n",
230                          buf);
231                 return -1;
232             }
233         }
234 
235         while (len > 0) {
236             /* parse ENDSOURCETABLE */
237             if (str_starts_with(line, NTRIP_ENDSOURCETABLE))
238                 goto done;
239 
240             /* coverity[string_null] - nul-terminated by previous memset */
241             if (!(eol = strstr(line, NTRIP_BR)))
242                 break;
243 
244             GPSD_LOG(LOG_DATA, &device->context->errout,
245                      "next Ntrip source table line %s\n", line);
246 
247             *eol = '\0';
248             llen = (ssize_t) (eol - line);
249 
250             /* TODO: parse headers */
251 
252             /* parse STR */
253             if (str_starts_with(line, NTRIP_STR)) {
254                 ntrip_str_parse(line + strlen(NTRIP_STR),
255                                 (size_t) (llen - strlen(NTRIP_STR)),
256                                 &hold, &device->context->errout);
257                 if (strcmp(device->ntrip.stream.mountpoint,
258                            hold.mountpoint) == 0) {
259                     /* TODO: support for RTCM 3.0, SBAS (WAAS, EGNOS), ... */
260                     if (hold.format == fmt_unknown) {
261                         GPSD_LOG(LOG_ERROR, &device->context->errout,
262                                  "Ntrip stream %s format not supported\n",
263                                  line);
264                         return -1;
265                     }
266                     /* TODO: support encryption and compression algorithms */
267                     if (hold.compr_encryp != cmp_enc_none) {
268                         GPSD_LOG(LOG_ERROR, &device->context->errout,
269                                  "Ntrip stream %s compression/encryption "
270                                  "algorithm not supported\n",
271                                  line);
272                         return -1;
273                     }
274                     /* TODO: support digest authentication */
275                     if (hold.authentication != auth_none
276                             && hold.authentication != auth_basic) {
277                         GPSD_LOG(LOG_ERROR, &device->context->errout,
278                                  "Ntrip stream %s authentication method "
279                                  "not supported\n",
280                                 line);
281                         return -1;
282                     }
283                     /* no memcpy, so we can keep the other infos */
284                     device->ntrip.stream.format = hold.format;
285                     device->ntrip.stream.carrier = hold.carrier;
286                     device->ntrip.stream.latitude = hold.latitude;
287                     device->ntrip.stream.longitude = hold.longitude;
288                     device->ntrip.stream.nmea = hold.nmea;
289                     device->ntrip.stream.compr_encryp = hold.compr_encryp;
290                     device->ntrip.stream.authentication = hold.authentication;
291                     device->ntrip.stream.fee = hold.fee;
292                     device->ntrip.stream.bitrate = hold.bitrate;
293                     device->ntrip.stream.set = true;
294                     match = true;
295                 }
296                 /* TODO: compare stream location to own location to
297                  * find nearest stream if user hasn't provided one */
298             }
299             /* TODO: parse CAS */
300             /* else if (str_starts_with(line, NTRIP_CAS)); */
301 
302             /* TODO: parse NET */
303             /* else if (str_starts_with(line, NTRIP_NET)); */
304 
305             llen += strlen(NTRIP_BR);
306             line += llen;
307             len -= llen;
308             GPSD_LOG(LOG_RAW, &device->context->errout,
309                      "Remaining Ntrip source table buffer %zd %s\n", len,
310                      line);
311         }
312         /* message too big to fit into buffer */
313         if ((size_t)len == blen - 1)
314             return -1;
315 
316         if (len > 0)
317             memmove(buf, &buf[rlen - len], (size_t) len);
318     }
319 
320 done:
321     return match ? 1 : -1;
322 }
323 
ntrip_stream_req_probe(const struct ntrip_stream_t * stream,struct gpsd_errout_t * errout)324 static int ntrip_stream_req_probe(const struct ntrip_stream_t *stream,
325                                   struct gpsd_errout_t *errout)
326 {
327     int dsock;
328     ssize_t r;
329     char buf[BUFSIZ];
330 
331     dsock = netlib_connectsock(AF_UNSPEC, stream->url, stream->port, "tcp");
332     if (dsock < 0) {
333         GPSD_LOG(LOG_ERROR, errout,
334                  "ntrip stream connect error %d in req probe\n", dsock);
335         return -1;
336     }
337     GPSD_LOG(LOG_SPIN, errout,
338              "ntrip stream for req probe connected on fd %d\n", dsock);
339     (void)snprintf(buf, sizeof(buf),
340             "GET / HTTP/1.1\r\n"
341             "User-Agent: NTRIP gpsd/%s\r\n"
342             "Host: %s\r\n"
343             "Connection: close\r\n"
344             "\r\n", VERSION, stream->url);
345     r = write(dsock, buf, strlen(buf));
346     if (r != (ssize_t)strlen(buf)) {
347         GPSD_LOG(LOG_ERROR, errout,
348                  "ntrip stream write error %d on fd %d "
349                  "during probe request %zd\n",
350                  errno, dsock, r);
351         (void)close(dsock);
352         return -1;
353     }
354     /* coverity[leaked_handle] This is an intentional allocation */
355     return dsock;
356 }
357 
ntrip_auth_encode(const struct ntrip_stream_t * stream,const char * auth,char buf[],size_t size)358 static int ntrip_auth_encode(const struct ntrip_stream_t *stream,
359                              const char *auth,
360                              char buf[],
361                              size_t size)
362 {
363     memset(buf, 0, size);
364     if (stream->authentication == auth_none)
365         return 0;
366     else if (stream->authentication == auth_basic) {
367         char authenc[64];
368         if (!auth)
369             return -1;
370         memset(authenc, 0, sizeof(authenc));
371         if (b64_ntop
372                 ((unsigned char *)auth, strlen(auth), authenc,
373                  sizeof(authenc) - 1) < 0)
374             return -1;
375         (void)snprintf(buf, size - 1, "Authorization: Basic %s\r\n", authenc);
376     } else {
377         /* TODO: support digest authentication */
378     }
379     return 0;
380 }
381 
382 /* *INDENT-ON* */
383 
ntrip_stream_get_req(const struct ntrip_stream_t * stream,const struct gpsd_errout_t * errout)384 static int ntrip_stream_get_req(const struct ntrip_stream_t *stream,
385                                 const struct gpsd_errout_t *errout)
386 {
387     int dsock;
388     char buf[BUFSIZ];
389 
390     dsock = netlib_connectsock(AF_UNSPEC, stream->url, stream->port, "tcp");
391     if (BAD_SOCKET(dsock)) {
392         GPSD_LOG(LOG_ERROR, errout,
393                  "ntrip stream connect error %d\n", dsock);
394         return -1;
395     }
396 
397     GPSD_LOG(LOG_SPIN, errout,
398              "netlib_connectsock() returns socket on fd %d\n",
399              dsock);
400 
401     (void)snprintf(buf, sizeof(buf),
402             "GET /%s HTTP/1.1\r\n"
403             "User-Agent: NTRIP gpsd/%s\r\n"
404             "Host: %s\r\n"
405             "Accept: rtk/rtcm, dgps/rtcm\r\n"
406             "%s"
407             "Connection: close\r\n"
408             "\r\n", stream->mountpoint, VERSION, stream->url, stream->authStr);
409     if (write(dsock, buf, strlen(buf)) != (ssize_t) strlen(buf)) {
410         GPSD_LOG(LOG_ERROR, errout,
411                  "ntrip stream write error %d on fd %d during get request\n",
412                  errno, dsock);
413         (void)close(dsock);
414         return -1;
415     }
416     return dsock;
417 }
418 
ntrip_stream_get_parse(const struct ntrip_stream_t * stream,const int dsock,const struct gpsd_errout_t * errout)419 static int ntrip_stream_get_parse(const struct ntrip_stream_t *stream,
420                                   const int dsock,
421                                   const struct gpsd_errout_t *errout)
422 {
423     char buf[BUFSIZ];
424     int opts;
425     memset(buf, 0, sizeof(buf));
426     while (read(dsock, buf, sizeof(buf) - 1) == -1) {
427         if (errno == EINTR)
428             continue;
429         GPSD_LOG(LOG_ERROR, errout,
430                  "ntrip stream read error %d on fd %d during get rsp\n", errno,
431                  dsock);
432         goto close;
433     }
434 
435     /* parse 401 Unauthorized */
436     /* coverity[string_null] - guaranteed terminated by the memset above */
437     if (strstr(buf, NTRIP_UNAUTH)!=NULL) {
438         GPSD_LOG(LOG_ERROR, errout,
439                  "not authorized for Ntrip stream %s/%s\n", stream->url,
440                  stream->mountpoint);
441         goto close;
442     }
443     /* parse SOURCETABLE */
444     if (strstr(buf, NTRIP_SOURCETABLE)!=NULL) {
445         GPSD_LOG(LOG_ERROR, errout,
446                  "Broadcaster doesn't recognize Ntrip stream %s:%s/%s\n",
447                  stream->url, stream->port, stream->mountpoint);
448         goto close;
449     }
450     /* parse ICY 200 OK */
451     if (strstr(buf, NTRIP_ICY)==NULL) {
452         GPSD_LOG(LOG_ERROR, errout,
453                  "Unknown reply %s from Ntrip service %s:%s/%s\n", buf,
454                  stream->url, stream->port, stream->mountpoint);
455         goto close;
456     }
457     opts = fcntl(dsock, F_GETFL);
458 
459     if (opts >= 0)
460         (void)fcntl(dsock, F_SETFL, opts | O_NONBLOCK);
461 
462     return dsock;
463 close:
464     (void)close(dsock);
465     return -1;
466 }
467 
ntrip_open(struct gps_device_t * device,char * caster)468 int ntrip_open(struct gps_device_t *device, char *caster)
469     /* open a connection to a Ntrip broadcaster */
470 {
471     char *amp, *colon, *slash;
472     char *auth = NULL;
473     char *port = NULL;
474     char *stream = NULL;
475     char *url = NULL;
476     int ret = -1;
477 
478     switch (device->ntrip.conn_state) {
479         case ntrip_conn_init:
480             /* this has to be done here,
481              * because it is needed for multi-stage connection */
482             device->servicetype = service_ntrip;
483             device->ntrip.works = false;
484             device->ntrip.sourcetable_parse = false;
485             device->ntrip.stream.set = false;
486 
487             if ((amp = strchr(caster, '@')) != NULL) {
488                 if (((colon = strchr(caster, ':')) != NULL) && colon < amp) {
489                     auth = caster;
490                     *amp = '\0';
491                     caster = amp + 1;
492                     url = caster;
493                 } else {
494                     GPSD_LOG(LOG_ERROR, &device->context->errout,
495                              "can't extract user-ID and password from %s\n",
496                              caster);
497                     device->ntrip.conn_state = ntrip_conn_err;
498                     return -1;
499                 }
500             }
501             if ((slash = strchr(caster, '/')) != NULL) {
502                 *slash = '\0';
503                 stream = slash + 1;
504             } else {
505                 /* TODO: add autoconnect like in dgpsip.c */
506                 GPSD_LOG(LOG_ERROR, &device->context->errout,
507                          "can't extract Ntrip stream from %s\n",
508                          caster);
509                 device->ntrip.conn_state = ntrip_conn_err;
510                 return -1;
511             }
512             if ((colon = strchr(caster, ':')) != NULL) {
513                 port = colon + 1;
514                 *colon = '\0';
515             }
516             if (!port) {
517                 port = "rtcm-sc104";
518                 if (!getservbyname(port, "tcp"))
519                     port = DEFAULT_RTCM_PORT;
520             }
521 
522             (void)strlcpy(device->ntrip.stream.mountpoint,
523                     stream,
524                     sizeof(device->ntrip.stream.mountpoint));
525             if (auth != NULL)
526                 (void)strlcpy(device->ntrip.stream.credentials,
527                               auth,
528                               sizeof(device->ntrip.stream.credentials));
529             /*
530              * Semantically url and port ought to be non-NULL by now,
531              * but just in case...this code appeases Coverity.
532              */
533             if (url != NULL)
534                 (void)strlcpy(device->ntrip.stream.url,
535                               url,
536                               sizeof(device->ntrip.stream.url));
537             if (port != NULL)
538                 (void)strlcpy(device->ntrip.stream.port,
539                               port,
540                               sizeof(device->ntrip.stream.port));
541 
542             ret = ntrip_stream_req_probe(&device->ntrip.stream,
543                                          &device->context->errout);
544             if (ret == -1) {
545                 device->ntrip.conn_state = ntrip_conn_err;
546                 return -1;
547             }
548             device->gpsdata.gps_fd = ret;
549             device->ntrip.conn_state = ntrip_conn_sent_probe;
550             return ret;
551         case ntrip_conn_sent_probe:
552             ret = ntrip_sourcetable_parse(device);
553             if (ret == -1) {
554                 device->ntrip.conn_state = ntrip_conn_err;
555                 return -1;
556             }
557             if (ret == 0 && device->ntrip.stream.set == false) {
558                 return ret;
559             }
560             (void)close(device->gpsdata.gps_fd);
561             if (ntrip_auth_encode(&device->ntrip.stream,
562                                   device->ntrip.stream.credentials,
563                                   device->ntrip.stream.authStr,
564                                   sizeof(device->ntrip.stream.authStr)) != 0) {
565                 device->ntrip.conn_state = ntrip_conn_err;
566                 return -1;
567             }
568             ret = ntrip_stream_get_req(&device->ntrip.stream,
569                                        &device->context->errout);
570             if (ret == -1) {
571                 device->ntrip.conn_state = ntrip_conn_err;
572                 return -1;
573             }
574             device->gpsdata.gps_fd = ret;
575             device->ntrip.conn_state = ntrip_conn_sent_get;
576             break;
577         case ntrip_conn_sent_get:
578             ret = ntrip_stream_get_parse(&device->ntrip.stream,
579                                          device->gpsdata.gps_fd,
580                                          &device->context->errout);
581             if (ret == -1) {
582                 device->ntrip.conn_state = ntrip_conn_err;
583                 return -1;
584             }
585             device->ntrip.conn_state = ntrip_conn_established;
586             device->ntrip.works = true; // we know, this worked.
587             break;
588         case ntrip_conn_established:
589         case ntrip_conn_err:
590             return -1;
591     }
592     return ret;
593 }
594 
ntrip_report(struct gps_context_t * context,struct gps_device_t * gps,struct gps_device_t * caster)595 void ntrip_report(struct gps_context_t *context,
596                   struct gps_device_t *gps,
597                   struct gps_device_t *caster)
598     /* may be time to ship a usage report to the Ntrip caster */
599 {
600     static int count;
601     /*
602      * 10 is an arbitrary number, the point is to have gotten several good
603      * fixes before reporting usage to our Ntrip caster.
604      *
605      * count % 5 is as arbitrary a number as the fixcnt. But some delay
606      * was needed here
607      */
608     count ++;
609     if (caster->ntrip.stream.nmea != 0 &&
610         context->fixcnt > 10 && (count % 5) == 0) {
611         if (caster->gpsdata.gps_fd > -1) {
612             char buf[BUFSIZ];
613             gpsd_position_fix_dump(gps, buf, sizeof(buf));
614             if (write(caster->gpsdata.gps_fd, buf, strlen(buf)) ==
615                     (ssize_t) strlen(buf)) {
616                 GPSD_LOG(LOG_IO, &context->errout, "=> dgps %s\n", buf);
617             } else {
618                 GPSD_LOG(LOG_IO, &context->errout,
619                          "ntrip report write failed\n");
620             }
621         }
622     }
623 }
624 
625 // vim: set expandtab shiftwidth=4
626