Garmin serial binary is very similar to TSIP binary.

This modifies the TSIP binary detection to also detect Garmin serial
binary.  Nothing is done yet with the packet after it is identified.

4 files changed, 80 insertions, 12 deletions

diff --git a/drivers.c b/drivers.c
index 01889729..d6dbed08 100644
--- a/drivers.c
+++ b/drivers.c
@@ -46,6 +46,17 @@ gps_mask_t nmea_parse_input(struct gps_device_t *session)
 #else
 	return 0;
 #endif /* EVERMORE_ENABLE */
+    } else if (session->packet_type == GARMIN_PACKET) {
+	gpsd_report(2, "Garmin packet seen when NMEA expected.\n");
+#ifdef GARMIN_ENABLE_UNUSED
+	/* we might never see a trigger, have this as a backstop */
+	/*
+	(void)gpsd_switch_driver(session, "EverMore binary");
+	return evermore_parse(session, session->outbuffer, session->outbuflen);
+	*/
+#else
+	return 0;
+#endif /* GARMIN_ENABLE */
     } else if (session->packet_type == NMEA_PACKET) {
 	gps_mask_t st = 0;
 	gpsd_report(2, "<= GPS: %s", session->outbuffer);
@@ -169,7 +180,7 @@ static void garmin_nmea_initializer(struct gps_device_t *session)
     (void)nmea_send(session->gpsdata.gps_fd, "$PGRMI,,,,,,,R");
     /* probe for Garmin serial binary by trying to Product Data request */
     /* DLE, PktID, Size, data (none), CHksum, DLE, ETX 
-    (void)gpsd_write(session, "\x10\xFE\x00\xf1\x10\x03", 6); */
+    (void)gpsd_write(session, "\x10\xFE\x00\x02\x10\x03", 6); */
 #endif /* GARMIN_ENABLE */
 }
 
diff --git a/libgpsd_core.c b/libgpsd_core.c
index bffab6b0..a6f55795 100644
--- a/libgpsd_core.c
+++ b/libgpsd_core.c
@@ -528,6 +528,11 @@ gps_mask_t gpsd_poll(struct gps_device_t *session)
 		(void)gpsd_switch_driver(session, "Trimble TSIP");
 		break;
 #endif /* TSIP_ENABLE */
+#ifdef GARMIN_ENABLE
+	    case GARMIN_PACKET:
+		(void)gpsd_switch_driver(session, "Garmin binary");
+		break;
+#endif /* GARMIN_ENABLE */
 #ifdef NMEA_ENABLE
 	    case NMEA_PACKET:
 		(void)gpsd_switch_driver(session, "Generic NMEA");
diff --git a/packet.c b/packet.c
index ff249614..710d0fe3 100644
--- a/packet.c
+++ b/packet.c
@@ -94,12 +94,12 @@ static void nextstate(struct gps_device_t *session, unsigned char c)
 	    break;
 	}
 #endif /* SIRF_ENABLE */
-#if defined(TSIP_ENABLE) || defined(EVERMORE_ENABLE)
+#if defined(TSIP_ENABLE) || defined(EVERMORE_ENABLE) || defined(GARMIN_ENABLE)
         if (c == 0x10) {
 	    session->packet_state = DLE_LEADER;
 	    break;
 	}
