From 1725ac66d731ff81f8a27a4995a7a6db55780057 Mon Sep 17 00:00:00 2001
From: Ravi kumar Veeramally <ravikumar.veeramally@linux.intel.com>
Date: Mon, 11 Jul 2016 11:03:30 +0300
Subject: [PATCH] net: Add 6lowpan without compression header support

Add support for IPv6 header dispatch (without IPv6 compression
and uncompression). Only IPv6 dispatch header will be added at the
beginning of the buffer.

Change-Id: I7400a4dd115ac3b4b3d9deecd3cdb9ea292573fb
Signed-off-by: Ravi kumar Veeramally <ravikumar.veeramally@linux.intel.com>
---
 net/yaip/6lo.c           | 56 +++++++++++++++++++++++++++++++++++-----
 net/yaip/6lo.h           |  3 ++-
 tests/net/6lo/src/main.c |  2 +-
 3 files changed, 53 insertions(+), 8 deletions(-)

diff --git a/net/yaip/6lo.c b/net/yaip/6lo.c
index 19b5210b183..46ba06deb5a 100644
--- a/net/yaip/6lo.c
+++ b/net/yaip/6lo.c
@@ -751,20 +751,64 @@ fail:
 	return false;
 }
 
-bool net_6lo_compress(struct net_buf *buf)
+/* Adds IPv6 dispatch as first byte and adjust fragments  */
+static inline bool compress_ipv6_header(struct net_buf *buf)
 {
-	return compress_IPHC_header(buf);
+	struct net_buf *frag;
+
+	frag = net_nbuf_get_reserve_data(net_nbuf_ll_reserve(buf));
+	if (!frag) {
+		return false;
+	}
+
+	frag->data[0] = NET6LO_DISPATCH_IPV6;
+	net_buf_add(frag, 1);
+
+	net_buf_frag_insert(buf, frag);
+
+	/* compact the fragments, so that gaps will be filled */
+	buf->frags = net_nbuf_compact(buf->frags);
+
+	return true;
+}
+
+static inline bool uncompress_ipv6_header(struct net_buf *buf)
+{
+	struct net_buf *frag = buf->frags;
+
+	/* Pull off IPv6 dispatch header and adjust data and length */
+	memmove(frag->data, frag->data + 1, frag->len - 1);
+	frag->len -= 1;
+
+	return true;
+}
+
+bool net_6lo_compress(struct net_buf *buf, bool iphc)
+{
+	if (iphc) {
+		return compress_IPHC_header(buf);
+	} else {
+		return compress_ipv6_header(buf);
+	}
 }
 
 bool net_6lo_uncompress(struct net_buf *buf)
 {
 	if (!buf || !buf->frags) {
-		return NULL;
+		return false;
 	}
 
-	if (!(buf->frags->data[0] & NET6LO_DISPATCH_IPHC)) {
-		return NULL;
+	if ((buf->frags->data[0] & NET6LO_DISPATCH_IPHC) ==
+	    NET6LO_DISPATCH_IPHC) {
+		/* uncompress IPHC header */
+		return uncompress_IPHC_header(buf);
+
+	} else if ((buf->frags->data[0] & NET6LO_DISPATCH_IPV6) ==
+		   NET6LO_DISPATCH_IPV6) {
+		/* uncompress IPv6 header, it has only IPv6 dispatch in the
+		 * beginning */
+		return uncompress_ipv6_header(buf);
 	}
 
-	return uncompress_IPHC_header(buf);
+	return false;
 }
diff --git a/net/yaip/6lo.h b/net/yaip/6lo.h
index 91a3c35d148..ce12f43ff65 100644
--- a/net/yaip/6lo.h
+++ b/net/yaip/6lo.h
@@ -36,10 +36,11 @@
  *  will be adjusted according to remaining space in fragments.
  *
  *  @param Pointer to network buffer
+ *  @param iphc true for IPHC compression, false for IPv6 dispatch header
  *
  *  @return True on success, false otherwise
  */
-bool net_6lo_compress(struct net_buf *buf);
+bool net_6lo_compress(struct net_buf *buf, bool iphc);
 
 /**
  *  @brief Unompress IPv6 packet as per RFC 6282
diff --git a/tests/net/6lo/src/main.c b/tests/net/6lo/src/main.c
index 2bfbdca5c37..6e860235c26 100644
--- a/tests/net/6lo/src/main.c
+++ b/tests/net/6lo/src/main.c
@@ -374,7 +374,7 @@ static int test_6lo(struct net_6lo_data *data)
 	net_hexdump_frags("before-compression", buf);
 #endif
 
-	if (!net_6lo_compress(buf)) {
+	if (!net_6lo_compress(buf, true)) {
 		TC_PRINT("compression failed\n");
 		goto end;
 	}