From: Fabrice Desclaux <serpilliere@droids-corp.org>
Date: Mon, 20 Feb 2012 20:35:20 +0000 (+0100)
Subject: xbee_proto: fix parsing hdr len
X-Git-Url: http://git.droids-corp.org/?a=commitdiff_plain;h=32fabd6d0e6718e2e5a85ef8b40dc37c20f20e31;p=protos%2Fxbee-avr.git

xbee_proto: fix parsing hdr len
---

diff --git a/xbee_proto.c b/xbee_proto.c
index 7072a6c..bd284dd 100644
--- a/xbee_proto.c
+++ b/xbee_proto.c
@@ -116,6 +116,7 @@ static int xbee_proto_parse_frame(struct xbee_dev *dev)
 {
 	void *buf = dev->frame;
 	uint8_t len = dev->frame_len;
+	uint8_t hdrlen;
 	struct xbee_hdr *hdr = buf;
 	int i;
 	uint8_t cksum = 0;
@@ -123,8 +124,19 @@ static int xbee_proto_parse_frame(struct xbee_dev *dev)
 
 	dev->stats.rx_frame++;
 
+	switch (hdr->type) {
+		case XBEE_TYPE_MODEM_STATUS:
+		case XBEE_TYPE_RECV:
+		case XBEE_TYPE_EXPL_RECV:
+			hdrlen = sizeof(struct xbee_hdr) - 1; /* no frame ID */
+			break;
+		default:
+			hdrlen = sizeof(struct xbee_hdr);
+			break;
+	}
+
 	/* check frame len */
-	if (len < (sizeof(*hdr) + 1)) {
+	if (len < (hdrlen + 1)) {
 		dev->stats.rx_frame_too_small++;
 		fprintf(stderr, "Frame too small\n");
 		return -1;
@@ -192,8 +204,8 @@ static int xbee_proto_parse_frame(struct xbee_dev *dev)
 	/* execute the callback if any */
 	if (dev->channel[channel].rx_cb != NULL)
 		dev->channel[channel].rx_cb(dev, channel, hdr->type,
-					    buf + sizeof(struct xbee_hdr),
-					    len - sizeof(struct xbee_hdr) - 1,
+					    buf + hdrlen,
+					    len - hdrlen - 1,
 					    dev->channel[channel].arg);
 
 	return 0;
@@ -292,6 +304,9 @@ void xbee_proto_rx(struct xbee_dev *dev)
 			continue;
 		}
 
+		if (dev->frame_len == 0 && c != XBEE_DELIMITER)
+			continue;
+
 		dev->frame[dev->frame_len++] = c;
 
 		/* not enough data to read len */
@@ -301,6 +316,12 @@ void xbee_proto_rx(struct xbee_dev *dev)
 		framelen = ntohs(hdr->len);
 		framelen += 4; /* 1 for delimiter, 2 for len, 1 for cksum */
 
+		/* frame too long XXX stats */
+		if (framelen >= XBEE_MAX_FRAME_LEN) {
+			dev->frame_len = 0;
+			continue;
+		}
+
 		/* not enough data */
 		if (dev->frame_len < framelen)
 			continue;