packet.c

来自「gpsd, a popular GPS daemon.」· C语言 代码 · 共 1,271 行 · 第 1/3 页

	if (lexer->state == GROUND_STATE) {
	    character_discard(lexer);
	}
	else if (lexer->state == COMMENT_RECOGNIZED) {
	    packet_accept(lexer, COMMENT_PACKET);
	    packet_discard(lexer);
	    lexer->state = GROUND_STATE;
	    break;
	}
#ifdef NMEA_ENABLE
	else if (lexer->state == NMEA_RECOGNIZED) {
	    bool checksum_ok = true;
	    char csum[3];
	    char *trailer = (char *)lexer->inbufptr-5;
	    if (*trailer == '*') {
		unsigned int n, crc = 0;
		for (n = 1; (char *)lexer->inbuffer + n < trailer; n++)
		    crc ^= lexer->inbuffer[n];
		(void)snprintf(csum, sizeof(csum), "%02X", crc);
		checksum_ok = (csum[0]==toupper(trailer[1])
				&& csum[1]==toupper(trailer[2]));
	    }
	    if (checksum_ok)
		packet_accept(lexer, NMEA_PACKET);
	    else
		lexer->state = GROUND_STATE;
	    packet_discard(lexer);
	    break;
	}
#endif /* NMEA_ENABLE */
#ifdef SIRF_ENABLE
	else if (lexer->state == SIRF_RECOGNIZED) {
	    unsigned char *trailer = lexer->inbufptr-4;
	    unsigned int checksum = (unsigned)((trailer[0] << 8) | trailer[1]);
	    unsigned int n, crc = 0;
	    for (n = 4; n < (unsigned)(trailer - lexer->inbuffer); n++)
		crc += (int)lexer->inbuffer[n];
	    crc &= 0x7fff;
	    if (checksum == crc)
		packet_accept(lexer, SIRF_PACKET);
	    else
		lexer->state = GROUND_STATE;
	    packet_discard(lexer);
	    break;
	}
