isgps.c

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