This patch fixes the fragmentation of receiving 6lowpan packets.
The current fragmentation isn't rfc4944 compatible.

Signed-off-by: Alexander Aring <alex.ar...@gmail.com>
---
 net/ieee802154/6lowpan.c | 268 +++++++++++++++++++++++++++--------------------
 1 file changed, 154 insertions(+), 114 deletions(-)

diff --git a/net/ieee802154/6lowpan.c b/net/ieee802154/6lowpan.c
index 4c2072d..6df928f 100644
--- a/net/ieee802154/6lowpan.c
+++ b/net/ieee802154/6lowpan.c
@@ -402,7 +402,8 @@ static inline int lowpan_fetch_skb_u16(struct sk_buff *skb, 
u16 *val)
 }
 
 static int
-lowpan_uncompress_udp_header(struct sk_buff *skb, struct udphdr *uh)
+lowpan_uncompress_udp_header(struct sk_buff *skb,
+               struct udphdr *uh, size_t udplen)
 {
        u8 tmp;
 
@@ -456,7 +457,11 @@ lowpan_uncompress_udp_header(struct sk_buff *skb, struct 
udphdr *uh)
                 * here, we obtain the hint from the remaining size of the
                 * frame
                 */
-               uh->len = htons(skb->len + sizeof(struct udphdr));
+               if (udplen)
+                       uh->len = htons(udplen);
+               else
+                       uh->len = htons(skb->len + sizeof(struct udphdr));
+
                pr_debug("uncompressed UDP length: src = %d", uh->len);
        } else {
                pr_debug("ERROR: unsupported NH format\n");
@@ -757,16 +762,11 @@ lowpan_alloc_new_frame(struct sk_buff *skb, u16 len, u16 
tag)
        frame->tag = tag;
 
        /* allocate buffer for frame assembling */
-       frame->skb = netdev_alloc_skb_ip_align(skb->dev, frame->length +
-                                              sizeof(struct ipv6hdr));
-
+       frame->skb = netdev_alloc_skb(skb->dev, frame->length);
        if (!frame->skb)
                goto skb_err;
 
        frame->skb->priority = skb->priority;
-
-       /* reserve headroom for uncompressed ipv6 header */
-       skb_reserve(frame->skb, sizeof(struct ipv6hdr));
        skb_put(frame->skb, frame->length);
 
        /* copy the first control block to keep a
@@ -786,7 +786,6 @@ lowpan_alloc_new_frame(struct sk_buff *skb, u16 len, u16 
tag)
        list_add_tail(&frame->list, &lowpan_fragments);
 
        return frame;
-
 skb_err:
        kfree(frame);
 frame_err:
@@ -794,7 +793,7 @@ frame_err:
 }
 
 static int
-lowpan_process_data(struct sk_buff *skb)
+lowpan_process_data(struct sk_buff *skb, size_t d_size)
 {
        struct ipv6hdr hdr = {};
        u8 tmp, iphc0, iphc1, num_context = 0;
@@ -810,94 +809,6 @@ lowpan_process_data(struct sk_buff *skb)
        if (lowpan_fetch_skb_u8(skb, &iphc0))
                goto drop;
 
-       /* fragments assembling */
-       switch (iphc0 & LOWPAN_DISPATCH_MASK) {
-       case LOWPAN_DISPATCH_FRAG1:
-       case LOWPAN_DISPATCH_FRAGN:
-       {
-               struct lowpan_fragment *frame;
-               /* slen stores the rightmost 8 bits of the 11 bits length */
-               u8 slen, offset = 0;
-               u16 len, tag;
-               bool found = false;
-
-               if (lowpan_fetch_skb_u8(skb, &slen) || /* frame length */
-                   lowpan_fetch_skb_u16(skb, &tag))  /* fragment tag */
-                       goto drop;
-
-               /* adds the 3 MSB to the 8 LSB to retrieve the 11 bits length */
-               len = ((iphc0 & 7) << 8) | slen;
-
-               if ((iphc0 & LOWPAN_DISPATCH_MASK) == LOWPAN_DISPATCH_FRAG1) {
-                       pr_debug("%s received a FRAG1 packet (tag: %d, "
-                                "size of the entire IP packet: %d)",
-                                __func__, tag, len);
-               } else { /* FRAGN */
-                       if (lowpan_fetch_skb_u8(skb, &offset))
-                               goto unlock_and_drop;
-                       pr_debug("%s received a FRAGN packet (tag: %d, "
-                                "size of the entire IP packet: %d, "
-                                "offset: %d)", __func__, tag, len, offset * 8);
-               }
-
-               /*
-                * check if frame assembling with the same tag is
-                * already in progress
-                */
-               spin_lock_bh(&flist_lock);
-
-               list_for_each_entry(frame, &lowpan_fragments, list)
-                       if (frame->tag == tag) {
-                               found = true;
-                               break;
-                       }
-
-               /* alloc new frame structure */
-               if (!found) {
-                       pr_debug("%s first fragment received for tag %d, "
-                                "begin packet reassembly", __func__, tag);
-                       frame = lowpan_alloc_new_frame(skb, len, tag);
-                       if (!frame)
-                               goto unlock_and_drop;
-               }
-
-               /* if payload fits buffer, copy it */
-               if (likely((offset * 8 + skb->len) <= frame->length))
-                       skb_copy_to_linear_data_offset(frame->skb, offset * 8,
-                                                       skb->data, skb->len);
-               else
-                       goto unlock_and_drop;
-
-               frame->bytes_rcv += skb->len;
-
-               /* frame assembling complete */
-               if ((frame->bytes_rcv == frame->length) &&
-                    frame->timer.expires > jiffies) {
-                       /* if timer haven't expired - first of all delete it */
-                       del_timer_sync(&frame->timer);
-                       list_del(&frame->list);
-                       spin_unlock_bh(&flist_lock);
-
-                       pr_debug("%s successfully reassembled fragment "
-                                "(tag %d)", __func__, tag);
-
-                       dev_kfree_skb(skb);
-                       skb = frame->skb;
-                       kfree(frame);
-
-                       if (lowpan_fetch_skb_u8(skb, &iphc0))
-                               goto drop;
-
-                       break;
-               }
-               spin_unlock_bh(&flist_lock);
-
-               return kfree_skb(skb), 0;
-       }
-       default:
-               break;
-       }
-
        if (lowpan_fetch_skb_u8(skb, &iphc1))
                goto drop;
 
@@ -1021,8 +932,15 @@ lowpan_process_data(struct sk_buff *skb)
        /* UDP data uncompression */
        if (iphc0 & LOWPAN_IPHC_NH_C) {
                struct udphdr uh;
-               if (lowpan_uncompress_udp_header(skb, &uh))
-                       goto drop;
+
+               if (d_size) {
+                       if (lowpan_uncompress_udp_header(skb, &uh, d_size -
+                                               sizeof(struct ipv6hdr)))
+                               goto drop;
+               } else {
+                       if (lowpan_uncompress_udp_header(skb, &uh, 0))
+                               goto drop;
+               }
 
                skb_push(skb, sizeof(struct udphdr));
                skb_copy_to_linear_data(skb, &uh, sizeof(struct udphdr));
@@ -1033,8 +951,10 @@ lowpan_process_data(struct sk_buff *skb)
                hdr.nexthdr = UIP_PROTO_UDP;
        }
 
-       /* Not fragmented package */
-       hdr.payload_len = htons(skb->len);
+       if (d_size)
+               hdr.payload_len = htons(d_size - sizeof(struct ipv6hdr));
+       else
+               hdr.payload_len = htons(skb->len);
 
        pr_debug("skb headroom size = %d, data length = %d\n",
                 skb_headroom(skb), skb->len);
@@ -1049,12 +969,8 @@ lowpan_process_data(struct sk_buff *skb)
        skb_push(skb, sizeof(struct ipv6hdr));
        skb_copy_to_linear_data(skb, &hdr, sizeof(struct ipv6hdr));
 
-       return lowpan_give_skb_to_devices(skb);
-
-unlock_and_drop:
-       spin_unlock_bh(&flist_lock);
+       return 0;
 drop:
-       kfree_skb(skb);
        return -EINVAL;
 }
 