-#endif /* defined(TSIP_ENABLE) || defined(EVERMORE_ENABLE) */
+#endif /* TSIP_ENABLE || EVERMORE_ENABLE || GARMIN_ENABLE */
 #ifdef TRIPMATE_ENABLE
         if (c == 'A') {
 #ifdef RTCM104_ENABLE
@@ -340,14 +340,15 @@ static void nextstate(struct gps_device_t *session, unsigned char c)
 	    session->packet_state = GROUND_STATE;
 	break;
 #endif /* SIRF_ENABLE */
-#if defined(TSIP_ENABLE) || defined(EVERMORE_ENABLE)
+#if defined(TSIP_ENABLE) || defined(EVERMORE_ENABLE) || defined(GARMIN_ENABLE)
     case DLE_LEADER:
 #ifdef EVERMORE_ENABLE
 	if (c == 0x02)
 	    session->packet_state = EVERMORE_LEADER_2;
 	else
 #endif /* EVERMORE_ENABLE */
-#ifdef TSIP_ENABLE
+#if defined(TSIP_ENABLE) || defined(GARMIN_ENABLE)
+	/* garmin is special case of TSIP */
 	/* check last because there's no checksum */
 	if (c >= 0x13)
 	    session->packet_state = TSIP_PAYLOAD;
@@ -355,7 +356,7 @@ static void nextstate(struct gps_device_t *session, unsigned char c)
 #endif /* TSIP_ENABLE */
 	    session->packet_state = GROUND_STATE;
 	break;
-#endif /* defined(TSIP_ENABLE) || defined(EVERMORE_ENABLE) */
+#endif /* TSIP_ENABLE || EVERMORE_ENABLE || GARMIN_ENABLE */
 #ifdef ZODIAC_ENABLE
     case ZODIAC_EXPECTED:
     case ZODIAC_RECOGNIZED:
@@ -679,15 +680,63 @@ ssize_t packet_parse(struct gps_device_t *session, size_t fix)
 	    packet_discard(session);
             break;
 #endif /* SIRF_ENABLE */
-#ifdef TSIP_ENABLE
+#if defined(TSIP_ENABLE) || defined(GARMIN_ENABLE)
 	} else if (session->packet_state == TSIP_RECOGNIZED) {
-	    if ((session->inbufptr - session->inbuffer) >= 4)
+	    if ((session->inbufptr - session->inbuffer) >= 4) {
+#ifdef GARMIN_ENABLE
+              unsigned int n, chksum, c, len;
+	      bool ok = false;
+
+	      n = 0;
+	      /*@ +charint */
+	      do {
+
+	         if (session->inbuffer[n++] != 0x10) break;
+	         chksum = session->inbuffer[n++]; // skip pkt ID */
+	         len = session->inbuffer[n++];
+		 chksum += len;
+	         if (len == 0x10) {
+		    if (session->inbuffer[n++] != 0x10) break;
+	         }
+	         for (; len > 0; len--) {
+		    chksum += session->inbuffer[n];
+		    gpsd_report(4, "Garmin char : %02x\n",
+			    session->inbuffer[n]);
+		    if (session->inbuffer[n++] == 0x10) {
+		       if (session->inbuffer[n++] != 0x10) break;
+		    }
+	         }
+	         if (len > 0) break;
+		 /* check sum byte */
+	         c = session->inbuffer[n++];
+	         chksum += c;
+	         if (c == 0x10) {
+		    if (session->inbuffer[n++] != 0x10) break;
+	         }
+	         if (session->inbuffer[n++] != 0x10) break;
+	         if (session->inbuffer[n++] != 0x03) break;
+	         chksum &= 0xff;
+
+	         if ( chksum) {
+		    gpsd_report(4, "Garmin checksum failed: %02x != 0\n"
+			  , chksum);
+		    break;
+	         }
+	         ok = true;
+	      } while (0);
+	      /*@ +charint */
+
+	      gpsd_report(4, "Garmin n= %02x\n", n);
+	      if (ok)
+		  packet_accept(session, GARMIN_PACKET);
+	      else 
+#endif /* GARMIN_ENABLE */
 		packet_accept(session, TSIP_PACKET);
-	    else
+	    } else
 		session->packet_state = GROUND_STATE;
 	    packet_discard(session);
             break;
-#endif /* TSIP_ENABLE */
+#endif /* TSIP_ENABLE || GARMIN_ENABLE */
 #ifdef ZODIAC_ENABLE
 	} else if (session->packet_state == ZODIAC_RECOGNIZED) {
 	    short len, n, sum;
diff --git a/packet_states.h b/packet_states.h
index 29d8802a..8fad28f0 100644
--- a/packet_states.h
+++ b/packet_states.h
@@ -85,16 +85,19 @@
  * be recognized, that will be preferred.
  */
 
-#ifdef TSIP_ENABLE
+#if defined(TSIP_ENABLE) || defined(GARMIN_ENABLE)
    TSIP_LEADER,		/* a DLE after having seen TSIP data */
    TSIP_PAYLOAD,	/* we're in TSIP payload */
    TSIP_DLE,		/* we've seen a DLE in TSIP payload */
    TSIP_RECOGNIZED,	/* found end of the TSIP packet */
-#endif /* TSIP_ENABLE */
+   GARMIN_RECOGNIZED,	/* found end of Garmin packet */
+#endif /* TSIP_ENABLE GARMIN_ENABLE */
 
 #ifdef RTCM104_ENABLE
    RTCM_SYNC_STATE,	/* we have sync lock */
    RTCM_SKIP_STATE,	/* we have sync lock, but this character is bad */
    RTCM_RECOGNIZED,	/* we have an RTCM packet */
 #endif /* RTCM104_ENABLE */
+
+
 /* end of packet_states.h */