diff options
| -rw-r--r-- | driver_aivdm.c | 167 | ||||
| -rw-r--r-- | drivers.c | 167 | ||||
| -rw-r--r-- | gpsd.h-tail | 6 |
3 files changed, 170 insertions, 170 deletions
diff --git a/driver_aivdm.c b/driver_aivdm.c index 83c61daa..890de12c 100644 --- a/driver_aivdm.c +++ b/driver_aivdm.c @@ -61,7 +61,7 @@ static void from_sixbit(char *bitvec, uint start, int count, char *to) } /*@ +charint @*/ -static bool ais_binary_decode(struct ais_t *ais, +bool ais_binary_decode(struct ais_t *ais, const unsigned char *bits, size_t bitlen, struct ais_type24_queue_t *type24_queue) /* decode an AIS binary packet */ @@ -967,169 +967,4 @@ static bool ais_binary_decode(struct ais_t *ais, } /*@ -charint @*/ -/*@ -fixedformalarray -usedef -branchstate @*/ -bool aivdm_decode(const char *buf, size_t buflen, - struct aivdm_context_t ais_contexts[AIVDM_CHANNELS], - struct ais_t *ais, - int debug) -{ -#ifdef __UNUSED_DEBUG__ - char *sixbits[64] = { - "000000", "000001", "000010", "000011", "000100", - "000101", "000110", "000111", "001000", "001001", - "001010", "001011", "001100", "001101", "001110", - "001111", "010000", "010001", "010010", "010011", - "010100", "010101", "010110", "010111", "011000", - "011001", "011010", "011011", "011100", "011101", - "011110", "011111", "100000", "100001", "100010", - "100011", "100100", "100101", "100110", "100111", - "101000", "101001", "101010", "101011", "101100", - "101101", "101110", "101111", "110000", "110001", - "110010", "110011", "110100", "110101", "110110", - "110111", "111000", "111001", "111010", "111011", - "111100", "111101", "111110", "111111", - }; -#endif /* __UNUSED_DEBUG__ */ - int nfrags, ifrag, nfields = 0; - unsigned char *field[NMEA_MAX*2]; - unsigned char fieldcopy[NMEA_MAX*2+1]; - unsigned char *data, *cp; - unsigned char ch, pad; - struct aivdm_context_t *ais_context; - int i; - - if (buflen == 0) - return false; - - /* we may need to dump the raw packet */ - gpsd_report(LOG_PROG, "AIVDM packet length %zd: %s\n", buflen, buf); - - /* first clear the result, making sure we don't return garbage */ - memset(ais, 0, sizeof(*ais)); - - /* discard overlong sentences */ - if (strlen(buf) > sizeof(fieldcopy)-1) { - gpsd_report(LOG_ERROR, "overlong AIVDM packet.\n"); - return false; - } - - /* extract packet fields */ - (void)strlcpy((char *)fieldcopy, buf, sizeof(fieldcopy)); - field[nfields++] = (unsigned char *)buf; - for (cp = fieldcopy; - cp < fieldcopy + buflen; cp++) - if (*cp == ',') { - *cp = '\0'; - field[nfields++] = cp + 1; - } - - /* discard sentences with exiguous commas; catches run-ons */ - if (nfields < 7) { - gpsd_report(LOG_ERROR, "malformed AIVDM packet.\n"); - return false; - } - - switch (field[4][0]) { - /* FIXME: if fields[4] == "12", it doesn't detect the error */ - case '\0': - /* - * Apparently an empty channel is normal for AIVDO sentences, - * which makes sense as they don't come in over radio. This - * is going to break if there's ever an AIVDO type 24, though. - */ - if (strncmp((const char *)field[0], "!AIVDO", 6) != 0) - gpsd_report(LOG_ERROR, "invalid empty AIS channel. Assuming 'A'\n"); - ais_context = &ais_contexts[0]; - break; - case '1': - gpsd_report(LOG_ERROR, "invalid AIS channel 0x%0x '%c'. Assuming 'A'\n", - field[4][0], (field[4][0] != '\0' ? field[4][0]:' ')); - /*@fallthrough@*/ - case 'A': - ais_context = &ais_contexts[0]; - break; - case '2': - gpsd_report(LOG_ERROR, "invalid AIS channel '2'. Assuming 'B'.\n"); - /*@fallthrough@*/ - case 'B': - ais_context = &ais_contexts[1]; - break; - default: - gpsd_report(LOG_ERROR, "invalid AIS channel 0x%0X .\n", field[4][0]); - return false; - } - - nfrags = atoi((char *)field[1]); /* number of fragments to expect */ - ifrag = atoi((char *)field[2]); /* fragment id */ - data = field[5]; - pad = field[6][0]; /* number of padding bits */ - gpsd_report(LOG_PROG, "nfrags=%d, ifrag=%d, decoded_frags=%d, data=%s\n", - nfrags, ifrag, ais_context->decoded_frags, data); - - /* assemble the binary data */ - - /* check fragment ordering */ - if (ifrag != ais_context->decoded_frags + 1) { - gpsd_report(LOG_ERROR, "invalid fragment #%d received, expected #%d.\n", - ifrag, ais_context->decoded_frags + 1); - if (ifrag != 1) - return false; - /* else, ifrag==1: Just discard all that was previously decoded and - * simply handle that packet */ - ais_context->decoded_frags = 0; - } - if (ifrag == 1) { - (void)memset(ais_context->bits, '\0', sizeof(ais_context->bits)); - ais_context->bitlen = 0; - } - - /* wacky 6-bit encoding, shades of FIELDATA */ - /*@ +charint @*/ - for (cp = data; cp < data + strlen((char *)data); cp++) { - ch = *cp; - ch -= 48; - if (ch >= 40) - ch -= 8; -#ifdef __UNUSED_DEBUG__ - gpsd_report(LOG_RAW, "%c: %s\n", *cp, sixbits[ch]); -#endif /* __UNUSED_DEBUG__ */ - /*@ -shiftnegative @*/ - for (i = 5; i >= 0; i--) { - if ((ch >> i) & 0x01) { - ais_context->bits[ais_context->bitlen / 8] |= - (1 << (7 - ais_context->bitlen % 8)); - } - ais_context->bitlen++; - } - /*@ +shiftnegative @*/ - } - if (isdigit(pad)) - ais_context->bitlen -= (pad - '0'); /* ASCII assumption */ - /*@ -charint @*/ - - /* time to pass buffered-up data to where it's actually processed? */ - if (ifrag == nfrags) { - if (debug >= LOG_INF) { - size_t clen = (ais_context->bitlen + 7) / 8; - gpsd_report(LOG_INF, "AIVDM payload is %zd bits, %zd chars: %s\n", - ais_context->bitlen, clen, - gpsd_hexdump((char *)ais_context->bits, clen)); - } - - /* clear waiting fragments count */ - ais_context->decoded_frags = 0; - - /* decode the assembled binary packet */ - return ais_binary_decode(ais, - ais_context->bits, - ais_context->bitlen, - &ais_context->type24_queue); - } - - /* we're still waiting on another sentence */ - ais_context->decoded_frags++; - return false; -} -/*@ +fixedformalarray +usedef +branchstate @*/ - /* driver_aivdm.c ends here */ @@ -1194,10 +1194,175 @@ const struct gps_type_t mtk3301 = { #ifdef AIVDM_ENABLE /************************************************************************** * - * AIVDM + * AIVDM - ASCII armoring of binary AIS packets * **************************************************************************/ +/*@ -fixedformalarray -usedef -branchstate @*/ +static bool aivdm_decode(const char *buf, size_t buflen, + struct aivdm_context_t ais_contexts[AIVDM_CHANNELS], + struct ais_t *ais, + int debug) +{ +#ifdef __UNUSED_DEBUG__ + char *sixbits[64] = { + "000000", "000001", "000010", "000011", "000100", + "000101", "000110", "000111", "001000", "001001", + "001010", "001011", "001100", "001101", "001110", + "001111", "010000", "010001", "010010", "010011", + "010100", "010101", "010110", "010111", "011000", + "011001", "011010", "011011", "011100", "011101", + "011110", "011111", "100000", "100001", "100010", + "100011", "100100", "100101", "100110", "100111", + "101000", "101001", "101010", "101011", "101100", + "101101", "101110", "101111", "110000", "110001", + "110010", "110011", "110100", "110101", "110110", + "110111", "111000", "111001", "111010", "111011", + "111100", "111101", "111110", "111111", + }; +#endif /* __UNUSED_DEBUG__ */ + int nfrags, ifrag, nfields = 0; + unsigned char *field[NMEA_MAX*2]; + unsigned char fieldcopy[NMEA_MAX*2+1]; + unsigned char *data, *cp; + unsigned char ch, pad; + struct aivdm_context_t *ais_context; + int i; + + if (buflen == 0) + return false; + + /* we may need to dump the raw packet */ + gpsd_report(LOG_PROG, "AIVDM packet length %zd: %s\n", buflen, buf); + + /* first clear the result, making sure we don't return garbage */ + memset(ais, 0, sizeof(*ais)); + + /* discard overlong sentences */ + if (strlen(buf) > sizeof(fieldcopy)-1) { + gpsd_report(LOG_ERROR, "overlong AIVDM packet.\n"); + return false; + } + + /* extract packet fields */ + (void)strlcpy((char *)fieldcopy, buf, sizeof(fieldcopy)); + field[nfields++] = (unsigned char *)buf; + for (cp = fieldcopy; + cp < fieldcopy + buflen; cp++) + if (*cp == (unsigned char)',') { + *cp = '\0'; + field[nfields++] = cp + 1; + } + + /* discard sentences with exiguous commas; catches run-ons */ + if (nfields < 7) { + gpsd_report(LOG_ERROR, "malformed AIVDM packet.\n"); + return false; + } + + switch (field[4][0]) { + /* FIXME: if fields[4] == "12", it doesn't detect the error */ + case '\0': + /* + * Apparently an empty channel is normal for AIVDO sentences, + * which makes sense as they don't come in over radio. This + * is going to break if there's ever an AIVDO type 24, though. + */ + if (strncmp((const char *)field[0], "!AIVDO", 6) != 0) + gpsd_report(LOG_ERROR, "invalid empty AIS channel. Assuming 'A'\n"); + ais_context = &ais_contexts[0]; + break; + case '1': + gpsd_report(LOG_ERROR, "invalid AIS channel 0x%0x '%c'. Assuming 'A'\n", + field[4][0], (field[4][0] != (unsigned char)'\0' ? field[4][0]:' ')); + /*@fallthrough@*/ + case 'A': + ais_context = &ais_contexts[0]; + break; + case '2': + gpsd_report(LOG_ERROR, "invalid AIS channel '2'. Assuming 'B'.\n"); + /*@fallthrough@*/ + case 'B': + ais_context = &ais_contexts[1]; + break; + default: + gpsd_report(LOG_ERROR, "invalid AIS channel 0x%0X .\n", field[4][0]); + return false; + } + + nfrags = atoi((char *)field[1]); /* number of fragments to expect */ + ifrag = atoi((char *)field[2]); /* fragment id */ + data = field[5]; + pad = field[6][0]; /* number of padding bits */ + gpsd_report(LOG_PROG, "nfrags=%d, ifrag=%d, decoded_frags=%d, data=%s\n", + nfrags, ifrag, ais_context->decoded_frags, data); + + /* assemble the binary data */ + + /* check fragment ordering */ + if (ifrag != ais_context->decoded_frags + 1) { + gpsd_report(LOG_ERROR, "invalid fragment #%d received, expected #%d.\n", + ifrag, ais_context->decoded_frags + 1); + if (ifrag != 1) + return false; + /* else, ifrag==1: Just discard all that was previously decoded and + * simply handle that packet */ + ais_context->decoded_frags = 0; + } + if (ifrag == 1) { + (void)memset(ais_context->bits, '\0', sizeof(ais_context->bits)); + ais_context->bitlen = 0; + } + + /* wacky 6-bit encoding, shades of FIELDATA */ + /*@ +charint @*/ + for (cp = data; cp < data + strlen((char *)data); cp++) { + ch = *cp; + ch -= 48; + if (ch >= 40) + ch -= 8; +#ifdef __UNUSED_DEBUG__ + gpsd_report(LOG_RAW, "%c: %s\n", *cp, sixbits[ch]); +#endif /* __UNUSED_DEBUG__ */ + /*@ -shiftnegative @*/ + for (i = 5; i >= 0; i--) { + if ((ch >> i) & 0x01) { + ais_context->bits[ais_context->bitlen / 8] |= + (1 << (7 - ais_context->bitlen % 8)); + } + ais_context->bitlen++; + } + /*@ +shiftnegative @*/ + } + if (isdigit(pad)) + ais_context->bitlen -= (pad - '0'); /* ASCII assumption */ + /*@ -charint @*/ + + /* time to pass buffered-up data to where it's actually processed? */ + if (ifrag == nfrags) { + if (debug >= LOG_INF) { + size_t clen = (ais_context->bitlen + 7) / 8; + gpsd_report(LOG_INF, "AIVDM payload is %zd bits, %zd chars: %s\n", + ais_context->bitlen, clen, + gpsd_hexdump((char *)ais_context->bits, clen)); + } + + /* clear waiting fragments count */ + ais_context->decoded_frags = 0; + + /* decode the assembled binary packet */ + return ais_binary_decode(ais, + ais_context->bits, + ais_context->bitlen, + &ais_context->type24_queue); + } + + /* we're still waiting on another sentence */ + ais_context->decoded_frags++; + return false; +} +/*@ +fixedformalarray +usedef +branchstate @*/ + static gps_mask_t aivdm_analyze(struct gps_device_t *session) { if (session->packet.type == AIVDM_PACKET) { diff --git a/gpsd.h-tail b/gpsd.h-tail index d5af7510..f820bb0d 100644 --- a/gpsd.h-tail +++ b/gpsd.h-tail @@ -800,9 +800,9 @@ extern /*@observer@*/const char *gpsd_maskdump(gps_mask_t); /* exceptional driver methods */ extern bool ubx_write(struct gps_device_t *, unsigned int, unsigned int, /*@null@*/unsigned char *, size_t); -extern bool aivdm_decode(const char *, size_t, - struct aivdm_context_t [], - struct ais_t *, int); +extern bool ais_binary_decode(struct ais_t *, + const unsigned char *, size_t, + struct ais_type24_queue_t *); /* debugging apparatus for the client library */ #ifdef CLIENTDEBUG_ENABLE |