@@ -1272,9 +1188,76 @@ static int lowpan_validate(struct nlattr *tb[], struct 
nlattr *data[])
        return 0;
 }
 
+static int lowpan_get_frag_info(struct sk_buff *skb, u16 *d_tag,
+               u16 *d_size, u8 *d_offset)
+{
+       int err;
+       u8 pattern = 0, low = 0;
+
+       err = lowpan_fetch_skb(skb, &pattern, 1);
+       if (err < 0)
+               goto parse_err;
+
+       err = lowpan_fetch_skb(skb, &low, 1);
+       if (err < 0)
+               goto parse_err;
+
+       *d_size = (pattern & 7) << 8 | low;
+
+       err = lowpan_fetch_skb(skb, d_tag, 2);
+       if (err < 0)
+               goto parse_err;
+
+       /* We read this value in comming process */
+       *d_tag = ntohs(*d_tag);
+
+       if (d_offset) {
+               err = lowpan_fetch_skb(skb, d_offset, 1);
+               if (err < 0)
+                       goto parse_err;
+       }
+
+       return 0;
+parse_err:
+       return -1;
+}
+
+static struct lowpan_fragment *lowpan_get_frame(
+               struct sk_buff *skb, u16 d_tag, u16 d_size)
+{
+       struct lowpan_fragment *frame;
+
+       list_for_each_entry(frame, &lowpan_fragments, list) {
+               if (frame->tag == d_tag)
+                       return frame;
+       }
+
+       return lowpan_alloc_new_frame(skb, d_size, d_tag);
+}
+
+static int lowpan_check_frag_complete(struct lowpan_fragment *frame)
+{
+       int ret = 0;
+
+       if (frame->bytes_rcv != frame->length)
+               return ret;
+
+       list_del(&frame->list);
+       del_timer_sync(&frame->timer);
+
+       ret = lowpan_give_skb_to_devices(frame->skb);
+       kfree(frame);
+       return ret;
+}
+
 static int lowpan_rcv(struct sk_buff *skb, struct net_device *dev,
-       struct packet_type *pt, struct net_device *orig_dev)
+               struct packet_type *pt, struct net_device *orig_dev)
 {
+       int ret;
+       struct lowpan_fragment *frame;
+       u16 d_tag, d_size;
+       u8 d_offset;
+
        if (!netif_running(dev))
                goto drop;
 
@@ -1284,15 +1267,71 @@ static int lowpan_rcv(struct sk_buff *skb, struct 
net_device *dev,
        /* check that it's our buffer */
        if (skb->data[0] == LOWPAN_DISPATCH_IPV6) {
                /* Pull off the 1-byte of 6lowpan header. */
-               skb_pull(skb, 1);
-
-               lowpan_give_skb_to_devices(skb);
+               ret = lowpan_fetch_skb(skb, NULL, 1);
+               if (ret)
+                       goto drop;
+               ret = lowpan_give_skb_to_devices(skb);
+               if (ret)
+                       goto drop;
        } else {
                switch (skb->data[0] & 0xe0) {
                case LOWPAN_DISPATCH_IPHC:      /* ipv6 datagram */
+                       ret = lowpan_process_data(skb, 0);
+                       if (ret < 0)
+                               goto drop;
+
+                       ret = lowpan_give_skb_to_devices(skb);
+                       if (ret)
+                               goto drop;
+                       break;
                case LOWPAN_DISPATCH_FRAG1:     /* first fragment header */
+                       ret = lowpan_get_frag_info(skb, &d_tag, &d_size, NULL);
+                       if (ret < 0)
+                               goto drop;
+
+                       spin_lock_bh(&flist_lock);
+
+                       frame = lowpan_get_frame(skb, d_tag, d_size);
+                       if (!frame)
+                               goto drop_unlock;
+
+                       ret = lowpan_process_data(skb, d_size);
+                       if (ret < 0)
+                               goto drop_unlock;
+
+                       skb_copy_to_linear_data(frame->skb,
+                                       skb->data, skb->len);
+
+                       frame->bytes_rcv += skb->len;
+                       ret = lowpan_check_frag_complete(frame);
+                       if (ret)
+                               goto drop_unlock;
+
+                       spin_unlock_bh(&flist_lock);
+                       dev_kfree_skb(skb);
+                       break;
                case LOWPAN_DISPATCH_FRAGN:     /* next fragments headers */
-                       lowpan_process_data(skb);
+                       ret = lowpan_get_frag_info(skb, &d_tag,
+                                       &d_size, &d_offset);
+                       if (ret < 0)
+                               goto drop;
+
+                       spin_lock_bh(&flist_lock);
+
+                       frame = lowpan_get_frame(skb, d_tag, d_size);
+                       if (!frame)
+                               goto drop_unlock;
+
+                       skb_copy_to_linear_data_offset(frame->skb,
+                                       (d_offset * 8), skb->data, skb->len);
+
+                       frame->bytes_rcv += skb->len;
+                       ret = lowpan_check_frag_complete(frame);
+                       if (ret)
+                               goto drop_unlock;
+
+                       spin_unlock_bh(&flist_lock);
+                       dev_kfree_skb(skb);
                        break;
                default:
                        break;
@@ -1300,9 +1339,10 @@ static int lowpan_rcv(struct sk_buff *skb, struct 
net_device *dev,
        }
 
        return NET_RX_SUCCESS;
-
+drop_unlock:
+       spin_unlock_bh(&flist_lock);
 drop:
-       kfree_skb(skb);
+       dev_kfree_skb(skb);
        return NET_RX_DROP;
 }
 
-- 
1.8.4


------------------------------------------------------------------------------
October Webinars: Code for Performance
Free Intel webinars can help you accelerate application performance.
Explore tips for MPI, OpenMP, advanced profiling, and more. Get the most from 
the latest Intel processors and coprocessors. See abstracts and register >
http://pubads.g.doubleclick.net/gampad/clk?id=60134071&iu=/4140/ostg.clktrk
_______________________________________________
Linux-zigbee-devel mailing list
Linux-zigbee-devel@lists.sourceforge.net
https://lists.sourceforge.net/lists/listinfo/linux-zigbee-devel

Reply via email to