You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.

626 lines
22KB

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