Merge branch 'for-upstream' of git://git.kernel.org/pub/scm/linux/kernel/git/bluetooth/bluetooth-next

13 files changed, 1941 insertions, 797 deletions

diff --git a/net/bluetooth/6lowpan.c b/net/bluetooth/6lowpan.c
new file mode 100644
index 0000000..adb3ea0
--- /dev/null
+++ b/net/bluetooth/6lowpan.c
@@ -0,0 +1,860 @@
+/*
+   Copyright (c) 2013 Intel Corp.
+
+   This program is free software; you can redistribute it and/or modify
+   it under the terms of the GNU General Public License version 2 and
+   only version 2 as published by the Free Software Foundation.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+*/
+
+#include <linux/if_arp.h>
+#include <linux/netdevice.h>
+#include <linux/etherdevice.h>
+
+#include <net/ipv6.h>
+#include <net/ip6_route.h>
+#include <net/addrconf.h>
+
+#include <net/af_ieee802154.h> /* to get the address type */
+
+#include <net/bluetooth/bluetooth.h>
+#include <net/bluetooth/hci_core.h>
+#include <net/bluetooth/l2cap.h>
+
+#include "6lowpan.h"
+
+#include "../ieee802154/6lowpan.h" /* for the compression support */
+
+#define IFACE_NAME_TEMPLATE "bt%d"
+#define EUI64_ADDR_LEN 8
+
+struct skb_cb {
+	struct in6_addr addr;
+	struct l2cap_conn *conn;
+};
+#define lowpan_cb(skb) ((struct skb_cb *)((skb)->cb))
+
+/* The devices list contains those devices that we are acting
+ * as a proxy. The BT 6LoWPAN device is a virtual device that
+ * connects to the Bluetooth LE device. The real connection to
+ * BT device is done via l2cap layer. There exists one
+ * virtual device / one BT 6LoWPAN network (=hciX device).
+ * The list contains struct lowpan_dev elements.
+ */
+static LIST_HEAD(bt_6lowpan_devices);
+static DEFINE_RWLOCK(devices_lock);
+
+struct lowpan_peer {
+	struct list_head list;
+	struct l2cap_conn *conn;
+
+	/* peer addresses in various formats */
+	unsigned char eui64_addr[EUI64_ADDR_LEN];
+	struct in6_addr peer_addr;
+};
+
+struct lowpan_dev {
+	struct list_head list;
+
+	struct hci_dev *hdev;
+	struct net_device *netdev;
+	struct list_head peers;
+	atomic_t peer_count; /* number of items in peers list */
+
+	struct work_struct delete_netdev;
+	struct delayed_work notify_peers;
+};
+
+static inline struct lowpan_dev *lowpan_dev(const struct net_device *netdev)
+{
+	return netdev_priv(netdev);
+}
+
+static inline void peer_add(struct lowpan_dev *dev, struct lowpan_peer *peer)
+{
+	list_add(&peer->list, &dev->peers);
+	atomic_inc(&dev->peer_count);
+}
+
+static inline bool peer_del(struct lowpan_dev *dev, struct lowpan_peer *peer)
+{
+	list_del(&peer->list);
+
+	if (atomic_dec_and_test(&dev->peer_count)) {
+		BT_DBG("last peer");
+		return true;
+	}
+
+	return false;
+}
+
+static inline struct lowpan_peer *peer_lookup_ba(struct lowpan_dev *dev,
+						 bdaddr_t *ba, __u8 type)
+{
+	struct lowpan_peer *peer, *tmp;
+
+	BT_DBG("peers %d addr %pMR type %d", atomic_read(&dev->peer_count),
+	       ba, type);
+
+	list_for_each_entry_safe(peer, tmp, &dev->peers, list) {
+		BT_DBG("addr %pMR type %d",
+		       &peer->conn->hcon->dst, peer->conn->hcon->dst_type);
+
+		if (bacmp(&peer->conn->hcon->dst, ba))
+			continue;
+
+		if (type == peer->conn->hcon->dst_type)
+			return peer;
+	}
+
+	return NULL;
+}
+
+static inline struct lowpan_peer *peer_lookup_conn(struct lowpan_dev *dev,
+						   struct l2cap_conn *conn)
+{
+	struct lowpan_peer *peer, *tmp;
+
+	list_for_each_entry_safe(peer, tmp, &dev->peers, list) {
+		if (peer->conn == conn)
+			return peer;
+	}
+
+	return NULL;
+}
+
+static struct lowpan_peer *lookup_peer(struct l2cap_conn *conn)
+{
+	struct lowpan_dev *entry, *tmp;
+	struct lowpan_peer *peer = NULL;
+	unsigned long flags;
+
+	read_lock_irqsave(&devices_lock, flags);
+
+	list_for_each_entry_safe(entry, tmp, &bt_6lowpan_devices, list) {
+		peer = peer_lookup_conn(entry, conn);
+		if (peer)
+			break;
+	}
+
+	read_unlock_irqrestore(&devices_lock, flags);
+
+	return peer;
+}
+
+static struct lowpan_dev *lookup_dev(struct l2cap_conn *conn)
+{
+	struct lowpan_dev *entry, *tmp;
+	struct lowpan_dev *dev = NULL;
+	unsigned long flags;
+
+	read_lock_irqsave(&devices_lock, flags);
+
+	list_for_each_entry_safe(entry, tmp, &bt_6lowpan_devices, list) {
+		if (conn->hcon->hdev == entry->hdev) {
+			dev = entry;
+			break;
+		}
+	}
+
+	read_unlock_irqrestore(&devices_lock, flags);
+
+	return dev;
+}
+
+static int give_skb_to_upper(struct sk_buff *skb, struct net_device *dev)
+{
+	struct sk_buff *skb_cp;
+	int ret;
+
+	skb_cp = skb_copy(skb, GFP_ATOMIC);
+	if (!skb_cp)
+		return -ENOMEM;
+
+	ret = netif_rx(skb_cp);
+
+	BT_DBG("receive skb %d", ret);
+	if (ret < 0)
+		return NET_RX_DROP;
+
+	return ret;
+}
+
+static int process_data(struct sk_buff *skb, struct net_device *netdev,
+			struct l2cap_conn *conn)
+{
+	const u8 *saddr, *daddr;
+	u8 iphc0, iphc1;
+	struct lowpan_dev *dev;
+	struct lowpan_peer *peer;
+	unsigned long flags;
+
+	dev = lowpan_dev(netdev);
+
+	read_lock_irqsave(&devices_lock, flags);
+	peer = peer_lookup_conn(dev, conn);
+	read_unlock_irqrestore(&devices_lock, flags);
+	if (!peer)
+		goto drop;
+
+	saddr = peer->eui64_addr;
+	daddr = dev->netdev->dev_addr;
+
+	/* at least two bytes will be used for the encoding */
+	if (skb->len < 2)
+		goto drop;
+
+	if (lowpan_fetch_skb_u8(skb, &iphc0))
+		goto drop;
+
+	if (lowpan_fetch_skb_u8(skb, &iphc1))
+		goto drop;
+
+	return lowpan_process_data(skb, netdev,
+				   saddr, IEEE802154_ADDR_LONG, EUI64_ADDR_LEN,
+				   daddr, IEEE802154_ADDR_LONG, EUI64_ADDR_LEN,
+				   iphc0, iphc1, give_skb_to_upper);
+
+drop:
+	kfree_skb(skb);
+	return -EINVAL;
+}
+
+static int recv_pkt(struct sk_buff *skb, struct net_device *dev,
+		    struct l2cap_conn *conn)
+{
+	struct sk_buff *local_skb;
+	int ret;
+
+	if (!netif_running(dev))
+		goto drop;
+
+	if (dev->type != ARPHRD_6LOWPAN)
+		goto drop;
+
+	/* check that it's our buffer */
+	if (skb->data[0] == LOWPAN_DISPATCH_IPV6) {
+		/* Copy the packet so that the IPv6 header is
+		 * properly aligned.
+		 */
+		local_skb = skb_copy_expand(skb, NET_SKB_PAD - 1,
+					    skb_tailroom(skb), GFP_ATOMIC);
+		if (!local_skb)
+			goto drop;
+
+		local_skb->protocol = htons(ETH_P_IPV6);
+		local_skb->pkt_type = PACKET_HOST;
+
+		skb_reset_network_header(local_skb);
+		skb_set_transport_header(local_skb, sizeof(struct ipv6hdr));
+
+		if (give_skb_to_upper(local_skb, dev) != NET_RX_SUCCESS) {
+			kfree_skb(local_skb);
+			goto drop;
+		}
+
+		dev->stats.rx_bytes += skb->len;
+		dev->stats.rx_packets++;
+
+		kfree_skb(local_skb);
+		kfree_skb(skb);
+	} else {
+		switch (skb->data[0] & 0xe0) {
+		case LOWPAN_DISPATCH_IPHC:	/* ipv6 datagram */
+			local_skb = skb_clone(skb, GFP_ATOMIC);
+			if (!local_skb)
+				goto drop;
+
+			ret = process_data(local_skb, dev, conn);
+			if (ret != NET_RX_SUCCESS)
+				goto drop;
+
+			dev->stats.rx_bytes += skb->len;
+			dev->stats.rx_packets++;
+
+			kfree_skb(skb);
+			break;
+		default:
+			break;
+		}
+	}
+
+	return NET_RX_SUCCESS;
+
+drop:
+	kfree_skb(skb);
+	return NET_RX_DROP;
+}
+
+/* Packet from BT LE device */
+int bt_6lowpan_recv(struct l2cap_conn *conn, struct sk_buff *skb)
+{
+	struct lowpan_dev *dev;
+	struct lowpan_peer *peer;
+	int err;
+
+	peer = lookup_peer(conn);
+	if (!peer)
+		return -ENOENT;
+
+	dev = lookup_dev(conn);
+	if (!dev || !dev->netdev)
+		return -ENOENT;
+
+	err = recv_pkt(skb, dev->netdev, conn);
+	BT_DBG("recv pkt %d", err);
+
+	return err;
+}
+
+static inline int skbuff_copy(void *msg, int len, int count, int mtu,
+			      struct sk_buff *skb, struct net_device *dev)
+{
+	struct sk_buff **frag;
+	int sent = 0;
+
+	memcpy(skb_put(skb, count), msg, count);
+
+	sent += count;
+	msg  += count;
+	len  -= count;
+
+	dev->stats.tx_bytes += count;
+	dev->stats.tx_packets++;
+
+	raw_dump_table(__func__, "Sending", skb->data, skb->len);
+
+	/* Continuation fragments (no L2CAP header) */
+	frag = &skb_shinfo(skb)->frag_list;
+	while (len > 0) {
+		struct sk_buff *tmp;
+
+		count = min_t(unsigned int, mtu, len);
+
+		tmp = bt_skb_alloc(count, GFP_ATOMIC);
+		if (!tmp)
+			return -ENOMEM;
+
+		*frag = tmp;
+
+		memcpy(skb_put(*frag, count), msg, count);
+
+		raw_dump_table(__func__, "Sending fragment",
+			       (*frag)->data, count);
+
+		(*frag)->priority = skb->priority;
+
+		sent += count;
+		msg  += count;
+		len  -= count;
+
+		skb->len += (*frag)->len;
+		skb->data_len += (*frag)->len;
+
+		frag = &(*frag)->next;
+
+		dev->stats.tx_bytes += count;
+		dev->stats.tx_packets++;
+	}
+
+	return sent;
+}
+
+static struct sk_buff *create_pdu(struct l2cap_conn *conn, void *msg,
+				  size_t len, u32 priority,
+				  struct net_device *dev)
+{
+	struct sk_buff *skb;
+	int err, count;
+	struct l2cap_hdr *lh;
+
+	/* FIXME: This mtu check should be not needed and atm is only used for
+	 * testing purposes
+	 */
+	if (conn->mtu > (L2CAP_LE_MIN_MTU + L2CAP_HDR_SIZE))
+		conn->mtu = L2CAP_LE_MIN_MTU + L2CAP_HDR_SIZE;
+
+	count = min_t(unsigned int, (conn->mtu - L2CAP_HDR_SIZE), len);
+
+	BT_DBG("conn %p len %zu mtu %d count %d", conn, len, conn->mtu, count);
+
+	skb = bt_skb_alloc(count + L2CAP_HDR_SIZE, GFP_ATOMIC);
+	if (!skb)
+		return ERR_PTR(-ENOMEM);
+
+	skb->priority = priority;
+
+	lh = (struct l2cap_hdr *)skb_put(skb, L2CAP_HDR_SIZE);
+	lh->cid = cpu_to_le16(L2CAP_FC_6LOWPAN);
+	lh->len = cpu_to_le16(len);
+
+	err = skbuff_copy(msg, len, count, conn->mtu, skb, dev);
+	if (unlikely(err < 0)) {
+		kfree_skb(skb);
+		BT_DBG("skbuff copy %d failed", err);
+		return ERR_PTR(err);
+	}
+
+	return skb;
+}
+
+static int conn_send(struct l2cap_conn *conn,
+		     void *msg, size_t len, u32 priority,
+		     struct net_device *dev)
+{
+	struct sk_buff *skb;
+
+	skb = create_pdu(conn, msg, len, priority, dev);
+	if (IS_ERR(skb))
+		return -EINVAL;
+
+	BT_DBG("conn %p skb %p len %d priority %u", conn, skb, skb->len,
+	       skb->priority);
+
+	hci_send_acl(conn->hchan, skb, ACL_START);
+
+	return 0;
+}
+
+static void get_dest_bdaddr(struct in6_addr *ip6_daddr,
+			    bdaddr_t *addr, u8 *addr_type)
+{
+	u8 *eui64;
+
+	eui64 = ip6_daddr->s6_addr + 8;
+
+	addr->b[0] = eui64[7];
+	addr->b[1] = eui64[6];
+	addr->b[2] = eui64[5];
+	addr->b[3] = eui64[2];
+	addr->b[4] = eui64[1];
+	addr->b[5] = eui64[0];
+
+	addr->b[5] ^= 2;
+
+	/* Set universal/local bit to 0 */
+	if (addr->b[5] & 1) {
+		addr->b[5] &= ~1;
+		*addr_type = ADDR_LE_DEV_PUBLIC;
+	} else {
+		*addr_type = ADDR_LE_DEV_RANDOM;
+	}
+}
+
+static int header_create(struct sk_buff *skb, struct net_device *netdev,
+		         unsigned short type, const void *_daddr,
+		         const void *_saddr, unsigned int len)
+{
+	struct ipv6hdr *hdr;
+	struct lowpan_dev *dev;
+	struct lowpan_peer *peer;
+	bdaddr_t addr, *any = BDADDR_ANY;
+	u8 *saddr, *daddr = any->b;
+	u8 addr_type;
+
+	if (type != ETH_P_IPV6)
+		return -EINVAL;
+
+	hdr = ipv6_hdr(skb);
+
+	dev = lowpan_dev(netdev);
+
+	if (ipv6_addr_is_multicast(&hdr->daddr)) {
+		memcpy(&lowpan_cb(skb)->addr, &hdr->daddr,
+		       sizeof(struct in6_addr));
+		lowpan_cb(skb)->conn = NULL;
+	} else {
+		unsigned long flags;
+
+		/* Get destination BT device from skb.
+		 * If there is no such peer then discard the packet.
+		 */
+		get_dest_bdaddr(&hdr->daddr, &addr, &addr_type);
+
+		BT_DBG("dest addr %pMR type %d", &addr, addr_type);
+
+		read_lock_irqsave(&devices_lock, flags);
+		peer = peer_lookup_ba(dev, &addr, addr_type);
+		read_unlock_irqrestore(&devices_lock, flags);
+
+		if (!peer) {
+			BT_DBG("no such peer %pMR found", &addr);
+			return -ENOENT;
+		}
+
+		daddr = peer->eui64_addr;
+
+		memcpy(&lowpan_cb(skb)->addr, &hdr->daddr,
+		       sizeof(struct in6_addr));
+		lowpan_cb(skb)->conn = peer->conn;
+	}
+
+	saddr = dev->netdev->dev_addr;
+
+	return lowpan_header_compress(skb, netdev, type, daddr, saddr, len);
+}
+
+/* Packet to BT LE device */
+static int send_pkt(struct l2cap_conn *conn, const void *saddr,
+		    const void *daddr, struct sk_buff *skb,
+		    struct net_device *netdev)
+{
+	raw_dump_table(__func__, "raw skb data dump before fragmentation",
+		       skb->data, skb->len);
+
+	return conn_send(conn, skb->data, skb->len, 0, netdev);
+}
+
+static void send_mcast_pkt(struct sk_buff *skb, struct net_device *netdev)
+{
+	struct sk_buff *local_skb;
+	struct lowpan_dev *entry, *tmp;
+	unsigned long flags;
+
+	read_lock_irqsave(&devices_lock, flags);
+
+	list_for_each_entry_safe(entry, tmp, &bt_6lowpan_devices, list) {
+		struct lowpan_peer *pentry, *ptmp;
+		struct lowpan_dev *dev;
+
+		if (entry->netdev != netdev)
+			continue;
+
+		dev = lowpan_dev(entry->netdev);
+
+		list_for_each_entry_safe(pentry, ptmp, &dev->peers, list) {
+			local_skb = skb_clone(skb, GFP_ATOMIC);
+
+			send_pkt(pentry->conn, netdev->dev_addr,
+				 pentry->eui64_addr, local_skb, netdev);
+
+			kfree_skb(local_skb);
+		}
+	}
+
+	read_unlock_irqrestore(&devices_lock, flags);
+}
+
+static netdev_tx_t bt_xmit(struct sk_buff *skb, struct net_device *netdev)
+{
+	int err = 0;
+	unsigned char *eui64_addr;
+	struct lowpan_dev *dev;
+	struct lowpan_peer *peer;
+	bdaddr_t addr;
+	u8 addr_type;
+
+	if (ipv6_addr_is_multicast(&lowpan_cb(skb)->addr)) {
+		/* We need to send the packet to every device
+		 * behind this interface.
+		 */
+		send_mcast_pkt(skb, netdev);
+	} else {
+		unsigned long flags;
+
+		get_dest_bdaddr(&lowpan_cb(skb)->addr, &addr, &addr_type);
+		eui64_addr = lowpan_cb(skb)->addr.s6_addr + 8;
+		dev = lowpan_dev(netdev);
+
+		read_lock_irqsave(&devices_lock, flags);
+		peer = peer_lookup_ba(dev, &addr, addr_type);
+		read_unlock_irqrestore(&devices_lock, flags);
+
+		BT_DBG("xmit from %s to %pMR (%pI6c) peer %p", netdev->name,
+		       &addr, &lowpan_cb(skb)->addr, peer);
+
+		if (peer && peer->conn)
+			err = send_pkt(peer->conn, netdev->dev_addr,
+				       eui64_addr, skb, netdev);
+	}
+	dev_kfree_skb(skb);
+
+	if (err)
+		BT_DBG("ERROR: xmit failed (%d)", err);
+
+	return (err < 0) ? NET_XMIT_DROP : err;
+}
+
+static const struct net_device_ops netdev_ops = {
+	.ndo_start_xmit		= bt_xmit,
+};
+
+static struct header_ops header_ops = {
+	.create	= header_create,
+};
+
+static void netdev_setup(struct net_device *dev)
+{
+	dev->addr_len		= EUI64_ADDR_LEN;
+	dev->type		= ARPHRD_6LOWPAN;
+
+	dev->hard_header_len	= 0;
+	dev->needed_tailroom	= 0;
+	dev->mtu		= IPV6_MIN_MTU;
+	dev->tx_queue_len	= 0;
+	dev->flags		= IFF_RUNNING | IFF_POINTOPOINT;
+	dev->watchdog_timeo	= 0;
+
+	dev->netdev_ops		= &netdev_ops;
+	dev->header_ops		= &header_ops;
+	dev->destructor		= free_netdev;
+}
+
+static struct device_type bt_type = {
+	.name	= "bluetooth",
+};
+
+static void set_addr(u8 *eui, u8 *addr, u8 addr_type)
+{
+	/* addr is the BT address in little-endian format */
+	eui[0] = addr[5];
+	eui[1] = addr[4];
+	eui[2] = addr[3];
+	eui[3] = 0xFF;
+	eui[4] = 0xFE;
+	eui[5] = addr[2];
+	eui[6] = addr[1];
+	eui[7] = addr[0];
+
+	eui[0] ^= 2;
+
+	/* Universal/local bit set, RFC 4291 */
+	if (addr_type == ADDR_LE_DEV_PUBLIC)
+		eui[0] |= 1;
+	else
+		eui[0] &= ~1;
+}
+
+static void set_dev_addr(struct net_device *netdev, bdaddr_t *addr,
+		         u8 addr_type)
+{
+	netdev->addr_assign_type = NET_ADDR_PERM;
+	set_addr(netdev->dev_addr, addr->b, addr_type);
+	netdev->dev_addr[0] ^= 2;
+}
+
+static void ifup(struct net_device *netdev)
+{
+	int err;
+
+	rtnl_lock();
+	err = dev_open(netdev);
+	if (err < 0)
+		BT_INFO("iface %s cannot be opened (%d)", netdev->name, err);
+	rtnl_unlock();
+}
+
+static void do_notify_peers(struct work_struct *work)
+{
+	struct lowpan_dev *dev = container_of(work, struct lowpan_dev,
+					      notify_peers.work);
+
+	netdev_notify_peers(dev->netdev); /* send neighbour adv at startup */
+}
+
+static bool is_bt_6lowpan(struct hci_conn *hcon)
+{
+	if (hcon->type != LE_LINK)
+		return false;
+
+	return test_bit(HCI_CONN_6LOWPAN, &hcon->flags);
+}
+
+static int add_peer_conn(struct l2cap_conn *conn, struct lowpan_dev *dev)
+{
+	struct lowpan_peer *peer;
+	unsigned long flags;
+
+	peer = kzalloc(sizeof(*peer), GFP_ATOMIC);
+	if (!peer)
+		return -ENOMEM;
+
+	peer->conn = conn;
+	memset(&peer->peer_addr, 0, sizeof(struct in6_addr));
+
+	/* RFC 2464 ch. 5 */
+	peer->peer_addr.s6_addr[0] = 0xFE;
+	peer->peer_addr.s6_addr[1] = 0x80;
+	set_addr((u8 *)&peer->peer_addr.s6_addr + 8, conn->hcon->dst.b,
+	         conn->hcon->dst_type);
+
+	memcpy(&peer->eui64_addr, (u8 *)&peer->peer_addr.s6_addr + 8,
+	       EUI64_ADDR_LEN);
+	peer->eui64_addr[0] ^= 2; /* second bit-flip (Universe/Local)
+				   * is done according RFC2464
+				   */
+
+	raw_dump_inline(__func__, "peer IPv6 address",
+			(unsigned char *)&peer->peer_addr, 16);
+	raw_dump_inline(__func__, "peer EUI64 address", peer->eui64_addr, 8);
+
+	write_lock_irqsave(&devices_lock, flags);
+	INIT_LIST_HEAD(&peer->list);
+	peer_add(dev, peer);
+	write_unlock_irqrestore(&devices_lock, flags);
+
+	/* Notifying peers about us needs to be done without locks held */
+	INIT_DELAYED_WORK(&dev->notify_peers, do_notify_peers);
+	schedule_delayed_work(&dev->notify_peers, msecs_to_jiffies(100));
+
+	return 0;
+}
+
+/* This gets called when BT LE 6LoWPAN device is connected. We then
+ * create network device that acts as a proxy between BT LE device
+ * and kernel network stack.
+ */
+int bt_6lowpan_add_conn(struct l2cap_conn *conn)
+{
+	struct lowpan_peer *peer = NULL;
+	struct lowpan_dev *dev;
+	struct net_device *netdev;
+	int err = 0;
+	unsigned long flags;
+
+	if (!is_bt_6lowpan(conn->hcon))
+		return 0;
+
+	peer = lookup_peer(conn);
+	if (peer)
+		return -EEXIST;
+
+	dev = lookup_dev(conn);
+	if (dev)
+		return add_peer_conn(conn, dev);
+
+	netdev = alloc_netdev(sizeof(*dev), IFACE_NAME_TEMPLATE, netdev_setup);
+	if (!netdev)
+		return -ENOMEM;
+
+	set_dev_addr(netdev, &conn->hcon->src, conn->hcon->src_type);
+
+	netdev->netdev_ops = &netdev_ops;
+	SET_NETDEV_DEV(netdev, &conn->hcon->dev);
+	SET_NETDEV_DEVTYPE(netdev, &bt_type);
+
+	err = register_netdev(netdev);
+	if (err < 0) {
+		BT_INFO("register_netdev failed %d", err);
+		free_netdev(netdev);
+		goto out;
+	}
+
+	BT_DBG("ifindex %d peer bdaddr %pMR my addr %pMR",
+	       netdev->ifindex, &conn->hcon->dst, &conn->hcon->src);
+	set_bit(__LINK_STATE_PRESENT, &netdev->state);
+
+	dev = netdev_priv(netdev);
+	dev->netdev = netdev;
+	dev->hdev = conn->hcon->hdev;
+	INIT_LIST_HEAD(&dev->peers);
+
+	write_lock_irqsave(&devices_lock, flags);
+	INIT_LIST_HEAD(&dev->list);
+	list_add(&dev->list, &bt_6lowpan_devices);
+	write_unlock_irqrestore(&devices_lock, flags);
+
+	ifup(netdev);
+
+	return add_peer_conn(conn, dev);
+
+out:
+	return err;
+}
+
+static void delete_netdev(struct work_struct *work)
+{
+	struct lowpan_dev *entry = container_of(work, struct lowpan_dev,
+						delete_netdev);
+
+	unregister_netdev(entry->netdev);
+
+	/* The entry pointer is deleted in device_event() */
+}
+
+int bt_6lowpan_del_conn(struct l2cap_conn *conn)
+{
+	struct lowpan_dev *entry, *tmp;
+	struct lowpan_dev *dev = NULL;
+	struct lowpan_peer *peer;
+	int err = -ENOENT;
+	unsigned long flags;
+	bool last = false;
+
+	if (!conn || !is_bt_6lowpan(conn->hcon))
+		return 0;
+
+	write_lock_irqsave(&devices_lock, flags);
+
+	list_for_each_entry_safe(entry, tmp, &bt_6lowpan_devices, list) {
+		dev = lowpan_dev(entry->netdev);
+		peer = peer_lookup_conn(dev, conn);
+		if (peer) {
+			last = peer_del(dev, peer);
+			err = 0;
+			break;
+		}
+	}
+
+	if (!err && last && dev && !atomic_read(&dev->peer_count)) {
+		write_unlock_irqrestore(&devices_lock, flags);
+
+		cancel_delayed_work_sync(&dev->notify_peers);
+
+		/* bt_6lowpan_del_conn() is called with hci dev lock held which
+		 * means that we must delete the netdevice in worker thread.
+		 */
+		INIT_WORK(&entry->delete_netdev, delete_netdev);
+		schedule_work(&entry->delete_netdev);
+	} else {
+		write_unlock_irqrestore(&devices_lock, flags);
+	}
+
+	return err;
+}
+
+static int device_event(struct notifier_block *unused,
+			unsigned long event, void *ptr)
+{
+	struct net_device *netdev = netdev_notifier_info_to_dev(ptr);
+	struct lowpan_dev *entry, *tmp;
+	unsigned long flags;
+
+	if (netdev->type != ARPHRD_6LOWPAN)
+		return NOTIFY_DONE;
+
+	switch (event) {
+	case NETDEV_UNREGISTER:
+		write_lock_irqsave(&devices_lock, flags);
+		list_for_each_entry_safe(entry, tmp, &bt_6lowpan_devices,
+					 list) {
+			if (entry->netdev == netdev) {
+				list_del(&entry->list);
+				kfree(entry);
+				break;
+			}
+		}
+		write_unlock_irqrestore(&devices_lock, flags);
+		break;
+	}
+
+	return NOTIFY_DONE;
+}
+
+static struct notifier_block bt_6lowpan_dev_notifier = {
+	.notifier_call = device_event,
+};
+
+int bt_6lowpan_init(void)
+{
+	return register_netdevice_notifier(&bt_6lowpan_dev_notifier);
+}
+
+void bt_6lowpan_cleanup(void)
+{
+	unregister_netdevice_notifier(&bt_6lowpan_dev_notifier);
+}
diff --git a/net/bluetooth/6lowpan.h b/net/bluetooth/6lowpan.h
new file mode 100644
index 0000000..680eac8
--- /dev/null
+++ b/net/bluetooth/6lowpan.h
@@ -0,0 +1,26 @@
+/*
+   Copyright (c) 2013 Intel Corp.
+
+   This program is free software; you can redistribute it and/or modify
+   it under the terms of the GNU General Public License version 2 and
+   only version 2 as published by the Free Software Foundation.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+*/
+
+#ifndef __6LOWPAN_H
+#define __6LOWPAN_H
+
+#include <linux/skbuff.h>
+#include <net/bluetooth/l2cap.h>
+
+int bt_6lowpan_recv(struct l2cap_conn *conn, struct sk_buff *skb);
+int bt_6lowpan_add_conn(struct l2cap_conn *conn);
+int bt_6lowpan_del_conn(struct l2cap_conn *conn);
+int bt_6lowpan_init(void);
+void bt_6lowpan_cleanup(void);
+
+#endif /* __6LOWPAN_H */
diff --git a/net/bluetooth/Makefile b/net/bluetooth/Makefile
index 6a791e7..cc6827e 100644
--- a/net/bluetooth/Makefile
+++ b/net/bluetooth/Makefile
@@ -10,6 +10,10 @@ obj-$(CONFIG_BT_HIDP)	+= hidp/
 
 bluetooth-y := af_bluetooth.o hci_core.o hci_conn.o hci_event.o mgmt.o \
 	hci_sock.o hci_sysfs.o l2cap_core.o l2cap_sock.o smp.o sco.o lib.o \
