Add a CRC-24Q implementation. It's wrong, but it's a start.

8 files changed, 212 insertions, 22 deletions

diff --git a/Makefile.am b/Makefile.am
index 27c485f4..b28d4aa7 100644
--- a/Makefile.am
+++ b/Makefile.am
@@ -141,7 +141,8 @@ libgpsd_c_sources = \
 	tsip.c \
 	evermore.c \
 	italk.c \
-	navcom.c
+	navcom.c \
+	crc24q.c
 
 libgpsd_h_sources = \
 	italk.h \
@@ -150,6 +151,7 @@ libgpsd_h_sources = \
 	bits.h \
 	packet_names.h \
 	rtcm2.h \
+	crc24q.h \
 	gpsd.h
 
 BUILT_SOURCES = packet_names.h gpsd.h
diff --git a/crc24q.c b/crc24q.c
new file mode 100644
index 00000000..cabc5da1
--- /dev/null
+++ b/crc24q.c
@@ -0,0 +1,111 @@
+/*
+ * This is an implementation of the CRC-24Q cyclic redundancy checksum
+ * used by Qualcomm, Nokia, and RTCM104V3. According to the RTCM104V3
+ * standard,
+ *
+ * 1) It detects all single bit errors per 24-bit code word.
+ * 2) It detects all double bit error combinations in a code word.
+ * 3) It detects any odd number of errors.
+ * 4) It detects any burst error for which the length of the burst is less than
+ *    or equal to 24 bits.
+ * 5) It detects most large error bursts with length greater than 24 bits;
+ *    the odds of a false positive are at most 2^-23.
+ *
+ * This implementation by Eric Steven Raymond is Copyright (c) 2008
+ * for the GPSD project, and is released under BSD terms.
+ */
+#include <stdbool.h>
+
+#include "crc24q.h"
+
+static const unsigned crc24q[256] = {
+	0x00000000, 0x00D6A776, 0x00F64557, 0x0020E221, 0x00B78115, 
+	0x00612663, 0x0041C442, 0x00976334, 0x00340991, 0x00E2AEE7, 
+	0x00C24CC6, 0x0014EBB0,	0x00838884, 0x00552FF2, 0x0075CDD3, 
+	0x00A36AA5, 0x00681322, 0x00BEB454, 0x009E5675, 0x0048F103, 
+	0x00DF9237, 0x00093541, 0x0029D760, 0x00FF7016,	0x005C1AB3, 
+	0x008ABDC5, 0x00AA5FE4, 0x007CF892, 0x00EB9BA6, 0x003D3CD0,
+	0x001DDEF1, 0x00CB7987, 0x00D02644, 0x00068132, 0x00266313, 
+	0x00F0C465, 0x0067A751, 0x00B10027, 0x0091E206, 0x00474570, 
+	0x00E42FD5, 0x003288A3,	0x00126A82, 0x00C4CDF4, 0x0053AEC0, 
+	0x008509B6, 0x00A5EB97, 0x00734CE1, 0x00B83566, 0x006E9210, 
+	0x004E7031, 0x0098D747, 0x000FB473, 0x00D91305, 0x00F9F124, 
+	0x002F5652, 0x008C3CF7, 0x005A9B81, 0x007A79A0, 0x00ACDED6,
+	0x003BBDE2, 0x00ED1A94, 0x00CDF8B5, 0x001B5FC3, 0x00FB4733, 
+	0x002DE045, 0x000D0264, 0x00DBA512, 0x004CC626, 0x009A6150, 
+	0x00BA8371, 0x006C2407,	0x00CF4EA2, 0x0019E9D4, 0x00390BF5, 
+	0x00EFAC83, 0x0078CFB7, 0x00AE68C1, 0x008E8AE0, 0x00582D96, 
+	0x00935411, 0x0045F367, 0x00651146, 0x00B3B630,	0x0024D504, 
+	0x00F27272, 0x00D29053, 0x00043725, 0x00A75D80, 0x0071FAF6,
+	0x005118D7, 0x0087BFA1, 0x0010DC95, 0x00C67BE3, 0x00E699C2, 
+	0x00303EB4, 0x002B6177, 0x00FDC601, 0x00DD2420, 0x000B8356, 
+	0x009CE062, 0x004A4714,	0x006AA535, 0x00BC0243, 0x001F68E6, 
+	0x00C9CF90, 0x00E92DB1, 0x003F8AC7, 0x00A8E9F3, 0x007E4E85, 
+	0x005EACA4, 0x00880BD2, 0x00437255, 0x0095D523,	0x00B53702, 
+	0x00639074, 0x00F4F340, 0x00225436, 0x0002B617, 0x00D41161,
+	0x00777BC4, 0x00A1DCB2, 0x00813E93, 0x005799E5, 0x00C0FAD1, 
+	0x00165DA7, 0x0036BF86, 0x00E018F0, 0x00AD85DD, 0x007B22AB, 
+	0x005BC08A, 0x008D67FC,	0x001A04C8, 0x00CCA3BE, 0x00EC419F, 
+	0x003AE6E9, 0x00998C4C, 0x004F2B3A, 0x006FC91B, 0x00B96E6D, 
+	0x002E0D59, 0x00F8AA2F, 0x00D8480E, 0x000EEF78,	0x00C596FF, 
+	0x00133189, 0x0033D3A8, 0x00E574DE, 0x007217EA, 0x00A4B09C,
+	0x008452BD, 0x0052F5CB, 0x00F19F6E, 0x00273818, 0x0007DA39, 
+	0x00D17D4F, 0x00461E7B, 0x0090B90D, 0x00B05B2C, 0x0066FC5A, 
+	0x007DA399, 0x00AB04EF,	0x008BE6CE, 0x005D41B8, 0x00CA228C, 
+	0x001C85FA, 0x003C67DB, 0x00EAC0AD, 0x0049AA08, 0x009F0D7E, 
+	0x00BFEF5F, 0x00694829, 0x00FE2B1D, 0x00288C6B,	0x00086E4A, 
+	0x00DEC93C, 0x0015B0BB, 0x00C317CD, 0x00E3F5EC, 0x0035529A,
+	0x00A231AE, 0x007496D8, 0x005474F9, 0x0082D38F, 0x0021B92A, 
+	0x00F71E5C, 0x00D7FC7D, 0x00015B0B, 0x0096383F, 0x00409F49, 
+	0x00607D68, 0x00B6DA1E,	0x0056C2EE, 0x00806598, 0x00A087B9, 
+	0x007620CF, 0x00E143FB, 0x0037E48D, 0x001706AC, 0x00C1A1DA, 
+	0x0062CB7F, 0x00B46C09, 0x00948E28, 0x0042295E,	0x00D54A6A, 
+	0x0003ED1C, 0x00230F3D, 0x00F5A84B, 0x003ED1CC, 0x00E876BA,
+	0x00C8949B, 0x001E33ED, 0x008950D9, 0x005FF7AF, 0x007F158E, 
+	0x00A9B2F8, 0x000AD85D, 0x00DC7F2B, 0x00FC9D0A, 0x002A3A7C, 
+	0x00BD5948, 0x006BFE3E,	0x004B1C1F, 0x009DBB69, 0x0086E4AA, 
+	0x005043DC, 0x0070A1FD, 0x00A6068B, 0x003165BF, 0x00E7C2C9, 
+	0x00C720E8, 0x0011879E, 0x00B2ED3B, 0x00644A4D,	0x0044A86C, 
+	0x00920F1A, 0x00056C2E, 0x00D3CB58, 0x00F32979, 0x00258E0F,
+	0x00EEF788, 0x003850FE, 0x0018B2DF, 0x00CE15A9, 0x0059769D, 
+	0x008FD1EB, 0x00AF33CA, 0x007994BC, 0x00DAFE19, 0x000C596F, 
+	0x002CBB4E, 0x00FA1C38,	0x006D7F0C, 0x00BBD87A, 0x009B3A5B, 
+	0x004D9D2D
+};
+
+unsigned crc24q_hash(unsigned char *data, int len)
+{
+    int i;
+    unsigned crc = 0xffffff;
+
+    for (i = 0; i < len; i++) {
+	crc = (crc << 8) ^ crc24q[data[i] ^ (unsigned char)(crc>>16)];    
+    }
+
+    crc = (crc & 0x00ffffff);
+
+    return crc;
+}
+
+#define LO(x)	((x) & 0x0000ffff)
+#define MID(x)	(((x) >> 8) & 0x0000ffff)
+#define HI(x)	(((x) >> 16) & 0x0000ffff)
+
+void crc24q_sign(unsigned char *data, int len)
+{
+    unsigned crc = crc24q_hash(data, len);
+
+    data[len]   = LO(crc);
+    data[len+1] = MID(crc);
+    data[len+2] = HI(crc);
+}
+
+bool crc24q_check(unsigned char *data, int len)
+{
+    unsigned crc = crc24q_hash(data, len);
+
+    return (((data[len-3] == LO(crc)) &&
+	     (data[len-2] == MID(crc)) &&
+	     (data[len-1] == HI(crc))));
+}
+
diff --git a/drivers.c b/drivers.c
index b8c2a1f0..76c1eddf 100644
--- a/drivers.c
+++ b/drivers.c
@@ -739,22 +739,22 @@ struct gps_type_t trueNorth = {
 #ifdef RTCM104V2_ENABLE
 /**************************************************************************
  *
- * RTCM-104, used for broadcasting DGPS corrections and by DGPS radios
+ * RTCM-104 (v2), used for broadcasting DGPS corrections and by DGPS radios
  *
  **************************************************************************/
 
-static gps_mask_t rtcm104_analyze(struct gps_device_t *session)
+static gps_mask_t rtcm104v2_analyze(struct gps_device_t *session)
 {
-    rtcm2_unpack(&session->gpsdata.rtcm, (char *)session->packet.isgps.buf);
-    gpsd_report(LOG_RAW, "RTCM packet type 0x%02x length %d words: %s\n",
-		session->gpsdata.rtcm.type,
-		session->gpsdata.rtcm.length+2,
-		gpsd_hexdump(session->packet.isgps.buf, (session->gpsdata.rtcm.length+2)*sizeof(isgps30bits_t)));
+    rtcm2_unpack(&session->gpsdata.rtcm2, (char *)session->packet.isgps.buf);
+    gpsd_report(LOG_RAW, "RTCM 2.x packet type 0x%02x length %d words: %s\n",
+		session->gpsdata.rtcm2.type,
+		session->gpsdata.rtcm2.length+2,
+		gpsd_hexdump(session->packet.isgps.buf, (session->gpsdata.rtcm2.length+2)*sizeof(isgps30bits_t)));
     return RTCM2_SET;
 }
 
-static struct gps_type_t rtcm104 = {
-    .type_name     = "RTCM104",		/* full name of type */
+static struct gps_type_t rtcm104v2 = {
+    .type_name     = "RTCM104V2",	/* full name of type */
     .trigger       = NULL,		/* no recognition string */
     .channels      = 0,			/* not used */
     .probe_wakeup  = NULL,		/* no wakeup to be done before hunt */
@@ -764,7 +764,7 @@ static struct gps_type_t rtcm104 = {
     .configurator  = NULL,		/* no configurator */
 #endif /* ALLOW_RECONFIGURE */
     .get_packet    = generic_get,	/* how to get a packet */
-    .parse_packet  = rtcm104_analyze,	/*  */
+    .parse_packet  = rtcm104v2_analyze,	/*  */
     .rtcm_writer   = NULL,		/* don't send RTCM data,  */
     .speed_switcher= NULL,		/* no speed switcher */
     .mode_switcher = NULL,		/* no mode switcher */
@@ -777,6 +777,47 @@ static struct gps_type_t rtcm104 = {
     .cycle	   = 1,			/* updates every second */
 };
 #endif /* RTCM104V2_ENABLE */
+#ifdef RTCM104V3_ENABLE
+/**************************************************************************
+ *
+ * RTCM-104 (v3), used for broadcasting DGPS corrections and by DGPS radios
+ *
+ **************************************************************************/
+
+static gps_mask_t rtcm104v3_analyze(struct gps_device_t *session)
+{
+    // FIXME: Add a real unpacking function
+    gpsd_report(LOG_RAW, "RTCM 3.x packet type 0x%02x length %d words: %s\n",
+		session->gpsdata.rtcm3.type,
+		session->gpsdata.rtcm3.length,
+		gpsd_hexdump(session->packet.inbuffer, (session->gpsdata.rtcm3.length)));
+    return RTCM3_SET;
+}
+
+static struct gps_type_t rtcm104v3 = {
+    .type_name     = "RTCM104V3",	/* full name of type */
+    .trigger       = NULL,		/* no recognition string */
+    .channels      = 0,			/* not used */
+    .probe_wakeup  = NULL,		/* no wakeup to be done before hunt */
+    .probe_detect  = NULL,		/* no probe */
+    .probe_subtype = NULL,		/* no subtypes */
+#ifdef ALLOW_RECONFIGURE
+    .configurator  = NULL,		/* no configurator */
+#endif /* ALLOW_RECONFIGURE */
+    .get_packet    = generic_get,	/* how to get a packet */
+    .parse_packet  = rtcm104v3_analyze,	/*  */
+    .rtcm_writer   = NULL,		/* don't send RTCM data,  */
+    .speed_switcher= NULL,		/* no speed switcher */
+    .mode_switcher = NULL,		/* no mode switcher */
+    .rate_switcher = NULL,		/* no sample-rate switcher */
+    .cycle_chars   = -1,		/* not relevant, no rate switch */
+#ifdef ALLOW_RECONFIGURE
+    .revert	   = NULL,		/* no setting-reversion method */
+#endif /* ALLOW_RECONFIGURE */
+    .wrapup	   = NULL,		/* no wrapup code */
+    .cycle	   = 1,			/* updates every second */
+};
+#endif /* RTCM104V3_ENABLE */
 
 #ifdef GARMINTXT_ENABLE
 /**************************************************************************
@@ -875,8 +916,11 @@ static struct gps_type_t *gpsd_driver_array[] = {
     &italk_binary,
 #endif /* ITRAX_ENABLE */
 #ifdef RTCM104V2_ENABLE
-    &rtcm104,
+    &rtcm104v2,
 #endif /* RTCM104V2_ENABLE */
+#ifdef RTCM104V3_ENABLE
+    &rtcm104v3,
+#endif /* RTCM104V3_ENABLE */
 #ifdef GARMINTXT_ENABLE
     &garmintxt,
 #endif /* GARMINTXT_ENABLE */
diff --git a/gps.h b/gps.h
index a46be8c8..abc5ac78 100644
--- a/gps.h
+++ b/gps.h
@@ -94,7 +94,7 @@ struct gps_fix_t {
 /*
  * A nominally 30-bit word (24 bits of data, 6 bits of parity)
  * used both in the GPS downlink protocol described in IS-GPS-200
- * and in the format for DGPS corrections used in RTCM-104.
+ * and in the format for DGPS corrections used in RTCM-104v2.
  */
 typedef /*@unsignedintegraltype@*/ uint32_t isgps30bits_t;
 #endif /* S_SPLINT_S */
@@ -208,7 +208,8 @@ struct rtcm3_extended_rtk {
 
 struct rtcm3_t {
     /* header contents */
-    unsigned type;	/* RTCM message type */
+    unsigned type;	/* RTCM 3.x message type */
+    unsigned length;	/* payload length, exclusive of checksum */
 
     /* message data in decoded form */
     union {
@@ -346,6 +347,7 @@ struct gps_data_t {
 #define ERROR_SET	0x08000000u
 #define CYCLE_START_SET	0x10000000u
 #define RTCM2_SET	0x20000000u
+#define RTCM3_SET	0x40000000u
 #define FIX_SET		(TIME_SET|MODE_SET|TIMERR_SET|LATLON_SET|HERR_SET|ALTITUDE_SET|VERR_SET|TRACK_SET|TRACKERR_SET|SPEED_SET|SPEEDERR_SET|CLIMB_SET|CLIMBERR_SET)
     double online;		/* NZ if GPS is on line, 0 if not.
 				 *
@@ -410,7 +412,8 @@ struct gps_data_t {
     unsigned int driver_mode;	/* whether driver is in native mode or not */
 
     /* RTCM-104 data */
-    struct rtcm2_t	rtcm;
+    struct rtcm2_t	rtcm2;
+    struct rtcm3_t	rtcm3;
     
     /* device list */
     int ndevices;		/* count of available devices */
diff --git a/gpsd.c b/gpsd.c
index 036e41f5..bb9db61e 100644
--- a/gpsd.c
+++ b/gpsd.c
@@ -1680,7 +1680,7 @@ int main(int argc, char *argv[])
 		}
 #ifdef RTCM104_SERVICE
 		/* copy each RTCM-104 correction to all GPSes */
-		if ((changed & RTCM_SET) != 0) {
+		if ((changed & RTCM2_SET) != 0 || (changed & RTCM3_SET) != 0) {
 		    for (gps = channels; gps < channels + MAXDEVICES; gps++)
 			if (gps->device_type != NULL && gps->device_type->rtcm_writer != NULL)
 			    (void)gps->device_type->rtcm_writer(gps, (char *)channel->outbuffer, channel->outbuflen);
diff --git a/libgpsd_core.c b/libgpsd_core.c
index 5787b3c1..65c86b29 100644
--- a/libgpsd_core.c
+++ b/libgpsd_core.c
@@ -734,7 +734,12 @@ gps_mask_t gpsd_poll(struct gps_device_t *session)
 #endif /* ITRAX_ENABLE */
 #ifdef RTCM104V2_ENABLE
 	    case RTCM2_PACKET:
-		(void)gpsd_switch_driver(session, "RTCM104");
+		(void)gpsd_switch_driver(session, "RTCM104V2");
+		break;
+#endif /* RTCM104V2_ENABLE */
+#ifdef RTCM104V3_ENABLE
+	    case RTCM3_PACKET:
+		(void)gpsd_switch_driver(session, "RTCM104V3");
 		break;
 #endif /* RTCM104V2_ENABLE */
 	    }
@@ -817,9 +822,10 @@ gps_mask_t gpsd_poll(struct gps_device_t *session)
 	    char buf2[MAX_PACKET_LENGTH*3+2];
 
 	    buf2[0] = '\0';
+	    // FIXME: Add RTCMV3 handking as well.
 #ifdef RTCM104V2_ENABLE
 	    if ((session->gpsdata.set & RTCM2_SET) != 0)
-		rtcm2_dump(&session->gpsdata.rtcm, 
+		rtcm2_dump(&session->gpsdata.rtcm2, 
 			  buf2+strlen(buf2), 
 			  (sizeof(buf2)-strlen(buf2)));
 	    else {
diff --git a/packet.c b/packet.c
index 02f79fa2..c5daa266 100644
--- a/packet.c
+++ b/packet.c
@@ -32,6 +32,7 @@ others apart and distinguish them from baud barf.
 #include <errno.h>
 #include "gpsd_config.h"
 #include "gpsd.h"
+#include "crc24q.h"
 
 /*
  * The packet-recognition state machine.  It can be fooled by garbage
@@ -534,7 +535,11 @@ static void nextstate(struct gps_packet_t *lexer,
     case RTCM3_LEADER_2:
 	lexer->length |= c;
 	lexer->length += 3;	/* to get the three checksum bytes */
-	lexer->state = RTCM3_RECOGNIZED;
+	lexer->state = RTCM3_PAYLOAD;
+	break;
+    case RTCM3_PAYLOAD:
+	if (--lexer->length == 0)
+	    lexer->state = RTCM3_RECOGNIZED;
 	break;
 #endif /* RTCM104V3_ENABLE */
 #ifdef ZODIAC_ENABLE
@@ -1067,9 +1072,16 @@ ssize_t packet_parse(struct gps_packet_t *lexer, size_t fix)
 #endif /* TSIP_ENABLE || GARMIN_ENABLE */
 #ifdef RTCM104V3_ENABLE
 	else if (lexer->state == RTCM3_RECOGNIZED) {
-	    // FIXME: Do the "Qualcomm CRC" check against the last three bytes
-	    packet_accept(lexer, RTCM3_PACKET);
-	    packet_discard(lexer);
+	    if (crc24q_check(lexer->inbuffer, 
+			     lexer->inbufptr-lexer->inbuffer - 3))
+		packet_accept(lexer, RTCM3_PACKET);
+	    else {
+		gpsd_report(LOG_IO, "RTCM3 data checksum failure, %0x\n", 
+			    crc24q_hash(lexer->inbuffer, 
+					 lexer->inbufptr-lexer->inbuffer - 3));
+		packet_discard(lexer);
+		lexer->state = GROUND_STATE;
+	    }
 	    break;
 	}
 #endif /* RTCM104V3_ENABLE */
diff --git a/packet_test.c b/packet_test.c
index b2be347b..9576a902 100644
--- a/packet_test.c
+++ b/packet_test.c
@@ -216,6 +216,18 @@ static struct map tests[] = {
 	.garbage_offset = 0,
 	.type = RTCM3_PACKET,                         
     },
+    {
+	.legend = "RTCM104V3 type 1005 packet with 4th byte garbled",
+	.test = {
+	    0xD3, 0x00, 0x13, 0x3F, 0xD7, 0xD3, 0x02, 0x02, 
+	    0x98, 0x0E, 0xDE, 0xEF, 0x34, 0xB4, 0xBD, 0x62,
+	    0xAC, 0x09, 0x41, 0x98, 0x6F, 0x33, 0x36, 0x0B, 
+	    0x98,
+	    },
+	.testlen = 25,
+	.garbage_offset = 0,
+	.type = BAD_PACKET,                         
+    },
 };
 /*@ +initallelements -charint +usedef @*/