From patchwork Wed Nov 2 16:32:49 2011 Content-Type: text/plain; charset="utf-8" MIME-Version: 1.0 Content-Transfer-Encoding: 7bit X-Patchwork-Submitter: alex.bluesman.smirnov@gmail.com X-Patchwork-Id: 123290 X-Patchwork-Delegate: davem@davemloft.net Return-Path: X-Original-To: patchwork-incoming@ozlabs.org Delivered-To: patchwork-incoming@ozlabs.org Received: from vger.kernel.org (vger.kernel.org [209.132.180.67]) by ozlabs.org (Postfix) with ESMTP id BC4F7B6F7B for ; Thu, 3 Nov 2011 03:32:43 +1100 (EST) Received: (majordomo@vger.kernel.org) by vger.kernel.org via listexpand id S933423Ab1KBQcf (ORCPT ); Wed, 2 Nov 2011 12:32:35 -0400 Received: from mail-wy0-f174.google.com ([74.125.82.174]:34268 "EHLO mail-wy0-f174.google.com" rhost-flags-OK-OK-OK-OK) by vger.kernel.org with ESMTP id S932709Ab1KBQce (ORCPT ); Wed, 2 Nov 2011 12:32:34 -0400 Received: by wyh15 with SMTP id 15so328764wyh.19 for ; Wed, 02 Nov 2011 09:32:33 -0700 (PDT) DKIM-Signature: v=1; a=rsa-sha256; c=relaxed/relaxed; d=gmail.com; s=gamma; h=from:to:cc:subject:date:message-id:x-mailer:in-reply-to:references; bh=3OMkIEJivdlYd7Zu/K+JP21MxWstO4hV3UsBAmwwxZ8=; b=s2Wt8Wu94/YUxLfPSiBrRwWTSxUQavI5c8pLLTBoCLoNghlKww6sG3XWta1GNydDxO lFvNUlrhPpG1G6nn7uPXMdEe4EtJNIacvx5cL4maJ2bwQ+cHLzv9k61x0+XWmMoT2PEy Uols2NY85ZxSdarzpjVS990f5rkkwiMBX9+z0= Received: by 10.227.197.71 with SMTP id ej7mr6436924wbb.15.1320251553289; Wed, 02 Nov 2011 09:32:33 -0700 (PDT) Received: from localhost.localdomain ([91.213.169.4]) by mx.google.com with ESMTPS id b5sm5086482wbh.4.2011.11.02.09.32.31 (version=TLSv1/SSLv3 cipher=OTHER); Wed, 02 Nov 2011 09:32:32 -0700 (PDT) From: Alexander Smirnov To: davem@davemloft.net Cc: dbaryshkov@gmail.com, linux-zigbee-devel@lists.sourceforge.net, netdev@vger.kernel.org, Alexander Smirnov Subject: [PATCH 1/6] [6LoWPAN] add fragmentation support Date: Wed, 2 Nov 2011 19:32:49 +0300 Message-Id: <1320251569-2777-1-git-send-email-alex.bluesman.smirnov@gmail.com> X-Mailer: git-send-email 1.7.2.5 In-Reply-To: <20111102162520.GA2669@avtobot.cybertron> References: <20111102162520.GA2669@avtobot.cybertron> Sender: netdev-owner@vger.kernel.org Precedence: bulk List-ID: X-Mailing-List: netdev@vger.kernel.org This patch adds support for frame fragmentation. Signed-off-by: Alexander Smirnov --- include/net/ieee802154.h | 6 + net/ieee802154/6lowpan.c | 254 +++++++++++++++++++++++++++++++++++++++++++++- net/ieee802154/6lowpan.h | 18 ++++ 3 files changed, 274 insertions(+), 4 deletions(-) diff --git a/include/net/ieee802154.h b/include/net/ieee802154.h index d52685d..ee59f8b 100644 --- a/include/net/ieee802154.h +++ b/include/net/ieee802154.h @@ -21,11 +21,14 @@ * Maxim Gorbachyov * Maxim Osipov * Dmitry Eremin-Solenikov + * Alexander Smirnov */ #ifndef NET_IEEE802154_H #define NET_IEEE802154_H +#define IEEE802154_MTU 127 + #define IEEE802154_FC_TYPE_BEACON 0x0 /* Frame is beacon */ #define IEEE802154_FC_TYPE_DATA 0x1 /* Frame is data */ #define IEEE802154_FC_TYPE_ACK 0x2 /* Frame is acknowledgment */ @@ -56,6 +59,9 @@ (((x) & IEEE802154_FC_DAMODE_MASK) >> IEEE802154_FC_DAMODE_SHIFT) +/* MAC footer size */ +#define IEEE802154_MFR_SIZE 2 /* 2 octets */ + /* MAC's Command Frames Identifiers */ #define IEEE802154_CMD_ASSOCIATION_REQ 0x01 #define IEEE802154_CMD_ASSOCIATION_RESP 0x02 diff --git a/net/ieee802154/6lowpan.c b/net/ieee802154/6lowpan.c index 19d6aef..890ecef 100644 --- a/net/ieee802154/6lowpan.c +++ b/net/ieee802154/6lowpan.c @@ -113,6 +113,20 @@ struct lowpan_dev_record { struct list_head list; }; +struct lowpan_fragment { + struct sk_buff *skb; /* skb to be assembled */ + spinlock_t lock; /* concurency lock */ + u16 length; /* length to be assemled */ + u32 bytes_rcv; /* bytes received */ + u16 tag; /* current fragment tag */ + struct timer_list timer; /* assembling timer */ + struct list_head list; /* fragments list */ +}; + +static unsigned short fragment_tag; +static LIST_HEAD(lowpan_fragments); +spinlock_t flist_lock; + static inline struct lowpan_dev_info *lowpan_dev_info(const struct net_device *dev) { @@ -244,6 +258,17 @@ static u8 lowpan_fetch_skb_u8(struct sk_buff *skb) return ret; } +static u16 lowpan_fetch_skb_u16(struct sk_buff *skb) +{ + u16 ret; + + BUG_ON(!pskb_may_pull(skb, 2)); + + ret = skb->data[0] | (skb->data[1] << 8); + skb_pull(skb, 2); + return ret; +} + static int lowpan_header_create(struct sk_buff *skb, struct net_device *dev, unsigned short type, const void *_daddr, @@ -467,6 +492,7 @@ static int lowpan_header_create(struct sk_buff *skb, memcpy(&(sa.hwaddr), saddr, 8); mac_cb(skb)->flags = IEEE802154_FC_TYPE_DATA; + return dev_hard_header(skb, lowpan_dev_info(dev)->real_dev, type, (void *)&da, (void *)&sa, skb->len); } @@ -511,6 +537,21 @@ static int lowpan_skb_deliver(struct sk_buff *skb, struct ipv6hdr *hdr) return stat; } +static void lowpan_fragment_timer_expired(unsigned long entry_addr) +{ + struct lowpan_fragment *entry = (struct lowpan_fragment *)entry_addr; + + pr_debug("%s: timer expired for frame with tag %d\n", __func__, + entry->tag); + + spin_lock(&flist_lock); + list_del(&entry->list); + spin_unlock(&flist_lock); + + dev_kfree_skb(entry->skb); + kfree(entry); +} + static int lowpan_process_data(struct sk_buff *skb) { @@ -525,6 +566,104 @@ lowpan_process_data(struct sk_buff *skb) if (skb->len < 2) goto drop; iphc0 = lowpan_fetch_skb_u8(skb); + + /* fragments assembling */ + switch (iphc0 & LOWPAN_DISPATCH_MASK) { + case LOWPAN_DISPATCH_FRAG1: + case LOWPAN_DISPATCH_FRAGN: + { + struct lowpan_fragment *frame; + u8 len, offset; + u16 tag; + + len = lowpan_fetch_skb_u8(skb); /* frame length */ + tag = lowpan_fetch_skb_u16(skb); + + /* + * check if frame assembling with the same tag is + * already in progress + */ + spin_lock(&flist_lock); + + list_for_each_entry(frame, &lowpan_fragments, list) + if (frame->tag == tag) + break; + + /* alloc new frame structure */ + if (frame->tag != tag) { + frame = kzalloc(sizeof(struct lowpan_fragment), + GFP_ATOMIC); + if (!frame) + goto drop; + + INIT_LIST_HEAD(&frame->list); + + frame->length = (iphc0 & 7) | (len << 3); + frame->tag = tag; + + /* allocate buffer for frame assembling */ + frame->skb = alloc_skb(frame->length + + sizeof(struct ipv6hdr), GFP_ATOMIC); + + frame->skb->priority = skb->priority; + frame->skb->dev = skb->dev; + + if (!frame->skb) { + kfree(frame); + goto drop; + } + + /* reserve headroom for uncompressed ipv6 header */ + skb_reserve(frame->skb, sizeof(struct ipv6hdr)); + skb_put(frame->skb, frame->length); + + init_timer(&frame->timer); + /* time out is the same as for ipv6 - 60 sec */ + frame->timer.expires = jiffies + LOWPAN_FRAG_TIMEOUT; + frame->timer.data = (unsigned long)frame; + frame->timer.function = lowpan_fragment_timer_expired; + + add_timer(&frame->timer); + + list_add_tail(&frame->list, &lowpan_fragments); + } + + if ((iphc0 & LOWPAN_DISPATCH_MASK) == LOWPAN_DISPATCH_FRAG1) + goto drop; + + offset = lowpan_fetch_skb_u8(skb); /* fetch offset */ + + /* 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 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(&frame->timer); + list_del(&frame->list); + spin_unlock(&flist_lock); + + dev_kfree_skb(skb); + skb = frame->skb; + kfree(frame); + iphc0 = lowpan_fetch_skb_u8(skb); + break; + } + spin_unlock(&flist_lock); + + return kfree_skb(skb), 0; + } + default: + break; + } + iphc1 = lowpan_fetch_skb_u8(skb); _saddr = mac_cb(skb)->sa.hwaddr; @@ -692,18 +831,118 @@ static int lowpan_set_address(struct net_device *dev, void *p) return 0; } +static int lowpan_get_mac_header_length(struct sk_buff *skb) +{ + /* + * Currently long addressing mode is supported only, so the overall + * header size is 21: + * FC SeqNum DPAN DA SA Sec + * 2 + 1 + 2 + 8 + 8 + 0 = 21 + */ + return 21; +} + +static int +lowpan_fragment_xmit(struct sk_buff *skb, u8 *head, + int mlen, int plen, int offset) +{ + struct sk_buff *frag; + int hlen, ret; + + /* if payload length is zero, therefore it's a first fragment */ + hlen = (plen == 0 ? LOWPAN_FRAG1_HEAD_SIZE : LOWPAN_FRAGN_HEAD_SIZE); + + lowpan_raw_dump_inline(__func__, "6lowpan fragment header", head, hlen); + + frag = dev_alloc_skb(hlen + mlen + plen + IEEE802154_MFR_SIZE); + if (!frag) + return -ENOMEM; + + frag->priority = skb->priority; + frag->dev = skb->dev; + + /* copy header, MFR and payload */ + memcpy(skb_put(frag, mlen), skb->data, mlen); + memcpy(skb_put(frag, hlen), head, hlen); + + if (plen) + skb_copy_from_linear_data_offset(skb, offset + mlen, + skb_put(frag, plen), plen); + + lowpan_raw_dump_table(__func__, " raw fragment dump", frag->data, + frag->len); + + ret = dev_queue_xmit(frag); + + if (ret < 0) + dev_kfree_skb(frag); + + return ret; +} + +static int +lowpan_skb_fragmentation(struct sk_buff *skb) +{ + int err, header_length, payload_length, tag, offset = 0; + u8 head[5]; + + header_length = lowpan_get_mac_header_length(skb); + payload_length = skb->len - header_length; + tag = fragment_tag++; + + /* first fragment header */ + head[0] = LOWPAN_DISPATCH_FRAG1 | (payload_length & 0x7); + head[1] = (payload_length >> 3) & 0xff; + head[2] = tag & 0xff; + head[3] = tag >> 8; + + err = lowpan_fragment_xmit(skb, head, header_length, 0, 0); + + /* next fragment header */ + head[0] &= ~LOWPAN_DISPATCH_FRAG1; + head[0] |= LOWPAN_DISPATCH_FRAGN; + + while ((payload_length - offset > 0) && (err >= 0)) { + int len = LOWPAN_FRAG_SIZE; + + head[4] = offset / 8; + + if (payload_length - offset < len) + len = payload_length - offset; + + err = lowpan_fragment_xmit(skb, head, header_length, + len, offset); + offset += len; + } + + return err; +} + static netdev_tx_t lowpan_xmit(struct sk_buff *skb, struct net_device *dev) { - int err = 0; + int err = -1; pr_debug("(%s): package xmit\n", __func__); skb->dev = lowpan_dev_info(dev)->real_dev; if (skb->dev == NULL) { pr_debug("(%s) ERROR: no real wpan device found\n", __func__); - dev_kfree_skb(skb); - } else + goto error; + } + + if (skb->len <= IEEE802154_MTU) { err = dev_queue_xmit(skb); + goto out; + } + + pr_debug("(%s): frame is too big, fragmentation is needed\n", + __func__); + err = lowpan_skb_fragmentation(skb); +error: + dev_kfree_skb(skb); +out: + if (err < 0) + pr_debug("(%s): ERROR: xmit failed\n", __func__); return (err < 0 ? NETDEV_TX_BUSY : NETDEV_TX_OK); } @@ -765,8 +1004,15 @@ static int lowpan_rcv(struct sk_buff *skb, struct net_device *dev, goto drop; /* check that it's our buffer */ - if ((skb->data[0] & 0xe0) == 0x60) + switch (skb->data[0] & 0xe0) { + case LOWPAN_DISPATCH_IPHC: /* ipv6 datagram */ + case LOWPAN_DISPATCH_FRAG1: /* first fragment header */ + case LOWPAN_DISPATCH_FRAGN: /* next fragments headers */ lowpan_process_data(skb); + break; + default: + break; + } return NET_RX_SUCCESS; diff --git a/net/ieee802154/6lowpan.h b/net/ieee802154/6lowpan.h index 5d8cf80..5d2e5a0 100644 --- a/net/ieee802154/6lowpan.h +++ b/net/ieee802154/6lowpan.h @@ -159,6 +159,24 @@ #define LOWPAN_DISPATCH_FRAG1 0xc0 /* 11000xxx */ #define LOWPAN_DISPATCH_FRAGN 0xe0 /* 11100xxx */ +#define LOWPAN_DISPATCH_MASK 0xf8 /* 11111000 */ + +#define LOWPAN_FRAG_TIMEOUT (HZ * 60) /* time-out 60 sec */ + +#define LOWPAN_FRAG1_HEAD_SIZE 0x4 +#define LOWPAN_FRAGN_HEAD_SIZE 0x5 + +/* + * According IEEE802.15.4 standard: + * - MTU is 127 octets + * - maximum MHR size is 37 octets + * - MFR size is 2 octets + * + * so minimal payload size that we may guarantee is: + * MTU - MHR - MFR = 88 octets + */ +#define LOWPAN_FRAG_SIZE 88 + /* * Values of fields within the IPHC encoding first byte * (C stands for compressed and I for inline)