1 /*
2 * Handle the Rockwell binary packet format supported by the old Zodiac chipset
3 *
4 * Week counters are not limited to 10 bits. It's unknown what
5 * the firmware is doing to disambiguate them, if anything; it might just
6 * be adding a fixed offset based on a hidden epoch value, in which case
7 * unhappy things will occur on the next rollover.
8 *
9 * This file is Copyright (c) 2010-2018 by the GPSD project
10 * SPDX-License-Identifier: BSD-2-clause
11 */
12
13 #include "gpsd_config.h" /* must be before all includes */
14
15 #include <stdio.h>
16 #include <stdbool.h>
17 #include <string.h>
18 #include <math.h>
19 #include <unistd.h>
20
21 #include "gpsd.h"
22 #include "bits.h"
23 #include "strfuncs.h"
24
25 /* Zodiac protocol description uses 1-origin indexing by little-endian word */
26 #define get16z(buf, n) ( (buf[2*(n)-2]) \
27 | (buf[2*(n)-1] << 8))
28 #define get32z(buf, n) ( (buf[2*(n)-2]) \
29 | (buf[2*(n)-1] << 8) \
30 | (buf[2*(n)+0] << 16) \
31 | (buf[2*(n)+1] << 24))
32 #define getstringz(to, from, s, e) \
33 (void)memcpy(to, from+2*(s)-2, 2*((e)-(s)+1))
34
35 #ifdef ZODIAC_ENABLE
36 struct header
37 {
38 unsigned short sync;
39 unsigned short id;
40 unsigned short ndata;
41 unsigned short flags;
42 unsigned short csum;
43 };
44
zodiac_checksum(unsigned short * w,int n)45 static unsigned short zodiac_checksum(unsigned short *w, int n)
46 {
47 unsigned short csum = 0;
48
49 while (n-- > 0)
50 csum += *(w++);
51 return -csum;
52 }
53
end_write(int fd,void * d,size_t len)54 static ssize_t end_write(int fd, void *d, size_t len)
55 /* write an array of shorts in little-endian format */
56 {
57 unsigned char buf[BUFSIZ];
58 short *data = (short *)d;
59
60 size_t n;
61 for (n = 0; n < (size_t)(len/2); n++)
62 putle16(buf, n*2, data[n]);
63 return write(fd, (char*)buf, len);
64 }
65
66 /* zodiac_spew - Takes a message type, an array of data words, and a length
67 * for the array, and prepends a 5 word header (including checksum).
68 * The data words are expected to be checksummed.
69 */
zodiac_spew(struct gps_device_t * session,unsigned short type,unsigned short * dat,int dlen)70 static ssize_t zodiac_spew(struct gps_device_t *session, unsigned short type,
71 unsigned short *dat, int dlen)
72 {
73 struct header h;
74 int i;
75 char buf[BUFSIZ];
76
77 h.sync = 0x81ff;
78 h.id = (unsigned short)type;
79 h.ndata = (unsigned short)(dlen - 1);
80 h.flags = 0;
81 h.csum = zodiac_checksum((unsigned short *)&h, 4);
82
83 if (!BAD_SOCKET(session->gpsdata.gps_fd)) {
84 size_t hlen, datlen;
85 hlen = sizeof(h);
86 datlen = sizeof(unsigned short) * dlen;
87 if (end_write(session->gpsdata.gps_fd, &h, hlen) != (ssize_t) hlen ||
88 end_write(session->gpsdata.gps_fd, dat,
89 datlen) != (ssize_t) datlen) {
90 GPSD_LOG(LOG_INFO, &session->context->errout,
91 "Reconfigure write failed\n");
92 return -1;
93 }
94 }
95
96 (void)snprintf(buf, sizeof(buf),
97 "%04x %04x %04x %04x %04x",
98 h.sync, h.id, h.ndata, h.flags, h.csum);
99 for (i = 0; i < dlen; i++)
100 str_appendf(buf, sizeof(buf), " %04x", dat[i]);
101
102 GPSD_LOG(LOG_RAW, &session->context->errout,
103 "Sent Zodiac packet: %s\n", buf);
104
105 return 0;
106 }
107
send_rtcm(struct gps_device_t * session,const char * rtcmbuf,size_t rtcmbytes)108 static void send_rtcm(struct gps_device_t *session,
109 const char *rtcmbuf, size_t rtcmbytes)
110 {
111 unsigned short data[34];
112 int n = 1 + (int)(rtcmbytes / 2 + rtcmbytes % 2);
113
114 if (session->driver.zodiac.sn++ > 32767)
115 session->driver.zodiac.sn = 0;
116
117 memset(data, 0, sizeof(data));
118 data[0] = session->driver.zodiac.sn; /* sequence number */
119 memcpy(&data[1], rtcmbuf, rtcmbytes);
120 data[n] = zodiac_checksum(data, n);
121
122 (void)zodiac_spew(session, 1351, data, n + 1);
123 }
124
zodiac_send_rtcm(struct gps_device_t * session,const char * rtcmbuf,size_t rtcmbytes)125 static ssize_t zodiac_send_rtcm(struct gps_device_t *session,
126 const char *rtcmbuf, size_t rtcmbytes)
127 {
128 while (rtcmbytes > 0) {
129 size_t len = (size_t) (rtcmbytes > 64 ? 64 : rtcmbytes);
130 send_rtcm(session, rtcmbuf, len);
131 rtcmbytes -= len;
132 rtcmbuf += len;
133 }
134 return 1;
135 }
136
137 #define getzword(n) get16z(session->lexer.outbuffer, n)
138 #define getzlong(n) get32z(session->lexer.outbuffer, n)
139
handle1000(struct gps_device_t * session)140 static gps_mask_t handle1000(struct gps_device_t *session)
141 /* time-position-velocity report */
142 {
143 gps_mask_t mask;
144 struct tm unpacked_date;
145 int datum;
146 char ts_buf[TIMESPEC_LEN];
147
148 /* ticks = getzlong(6); */
149 /* sequence = getzword(8); */
150 /* measurement_sequence = getzword(9); */
151 session->gpsdata.status = (getzword(10) & 0x1c) ? 0 : 1;
152 if (session->gpsdata.status != 0)
153 session->newdata.mode = (getzword(10) & 1) ? MODE_2D : MODE_3D;
154 else
155 session->newdata.mode = MODE_NO_FIX;
156
157 /* solution_type = getzword(11); */
158 session->gpsdata.satellites_used = (int)getzword(12);
159 /* polar_navigation = getzword(13); */
160 session->context->gps_week = (unsigned short)getzword(14);
161 /* gps_seconds = getzlong(15); */
162 /* gps_nanoseconds = getzlong(17); */
163 unpacked_date.tm_mday = (int)getzword(19);
164 unpacked_date.tm_mon = (int)getzword(20) - 1;
165 unpacked_date.tm_year = (int)getzword(21) - 1900;
166 unpacked_date.tm_hour = (int)getzword(22);
167 unpacked_date.tm_min = (int)getzword(23);
168 unpacked_date.tm_sec = (int)getzword(24);
169 unpacked_date.tm_isdst = 0;
170 session->newdata.time.tv_sec = mkgmtime(&unpacked_date);
171 session->newdata.time.tv_nsec = getzlong(25);
172 session->newdata.latitude = ((long)getzlong(27)) * RAD_2_DEG * 1e-8;
173 session->newdata.longitude = ((long)getzlong(29)) * RAD_2_DEG * 1e-8;
174 /*
175 * The Rockwell Jupiter TU30-D140 reports altitude as uncorrected height
176 * above WGS84 geoid. The Zodiac binary protocol manual does not
177 * specify whether word 31 is geodetic or WGS 84.
178 * Here we assume altitude is always wgs84.
179 */
180 session->newdata.altHAE = ((long)getzlong(31)) * 1e-2;
181 session->newdata.geoid_sep = ((short)getzword(33)) * 1e-2;
182 session->newdata.speed = (int)getzlong(34) * 1e-2;
183 session->newdata.track = (int)getzword(36) * RAD_2_DEG * 1e-3;
184 session->newdata.magnetic_var = ((short)getzword(37)) * RAD_2_DEG * 1e-4;
185 session->newdata.climb = ((short)getzword(38)) * 1e-2;
186 datum = getzword(39);
187 datum_code_string(datum, session->newdata.datum,
188 sizeof(session->newdata.datum));
189 /*
190 * The manual says these are 1-sigma. Device reports only eph, circular
191 * error. Let gpsd_model_error() do the rest
192 */
193 session->newdata.eph = (int)getzlong(40) * 1e-2 * GPSD_CONFIDENCE;
194 session->newdata.epv = (int)getzlong(42) * 1e-2 * GPSD_CONFIDENCE;
195 session->newdata.ept = (int)getzlong(44) * 1e-2 * GPSD_CONFIDENCE;
196 session->newdata.eps = (int)getzword(46) * 1e-2 * GPSD_CONFIDENCE;
197 /* clock_bias = (int)getzlong(47) * 1e-2; */
198 /* clock_bias_sd = (int)getzlong(49) * 1e-2; */
199 /* clock_drift = (int)getzlong(51) * 1e-2; */
200 /* clock_drift_sd = (int)getzlong(53) * 1e-2; */
201
202 mask = TIME_SET | NTPTIME_IS | LATLON_SET | ALTITUDE_SET | CLIMB_SET |
203 SPEED_SET | TRACK_SET | STATUS_SET | MODE_SET |
204 HERR_SET | SPEEDERR_SET | VERR_SET;
205 GPSD_LOG(LOG_DATA, &session->context->errout,
206 "1000: time=%s lat=%.2f lon=%.2f altHAE=%.2f track=%.2f "
207 "speed=%.2f climb=%.2f mode=%d status=%d\n",
208 timespec_str(&session->newdata.time, ts_buf, sizeof(ts_buf)),
209 session->newdata.latitude,
210 session->newdata.longitude, session->newdata.altHAE,
211 session->newdata.track, session->newdata.speed,
212 session->newdata.climb, session->newdata.mode,
213 session->gpsdata.status);
214 return mask;
215 }
216
217 /* Message 1002: Channel Summary Message */
handle1002(struct gps_device_t * session)218 static gps_mask_t handle1002(struct gps_device_t *session)
219 {
220 int i;
221 timespec_t ts_tow;
222
223 /* ticks = getzlong(6); */
224 /* sequence = getzword(8); */
225 /* measurement_sequence = getzword(9); */
226 int gps_week = getzword(10);
227 time_t gps_seconds = getzlong(11);
228 long gps_nanoseconds = getzlong(13);
229 char ts_buf[TIMESPEC_LEN];
230
231 /* Note: this week counter is not limited to 10 bits. */
232 session->context->gps_week = (unsigned short)gps_week;
233 session->gpsdata.satellites_used = 0;
234 for (i = 0; i < ZODIAC_CHANNELS; i++) {
235 int status, prn;
236 session->driver.zodiac.Zv[i] = status = (int)getzword(15 + (3 * i));
237 session->driver.zodiac.Zs[i] = prn = (int)getzword(16 + (3 * i));
238
239 if (status & 1)
240 session->gpsdata.satellites_used++;
241
242 session->gpsdata.skyview[i].PRN = (short)prn;
243 session->gpsdata.skyview[i].ss = (float)getzword(17 + (3 * i));
244 session->gpsdata.skyview[i].used = (bool)(status & 1);
245 }
246 ts_tow.tv_sec = gps_seconds;
247 ts_tow.tv_nsec = gps_nanoseconds;
248 session->gpsdata.skyview_time = gpsd_gpstime_resolv(session,
249 (unsigned short)gps_week,
250 ts_tow);
251 GPSD_LOG(LOG_DATA, &session->context->errout,
252 "1002: visible=%d used=%d mask={SATELLITE|USED} time %s\n",
253 session->gpsdata.satellites_visible,
254 session->gpsdata.satellites_used,
255 timespec_str(&session->gpsdata.skyview_time, ts_buf,
256 sizeof(ts_buf)));
257 return SATELLITE_SET | USED_IS;
258 }
259
handle1003(struct gps_device_t * session)260 static gps_mask_t handle1003(struct gps_device_t *session)
261 /* skyview report */
262 {
263 int i, n;
264
265 /* The Polaris (and probably the DAGR) emit some strange variant of
266 * this message which causes gpsd to crash filtering on impossible
267 * number of satellites avoids this */
268 n = (int)getzword(14);
269 if ((n < 0) || (n > 12))
270 return 0;
271
272 gpsd_zero_satellites(&session->gpsdata);
273
274 /* ticks = getzlong(6); */
275 /* sequence = getzword(8); */
276 session->gpsdata.dop.gdop = (unsigned int)getzword(9) * 1e-2;
277 session->gpsdata.dop.pdop = (unsigned int)getzword(10) * 1e-2;
278 session->gpsdata.dop.hdop = (unsigned int)getzword(11) * 1e-2;
279 session->gpsdata.dop.vdop = (unsigned int)getzword(12) * 1e-2;
280 session->gpsdata.dop.tdop = (unsigned int)getzword(13) * 1e-2;
281 session->gpsdata.satellites_visible = n;
282
283 for (i = 0; i < ZODIAC_CHANNELS; i++) {
284 if (i < session->gpsdata.satellites_visible) {
285 session->gpsdata.skyview[i].PRN = (short)getzword(15 + (3 * i));
286 session->gpsdata.skyview[i].azimuth =
287 (((double)getzword(16 + (3 * i))) * RAD_2_DEG * 1e-4);
288 if (session->gpsdata.skyview[i].azimuth < 0)
289 session->gpsdata.skyview[i].azimuth += 360;
290 session->gpsdata.skyview[i].elevation =
291 (((double)getzword(17 + (3 * i))) * RAD_2_DEG * 1e-4);
292 } else {
293 session->gpsdata.skyview[i].PRN = 0;
294 session->gpsdata.skyview[i].azimuth = NAN;
295 session->gpsdata.skyview[i].elevation = NAN;
296 session->gpsdata.skyview[i].ss = NAN;
297 }
298 }
299 session->gpsdata.skyview_time.tv_sec = 0;
300 session->gpsdata.skyview_time.tv_nsec = 0;
301 GPSD_LOG(LOG_DATA, &session->context->errout,
302 "NAVDOP: visible=%d gdop=%.2f pdop=%.2f "
303 "hdop=%.2f vdop=%.2f tdop=%.2f mask={SATELLITE|DOP}\n",
304 session->gpsdata.satellites_visible,
305 session->gpsdata.dop.gdop,
306 session->gpsdata.dop.hdop,
307 session->gpsdata.dop.vdop,
308 session->gpsdata.dop.pdop, session->gpsdata.dop.tdop);
309 return SATELLITE_SET | DOP_SET;
310 }
311
handle1005(struct gps_device_t * session UNUSED)312 static void handle1005(struct gps_device_t *session UNUSED)
313 /* fix quality report */
314 {
315 /* ticks = getzlong(6); */
316 /* sequence = getzword(8); */
317 int numcorrections = (int)getzword(12);
318
319 if (session->newdata.mode == MODE_NO_FIX)
320 session->gpsdata.status = STATUS_NO_FIX;
321 else if (numcorrections == 0)
322 session->gpsdata.status = STATUS_FIX;
323 else
324 session->gpsdata.status = STATUS_DGPS_FIX;
325 }
326
handle1011(struct gps_device_t * session)327 static gps_mask_t handle1011(struct gps_device_t *session)
328 /* version report */
329 {
330 /*
331 * This is UNTESTED -- but harmless if buggy. Added to support
332 * client querying of the ID with firmware version in 2006.
333 * The Zodiac is supposed to send one of these messages on startup.
334 */
335 getstringz(session->subtype, session->lexer.outbuffer, 19, 28); /* software version field */
336 GPSD_LOG(LOG_DATA, &session->context->errout,
337 "1011: subtype=%s mask={DEVICEID}\n",
338 session->subtype);
339 return DEVICEID_SET;
340 }
341
342
handle1108(struct gps_device_t * session)343 static void handle1108(struct gps_device_t *session)
344 /* leap-second correction report */
345 {
346 /* ticks = getzlong(6); */
347 /* sequence = getzword(8); */
348 /* utc_week_seconds = getzlong(14); */
349 /* leap_nanoseconds = getzlong(17); */
350 if ((int)(getzword(19) & 3) == 3) {
351 session->context->valid |= LEAP_SECOND_VALID;
352 session->context->leap_seconds = (int)getzword(16);
353 }
354 }
355
zodiac_analyze(struct gps_device_t * session)356 static gps_mask_t zodiac_analyze(struct gps_device_t *session)
357 {
358 unsigned int id =
359 (unsigned int)((session->lexer.outbuffer[3] << 8) |
360 session->lexer.outbuffer[2]);
361 /*
362 * The guard looks superfluous, but it keeps the rather expensive
363 * gpsd_packetdump() function from being called even when the debug
364 * level does not actually require it.
365 */
366 if (session->context->errout.debug >= LOG_RAW)
367 GPSD_LOG(LOG_RAW, &session->context->errout,
368 "Raw Zodiac packet type %d length %zd: %s\n",
369 id, session->lexer.outbuflen, gpsd_prettydump(session));
370
371 if (session->lexer.outbuflen < 10)
372 return 0;
373
374 /*
375 * Normal cycle for these devices is 1001 1002.
376 * We count 1001 as end of cycle because 1002 doesn't
377 * carry fix information.
378 */
379 session->cycle_end_reliable = true;
380
381 switch (id) {
382 case 1000:
383 return handle1000(session) | (CLEAR_IS | REPORT_IS);
384 case 1002:
385 return handle1002(session);
386 case 1003:
387 return handle1003(session);
388 case 1005:
389 handle1005(session);
390 return 0;
391 case 1011:
392 return handle1011(session);
393 case 1108:
394 handle1108(session);
395 return 0;
396 default:
397 return 0;
398 }
399 }
400
401 #ifdef CONTROLSEND_ENABLE
zodiac_control_send(struct gps_device_t * session,char * msg,size_t len)402 static ssize_t zodiac_control_send(struct gps_device_t *session,
403 char *msg, size_t len)
404 {
405 unsigned short shortwords[256];
406
407 #define min(x,y) ((x) < (y) ? x : y)
408 /*
409 * We used to just cast msg to an unsigned short pointer.
410 * This can fail on word-oriented achitectures like a SPARC.
411 */
412 memcpy((char *)shortwords, msg, min(sizeof(shortwords), len));
413
414 /* and if len isn't even, it's your own fault */
415 return zodiac_spew(session, shortwords[0], shortwords + 1,
416 (int)(len / 2 - 1));
417 }
418 #endif /* CONTROLSEND_ENABLE */
419
420 #ifdef RECONFIGURE_ENABLE
zodiac_speed_switch(struct gps_device_t * session,speed_t speed,char parity,int stopbits)421 static bool zodiac_speed_switch(struct gps_device_t *session,
422 speed_t speed, char parity, int stopbits)
423 {
424 unsigned short data[15];
425
426 if (session->driver.zodiac.sn++ > 32767)
427 session->driver.zodiac.sn = 0;
428
429 switch (parity) {
430 case 'E':
431 case 2:
432 parity = (char)2;
433 break;
434 case 'O':
435 case 1:
436 parity = (char)1;
437 break;
438 case 'N':
439 case 0:
440 default:
441 parity = (char)0;
442 break;
443 }
444
445 memset(data, 0, sizeof(data));
446 /* data is the part of the message starting at word 6 */
447 data[0] = session->driver.zodiac.sn; /* sequence number */
448 data[1] = 1; /* port 1 data valid */
449 data[2] = (unsigned short)parity; /* port 1 character width (8 bits) */
450 data[3] = (unsigned short)(stopbits - 1); /* port 1 stop bits (1 stopbit) */
451 data[4] = 0; /* port 1 parity (none) */
452 data[5] = (unsigned short)(round(log((double)speed / 300) / GPS_LN2) + 1); /* port 1 speed */
453 data[14] = zodiac_checksum(data, 14);
454
455 (void)zodiac_spew(session, 1330, data, 15);
456 return true; /* it would be nice to error-check this */
457 }
458 #endif /* RECONFIGURE_ENABLE */
459
zodiac_time_offset(struct gps_device_t * session UNUSED)460 static double zodiac_time_offset(struct gps_device_t *session UNUSED)
461 {
462 /* Removing/changing the magic number below is likely to disturb
463 * the handling of the 1pps signal from the gps device. The regression
464 * tests and simple gps applications do not detect this. A live test
465 * with the 1pps signal active is required. */
466 return 1.1;
467 }
468
469 /* this is everything we export */
470 /* *INDENT-OFF* */
471 const struct gps_type_t driver_zodiac =
472 {
473 .type_name = "Zodiac", /* full name of type */
474 .packet_type = ZODIAC_PACKET, /* associated lexer packet type */
475 .flags = DRIVER_STICKY, /* no flags set */
476 .trigger = NULL, /* no trigger */
477 .channels = 12, /* consumer-grade GPS */
478 .probe_detect = NULL, /* no probe */
479 .get_packet = generic_get, /* use the generic packet getter */
480 .parse_packet = zodiac_analyze, /* parse message packets */
481 .rtcm_writer = zodiac_send_rtcm, /* send DGPS correction */
482 .init_query = NULL, /* non-perturbing initial query */
483 .event_hook = NULL, /* no configuration */
484 #ifdef RECONFIGURE_ENABLE
485 .speed_switcher = zodiac_speed_switch,/* we can change baud rate */
486 .mode_switcher = NULL, /* no mode switcher */
487 .rate_switcher = NULL, /* no sample-rate switcher */
488 .min_cycle.tv_sec = 1, /* not relevant, no rate switch */
489 .min_cycle.tv_nsec = 0, /* not relevant, no rate switch */
490 #endif /* RECONFIGURE_ENABLE */
491 #ifdef CONTROLSEND_ENABLE
492 .control_send = zodiac_control_send, /* for gpsctl and friends */
493 #endif /* CONTROLSEND_ENABLE */
494 .time_offset = zodiac_time_offset, /* compute NTO fudge factor */
495 };
496 /* *INDENT-ON* */
497
498 #endif /* ZODIAC_ENABLE */
499