-	a2mp.o amp.o
+	a2mp.o amp.o 6lowpan.o
+
+ifeq ($(CONFIG_IEEE802154_6LOWPAN),)
+  bluetooth-y +=  ../ieee802154/6lowpan_iphc.o
+endif
 
 subdir-ccflags-y += -D__CHECK_ENDIAN__
diff --git a/net/bluetooth/hci_core.c b/net/bluetooth/hci_core.c
index 8b8b5f8..5e8663c 100644
--- a/net/bluetooth/hci_core.c
+++ b/net/bluetooth/hci_core.c
@@ -636,6 +636,49 @@ static int conn_max_interval_get(void *data, u64 *val)
 DEFINE_SIMPLE_ATTRIBUTE(conn_max_interval_fops, conn_max_interval_get,
 			conn_max_interval_set, "%llu\n");
 
+static ssize_t lowpan_read(struct file *file, char __user *user_buf,
+			   size_t count, loff_t *ppos)
+{
+	struct hci_dev *hdev = file->private_data;
+	char buf[3];
+
+	buf[0] = test_bit(HCI_6LOWPAN_ENABLED, &hdev->dev_flags) ? 'Y' : 'N';
+	buf[1] = '\n';
+	buf[2] = '\0';
+	return simple_read_from_buffer(user_buf, count, ppos, buf, 2);
+}
+
+static ssize_t lowpan_write(struct file *fp, const char __user *user_buffer,
+			    size_t count, loff_t *position)
+{
+	struct hci_dev *hdev = fp->private_data;
+	bool enable;
+	char buf[32];
+	size_t buf_size = min(count, (sizeof(buf)-1));
+
+	if (copy_from_user(buf, user_buffer, buf_size))
+		return -EFAULT;
+
+	buf[buf_size] = '\0';
+
+	if (strtobool(buf, &enable) < 0)
+		return -EINVAL;
+
+	if (enable == test_bit(HCI_6LOWPAN_ENABLED, &hdev->dev_flags))
+		return -EALREADY;
+
+	change_bit(HCI_6LOWPAN_ENABLED, &hdev->dev_flags);
+
+	return count;
+}
+
+static const struct file_operations lowpan_debugfs_fops = {
+	.open		= simple_open,
+	.read		= lowpan_read,
+	.write		= lowpan_write,
+	.llseek		= default_llseek,
+};
+
 /* ---- HCI requests ---- */
 
 static void hci_req_sync_complete(struct hci_dev *hdev, u8 result)