#endif /* SIRF_ENABLE */
#if defined(TSIP_ENABLE) || defined(GARMIN_ENABLE)
	else if (lexer->state == TSIP_RECOGNIZED) {
	    size_t packetlen = lexer->inbufptr - lexer->inbuffer;
	    if ( packetlen < 5) {
		lexer->state = GROUND_STATE;
	    } else {
		unsigned int pkt_id, len;
		size_t n;
#ifdef GARMIN_ENABLE
		unsigned int ch, chksum;
		n = 0;
		/*@ +charint */
#ifdef TSIP_ENABLE
		/* shortcut garmin */
		if (TSIP_PACKET == lexer->type)
			goto not_garmin;
#endif /* TSIP_ENABLE */
		if (lexer->inbuffer[n++] != DLE)
		    goto not_garmin;
		pkt_id = lexer->inbuffer[n++]; /* packet ID */
		len = lexer->inbuffer[n++];
		chksum = len + pkt_id;
		if (len == DLE) {
		    if (lexer->inbuffer[n++] != DLE)
			goto not_garmin;
		}
		for (; len > 0; len--) {
		    chksum += lexer->inbuffer[n];
		    if (lexer->inbuffer[n++] == DLE) {
			if (lexer->inbuffer[n++] != DLE)
			    goto not_garmin;
		    }
		}
		/* check sum byte */
		ch = lexer->inbuffer[n++];
		chksum += ch;
		if (ch == DLE) {
		    if (lexer->inbuffer[n++] != DLE)
			goto not_garmin;
		}
		if (lexer->inbuffer[n++] != DLE)
		    goto not_garmin;
		if (lexer->inbuffer[n++] != ETX)
		    goto not_garmin;
		/*@ +charint */
		chksum &= 0xff;
		if (chksum) {
		    gpsd_report(LOG_IO,
				"Garmin checksum failed: %02x!=0\n",chksum);
		    goto not_garmin;
		}
		/* Debug
		   gpsd_report(LOG_IO, "Garmin n= %#02x\n %s\n", n,
		   gpsd_hexdump(lexer->inbuffer, packetlen));
		*/
		packet_accept(lexer, GARMIN_PACKET);
		packet_discard(lexer);
		break;
	    not_garmin:;
		gpsd_report(LOG_RAW+1,"Not a Garmin packet\n");
#endif /* GARMIN_ENABLE */
#ifdef TSIP_ENABLE
		/* check for some common TSIP packet types:
		 * 0x13, TSIP Parsing Error Notification
		 * 0x41, GPS time, data length 10
		 * 0x42, Single Precision Fix, data length 16
		 * 0x43, Velocity Fix, data length 20
		 * 0x45, Software Version Information, data length 10
		 * 0x46, Health of Receiver, data length 2
		 * 0x4a, LLA Position, data length 20
		 * 0x4b, Machine Code Status, data length 3
		 * 0x56, Velocity Fix (ENU), data length 20
		 * 0x5c, Satellite Tracking Status, data length 24
		 * 0x6d, All-In-View Satellite Selection, data length 16+numSV
		 * 0x82, Differential Position Fix Mode, data length 1
		 * 0x83, Double Precision XYZ, data length 36
		 * 0x84, Double Precision LLA, data length 36
		 * 0xbb, GPS Navigation Configuration
		 * 0xbc, Receiver Port Configuration
		 *
		 * <DLE>[pkt id] [data] <DLE><ETX>
		 */
		/*@ +charint @*/
		pkt_id = lexer->inbuffer[1]; /* packet ID */
		if (!((0x13 == pkt_id) || (0xbb == pkt_id) || (0xbc == pkt_id)) &&
		    ((0x41 > pkt_id) || (0x8f < pkt_id))) {
		    gpsd_report(LOG_IO, "Packet ID 0x%02x out of range for TSIP\n", pkt_id);
		    goto not_tsip;
		}
		/*@ -ifempty */
		if ((0x13 == pkt_id) && (0x01 <= packetlen))
		    /* pass */;
		else if ((0x41 == pkt_id) && (0x0e == packetlen))
		    /* pass */;
		else if ((0x42 == pkt_id) && (0x14 == packetlen ))
		    /* pass */;
		else if ((0x43 == pkt_id) && (0x18 == packetlen))
		    /* pass */;
		else if ((0x45 == pkt_id) && (0x0e == packetlen))
		    /* pass */;
		else if ((0x46 == pkt_id) && (0x06 == packetlen))
		    /* pass */;
		else if ((0x4a == pkt_id) && (0x18 == packetlen))
		    /* pass */;
		else if ((0x4b == pkt_id) && (0x07 == packetlen))
		    /* pass */;
		else if ((0x55 == pkt_id) && (0x08 == packetlen))
		    /* pass */;
		else if ((0x56 == pkt_id) && (0x18 == packetlen))
		    /* pass */;
		else if ((0x5c == pkt_id) && ((0x1c <= packetlen) && (0x1e >= packetlen)))
		    /* pass */;
		else if ((0x6d == pkt_id) && ((0x14 <= packetlen) && (0x20 >= packetlen)))
		    /* pass */;
		else if ((0x82 == pkt_id) && (0x05 == packetlen))
		    /* pass */;
		else if ((0x84 == pkt_id) && ((0x28 <= packetlen) && (0x29 >= packetlen)))
		    /* pass */;
		else if ((0x8e == pkt_id))
		    /* pass */;
		else if ((0x8f == pkt_id))
		    /* pass */;
		else if ((0xbb == pkt_id) && (0x2c == packetlen))
		    /* pass */;
		else {
		    gpsd_report(LOG_IO,
			"TSIP REJECT pkt_id = %#02x, packetlen= %#02x\n",
			pkt_id, packetlen);
		    goto not_tsip;
		}
		/* Debug */
		gpsd_report(LOG_RAW,
		    "TSIP pkt_id = %#02x, packetlen= %#02x\n",
		    pkt_id, packetlen);
		/*@ -charint +ifempty @*/
		packet_accept(lexer, TSIP_PACKET);
		packet_discard(lexer);
		break;
	    not_tsip:
		gpsd_report(LOG_RAW+1,"Not a TSIP packet\n");
		/*
		 * More attempts to recognize ambiguous TSIP-like
		 * packet types could go here.
		 */
		lexer->state = GROUND_STATE;
		packet_discard(lexer);
		break;
#endif /* TSIP_ENABLE */
	    }
	}
