Re: [PATCH 4/6] [6LoWPAN] UDP header compression

Signed-off-by: Alexander Smirnov <redacted>
---
 net/ieee802154/6lowpan.c |   51
++++++++++++++++++++++++++++++++++++++++++---
 net/ieee802154/6lowpan.h |    5 ++++
 2 files changed, 52 insertions(+), 4 deletions(-)

diff --git a/net/ieee802154/6lowpan.c b/net/ieee802154/6lowpan.c
index c4aae4e..6da3357 100644
--- a/net/ieee802154/6lowpan.c
+++ b/net/ieee802154/6lowpan.c

@@ -365,8 +365,6 @@ static int lowpan_header_create(struct sk_buff *skb,
 	if (hdr->nexthdr == UIP_PROTO_UDP)
 		iphc0 |= LOWPAN_IPHC_NH_C;

-/* TODO: next header compression */
-
 	if ((iphc0 & LOWPAN_IPHC_NH_C) == 0) {
 		*hc06_ptr = hdr->nexthdr;
 		hc06_ptr += 1;

@@ -454,8 +452,53 @@ static int lowpan_header_create(struct sk_buff *skb,
 		}
 	}

-	/* TODO: UDP header compression */
-	/* TODO: Next Header compression */
+	/* UDP header compression */
+	if (hdr->nexthdr == UIP_PROTO_UDP) {
+		struct udphdr *uh = udp_hdr(skb);
+
+		pr_debug("(%s): UDP header compression\n", __func__);
+
+		if (((uh->source & LOWPAN_NHC_UDP_4BIT_MASK) ==
+					LOWPAN_NHC_UDP_4BIT_PORT) &&
+		    ((uh->dest & LOWPAN_NHC_UDP_4BIT_MASK) ==
+					LOWPAN_NHC_UDP_4BIT_PORT)) {
+			pr_debug("(%s): both ports compression to 4 bits\n",
+								__func__);
+			*hc06_ptr = LOWPAN_NHC_UDP_CS_P_11;
+			*(hc06_ptr + 1) = /* subtraction is faster */
+			   (u8)((uh->dest - LOWPAN_NHC_UDP_4BIT_PORT) +
+			       ((uh->source & LOWPAN_NHC_UDP_4BIT_PORT) << 4));
+			hc06_ptr += 2;
+		} else if ((uh->dest & LOWPAN_NHC_UDP_8BIT_MASK) ==
+				LOWPAN_NHC_UDP_8BIT_PORT) {
+			pr_debug("(%s): remove 8 bits of dest\n", __func__);
+			*hc06_ptr = LOWPAN_NHC_UDP_CS_P_01;
+			memcpy(hc06_ptr + 1, &uh->source, 2);
+			*(hc06_ptr + 3) =
+				(u8)(uh->dest - LOWPAN_NHC_UDP_8BIT_PORT);
+			hc06_ptr += 4;
+		} else if ((uh->source & LOWPAN_NHC_UDP_8BIT_MASK) ==
+				LOWPAN_NHC_UDP_8BIT_PORT) {
+			pr_debug("(%s): remove 8 bits of source\n", __func__);
+			*hc06_ptr = LOWPAN_NHC_UDP_CS_P_10;
+			memcpy(hc06_ptr + 1, &uh->dest, 2);
+			*(hc06_ptr + 3) =
+				(u8)(uh->source - LOWPAN_NHC_UDP_8BIT_PORT);
+			hc06_ptr += 4;
+		} else {
+			pr_debug("(%s): can't compress header\n", __func__);
+			*hc06_ptr = LOWPAN_NHC_UDP_CS_P_00;
+			memcpy(hc06_ptr + 1, &uh->source, 2);
+			memcpy(hc06_ptr + 3, &uh->dest, 2);
+			hc06_ptr += 5;
+		}
+
+		/* checksum is always inline */
+		memcpy(hc06_ptr, &uh->check, 2);
+		hc06_ptr += 2;
+	}
+
+	/* TODO: other NH types support? */

 	head[0] = iphc0;
 	head[1] = iphc1;

diff --git a/net/ieee802154/6lowpan.h b/net/ieee802154/6lowpan.h
index 5d2e5a0..aeff3f3 100644
--- a/net/ieee802154/6lowpan.h
+++ b/net/ieee802154/6lowpan.h

@@ -219,6 +219,11 @@
 #define LOWPAN_NHC_UDP_CHECKSUMC	0x04
 #define LOWPAN_NHC_UDP_CHECKSUMI	0x00

+#define LOWPAN_NHC_UDP_4BIT_PORT	0xF0B0
+#define LOWPAN_NHC_UDP_4BIT_MASK	0xFFF0
+#define LOWPAN_NHC_UDP_8BIT_PORT	0xF000
+#define LOWPAN_NHC_UDP_8BIT_MASK	0xFF00
+
 /* values for port compression, _with checksum_ ie bit 5 set to 0 */
 #define LOWPAN_NHC_UDP_CS_P_00	0xF0 /* all inline */
 #define LOWPAN_NHC_UDP_CS_P_01	0xF1 /* source 16bit inline,
--

1.7.2.5