[gpsd-dev] [PATCH 2/4] [stash] Add support for stashed partial messages

[Michael Brown]

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.

Signed-off-by: Michael Brown <address@hidden>
---
 SConstruct      |  1 +
 gpsd.h-tail     |  4 +++
 packet.c        | 78 +++++++++++++++++++++++++++++++++++++++++++++++++++++++--
 packet_states.h |  4 +++
 test_packet.c   | 14 +++++++++++
 5 files changed, 99 insertions(+), 2 deletions(-)

diff --git a/SConstruct b/SConstruct
index c376f62..bf1f0b8 100644
--- a/SConstruct
+++ b/SConstruct
@@ -142,6 +142,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 68ea3ab..b522552 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 4ba40e5..bb82064 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 8839b98..d08b464 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 3c27025..156dfe0 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",
-- 
2.3.8