#endif /* TSIP_ENABLE || GARMIN_ENABLE */
#ifdef ZODIAC_ENABLE
	else if (lexer->state == ZODIAC_RECOGNIZED) {
	    short len, n, sum;
	    len = getword(2);
	    for (n = sum = 0; n < len; n++)
		sum += getword(5+n);
	    sum *= -1;
	    if (len == 0 || sum == getword(5 + len)) {
		packet_accept(lexer, ZODIAC_PACKET);
	    } else {
		gpsd_report(LOG_IO,
		    "Zodiac data checksum 0x%hx over length %hd, expecting 0x%hx\n",
			sum, len, getword(5 + len));
		lexer->state = GROUND_STATE;
	    }
	    packet_discard(lexer);
	    break;
	}
#endif /* ZODIAC_ENABLE */
#ifdef UBX_ENABLE
	else if (lexer->state == UBX_RECOGNIZED) {
	    /* UBX use a TCP like checksum */
	    int n, len;
	    unsigned char ck_a = (unsigned char)0;
	    unsigned char ck_b = (unsigned char)0;
	    len = lexer->inbufptr - lexer->inbuffer;
	    gpsd_report(LOG_IO, "UBX: len %d\n", len);
	    for (n = 2; n < (len-2); n++) {
		ck_a += lexer->inbuffer[n];
		ck_b += ck_a;
	    }
	    if (ck_a == lexer->inbuffer[len-2] &&
		ck_b == lexer->inbuffer[len-1])
		packet_accept(lexer, UBX_PACKET);
	    else {
		gpsd_report(LOG_IO,
		    "UBX checksum 0x%02hhx%02hhx over length %hd,"\
		    " expecting 0x%02hhx%02hhx (type 0x%02hhx%02hhx)\n",
			ck_a,
			ck_b,
			len,
			lexer->inbuffer[len-2],
			lexer->inbuffer[len-1],
			lexer->inbuffer[2],
			lexer->inbuffer[3]);
		lexer->state = GROUND_STATE;
	    }
	    packet_discard(lexer);
	    break;
	}
#endif /* UBX_ENABLE */
#ifdef EVERMORE_ENABLE
	else if (lexer->state == EVERMORE_RECOGNIZED) {
	    unsigned int n, crc, checksum, len;
	    n = 0;
	    /*@ +charint */
	    if (lexer->inbuffer[n++] != DLE)
		goto not_evermore;
	    if (lexer->inbuffer[n++] != STX)
		goto not_evermore;
	    len = lexer->inbuffer[n++];
	    if (len == DLE) {
		if (lexer->inbuffer[n++] != DLE)
		    goto not_evermore;
	    }
	    len -= 2;
	    crc = 0;
	    for (; len > 0; len--) {
		crc += lexer->inbuffer[n];
		if (lexer->inbuffer[n++] == DLE) {
		    if (lexer->inbuffer[n++] != DLE)
			goto not_evermore;
		}
	    }
	    checksum = lexer->inbuffer[n++];
	    if (checksum == DLE) {
		if (lexer->inbuffer[n++] != DLE)
		    goto not_evermore;
	    }
	    if (lexer->inbuffer[n++] != DLE)
		goto not_evermore;
	    if (lexer->inbuffer[n++] != ETX)
		goto not_evermore;
	    crc &= 0xff;
	    if (crc != checksum) {
		gpsd_report(LOG_IO,
			    "EverMore checksum failed: %02x != %02x\n",
			    crc, checksum);
		goto not_evermore;
	    }
	    /*@ +charint */
	    packet_accept(lexer, EVERMORE_PACKET);
	    packet_discard(lexer);
	    break;
	not_evermore:
	    lexer->state = GROUND_STATE;
	    packet_discard(lexer);
	    break;
	}
