1 /*
2 * gps2udp
3 *
4 * Dump NMEA to UDP socket for AIShub
5 * gps2udp -u data.aishub.net:1234
6 *
7 * Author: Fulup Ar Foll (directly inspired from gpspipe.c)
8 * Date: 2013-03-01
9 *
10 * This file is Copyright (c) 2013-2018 by the GPSD project
11 * SPDX-License-Identifier: BSD-2-clause
12 *
13 */
14
15 #include "gpsd_config.h" /* must be before all includes */
16
17 #include <arpa/inet.h>
18 #include <assert.h>
19 #include <errno.h>
20 #include <fcntl.h>
21 #include <netdb.h> /* for gethostbyname() */
22 #include <netinet/in.h>
23 #include <stdbool.h>
24 #include <stdio.h>
25 #include <stdlib.h>
26 #include <string.h> /* for strlcpy(), strsep(), etc. */
27 #include <strings.h>
28 #include <sys/select.h>
29 #include <sys/socket.h>
30 #include <sys/stat.h>
31 #include <sys/time.h>
32 #include <sys/types.h>
33 #include <termios.h>
34 #include <time.h>
35 #include <unistd.h>
36
37 #include "gpsd.h"
38 #include "gpsdclient.h"
39 #include "revision.h"
40 #include "strfuncs.h"
41 #include "timespec.h"
42
43 #define MAX_TIME_LEN 80
44 #define MAX_GPSD_RETRY 10
45
46 static struct gps_data_t gpsdata;
47
48 /* UDP socket variables */
49 #define MAX_UDP_DEST 5
50 static struct sockaddr_in remote[MAX_UDP_DEST];
51 static int sock[MAX_UDP_DEST];
52 static int udpchannel;
53
54 /* gpsclient source */
55 static struct fixsource_t gpsd_source;
56 static unsigned int flags;
57 static int debug = 0;
58 static bool aisonly = false;
59
time2string(void)60 static char* time2string(void)
61 /* return local time hh:mm:ss */
62 {
63 static char buffer[MAX_TIME_LEN];
64 time_t curtime;
65 struct tm *loctime;
66
67 /* Get the current time. */
68 curtime = time (NULL);
69
70 /* Convert it to local time representation. */
71 loctime = localtime (&curtime);
72
73 /* Print it out in a nice format. */
74 (void)strftime (buffer, sizeof(buffer), "%H:%M:%S", loctime);
75
76 return (buffer);
77 }
78
send_udp(char * nmeastring,size_t ind)79 static int send_udp (char *nmeastring, size_t ind)
80 {
81 char message [255];
82 char *buffer;
83 int channel;
84
85 /* if string length is unknow make a copy and compute it */
86 if (ind == 0) {
87 /* compute message size and add 0x0a 0x0d */
88 for (ind=0; nmeastring [ind] != '\0'; ind ++) {
89 if (ind >= sizeof(message) - 3) {
90 (void)fprintf(stderr, "gps2udp: too big [%s] \n", nmeastring);
91 return -1;
92 }
93 message[ind] = nmeastring[ind];
94 }
95 buffer = message;
96 } else {
97 /* use directly nmeastring but change terminition */
98 buffer = nmeastring;
99 ind = ind-1;
100 }
101 /* Add termination to NMEA feed for AISHUB */
102 buffer[ind] = '\r'; ind++;
103 buffer[ind] = '\n'; ind++;
104 buffer[ind] = '\0';
105
106 if ((flags & WATCH_JSON)==0 && buffer[0] == '{') {
107 /* do not send JSON when not configured to do so */
108 return 0;
109 }
110
111 /* send message on udp channel */
112 for (channel=0; channel < udpchannel; channel ++) {
113 ssize_t status = sendto(sock[channel],
114 buffer,
115 ind,
116 0,
117 (const struct sockaddr *)&remote[channel],
118 (int)sizeof(remote));
119 if (status < (ssize_t)ind) {
120 (void)fprintf(stderr, "gps2udp: failed to send [%s] \n", nmeastring);
121 return -1;
122 }
123 }
124 return 0;
125 }
126
127
open_udp(char ** hostport)128 static int open_udp(char **hostport)
129 /* Open and bind udp socket to host */
130 {
131 int channel;
132
133 for (channel=0; channel <udpchannel; channel ++)
134 {
135 char *hostname = NULL;
136 char *portname = NULL;
137 char *endptr = NULL;
138 int portnum;
139 struct hostent *hp;
140
141 /* parse argument */
142 hostname = strsep(&hostport[channel], ":");
143 portname = strsep(&hostport[channel], ":");
144 if ((hostname == NULL) || (portname == NULL)) {
145 (void)fprintf(stderr, "gps2udp: syntax is [-u hostname:port]\n");
146 return (-1);
147 }
148
149 errno = 0;
150 portnum = (int)strtol(portname, &endptr, 10);
151 if (1 > portnum || 65535 < portnum || '\0' != *endptr || 0 != errno) {
152 (void)fprintf(stderr, "gps2udp: syntax is [-u hostname:port] [%s] is not a valid port number\n",portname);
153 return (-1);
154 }
155
156 sock[channel]= socket(AF_INET, SOCK_DGRAM, 0);
157 if (sock[channel] < 0) {
158 (void)fprintf(stderr, "gps2udp: error creating UDP socket\n");
159 return (-1);
160 }
161
162 remote[channel].sin_family = (sa_family_t)AF_INET;
163 hp = gethostbyname(hostname);
164 if (hp==NULL) {
165 (void)fprintf(stderr,
166 "gps2udp: syntax is [-u hostname:port] [%s]"
167 " is not a valid hostname\n",
168 hostname);
169 return (-1);
170 }
171
172 memcpy( &remote[channel].sin_addr, hp->h_addr_list[0], hp->h_length);
173 remote[channel].sin_port = htons((in_port_t)portnum);
174 }
175 return (0);
176 }
177
usage(void)178 static void usage(void)
179 {
180 (void)fprintf(stderr,
181 "Usage: gps2udp [OPTIONS] [server[:port[:device]]]\n\n"
182 "-h Show this help.\n"
183 "-u Send UDP NMEA/JSON feed to host:port [multiple -u host:port accepted]\n"
184 "-n Feed NMEA.\n"
185 "-j Feed JSON.\n"
186 "-a Select AIS message only.\n"
187 "-c [count] exit after count packets.\n"
188 "-b Run in background as a daemon.\n"
189 "-d [0-2] 1 display sent packets, 2 display ignored packets.\n"
190 "-v Print version and exit.\n\n"
191 "example: gps2udp -a -n -c 2 -d 1 -u data.aishub.net:2222 fridu.net\n"
192 );
193 }
194
connect2gpsd(bool restart)195 static void connect2gpsd(bool restart)
196 /* loop until we connect with gpsd */
197 {
198 unsigned int delay;
199
200 if (restart) {
201 (void)gps_close(&gpsdata);
202 if (debug > 0)
203 (void)fprintf(stdout,
204 "gps2udp [%s] reset gpsd connection\n",
205 time2string());
206 }
207
208 /* loop until we reach GPSd */
209 for (delay = 10; ; delay = delay*2) {
210 int status = gps_open(gpsd_source.server, gpsd_source.port, &gpsdata);
211 if (status != 0) {
212 (void)fprintf(stderr,
213 "gps2udp [%s] connection failed at %s:%s\n",
214 time2string(), gpsd_source.server, gpsd_source.port);
215 (void)sleep(delay);
216 } else {
217 if (debug > 0)
218 (void)fprintf(stdout, "gps2udp [%s] connect to gpsd %s:%s\n",
219 time2string(), gpsd_source.server, gpsd_source.port);
220 break;
221 }
222 }
223 /* select the right set of gps data */
224 (void)gps_stream(&gpsdata, flags, gpsd_source.device);
225
226 }
227
read_gpsd(char * message,size_t len)228 static ssize_t read_gpsd(char *message, size_t len)
229 /* get data from gpsd */
230 {
231 int ind;
232 char c;
233 int retry=0;
234
235 /* allow room for trailing NUL */
236 len--;
237
238 /* loop until we get some data or an error */
239 for (ind = 0; ind < (int)len;) {
240 /* prepare for a blocking read with a 10s timeout */
241 int result = nanowait(gpsdata.gps_fd, 10 % NS_IN_SEC);
242
243 switch (result)
244 {
245 case 1: /* we have data waiting, let's process them */
246 result = (int)read(gpsdata.gps_fd, &c, 1);
247
248 /* If we lost gpsd connection reset it */
249 if (result != 1) {
250 connect2gpsd (true);
251 }
252
253 if ((c == '\n') || (c == '\r')){
254 message[ind]='\0';
255
256 if (ind > 0) {
257 if (retry > 0) {
258 if (debug ==1)
259 (void)fprintf (stdout,"\r");
260 if (debug > 1)
261 (void)fprintf(stdout,
262 " [%s] No Data for: %ds\n",
263 time2string(), retry*10);
264 }
265
266 if (aisonly && message[0] != '!') {
267 if (debug >1)
268 (void)fprintf(stdout,
269 ".... [%s %d] %s\n", time2string(),
270 ind, message);
271 return(0);
272 }
273 }
274
275 return ((ssize_t)ind+1);
276 } else {
277 message[ind]= c;
278 ind++;
279 }
280 break;
281
282 case 0: /* no data fail in timeout */
283 retry++;
284 /* if too many empty packets are received reset gpsd connection */
285 if (retry > MAX_GPSD_RETRY)
286 {
287 connect2gpsd(true);
288 retry = 0;
289 }
290 if (debug > 0)
291 ignore_return(write (1, ".", 1));
292 break;
293
294 default: /* we lost connection with gpsd */
295 connect2gpsd(true);
296 break;
297 }
298 }
299 message[ind] = '\0';
300 (void)fprintf (stderr,"\n gps2udp: message too big [%s]\n", message);
301 return(-1);
302 }
303
AISto6bit(unsigned char c)304 static unsigned char AISto6bit(unsigned char c)
305 /* 6 bits decoding of AIS payload */
306 {
307 unsigned char cp = c;
308
309 if(c < (unsigned char)0x30)
310 return (unsigned char)-1;
311 if(c > (unsigned char)0x77)
312 return (unsigned char)-1;
313 if(((unsigned char)0x57 < c) && (c < (unsigned char)0x60))
314 return (unsigned char)-1;
315
316 cp += (unsigned char)0x28;
317
318 if(cp > (unsigned char)0x80)
319 cp += (unsigned char)0x20;
320 else
321 cp += (unsigned char)0x28;
322 return (unsigned char)(cp & (unsigned char)0x3f);
323 }
324
AISGetInt(unsigned char * bitbytes,unsigned int sp,unsigned int len)325 static unsigned int AISGetInt(unsigned char *bitbytes, unsigned int sp, unsigned int len)
326 /* get MMSI from AIS bit string */
327 {
328 unsigned int acc = 0;
329 unsigned int s0p = sp-1; // to zero base
330 unsigned int i;
331
332 for(i=0 ; i<len ; i++)
333 {
334 unsigned int cp, cx, c0;
335 acc = acc << 1;
336 cp = (s0p + i) / 6;
337 cx = (unsigned int)bitbytes[cp]; // what if cp >= byte_length?
338 c0 = (cx >> (5 - ((s0p + i) % 6))) & 1;
339 acc |= c0;
340 }
341
342 return acc;
343 }
344
main(int argc,char ** argv)345 int main(int argc, char **argv)
346 {
347 bool daemonize = false;
348 long count = -1;
349 int option;
350 char *udphostport[MAX_UDP_DEST];
351
352 flags = WATCH_ENABLE;
353 while ((option = getopt(argc, argv, "?habnjvc:l:u:d:")) != -1)
354 {
355 switch (option) {
356 case 'd':
357 debug = atoi(optarg);
358 if ((debug <1) || (debug > 2)) {
359 usage();
360 exit(1);
361 }
362 break;
363 case 'n':
364 if (debug >0)
365 (void)fprintf(stdout, "NMEA selected\n");
366 flags |= WATCH_NMEA;
367 break;
368 case 'j':
369 if (debug >0)
370 (void)fprintf(stdout, "JSON selected\n");
371 flags |= WATCH_JSON;
372 break;
373 case 'a':
374 aisonly = true;
375 break;
376 case 'c':
377 count = atol(optarg);
378 break;
379 case 'b':
380 daemonize = true;
381 break;
382 case 'u':
383 if (udpchannel >= MAX_UDP_DEST) {
384 (void)fprintf(stderr,
385 "gps2udp: too many UDP destinations (max=%d)\n",
386 MAX_UDP_DEST);
387 } else {
388 udphostport[udpchannel++] = optarg;
389 }
390 break;
391 case 'v':
392 (void)fprintf(stderr, "%s: %s (revision %s)\n",
393 argv[0], VERSION, REVISION);
394 exit(0);
395 case '?':
396 case 'h':
397 default:
398 usage();
399 exit(1);
400 }
401 }
402
403 /* Grok the server, port, and device. */
404 if (optind < argc)
405 gpsd_source_spec(argv[optind], &gpsd_source);
406 else
407 gpsd_source_spec(NULL, &gpsd_source);
408 if (gpsd_source.device != NULL)
409 flags |= WATCH_DEVICE;
410
411 /* check before going background if we can connect to gpsd */
412 connect2gpsd(false);
413
414 /* Open UDP port */
415 if (udpchannel > 0) {
416 int status = open_udp(udphostport);
417 if (status !=0) exit (1);
418 }
419
420 /* Daemonize if the user requested it. */
421 if (daemonize) {
422 if (os_daemon(0, 0) != 0) {
423 (void)fprintf(stderr,
424 "gps2udp: daemonization failed: %s\n",
425 strerror(errno));
426 }
427 }
428
429 /* infinite loop to get data from gpsd and push them to aggregators */
430 for (;;)
431 {
432 char buffer[512];
433 ssize_t len;
434
435 len = read_gpsd(buffer, sizeof(buffer));
436
437 /* ignore empty message */
438 if (len > 3)
439 {
440 if (debug > 0)
441 {
442 (void)fprintf (stdout,"---> [%s] -- %s",time2string(),buffer);
443
444 // Try to extract MMSI from AIS payload
445 if (str_starts_with(buffer, "!AIVDM"))
446 {
447 #define MAX_INFO 6
448 int i,j;
449 unsigned char packet[512];
450 unsigned char *adrpkt = packet;
451 unsigned char *info[MAX_INFO];
452 unsigned int mmsi;
453 unsigned char bitstrings [255];
454
455 // strtok break original string
456 (void)strlcpy((char *)packet, buffer, sizeof(packet));
457 for (j=0; j<MAX_INFO; j++) {
458 info[j] = (unsigned char *)strsep((char **)&adrpkt, ",");
459 }
460
461 for(i=0 ; i < (int)strlen((char *)info[5]); i++) {
462 if (i > (int) sizeof (bitstrings)) break;
463 bitstrings[i] = AISto6bit(info[5][i]);
464 }
465
466 mmsi = AISGetInt(bitstrings, 9, 30);
467 (void)fprintf(stdout," MMSI=%9u", mmsi);
468
469 }
470 (void)fprintf(stdout,"\n");
471 }
472
473 // send to all UDP destinations
474 if (udpchannel > 0)
475 (void)send_udp(buffer, (size_t)len);
476
477 // if we count messages check it now
478 if (count >= 0) {
479 if (count-- == 0) {
480 /* completed count */
481 (void)fprintf(stderr,
482 "gpsd2udp: normal exit after counted packets\n");
483 exit (0);
484 }
485 } // end count
486 } // end len > 3
487 } // end for (;;)
488
489 // This is an infinite loop, should never be here
490 (void)fprintf (stderr, "gpsd2udp ERROR abnormal exit\n");
491 exit (-1);
492 }
493
494 /* end */
495