Add support for stashed partial messages

The Spectratime iSync GRClok and LNRClok devices generate NMEA-format
status packets, which report the status of the GPS-disciplined
rubidium oscillator.  These packets are inserted randomly into the
middle of packets from the embedded u-Blox GPS.

Add support for a stash buffer, used to hold the initial portion of
interrupted NMEA packets.

5 files changed, 99 insertions, 2 deletions

diff --git a/SConstruct b/SConstruct
index 3b4cb89c..d136306f 100644
--- a/SConstruct
+++ b/SConstruct
@@ -146,6 +146,7 @@ boolopts = (
     ("timing",        False, "latency timing support"),
     ("control_socket", True,  "control socket for hotplug notifications"),
     ("systemd",       systemd, "systemd socket activation"),
+    ("stash",         True,  "enable stash buffer"),
     # Client-side options
     ("clientdebug",   True,  "client debugging support"),
     ("ncurses",       True,  "build with ncurses"),
diff --git a/gpsd.h-tail b/gpsd.h-tail
index 9516573d..c51cda98 100644
--- a/gpsd.h-tail
+++ b/gpsd.h-tail
@@ -187,6 +187,10 @@ struct gps_lexer_t {
     unsigned int json_depth;
     unsigned int json_after;
 #endif /* PASSTHROUGH_ENABLE */
+#ifdef STASH_ENABLE
+    unsigned char stashbuffer[MAX_PACKET_LENGTH];
+    size_t stashbuflen;
+#endif /* STASH_ENABLE */
 };
 
 extern void lexer_init(struct gps_lexer_t *);
diff --git a/packet.c b/packet.c
index 4ba40e52..bb820642 100644
--- a/packet.c
+++ b/packet.c
@@ -221,6 +221,9 @@ static bool nextstate(struct gps_lexer_t *lexer, unsigned char c)
     switch (lexer->state) {
     case GROUND_STATE:
 	n = 0;
+#ifdef STASH_ENABLE
+	lexer->stashbuflen = 0;
+#endif
 	if (c == '#') {
 	    lexer->state = COMMENT_BODY;
 	    break;
@@ -531,9 +534,13 @@ static bool nextstate(struct gps_lexer_t *lexer, unsigned char c)
 	else if (c == '\n')
 	    /* not strictly correct, but helps for interpreting logfiles */
 	    lexer->state = NMEA_RECOGNIZED;
-	else if (c == '$')
+	else if (c == '$') {
+#ifdef STASH_ENABLE
+	    (void) character_pushback(lexer, STASH_RECOGNIZED);
+#else
 	    (void) character_pushback(lexer, GROUND_STATE);
-	else if (!isprint(c))
+#endif
+	} else if (!isprint(c))
 	    (void) character_pushback(lexer, GROUND_STATE);
 	break;
     case NMEA_CR:
@@ -1421,6 +1428,14 @@ static bool nextstate(struct gps_lexer_t *lexer, unsigned char c)
 	    return character_pushback(lexer, GROUND_STATE);
 	break;
 #endif /* PASSTHROUGH_ENABLE */
+#ifdef STASH_ENABLE
+    case STASH_RECOGNIZED:
+	if (c == '$')
+	    lexer->state = NMEA_DOLLAR;
+	else
+	    return character_pushback(lexer, GROUND_STATE);
+	break;
+#endif /* STASH_ENABLE */
     }
 
     return true;	/* no pushback */
@@ -1469,6 +1484,52 @@ static void packet_discard(struct gps_lexer_t *lexer)
     }
 }
 
