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