diff options
| -rw-r--r-- | Makefile.am | 1 | ||||
| -rw-r--r-- | gpsd.h | 7 | ||||
| -rw-r--r-- | isgps.c | 334 | ||||
| -rw-r--r-- | rtcm.c | 328 |
4 files changed, 342 insertions, 328 deletions
diff --git a/Makefile.am b/Makefile.am index eb56fa29..14565d8a 100644 --- a/Makefile.am +++ b/Makefile.am @@ -91,6 +91,7 @@ libgps_c_sources = \ dgpsip.c \ sirf.c \ report.c \ + isgps.c \ rtcm.c \ srecord.c \ bits.h \ @@ -33,6 +33,7 @@ enum isgpsstat_t { ISGPS_NO_SYNC, ISGPS_SYNC, ISGPS_SKIP, ISGPS_MESSAGE, }; +#define PREAMBLE_PATTERN 0x66 #define RTCM_ERRLEVEL_BASE 5 @@ -258,7 +259,13 @@ extern int ntpshm_put(struct gps_device_t *, double); extern int ntpshm_pps(struct gps_device_t *,struct timeval *); extern void isgps_init(/*@out@*/struct gps_device_t *); +enum isgpsstat_t isgps_decode(struct gps_device_t *session, + bool (*preamble_match)(isgps30bits_t *), + bool (*length_check)(struct gps_device_t *), + unsigned int c); extern enum isgpsstat_t rtcm_decode(struct gps_device_t *, unsigned int); + + extern void rtcm_dump(struct gps_device_t *, /*@out@*/char[], size_t); extern void ecef_to_wgs84fix(struct gps_data_t *, diff --git a/isgps.c b/isgps.c new file mode 100644 index 00000000..097c0d21 --- /dev/null +++ b/isgps.c @@ -0,0 +1,334 @@ +/***************************************************************************** + +This is a decoder for RTCM-104, an obscure and complicated serial +protocol used for broadcasting pseudorange corrections from +differential-GPS reference stations. The applicable +standard is + +RTCM RECOMMENDED STANDARDS FOR DIFFERENTIAL NAVSTAR GPS SERVICE, +RTCM PAPER 194-93/SC 104-STD + +Ordering instructions are accessible from <http://www.rtcm.org/> +under "Publications". + +Also applicable is ITU-R M.823: "Technical characteristics of +differential transmissions for global navigation satellite systems +from maritime radio beacons in the frequency band 283.5 - 315 kHz in +region 1 and 285 - 325 kHz in regions 2 & 3." + +The code was originally by Wolfgang Rupprecht. ESR severely hacked +it, with Wolfgang's help, in order to separate message analysis from +message dumping (he also added support for big-endian machines and +repacking). You are not expected to understand any of it. + +However, in case you think you need to, this decoder is divided into +two layers. The lower one just handles synchronizing with the +incoming bitstream and parity checking, Here are Wolfgang's original +rather cryptic notes on the lower level: + +-------------------------------------------------------------------------- +1) trim and bitflip the input. + +While syncing the msb of the input gets shifted into lsb of the +assembled word. + word <<= 1, or in input >> 5 + word <<= 1, or in input >> 4 + word <<= 1, or in input >> 3 + word <<= 1, or in input >> 2 + word <<= 1, or in input >> 1 + word <<= 1, or in input + +At one point it should sync-lock. + +---- + +Shift 6 bytes of RTCM data in as such: + +---> (trim-bits-to-5-bits) ---> (end-for-end-bit-flip) ---> + +---> shift-into-30-bit-shift-register + ||||||||||||||||||||||| + detector-for-preamble + ||||||||||||||||||||||| + detector-for-parity + ||||||||||||||||||||||| +-------------------------------------------------------------------------- + +The lower layer's job is done when it has assembled a message of up to +33 words of clean parity-checked data. At this point the upper layer +takes over. struct rtcm_msg_t is overlaid on the buffer and the bitfields +are used to extract pieces of it (which, if you're on a big-endian machine +may need to be swapped end-for-end). Those pieces are copied and (where +necessary) reassembled into a struct rtcm_t. + +Wolfgang's decoder was loosely based on one written by John Sanger in +1999 (in particular the dump function emits Sanger's dump format). +Here are John Sanger's original notes: + +-------------------------------------------------------------------------- +The RTCM decoder prints a legible representation of the input data. +The RTCM SC-104 specification is copyrighted, so I cannot +quote it - in fact, I have never read it! Most of the information +used to develop the decoder came from publication ITU-R M.823. +This is a specification of the data transmitted from LF DGPS +beacons in the 300kHz band. M.823 contains most of those parts of +RTCM SC-104 directly relevant to the air interface (there +are one or two annoying and vital omissions!). Information +about the serial interface format was gleaned from studying +the output of a beacon receiver test program made available on +Starlink's website. +-------------------------------------------------------------------------- + +*****************************************************************************/ + +#include <unistd.h> +#include <stdlib.h> +#include <string.h> +#include <stdbool.h> + +#include "gpsd.h" + +#define MAG_SHIFT 6u +#define MAG_TAG_DATA (1 << MAG_SHIFT) +#define MAG_TAG_MASK (3 << MAG_SHIFT) + +#define PREAMBLE_SHIFT 22 +#define PREAMBLE_MASK (0xFF << PREAMBLE_SHIFT) + +#define W_DATA_MASK 0x3fffffc0u + +/*@ +charint @*/ +static unsigned char parity_array[] = { + 0, 1, 1, 0, 1, 0, 0, 1, 1, 0, 0, 1, 0, 1, 1, 0, + 1, 0, 0, 1, 0, 1, 1, 0, 0, 1, 1, 0, 1, 0, 0, 1, + 1, 0, 0, 1, 0, 1, 1, 0, 0, 1, 1, 0, 1, 0, 0, 1, + 0, 1, 1, 0, 1, 0, 0, 1, 1, 0, 0, 1, 0, 1, 1, 0, + 1, 0, 0, 1, 0, 1, 1, 0, 0, 1, 1, 0, 1, 0, 0, 1, + 0, 1, 1, 0, 1, 0, 0, 1, 1, 0, 0, 1, 0, 1, 1, 0, + 0, 1, 1, 0, 1, 0, 0, 1, 1, 0, 0, 1, 0, 1, 1, 0, + 1, 0, 0, 1, 0, 1, 1, 0, 0, 1, 1, 0, 1, 0, 0, 1, + 1, 0, 0, 1, 0, 1, 1, 0, 0, 1, 1, 0, 1, 0, 0, 1, + 0, 1, 1, 0, 1, 0, 0, 1, 1, 0, 0, 1, 0, 1, 1, 0, + 0, 1, 1, 0, 1, 0, 0, 1, 1, 0, 0, 1, 0, 1, 1, 0, + 1, 0, 0, 1, 0, 1, 1, 0, 0, 1, 1, 0, 1, 0, 0, 1, + 0, 1, 1, 0, 1, 0, 0, 1, 1, 0, 0, 1, 0, 1, 1, 0, + 1, 0, 0, 1, 0, 1, 1, 0, 0, 1, 1, 0, 1, 0, 0, 1, + 1, 0, 0, 1, 0, 1, 1, 0, 0, 1, 1, 0, 1, 0, 0, 1, + 0, 1, 1, 0, 1, 0, 0, 1, 1, 0, 0, 1, 0, 1, 1, 0 +}; + +static unsigned int reverse_bits[] = { + 0, 32, 16, 48, 8, 40, 24, 56, 4, 36, 20, 52, 12, 44, 28, 60, + 2, 34, 18, 50, 10, 42, 26, 58, 6, 38, 22, 54, 14, 46, 30, 62, + 1, 33, 17, 49, 9, 41, 25, 57, 5, 37, 21, 53, 13, 45, 29, 61, + 3, 35, 19, 51, 11, 43, 27, 59, 7, 39, 23, 55, 15, 47, 31, 63 +}; +/*@ -charint @*/ + +static unsigned int isgpsparity(isgps30bits_t th) +{ +#define P_30_MASK 0x40000000u + +#define PARITY_25 0xbb1f3480u +#define PARITY_26 0x5d8f9a40u +#define PARITY_27 0xaec7cd00u +#define PARITY_28 0x5763e680u +#define PARITY_29 0x6bb1f340u +#define PARITY_30 0x8b7a89c0u + isgps30bits_t t; + unsigned int p; + + /* + if (th & P_30_MASK) + th ^= W_DATA_MASK; + */ + + /*@ +charint @*/ + t = th & PARITY_25; + p = parity_array[t & 0xff] ^ parity_array[(t >> 8) & 0xff] ^ + parity_array[(t >> 16) & 0xff] ^ parity_array[(t >> 24) & 0xff]; + t = th & PARITY_26; + p = (p << 1) | (parity_array[t & 0xff] ^ parity_array[(t >> 8) & 0xff] ^ + parity_array[(t >> 16) & 0xff] ^ parity_array[(t >> 24) & 0xff]); + t = th & PARITY_27; + p = (p << 1) | (parity_array[t & 0xff] ^ parity_array[(t >> 8) & 0xff] ^ + parity_array[(t >> 16) & 0xff] ^ parity_array[(t >> 24) & 0xff]); + t = th & PARITY_28; + p = (p << 1) | (parity_array[t & 0xff] ^ parity_array[(t >> 8) & 0xff] ^ + parity_array[(t >> 16) & 0xff] ^ parity_array[(t >> 24) & 0xff]); + t = th & PARITY_29; + p = (p << 1) | (parity_array[t & 0xff] ^ parity_array[(t >> 8) & 0xff] ^ + parity_array[(t >> 16) & 0xff] ^ parity_array[(t >> 24) & 0xff]); + t = th & PARITY_30; + p = (p << 1) | (parity_array[t & 0xff] ^ parity_array[(t >> 8) & 0xff] ^ + parity_array[(t >> 16) & 0xff] ^ parity_array[(t >> 24) & 0xff]); + /*@ -charint @*/ + + gpsd_report(RTCM_ERRLEVEL_BASE+2, "parity %u\n", p); + return (p); +} + + +#define isgpsparityok(w) (isgpsparity(w) == ((w) & 0x3f)) + +#if 0 +/* + * ESR found a doozy of a bug... + * + * Defining the above as a function triggers an optimizer bug in gcc 3.4.2. + * The symptom is that parity computation is screwed up and the decoder + * never achieves sync lock. Something steps on the argument to + * isgpsparity(); the lossage appears to be related to the compiler's + * attempt to fold the isgpsparity() call into isgpsparityok() in some + * tail-recursion-like manner. This happens under -O2, but not -O1, on + * both i386 and amd64. Disabling all of the individual -O2 suboptions + * does *not* fix it. + * + * And the fun doesn't stop there! It turns out that even with this fix, bare + * -O2 generates bad code. It takes "-O2 -fschedule-insns" to generate good + * code under 3.4.[23]...which is weird because -O2 is supposed to *imply* + * -fschedule-insns. + * + * gcc 4.0 does not manifest these bugs. + */ +static bool isgpsparityok(isgps30bits_t w) +{ + return (isgpsparity(w) == (w & 0x3f)); +} +#endif + +void isgps_init(/*@out@*/struct gps_device_t *session) +{ + session->isgps.curr_word = 0; + session->isgps.curr_offset = 24; /* first word */ + session->isgps.locked = false; + session->isgps.bufindex = 0; +} + +/*@ -usereleased -compdef @*/ +enum isgpsstat_t isgps_decode(struct gps_device_t *session, + bool (*preamble_match)(isgps30bits_t *), + bool (*length_check)(struct gps_device_t *), + unsigned int c) +{ + enum isgpsstat_t res; + + if ((c & MAG_TAG_MASK) != MAG_TAG_DATA) { + gpsd_report(RTCM_ERRLEVEL_BASE+1, + "word tag not correct, skipping\n"); + return ISGPS_SKIP; + } + c = reverse_bits[c & 0x3f]; + + /*@ -shiftnegative @*/ + if (!session->isgps.locked) { + session->isgps.curr_offset = -5; + session->isgps.bufindex = 0; + + while (session->isgps.curr_offset <= 0) { + gpsd_report(RTCM_ERRLEVEL_BASE+2, "syncing\n"); + session->isgps.curr_word <<= 1; + if (session->isgps.curr_offset > 0) { + session->isgps.curr_word |= c << session->isgps.curr_offset; + } else { + session->isgps.curr_word |= c >> -(session->isgps.curr_offset); + } + + if (preamble_match(&session->isgps.curr_word)) { + if (isgpsparityok(session->isgps.curr_word)) { + gpsd_report(RTCM_ERRLEVEL_BASE+1, + "preamble ok, parity ok -- locked\n"); + session->isgps.locked = true; + /* session->isgps.curr_offset; XXX - testing */ + break; + } + gpsd_report(RTCM_ERRLEVEL_BASE+1, + "preamble ok, parity fail\n"); + } + session->isgps.curr_offset++; + } /* end while */ + } + if (session->isgps.locked) { + res = ISGPS_SYNC; + + if (session->isgps.curr_offset > 0) { + session->isgps.curr_word |= c << session->isgps.curr_offset; + } else { + session->isgps.curr_word |= c >> -(session->isgps.curr_offset); + } + + if (session->isgps.curr_offset <= 0) { + /* weird-assed inversion */ + if (session->isgps.curr_word & P_30_MASK) + session->isgps.curr_word ^= W_DATA_MASK; + + if (isgpsparityok(session->isgps.curr_word)) { +#if 0 + /* + * Don't clobber the buffer just because we spot + * another preamble pattern in the data stream. -wsr + */ + if (preamble_match(&session->isgps.curr_word)) { + gpsd_report(RTCM_ERRLEVEL_BASE+2, + "Preamble spotted (index: %u)\n", + session->isgps.bufindex); + session->isgps.bufindex = 0; + } +#endif + gpsd_report(RTCM_ERRLEVEL_BASE+2, + "processing word %u (offset %d)\n", + session->isgps.bufindex, session->isgps.curr_offset); + { + /* + * Guard against a buffer overflow attack. Just wait for + * the next PREAMBLE_PATTERN and go on from there. + */ + if (session->isgps.bufindex >= RTCM_WORDS_MAX){ + session->isgps.bufindex = 0; + gpsd_report(RTCM_ERRLEVEL_BASE+1, + "RTCM buffer overflowing -- resetting\n"); + return ISGPS_NO_SYNC; + } + + session->isgps.buf[session->isgps.bufindex] = session->isgps.curr_word; + + if ((session->isgps.bufindex == 0) && + !preamble_match((isgps30bits_t *)session->isgps.buf)) { + gpsd_report(RTCM_ERRLEVEL_BASE+1, + "word 0 not a preamble- punting\n"); + return ISGPS_NO_SYNC; + } + session->isgps.bufindex++; + + if (length_check(session)) { + /* jackpot, we have a complete packet*/ + session->isgps.bufindex = 0; + res = ISGPS_MESSAGE; + } + } + session->isgps.curr_word <<= 30; /* preserve the 2 low bits */ + session->isgps.curr_offset += 30; + if (session->isgps.curr_offset > 0) { + session->isgps.curr_word |= c << session->isgps.curr_offset; + } else { + session->isgps.curr_word |= c >> -(session->isgps.curr_offset); + } + } else { + gpsd_report(RTCM_ERRLEVEL_BASE+0, + "parity failure, lost lock\n"); + session->isgps.locked = false; + } + } + session->isgps.curr_offset -= 6; + gpsd_report(RTCM_ERRLEVEL_BASE+2, "residual %d\n", session->isgps.curr_offset); + return res; + } + /*@ +shiftnegative @*/ + + /* never achieved lock */ + gpsd_report(RTCM_ERRLEVEL_BASE+1, + "lock never achieved\n"); + return ISGPS_NO_SYNC; +} +/*@ +usereleased +compdef @*/ + @@ -1,86 +1,3 @@ -/***************************************************************************** - -This is a decoder for RTCM-104, an obscure and complicated serial -protocol used for broadcasting pseudorange corrections from -differential-GPS reference stations. The applicable -standard is - -RTCM RECOMMENDED STANDARDS FOR DIFFERENTIAL NAVSTAR GPS SERVICE, -RTCM PAPER 194-93/SC 104-STD - -Ordering instructions are accessible from <http://www.rtcm.org/> -under "Publications". - -Also applicable is ITU-R M.823: "Technical characteristics of -differential transmissions for global navigation satellite systems -from maritime radio beacons in the frequency band 283.5 - 315 kHz in -region 1 and 285 - 325 kHz in regions 2 & 3." - -The code was originally by Wolfgang Rupprecht. ESR severely hacked -it, with Wolfgang's help, in order to separate message analysis from -message dumping (he also added support for big-endian machines and -repacking). You are not expected to understand any of it. - -However, in case you think you need to, this decoder is divided into -two layers. The lower one just handles synchronizing with the -incoming bitstream and parity checking, Here are Wolfgang's original -rather cryptic notes on the lower level: - --------------------------------------------------------------------------- -1) trim and bitflip the input. - -While syncing the msb of the input gets shifted into lsb of the -assembled word. - word <<= 1, or in input >> 5 - word <<= 1, or in input >> 4 - word <<= 1, or in input >> 3 - word <<= 1, or in input >> 2 - word <<= 1, or in input >> 1 - word <<= 1, or in input - -At one point it should sync-lock. - ----- - -Shift 6 bytes of RTCM data in as such: - ----> (trim-bits-to-5-bits) ---> (end-for-end-bit-flip) ---> - ----> shift-into-30-bit-shift-register - ||||||||||||||||||||||| - detector-for-preamble - ||||||||||||||||||||||| - detector-for-parity - ||||||||||||||||||||||| --------------------------------------------------------------------------- - -The lower layer's job is done when it has assembled a message of up to -33 words of clean parity-checked data. At this point the upper layer -takes over. struct rtcm_msg_t is overlaid on the buffer and the bitfields -are used to extract pieces of it (which, if you're on a big-endian machine -may need to be swapped end-for-end). Those pieces are copied and (where -necessary) reassembled into a struct rtcm_t. - -Wolfgang's decoder was loosely based on one written by John Sanger in -1999 (in particular the dump function emits Sanger's dump format). -Here are John Sanger's original notes: - --------------------------------------------------------------------------- -The RTCM decoder prints a legible representation of the input data. -The RTCM SC-104 specification is copyrighted, so I cannot -quote it - in fact, I have never read it! Most of the information -used to develop the decoder came from publication ITU-R M.823. -This is a specification of the data transmitted from LF DGPS -beacons in the 300kHz band. M.823 contains most of those parts of -RTCM SC-104 directly relevant to the air interface (there -are one or two annoying and vital omissions!). Information -about the serial interface format was gleaned from studying -the output of a beacon receiver test program made available on -Starlink's website. --------------------------------------------------------------------------- - -*****************************************************************************/ - #include <unistd.h> #include <stdlib.h> #include <string.h> @@ -89,251 +6,6 @@ Starlink's website. #include "gpsd.h" -#define MAG_SHIFT 6u -#define MAG_TAG_DATA (1 << MAG_SHIFT) -#define MAG_TAG_MASK (3 << MAG_SHIFT) - -#define PREAMBLE_PATTERN 0x66 -#define PREAMBLE_SHIFT 22 -#define PREAMBLE_MASK (0xFF << PREAMBLE_SHIFT) - -#define W_DATA_MASK 0x3fffffc0u - -/*@ +charint @*/ -static unsigned char parity_array[] = { - 0, 1, 1, 0, 1, 0, 0, 1, 1, 0, 0, 1, 0, 1, 1, 0, - 1, 0, 0, 1, 0, 1, 1, 0, 0, 1, 1, 0, 1, 0, 0, 1, - 1, 0, 0, 1, 0, 1, 1, 0, 0, 1, 1, 0, 1, 0, 0, 1, - 0, 1, 1, 0, 1, 0, 0, 1, 1, 0, 0, 1, 0, 1, 1, 0, - 1, 0, 0, 1, 0, 1, 1, 0, 0, 1, 1, 0, 1, 0, 0, 1, - 0, 1, 1, 0, 1, 0, 0, 1, 1, 0, 0, 1, 0, 1, 1, 0, - 0, 1, 1, 0, 1, 0, 0, 1, 1, 0, 0, 1, 0, 1, 1, 0, - 1, 0, 0, 1, 0, 1, 1, 0, 0, 1, 1, 0, 1, 0, 0, 1, - 1, 0, 0, 1, 0, 1, 1, 0, 0, 1, 1, 0, 1, 0, 0, 1, - 0, 1, 1, 0, 1, 0, 0, 1, 1, 0, 0, 1, 0, 1, 1, 0, - 0, 1, 1, 0, 1, 0, 0, 1, 1, 0, 0, 1, 0, 1, 1, 0, - 1, 0, 0, 1, 0, 1, 1, 0, 0, 1, 1, 0, 1, 0, 0, 1, - 0, 1, 1, 0, 1, 0, 0, 1, 1, 0, 0, 1, 0, 1, 1, 0, - 1, 0, 0, 1, 0, 1, 1, 0, 0, 1, 1, 0, 1, 0, 0, 1, - 1, 0, 0, 1, 0, 1, 1, 0, 0, 1, 1, 0, 1, 0, 0, 1, - 0, 1, 1, 0, 1, 0, 0, 1, 1, 0, 0, 1, 0, 1, 1, 0 -}; - -static unsigned int reverse_bits[] = { - 0, 32, 16, 48, 8, 40, 24, 56, 4, 36, 20, 52, 12, 44, 28, 60, - 2, 34, 18, 50, 10, 42, 26, 58, 6, 38, 22, 54, 14, 46, 30, 62, - 1, 33, 17, 49, 9, 41, 25, 57, 5, 37, 21, 53, 13, 45, 29, 61, - 3, 35, 19, 51, 11, 43, 27, 59, 7, 39, 23, 55, 15, 47, 31, 63 -}; -/*@ -charint @*/ - -static unsigned int isgpsparity(isgps30bits_t th) -{ -#define P_30_MASK 0x40000000u - -#define PARITY_25 0xbb1f3480u -#define PARITY_26 0x5d8f9a40u -#define PARITY_27 0xaec7cd00u -#define PARITY_28 0x5763e680u -#define PARITY_29 0x6bb1f340u -#define PARITY_30 0x8b7a89c0u - isgps30bits_t t; - unsigned int p; - - /* - if (th & P_30_MASK) - th ^= W_DATA_MASK; - */ - - /*@ +charint @*/ - t = th & PARITY_25; - p = parity_array[t & 0xff] ^ parity_array[(t >> 8) & 0xff] ^ - parity_array[(t >> 16) & 0xff] ^ parity_array[(t >> 24) & 0xff]; - t = th & PARITY_26; - p = (p << 1) | (parity_array[t & 0xff] ^ parity_array[(t >> 8) & 0xff] ^ - parity_array[(t >> 16) & 0xff] ^ parity_array[(t >> 24) & 0xff]); - t = th & PARITY_27; - p = (p << 1) | (parity_array[t & 0xff] ^ parity_array[(t >> 8) & 0xff] ^ - parity_array[(t >> 16) & 0xff] ^ parity_array[(t >> 24) & 0xff]); - t = th & PARITY_28; - p = (p << 1) | (parity_array[t & 0xff] ^ parity_array[(t >> 8) & 0xff] ^ - parity_array[(t >> 16) & 0xff] ^ parity_array[(t >> 24) & 0xff]); - t = th & PARITY_29; - p = (p << 1) | (parity_array[t & 0xff] ^ parity_array[(t >> 8) & 0xff] ^ - parity_array[(t >> 16) & 0xff] ^ parity_array[(t >> 24) & 0xff]); - t = th & PARITY_30; - p = (p << 1) | (parity_array[t & 0xff] ^ parity_array[(t >> 8) & 0xff] ^ - parity_array[(t >> 16) & 0xff] ^ parity_array[(t >> 24) & 0xff]); - /*@ -charint @*/ - - gpsd_report(RTCM_ERRLEVEL_BASE+2, "parity %u\n", p); - return (p); -} - - -#define isgpsparityok(w) (isgpsparity(w) == ((w) & 0x3f)) - -#if 0 -/* - * ESR found a doozy of a bug... - * - * Defining the above as a function triggers an optimizer bug in gcc 3.4.2. - * The symptom is that parity computation is screwed up and the decoder - * never achieves sync lock. Something steps on the argument to - * isgpsparity(); the lossage appears to be related to the compiler's - * attempt to fold the isgpsparity() call into isgpsparityok() in some - * tail-recursion-like manner. This happens under -O2, but not -O1, on - * both i386 and amd64. Disabling all of the individual -O2 suboptions - * does *not* fix it. - * - * And the fun doesn't stop there! It turns out that even with this fix, bare - * -O2 generates bad code. It takes "-O2 -fschedule-insns" to generate good - * code under 3.4.[23]...which is weird because -O2 is supposed to *imply* - * -fschedule-insns. - * - * gcc 4.0 does not manifest these bugs. - */ -static bool isgpsparityok(isgps30bits_t w) -{ - return (isgpsparity(w) == (w & 0x3f)); -} -#endif - -void isgps_init(/*@out@*/struct gps_device_t *session) -{ - session->isgps.curr_word = 0; - session->isgps.curr_offset = 24; /* first word */ - session->isgps.locked = false; - session->isgps.bufindex = 0; -} - -/*@ -usereleased -compdef @*/ -static enum isgpsstat_t isgps_decode(struct gps_device_t *session, - bool (*preamble_match)(isgps30bits_t *), - bool (*length_check)(struct gps_device_t *), - unsigned int c) -{ - enum isgpsstat_t res; - - if ((c & MAG_TAG_MASK) != MAG_TAG_DATA) { - gpsd_report(RTCM_ERRLEVEL_BASE+1, - "word tag not correct, skipping\n"); - return ISGPS_SKIP; - } - c = reverse_bits[c & 0x3f]; - - /*@ -shiftnegative @*/ - if (!session->isgps.locked) { - session->isgps.curr_offset = -5; - session->isgps.bufindex = 0; - - while (session->isgps.curr_offset <= 0) { - gpsd_report(RTCM_ERRLEVEL_BASE+2, "syncing\n"); - session->isgps.curr_word <<= 1; - if (session->isgps.curr_offset > 0) { - session->isgps.curr_word |= c << session->isgps.curr_offset; - } else { - session->isgps.curr_word |= c >> -(session->isgps.curr_offset); - } - - if (preamble_match(&session->isgps.curr_word)) { - if (isgpsparityok(session->isgps.curr_word)) { - gpsd_report(RTCM_ERRLEVEL_BASE+1, - "preamble ok, parity ok -- locked\n"); - session->isgps.locked = true; - /* session->isgps.curr_offset; XXX - testing */ - break; - } - gpsd_report(RTCM_ERRLEVEL_BASE+1, - "preamble ok, parity fail\n"); - } - session->isgps.curr_offset++; - } /* end while */ - } - if (session->isgps.locked) { - res = ISGPS_SYNC; - - if (session->isgps.curr_offset > 0) { - session->isgps.curr_word |= c << session->isgps.curr_offset; - } else { - session->isgps.curr_word |= c >> -(session->isgps.curr_offset); - } - - if (session->isgps.curr_offset <= 0) { - /* weird-assed inversion */ - if (session->isgps.curr_word & P_30_MASK) - session->isgps.curr_word ^= W_DATA_MASK; - - if (isgpsparityok(session->isgps.curr_word)) { -#if 0 - /* - * Don't clobber the buffer just because we spot - * another preamble pattern in the data stream. -wsr - */ - if (preamble_match(&session->isgps.curr_word)) { - gpsd_report(RTCM_ERRLEVEL_BASE+2, - "Preamble spotted (index: %u)\n", - session->isgps.bufindex); - session->isgps.bufindex = 0; - } -#endif - gpsd_report(RTCM_ERRLEVEL_BASE+2, - "processing word %u (offset %d)\n", - session->isgps.bufindex, session->isgps.curr_offset); - { - /* - * Guard against a buffer overflow attack. Just wait for - * the next PREAMBLE_PATTERN and go on from there. - */ - if (session->isgps.bufindex >= RTCM_WORDS_MAX){ - session->isgps.bufindex = 0; - gpsd_report(RTCM_ERRLEVEL_BASE+1, - "RTCM buffer overflowing -- resetting\n"); - return ISGPS_NO_SYNC; - } - - session->isgps.buf[session->isgps.bufindex] = session->isgps.curr_word; - - if ((session->isgps.bufindex == 0) && - !preamble_match((isgps30bits_t *)session->isgps.buf)) { - gpsd_report(RTCM_ERRLEVEL_BASE+1, - "word 0 not a preamble- punting\n"); - return ISGPS_NO_SYNC; - } - session->isgps.bufindex++; - - if (length_check(session)) { - /* jackpot, we have a complete packet*/ - session->isgps.bufindex = 0; - res = ISGPS_MESSAGE; - } - } - session->isgps.curr_word <<= 30; /* preserve the 2 low bits */ - session->isgps.curr_offset += 30; - if (session->isgps.curr_offset > 0) { - session->isgps.curr_word |= c << session->isgps.curr_offset; - } else { - session->isgps.curr_word |= c >> -(session->isgps.curr_offset); - } - } else { - gpsd_report(RTCM_ERRLEVEL_BASE+0, - "parity failure, lost lock\n"); - session->isgps.locked = false; - } - } - session->isgps.curr_offset -= 6; - gpsd_report(RTCM_ERRLEVEL_BASE+2, "residual %d\n", session->isgps.curr_offset); - return res; - } - /*@ +shiftnegative @*/ - - /* never achieved lock */ - gpsd_report(RTCM_ERRLEVEL_BASE+1, - "lock never achieved\n"); - return ISGPS_NO_SYNC; -} -/*@ +usereleased +compdef @*/ - /* * Structures for interpreting words in an RTCM-104 message (after * parity checking and removing inversion). |