1 /*****************************************************************************
2
3 This is a decoder for the unnamed protocol described in IS-GPS-200,
4 the Navstar GPS Interface Specification, and used as a transport layer
5 for both GPS satellite downlink transmissions and the RTCM104 version 2
6 format for broadcasting differential-GPS corrections.
7
8 The purpose of this protocol is to support analyzing a serial bit
9 stream without byte framing into parity-checked 30-bit words.
10 Interpretation of the words is left to an upper layer. Note that
11 RTCM104 version 3 does *not* use this code; it assumes a byte-oriented
12 underlayer.
13
14 The upper layer must supply a preamble_match() hook to tell our
15 decoder when it has a legitimate start of packet, and a length_check()
16 hook to tell it when the packet has reached the length it is supposed
17 to have.
18
19 This decoder is overkill as GNSS receivers already find the preamble
20 and word boundaries of the bitstream.
21
22 Here are Wolfgang's original rather cryptic notes on this code:
23
24 --------------------------------------------------------------------------
25 1) trim and bitflip the input.
26
27 While syncing the msb of the input gets shifted into lsb of the
28 assembled word.
29 word <<= 1, or in input >> 5
30 word <<= 1, or in input >> 4
31 word <<= 1, or in input >> 3
32 word <<= 1, or in input >> 2
33 word <<= 1, or in input >> 1
34 word <<= 1, or in input
35
36 At one point it should sync-lock.
37
38 ----
39
40 Shift 6 bytes of RTCM data in as such:
41
42 ---> (trim-bits-to-5-bits) ---> (end-for-end-bit-flip) --->
43
44 ---> shift-into-30-bit-shift-register
45 |||||||||||||||||||||||
46 detector-for-preamble
47 |||||||||||||||||||||||
48 detector-for-parity
49 |||||||||||||||||||||||
50 --------------------------------------------------------------------------
51
52 The code was originally by Wolfgang Rupprecht. ESR severely hacked
53 it, with Wolfgang's help, in order to separate message analysis from
54 message dumping and separate this lower layer from the upper layer
55 handing GPS-subframe and RTCM decoding.
56
57 You are not expected to understand any of this.
58
59 This file is Copyright (c) 2010-2018 by the GPSD project
60 SPDX-License-Identifier: BSD-2-clause
61
62 *****************************************************************************/
63
64 #include "gpsd_config.h" /* must be before all includes */
65
66 #include <stdbool.h>
67 #include "gpsd.h"
68
69 #define MAG_SHIFT 6u
70 #define MAG_TAG_DATA (1 << MAG_SHIFT)
71 #define MAG_TAG_MASK (3 << MAG_SHIFT)
72
73 #define W_DATA_MASK 0x3fffffc0u
74
75 static unsigned char parity_array[] = {
76 0, 1, 1, 0, 1, 0, 0, 1, 1, 0, 0, 1, 0, 1, 1, 0,
77 1, 0, 0, 1, 0, 1, 1, 0, 0, 1, 1, 0, 1, 0, 0, 1,
78 1, 0, 0, 1, 0, 1, 1, 0, 0, 1, 1, 0, 1, 0, 0, 1,
79 0, 1, 1, 0, 1, 0, 0, 1, 1, 0, 0, 1, 0, 1, 1, 0,
80 1, 0, 0, 1, 0, 1, 1, 0, 0, 1, 1, 0, 1, 0, 0, 1,
81 0, 1, 1, 0, 1, 0, 0, 1, 1, 0, 0, 1, 0, 1, 1, 0,
82 0, 1, 1, 0, 1, 0, 0, 1, 1, 0, 0, 1, 0, 1, 1, 0,
83 1, 0, 0, 1, 0, 1, 1, 0, 0, 1, 1, 0, 1, 0, 0, 1,
84 1, 0, 0, 1, 0, 1, 1, 0, 0, 1, 1, 0, 1, 0, 0, 1,
85 0, 1, 1, 0, 1, 0, 0, 1, 1, 0, 0, 1, 0, 1, 1, 0,
86 0, 1, 1, 0, 1, 0, 0, 1, 1, 0, 0, 1, 0, 1, 1, 0,
87 1, 0, 0, 1, 0, 1, 1, 0, 0, 1, 1, 0, 1, 0, 0, 1,
88 0, 1, 1, 0, 1, 0, 0, 1, 1, 0, 0, 1, 0, 1, 1, 0,
89 1, 0, 0, 1, 0, 1, 1, 0, 0, 1, 1, 0, 1, 0, 0, 1,
90 1, 0, 0, 1, 0, 1, 1, 0, 0, 1, 1, 0, 1, 0, 0, 1,
91 0, 1, 1, 0, 1, 0, 0, 1, 1, 0, 0, 1, 0, 1, 1, 0
92 };
93
94 static unsigned int reverse_bits[] = {
95 0, 32, 16, 48, 8, 40, 24, 56, 4, 36, 20, 52, 12, 44, 28, 60,
96 2, 34, 18, 50, 10, 42, 26, 58, 6, 38, 22, 54, 14, 46, 30, 62,
97 1, 33, 17, 49, 9, 41, 25, 57, 5, 37, 21, 53, 13, 45, 29, 61,
98 3, 35, 19, 51, 11, 43, 27, 59, 7, 39, 23, 55, 15, 47, 31, 63
99 };
100
101
isgps_parity(isgps30bits_t th)102 unsigned int isgps_parity(isgps30bits_t th)
103 {
104 #define P_30_MASK 0x40000000u
105
106 #define PARITY_25 0xbb1f3480u
107 #define PARITY_26 0x5d8f9a40u
108 #define PARITY_27 0xaec7cd00u
109 #define PARITY_28 0x5763e680u
110 #define PARITY_29 0x6bb1f340u
111 #define PARITY_30 0x8b7a89c0u
112 isgps30bits_t t;
113 unsigned int p;
114
115 /*
116 * if (th & P_30_MASK)
117 * th ^= W_DATA_MASK;
118 */
119
120 t = th & PARITY_25;
121 p = parity_array[t & 0xff] ^ parity_array[(t >> 8) & 0xff] ^
122 parity_array[(t >> 16) & 0xff] ^ parity_array[(t >> 24) & 0xff];
123 t = th & PARITY_26;
124 p = (p << 1) | (parity_array[t & 0xff] ^ parity_array[(t >> 8) & 0xff] ^
125 parity_array[(t >> 16) & 0xff] ^ parity_array[(t >> 24) &
126 0xff]);
127 t = th & PARITY_27;
128 p = (p << 1) | (parity_array[t & 0xff] ^ parity_array[(t >> 8) & 0xff] ^
129 parity_array[(t >> 16) & 0xff] ^ parity_array[(t >> 24) &
130 0xff]);
131 t = th & PARITY_28;
132 p = (p << 1) | (parity_array[t & 0xff] ^ parity_array[(t >> 8) & 0xff] ^
133 parity_array[(t >> 16) & 0xff] ^ parity_array[(t >> 24) &
134 0xff]);
135 t = th & PARITY_29;
136 p = (p << 1) | (parity_array[t & 0xff] ^ parity_array[(t >> 8) & 0xff] ^
137 parity_array[(t >> 16) & 0xff] ^ parity_array[(t >> 24) &
138 0xff]);
139 t = th & PARITY_30;
140 p = (p << 1) | (parity_array[t & 0xff] ^ parity_array[(t >> 8) & 0xff] ^
141 parity_array[(t >> 16) & 0xff] ^ parity_array[(t >> 24) &
142 0xff]);
143
144 #ifdef __UNUSED__
145 GPSD_LOG(ISGPS_ERRLEVEL_BASE + 2, errout, "ISGPS parity %u\n", p);
146 #endif /* __UNUSED__ */
147 return (p);
148 }
149
150 /*
151 * ESR found a doozy of a bug...
152 *
153 * Defining isgps_parityok as a function triggers an optimizer bug in gcc
154 * 3.4.2. The symptom is that parity computation is screwed up and the decoder
155 * never achieves sync lock. Something steps on the argument to
156 * isgpsparity(); the lossage appears to be related to the compiler's
157 * attempt to fold the isgps_parity() call into isgps_parityok() in some
158 * tail-recursion-like manner. This happens under -O2, but not -O1, on
159 * both i386 and amd64. Disabling all of the individual -O2 suboptions
160 * does *not* fix it.
161 *
162 * And the fun doesn't stop there! It turns out that even with this fix, bare
163 * -O2 generates bad code. It takes "-O2 -fschedule-insns" to generate good
164 * code under 3.4.[23]...which is weird because -O2 is supposed to *imply*
165 * -fschedule-insns.
166 *
167 * gcc 4.0 does not manifest these bugs.
168 */
169 #define isgps_parityok(w) (isgps_parity(w) == ((w) & 0x3f))
170
isgps_init(struct gps_lexer_t * lexer)171 void isgps_init(struct gps_lexer_t *lexer)
172 {
173 lexer->isgps.curr_word = 0;
174 lexer->isgps.curr_offset = 24; /* first word */
175 lexer->isgps.locked = false;
176 lexer->isgps.bufindex = 0;
177 lexer->isgps.buflen = 0;
178 }
179
180 // This works around cppcheck not looking into enough config branches
181 // cppcheck-suppress unusedFunction
isgps_decode(struct gps_lexer_t * lexer,bool (* preamble_match)(isgps30bits_t *),bool (* length_check)(struct gps_lexer_t *),size_t maxlen,unsigned int c)182 enum isgpsstat_t isgps_decode(struct gps_lexer_t *lexer,
183 bool(*preamble_match) (isgps30bits_t *),
184 bool(*length_check) (struct gps_lexer_t *),
185 size_t maxlen, unsigned int c)
186 {
187 /* ASCII characters 64-127, @ through DEL */
188 if ((c & MAG_TAG_MASK) != MAG_TAG_DATA) {
189 GPSD_LOG(ISGPS_ERRLEVEL_BASE + 1, &lexer->errout,
190 "ISGPS word tag not correct, skipping byte\n");
191 return ISGPS_SKIP;
192 }
193
194 c = reverse_bits[c & 0x3f];
195
196 if (!lexer->isgps.locked) {
197 lexer->isgps.curr_offset = -5;
198 lexer->isgps.bufindex = 0;
199
200 while (lexer->isgps.curr_offset <= 0) {
201 lexer->isgps.curr_word <<= 1;
202 if (lexer->isgps.curr_offset > 0) {
203 lexer->isgps.curr_word |= c << lexer->isgps.curr_offset;
204 } else {
205 lexer->isgps.curr_word |=
206 c >> -(lexer->isgps.curr_offset);
207 }
208 GPSD_LOG(ISGPS_ERRLEVEL_BASE + 2, &lexer->errout,
209 "ISGPS syncing at byte %lu: 0x%08x\n",
210 lexer->char_counter, lexer->isgps.curr_word);
211
212 if (preamble_match(&lexer->isgps.curr_word)) {
213 if (isgps_parityok(lexer->isgps.curr_word)) {
214 GPSD_LOG(ISGPS_ERRLEVEL_BASE + 1, &lexer->errout,
215 "ISGPS preamble ok, parity ok -- locked\n");
216 lexer->isgps.locked = true;
217 break;
218 }
219 GPSD_LOG(ISGPS_ERRLEVEL_BASE + 1, &lexer->errout,
220 "ISGPS preamble ok, parity fail\n");
221 }
222 lexer->isgps.curr_offset++;
223 } /* end while */
224 }
225 if (lexer->isgps.locked) {
226 enum isgpsstat_t res;
227
228 res = ISGPS_SYNC;
229
230 if (lexer->isgps.curr_offset > 0) {
231 lexer->isgps.curr_word |= c << lexer->isgps.curr_offset;
232 } else {
233 lexer->isgps.curr_word |= c >> -(lexer->isgps.curr_offset);
234 }
235
236 if (lexer->isgps.curr_offset <= 0) {
237 /* weird-assed inversion */
238 if (lexer->isgps.curr_word & P_30_MASK)
239 lexer->isgps.curr_word ^= W_DATA_MASK;
240
241 if (isgps_parityok(lexer->isgps.curr_word)) {
242 #if 0
243 /*
244 * Don't clobber the buffer just because we spot
245 * another preamble pattern in the data stream. -wsr
246 */
247 if (preamble_match(&lexer->isgps.curr_word)) {
248 GPSD_LOG(ISGPS_ERRLEVEL_BASE + 2, &lexer->errout,
249 "ISGPS preamble spotted (index: %u)\n",
250 lexer->isgps.bufindex);
251 lexer->isgps.bufindex = 0;
252 }
253 #endif
254 GPSD_LOG(ISGPS_ERRLEVEL_BASE + 2, &lexer->errout,
255 "ISGPS processing word %u (offset %d)\n",
256 lexer->isgps.bufindex,
257 lexer->isgps.curr_offset);
258 {
259 /*
260 * Guard against a buffer overflow attack. Just wait for
261 * the next preamble match and go on from there.
262 */
263 if (lexer->isgps.bufindex >= (unsigned)maxlen) {
264 lexer->isgps.bufindex = 0;
265 GPSD_LOG(ISGPS_ERRLEVEL_BASE + 1, &lexer->errout,
266 "ISGPS buffer overflowing -- resetting\n");
267 return ISGPS_NO_SYNC;
268 }
269
270 lexer->isgps.buf[lexer->isgps.bufindex] =
271 lexer->isgps.curr_word;
272
273 /* *INDENT-OFF* */
274 if ((lexer->isgps.bufindex == 0) &&
275 !preamble_match((isgps30bits_t *) lexer->isgps.buf)) {
276 GPSD_LOG(ISGPS_ERRLEVEL_BASE + 1, &lexer->errout,
277 "ISGPS word 0 not a preamble- punting\n");
278 return ISGPS_NO_SYNC;
279 }
280 /* *INDENT-ON* */
281 lexer->isgps.bufindex++;
282
283 if (length_check(lexer)) {
284 /* jackpot, we have a complete packet */
285 lexer->isgps.buflen = lexer->isgps.bufindex * sizeof(isgps30bits_t);
286 lexer->isgps.bufindex = 0;
287 res = ISGPS_MESSAGE;
288 }
289 }
290 lexer->isgps.curr_word <<= 30; /* preserve the 2 low bits */
291 lexer->isgps.curr_offset += 30;
292 if (lexer->isgps.curr_offset > 0) {
293 lexer->isgps.curr_word |=
294 c << lexer->isgps.curr_offset;
295 } else {
296 lexer->isgps.curr_word |=
297 c >> -(lexer->isgps.curr_offset);
298 }
299 } else {
300 GPSD_LOG(ISGPS_ERRLEVEL_BASE, &lexer->errout,
301 "ISGPS parity failure, lost lock\n");
302 lexer->isgps.locked = false;
303 }
304 }
305 lexer->isgps.curr_offset -= 6;
306 GPSD_LOG(ISGPS_ERRLEVEL_BASE + 2, &lexer->errout,
307 "ISGPS residual %d\n",
308 lexer->isgps.curr_offset);
309 return res;
310 }
311
312 /* never achieved lock */
313 GPSD_LOG(ISGPS_ERRLEVEL_BASE + 1, &lexer->errout,
314 "ISGPS lock never achieved\n");
315 return ISGPS_NO_SYNC;
316 }
317
318