+#ifdef STASH_ENABLE
+static void packet_stash(struct gps_lexer_t *lexer)
+/* stash the input buffer up to current input pointer */
+{
+    size_t stashlen = lexer->inbufptr - lexer->inbuffer;
+
+    memcpy(lexer->stashbuffer, lexer->inbuffer, stashlen);
+    lexer->stashbuflen = stashlen;
+    if (lexer->errout.debug >= LOG_RAW+1) {
+	char scratchbuf[MAX_PACKET_LENGTH*2+1];
+	gpsd_log(&lexer->errout, LOG_RAW+1,
+		 "Packet stash of %zu = %s\n",
+		 stashlen,
+		 gpsd_packetdump(scratchbuf, sizeof(scratchbuf),
+				 (char *)lexer->stashbuffer,
+				 lexer->stashbuflen));
+    }
+}
+
+static void packet_unstash(struct gps_lexer_t *lexer)
+/* return stash to start of input buffer */
+{
+    size_t available = sizeof(lexer->inbuffer) - lexer->inbuflen;
+    size_t stashlen = lexer->stashbuflen;
+
+    if (stashlen <= available) {
+	memmove(lexer->inbuffer + stashlen, lexer->inbuffer, lexer->inbuflen);
+	memcpy(lexer->inbuffer, lexer->stashbuffer, stashlen);
+	lexer->inbuflen += stashlen;
+	lexer->stashbuflen = 0;
+	if (lexer->errout.debug >= LOG_RAW+1) {
+	    char scratchbuf[MAX_PACKET_LENGTH*2+1];
+	    gpsd_log(&lexer->errout, LOG_RAW+1,
+		     "Packet unstash of %zu, reconstructed is %zu = %s\n",
+		     stashlen, lexer->inbuflen,
+		     gpsd_packetdump(scratchbuf, sizeof(scratchbuf),
+				     (char *)lexer->inbuffer, lexer->inbuflen));
+	}
+    } else {
+	gpsd_log(&lexer->errout, LOG_ERROR,
+		 "Rejected too long unstash of %zu\n", stashlen);
+	lexer->stashbuflen = 0;
+    }
+}
+#endif /* STASH_ENABLE */
+
 static void character_discard(struct gps_lexer_t *lexer)
 /* shift the input buffer to discard one character and reread data */
 {
@@ -1589,6 +1650,10 @@ void packet_parse(struct gps_lexer_t *lexer)
 #endif /* AIVDM_ENABLE */
 		packet_accept(lexer, NMEA_PACKET);
 	    packet_discard(lexer);
+#ifdef STASH_ENABLE
+	    if (lexer->stashbuflen)
+		packet_unstash(lexer);
+#endif /* STASH_ENABLE */
 	    break;
 	}
 #endif /* NMEA0183_ENABLE */
@@ -2121,6 +2186,12 @@ void packet_parse(struct gps_lexer_t *lexer)
 	    break;
 	}
 #endif /* PASSTHROUGH_ENABLE */
+#ifdef STASH_ENABLE
+	else if (lexer->state == STASH_RECOGNIZED) {
+	    packet_stash(lexer);
+	    packet_discard(lexer);
+	}
+#endif /* STASH_ENABLE */
     }				/* while */
 }
 
@@ -2217,6 +2288,9 @@ void packet_reset(struct gps_lexer_t *lexer)
 #ifdef BINARY_ENABLE
     isgps_init(lexer);
 #endif /* BINARY_ENABLE */
+#ifdef STASH_ENABLE
+    lexer->stashbuflen = 0;
+#endif /* STASH_ENABLE */
 }
 
 
diff --git a/packet_states.h b/packet_states.h
index 8839b98e..d08b464d 100644
--- a/packet_states.h
+++ b/packet_states.h
@@ -206,4 +206,8 @@
    JSON_RECOGNIZED,     /* JSON packet recognized */
 #endif
 
+#ifdef STASH_ENABLE
+   STASH_RECOGNIZED,    /* stashable prefix recognized */
+#endif
+
 /* end of packet_states.h */
diff --git a/test_packet.c b/test_packet.c
index 3c27025c..156dfe08 100644
--- a/test_packet.c
+++ b/test_packet.c
@@ -62,6 +62,20 @@ static struct map singletests[] = {
 	.garbage_offset = 0,
 	.type = BAD_PACKET,
     },
+    {
+	.legend = "NMEA interspersed packet",
+	.test = "$GPZDA,112533.00,20,01,20$PTNTA,20000102173852,1,T4,,,6,1,0*32\r\n",
+	.testlen = 64,
+	.garbage_offset = 25,
+	.type = NMEA_PACKET,
+    },
+    {
+	.legend = "NMEA interrupted packet",
+	.test = "$GPZDA,112533.00,20,01,2016,00,00*67\r\n$GPZDA,112533.00,20,01,20$PTNTA,20000102173852,1,T4,,,6,1,0*32\r\n16,00,00*67\r\n",
+	.testlen = 115,
+	.garbage_offset = 0,
+	.type = NMEA_PACKET,
+    },
     /* SiRF tests */
     {
 	.legend = "SiRF WAAS version ID",