X-Git-Url: http://git.droids-corp.org/?p=protos%2Fxbee-avr.git;a=blobdiff_plain;f=xbee_proto.c;h=409d9a95aed82568b81291c54e2a2bfdb4ec11f2;hp=7072a6c0ba06ff5317cd2012ef74d77f50a6c5cd;hb=f74db5fd03bcd38af3ca533e7531dd70454dfaaf;hpb=d6014994e06ebe952d51bef2e36a795f624c4b4d

diff --git a/xbee_proto.c b/xbee_proto.c
index 7072a6c..409d9a9 100644
--- a/xbee_proto.c
+++ b/xbee_proto.c
@@ -26,6 +26,7 @@
  */
 
 #include <aversive.h>
+#include <aversive/pgmspace.h>
 #include <aversive/queue.h>
 #include <aversive/endian.h>
 
@@ -116,6 +117,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,10 +125,21 @@ 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");
+		fprintf_P(stderr, PSTR("Frame too small\r\n"));
 		return -1;
 	}
 
@@ -135,7 +148,7 @@ static int xbee_proto_parse_frame(struct xbee_dev *dev)
 		cksum += ((uint8_t *)buf)[i];
 	cksum = 0xff - cksum;
 	if (cksum != ((uint8_t *)buf)[len-1]) {
-		fprintf(stderr, "Invalid cksum\n");
+		fprintf_P(stderr, PSTR("Invalid cksum\r\n"));
 		dev->stats.rx_invalid_cksum++;
 		return -1;
 	}
@@ -192,8 +205,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;
@@ -217,7 +230,7 @@ int xbee_proto_xmit(struct xbee_dev *dev, uint8_t channel_id, uint8_t type,
 	hdr.type = type;
 	hdr.id = channel_id;
 
-	if (channel_id < 0 || channel_id >= XBEE_MAX_CHANNEL ||
+	if (channel_id >= XBEE_MAX_CHANNEL ||
 	    dev->channel[channel_id].registered == 0) {
 		dev->stats.tx_invalid_channel ++;
 		return -1;
@@ -261,7 +274,8 @@ int xbee_proto_xmit(struct xbee_dev *dev, uint8_t channel_id, uint8_t type,
 		case XBEE_TYPE_RMT_ATRESP:
 		default:
 			dev->stats.tx_invalid_type ++;
-			fprintf(stderr, "unhandled xmit type=%x\n", hdr.type);
+			fprintf_P(stderr, PSTR("unhandled xmit type=%x\r\n"),
+				  hdr.type);
 			return -1;
 	}
 
@@ -292,6 +306,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,11 +318,18 @@ 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;
-		if (xbee_proto_parse_frame(dev) < 0)
+		if (xbee_proto_parse_frame(dev) < 0) {
 			;//XXX stats
+		}
 		dev->frame_len = 0;
 	}
 }