Return-Path: From: Jukka Rissanen To: linux-bluetooth@vger.kernel.org Subject: [RFC v3 3/8] Bluetooth: Initial skeleton code for BT 6LoWPAN Date: Fri, 8 Nov 2013 17:23:10 +0200 Message-Id: <1383924195-7936-4-git-send-email-jukka.rissanen@linux.intel.com> In-Reply-To: <1383924195-7936-1-git-send-email-jukka.rissanen@linux.intel.com> References: <1383924195-7936-1-git-send-email-jukka.rissanen@linux.intel.com> Sender: linux-bluetooth-owner@vger.kernel.org List-ID: Signed-off-by: Jukka Rissanen --- include/net/bluetooth/l2cap.h | 1 + net/bluetooth/6lowpan.c | 546 ++++++++++++++++++++++++++++++++++++++++++ net/bluetooth/6lowpan.h | 26 ++ net/bluetooth/Makefile | 2 +- net/bluetooth/l2cap_core.c | 22 +- 5 files changed, 595 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..2982a4e --- /dev/null +++ b/net/bluetooth/6lowpan.c @@ -0,0 +1,546 @@ +/* + 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 +#include +#include +#include +#include + +#include +#include +#include + +#include + +#include +#include +#include + +#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) +{ +#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 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 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; + struct inet6_dev *idev; + int err; + + 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); + + idev = in6_dev_get(net); + if (idev) { + idev->cnf.autoconf = 1; + idev->cnf.forwarding = 1; + idev->cnf.accept_ra = 2; + + in6_dev_put(idev); + } + + 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; + + write_lock(&devices_lock); + INIT_LIST_HEAD(&peer->list); + peer_add(dev, peer); + write_unlock(&devices_lock); + +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 +#include + +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