From f90fb542ffa7cec81db48dae1f1a3ea3104ad04c Mon Sep 17 00:00:00 2001
From: Ralph Droms <rdroms@gmail.com>
Date: Fri, 5 Apr 2013 14:59:29 -0400
Subject: [PATCH] ieee802154: 6lowpan.c: Fix fraglet generation code so first
 fraglet and offset is compliant with RFC 4944

RFC 4944 specifies that the first fraglet must contain some data and that
the offset field in subsequent fraglets is computed based on the original
IPv6 datagram, before any header compression was performed.  This patch
fixes 6lowpan.c so the first fraglet is non-empty and the offset
is correctly computed for subsequent fraglets.

Signed-off-by: Ralph Droms <rdroms@cisco.com>
---
 net/ieee802154/6lowpan.c |   78 ++++++++++++++++++++++++++++++++++++++--------
 1 files changed, 65 insertions(+), 13 deletions(-)

diff --git a/net/ieee802154/6lowpan.c b/net/ieee802154/6lowpan.c
index ccd7308..35b6f5e 100644
--- a/net/ieee802154/6lowpan.c
+++ b/net/ieee802154/6lowpan.c
@@ -978,24 +978,37 @@ static int lowpan_set_address(struct net_device *dev, void *p)
 
 static int lowpan_get_mac_header_length(struct sk_buff *skb)
 {
+	u8	len, fc;
+
+	fc = skb->data[1];
+
 	/*
-	 * 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
+	 * Using 6 and 4 instead of IEEE802154_FC_SAMODE_SHIFT and
+	 * IEEE802154_FC_DAMODE_SHIFT because fc is just one header byte
+	 *
+	 * Assumes PAN ID compression
 	 */
-	return 21;
+
+	len = 2 + 1 + 2; 
+	len += (((fc >> 6) & 0x3) == IEEE802154_ADDR_LONG)?8:2;
+	len += (((fc >> 2) & 0x3) == IEEE802154_ADDR_LONG)?8:2;
+
+	pr_debug("addr type flags %02x saddr %d daddr %d len %d\n", fc,
+		 (fc >> 6) & 0x3, (fc >> 2) & 0x3, len);
+
+	return len;
 }
 
 static int
 lowpan_fragment_xmit(struct sk_buff *skb, u8 *head,
-			int mlen, int plen, int offset)
+		     int mlen, int plen, int offset, int type)
 {
 	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);
+	/* type parameter indicates whether this is a first fragment */
+	hlen = (type == LOWPAN_DISPATCH_FRAG1 ? LOWPAN_FRAG1_HEAD_SIZE :
+		LOWPAN_FRAGN_HEAD_SIZE);
 
 	lowpan_raw_dump_inline(__func__, "6lowpan fragment header", head, hlen);
 
@@ -1022,14 +1035,40 @@ lowpan_fragment_xmit(struct sk_buff *skb, u8 *head,
 	return ret;
 }
 
+static int exp_tc_fl[4] = { 0,  1,  3,  4};
+static int exp_uac[8] =   { 0,  8, 14, 16, 16,  8, 14, 16};
+static int exp_mac[8] =   { 0, 10, 12, 15, 10,  0,  0,  0};
+ 
+static int
+header_expansion(struct sk_buff *skb, int offset)
+{
+	int ihc0, ihc1, exp = 0;
+
+	ihc0 = skb->data[offset];
+	ihc1 = skb->data[offset + 1];
+
+	exp += exp_tc_fl[(ihc0 & 0x18) >> 3];
+	if ((ihc0 & 0x3) != 0)
+		exp++;
+	exp += exp_uac[(ihc1 & 0x70) >> 4];
+	if (ihc1 & 0x08)
+		exp += exp_mac[ihc1 & 0x7];
+	else
+		exp += exp_uac[ihc1 & 0x7];
+       
+	pr_debug("ihc0 %02x, ihc1 %02x, exp %d\n", ihc0, ihc1, exp);
+	return exp;
+}
+
 static int
 lowpan_skb_fragmentation(struct sk_buff *skb)
 {
-	int  err, header_length, payload_length, tag, offset = 0;
+	int  err, header_length, payload_length, tag, offset = 0, first_frag_exp;
 	u8 head[5];
 
 	header_length = lowpan_get_mac_header_length(skb);
-	payload_length = skb->len - header_length;
+ 	first_frag_exp = header_expansion(skb, header_length);
+ 	payload_length = skb->len - header_length + first_frag_exp;
 	tag = fragment_tag++;
 
 	/* first fragment header */
@@ -1038,7 +1077,14 @@ lowpan_skb_fragmentation(struct sk_buff *skb)
 	head[2] = tag >> 8;
 	head[3] = tag & 0xff;
 
-	err = lowpan_fragment_xmit(skb, head, header_length, 0, 0);
+ 	offset = ((LOWPAN_FRAG_SIZE + first_frag_exp) >> 3) << 3;
+ 	err = lowpan_fragment_xmit(skb, head, header_length, offset - first_frag_exp,
+ 				   0, LOWPAN_DISPATCH_FRAG1);
+ 	if (err) {
+ 		pr_debug("%s unable to send FRAG1 packet (tag: %d)",
+ 			 __func__, tag);
+ 		goto exit;
+ 	}
 
 	/* next fragment header */
 	head[0] &= ~LOWPAN_DISPATCH_FRAG1;
@@ -1053,10 +1099,16 @@ lowpan_skb_fragmentation(struct sk_buff *skb)
 			len = payload_length - offset;
 
 		err = lowpan_fragment_xmit(skb, head, header_length,
-							len, offset);
+ 					   len, offset - first_frag_exp,
+ 					   LOWPAN_DISPATCH_FRAGN);
+ 		if (err) {
+ 			pr_debug("%s unable to send a subsequent FRAGN packet "
+ 				 "(tag: %d, offset: %d", __func__, tag, offset);
+ 		}
+
 		offset += len;
 	}
-
+exit:
 	return err;
 }
 
-- 
1.7.7.6