@@ -1261,8 +1304,13 @@ static void hci_init3_req(struct hci_request *req, unsigned long opt)
 	 * as supported send it. If not supported assume that the controller
 	 * does not have actual support for stored link keys which makes this
 	 * command redundant anyway.
+	 *
+	 * Some controllers indicate that they support handling deleting
+	 * stored link keys, but they don't. The quirk lets a driver
+	 * just disable this command.
 	 */
-	if (hdev->commands[6] & 0x80) {
+	if (hdev->commands[6] & 0x80 &&
+	    !test_bit(HCI_QUIRK_BROKEN_STORED_LINK_KEY, &hdev->quirks)) {
 		struct hci_cp_delete_stored_link_key cp;
 
 		bacpy(&cp.bdaddr, BDADDR_ANY);
@@ -1406,6 +1454,8 @@ static int __hci_init(struct hci_dev *hdev)
 				    hdev, &conn_min_interval_fops);
 		debugfs_create_file("conn_max_interval", 0644, hdev->debugfs,
 				    hdev, &conn_max_interval_fops);
+		debugfs_create_file("6lowpan", 0644, hdev->debugfs, hdev,
+				    &lowpan_debugfs_fops);
 	}
 
 	return 0;
diff --git a/net/bluetooth/hci_event.c b/net/bluetooth/hci_event.c
index 5fb3df66..5f81245 100644
--- a/net/bluetooth/hci_event.c
+++ b/net/bluetooth/hci_event.c
@@ -3533,6 +3533,9 @@ static void hci_le_conn_complete_evt(struct hci_dev *hdev, struct sk_buff *skb)
 	conn->handle = __le16_to_cpu(ev->handle);
 	conn->state = BT_CONNECTED;
 
+	if (test_bit(HCI_6LOWPAN_ENABLED, &hdev->dev_flags))
+		set_bit(HCI_CONN_6LOWPAN, &conn->flags);
+
 	hci_conn_add_sysfs(conn);
 
 	hci_proto_connect_cfm(conn, ev->status);
diff --git a/net/bluetooth/l2cap_core.c b/net/bluetooth/l2cap_core.c
index b6bca64..b0ad2c7 100644
--- a/net/bluetooth/l2cap_core.c
+++ b/net/bluetooth/l2cap_core.c
@@ -40,6 +40,7 @@
 #include "smp.h"
 #include "a2mp.h"
 #include "amp.h"
+#include "6lowpan.h"
 
 bool disable_ertm;
 
@@ -1468,6 +1469,8 @@ static void l2cap_le_conn_ready(struct l2cap_conn *conn)
 
 	BT_DBG("");
 
+	bt_6lowpan_add_conn(conn);
+
 	/* Check if we have socket listening on cid */
 	pchan = l2cap_global_chan_by_scid(BT_LISTEN, L2CAP_CID_ATT,
 					  &hcon->src, &hcon->dst);
@@ -7119,6 +7122,10 @@ static void l2cap_recv_frame(struct l2cap_conn *conn, struct sk_buff *skb)
 			l2cap_conn_del(conn->hcon, EACCES);
 		break;
 
+	case L2CAP_FC_6LOWPAN:
+		bt_6lowpan_recv(conn, skb);
+		break;
+
 	default:
 		l2cap_data_channel(conn, cid, skb);
 		break;
@@ -7186,6 +7193,8 @@ void l2cap_disconn_cfm(struct hci_conn *hcon, u8 reason)
 {
 	BT_DBG("hcon %p reason %d", hcon, reason);
 
+	bt_6lowpan_del_conn(hcon->l2cap_data);
+
 	l2cap_conn_del(hcon, bt_to_errno(reason));
 }
 
@@ -7467,11 +7476,14 @@ int __init l2cap_init(void)
 	debugfs_create_u16("l2cap_le_default_mps", 0466, bt_debugfs,
 			   &le_default_mps);
 
+	bt_6lowpan_init();
+
 	return 0;
 }
 
 void l2cap_exit(void)
 {
+	bt_6lowpan_cleanup();
 	debugfs_remove(l2cap_debugfs);
 	l2cap_cleanup_sockets();
 }
diff --git a/net/bluetooth/l2cap_sock.c b/net/bluetooth/l2cap_sock.c
index e7806e6..20ef748 100644
--- a/net/bluetooth/l2cap_sock.c
+++ b/net/bluetooth/l2cap_sock.c
@@ -147,6 +147,9 @@ static int l2cap_sock_bind(struct socket *sock, struct sockaddr *addr, int alen)
 		    __le16_to_cpu(la.l2_psm) == L2CAP_PSM_RFCOMM)
 			chan->sec_level = BT_SECURITY_SDP;
 		break;
+	case L2CAP_CHAN_RAW:
+		chan->sec_level = BT_SECURITY_SDP;
+		break;
 	}
 
 	bacpy(&chan->src, &la.l2_bdaddr);
diff --git a/net/bluetooth/rfcomm/tty.c b/net/bluetooth/rfcomm/tty.c
index 84fcf9f..f9c0980a 100644
--- a/net/bluetooth/rfcomm/tty.c
+++ b/net/bluetooth/rfcomm/tty.c
@@ -58,6 +58,7 @@ struct rfcomm_dev {
 	uint			modem_status;
 
 	struct rfcomm_dlc	*dlc;
+	wait_queue_head_t       conn_wait;
 
 	struct device		*tty_dev;
 
@@ -103,20 +104,60 @@ static void rfcomm_dev_destruct(struct tty_port *port)
 	module_put(THIS_MODULE);
 }
 
-/* device-specific initialization: open the dlc */
-static int rfcomm_dev_activate(struct tty_port *port, struct tty_struct *tty)
+static struct device *rfcomm_get_device(struct rfcomm_dev *dev)
 {
-	struct rfcomm_dev *dev = container_of(port, struct rfcomm_dev, port);
+	struct hci_dev *hdev;
+	struct hci_conn *conn;
 
-	return rfcomm_dlc_open(dev->dlc, &dev->src, &dev->dst, dev->channel);
+	hdev = hci_get_route(&dev->dst, &dev->src);
+	if (!hdev)
+		return NULL;
+
+	conn = hci_conn_hash_lookup_ba(hdev, ACL_LINK, &dev->dst);
+
+	hci_dev_put(hdev);
+
+	return conn ? &conn->dev : NULL;
 }
 
-/* we block the open until the dlc->state becomes BT_CONNECTED */
-static int rfcomm_dev_carrier_raised(struct tty_port *port)
+/* device-specific initialization: open the dlc */
+static int rfcomm_dev_activate(struct tty_port *port, struct tty_struct *tty)
 {
 	struct rfcomm_dev *dev = container_of(port, struct rfcomm_dev, port);
+	DEFINE_WAIT(wait);
+	int err;
+
+	err = rfcomm_dlc_open(dev->dlc, &dev->src, &dev->dst, dev->channel);
+	if (err)
+		return err;
+
+	while (1) {
+		prepare_to_wait(&dev->conn_wait, &wait, TASK_INTERRUPTIBLE);
+
+		if (dev->dlc->state == BT_CLOSED) {
+			err = -dev->err;
+			break;
+		}
+
+		if (dev->dlc->state == BT_CONNECTED)
+			break;
+
+		if (signal_pending(current)) {
+			err = -ERESTARTSYS;
+			break;
+		}
+
+		tty_unlock(tty);
+		schedule();
+		tty_lock(tty);
+	}
+	finish_wait(&dev->conn_wait, &wait);
+
+	if (!err)
+		device_move(dev->tty_dev, rfcomm_get_device(dev),
+			    DPM_ORDER_DEV_AFTER_PARENT);
 
-	return (dev->dlc->state == BT_CONNECTED);
+	return err;
 }
 
 /* device-specific cleanup: close the dlc */
@@ -135,7 +176,6 @@ static const struct tty_port_operations rfcomm_port_ops = {
 	.destruct = rfcomm_dev_destruct,
 	.activate = rfcomm_dev_activate,
 	.shutdown = rfcomm_dev_shutdown,
-	.carrier_raised = rfcomm_dev_carrier_raised,
 };
 
 static struct rfcomm_dev *__rfcomm_dev_get(int id)
@@ -169,22 +209,6 @@ static struct rfcomm_dev *rfcomm_dev_get(int id)
 	return dev;
 }
 
-static struct device *rfcomm_get_device(struct rfcomm_dev *dev)
-{
-	struct hci_dev *hdev;
-	struct hci_conn *conn;
-
-	hdev = hci_get_route(&dev->dst, &dev->src);
-	if (!hdev)
-		return NULL;
-
-	conn = hci_conn_hash_lookup_ba(hdev, ACL_LINK, &dev->dst);
-
-	hci_dev_put(hdev);
-
-	return conn ? &conn->dev : NULL;
-}
-
 static ssize_t show_address(struct device *tty_dev, struct device_attribute *attr, char *buf)
 {
 	struct rfcomm_dev *dev = dev_get_drvdata(tty_dev);
@@ -258,6 +282,7 @@ static int rfcomm_dev_add(struct rfcomm_dev_req *req, struct rfcomm_dlc *dlc)
 
 	tty_port_init(&dev->port);
 	dev->port.ops = &rfcomm_port_ops;
+	init_waitqueue_head(&dev->conn_wait);
 
 	skb_queue_head_init(&dev->pending);
 
@@ -437,7 +462,8 @@ static int rfcomm_release_dev(void __user *arg)
 		tty_kref_put(tty);
 	}
 
-	if (!test_and_set_bit(RFCOMM_TTY_RELEASED, &dev->flags))
+	if (!test_bit(RFCOMM_RELEASE_ONHUP, &dev->flags) &&
+	    !test_and_set_bit(RFCOMM_TTY_RELEASED, &dev->flags))
 		tty_port_put(&dev->port);
 
 	tty_port_put(&dev->port);
@@ -575,12 +601,9 @@ static void rfcomm_dev_state_change(struct rfcomm_dlc *dlc, int err)
 	BT_DBG("dlc %p dev %p err %d", dlc, dev, err);
 
 	dev->err = err;
-	if (dlc->state == BT_CONNECTED) {
-		device_move(dev->tty_dev, rfcomm_get_device(dev),
-			    DPM_ORDER_DEV_AFTER_PARENT);
+	wake_up_interruptible(&dev->conn_wait);
 
-		wake_up_interruptible(&dev->port.open_wait);
-	} else if (dlc->state == BT_CLOSED)
+	if (dlc->state == BT_CLOSED)
 		tty_port_tty_hangup(&dev->port, false);
 }
 
@@ -670,10 +693,20 @@ static int rfcomm_tty_install(struct tty_driver *driver, struct tty_struct *tty)
 
 	/* install the tty_port */
 	err = tty_port_install(&dev->port, driver, tty);
-	if (err)
+	if (err) {
 		rfcomm_tty_cleanup(tty);
+		return err;
+	}
 
-	return err;
+	/* take over the tty_port reference if the port was created with the
+	 * flag RFCOMM_RELEASE_ONHUP. This will force the release of the port
+	 * when the last process closes the tty. The behaviour is expected by
+	 * userspace.
+	 */
+	if (test_bit(RFCOMM_RELEASE_ONHUP, &dev->flags))
+		tty_port_put(&dev->port);
+
+	return 0;
 }
 
 static int rfcomm_tty_open(struct tty_struct *tty, struct file *filp)
@@ -1010,10 +1043,6 @@ static void rfcomm_tty_hangup(struct tty_struct *tty)
 	BT_DBG("tty %p dev %p", tty, dev);
 
 	tty_port_hangup(&dev->port);
