xbee_proto: fix parsing hdr len

diff --git a/xbee_proto.c b/xbee_proto.c
index 7072a6c..bd284dd 100644 (file)
--- 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;
 {
        void *buf = dev->frame;
        uint8_t len = dev->frame_len;

+       uint8_t hdrlen;

        struct xbee_hdr *hdr = buf;
        int i;
        uint8_t cksum = 0;
        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++;
 
 
        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 */
        /* 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;
                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,
        /* 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;
                                            dev->channel[channel].arg);
 
        return 0;


@@ -292,6 +304,9 @@ void xbee_proto_rx(struct xbee_dev *dev)
                        continue;
                }
 
                        continue;
                }
 

+               if (dev->frame_len == 0 && c != XBEE_DELIMITER)
+                       continue;
+

                dev->frame[dev->frame_len++] = c;
 
                /* not enough data to read len */
                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 */
 
                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;
                /* not enough data */
                if (dev->frame_len < framelen)
                        continue;