[RFC v4 3/6] Bluetooth: Initial skeleton code for BT 6LoWPAN

[Date Prev][Date Next][Thread Prev][Thread Next][Date Index][Thread Index]

 



Signed-off-by: Jukka Rissanen <jukka.rissanen@xxxxxxxxxxxxxxx>
---
 include/net/bluetooth/l2cap.h |   1 +
 net/bluetooth/6lowpan.c       | 544 ++++++++++++++++++++++++++++++++++++++++++
 net/bluetooth/6lowpan.h       |  26 ++
 net/bluetooth/Makefile        |   2 +-
 net/bluetooth/l2cap_core.c    |  22 +-
 5 files changed, 593 insertions(+), 2 deletions(-)
 create mode 100644 net/bluetooth/6lowpan.c
 create mode 100644 net/bluetooth/6lowpan.h

diff --git a/include/net/bluetooth/l2cap.h b/include/net/bluetooth/l2cap.h
index 5132990..c28ac0d 100644
--- a/include/net/bluetooth/l2cap.h
+++ b/include/net/bluetooth/l2cap.h
@@ -133,6 +133,7 @@ struct l2cap_conninfo {
 #define L2CAP_FC_L2CAP		0x02
 #define L2CAP_FC_CONNLESS	0x04
 #define L2CAP_FC_A2MP		0x08
+#define L2CAP_FC_6LOWPAN        0x3e
 
 /* L2CAP Control Field bit masks */
 #define L2CAP_CTRL_SAR			0xC000
diff --git a/net/bluetooth/6lowpan.c b/net/bluetooth/6lowpan.c
new file mode 100644
index 0000000..85754e2
--- /dev/null
+++ b/net/bluetooth/6lowpan.c
@@ -0,0 +1,544 @@
+/*
+   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/version.h>
+#include <linux/bitops.h>
+#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>
+
+#include <net/bluetooth/bluetooth.h>
+#include <net/bluetooth/hci_core.h>
+#include <net/bluetooth/l2cap.h>
+
+#include "../ieee802154/6lowpan.h" /* for the compression defines */
+
+#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_dev {
+	struct net_device *dev;
+	struct work_struct delete_netdev;
+	struct list_head list;
+};
+
+struct 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_info {
+	struct net_device *net;
+	struct list_head peers;
+	int peer_count;
+};
+
+static inline struct lowpan_info *lowpan_info(const struct net_device *dev)
+{
+	return netdev_priv(dev);
+}
+
+static inline void peer_add(struct lowpan_info *info, struct peer *peer)
+{
+	list_add(&peer->list, &info->peers);
+	info->peer_count++;
+}
+
+static inline void peer_del(struct lowpan_info *info, struct peer *peer)
+{
+	list_del(&peer->list);
+	info->peer_count--;
+	if (info->peer_count < 0) {
+		BT_ERR("peer count underflow");
+		info->peer_count = 0;
+	}
+}
+
+static inline struct peer *peer_lookup_ba(struct lowpan_info *info,
+						__u8 type, bdaddr_t *ba)
+{
+	struct peer *peer, *tmp;
+
+	BT_DBG("peers %d addr %pMR type %d", info->peer_count, ba, type);
+
+	list_for_each_entry_safe(peer, tmp, &info->peers, list) {
+		BT_DBG("addr %pMR type %d",
+			&peer->conn->hcon->dst, peer->conn->hcon->dst_type);
+
+		if (!bacmp(&peer->conn->hcon->dst, ba))
+			return peer;
+	}
+
+	return NULL;
+}
+
+static inline struct peer *peer_lookup_conn(struct lowpan_info *info,
+						struct l2cap_conn *conn)
+{
+	struct peer *peer, *tmp;
+
+	list_for_each_entry_safe(peer, tmp, &info->peers, list) {
+		if (peer->conn == conn)
+			return peer;
+	}
+
+	return NULL;
+}
+
+static struct peer *lookup_peer(struct l2cap_conn *conn,
+				struct lowpan_info **dev)
+{
+	struct lowpan_dev *entry, *tmp;
+	struct peer *peer = NULL;
+
+	write_lock(&devices_lock);
+
+	list_for_each_entry_safe(entry, tmp, &bt_6lowpan_devices, list) {
+		struct lowpan_info *info = lowpan_info(entry->dev);
+
+		if (dev)
+			*dev = info;
+
+		peer = peer_lookup_conn(info, conn);
+		if (peer)
+			break;
+	}
+
+	write_unlock(&devices_lock);
+
+	return peer;
+}
+
+/* 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);
+}
+
+static int recv_pkt(struct sk_buff *skb, struct net_device *dev)
+{
+	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_info *info = NULL;
+	struct peer *peer;
+	int err = -ENOENT;
+
+	peer = lookup_peer(conn, &info);
+	if (!peer)
+		return -ENOENT;
+
+	if (info && info->net) {
+		err = recv_pkt(skb, info->net, conn);
+		BT_DBG("recv pkt %d", err);
+	}
+
+	return err;
+}
+
+static void do_send(struct l2cap_conn *conn, struct sk_buff *skb)
+{
+	BT_DBG("conn %p, skb %p len %d priority %u", conn, skb, skb->len,
+	       skb->priority);
+
+	return;
+}
+
+static int conn_send(struct l2cap_conn *conn,
+			void *msg, size_t len, u32 priority,
+			struct net_device *dev)
+{
+	struct sk_buff *skb = {0};
+
+	do_send(conn, skb);
+	return 0;
+}
+
+/* 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 *dev)
+{
+	raw_dump_table(__func__, "raw skb data dump before fragmentation",
+				skb->data, skb->len);
+
+	return conn_send(conn, skb->data, skb->len, 0, dev);
+}
+
+static int send_mcast_pkt(struct sk_buff *skb, struct net_device *dev)
+{
+	struct sk_buff *local_skb;
+	struct lowpan_dev *entry, *tmp;
+	int err = 0;
+
+	write_lock(&devices_lock);
+
+	list_for_each_entry_safe(entry, tmp, &bt_6lowpan_devices, list) {
+		struct peer *pentry, *ptmp;
+		struct lowpan_info *info;
+
+		if (entry->dev != dev)
+			continue;
+
+		info = lowpan_info(entry->dev);
+
+		list_for_each_entry_safe(pentry, ptmp, &info->peers, list) {
+			local_skb = skb_clone(skb, GFP_ATOMIC);
+
+			err = send_pkt(pentry->conn, dev->dev_addr,
+					pentry->eui64_addr,
+					local_skb, dev);
+
+			kfree_skb(local_skb);
+		}
+	}
+
+	write_unlock(&devices_lock);
+
+	return err;
+}
+
+static netdev_tx_t xmit(struct sk_buff *skb, struct net_device *dev)
+{
+	int err = -ENOENT;
+	unsigned char *eui64_addr;
+	struct lowpan_info *info;
+	struct 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.
+		 */
+		err = send_mcast_pkt(skb, dev);
+	} else {
+		get_dest_bdaddr(&lowpan_cb(skb)->addr, &addr, &addr_type);
+		eui64_addr = lowpan_cb(skb)->addr.s6_addr + 8;
+		info = lowpan_info(dev);
+
+		write_lock(&devices_lock);
+		peer = peer_lookup_ba(info, addr_type, &addr);
+		write_unlock(&devices_lock);
+
+		BT_DBG("xmit from %s to %pMR (%pI6c), peer %p", dev->name,
+			&addr, &lowpan_cb(skb)->addr, peer);
+
+		if (peer && peer->conn)
+			err = send_pkt(peer->conn,
+					dev->dev_addr,
+					eui64_addr,
+					skb,
+					dev);
+	}
+	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		= xmit,
+};
+
+static void netdev_setup(struct net_device *dev)
+{
+	dev->addr_len		= EUI64_ADDR_LEN;
+	dev->type		= ARPHRD_RAWIP;
+
+	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->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 == BDADDR_LE_PUBLIC)
+		eui[0] |= 1;
+	else
+		eui[0] &= ~1;
+}
+
+static void set_dev_addr(struct net_device *net, bdaddr_t *addr,
+			u8 addr_type)
+{
+	net->addr_assign_type = NET_ADDR_PERM;
+	set_addr(net->dev_addr, addr->b, addr_type);
+	net->dev_addr[0] ^= 2;
+}
+
+static void ifup(struct net_device *net)
+{
+	int err;
+
+	rtnl_lock();
+	err = dev_open(net);
+	if (err < 0)
+		BT_INFO("iface %s cannot be opened (%d)", net->name, err);
+	rtnl_unlock();
+}
+
+/*
+ * 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_info *dev = NULL;
+	struct peer *peer = NULL;
+	struct net_device *net;
+	struct lowpan_dev *entry;
+	int err = 0;
+
+	peer = lookup_peer(conn, &dev);
+	if (peer)
+		return -EEXIST;
+
+	/*
+	 * If net device exists already, just add route.
+	 */
+	if (dev && !peer)
+		goto add_peer;
+
+	net = alloc_netdev(sizeof(struct lowpan_info), IFACE_NAME_TEMPLATE,
+			netdev_setup);
+	if (!net)
+		return -ENOMEM;
+
+	dev = netdev_priv(net);
+	dev->net = net;
+	INIT_LIST_HEAD(&dev->peers);
+
+	set_dev_addr(net, &conn->hcon->src, conn->hcon->src_type);
+
+	net->netdev_ops = &netdev_ops;
+	SET_NETDEV_DEV(net, &conn->hcon->dev);
+	SET_NETDEV_DEVTYPE(net, &bt_type);
+
+	err = register_netdev(net);
+	if (err < 0) {
+		BT_INFO("register_netdev failed %d", err);
+		free_netdev(net);
+		goto out;
+	}
+
+	BT_DBG("ifindex %d peer bdaddr %pMR my addr %pMR",
+		net->ifindex, &conn->hcon->dst, &conn->hcon->src);
+	set_bit(__LINK_STATE_PRESENT, &net->state);
+
+	entry = kzalloc(sizeof(struct lowpan_dev), GFP_KERNEL);
+	if (!entry) {
+		unregister_netdev(net);
+		return -ENOMEM;
+	}
+
+	entry->dev = net;
+
+	write_lock(&devices_lock);
+	INIT_LIST_HEAD(&entry->list);
+	list_add(&entry->list, &bt_6lowpan_devices);
+	write_unlock(&devices_lock);
+
+	ifup(net);
+
+add_peer:
+	peer = kzalloc(sizeof(struct peer), GFP_KERNEL);
+	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);
+
+	write_lock(&devices_lock);
+	INIT_LIST_HEAD(&peer->list);
+	peer_add(dev, peer);
+	write_unlock(&devices_lock);
+
+	netdev_notify_peers(dev->net); /* send neighbour adv at startup */
+
+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->dev);
+
+	/* The entry pointer is deleted in device_event() */
+}
+
+int bt_6lowpan_del_conn(struct l2cap_conn *conn)
+{
+	struct lowpan_dev *entry, *tmp;
+	struct lowpan_info *info = NULL;
+	struct peer *peer;
+	int err = -ENOENT;
+
+	write_lock(&devices_lock);
+
+	list_for_each_entry_safe(entry, tmp, &bt_6lowpan_devices, list) {
+		info = lowpan_info(entry->dev);
+		peer = peer_lookup_conn(info, conn);
+		if (peer) {
+			peer_del(info, peer);
+			err = 0;
+			break;
+		}
+	}
+
+	write_unlock(&devices_lock);
+
+	if (!err && info && info->peer_count == 0) {
+		/*
+		 * This function 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);
+	}
+
+	return err;
+}
+
+static int device_event(struct notifier_block *unused,
+				unsigned long event, void *ptr)
+{
+	struct net_device *dev = netdev_notifier_info_to_dev(ptr);
+	struct lowpan_dev *entry, *tmp;
+
+	if (dev->type != ARPHRD_RAWIP)
+		return NOTIFY_DONE;
+
+	switch (event) {
+	case NETDEV_UNREGISTER:
+		write_lock(&devices_lock);
+		list_for_each_entry_safe(entry, tmp, &bt_6lowpan_devices,
+					list) {
+			if (entry->dev == dev) {
+				list_del(&entry->list);
+				kfree(entry);
+				break;
+			}
+		}
+		write_unlock(&devices_lock);
+		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..80cb215 100644
--- a/net/bluetooth/Makefile
+++ b/net/bluetooth/Makefile
@@ -10,6 +10,6 @@ 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
 
 subdir-ccflags-y += -D__CHECK_ENDIAN__
diff --git a/net/bluetooth/l2cap_core.c b/net/bluetooth/l2cap_core.c
index 4af3821..155485f 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;
 
@@ -6473,6 +6474,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;
@@ -6510,6 +6515,11 @@ int l2cap_connect_ind(struct hci_dev *hdev, bdaddr_t *bdaddr)
 	return exact ? lm1 : lm2;
 }
 
+static bool is_bt_6lowpan(struct hci_conn *hcon)
+{
+	return false;
+}
+
 void l2cap_connect_cfm(struct hci_conn *hcon, u8 status)
 {
 	struct l2cap_conn *conn;
@@ -6518,8 +6528,12 @@ void l2cap_connect_cfm(struct hci_conn *hcon, u8 status)
 
 	if (!status) {
 		conn = l2cap_conn_add(hcon);
-		if (conn)
+		if (conn) {
 			l2cap_conn_ready(conn);
+
+			if (is_bt_6lowpan(hcon))
+				bt_6lowpan_add_conn(conn);
+		}
 	} else {
 		l2cap_conn_del(hcon, bt_to_errno(status));
 	}
@@ -6540,6 +6554,9 @@ void l2cap_disconn_cfm(struct hci_conn *hcon, u8 reason)
 {
 	BT_DBG("hcon %p reason %d", hcon, reason);
 
+	if (is_bt_6lowpan(hcon))
+		bt_6lowpan_del_conn(hcon->l2cap_data);
+
 	l2cap_conn_del(hcon, bt_to_errno(reason));
 }
 
@@ -6817,11 +6834,14 @@ int __init l2cap_init(void)
 	l2cap_debugfs = debugfs_create_file("l2cap", 0444, bt_debugfs,
 					    NULL, &l2cap_debugfs_fops);
 
+	bt_6lowpan_init();
+
 	return 0;
 }
 
 void l2cap_exit(void)
 {
+	bt_6lowpan_cleanup();
 	debugfs_remove(l2cap_debugfs);
 	l2cap_cleanup_sockets();
 }
-- 
1.8.3.1

--
To unsubscribe from this list: send the line "unsubscribe linux-bluetooth" in
the body of a message to majordomo@xxxxxxxxxxxxxxx
More majordomo info at  http://vger.kernel.org/majordomo-info.html




[Index of Archives]     [Bluez Devel]     [Linux Wireless Networking]     [Linux Wireless Personal Area Networking]     [Linux ATH6KL]     [Linux USB Devel]     [Linux Media Drivers]     [Linux Audio Users]     [Linux Kernel]     [Linux SCSI]     [Big List of Linux Books]

  Powered by Linux