-
-	if (test_bit(RFCOMM_RELEASE_ONHUP, &dev->flags) &&
-	    !test_and_set_bit(RFCOMM_TTY_RELEASED, &dev->flags))
-		tty_port_put(&dev->port);
 }
 
 static int rfcomm_tty_tiocmget(struct tty_struct *tty)
@@ -1096,7 +1125,7 @@ int __init rfcomm_init_ttys(void)
 	rfcomm_tty_driver->subtype	= SERIAL_TYPE_NORMAL;
 	rfcomm_tty_driver->flags	= TTY_DRIVER_REAL_RAW | TTY_DRIVER_DYNAMIC_DEV;
 	rfcomm_tty_driver->init_termios	= tty_std_termios;
-	rfcomm_tty_driver->init_termios.c_cflag	= B9600 | CS8 | CREAD | HUPCL;
+	rfcomm_tty_driver->init_termios.c_cflag	= B9600 | CS8 | CREAD | HUPCL | CLOCAL;
 	rfcomm_tty_driver->init_termios.c_lflag &= ~ICANON;
 	tty_set_operations(rfcomm_tty_driver, &rfcomm_ops);
 
diff --git a/net/ieee802154/6lowpan.c b/net/ieee802154/6lowpan.c
index 459e200..48b25c0 100644
--- a/net/ieee802154/6lowpan.c
+++ b/net/ieee802154/6lowpan.c
@@ -62,9 +62,6 @@
 
 #include "6lowpan.h"
 
-/* TTL uncompression values */
-static const u8 lowpan_ttl_values[] = {0, 1, 64, 255};
-
 static LIST_HEAD(lowpan_devices);
 
 /* private device info */
@@ -104,378 +101,14 @@ static inline void lowpan_address_flip(u8 *src, u8 *dest)
 		(dest)[IEEE802154_ADDR_LEN - i - 1] = (src)[i];
 }
 