#endif /* EVERMORE_ENABLE */
/* XXX CSK */
#ifdef ITRAX_ENABLE
#define getib(j) ((u_int8_t)lexer->inbuffer[(j)])
#define getiw(i) ((u_int16_t)(((u_int16_t)getib((i)+1) << 8) | (u_int16_t)getib((i))))

	else if (lexer->state == ITALK_RECOGNIZED) {
	    volatile u_int16_t len, n, csum, xsum, tmpw;
	    volatile u_int32_t tmpdw;

	    /* number of words */
	    len = (unsigned short)(lexer->inbuffer[6] &0xff);

	    /*@ -type @*/
	    /* initialize all my registers */
	    csum = tmpw = tmpdw = 0;
	    /* expected checksum */
	    xsum = getiw(7+2*len);

	    for (n = 0; n < len; n++){
		tmpw = getiw(7 + 2*n);
		tmpdw = (csum + 1) * (tmpw + n);
		csum ^= (tmpdw & 0xffff) ^ ((tmpdw >>16) & 0xffff);
	    }
	    /*@ +type @*/
	    if (len == 0 || csum == xsum)
		packet_accept(lexer, ITALK_PACKET);
	    else {
		gpsd_report(LOG_IO,
		    "ITALK: checksum failed - "
		    "type 0x%02x expected 0x%04x got 0x%04x\n",
		    lexer->inbuffer[4], xsum, csum);
		lexer->state = GROUND_STATE;
	    }
	    packet_discard(lexer);
	    break;
	}
#undef getiw
#undef getib
#endif /* ITRAX_ENABLE */
#ifdef NAVCOM_ENABLE
	else if (lexer->state == NAVCOM_RECOGNIZED) {
	    /* By the time we got here we know checksum is OK */
	    packet_accept(lexer, NAVCOM_PACKET);
	    packet_discard(lexer);
	    break;
	}
#endif /* NAVCOM_ENABLE */
#ifdef RTCM104_ENABLE
	else if (lexer->state == RTCM_RECOGNIZED) {
	    /*
	     * RTCM packets don't have checksums.  The six bits of parity
	     * per word and the preamble better be good enough.
	     */
	    packet_accept(lexer, RTCM_PACKET);
	    lexer->state = RTCM_SYNC_STATE;
	    packet_discard(lexer);
	    break;
	}
#endif /* RTCM104_ENABLE */
    } /* while */

    return (ssize_t)fix;
}
#undef getword

ssize_t packet_get(int fd, struct gps_packet_t *lexer)
/* grab a packet; returns either BAD_PACKET or the length */
{
    ssize_t recvd;

    /*@ -modobserver @*/
    recvd = read(fd, lexer->inbuffer+lexer->inbuflen,
			sizeof(lexer->inbuffer)-(lexer->inbuflen));
#ifdef STATEDEBUG
    gpsd_report(LOG_RAW+1, "%d raw bytes read: %s\n",
		recvd, gpsd_hexdump(lexer->inbuffer+lexer->inbuflen, recvd));
#endif /* STATEDEBUG */
    /*@ +modobserver @*/
    if (recvd == -1) {
	if ((errno == EAGAIN) || (errno == EINTR)) {
	    return 0;
	} else {
#ifdef STATEDEBUG
	    gpsd_report(LOG_RAW+1, "errno: %s\n", strerror(errno));
#endif /* STATEDEBUG */
	    return BAD_PACKET;
	}
    }

    if (recvd == 0)
	return 0;
    return packet_parse(lexer, (size_t)recvd);
}

void packet_reset(struct gps_packet_t *lexer)
/* return the packet machine to the ground state */
{
    lexer->type = BAD_PACKET;
    lexer->state = GROUND_STATE;
    lexer->inbuflen = 0;
    lexer->inbufptr = lexer->inbuffer;
#ifdef BINARY_ENABLE
    isgps_init(lexer);
#endif /* BINARY_ENABLE */
}


#ifdef __UNUSED__
void packet_pushback(struct gps_packet_t *lexer)
/* push back the last packet grabbed */
{
    if (lexer->outbuflen + lexer->inbuflen < MAX_PACKET_LENGTH) {
	memmove(lexer->inbuffer+lexer->outbuflen,
		lexer->inbuffer,
		lexer->inbuflen);
	memmove(lexer->inbuffer,
		lexer->outbuffer,
		lexer->outbuflen);
	lexer->inbuflen += lexer->outbuflen;
	lexer->inbufptr += lexer->outbuflen;
	lexer->outbuflen = 0;
    }
}
#endif /* __UNUSED */