-/* list of all 6lowpan devices, uses for package delivering */
-/* print data in line */
-static inline void lowpan_raw_dump_inline(const char *caller, char *msg,
-				   unsigned char *buf, int len)
-{
-#ifdef DEBUG
-	if (msg)
-		pr_debug("(%s) %s: ", caller, msg);
-	print_hex_dump(KERN_DEBUG, "", DUMP_PREFIX_NONE,
-		       16, 1, buf, len, false);
-#endif /* DEBUG */
-}
-
-/*
- * print data in a table format:
- *
- * addr: xx xx xx xx xx xx
- * addr: xx xx xx xx xx xx
- * ...
- */
-static inline void lowpan_raw_dump_table(const char *caller, char *msg,
-				   unsigned char *buf, int len)
-{
-#ifdef DEBUG
-	if (msg)
-		pr_debug("(%s) %s:\n", caller, msg);
-	print_hex_dump(KERN_DEBUG, "\t", DUMP_PREFIX_OFFSET,
-		       16, 1, buf, len, false);
-#endif /* DEBUG */
-}
-
-static u8
-lowpan_compress_addr_64(u8 **hc06_ptr, u8 shift, const struct in6_addr *ipaddr,
-		 const unsigned char *lladdr)
-{
-	u8 val = 0;
-
-	if (is_addr_mac_addr_based(ipaddr, lladdr))
-		val = 3; /* 0-bits */
-	else if (lowpan_is_iid_16_bit_compressable(ipaddr)) {
-		/* compress IID to 16 bits xxxx::XXXX */
-		memcpy(*hc06_ptr, &ipaddr->s6_addr16[7], 2);
-		*hc06_ptr += 2;
-		val = 2; /* 16-bits */
-	} else {
-		/* do not compress IID => xxxx::IID */
-		memcpy(*hc06_ptr, &ipaddr->s6_addr16[4], 8);
-		*hc06_ptr += 8;
-		val = 1; /* 64-bits */
-	}
-
-	return rol8(val, shift);
-}
-
-/*
- * Uncompress address function for source and
- * destination address(non-multicast).
- *
- * address_mode is sam value or dam value.
- */
-static int
-lowpan_uncompress_addr(struct sk_buff *skb,
-		struct in6_addr *ipaddr,
-		const u8 address_mode,
-		const struct ieee802154_addr *lladdr)
-{
-	bool fail;
-
-	switch (address_mode) {
-	case LOWPAN_IPHC_ADDR_00:
-		/* for global link addresses */
-		fail = lowpan_fetch_skb(skb, ipaddr->s6_addr, 16);
-		break;
-	case LOWPAN_IPHC_ADDR_01:
-		/* fe:80::XXXX:XXXX:XXXX:XXXX */
-		ipaddr->s6_addr[0] = 0xFE;
-		ipaddr->s6_addr[1] = 0x80;
-		fail = lowpan_fetch_skb(skb, &ipaddr->s6_addr[8], 8);
-		break;
-	case LOWPAN_IPHC_ADDR_02:
-		/* fe:80::ff:fe00:XXXX */
-		ipaddr->s6_addr[0] = 0xFE;
-		ipaddr->s6_addr[1] = 0x80;
-		ipaddr->s6_addr[11] = 0xFF;
-		ipaddr->s6_addr[12] = 0xFE;
-		fail = lowpan_fetch_skb(skb, &ipaddr->s6_addr[14], 2);
-		break;
-	case LOWPAN_IPHC_ADDR_03:
-		fail = false;
-		switch (lladdr->addr_type) {
-		case IEEE802154_ADDR_LONG:
-			/* fe:80::XXXX:XXXX:XXXX:XXXX
-			 *        \_________________/
-			 *              hwaddr
-			 */
-			ipaddr->s6_addr[0] = 0xFE;
-			ipaddr->s6_addr[1] = 0x80;
-			memcpy(&ipaddr->s6_addr[8], lladdr->hwaddr,
-					IEEE802154_ADDR_LEN);
-			/* second bit-flip (Universe/Local)
-			 * is done according RFC2464
-			 */
-			ipaddr->s6_addr[8] ^= 0x02;
-			break;
-		case IEEE802154_ADDR_SHORT:
-			/* fe:80::ff:fe00:XXXX
-			 *		  \__/
-			 *	       short_addr
-			 *
-			 * Universe/Local bit is zero.
-			 */
-			ipaddr->s6_addr[0] = 0xFE;
-			ipaddr->s6_addr[1] = 0x80;
-			ipaddr->s6_addr[11] = 0xFF;
-			ipaddr->s6_addr[12] = 0xFE;
-			ipaddr->s6_addr16[7] = htons(lladdr->short_addr);
-			break;
-		default:
-			pr_debug("Invalid addr_type set\n");
-			return -EINVAL;
-		}
-		break;
-	default:
-		pr_debug("Invalid address mode value: 0x%x\n", address_mode);
-		return -EINVAL;
-	}
-
-	if (fail) {
-		pr_debug("Failed to fetch skb data\n");
-		return -EIO;
-	}
-
-	lowpan_raw_dump_inline(NULL, "Reconstructed ipv6 addr is:\n",
-			ipaddr->s6_addr, 16);
-
-	return 0;
-}
-
-/* Uncompress address function for source context
- * based address(non-multicast).
- */
-static int
-lowpan_uncompress_context_based_src_addr(struct sk_buff *skb,
-		struct in6_addr *ipaddr,
-		const u8 sam)
-{
-	switch (sam) {
-	case LOWPAN_IPHC_ADDR_00:
-		/* unspec address ::
-		 * Do nothing, address is already ::
-		 */
-		break;
-	case LOWPAN_IPHC_ADDR_01:
-		/* TODO */
-	case LOWPAN_IPHC_ADDR_02:
-		/* TODO */
-	case LOWPAN_IPHC_ADDR_03:
-		/* TODO */
-		netdev_warn(skb->dev, "SAM value 0x%x not supported\n", sam);
-		return -EINVAL;
-	default:
-		pr_debug("Invalid sam value: 0x%x\n", sam);
-		return -EINVAL;
-	}
-
-	lowpan_raw_dump_inline(NULL,
-			"Reconstructed context based ipv6 src addr is:\n",
-			ipaddr->s6_addr, 16);
-
-	return 0;
-}
-
-/* Uncompress function for multicast destination address,
- * when M bit is set.
- */
-static int
-lowpan_uncompress_multicast_daddr(struct sk_buff *skb,
-		struct in6_addr *ipaddr,
-		const u8 dam)
-{
-	bool fail;
-
-	switch (dam) {
-	case LOWPAN_IPHC_DAM_00:
-		/* 00:  128 bits.  The full address
-		 * is carried in-line.
-		 */
-		fail = lowpan_fetch_skb(skb, ipaddr->s6_addr, 16);
-		break;
-	case LOWPAN_IPHC_DAM_01:
-		/* 01:  48 bits.  The address takes
-		 * the form ffXX::00XX:XXXX:XXXX.
-		 */
-		ipaddr->s6_addr[0] = 0xFF;
-		fail = lowpan_fetch_skb(skb, &ipaddr->s6_addr[1], 1);
-		fail |= lowpan_fetch_skb(skb, &ipaddr->s6_addr[11], 5);
-		break;
-	case LOWPAN_IPHC_DAM_10:
-		/* 10:  32 bits.  The address takes
-		 * the form ffXX::00XX:XXXX.
-		 */
-		ipaddr->s6_addr[0] = 0xFF;
-		fail = lowpan_fetch_skb(skb, &ipaddr->s6_addr[1], 1);
-		fail |= lowpan_fetch_skb(skb, &ipaddr->s6_addr[13], 3);
-		break;
-	case LOWPAN_IPHC_DAM_11:
-		/* 11:  8 bits.  The address takes
-		 * the form ff02::00XX.
-		 */
-		ipaddr->s6_addr[0] = 0xFF;
-		ipaddr->s6_addr[1] = 0x02;
-		fail = lowpan_fetch_skb(skb, &ipaddr->s6_addr[15], 1);
-		break;
-	default:
-		pr_debug("DAM value has a wrong value: 0x%x\n", dam);
-		return -EINVAL;
-	}
-
-	if (fail) {
-		pr_debug("Failed to fetch skb data\n");
-		return -EIO;
-	}
-
-	lowpan_raw_dump_inline(NULL, "Reconstructed ipv6 multicast addr is:\n",
-			ipaddr->s6_addr, 16);
-
-	return 0;
-}
-
-static void
-lowpan_compress_udp_header(u8 **hc06_ptr, struct sk_buff *skb)
-{
-	struct udphdr *uh = udp_hdr(skb);
-
-	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("UDP header: both ports compression to 4 bits\n");
-		**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("UDP header: remove 8 bits of dest\n");
-		**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("UDP header: remove 8 bits of source\n");
-		**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("UDP header: can't compress\n");
-		**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;
-
-	/* skip the UDP header */
-	skb_pull(skb, sizeof(struct udphdr));
-}
-
-static inline int lowpan_fetch_skb_u8(struct sk_buff *skb, u8 *val)
-{
-	if (unlikely(!pskb_may_pull(skb, 1)))
-		return -EINVAL;
-
-	*val = skb->data[0];
-	skb_pull(skb, 1);
-
-	return 0;
-}
-
-static inline int lowpan_fetch_skb_u16(struct sk_buff *skb, u16 *val)
-{
-	if (unlikely(!pskb_may_pull(skb, 2)))
-		return -EINVAL;
-
-	*val = (skb->data[0] << 8) | skb->data[1];
-	skb_pull(skb, 2);
-
-	return 0;
-}
-
-static int
-lowpan_uncompress_udp_header(struct sk_buff *skb, struct udphdr *uh)
-{
-	u8 tmp;
-
-	if (!uh)
-		goto err;
-
-	if (lowpan_fetch_skb_u8(skb, &tmp))
-		goto err;
-
-	if ((tmp & LOWPAN_NHC_UDP_MASK) == LOWPAN_NHC_UDP_ID) {
-		pr_debug("UDP header uncompression\n");
-		switch (tmp & LOWPAN_NHC_UDP_CS_P_11) {
-		case LOWPAN_NHC_UDP_CS_P_00:
-			memcpy(&uh->source, &skb->data[0], 2);
-			memcpy(&uh->dest, &skb->data[2], 2);
-			skb_pull(skb, 4);
-			break;
-		case LOWPAN_NHC_UDP_CS_P_01:
-			memcpy(&uh->source, &skb->data[0], 2);
-			uh->dest =
-			   skb->data[2] + LOWPAN_NHC_UDP_8BIT_PORT;
-			skb_pull(skb, 3);
-			break;
-		case LOWPAN_NHC_UDP_CS_P_10:
-			uh->source = skb->data[0] + LOWPAN_NHC_UDP_8BIT_PORT;
-			memcpy(&uh->dest, &skb->data[1], 2);
-			skb_pull(skb, 3);
-			break;
-		case LOWPAN_NHC_UDP_CS_P_11:
-			uh->source =
-			   LOWPAN_NHC_UDP_4BIT_PORT + (skb->data[0] >> 4);
-			uh->dest =
-			   LOWPAN_NHC_UDP_4BIT_PORT + (skb->data[0] & 0x0f);
-			skb_pull(skb, 1);
-			break;
-		default:
-			pr_debug("ERROR: unknown UDP format\n");
-			goto err;
-		}
-
-		pr_debug("uncompressed UDP ports: src = %d, dst = %d\n",
-			 uh->source, uh->dest);
-
-		/* copy checksum */
-		memcpy(&uh->check, &skb->data[0], 2);
-		skb_pull(skb, 2);
-
-		/*
-		 * UDP lenght needs to be infered from the lower layers
-		 * here, we obtain the hint from the remaining size of the
-		 * frame
-		 */
-		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");
-		goto err;
-	}
-
-	return 0;
-err:
-	return -EINVAL;
-}
-
 static int lowpan_header_create(struct sk_buff *skb,
 			   struct net_device *dev,
 			   unsigned short type, const void *_daddr,
 			   const void *_saddr, unsigned int len)
 {
-	u8 tmp, iphc0, iphc1, *hc06_ptr;
 	struct ipv6hdr *hdr;
 	const u8 *saddr = _saddr;
 	const u8 *daddr = _daddr;
-	u8 head[100];
 	struct ieee802154_addr sa, da;
 
 	/* TODO:
@@ -485,181 +118,14 @@ static int lowpan_header_create(struct sk_buff *skb,
 		return 0;
 
 	hdr = ipv6_hdr(skb);
-	hc06_ptr = head + 2;
-
-	pr_debug("IPv6 header dump:\n\tversion = %d\n\tlength  = %d\n"
-		 "\tnexthdr = 0x%02x\n\thop_lim = %d\n", hdr->version,
-		 ntohs(hdr->payload_len), hdr->nexthdr, hdr->hop_limit);
-
-	lowpan_raw_dump_table(__func__, "raw skb network header dump",
-		skb_network_header(skb), sizeof(struct ipv6hdr));
 
 	if (!saddr)
 		saddr = dev->dev_addr;
 
-	lowpan_raw_dump_inline(__func__, "saddr", (unsigned char *)saddr, 8);
-
-	/*
-	 * As we copy some bit-length fields, in the IPHC encoding bytes,
-	 * we sometimes use |=
-	 * If the field is 0, and the current bit value in memory is 1,
-	 * this does not work. We therefore reset the IPHC encoding here
-	 */
-	iphc0 = LOWPAN_DISPATCH_IPHC;
-	iphc1 = 0;
-
-	/* TODO: context lookup */
+	raw_dump_inline(__func__, "saddr", (unsigned char *)saddr, 8);
+	raw_dump_inline(__func__, "daddr", (unsigned char *)daddr, 8);
 
-	lowpan_raw_dump_inline(__func__, "daddr", (unsigned char *)daddr, 8);
-
-	/*
-	 * Traffic class, flow label
-	 * If flow label is 0, compress it. If traffic class is 0, compress it
-	 * We have to process both in the same time as the offset of traffic
-	 * class depends on the presence of version and flow label
-	 */
-
-	/* hc06 format of TC is ECN | DSCP , original one is DSCP | ECN */
-	tmp = (hdr->priority << 4) | (hdr->flow_lbl[0] >> 4);
-	tmp = ((tmp & 0x03) << 6) | (tmp >> 2);
-
-	if (((hdr->flow_lbl[0] & 0x0F) == 0) &&
-	     (hdr->flow_lbl[1] == 0) && (hdr->flow_lbl[2] == 0)) {
-		/* flow label can be compressed */
-		iphc0 |= LOWPAN_IPHC_FL_C;
-		if ((hdr->priority == 0) &&
-		   ((hdr->flow_lbl[0] & 0xF0) == 0)) {
-			/* compress (elide) all */
-			iphc0 |= LOWPAN_IPHC_TC_C;
-		} else {
-			/* compress only the flow label */
-			*hc06_ptr = tmp;
-			hc06_ptr += 1;
-		}
-	} else {
-		/* Flow label cannot be compressed */
-		if ((hdr->priority == 0) &&
-		   ((hdr->flow_lbl[0] & 0xF0) == 0)) {
-			/* compress only traffic class */
-			iphc0 |= LOWPAN_IPHC_TC_C;
-			*hc06_ptr = (tmp & 0xc0) | (hdr->flow_lbl[0] & 0x0F);
-			memcpy(hc06_ptr + 1, &hdr->flow_lbl[1], 2);
-			hc06_ptr += 3;
-		} else {
-			/* compress nothing */
-			memcpy(hc06_ptr, &hdr, 4);
-			/* replace the top byte with new ECN | DSCP format */
-			*hc06_ptr = tmp;
-			hc06_ptr += 4;
-		}
-	}
-
-	/* NOTE: payload length is always compressed */
-
-	/* Next Header is compress if UDP */
-	if (hdr->nexthdr == UIP_PROTO_UDP)
-		iphc0 |= LOWPAN_IPHC_NH_C;
-
-	if ((iphc0 & LOWPAN_IPHC_NH_C) == 0) {
-		*hc06_ptr = hdr->nexthdr;
-		hc06_ptr += 1;
-	}
-
-	/*
-	 * Hop limit
-	 * if 1:   compress, encoding is 01
-	 * if 64:  compress, encoding is 10
-	 * if 255: compress, encoding is 11
-	 * else do not compress
-	 */
-	switch (hdr->hop_limit) {
-	case 1:
-		iphc0 |= LOWPAN_IPHC_TTL_1;
-		break;
-	case 64:
-		iphc0 |= LOWPAN_IPHC_TTL_64;
-		break;
-	case 255:
-		iphc0 |= LOWPAN_IPHC_TTL_255;
-		break;
-	default:
-		*hc06_ptr = hdr->hop_limit;
-		hc06_ptr += 1;
-		break;
-	}
-
-	/* source address compression */
-	if (is_addr_unspecified(&hdr->saddr)) {
-		pr_debug("source address is unspecified, setting SAC\n");
-		iphc1 |= LOWPAN_IPHC_SAC;
-	/* TODO: context lookup */
-	} else if (is_addr_link_local(&hdr->saddr)) {
-		pr_debug("source address is link-local\n");
-		iphc1 |= lowpan_compress_addr_64(&hc06_ptr,
-				LOWPAN_IPHC_SAM_BIT, &hdr->saddr, saddr);
-	} else {
-		pr_debug("send the full source address\n");
-		memcpy(hc06_ptr, &hdr->saddr.s6_addr16[0], 16);
-		hc06_ptr += 16;
-	}
-
-	/* destination address compression */
-	if (is_addr_mcast(&hdr->daddr)) {
-		pr_debug("destination address is multicast: ");
-		iphc1 |= LOWPAN_IPHC_M;
-		if (lowpan_is_mcast_addr_compressable8(&hdr->daddr)) {
-			pr_debug("compressed to 1 octet\n");
-			iphc1 |= LOWPAN_IPHC_DAM_11;
-			/* use last byte */
-			*hc06_ptr = hdr->daddr.s6_addr[15];
-			hc06_ptr += 1;
-		} else if (lowpan_is_mcast_addr_compressable32(&hdr->daddr)) {
-			pr_debug("compressed to 4 octets\n");
-			iphc1 |= LOWPAN_IPHC_DAM_10;
-			/* second byte + the last three */
-			*hc06_ptr = hdr->daddr.s6_addr[1];
-			memcpy(hc06_ptr + 1, &hdr->daddr.s6_addr[13], 3);
-			hc06_ptr += 4;
-		} else if (lowpan_is_mcast_addr_compressable48(&hdr->daddr)) {
-			pr_debug("compressed to 6 octets\n");
-			iphc1 |= LOWPAN_IPHC_DAM_01;
-			/* second byte + the last five */
-			*hc06_ptr = hdr->daddr.s6_addr[1];
-			memcpy(hc06_ptr + 1, &hdr->daddr.s6_addr[11], 5);
-			hc06_ptr += 6;
-		} else {
-			pr_debug("using full address\n");
-			iphc1 |= LOWPAN_IPHC_DAM_00;
-			memcpy(hc06_ptr, &hdr->daddr.s6_addr[0], 16);
-			hc06_ptr += 16;
-		}
-	} else {
-		/* TODO: context lookup */
-		if (is_addr_link_local(&hdr->daddr)) {
-			pr_debug("dest address is unicast and link-local\n");
-			iphc1 |= lowpan_compress_addr_64(&hc06_ptr,
-				LOWPAN_IPHC_DAM_BIT, &hdr->daddr, daddr);
-		} else {
-			pr_debug("dest address is unicast: using full one\n");
-			memcpy(hc06_ptr, &hdr->daddr.s6_addr16[0], 16);
-			hc06_ptr += 16;
-		}
-	}
-
-	/* UDP header compression */
-	if (hdr->nexthdr == UIP_PROTO_UDP)
-		lowpan_compress_udp_header(&hc06_ptr, skb);
-
-	head[0] = iphc0;
-	head[1] = iphc1;
-
-	skb_pull(skb, sizeof(struct ipv6hdr));
-	skb_reset_transport_header(skb);
-	memcpy(skb_push(skb, hc06_ptr - head), head, hc06_ptr - head);
-	skb_reset_network_header(skb);
-
-	lowpan_raw_dump_table(__func__, "raw skb data dump", skb->data,
-				skb->len);
+	lowpan_header_compress(skb, dev, type, daddr, saddr, len);
 
 	/*
 	 * NOTE1: I'm still unsure about the fact that compression and WPAN
@@ -671,39 +137,38 @@ static int lowpan_header_create(struct sk_buff *skb,
 	 * from MAC subif of the 'dev' and 'real_dev' network devices, but
 	 * this isn't implemented in mainline yet, so currently we assign 0xff
 	 */
-	{
-		mac_cb(skb)->flags = IEEE802154_FC_TYPE_DATA;
-		mac_cb(skb)->seq = ieee802154_mlme_ops(dev)->get_dsn(dev);
+	mac_cb(skb)->flags = IEEE802154_FC_TYPE_DATA;
+	mac_cb(skb)->seq = ieee802154_mlme_ops(dev)->get_dsn(dev);
 
-		/* prepare wpan address data */
-		sa.addr_type = IEEE802154_ADDR_LONG;
-		sa.pan_id = ieee802154_mlme_ops(dev)->get_pan_id(dev);
+	/* prepare wpan address data */
+	sa.addr_type = IEEE802154_ADDR_LONG;
+	sa.pan_id = ieee802154_mlme_ops(dev)->get_pan_id(dev);
 
-		memcpy(&(sa.hwaddr), saddr, 8);
-		/* intra-PAN communications */
-		da.pan_id = ieee802154_mlme_ops(dev)->get_pan_id(dev);
+	memcpy(&(sa.hwaddr), saddr, 8);
+	/* intra-PAN communications */
+	da.pan_id = ieee802154_mlme_ops(dev)->get_pan_id(dev);
 
-		/*
-		 * if the destination address is the broadcast address, use the
-		 * corresponding short address
-		 */
-		if (lowpan_is_addr_broadcast(daddr)) {
-			da.addr_type = IEEE802154_ADDR_SHORT;
-			da.short_addr = IEEE802154_ADDR_BROADCAST;
-		} else {
-			da.addr_type = IEEE802154_ADDR_LONG;
-			memcpy(&(da.hwaddr), daddr, IEEE802154_ADDR_LEN);
-
-			/* request acknowledgment */
-			mac_cb(skb)->flags |= MAC_CB_FLAG_ACKREQ;
-		}
+	/*
+	 * if the destination address is the broadcast address, use the
+	 * corresponding short address
+	 */
+	if (lowpan_is_addr_broadcast(daddr)) {
+		da.addr_type = IEEE802154_ADDR_SHORT;
+		da.short_addr = IEEE802154_ADDR_BROADCAST;
+	} else {
+		da.addr_type = IEEE802154_ADDR_LONG;
+		memcpy(&(da.hwaddr), daddr, IEEE802154_ADDR_LEN);
 
-		return dev_hard_header(skb, lowpan_dev_info(dev)->real_dev,
-				type, (void *)&da, (void *)&sa, skb->len);
+		/* request acknowledgment */
+		mac_cb(skb)->flags |= MAC_CB_FLAG_ACKREQ;
 	}
+
+	return dev_hard_header(skb, lowpan_dev_info(dev)->real_dev,
+			type, (void *)&da, (void *)&sa, skb->len);
 }
 
-static int lowpan_give_skb_to_devices(struct sk_buff *skb)
+static int lowpan_give_skb_to_devices(struct sk_buff *skb,
+					struct net_device *dev)
 {
 	struct lowpan_dev_record *entry;
 	struct sk_buff *skb_cp;
@@ -726,31 +191,6 @@ static int lowpan_give_skb_to_devices(struct sk_buff *skb)
 	return stat;
 }
 
-static int lowpan_skb_deliver(struct sk_buff *skb, struct ipv6hdr *hdr)
-{
-	struct sk_buff *new;
-	int stat = NET_RX_SUCCESS;
-
-	new = skb_copy_expand(skb, sizeof(struct ipv6hdr), skb_tailroom(skb),
-								GFP_ATOMIC);
-	kfree_skb(skb);
-
-	if (!new)
-		return -ENOMEM;
-
-	skb_push(new, sizeof(struct ipv6hdr));
-	skb_copy_to_linear_data(new, hdr, sizeof(struct ipv6hdr));
-
-	new->protocol = htons(ETH_P_IPV6);
-	new->pkt_type = PACKET_HOST;
-
-	stat = lowpan_give_skb_to_devices(new);
-
-	kfree_skb(new);
-
-	return stat;
-}
-
 static void lowpan_fragment_timer_expired(unsigned long entry_addr)
 {
 	struct lowpan_fragment *entry = (struct lowpan_fragment *)entry_addr;
@@ -814,16 +254,12 @@ frame_err:
 	return NULL;
 }
 
-static int
-lowpan_process_data(struct sk_buff *skb)
+static int process_data(struct sk_buff *skb)
 {
-	struct ipv6hdr hdr = {};
-	u8 tmp, iphc0, iphc1, num_context = 0;
+	u8 iphc0, iphc1;
 	const struct ieee802154_addr *_saddr, *_daddr;
-	int err;
 
-	lowpan_raw_dump_table(__func__, "raw skb data dump", skb->data,
-				skb->len);
+	raw_dump_table(__func__, "raw skb data dump", skb->data, skb->len);
 	/* at least two bytes will be used for the encoding */
 	if (skb->len < 2)
 		goto drop;
@@ -925,162 +361,11 @@ lowpan_process_data(struct sk_buff *skb)
 	_saddr = &mac_cb(skb)->sa;
 	_daddr = &mac_cb(skb)->da;
 
-	pr_debug("iphc0 = %02x, iphc1 = %02x\n", iphc0, iphc1);
-
-	/* another if the CID flag is set */
-	if (iphc1 & LOWPAN_IPHC_CID) {
-		pr_debug("CID flag is set, increase header with one\n");
-		if (lowpan_fetch_skb_u8(skb, &num_context))
-			goto drop;
-	}
-
-	hdr.version = 6;
-
-	/* Traffic Class and Flow Label */
-	switch ((iphc0 & LOWPAN_IPHC_TF) >> 3) {
-	/*
-	 * Traffic Class and FLow Label carried in-line
-	 * ECN + DSCP + 4-bit Pad + Flow Label (4 bytes)
-	 */
-	case 0: /* 00b */
-		if (lowpan_fetch_skb_u8(skb, &tmp))
-			goto drop;
-
-		memcpy(&hdr.flow_lbl, &skb->data[0], 3);
-		skb_pull(skb, 3);
-		hdr.priority = ((tmp >> 2) & 0x0f);
-		hdr.flow_lbl[0] = ((tmp >> 2) & 0x30) | (tmp << 6) |
-					(hdr.flow_lbl[0] & 0x0f);
-		break;
-	/*
-	 * Traffic class carried in-line
-	 * ECN + DSCP (1 byte), Flow Label is elided
-	 */
-	case 2: /* 10b */
-		if (lowpan_fetch_skb_u8(skb, &tmp))
-			goto drop;
-
-		hdr.priority = ((tmp >> 2) & 0x0f);
-		hdr.flow_lbl[0] = ((tmp << 6) & 0xC0) | ((tmp >> 2) & 0x30);
-		break;
-	/*
-	 * Flow Label carried in-line
-	 * ECN + 2-bit Pad + Flow Label (3 bytes), DSCP is elided
-	 */
-	case 1: /* 01b */
-		if (lowpan_fetch_skb_u8(skb, &tmp))
-			goto drop;
-
-		hdr.flow_lbl[0] = (skb->data[0] & 0x0F) | ((tmp >> 2) & 0x30);
-		memcpy(&hdr.flow_lbl[1], &skb->data[0], 2);
-		skb_pull(skb, 2);
-		break;
-	/* Traffic Class and Flow Label are elided */
-	case 3: /* 11b */
-		break;
-	default:
-		break;
-	}
-
-	/* Next Header */
-	if ((iphc0 & LOWPAN_IPHC_NH_C) == 0) {
-		/* Next header is carried inline */
-		if (lowpan_fetch_skb_u8(skb, &(hdr.nexthdr)))
-			goto drop;
-
-		pr_debug("NH flag is set, next header carried inline: %02x\n",
-			 hdr.nexthdr);
-	}
-
-	/* Hop Limit */
-	if ((iphc0 & 0x03) != LOWPAN_IPHC_TTL_I)
-		hdr.hop_limit = lowpan_ttl_values[iphc0 & 0x03];
-	else {
-		if (lowpan_fetch_skb_u8(skb, &(hdr.hop_limit)))
-			goto drop;
-	}
-
-	/* Extract SAM to the tmp variable */
-	tmp = ((iphc1 & LOWPAN_IPHC_SAM) >> LOWPAN_IPHC_SAM_BIT) & 0x03;
-
-	if (iphc1 & LOWPAN_IPHC_SAC) {
-		/* Source address context based uncompression */
-		pr_debug("SAC bit is set. Handle context based source address.\n");
-		err = lowpan_uncompress_context_based_src_addr(
-				skb, &hdr.saddr, tmp);
-	} else {
-		/* Source address uncompression */
-		pr_debug("source address stateless compression\n");
-		err = lowpan_uncompress_addr(skb, &hdr.saddr, tmp, _saddr);
-	}
-
-	/* Check on error of previous branch */
-	if (err)
-		goto drop;
-
-	/* Extract DAM to the tmp variable */
-	tmp = ((iphc1 & LOWPAN_IPHC_DAM_11) >> LOWPAN_IPHC_DAM_BIT) & 0x03;
-
-	/* check for Multicast Compression */
-	if (iphc1 & LOWPAN_IPHC_M) {
-		if (iphc1 & LOWPAN_IPHC_DAC) {
-			pr_debug("dest: context-based mcast compression\n");
-			/* TODO: implement this */
-		} else {
-			err = lowpan_uncompress_multicast_daddr(
-					skb, &hdr.daddr, tmp);
-			if (err)
-				goto drop;
-		}
-	} else {
-		pr_debug("dest: stateless compression\n");
-		err = lowpan_uncompress_addr(skb, &hdr.daddr, tmp, _daddr);
-		if (err)
-			goto drop;
-	}
-
-	/* UDP data uncompression */
-	if (iphc0 & LOWPAN_IPHC_NH_C) {
-		struct udphdr uh;
-		struct sk_buff *new;
-		if (lowpan_uncompress_udp_header(skb, &uh))
-			goto drop;
-
-		/*
-		 * replace the compressed UDP head by the uncompressed UDP
-		 * header
-		 */
-		new = skb_copy_expand(skb, sizeof(struct udphdr),
-				      skb_tailroom(skb), GFP_ATOMIC);
-		kfree_skb(skb);
-
-		if (!new)
-			return -ENOMEM;
-
-		skb = new;
-
-		skb_push(skb, sizeof(struct udphdr));
-		skb_copy_to_linear_data(skb, &uh, sizeof(struct udphdr));
-
-		lowpan_raw_dump_table(__func__, "raw UDP header dump",
-				      (u8 *)&uh, sizeof(uh));
-
-		hdr.nexthdr = UIP_PROTO_UDP;
-	}
-
-	/* Not fragmented package */
-	hdr.payload_len = htons(skb->len);
-
-	pr_debug("skb headroom size = %d, data length = %d\n",
-		 skb_headroom(skb), skb->len);
-
-	pr_debug("IPv6 header dump:\n\tversion = %d\n\tlength  = %d\n\t"
-		 "nexthdr = 0x%02x\n\thop_lim = %d\n", hdr.version,
-		 ntohs(hdr.payload_len), hdr.nexthdr, hdr.hop_limit);
-
-	lowpan_raw_dump_table(__func__, "raw header dump", (u8 *)&hdr,
-							sizeof(hdr));
-	return lowpan_skb_deliver(skb, &hdr);
+	return lowpan_process_data(skb, skb->dev, (u8 *)_saddr->hwaddr,
+				_saddr->addr_type, IEEE802154_ADDR_LEN,
+				(u8 *)_daddr->hwaddr, _daddr->addr_type,
+				IEEE802154_ADDR_LEN, iphc0, iphc1,
+				lowpan_give_skb_to_devices);
 
 unlock_and_drop:
 	spin_unlock_bh(&flist_lock);
@@ -1112,7 +397,7 @@ lowpan_fragment_xmit(struct sk_buff *skb, u8 *head,
 	hlen = (type == LOWPAN_DISPATCH_FRAG1) ?
 			LOWPAN_FRAG1_HEAD_SIZE : LOWPAN_FRAGN_HEAD_SIZE;
 
-	lowpan_raw_dump_inline(__func__, "6lowpan fragment header", head, hlen);
+	raw_dump_inline(__func__, "6lowpan fragment header", head, hlen);
 
 	frag = netdev_alloc_skb(skb->dev,
 				hlen + mlen + plen + IEEE802154_MFR_SIZE);
@@ -1132,8 +417,7 @@ lowpan_fragment_xmit(struct sk_buff *skb, u8 *head,
 	skb_copy_to_linear_data_offset(frag, mlen + hlen,
 				       skb_network_header(skb) + offset, plen);
 
-	lowpan_raw_dump_table(__func__, " raw fragment dump", frag->data,
-								frag->len);
+	raw_dump_table(__func__, " raw fragment dump", frag->data, frag->len);
 
 	return dev_queue_xmit(frag);
 }
@@ -1316,7 +600,7 @@ static int lowpan_rcv(struct sk_buff *skb, struct net_device *dev,
 		/* Pull off the 1-byte of 6lowpan header. */
 		skb_pull(local_skb, 1);
 
-		lowpan_give_skb_to_devices(local_skb);
+		lowpan_give_skb_to_devices(local_skb, NULL);
 
 		kfree_skb(local_skb);
 		kfree_skb(skb);
@@ -1328,7 +612,7 @@ static int lowpan_rcv(struct sk_buff *skb, struct net_device *dev,
 			local_skb = skb_clone(skb, GFP_ATOMIC);
 			if (!local_skb)
 				goto drop;
-			lowpan_process_data(local_skb);
+			process_data(local_skb);
 
 			kfree_skb(skb);
 			break;
diff --git a/net/ieee802154/6lowpan.h b/net/ieee802154/6lowpan.h
index 2869c05..2b835db 100644
--- a/net/ieee802154/6lowpan.h
+++ b/net/ieee802154/6lowpan.h
@@ -231,6 +231,61 @@
 #define LOWPAN_NHC_UDP_CS_P_10	0xF2 /* source = 0xF0 + 8bit inline,
 					dest = 16 bit inline */
 #define LOWPAN_NHC_UDP_CS_P_11	0xF3 /* source & dest = 0xF0B + 4bit inline */
+#define LOWPAN_NHC_UDP_CS_C	0x04 /* checksum elided */
+
+#ifdef DEBUG
+/* print data in line */
+static inline void raw_dump_inline(const char *caller, char *msg,
+				   unsigned char *buf, int len)
+{
+	if (msg)
+		pr_debug("%s():%s: ", caller, msg);
+
+	print_hex_dump_debug("", DUMP_PREFIX_NONE, 16, 1, buf, len, false);
+}
+
+/* print data in a table format:
+ *
+ * addr: xx xx xx xx xx xx
+ * addr: xx xx xx xx xx xx
+ * ...
+ */
+static inline void raw_dump_table(const char *caller, char *msg,
+				  unsigned char *buf, int len)
+{
+	if (msg)
+		pr_debug("%s():%s:\n", caller, msg);
+
+	print_hex_dump_debug("\t", DUMP_PREFIX_OFFSET, 16, 1, buf, len, false);
+}
+#else
+static inline void raw_dump_table(const char *caller, char *msg,
+				  unsigned char *buf, int len) { }
+static inline void raw_dump_inline(const char *caller, char *msg,
+				   unsigned char *buf, int len) { }
+#endif
+
+static inline int lowpan_fetch_skb_u8(struct sk_buff *skb, u8 *val)
+{
+	if (unlikely(!pskb_may_pull(skb, 1)))
+		return -EINVAL;
+
+	*val = skb->data[0];
+	skb_pull(skb, 1);
+
+	return 0;
+}
+
+static inline int lowpan_fetch_skb_u16(struct sk_buff *skb, u16 *val)
+{
+	if (unlikely(!pskb_may_pull(skb, 2)))
+		return -EINVAL;
+
+	*val = (skb->data[0] << 8) | skb->data[1];
+	skb_pull(skb, 2);
+
+	return 0;
+}
 
 static inline bool lowpan_fetch_skb(struct sk_buff *skb,
 		void *data, const unsigned int len)
@@ -244,4 +299,21 @@ static inline bool lowpan_fetch_skb(struct sk_buff *skb,
 	return false;
 }
 
+static inline void lowpan_push_hc_data(u8 **hc_ptr, const void *data,
+				       const size_t len)
+{
+	memcpy(*hc_ptr, data, len);
+	*hc_ptr += len;
+}
+
+typedef int (*skb_delivery_cb)(struct sk_buff *skb, struct net_device *dev);
+
+int lowpan_process_data(struct sk_buff *skb, struct net_device *dev,
+		const u8 *saddr, const u8 saddr_type, const u8 saddr_len,
+		const u8 *daddr, const u8 daddr_type, const u8 daddr_len,
+		u8 iphc0, u8 iphc1, skb_delivery_cb skb_deliver);
+int lowpan_header_compress(struct sk_buff *skb, struct net_device *dev,
+			unsigned short type, const void *_daddr,
+			const void *_saddr, unsigned int len);
+
 #endif /* __6LOWPAN_H__ */
diff --git a/net/ieee802154/6lowpan_iphc.c b/net/ieee802154/6lowpan_iphc.c
new file mode 100644
index 0000000..11840f9
--- /dev/null
+++ b/net/ieee802154/6lowpan_iphc.c
@@ -0,0 +1,799 @@
+/*
+ * Copyright 2011, Siemens AG
+ * written by Alexander Smirnov <alex.bluesman.smirnov@gmail.com>
+ */
+
+/*
+ * Based on patches from Jon Smirl <jonsmirl@gmail.com>
+ * Copyright (c) 2011 Jon Smirl <jonsmirl@gmail.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2
+ * as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
+ */
+
+/* Jon's code is based on 6lowpan implementation for Contiki which is:
+ * Copyright (c) 2008, Swedish Institute of Computer Science.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ * 3. Neither the name of the Institute nor the names of its contributors
+ *    may be used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE INSTITUTE AND CONTRIBUTORS ``AS IS'' AND
+ * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED.  IN NO EVENT SHALL THE INSTITUTE OR CONTRIBUTORS BE LIABLE
+ * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
+ * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
+ * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
+ * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
+ * SUCH DAMAGE.
+ */
+
+#include <linux/bitops.h>
+#include <linux/if_arp.h>
+#include <linux/netdevice.h>
+#include <net/ipv6.h>
+#include <net/af_ieee802154.h>
+
+#include "6lowpan.h"
+
+/*
+ * Uncompress address function for source and
+ * destination address(non-multicast).
+ *
+ * address_mode is sam value or dam value.
+ */
+static int uncompress_addr(struct sk_buff *skb,
+				struct in6_addr *ipaddr, const u8 address_mode,
+				const u8 *lladdr, const u8 addr_type,
+				const u8 addr_len)
+{
+	bool fail;
+
+	switch (address_mode) {
+	case LOWPAN_IPHC_ADDR_00:
+		/* for global link addresses */
+		fail = lowpan_fetch_skb(skb, ipaddr->s6_addr, 16);
+		break;
+	case LOWPAN_IPHC_ADDR_01:
+		/* fe:80::XXXX:XXXX:XXXX:XXXX */
+		ipaddr->s6_addr[0] = 0xFE;
+		ipaddr->s6_addr[1] = 0x80;
+		fail = lowpan_fetch_skb(skb, &ipaddr->s6_addr[8], 8);
+		break;
+	case LOWPAN_IPHC_ADDR_02:
+		/* fe:80::ff:fe00:XXXX */
+		ipaddr->s6_addr[0] = 0xFE;
+		ipaddr->s6_addr[1] = 0x80;
+		ipaddr->s6_addr[11] = 0xFF;
+		ipaddr->s6_addr[12] = 0xFE;
+		fail = lowpan_fetch_skb(skb, &ipaddr->s6_addr[14], 2);
+		break;
+	case LOWPAN_IPHC_ADDR_03:
+		fail = false;
+		switch (addr_type) {
+		case IEEE802154_ADDR_LONG:
+			/* fe:80::XXXX:XXXX:XXXX:XXXX
+			 *        \_________________/
+			 *              hwaddr
+			 */
+			ipaddr->s6_addr[0] = 0xFE;
+			ipaddr->s6_addr[1] = 0x80;
+			memcpy(&ipaddr->s6_addr[8], lladdr, addr_len);
+			/* second bit-flip (Universe/Local)
+			 * is done according RFC2464
+			 */
+			ipaddr->s6_addr[8] ^= 0x02;
+			break;
+		case IEEE802154_ADDR_SHORT:
+			/* fe:80::ff:fe00:XXXX
+			 *		  \__/
+			 *	       short_addr
+			 *
+			 * Universe/Local bit is zero.
+			 */
+			ipaddr->s6_addr[0] = 0xFE;
+			ipaddr->s6_addr[1] = 0x80;
+			ipaddr->s6_addr[11] = 0xFF;
+			ipaddr->s6_addr[12] = 0xFE;
+			ipaddr->s6_addr16[7] = htons(*((u16 *)lladdr));
+			break;
+		default:
+			pr_debug("Invalid addr_type set\n");
+			return -EINVAL;
+		}
+		break;
+	default:
+		pr_debug("Invalid address mode value: 0x%x\n", address_mode);
+		return -EINVAL;
+	}
+
+	if (fail) {
+		pr_debug("Failed to fetch skb data\n");
+		return -EIO;
+	}
+
+	raw_dump_inline(NULL, "Reconstructed ipv6 addr is",
+			ipaddr->s6_addr, 16);
+
+	return 0;
+}
+
+/*
+ * Uncompress address function for source context
+ * based address(non-multicast).
+ */
+static int uncompress_context_based_src_addr(struct sk_buff *skb,
+						struct in6_addr *ipaddr,
+						const u8 sam)
+{
+	switch (sam) {
+	case LOWPAN_IPHC_ADDR_00:
+		/* unspec address ::
+		 * Do nothing, address is already ::
+		 */
+		break;
+	case LOWPAN_IPHC_ADDR_01:
+		/* TODO */
+	case LOWPAN_IPHC_ADDR_02:
+		/* TODO */
+	case LOWPAN_IPHC_ADDR_03:
+		/* TODO */
+		netdev_warn(skb->dev, "SAM value 0x%x not supported\n", sam);
+		return -EINVAL;
+	default:
+		pr_debug("Invalid sam value: 0x%x\n", sam);
+		return -EINVAL;
+	}
+
+	raw_dump_inline(NULL,
+			"Reconstructed context based ipv6 src addr is",
+			ipaddr->s6_addr, 16);
+
+	return 0;
+}
+
+static int skb_deliver(struct sk_buff *skb, struct ipv6hdr *hdr,
+		struct net_device *dev, skb_delivery_cb deliver_skb)
+{
+	struct sk_buff *new;
+	int stat;
+
+	new = skb_copy_expand(skb, sizeof(struct ipv6hdr), skb_tailroom(skb),
+								GFP_ATOMIC);
+	kfree_skb(skb);
+
+	if (!new)
+		return -ENOMEM;
+
+	skb_push(new, sizeof(struct ipv6hdr));
+	skb_reset_network_header(new);
+	skb_copy_to_linear_data(new, hdr, sizeof(struct ipv6hdr));
+
+	new->protocol = htons(ETH_P_IPV6);
+	new->pkt_type = PACKET_HOST;
+	new->dev = dev;
+
+	raw_dump_table(__func__, "raw skb data dump before receiving",
+			new->data, new->len);
+
+	stat = deliver_skb(new, dev);
+
+	kfree_skb(new);
+
+	return stat;
+}
+
+/* Uncompress function for multicast destination address,
+ * when M bit is set.
+ */
+static int
+lowpan_uncompress_multicast_daddr(struct sk_buff *skb,
+		struct in6_addr *ipaddr,
+		const u8 dam)
+{
+	bool fail;
+
+	switch (dam) {
+	case LOWPAN_IPHC_DAM_00:
+		/* 00:  128 bits.  The full address
+		 * is carried in-line.
+		 */
+		fail = lowpan_fetch_skb(skb, ipaddr->s6_addr, 16);
+		break;
+	case LOWPAN_IPHC_DAM_01:
+		/* 01:  48 bits.  The address takes
+		 * the form ffXX::00XX:XXXX:XXXX.
+		 */
+		ipaddr->s6_addr[0] = 0xFF;
+		fail = lowpan_fetch_skb(skb, &ipaddr->s6_addr[1], 1);
+		fail |= lowpan_fetch_skb(skb, &ipaddr->s6_addr[11], 5);
+		break;
+	case LOWPAN_IPHC_DAM_10:
+		/* 10:  32 bits.  The address takes
+		 * the form ffXX::00XX:XXXX.
+		 */
+		ipaddr->s6_addr[0] = 0xFF;
+		fail = lowpan_fetch_skb(skb, &ipaddr->s6_addr[1], 1);
+		fail |= lowpan_fetch_skb(skb, &ipaddr->s6_addr[13], 3);
+		break;
+	case LOWPAN_IPHC_DAM_11:
+		/* 11:  8 bits.  The address takes
+		 * the form ff02::00XX.
+		 */
+		ipaddr->s6_addr[0] = 0xFF;
+		ipaddr->s6_addr[1] = 0x02;
+		fail = lowpan_fetch_skb(skb, &ipaddr->s6_addr[15], 1);
+		break;
+	default:
+		pr_debug("DAM value has a wrong value: 0x%x\n", dam);
+		return -EINVAL;
+	}
+
+	if (fail) {
+		pr_debug("Failed to fetch skb data\n");
+		return -EIO;
+	}
+
+	raw_dump_inline(NULL, "Reconstructed ipv6 multicast addr is",
+				ipaddr->s6_addr, 16);
+
+	return 0;
+}
+
+static int
+uncompress_udp_header(struct sk_buff *skb, struct udphdr *uh)
+{
+	bool fail;
+	u8 tmp = 0, val = 0;
+
+	if (!uh)
+		goto err;
+
+	fail = lowpan_fetch_skb(skb, &tmp, 1);
+
+	if ((tmp & LOWPAN_NHC_UDP_MASK) == LOWPAN_NHC_UDP_ID) {
+		pr_debug("UDP header uncompression\n");
+		switch (tmp & LOWPAN_NHC_UDP_CS_P_11) {
+		case LOWPAN_NHC_UDP_CS_P_00:
+			fail |= lowpan_fetch_skb(skb, &uh->source, 2);
+			fail |= lowpan_fetch_skb(skb, &uh->dest, 2);
+			break;
+		case LOWPAN_NHC_UDP_CS_P_01:
+			fail |= lowpan_fetch_skb(skb, &uh->source, 2);
+			fail |= lowpan_fetch_skb(skb, &val, 1);
+			uh->dest = htons(val + LOWPAN_NHC_UDP_8BIT_PORT);
+			break;
+		case LOWPAN_NHC_UDP_CS_P_10:
+			fail |= lowpan_fetch_skb(skb, &val, 1);
+			uh->source = htons(val + LOWPAN_NHC_UDP_8BIT_PORT);
+			fail |= lowpan_fetch_skb(skb, &uh->dest, 2);
+			break;
+		case LOWPAN_NHC_UDP_CS_P_11:
+			fail |= lowpan_fetch_skb(skb, &val, 1);
+			uh->source = htons(LOWPAN_NHC_UDP_4BIT_PORT +
+					   (val >> 4));
+			uh->dest = htons(LOWPAN_NHC_UDP_4BIT_PORT +
+					 (val & 0x0f));
+			break;
+		default:
+			pr_debug("ERROR: unknown UDP format\n");
+			goto err;
+			break;
+		}
+
+		pr_debug("uncompressed UDP ports: src = %d, dst = %d\n",
+			 ntohs(uh->source), ntohs(uh->dest));
+
+		/* checksum */
+		if (tmp & LOWPAN_NHC_UDP_CS_C) {
+			pr_debug_ratelimited("checksum elided currently not supported\n");
+			goto err;
+		} else {
+			fail |= lowpan_fetch_skb(skb, &uh->check, 2);
+		}
+
+		/*
+		 * UDP lenght needs to be infered from the lower layers
+		 * here, we obtain the hint from the remaining size of the
+		 * frame
+		 */
+		uh->len = htons(skb->len + sizeof(struct udphdr));
+		pr_debug("uncompressed UDP length: src = %d", ntohs(uh->len));
+	} else {
+		pr_debug("ERROR: unsupported NH format\n");
+		goto err;
+	}
+
+	if (fail)
+		goto err;
+
+	return 0;
+err:
+	return -EINVAL;
+}
+
+/* TTL uncompression values */
+static const u8 lowpan_ttl_values[] = { 0, 1, 64, 255 };
+
+int lowpan_process_data(struct sk_buff *skb, struct net_device *dev,
+		const u8 *saddr, const u8 saddr_type, const u8 saddr_len,
+		const u8 *daddr, const u8 daddr_type, const u8 daddr_len,
+		u8 iphc0, u8 iphc1, skb_delivery_cb deliver_skb)
+{
+	struct ipv6hdr hdr = {};
+	u8 tmp, num_context = 0;
+	int err;
+
+	raw_dump_table(__func__, "raw skb data dump uncompressed",
+				skb->data, skb->len);
+
+	/* another if the CID flag is set */
+	if (iphc1 & LOWPAN_IPHC_CID) {
+		pr_debug("CID flag is set, increase header with one\n");
+		if (lowpan_fetch_skb_u8(skb, &num_context))
+			goto drop;
+	}
+
+	hdr.version = 6;
+
+	/* Traffic Class and Flow Label */
+	switch ((iphc0 & LOWPAN_IPHC_TF) >> 3) {
+	/*
+	 * Traffic Class and FLow Label carried in-line
+	 * ECN + DSCP + 4-bit Pad + Flow Label (4 bytes)
+	 */
+	case 0: /* 00b */
+		if (lowpan_fetch_skb_u8(skb, &tmp))
+			goto drop;
+
+		memcpy(&hdr.flow_lbl, &skb->data[0], 3);
+		skb_pull(skb, 3);
+		hdr.priority = ((tmp >> 2) & 0x0f);
+		hdr.flow_lbl[0] = ((tmp >> 2) & 0x30) | (tmp << 6) |
+					(hdr.flow_lbl[0] & 0x0f);
+		break;
+	/*
+	 * Traffic class carried in-line
+	 * ECN + DSCP (1 byte), Flow Label is elided
+	 */
+	case 2: /* 10b */
+		if (lowpan_fetch_skb_u8(skb, &tmp))
+			goto drop;
+
+		hdr.priority = ((tmp >> 2) & 0x0f);
+		hdr.flow_lbl[0] = ((tmp << 6) & 0xC0) | ((tmp >> 2) & 0x30);
+		break;
+	/*
+	 * Flow Label carried in-line
+	 * ECN + 2-bit Pad + Flow Label (3 bytes), DSCP is elided
+	 */
+	case 1: /* 01b */
+		if (lowpan_fetch_skb_u8(skb, &tmp))
+			goto drop;
+
+		hdr.flow_lbl[0] = (skb->data[0] & 0x0F) | ((tmp >> 2) & 0x30);
+		memcpy(&hdr.flow_lbl[1], &skb->data[0], 2);
+		skb_pull(skb, 2);
+		break;
+	/* Traffic Class and Flow Label are elided */
+	case 3: /* 11b */
+		break;
+	default:
+		break;
+	}
+
+	/* Next Header */
+	if ((iphc0 & LOWPAN_IPHC_NH_C) == 0) {
+		/* Next header is carried inline */
+		if (lowpan_fetch_skb_u8(skb, &(hdr.nexthdr)))
+			goto drop;
+
+		pr_debug("NH flag is set, next header carried inline: %02x\n",
+			 hdr.nexthdr);
+	}
+
+	/* Hop Limit */
+	if ((iphc0 & 0x03) != LOWPAN_IPHC_TTL_I)
+		hdr.hop_limit = lowpan_ttl_values[iphc0 & 0x03];
+	else {
+		if (lowpan_fetch_skb_u8(skb, &(hdr.hop_limit)))
+			goto drop;
+	}
+
+	/* Extract SAM to the tmp variable */
+	tmp = ((iphc1 & LOWPAN_IPHC_SAM) >> LOWPAN_IPHC_SAM_BIT) & 0x03;
+
+	if (iphc1 & LOWPAN_IPHC_SAC) {
+		/* Source address context based uncompression */
+		pr_debug("SAC bit is set. Handle context based source address.\n");
+		err = uncompress_context_based_src_addr(
+				skb, &hdr.saddr, tmp);
+	} else {
+		/* Source address uncompression */
+		pr_debug("source address stateless compression\n");
+		err = uncompress_addr(skb, &hdr.saddr, tmp, saddr,
+					saddr_type, saddr_len);
+	}
+
+	/* Check on error of previous branch */
+	if (err)
+		goto drop;
+
+	/* Extract DAM to the tmp variable */
+	tmp = ((iphc1 & LOWPAN_IPHC_DAM_11) >> LOWPAN_IPHC_DAM_BIT) & 0x03;
+
+	/* check for Multicast Compression */
+	if (iphc1 & LOWPAN_IPHC_M) {
+		if (iphc1 & LOWPAN_IPHC_DAC) {
+			pr_debug("dest: context-based mcast compression\n");
+			/* TODO: implement this */
+		} else {
+			err = lowpan_uncompress_multicast_daddr(
+						skb, &hdr.daddr, tmp);
+			if (err)
+				goto drop;
+		}
+	} else {
+		err = uncompress_addr(skb, &hdr.daddr, tmp, daddr,
+					daddr_type, daddr_len);
+		pr_debug("dest: stateless compression mode %d dest %pI6c\n",
+			tmp, &hdr.daddr);
+		if (err)
+			goto drop;
+	}
+
+	/* UDP data uncompression */
+	if (iphc0 & LOWPAN_IPHC_NH_C) {
+		struct udphdr uh;
+		struct sk_buff *new;
+		if (uncompress_udp_header(skb, &uh))
+			goto drop;
+
+		/*
+		 * replace the compressed UDP head by the uncompressed UDP
+		 * header
+		 */
+		new = skb_copy_expand(skb, sizeof(struct udphdr),
+				      skb_tailroom(skb), GFP_ATOMIC);
+		kfree_skb(skb);
+
+		if (!new)
+			return -ENOMEM;
+
+		skb = new;
+
+		skb_push(skb, sizeof(struct udphdr));
+		skb_reset_transport_header(skb);
+		skb_copy_to_linear_data(skb, &uh, sizeof(struct udphdr));
+
+		raw_dump_table(__func__, "raw UDP header dump",
+				      (u8 *)&uh, sizeof(uh));
+
+		hdr.nexthdr = UIP_PROTO_UDP;
+	}
+
+	hdr.payload_len = htons(skb->len);
+
+	pr_debug("skb headroom size = %d, data length = %d\n",
+		 skb_headroom(skb), skb->len);
+
+	pr_debug("IPv6 header dump:\n\tversion = %d\n\tlength  = %d\n\t"
+		 "nexthdr = 0x%02x\n\thop_lim = %d\n\tdest    = %pI6c\n",
+		hdr.version, ntohs(hdr.payload_len), hdr.nexthdr,
+		hdr.hop_limit, &hdr.daddr);
+
+	raw_dump_table(__func__, "raw header dump", (u8 *)&hdr,
+							sizeof(hdr));
+
+	return skb_deliver(skb, &hdr, dev, deliver_skb);
+
+drop:
+	kfree_skb(skb);
+	return -EINVAL;
+}
+EXPORT_SYMBOL_GPL(lowpan_process_data);
+
+static u8 lowpan_compress_addr_64(u8 **hc06_ptr, u8 shift,
+				const struct in6_addr *ipaddr,
+				const unsigned char *lladdr)
+{
+	u8 val = 0;
+
+	if (is_addr_mac_addr_based(ipaddr, lladdr)) {
+		val = 3; /* 0-bits */
+		pr_debug("address compression 0 bits\n");
+	} else if (lowpan_is_iid_16_bit_compressable(ipaddr)) {
+		/* compress IID to 16 bits xxxx::XXXX */
+		memcpy(*hc06_ptr, &ipaddr->s6_addr16[7], 2);
+		*hc06_ptr += 2;
+		val = 2; /* 16-bits */
+		raw_dump_inline(NULL, "Compressed ipv6 addr is (16 bits)",
+			*hc06_ptr - 2, 2);
+	} else {
+		/* do not compress IID => xxxx::IID */
+		memcpy(*hc06_ptr, &ipaddr->s6_addr16[4], 8);
+		*hc06_ptr += 8;
+		val = 1; /* 64-bits */
+		raw_dump_inline(NULL, "Compressed ipv6 addr is (64 bits)",
+			*hc06_ptr - 8, 8);
+	}
+
+	return rol8(val, shift);
+}
+
+static void compress_udp_header(u8 **hc06_ptr, struct sk_buff *skb)
+{
+	struct udphdr *uh = udp_hdr(skb);
+	u8 tmp;
+
+	if (((ntohs(uh->source) & LOWPAN_NHC_UDP_4BIT_MASK) ==
+	     LOWPAN_NHC_UDP_4BIT_PORT) &&
+	    ((ntohs(uh->dest) & LOWPAN_NHC_UDP_4BIT_MASK) ==
+	     LOWPAN_NHC_UDP_4BIT_PORT)) {
+		pr_debug("UDP header: both ports compression to 4 bits\n");
+		/* compression value */
+		tmp = LOWPAN_NHC_UDP_CS_P_11;
+		lowpan_push_hc_data(hc06_ptr, &tmp, sizeof(tmp));
+		/* source and destination port */
+		tmp = ntohs(uh->dest) - LOWPAN_NHC_UDP_4BIT_PORT +
+		      ((ntohs(uh->source) - LOWPAN_NHC_UDP_4BIT_PORT) << 4);
+		lowpan_push_hc_data(hc06_ptr, &tmp, sizeof(tmp));
+	} else if ((ntohs(uh->dest) & LOWPAN_NHC_UDP_8BIT_MASK) ==
+			LOWPAN_NHC_UDP_8BIT_PORT) {
+		pr_debug("UDP header: remove 8 bits of dest\n");
+		/* compression value */
+		tmp = LOWPAN_NHC_UDP_CS_P_01;
+		lowpan_push_hc_data(hc06_ptr, &tmp, sizeof(tmp));
+		/* source port */
+		lowpan_push_hc_data(hc06_ptr, &uh->source, sizeof(uh->source));
+		/* destination port */
+		tmp = ntohs(uh->dest) - LOWPAN_NHC_UDP_8BIT_PORT;
+		lowpan_push_hc_data(hc06_ptr, &tmp, sizeof(tmp));
+	} else if ((ntohs(uh->source) & LOWPAN_NHC_UDP_8BIT_MASK) ==
+			LOWPAN_NHC_UDP_8BIT_PORT) {
+		pr_debug("UDP header: remove 8 bits of source\n");
+		/* compression value */
+		tmp = LOWPAN_NHC_UDP_CS_P_10;
+		lowpan_push_hc_data(hc06_ptr, &tmp, sizeof(tmp));
+		/* source port */
+		tmp = ntohs(uh->source) - LOWPAN_NHC_UDP_8BIT_PORT;
+		lowpan_push_hc_data(hc06_ptr, &tmp, sizeof(tmp));
+		/* destination port */
+		lowpan_push_hc_data(hc06_ptr, &uh->dest, sizeof(uh->dest));
+	} else {
+		pr_debug("UDP header: can't compress\n");
+		/* compression value */
+		tmp = LOWPAN_NHC_UDP_CS_P_00;
+		lowpan_push_hc_data(hc06_ptr, &tmp, sizeof(tmp));
+		/* source port */
+		lowpan_push_hc_data(hc06_ptr, &uh->source, sizeof(uh->source));
+		/* destination port */
+		lowpan_push_hc_data(hc06_ptr, &uh->dest, sizeof(uh->dest));
+	}
+
+	/* checksum is always inline */
+	lowpan_push_hc_data(hc06_ptr, &uh->check, sizeof(uh->check));
+
+	/* skip the UDP header */
+	skb_pull(skb, sizeof(struct udphdr));
+}
+
+int lowpan_header_compress(struct sk_buff *skb, struct net_device *dev,
+			unsigned short type, const void *_daddr,
+			const void *_saddr, unsigned int len)
+{
+	u8 tmp, iphc0, iphc1, *hc06_ptr;
+	struct ipv6hdr *hdr;
+	u8 head[100] = {};
+
+	if (type != ETH_P_IPV6)
+		return -EINVAL;
+
+	hdr = ipv6_hdr(skb);
+	hc06_ptr = head + 2;
+
+	pr_debug("IPv6 header dump:\n\tversion = %d\n\tlength  = %d\n"
+		 "\tnexthdr = 0x%02x\n\thop_lim = %d\n\tdest    = %pI6c\n",
+		hdr->version, ntohs(hdr->payload_len), hdr->nexthdr,
+		hdr->hop_limit, &hdr->daddr);
+
+	raw_dump_table(__func__, "raw skb network header dump",
+		skb_network_header(skb), sizeof(struct ipv6hdr));
+
+	/*
+	 * As we copy some bit-length fields, in the IPHC encoding bytes,
+	 * we sometimes use |=
+	 * If the field is 0, and the current bit value in memory is 1,
+	 * this does not work. We therefore reset the IPHC encoding here
+	 */
+	iphc0 = LOWPAN_DISPATCH_IPHC;
+	iphc1 = 0;
+
+	/* TODO: context lookup */
+
+	raw_dump_inline(__func__, "saddr",
+			(unsigned char *)_saddr, IEEE802154_ADDR_LEN);
+	raw_dump_inline(__func__, "daddr",
+			(unsigned char *)_daddr, IEEE802154_ADDR_LEN);
+
+	raw_dump_table(__func__,
+			"sending raw skb network uncompressed packet",
+			skb->data, skb->len);
+
+	/*
+	 * Traffic class, flow label
+	 * If flow label is 0, compress it. If traffic class is 0, compress it
+	 * We have to process both in the same time as the offset of traffic
+	 * class depends on the presence of version and flow label
+	 */
+
+	/* hc06 format of TC is ECN | DSCP , original one is DSCP | ECN */
+	tmp = (hdr->priority << 4) | (hdr->flow_lbl[0] >> 4);
+	tmp = ((tmp & 0x03) << 6) | (tmp >> 2);
+
+	if (((hdr->flow_lbl[0] & 0x0F) == 0) &&
+	     (hdr->flow_lbl[1] == 0) && (hdr->flow_lbl[2] == 0)) {
+		/* flow label can be compressed */
+		iphc0 |= LOWPAN_IPHC_FL_C;
+		if ((hdr->priority == 0) &&
+		   ((hdr->flow_lbl[0] & 0xF0) == 0)) {
+			/* compress (elide) all */
+			iphc0 |= LOWPAN_IPHC_TC_C;
+		} else {
+			/* compress only the flow label */
+			*hc06_ptr = tmp;
+			hc06_ptr += 1;
+		}
+	} else {
+		/* Flow label cannot be compressed */
+		if ((hdr->priority == 0) &&
+		   ((hdr->flow_lbl[0] & 0xF0) == 0)) {
+			/* compress only traffic class */
+			iphc0 |= LOWPAN_IPHC_TC_C;
+			*hc06_ptr = (tmp & 0xc0) | (hdr->flow_lbl[0] & 0x0F);
+			memcpy(hc06_ptr + 1, &hdr->flow_lbl[1], 2);
+			hc06_ptr += 3;
+		} else {
+			/* compress nothing */
+			memcpy(hc06_ptr, &hdr, 4);
+			/* replace the top byte with new ECN | DSCP format */
+			*hc06_ptr = tmp;
+			hc06_ptr += 4;
+		}
+	}
+
+	/* NOTE: payload length is always compressed */
+
+	/* Next Header is compress if UDP */
+	if (hdr->nexthdr == UIP_PROTO_UDP)
+		iphc0 |= LOWPAN_IPHC_NH_C;
+
+	if ((iphc0 & LOWPAN_IPHC_NH_C) == 0) {
+		*hc06_ptr = hdr->nexthdr;
+		hc06_ptr += 1;
+	}
+
+	/*
+	 * Hop limit
+	 * if 1:   compress, encoding is 01
+	 * if 64:  compress, encoding is 10
+	 * if 255: compress, encoding is 11
+	 * else do not compress
+	 */
+	switch (hdr->hop_limit) {
+	case 1:
+		iphc0 |= LOWPAN_IPHC_TTL_1;
+		break;
+	case 64:
+		iphc0 |= LOWPAN_IPHC_TTL_64;
+		break;
+	case 255:
+		iphc0 |= LOWPAN_IPHC_TTL_255;
+		break;
+	default:
+		*hc06_ptr = hdr->hop_limit;
+		hc06_ptr += 1;
+		break;
+	}
+
+	/* source address compression */
+	if (is_addr_unspecified(&hdr->saddr)) {
+		pr_debug("source address is unspecified, setting SAC\n");
+		iphc1 |= LOWPAN_IPHC_SAC;
+	/* TODO: context lookup */
+	} else if (is_addr_link_local(&hdr->saddr)) {
+		iphc1 |= lowpan_compress_addr_64(&hc06_ptr,
+				LOWPAN_IPHC_SAM_BIT, &hdr->saddr, _saddr);
+		pr_debug("source address unicast link-local %pI6c "
+			"iphc1 0x%02x\n", &hdr->saddr, iphc1);
+	} else {
+		pr_debug("send the full source address\n");
+		memcpy(hc06_ptr, &hdr->saddr.s6_addr16[0], 16);
+		hc06_ptr += 16;
+	}
+
+	/* destination address compression */
+	if (is_addr_mcast(&hdr->daddr)) {
+		pr_debug("destination address is multicast: ");
+		iphc1 |= LOWPAN_IPHC_M;
+		if (lowpan_is_mcast_addr_compressable8(&hdr->daddr)) {
+			pr_debug("compressed to 1 octet\n");
+			iphc1 |= LOWPAN_IPHC_DAM_11;
+			/* use last byte */
+			*hc06_ptr = hdr->daddr.s6_addr[15];
+			hc06_ptr += 1;
+		} else if (lowpan_is_mcast_addr_compressable32(&hdr->daddr)) {
+			pr_debug("compressed to 4 octets\n");
+			iphc1 |= LOWPAN_IPHC_DAM_10;
+			/* second byte + the last three */
+			*hc06_ptr = hdr->daddr.s6_addr[1];
+			memcpy(hc06_ptr + 1, &hdr->daddr.s6_addr[13], 3);
+			hc06_ptr += 4;
+		} else if (lowpan_is_mcast_addr_compressable48(&hdr->daddr)) {
+			pr_debug("compressed to 6 octets\n");
+			iphc1 |= LOWPAN_IPHC_DAM_01;
+			/* second byte + the last five */
+			*hc06_ptr = hdr->daddr.s6_addr[1];
+			memcpy(hc06_ptr + 1, &hdr->daddr.s6_addr[11], 5);
+			hc06_ptr += 6;
+		} else {
+			pr_debug("using full address\n");
+			iphc1 |= LOWPAN_IPHC_DAM_00;
+			memcpy(hc06_ptr, &hdr->daddr.s6_addr[0], 16);
+			hc06_ptr += 16;
+		}
+	} else {
+		/* TODO: context lookup */
+		if (is_addr_link_local(&hdr->daddr)) {
+			iphc1 |= lowpan_compress_addr_64(&hc06_ptr,
+				LOWPAN_IPHC_DAM_BIT, &hdr->daddr, _daddr);
+			pr_debug("dest address unicast link-local %pI6c "
+				"iphc1 0x%02x\n", &hdr->daddr, iphc1);
+		} else {
+			pr_debug("dest address unicast %pI6c\n", &hdr->daddr);
+			memcpy(hc06_ptr, &hdr->daddr.s6_addr16[0], 16);
+			hc06_ptr += 16;
+		}
+	}
+
+	/* UDP header compression */
+	if (hdr->nexthdr == UIP_PROTO_UDP)
+		compress_udp_header(&hc06_ptr, skb);
+
+	head[0] = iphc0;
+	head[1] = iphc1;
+
+	skb_pull(skb, sizeof(struct ipv6hdr));
+	skb_reset_transport_header(skb);
+	memcpy(skb_push(skb, hc06_ptr - head), head, hc06_ptr - head);
+	skb_reset_network_header(skb);
+
+	pr_debug("header len %d skb %u\n", (int)(hc06_ptr - head), skb->len);
+
+	raw_dump_table(__func__, "raw skb data dump compressed",
+				skb->data, skb->len);
+	return 0;
+}
+EXPORT_SYMBOL_GPL(lowpan_header_compress);
diff --git a/net/ieee802154/Makefile b/net/ieee802154/Makefile
index d7716d6..951a83e 100644
--- a/net/ieee802154/Makefile
+++ b/net/ieee802154/Makefile
@@ -1,5 +1,5 @@
 obj-$(CONFIG_IEEE802154) += ieee802154.o af_802154.o
-obj-$(CONFIG_IEEE802154_6LOWPAN) += 6lowpan.o
+obj-$(CONFIG_IEEE802154_6LOWPAN) += 6lowpan.o 6lowpan_iphc.o
 
 ieee802154-y := netlink.o nl-mac.o nl-phy.o nl_policy.o wpan-class.o
 af_802154-y := af_ieee802154.o raw.o dgram.o
diff --git a/net/ipv6/addrconf.c b/net/ipv6/addrconf.c
index 12c97d8..d125fcd 100644
--- a/net/ipv6/addrconf.c
+++ b/net/ipv6/addrconf.c
@@ -1816,6 +1816,7 @@ static int ipv6_generate_eui64(u8 *eui, struct net_device *dev)
 		return addrconf_ifid_sit(eui, dev);
 	case ARPHRD_IPGRE:
 		return addrconf_ifid_gre(eui, dev);
+	case ARPHRD_6LOWPAN:
 	case ARPHRD_IEEE802154:
 		return addrconf_ifid_eui64(eui, dev);
 	case ARPHRD_IEEE1394:
@@ -2658,7 +2659,8 @@ static void addrconf_dev_config(struct net_device *dev)
 	    (dev->type != ARPHRD_INFINIBAND) &&
 	    (dev->type != ARPHRD_IEEE802154) &&
 	    (dev->type != ARPHRD_IEEE1394) &&
-	    (dev->type != ARPHRD_TUNNEL6)) {
+	    (dev->type != ARPHRD_TUNNEL6) &&
+	    (dev->type != ARPHRD_6LOWPAN)) {
 		/* Alas, we support only Ethernet autoconfiguration. */
 		return;
 	}