diff options
Diffstat (limited to 'kernel/net/bluetooth')
52 files changed, 52564 insertions, 0 deletions
diff --git a/kernel/net/bluetooth/6lowpan.c b/kernel/net/bluetooth/6lowpan.c new file mode 100644 index 000000000..1742b849f --- /dev/null +++ b/kernel/net/bluetooth/6lowpan.c @@ -0,0 +1,1469 @@ +/* + Copyright (c) 2013-2014 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 <linux/module.h> +#include <linux/debugfs.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 <net/6lowpan.h> /* for the compression support */ + +#define VERSION "0.1" + +static struct dentry *lowpan_enable_debugfs; +static struct dentry *lowpan_control_debugfs; + +#define IFACE_NAME_TEMPLATE "bt%d" +#define EUI64_ADDR_LEN 8 + +struct skb_cb { + struct in6_addr addr; + struct in6_addr gw; + struct l2cap_chan *chan; + int status; +}; +#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_SPINLOCK(devices_lock); + +static bool enable_6lowpan; + +/* We are listening incoming connections via this channel + */ +static struct l2cap_chan *listen_chan; + +struct lowpan_peer { + struct list_head list; + struct rcu_head rcu; + struct l2cap_chan *chan; + + /* 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_rcu(&peer->list, &dev->peers); + atomic_inc(&dev->peer_count); +} + +static inline bool peer_del(struct lowpan_dev *dev, struct lowpan_peer *peer) +{ + list_del_rcu(&peer->list); + kfree_rcu(peer, rcu); + + module_put(THIS_MODULE); + + 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; + + BT_DBG("peers %d addr %pMR type %d", atomic_read(&dev->peer_count), + ba, type); + + rcu_read_lock(); + + list_for_each_entry_rcu(peer, &dev->peers, list) { + BT_DBG("dst addr %pMR dst type %d", + &peer->chan->dst, peer->chan->dst_type); + + if (bacmp(&peer->chan->dst, ba)) + continue; + + if (type == peer->chan->dst_type) { + rcu_read_unlock(); + return peer; + } + } + + rcu_read_unlock(); + + return NULL; +} + +static inline struct lowpan_peer *__peer_lookup_chan(struct lowpan_dev *dev, + struct l2cap_chan *chan) +{ + struct lowpan_peer *peer; + + list_for_each_entry_rcu(peer, &dev->peers, list) { + if (peer->chan == chan) + return peer; + } + + return NULL; +} + +static inline struct lowpan_peer *__peer_lookup_conn(struct lowpan_dev *dev, + struct l2cap_conn *conn) +{ + struct lowpan_peer *peer; + + list_for_each_entry_rcu(peer, &dev->peers, list) { + if (peer->chan->conn == conn) + return peer; + } + + return NULL; +} + +static inline struct lowpan_peer *peer_lookup_dst(struct lowpan_dev *dev, + struct in6_addr *daddr, + struct sk_buff *skb) +{ + struct lowpan_peer *peer; + struct in6_addr *nexthop; + struct rt6_info *rt = (struct rt6_info *)skb_dst(skb); + int count = atomic_read(&dev->peer_count); + + BT_DBG("peers %d addr %pI6c rt %p", count, daddr, rt); + + /* If we have multiple 6lowpan peers, then check where we should + * send the packet. If only one peer exists, then we can send the + * packet right away. + */ + if (count == 1) { + rcu_read_lock(); + peer = list_first_or_null_rcu(&dev->peers, struct lowpan_peer, + list); + rcu_read_unlock(); + return peer; + } + + if (!rt) { + nexthop = &lowpan_cb(skb)->gw; + + if (ipv6_addr_any(nexthop)) + return NULL; + } else { + nexthop = rt6_nexthop(rt); + + /* We need to remember the address because it is needed + * by bt_xmit() when sending the packet. In bt_xmit(), the + * destination routing info is not set. + */ + memcpy(&lowpan_cb(skb)->gw, nexthop, sizeof(struct in6_addr)); + } + + BT_DBG("gw %pI6c", nexthop); + + rcu_read_lock(); + + list_for_each_entry_rcu(peer, &dev->peers, list) { + BT_DBG("dst addr %pMR dst type %d ip %pI6c", + &peer->chan->dst, peer->chan->dst_type, + &peer->peer_addr); + + if (!ipv6_addr_cmp(&peer->peer_addr, nexthop)) { + rcu_read_unlock(); + return peer; + } + } + + rcu_read_unlock(); + + return NULL; +} + +static struct lowpan_peer *lookup_peer(struct l2cap_conn *conn) +{ + struct lowpan_dev *entry; + struct lowpan_peer *peer = NULL; + + rcu_read_lock(); + + list_for_each_entry_rcu(entry, &bt_6lowpan_devices, list) { + peer = __peer_lookup_conn(entry, conn); + if (peer) + break; + } + + rcu_read_unlock(); + + return peer; +} + +static struct lowpan_dev *lookup_dev(struct l2cap_conn *conn) +{ + struct lowpan_dev *entry; + struct lowpan_dev *dev = NULL; + + rcu_read_lock(); + + list_for_each_entry_rcu(entry, &bt_6lowpan_devices, list) { + if (conn->hcon->hdev == entry->hdev) { + dev = entry; + break; + } + } + + rcu_read_unlock(); + + return dev; +} + +static int give_skb_to_upper(struct sk_buff *skb, struct net_device *dev) +{ + struct sk_buff *skb_cp; + + skb_cp = skb_copy(skb, GFP_ATOMIC); + if (!skb_cp) + return NET_RX_DROP; + + return netif_rx(skb_cp); +} + +static int iphc_decompress(struct sk_buff *skb, struct net_device *netdev, + struct l2cap_chan *chan) +{ + const u8 *saddr, *daddr; + u8 iphc0, iphc1; + struct lowpan_dev *dev; + struct lowpan_peer *peer; + + dev = lowpan_dev(netdev); + + rcu_read_lock(); + peer = __peer_lookup_chan(dev, chan); + rcu_read_unlock(); + if (!peer) + return -EINVAL; + + saddr = peer->eui64_addr; + daddr = dev->netdev->dev_addr; + + /* at least two bytes will be used for the encoding */ + if (skb->len < 2) + return -EINVAL; + + if (lowpan_fetch_skb_u8(skb, &iphc0)) + return -EINVAL; + + if (lowpan_fetch_skb_u8(skb, &iphc1)) + return -EINVAL; + + return lowpan_header_decompress(skb, netdev, + saddr, IEEE802154_ADDR_LONG, + EUI64_ADDR_LEN, daddr, + IEEE802154_ADDR_LONG, EUI64_ADDR_LEN, + iphc0, iphc1); + +} + +static int recv_pkt(struct sk_buff *skb, struct net_device *dev, + struct l2cap_chan *chan) +{ + struct sk_buff *local_skb; + int ret; + + if (!netif_running(dev)) + goto drop; + + if (dev->type != ARPHRD_6LOWPAN) + goto drop; + + skb = skb_share_check(skb, GFP_ATOMIC); + if (!skb) + 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++; + + consume_skb(local_skb); + consume_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 = iphc_decompress(local_skb, dev, chan); + if (ret < 0) { + kfree_skb(local_skb); + goto drop; + } + + local_skb->protocol = htons(ETH_P_IPV6); + local_skb->pkt_type = PACKET_HOST; + local_skb->dev = dev; + + 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++; + + consume_skb(local_skb); + consume_skb(skb); + break; + default: + break; + } + } + + return NET_RX_SUCCESS; + +drop: + dev->stats.rx_dropped++; + return NET_RX_DROP; +} + +/* Packet from BT LE device */ +static int chan_recv_cb(struct l2cap_chan *chan, struct sk_buff *skb) +{ + struct lowpan_dev *dev; + struct lowpan_peer *peer; + int err; + + peer = lookup_peer(chan->conn); + if (!peer) + return -ENOENT; + + dev = lookup_dev(chan->conn); + if (!dev || !dev->netdev) + return -ENOENT; + + err = recv_pkt(skb, dev->netdev, chan); + if (err) { + BT_DBG("recv pkt %d", err); + err = -EAGAIN; + } + + return err; +} + +static u8 get_addr_type_from_eui64(u8 byte) +{ + /* Is universal(0) or local(1) bit */ + return ((byte & 0x02) ? BDADDR_LE_RANDOM : BDADDR_LE_PUBLIC); +} + +static void copy_to_bdaddr(struct in6_addr *ip6_daddr, bdaddr_t *addr) +{ + u8 *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]; +} + +static void convert_dest_bdaddr(struct in6_addr *ip6_daddr, + bdaddr_t *addr, u8 *addr_type) +{ + copy_to_bdaddr(ip6_daddr, addr); + + /* We need to toggle the U/L bit that we got from IPv6 address + * so that we get the proper address and type of the BD address. + */ + addr->b[5] ^= 0x02; + + *addr_type = get_addr_type_from_eui64(addr->b[5]); +} + +static int setup_header(struct sk_buff *skb, struct net_device *netdev, + bdaddr_t *peer_addr, u8 *peer_addr_type) +{ + struct in6_addr ipv6_daddr; + struct lowpan_dev *dev; + struct lowpan_peer *peer; + bdaddr_t addr, *any = BDADDR_ANY; + u8 *daddr = any->b; + int err, status = 0; + + dev = lowpan_dev(netdev); + + memcpy(&ipv6_daddr, &lowpan_cb(skb)->addr, sizeof(ipv6_daddr)); + + if (ipv6_addr_is_multicast(&ipv6_daddr)) { + lowpan_cb(skb)->chan = NULL; + } else { + u8 addr_type; + + /* Get destination BT device from skb. + * If there is no such peer then discard the packet. + */ + convert_dest_bdaddr(&ipv6_daddr, &addr, &addr_type); + + BT_DBG("dest addr %pMR type %d IP %pI6c", &addr, + addr_type, &ipv6_daddr); + + peer = peer_lookup_ba(dev, &addr, addr_type); + if (!peer) { + /* The packet might be sent to 6lowpan interface + * because of routing (either via default route + * or user set route) so get peer according to + * the destination address. + */ + peer = peer_lookup_dst(dev, &ipv6_daddr, skb); + if (!peer) { + BT_DBG("no such peer %pMR found", &addr); + return -ENOENT; + } + } + + daddr = peer->eui64_addr; + *peer_addr = addr; + *peer_addr_type = addr_type; + lowpan_cb(skb)->chan = peer->chan; + + status = 1; + } + + lowpan_header_compress(skb, netdev, ETH_P_IPV6, daddr, + dev->netdev->dev_addr, skb->len); + + err = dev_hard_header(skb, netdev, ETH_P_IPV6, NULL, NULL, 0); + if (err < 0) + return err; + + return status; +} + +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; + + if (type != ETH_P_IPV6) + return -EINVAL; + + hdr = ipv6_hdr(skb); + + memcpy(&lowpan_cb(skb)->addr, &hdr->daddr, sizeof(struct in6_addr)); + + return 0; +} + +/* Packet to BT LE device */ +static int send_pkt(struct l2cap_chan *chan, struct sk_buff *skb, + struct net_device *netdev) +{ + struct msghdr msg; + struct kvec iv; + int err; + + /* Remember the skb so that we can send EAGAIN to the caller if + * we run out of credits. + */ + chan->data = skb; + + iv.iov_base = skb->data; + iv.iov_len = skb->len; + + memset(&msg, 0, sizeof(msg)); + iov_iter_kvec(&msg.msg_iter, WRITE | ITER_KVEC, &iv, 1, skb->len); + + err = l2cap_chan_send(chan, &msg, skb->len); + if (err > 0) { + netdev->stats.tx_bytes += err; + netdev->stats.tx_packets++; + return 0; + } + + if (!err) + err = lowpan_cb(skb)->status; + + if (err < 0) { + if (err == -EAGAIN) + netdev->stats.tx_dropped++; + else + netdev->stats.tx_errors++; + } + + return err; +} + +static int send_mcast_pkt(struct sk_buff *skb, struct net_device *netdev) +{ + struct sk_buff *local_skb; + struct lowpan_dev *entry; + int err = 0; + + rcu_read_lock(); + + list_for_each_entry_rcu(entry, &bt_6lowpan_devices, list) { + struct lowpan_peer *pentry; + struct lowpan_dev *dev; + + if (entry->netdev != netdev) + continue; + + dev = lowpan_dev(entry->netdev); + + list_for_each_entry_rcu(pentry, &dev->peers, list) { + int ret; + + local_skb = skb_clone(skb, GFP_ATOMIC); + + BT_DBG("xmit %s to %pMR type %d IP %pI6c chan %p", + netdev->name, + &pentry->chan->dst, pentry->chan->dst_type, + &pentry->peer_addr, pentry->chan); + ret = send_pkt(pentry->chan, local_skb, netdev); + if (ret < 0) + err = ret; + + kfree_skb(local_skb); + } + } + + rcu_read_unlock(); + + return err; +} + +static netdev_tx_t bt_xmit(struct sk_buff *skb, struct net_device *netdev) +{ + int err = 0; + bdaddr_t addr; + u8 addr_type; + + /* We must take a copy of the skb before we modify/replace the ipv6 + * header as the header could be used elsewhere + */ + skb = skb_unshare(skb, GFP_ATOMIC); + if (!skb) + return NET_XMIT_DROP; + + /* Return values from setup_header() + * <0 - error, packet is dropped + * 0 - this is a multicast packet + * 1 - this is unicast packet + */ + err = setup_header(skb, netdev, &addr, &addr_type); + if (err < 0) { + kfree_skb(skb); + return NET_XMIT_DROP; + } + + if (err) { + if (lowpan_cb(skb)->chan) { + BT_DBG("xmit %s to %pMR type %d IP %pI6c chan %p", + netdev->name, &addr, addr_type, + &lowpan_cb(skb)->addr, lowpan_cb(skb)->chan); + err = send_pkt(lowpan_cb(skb)->chan, skb, netdev); + } else { + err = -ENOENT; + } + } else { + /* We need to send the packet to every device behind this + * interface. + */ + err = send_mcast_pkt(skb, netdev); + } + + dev_kfree_skb(skb); + + if (err) + BT_DBG("ERROR: xmit failed (%d)", err); + + return err < 0 ? NET_XMIT_DROP : err; +} + +static struct lock_class_key bt_tx_busylock; +static struct lock_class_key bt_netdev_xmit_lock_key; + +static void bt_set_lockdep_class_one(struct net_device *dev, + struct netdev_queue *txq, + void *_unused) +{ + lockdep_set_class(&txq->_xmit_lock, &bt_netdev_xmit_lock_key); +} + +static int bt_dev_init(struct net_device *dev) +{ + netdev_for_each_tx_queue(dev, bt_set_lockdep_class_one, NULL); + dev->qdisc_tx_busylock = &bt_tx_busylock; + + return 0; +} + +static const struct net_device_ops netdev_ops = { + .ndo_init = bt_dev_init, + .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 | + IFF_MULTICAST; + 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]; + + /* Universal/local bit set, BT 6lowpan draft ch. 3.2.1 */ + if (addr_type == BDADDR_LE_PUBLIC) + eui[0] &= ~0x02; + else + eui[0] |= 0x02; + + BT_DBG("type %d addr %*phC", addr_type, 8, eui); +} + +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); +} + +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 ifdown(struct net_device *netdev) +{ + int err; + + rtnl_lock(); + err = dev_close(netdev); + if (err < 0) + BT_INFO("iface %s cannot be closed (%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; + + if (!enable_6lowpan) + return false; + + return true; +} + +static struct l2cap_chan *chan_create(void) +{ + struct l2cap_chan *chan; + + chan = l2cap_chan_create(); + if (!chan) + return NULL; + + l2cap_chan_set_defaults(chan); + + chan->chan_type = L2CAP_CHAN_CONN_ORIENTED; + chan->mode = L2CAP_MODE_LE_FLOWCTL; + chan->omtu = 65535; + chan->imtu = chan->omtu; + + return chan; +} + +static struct l2cap_chan *chan_open(struct l2cap_chan *pchan) +{ + struct l2cap_chan *chan; + + chan = chan_create(); + if (!chan) + return NULL; + + chan->remote_mps = chan->omtu; + chan->mps = chan->omtu; + + chan->state = BT_CONNECTED; + + return chan; +} + +static void set_ip_addr_bits(u8 addr_type, u8 *addr) +{ + if (addr_type == BDADDR_LE_PUBLIC) + *addr |= 0x02; + else + *addr &= ~0x02; +} + +static struct l2cap_chan *add_peer_chan(struct l2cap_chan *chan, + struct lowpan_dev *dev) +{ + struct lowpan_peer *peer; + + peer = kzalloc(sizeof(*peer), GFP_ATOMIC); + if (!peer) + return NULL; + + peer->chan = chan; + 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, chan->dst.b, + chan->dst_type); + + memcpy(&peer->eui64_addr, (u8 *)&peer->peer_addr.s6_addr + 8, + EUI64_ADDR_LEN); + + /* IPv6 address needs to have the U/L bit set properly so toggle + * it back here. + */ + set_ip_addr_bits(chan->dst_type, (u8 *)&peer->peer_addr.s6_addr + 8); + + spin_lock(&devices_lock); + INIT_LIST_HEAD(&peer->list); + peer_add(dev, peer); + spin_unlock(&devices_lock); + + /* 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 peer->chan; +} + +static int setup_netdev(struct l2cap_chan *chan, struct lowpan_dev **dev) +{ + struct net_device *netdev; + int err = 0; + + netdev = alloc_netdev(sizeof(struct lowpan_dev), IFACE_NAME_TEMPLATE, + NET_NAME_UNKNOWN, netdev_setup); + if (!netdev) + return -ENOMEM; + + set_dev_addr(netdev, &chan->src, chan->src_type); + + netdev->netdev_ops = &netdev_ops; + SET_NETDEV_DEV(netdev, &chan->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 type %d my addr %pMR type %d", + netdev->ifindex, &chan->dst, chan->dst_type, + &chan->src, chan->src_type); + set_bit(__LINK_STATE_PRESENT, &netdev->state); + + *dev = netdev_priv(netdev); + (*dev)->netdev = netdev; + (*dev)->hdev = chan->conn->hcon->hdev; + INIT_LIST_HEAD(&(*dev)->peers); + + spin_lock(&devices_lock); + INIT_LIST_HEAD(&(*dev)->list); + list_add_rcu(&(*dev)->list, &bt_6lowpan_devices); + spin_unlock(&devices_lock); + + return 0; + +out: + return err; +} + +static inline void chan_ready_cb(struct l2cap_chan *chan) +{ + struct lowpan_dev *dev; + + dev = lookup_dev(chan->conn); + + BT_DBG("chan %p conn %p dev %p", chan, chan->conn, dev); + + if (!dev) { + if (setup_netdev(chan, &dev) < 0) { + l2cap_chan_del(chan, -ENOENT); + return; + } + } + + if (!try_module_get(THIS_MODULE)) + return; + + add_peer_chan(chan, dev); + ifup(dev->netdev); +} + +static inline struct l2cap_chan *chan_new_conn_cb(struct l2cap_chan *pchan) +{ + struct l2cap_chan *chan; + + chan = chan_open(pchan); + chan->ops = pchan->ops; + + BT_DBG("chan %p pchan %p", chan, pchan); + + return chan; +} + +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() */ +} + +static void chan_close_cb(struct l2cap_chan *chan) +{ + struct lowpan_dev *entry; + struct lowpan_dev *dev = NULL; + struct lowpan_peer *peer; + int err = -ENOENT; + bool last = false, removed = true; + + BT_DBG("chan %p conn %p", chan, chan->conn); + + if (chan->conn && chan->conn->hcon) { + if (!is_bt_6lowpan(chan->conn->hcon)) + return; + + /* If conn is set, then the netdev is also there and we should + * not remove it. + */ + removed = false; + } + + spin_lock(&devices_lock); + + list_for_each_entry_rcu(entry, &bt_6lowpan_devices, list) { + dev = lowpan_dev(entry->netdev); + peer = __peer_lookup_chan(dev, chan); + if (peer) { + last = peer_del(dev, peer); + err = 0; + + BT_DBG("dev %p removing %speer %p", dev, + last ? "last " : "1 ", peer); + BT_DBG("chan %p orig refcnt %d", chan, + atomic_read(&chan->kref.refcount)); + + l2cap_chan_put(chan); + break; + } + } + + if (!err && last && dev && !atomic_read(&dev->peer_count)) { + spin_unlock(&devices_lock); + + cancel_delayed_work_sync(&dev->notify_peers); + + ifdown(dev->netdev); + + if (!removed) { + INIT_WORK(&entry->delete_netdev, delete_netdev); + schedule_work(&entry->delete_netdev); + } + } else { + spin_unlock(&devices_lock); + } + + return; +} + +static void chan_state_change_cb(struct l2cap_chan *chan, int state, int err) +{ + BT_DBG("chan %p conn %p state %s err %d", chan, chan->conn, + state_to_string(state), err); +} + +static struct sk_buff *chan_alloc_skb_cb(struct l2cap_chan *chan, + unsigned long hdr_len, + unsigned long len, int nb) +{ + /* Note that we must allocate using GFP_ATOMIC here as + * this function is called originally from netdev hard xmit + * function in atomic context. + */ + return bt_skb_alloc(hdr_len + len, GFP_ATOMIC); +} + +static void chan_suspend_cb(struct l2cap_chan *chan) +{ + struct sk_buff *skb = chan->data; + + BT_DBG("chan %p conn %p skb %p", chan, chan->conn, skb); + + if (!skb) + return; + + lowpan_cb(skb)->status = -EAGAIN; +} + +static void chan_resume_cb(struct l2cap_chan *chan) +{ + struct sk_buff *skb = chan->data; + + BT_DBG("chan %p conn %p skb %p", chan, chan->conn, skb); + + if (!skb) + return; + + lowpan_cb(skb)->status = 0; +} + +static long chan_get_sndtimeo_cb(struct l2cap_chan *chan) +{ + return L2CAP_CONN_TIMEOUT; +} + +static const struct l2cap_ops bt_6lowpan_chan_ops = { + .name = "L2CAP 6LoWPAN channel", + .new_connection = chan_new_conn_cb, + .recv = chan_recv_cb, + .close = chan_close_cb, + .state_change = chan_state_change_cb, + .ready = chan_ready_cb, + .resume = chan_resume_cb, + .suspend = chan_suspend_cb, + .get_sndtimeo = chan_get_sndtimeo_cb, + .alloc_skb = chan_alloc_skb_cb, + + .teardown = l2cap_chan_no_teardown, + .defer = l2cap_chan_no_defer, + .set_shutdown = l2cap_chan_no_set_shutdown, +}; + +static inline __u8 bdaddr_type(__u8 type) +{ + if (type == ADDR_LE_DEV_PUBLIC) + return BDADDR_LE_PUBLIC; + else + return BDADDR_LE_RANDOM; +} + +static struct l2cap_chan *chan_get(void) +{ + struct l2cap_chan *pchan; + + pchan = chan_create(); + if (!pchan) + return NULL; + + pchan->ops = &bt_6lowpan_chan_ops; + + return pchan; +} + +static int bt_6lowpan_connect(bdaddr_t *addr, u8 dst_type) +{ + struct l2cap_chan *pchan; + int err; + + pchan = chan_get(); + if (!pchan) + return -EINVAL; + + err = l2cap_chan_connect(pchan, cpu_to_le16(L2CAP_PSM_IPSP), 0, + addr, dst_type); + + BT_DBG("chan %p err %d", pchan, err); + if (err < 0) + l2cap_chan_put(pchan); + + return err; +} + +static int bt_6lowpan_disconnect(struct l2cap_conn *conn, u8 dst_type) +{ + struct lowpan_peer *peer; + + BT_DBG("conn %p dst type %d", conn, dst_type); + + peer = lookup_peer(conn); + if (!peer) + return -ENOENT; + + BT_DBG("peer %p chan %p", peer, peer->chan); + + l2cap_chan_close(peer->chan, ENOENT); + + return 0; +} + +static struct l2cap_chan *bt_6lowpan_listen(void) +{ + bdaddr_t *addr = BDADDR_ANY; + struct l2cap_chan *pchan; + int err; + + if (!enable_6lowpan) + return NULL; + + pchan = chan_get(); + if (!pchan) + return NULL; + + pchan->state = BT_LISTEN; + pchan->src_type = BDADDR_LE_PUBLIC; + + atomic_set(&pchan->nesting, L2CAP_NESTING_PARENT); + + BT_DBG("chan %p src type %d", pchan, pchan->src_type); + + err = l2cap_add_psm(pchan, addr, cpu_to_le16(L2CAP_PSM_IPSP)); + if (err) { + l2cap_chan_put(pchan); + BT_ERR("psm cannot be added err %d", err); + return NULL; + } + + return pchan; +} + +static int get_l2cap_conn(char *buf, bdaddr_t *addr, u8 *addr_type, + struct l2cap_conn **conn) +{ + struct hci_conn *hcon; + struct hci_dev *hdev; + bdaddr_t *src = BDADDR_ANY; + int n; + + n = sscanf(buf, "%hhx:%hhx:%hhx:%hhx:%hhx:%hhx %hhu", + &addr->b[5], &addr->b[4], &addr->b[3], + &addr->b[2], &addr->b[1], &addr->b[0], + addr_type); + + if (n < 7) + return -EINVAL; + + hdev = hci_get_route(addr, src); + if (!hdev) + return -ENOENT; + + hci_dev_lock(hdev); + hcon = hci_conn_hash_lookup_ba(hdev, LE_LINK, addr); + hci_dev_unlock(hdev); + + if (!hcon) + return -ENOENT; + + *conn = (struct l2cap_conn *)hcon->l2cap_data; + + BT_DBG("conn %p dst %pMR type %d", *conn, &hcon->dst, hcon->dst_type); + + return 0; +} + +static void disconnect_all_peers(void) +{ + struct lowpan_dev *entry; + struct lowpan_peer *peer, *tmp_peer, *new_peer; + struct list_head peers; + + INIT_LIST_HEAD(&peers); + + /* We make a separate list of peers as the close_cb() will + * modify the device peers list so it is better not to mess + * with the same list at the same time. + */ + + rcu_read_lock(); + + list_for_each_entry_rcu(entry, &bt_6lowpan_devices, list) { + list_for_each_entry_rcu(peer, &entry->peers, list) { + new_peer = kmalloc(sizeof(*new_peer), GFP_ATOMIC); + if (!new_peer) + break; + + new_peer->chan = peer->chan; + INIT_LIST_HEAD(&new_peer->list); + + list_add(&new_peer->list, &peers); + } + } + + rcu_read_unlock(); + + spin_lock(&devices_lock); + list_for_each_entry_safe(peer, tmp_peer, &peers, list) { + l2cap_chan_close(peer->chan, ENOENT); + + list_del_rcu(&peer->list); + kfree_rcu(peer, rcu); + + module_put(THIS_MODULE); + } + spin_unlock(&devices_lock); +} + +struct set_enable { + struct work_struct work; + bool flag; +}; + +static void do_enable_set(struct work_struct *work) +{ + struct set_enable *set_enable = container_of(work, + struct set_enable, work); + + if (!set_enable->flag || enable_6lowpan != set_enable->flag) + /* Disconnect existing connections if 6lowpan is + * disabled + */ + disconnect_all_peers(); + + enable_6lowpan = set_enable->flag; + + if (listen_chan) { + l2cap_chan_close(listen_chan, 0); + l2cap_chan_put(listen_chan); + } + + listen_chan = bt_6lowpan_listen(); + + kfree(set_enable); +} + +static int lowpan_enable_set(void *data, u64 val) +{ + struct set_enable *set_enable; + + set_enable = kzalloc(sizeof(*set_enable), GFP_KERNEL); + if (!set_enable) + return -ENOMEM; + + set_enable->flag = !!val; + INIT_WORK(&set_enable->work, do_enable_set); + + schedule_work(&set_enable->work); + + return 0; +} + +static int lowpan_enable_get(void *data, u64 *val) +{ + *val = enable_6lowpan; + return 0; +} + +DEFINE_SIMPLE_ATTRIBUTE(lowpan_enable_fops, lowpan_enable_get, + lowpan_enable_set, "%llu\n"); + +static ssize_t lowpan_control_write(struct file *fp, + const char __user *user_buffer, + size_t count, + loff_t *position) +{ + char buf[32]; + size_t buf_size = min(count, sizeof(buf) - 1); + int ret; + bdaddr_t addr; + u8 addr_type; + struct l2cap_conn *conn = NULL; + + if (copy_from_user(buf, user_buffer, buf_size)) + return -EFAULT; + + buf[buf_size] = '\0'; + + if (memcmp(buf, "connect ", 8) == 0) { + ret = get_l2cap_conn(&buf[8], &addr, &addr_type, &conn); + if (ret == -EINVAL) + return ret; + + if (listen_chan) { + l2cap_chan_close(listen_chan, 0); + l2cap_chan_put(listen_chan); + listen_chan = NULL; + } + + if (conn) { + struct lowpan_peer *peer; + + if (!is_bt_6lowpan(conn->hcon)) + return -EINVAL; + + peer = lookup_peer(conn); + if (peer) { + BT_DBG("6LoWPAN connection already exists"); + return -EALREADY; + } + + BT_DBG("conn %p dst %pMR type %d user %d", conn, + &conn->hcon->dst, conn->hcon->dst_type, + addr_type); + } + + ret = bt_6lowpan_connect(&addr, addr_type); + if (ret < 0) + return ret; + + return count; + } + + if (memcmp(buf, "disconnect ", 11) == 0) { + ret = get_l2cap_conn(&buf[11], &addr, &addr_type, &conn); + if (ret < 0) + return ret; + + ret = bt_6lowpan_disconnect(conn, addr_type); + if (ret < 0) + return ret; + + return count; + } + + return count; +} + +static int lowpan_control_show(struct seq_file *f, void *ptr) +{ + struct lowpan_dev *entry; + struct lowpan_peer *peer; + + spin_lock(&devices_lock); + + list_for_each_entry(entry, &bt_6lowpan_devices, list) { + list_for_each_entry(peer, &entry->peers, list) + seq_printf(f, "%pMR (type %u)\n", + &peer->chan->dst, peer->chan->dst_type); + } + + spin_unlock(&devices_lock); + + return 0; +} + +static int lowpan_control_open(struct inode *inode, struct file *file) +{ + return single_open(file, lowpan_control_show, inode->i_private); +} + +static const struct file_operations lowpan_control_fops = { + .open = lowpan_control_open, + .read = seq_read, + .write = lowpan_control_write, + .llseek = seq_lseek, + .release = single_release, +}; + +static void disconnect_devices(void) +{ + struct lowpan_dev *entry, *tmp, *new_dev; + struct list_head devices; + + INIT_LIST_HEAD(&devices); + + /* We make a separate list of devices because the unregister_netdev() + * will call device_event() which will also want to modify the same + * devices list. + */ + + rcu_read_lock(); + + list_for_each_entry_rcu(entry, &bt_6lowpan_devices, list) { + new_dev = kmalloc(sizeof(*new_dev), GFP_ATOMIC); + if (!new_dev) + break; + + new_dev->netdev = entry->netdev; + INIT_LIST_HEAD(&new_dev->list); + + list_add_rcu(&new_dev->list, &devices); + } + + rcu_read_unlock(); + + list_for_each_entry_safe(entry, tmp, &devices, list) { + ifdown(entry->netdev); + BT_DBG("Unregistering netdev %s %p", + entry->netdev->name, entry->netdev); + unregister_netdev(entry->netdev); + kfree(entry); + } +} + +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; + + if (netdev->type != ARPHRD_6LOWPAN) + return NOTIFY_DONE; + + switch (event) { + case NETDEV_UNREGISTER: + spin_lock(&devices_lock); + list_for_each_entry(entry, &bt_6lowpan_devices, list) { + if (entry->netdev == netdev) { + BT_DBG("Unregistered netdev %s %p", + netdev->name, netdev); + list_del(&entry->list); + kfree(entry); + break; + } + } + spin_unlock(&devices_lock); + break; + } + + return NOTIFY_DONE; +} + +static struct notifier_block bt_6lowpan_dev_notifier = { + .notifier_call = device_event, +}; + +static int __init bt_6lowpan_init(void) +{ + lowpan_enable_debugfs = debugfs_create_file("6lowpan_enable", 0644, + bt_debugfs, NULL, + &lowpan_enable_fops); + lowpan_control_debugfs = debugfs_create_file("6lowpan_control", 0644, + bt_debugfs, NULL, + &lowpan_control_fops); + + return register_netdevice_notifier(&bt_6lowpan_dev_notifier); +} + +static void __exit bt_6lowpan_exit(void) +{ + debugfs_remove(lowpan_enable_debugfs); + debugfs_remove(lowpan_control_debugfs); + + if (listen_chan) { + l2cap_chan_close(listen_chan, 0); + l2cap_chan_put(listen_chan); + } + + disconnect_devices(); + + unregister_netdevice_notifier(&bt_6lowpan_dev_notifier); +} + +module_init(bt_6lowpan_init); +module_exit(bt_6lowpan_exit); + +MODULE_AUTHOR("Jukka Rissanen <jukka.rissanen@linux.intel.com>"); +MODULE_DESCRIPTION("Bluetooth 6LoWPAN"); +MODULE_VERSION(VERSION); +MODULE_LICENSE("GPL"); diff --git a/kernel/net/bluetooth/Kconfig b/kernel/net/bluetooth/Kconfig new file mode 100644 index 000000000..b8c794b87 --- /dev/null +++ b/kernel/net/bluetooth/Kconfig @@ -0,0 +1,102 @@ +# +# Bluetooth subsystem configuration +# + +menuconfig BT + tristate "Bluetooth subsystem support" + depends on NET && !S390 + depends on RFKILL || !RFKILL + select CRC16 + select CRYPTO + select CRYPTO_BLKCIPHER + select CRYPTO_AES + select CRYPTO_CMAC + select CRYPTO_ECB + select CRYPTO_SHA256 + help + Bluetooth is low-cost, low-power, short-range wireless technology. + It was designed as a replacement for cables and other short-range + technologies like IrDA. Bluetooth operates in personal area range + that typically extends up to 10 meters. More information about + Bluetooth can be found at <http://www.bluetooth.com/>. + + Linux Bluetooth subsystem consist of several layers: + Bluetooth Core + HCI device and connection manager, scheduler + SCO audio links + L2CAP (Logical Link Control and Adaptation Protocol) + SMP (Security Manager Protocol) on LE (Low Energy) links + HCI Device drivers (Interface to the hardware) + RFCOMM Module (RFCOMM Protocol) + BNEP Module (Bluetooth Network Encapsulation Protocol) + CMTP Module (CAPI Message Transport Protocol) + HIDP Module (Human Interface Device Protocol) + + Say Y here to compile Bluetooth support into the kernel or say M to + compile it as module (bluetooth). + + To use Linux Bluetooth subsystem, you will need several user-space + utilities like hciconfig and bluetoothd. These utilities and updates + to Bluetooth kernel modules are provided in the BlueZ packages. For + more information, see <http://www.bluez.org/>. + +config BT_BREDR + bool "Bluetooth Classic (BR/EDR) features" + depends on BT + default y + +source "net/bluetooth/rfcomm/Kconfig" + +source "net/bluetooth/bnep/Kconfig" + +source "net/bluetooth/cmtp/Kconfig" + +source "net/bluetooth/hidp/Kconfig" + +config BT_LE + bool "Bluetooth Low Energy (LE) features" + depends on BT + default y + +config BT_6LOWPAN + tristate "Bluetooth 6LoWPAN support" + depends on BT_LE && 6LOWPAN + help + IPv6 compression over Bluetooth Low Energy. + +config BT_SELFTEST + bool "Bluetooth self testing support" + depends on BT && DEBUG_KERNEL + help + Run self tests when initializing the Bluetooth subsystem. This + is a developer option and can cause significant delay when booting + the system. + + When the Bluetooth subsystem is built as module, then the test + cases are run first thing at module load time. When the Bluetooth + subsystem is compiled into the kernel image, then the test cases + are run late in the initcall hierarchy. + +config BT_SELFTEST_ECDH + bool "ECDH test cases" + depends on BT_LE && BT_SELFTEST + help + Run test cases for ECDH cryptographic functionality used by the + Bluetooth Low Energy Secure Connections feature. + +config BT_SELFTEST_SMP + bool "SMP test cases" + depends on BT_LE && BT_SELFTEST + help + Run test cases for SMP cryptographic functionality, including both + legacy SMP as well as the Secure Connections features. + +config BT_DEBUGFS + bool "Export Bluetooth internals in debugfs" + depends on BT && DEBUG_FS + default y + help + Provide extensive information about internal Bluetooth states + in debugfs. + +source "drivers/bluetooth/Kconfig" diff --git a/kernel/net/bluetooth/Makefile b/kernel/net/bluetooth/Makefile new file mode 100644 index 000000000..9a8ea232d --- /dev/null +++ b/kernel/net/bluetooth/Makefile @@ -0,0 +1,21 @@ +# +# Makefile for the Linux Bluetooth subsystem. +# + +obj-$(CONFIG_BT) += bluetooth.o +obj-$(CONFIG_BT_RFCOMM) += rfcomm/ +obj-$(CONFIG_BT_BNEP) += bnep/ +obj-$(CONFIG_BT_CMTP) += cmtp/ +obj-$(CONFIG_BT_HIDP) += hidp/ +obj-$(CONFIG_BT_6LOWPAN) += bluetooth_6lowpan.o + +bluetooth_6lowpan-y := 6lowpan.o + +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 ecc.o hci_request.o mgmt_util.o + +bluetooth-$(CONFIG_BT_DEBUGFS) += hci_debugfs.o +bluetooth-$(CONFIG_BT_SELFTEST) += selftest.o + +subdir-ccflags-y += -D__CHECK_ENDIAN__ diff --git a/kernel/net/bluetooth/a2mp.c b/kernel/net/bluetooth/a2mp.c new file mode 100644 index 000000000..5a04eb1a7 --- /dev/null +++ b/kernel/net/bluetooth/a2mp.c @@ -0,0 +1,1025 @@ +/* + Copyright (c) 2010,2011 Code Aurora Forum. All rights reserved. + Copyright (c) 2011,2012 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 <net/bluetooth/bluetooth.h> +#include <net/bluetooth/hci_core.h> +#include <net/bluetooth/l2cap.h> + +#include "a2mp.h" +#include "amp.h" + +#define A2MP_FEAT_EXT 0x8000 + +/* Global AMP Manager list */ +static LIST_HEAD(amp_mgr_list); +static DEFINE_MUTEX(amp_mgr_list_lock); + +/* A2MP build & send command helper functions */ +static struct a2mp_cmd *__a2mp_build(u8 code, u8 ident, u16 len, void *data) +{ + struct a2mp_cmd *cmd; + int plen; + + plen = sizeof(*cmd) + len; + cmd = kzalloc(plen, GFP_KERNEL); + if (!cmd) + return NULL; + + cmd->code = code; + cmd->ident = ident; + cmd->len = cpu_to_le16(len); + + memcpy(cmd->data, data, len); + + return cmd; +} + +static void a2mp_send(struct amp_mgr *mgr, u8 code, u8 ident, u16 len, void *data) +{ + struct l2cap_chan *chan = mgr->a2mp_chan; + struct a2mp_cmd *cmd; + u16 total_len = len + sizeof(*cmd); + struct kvec iv; + struct msghdr msg; + + cmd = __a2mp_build(code, ident, len, data); + if (!cmd) + return; + + iv.iov_base = cmd; + iv.iov_len = total_len; + + memset(&msg, 0, sizeof(msg)); + + iov_iter_kvec(&msg.msg_iter, WRITE | ITER_KVEC, &iv, 1, total_len); + + l2cap_chan_send(chan, &msg, total_len); + + kfree(cmd); +} + +static u8 __next_ident(struct amp_mgr *mgr) +{ + if (++mgr->ident == 0) + mgr->ident = 1; + + return mgr->ident; +} + +static struct amp_mgr *amp_mgr_lookup_by_state(u8 state) +{ + struct amp_mgr *mgr; + + mutex_lock(&_mgr_list_lock); + list_for_each_entry(mgr, &_mgr_list, list) { + if (test_and_clear_bit(state, &mgr->state)) { + amp_mgr_get(mgr); + mutex_unlock(&_mgr_list_lock); + return mgr; + } + } + mutex_unlock(&_mgr_list_lock); + + return NULL; +} + +/* hci_dev_list shall be locked */ +static void __a2mp_add_cl(struct amp_mgr *mgr, struct a2mp_cl *cl) +{ + struct hci_dev *hdev; + int i = 1; + + cl[0].id = AMP_ID_BREDR; + cl[0].type = AMP_TYPE_BREDR; + cl[0].status = AMP_STATUS_BLUETOOTH_ONLY; + + list_for_each_entry(hdev, &hci_dev_list, list) { + if (hdev->dev_type == HCI_AMP) { + cl[i].id = hdev->id; + cl[i].type = hdev->amp_type; + if (test_bit(HCI_UP, &hdev->flags)) + cl[i].status = hdev->amp_status; + else + cl[i].status = AMP_STATUS_POWERED_DOWN; + i++; + } + } +} + +/* Processing A2MP messages */ +static int a2mp_command_rej(struct amp_mgr *mgr, struct sk_buff *skb, + struct a2mp_cmd *hdr) +{ + struct a2mp_cmd_rej *rej = (void *) skb->data; + + if (le16_to_cpu(hdr->len) < sizeof(*rej)) + return -EINVAL; + + BT_DBG("ident %d reason %d", hdr->ident, le16_to_cpu(rej->reason)); + + skb_pull(skb, sizeof(*rej)); + + return 0; +} + +static int a2mp_discover_req(struct amp_mgr *mgr, struct sk_buff *skb, + struct a2mp_cmd *hdr) +{ + struct a2mp_discov_req *req = (void *) skb->data; + u16 len = le16_to_cpu(hdr->len); + struct a2mp_discov_rsp *rsp; + u16 ext_feat; + u8 num_ctrl; + struct hci_dev *hdev; + + if (len < sizeof(*req)) + return -EINVAL; + + skb_pull(skb, sizeof(*req)); + + ext_feat = le16_to_cpu(req->ext_feat); + + BT_DBG("mtu %d efm 0x%4.4x", le16_to_cpu(req->mtu), ext_feat); + + /* check that packet is not broken for now */ + while (ext_feat & A2MP_FEAT_EXT) { + if (len < sizeof(ext_feat)) + return -EINVAL; + + ext_feat = get_unaligned_le16(skb->data); + BT_DBG("efm 0x%4.4x", ext_feat); + len -= sizeof(ext_feat); + skb_pull(skb, sizeof(ext_feat)); + } + + read_lock(&hci_dev_list_lock); + + /* at minimum the BR/EDR needs to be listed */ + num_ctrl = 1; + + list_for_each_entry(hdev, &hci_dev_list, list) { + if (hdev->dev_type == HCI_AMP) + num_ctrl++; + } + + len = num_ctrl * sizeof(struct a2mp_cl) + sizeof(*rsp); + rsp = kmalloc(len, GFP_ATOMIC); + if (!rsp) { + read_unlock(&hci_dev_list_lock); + return -ENOMEM; + } + + rsp->mtu = cpu_to_le16(L2CAP_A2MP_DEFAULT_MTU); + rsp->ext_feat = 0; + + __a2mp_add_cl(mgr, rsp->cl); + + read_unlock(&hci_dev_list_lock); + + a2mp_send(mgr, A2MP_DISCOVER_RSP, hdr->ident, len, rsp); + + kfree(rsp); + return 0; +} + +static int a2mp_discover_rsp(struct amp_mgr *mgr, struct sk_buff *skb, + struct a2mp_cmd *hdr) +{ + struct a2mp_discov_rsp *rsp = (void *) skb->data; + u16 len = le16_to_cpu(hdr->len); + struct a2mp_cl *cl; + u16 ext_feat; + bool found = false; + + if (len < sizeof(*rsp)) + return -EINVAL; + + len -= sizeof(*rsp); + skb_pull(skb, sizeof(*rsp)); + + ext_feat = le16_to_cpu(rsp->ext_feat); + + BT_DBG("mtu %d efm 0x%4.4x", le16_to_cpu(rsp->mtu), ext_feat); + + /* check that packet is not broken for now */ + while (ext_feat & A2MP_FEAT_EXT) { + if (len < sizeof(ext_feat)) + return -EINVAL; + + ext_feat = get_unaligned_le16(skb->data); + BT_DBG("efm 0x%4.4x", ext_feat); + len -= sizeof(ext_feat); + skb_pull(skb, sizeof(ext_feat)); + } + + cl = (void *) skb->data; + while (len >= sizeof(*cl)) { + BT_DBG("Remote AMP id %d type %d status %d", cl->id, cl->type, + cl->status); + + if (cl->id != AMP_ID_BREDR && cl->type != AMP_TYPE_BREDR) { + struct a2mp_info_req req; + + found = true; + req.id = cl->id; + a2mp_send(mgr, A2MP_GETINFO_REQ, __next_ident(mgr), + sizeof(req), &req); + } + + len -= sizeof(*cl); + cl = (void *) skb_pull(skb, sizeof(*cl)); + } + + /* Fall back to L2CAP init sequence */ + if (!found) { + struct l2cap_conn *conn = mgr->l2cap_conn; + struct l2cap_chan *chan; + + mutex_lock(&conn->chan_lock); + + list_for_each_entry(chan, &conn->chan_l, list) { + + BT_DBG("chan %p state %s", chan, + state_to_string(chan->state)); + + if (chan->scid == L2CAP_CID_A2MP) + continue; + + l2cap_chan_lock(chan); + + if (chan->state == BT_CONNECT) + l2cap_send_conn_req(chan); + + l2cap_chan_unlock(chan); + } + + mutex_unlock(&conn->chan_lock); + } + + return 0; +} + +static int a2mp_change_notify(struct amp_mgr *mgr, struct sk_buff *skb, + struct a2mp_cmd *hdr) +{ + struct a2mp_cl *cl = (void *) skb->data; + + while (skb->len >= sizeof(*cl)) { + BT_DBG("Controller id %d type %d status %d", cl->id, cl->type, + cl->status); + cl = (struct a2mp_cl *) skb_pull(skb, sizeof(*cl)); + } + + /* TODO send A2MP_CHANGE_RSP */ + + return 0; +} + +static int a2mp_getinfo_req(struct amp_mgr *mgr, struct sk_buff *skb, + struct a2mp_cmd *hdr) +{ + struct a2mp_info_req *req = (void *) skb->data; + struct hci_dev *hdev; + + if (le16_to_cpu(hdr->len) < sizeof(*req)) + return -EINVAL; + + BT_DBG("id %d", req->id); + + hdev = hci_dev_get(req->id); + if (!hdev || hdev->dev_type != HCI_AMP) { + struct a2mp_info_rsp rsp; + + rsp.id = req->id; + rsp.status = A2MP_STATUS_INVALID_CTRL_ID; + + a2mp_send(mgr, A2MP_GETINFO_RSP, hdr->ident, sizeof(rsp), + &rsp); + + goto done; + } + + set_bit(READ_LOC_AMP_INFO, &mgr->state); + hci_send_cmd(hdev, HCI_OP_READ_LOCAL_AMP_INFO, 0, NULL); + +done: + if (hdev) + hci_dev_put(hdev); + + skb_pull(skb, sizeof(*req)); + return 0; +} + +static int a2mp_getinfo_rsp(struct amp_mgr *mgr, struct sk_buff *skb, + struct a2mp_cmd *hdr) +{ + struct a2mp_info_rsp *rsp = (struct a2mp_info_rsp *) skb->data; + struct a2mp_amp_assoc_req req; + struct amp_ctrl *ctrl; + + if (le16_to_cpu(hdr->len) < sizeof(*rsp)) + return -EINVAL; + + BT_DBG("id %d status 0x%2.2x", rsp->id, rsp->status); + + if (rsp->status) + return -EINVAL; + + ctrl = amp_ctrl_add(mgr, rsp->id); + if (!ctrl) + return -ENOMEM; + + req.id = rsp->id; + a2mp_send(mgr, A2MP_GETAMPASSOC_REQ, __next_ident(mgr), sizeof(req), + &req); + + skb_pull(skb, sizeof(*rsp)); + return 0; +} + +static int a2mp_getampassoc_req(struct amp_mgr *mgr, struct sk_buff *skb, + struct a2mp_cmd *hdr) +{ + struct a2mp_amp_assoc_req *req = (void *) skb->data; + struct hci_dev *hdev; + struct amp_mgr *tmp; + + if (le16_to_cpu(hdr->len) < sizeof(*req)) + return -EINVAL; + + BT_DBG("id %d", req->id); + + /* Make sure that other request is not processed */ + tmp = amp_mgr_lookup_by_state(READ_LOC_AMP_ASSOC); + + hdev = hci_dev_get(req->id); + if (!hdev || hdev->amp_type == AMP_TYPE_BREDR || tmp) { + struct a2mp_amp_assoc_rsp rsp; + rsp.id = req->id; + + if (tmp) { + rsp.status = A2MP_STATUS_COLLISION_OCCURED; + amp_mgr_put(tmp); + } else { + rsp.status = A2MP_STATUS_INVALID_CTRL_ID; + } + + a2mp_send(mgr, A2MP_GETAMPASSOC_RSP, hdr->ident, sizeof(rsp), + &rsp); + + goto done; + } + + amp_read_loc_assoc(hdev, mgr); + +done: + if (hdev) + hci_dev_put(hdev); + + skb_pull(skb, sizeof(*req)); + return 0; +} + +static int a2mp_getampassoc_rsp(struct amp_mgr *mgr, struct sk_buff *skb, + struct a2mp_cmd *hdr) +{ + struct a2mp_amp_assoc_rsp *rsp = (void *) skb->data; + u16 len = le16_to_cpu(hdr->len); + struct hci_dev *hdev; + struct amp_ctrl *ctrl; + struct hci_conn *hcon; + size_t assoc_len; + + if (len < sizeof(*rsp)) + return -EINVAL; + + assoc_len = len - sizeof(*rsp); + + BT_DBG("id %d status 0x%2.2x assoc len %zu", rsp->id, rsp->status, + assoc_len); + + if (rsp->status) + return -EINVAL; + + /* Save remote ASSOC data */ + ctrl = amp_ctrl_lookup(mgr, rsp->id); + if (ctrl) { + u8 *assoc; + + assoc = kmemdup(rsp->amp_assoc, assoc_len, GFP_KERNEL); + if (!assoc) { + amp_ctrl_put(ctrl); + return -ENOMEM; + } + + ctrl->assoc = assoc; + ctrl->assoc_len = assoc_len; + ctrl->assoc_rem_len = assoc_len; + ctrl->assoc_len_so_far = 0; + + amp_ctrl_put(ctrl); + } + + /* Create Phys Link */ + hdev = hci_dev_get(rsp->id); + if (!hdev) + return -EINVAL; + + hcon = phylink_add(hdev, mgr, rsp->id, true); + if (!hcon) + goto done; + + BT_DBG("Created hcon %p: loc:%d -> rem:%d", hcon, hdev->id, rsp->id); + + mgr->bredr_chan->remote_amp_id = rsp->id; + + amp_create_phylink(hdev, mgr, hcon); + +done: + hci_dev_put(hdev); + skb_pull(skb, len); + return 0; +} + +static int a2mp_createphyslink_req(struct amp_mgr *mgr, struct sk_buff *skb, + struct a2mp_cmd *hdr) +{ + struct a2mp_physlink_req *req = (void *) skb->data; + + struct a2mp_physlink_rsp rsp; + struct hci_dev *hdev; + struct hci_conn *hcon; + struct amp_ctrl *ctrl; + + if (le16_to_cpu(hdr->len) < sizeof(*req)) + return -EINVAL; + + BT_DBG("local_id %d, remote_id %d", req->local_id, req->remote_id); + + rsp.local_id = req->remote_id; + rsp.remote_id = req->local_id; + + hdev = hci_dev_get(req->remote_id); + if (!hdev || hdev->amp_type == AMP_TYPE_BREDR) { + rsp.status = A2MP_STATUS_INVALID_CTRL_ID; + goto send_rsp; + } + + ctrl = amp_ctrl_lookup(mgr, rsp.remote_id); + if (!ctrl) { + ctrl = amp_ctrl_add(mgr, rsp.remote_id); + if (ctrl) { + amp_ctrl_get(ctrl); + } else { + rsp.status = A2MP_STATUS_UNABLE_START_LINK_CREATION; + goto send_rsp; + } + } + + if (ctrl) { + size_t assoc_len = le16_to_cpu(hdr->len) - sizeof(*req); + u8 *assoc; + + assoc = kmemdup(req->amp_assoc, assoc_len, GFP_KERNEL); + if (!assoc) { + amp_ctrl_put(ctrl); + return -ENOMEM; + } + + ctrl->assoc = assoc; + ctrl->assoc_len = assoc_len; + ctrl->assoc_rem_len = assoc_len; + ctrl->assoc_len_so_far = 0; + + amp_ctrl_put(ctrl); + } + + hcon = phylink_add(hdev, mgr, req->local_id, false); + if (hcon) { + amp_accept_phylink(hdev, mgr, hcon); + rsp.status = A2MP_STATUS_SUCCESS; + } else { + rsp.status = A2MP_STATUS_UNABLE_START_LINK_CREATION; + } + +send_rsp: + if (hdev) + hci_dev_put(hdev); + + /* Reply error now and success after HCI Write Remote AMP Assoc + command complete with success status + */ + if (rsp.status != A2MP_STATUS_SUCCESS) { + a2mp_send(mgr, A2MP_CREATEPHYSLINK_RSP, hdr->ident, + sizeof(rsp), &rsp); + } else { + set_bit(WRITE_REMOTE_AMP_ASSOC, &mgr->state); + mgr->ident = hdr->ident; + } + + skb_pull(skb, le16_to_cpu(hdr->len)); + return 0; +} + +static int a2mp_discphyslink_req(struct amp_mgr *mgr, struct sk_buff *skb, + struct a2mp_cmd *hdr) +{ + struct a2mp_physlink_req *req = (void *) skb->data; + struct a2mp_physlink_rsp rsp; + struct hci_dev *hdev; + struct hci_conn *hcon; + + if (le16_to_cpu(hdr->len) < sizeof(*req)) + return -EINVAL; + + BT_DBG("local_id %d remote_id %d", req->local_id, req->remote_id); + + rsp.local_id = req->remote_id; + rsp.remote_id = req->local_id; + rsp.status = A2MP_STATUS_SUCCESS; + + hdev = hci_dev_get(req->remote_id); + if (!hdev) { + rsp.status = A2MP_STATUS_INVALID_CTRL_ID; + goto send_rsp; + } + + hcon = hci_conn_hash_lookup_ba(hdev, AMP_LINK, + &mgr->l2cap_conn->hcon->dst); + if (!hcon) { + BT_ERR("No phys link exist"); + rsp.status = A2MP_STATUS_NO_PHYSICAL_LINK_EXISTS; + goto clean; + } + + /* TODO Disconnect Phys Link here */ + +clean: + hci_dev_put(hdev); + +send_rsp: + a2mp_send(mgr, A2MP_DISCONNPHYSLINK_RSP, hdr->ident, sizeof(rsp), &rsp); + + skb_pull(skb, sizeof(*req)); + return 0; +} + +static inline int a2mp_cmd_rsp(struct amp_mgr *mgr, struct sk_buff *skb, + struct a2mp_cmd *hdr) +{ + BT_DBG("ident %d code 0x%2.2x", hdr->ident, hdr->code); + + skb_pull(skb, le16_to_cpu(hdr->len)); + return 0; +} + +/* Handle A2MP signalling */ +static int a2mp_chan_recv_cb(struct l2cap_chan *chan, struct sk_buff *skb) +{ + struct a2mp_cmd *hdr; + struct amp_mgr *mgr = chan->data; + int err = 0; + + amp_mgr_get(mgr); + + while (skb->len >= sizeof(*hdr)) { + u16 len; + + hdr = (void *) skb->data; + len = le16_to_cpu(hdr->len); + + BT_DBG("code 0x%2.2x id %d len %u", hdr->code, hdr->ident, len); + + skb_pull(skb, sizeof(*hdr)); + + if (len > skb->len || !hdr->ident) { + err = -EINVAL; + break; + } + + mgr->ident = hdr->ident; + + switch (hdr->code) { + case A2MP_COMMAND_REJ: + a2mp_command_rej(mgr, skb, hdr); + break; + + case A2MP_DISCOVER_REQ: + err = a2mp_discover_req(mgr, skb, hdr); + break; + + case A2MP_CHANGE_NOTIFY: + err = a2mp_change_notify(mgr, skb, hdr); + break; + + case A2MP_GETINFO_REQ: + err = a2mp_getinfo_req(mgr, skb, hdr); + break; + + case A2MP_GETAMPASSOC_REQ: + err = a2mp_getampassoc_req(mgr, skb, hdr); + break; + + case A2MP_CREATEPHYSLINK_REQ: + err = a2mp_createphyslink_req(mgr, skb, hdr); + break; + + case A2MP_DISCONNPHYSLINK_REQ: + err = a2mp_discphyslink_req(mgr, skb, hdr); + break; + + case A2MP_DISCOVER_RSP: + err = a2mp_discover_rsp(mgr, skb, hdr); + break; + + case A2MP_GETINFO_RSP: + err = a2mp_getinfo_rsp(mgr, skb, hdr); + break; + + case A2MP_GETAMPASSOC_RSP: + err = a2mp_getampassoc_rsp(mgr, skb, hdr); + break; + + case A2MP_CHANGE_RSP: + case A2MP_CREATEPHYSLINK_RSP: + case A2MP_DISCONNPHYSLINK_RSP: + err = a2mp_cmd_rsp(mgr, skb, hdr); + break; + + default: + BT_ERR("Unknown A2MP sig cmd 0x%2.2x", hdr->code); + err = -EINVAL; + break; + } + } + + if (err) { + struct a2mp_cmd_rej rej; + + rej.reason = cpu_to_le16(0); + hdr = (void *) skb->data; + + BT_DBG("Send A2MP Rej: cmd 0x%2.2x err %d", hdr->code, err); + + a2mp_send(mgr, A2MP_COMMAND_REJ, hdr->ident, sizeof(rej), + &rej); + } + + /* Always free skb and return success error code to prevent + from sending L2CAP Disconnect over A2MP channel */ + kfree_skb(skb); + + amp_mgr_put(mgr); + + return 0; +} + +static void a2mp_chan_close_cb(struct l2cap_chan *chan) +{ + l2cap_chan_put(chan); +} + +static void a2mp_chan_state_change_cb(struct l2cap_chan *chan, int state, + int err) +{ + struct amp_mgr *mgr = chan->data; + + if (!mgr) + return; + + BT_DBG("chan %p state %s", chan, state_to_string(state)); + + chan->state = state; + + switch (state) { + case BT_CLOSED: + if (mgr) + amp_mgr_put(mgr); + break; + } +} + +static struct sk_buff *a2mp_chan_alloc_skb_cb(struct l2cap_chan *chan, + unsigned long hdr_len, + unsigned long len, int nb) +{ + struct sk_buff *skb; + + skb = bt_skb_alloc(hdr_len + len, GFP_KERNEL); + if (!skb) + return ERR_PTR(-ENOMEM); + + return skb; +} + +static const struct l2cap_ops a2mp_chan_ops = { + .name = "L2CAP A2MP channel", + .recv = a2mp_chan_recv_cb, + .close = a2mp_chan_close_cb, + .state_change = a2mp_chan_state_change_cb, + .alloc_skb = a2mp_chan_alloc_skb_cb, + + /* Not implemented for A2MP */ + .new_connection = l2cap_chan_no_new_connection, + .teardown = l2cap_chan_no_teardown, + .ready = l2cap_chan_no_ready, + .defer = l2cap_chan_no_defer, + .resume = l2cap_chan_no_resume, + .set_shutdown = l2cap_chan_no_set_shutdown, + .get_sndtimeo = l2cap_chan_no_get_sndtimeo, +}; + +static struct l2cap_chan *a2mp_chan_open(struct l2cap_conn *conn, bool locked) +{ + struct l2cap_chan *chan; + int err; + + chan = l2cap_chan_create(); + if (!chan) + return NULL; + + BT_DBG("chan %p", chan); + + chan->chan_type = L2CAP_CHAN_FIXED; + chan->scid = L2CAP_CID_A2MP; + chan->dcid = L2CAP_CID_A2MP; + chan->omtu = L2CAP_A2MP_DEFAULT_MTU; + chan->imtu = L2CAP_A2MP_DEFAULT_MTU; + chan->flush_to = L2CAP_DEFAULT_FLUSH_TO; + + chan->ops = &a2mp_chan_ops; + + l2cap_chan_set_defaults(chan); + chan->remote_max_tx = chan->max_tx; + chan->remote_tx_win = chan->tx_win; + + chan->retrans_timeout = L2CAP_DEFAULT_RETRANS_TO; + chan->monitor_timeout = L2CAP_DEFAULT_MONITOR_TO; + + skb_queue_head_init(&chan->tx_q); + + chan->mode = L2CAP_MODE_ERTM; + + err = l2cap_ertm_init(chan); + if (err < 0) { + l2cap_chan_del(chan, 0); + return NULL; + } + + chan->conf_state = 0; + + if (locked) + __l2cap_chan_add(conn, chan); + else + l2cap_chan_add(conn, chan); + + chan->remote_mps = chan->omtu; + chan->mps = chan->omtu; + + chan->state = BT_CONNECTED; + + return chan; +} + +/* AMP Manager functions */ +struct amp_mgr *amp_mgr_get(struct amp_mgr *mgr) +{ + BT_DBG("mgr %p orig refcnt %d", mgr, atomic_read(&mgr->kref.refcount)); + + kref_get(&mgr->kref); + + return mgr; +} + +static void amp_mgr_destroy(struct kref *kref) +{ + struct amp_mgr *mgr = container_of(kref, struct amp_mgr, kref); + + BT_DBG("mgr %p", mgr); + + mutex_lock(&_mgr_list_lock); + list_del(&mgr->list); + mutex_unlock(&_mgr_list_lock); + + amp_ctrl_list_flush(mgr); + kfree(mgr); +} + +int amp_mgr_put(struct amp_mgr *mgr) +{ + BT_DBG("mgr %p orig refcnt %d", mgr, atomic_read(&mgr->kref.refcount)); + + return kref_put(&mgr->kref, &_mgr_destroy); +} + +static struct amp_mgr *amp_mgr_create(struct l2cap_conn *conn, bool locked) +{ + struct amp_mgr *mgr; + struct l2cap_chan *chan; + + mgr = kzalloc(sizeof(*mgr), GFP_KERNEL); + if (!mgr) + return NULL; + + BT_DBG("conn %p mgr %p", conn, mgr); + + mgr->l2cap_conn = conn; + + chan = a2mp_chan_open(conn, locked); + if (!chan) { + kfree(mgr); + return NULL; + } + + mgr->a2mp_chan = chan; + chan->data = mgr; + + conn->hcon->amp_mgr = mgr; + + kref_init(&mgr->kref); + + /* Remote AMP ctrl list initialization */ + INIT_LIST_HEAD(&mgr->amp_ctrls); + mutex_init(&mgr->amp_ctrls_lock); + + mutex_lock(&_mgr_list_lock); + list_add(&mgr->list, &_mgr_list); + mutex_unlock(&_mgr_list_lock); + + return mgr; +} + +struct l2cap_chan *a2mp_channel_create(struct l2cap_conn *conn, + struct sk_buff *skb) +{ + struct amp_mgr *mgr; + + if (conn->hcon->type != ACL_LINK) + return NULL; + + mgr = amp_mgr_create(conn, false); + if (!mgr) { + BT_ERR("Could not create AMP manager"); + return NULL; + } + + BT_DBG("mgr: %p chan %p", mgr, mgr->a2mp_chan); + + return mgr->a2mp_chan; +} + +void a2mp_send_getinfo_rsp(struct hci_dev *hdev) +{ + struct amp_mgr *mgr; + struct a2mp_info_rsp rsp; + + mgr = amp_mgr_lookup_by_state(READ_LOC_AMP_INFO); + if (!mgr) + return; + + BT_DBG("%s mgr %p", hdev->name, mgr); + + rsp.id = hdev->id; + rsp.status = A2MP_STATUS_INVALID_CTRL_ID; + + if (hdev->amp_type != AMP_TYPE_BREDR) { + rsp.status = 0; + rsp.total_bw = cpu_to_le32(hdev->amp_total_bw); + rsp.max_bw = cpu_to_le32(hdev->amp_max_bw); + rsp.min_latency = cpu_to_le32(hdev->amp_min_latency); + rsp.pal_cap = cpu_to_le16(hdev->amp_pal_cap); + rsp.assoc_size = cpu_to_le16(hdev->amp_assoc_size); + } + + a2mp_send(mgr, A2MP_GETINFO_RSP, mgr->ident, sizeof(rsp), &rsp); + amp_mgr_put(mgr); +} + +void a2mp_send_getampassoc_rsp(struct hci_dev *hdev, u8 status) +{ + struct amp_mgr *mgr; + struct amp_assoc *loc_assoc = &hdev->loc_assoc; + struct a2mp_amp_assoc_rsp *rsp; + size_t len; + + mgr = amp_mgr_lookup_by_state(READ_LOC_AMP_ASSOC); + if (!mgr) + return; + + BT_DBG("%s mgr %p", hdev->name, mgr); + + len = sizeof(struct a2mp_amp_assoc_rsp) + loc_assoc->len; + rsp = kzalloc(len, GFP_KERNEL); + if (!rsp) { + amp_mgr_put(mgr); + return; + } + + rsp->id = hdev->id; + + if (status) { + rsp->status = A2MP_STATUS_INVALID_CTRL_ID; + } else { + rsp->status = A2MP_STATUS_SUCCESS; + memcpy(rsp->amp_assoc, loc_assoc->data, loc_assoc->len); + } + + a2mp_send(mgr, A2MP_GETAMPASSOC_RSP, mgr->ident, len, rsp); + amp_mgr_put(mgr); + kfree(rsp); +} + +void a2mp_send_create_phy_link_req(struct hci_dev *hdev, u8 status) +{ + struct amp_mgr *mgr; + struct amp_assoc *loc_assoc = &hdev->loc_assoc; + struct a2mp_physlink_req *req; + struct l2cap_chan *bredr_chan; + size_t len; + + mgr = amp_mgr_lookup_by_state(READ_LOC_AMP_ASSOC_FINAL); + if (!mgr) + return; + + len = sizeof(*req) + loc_assoc->len; + + BT_DBG("%s mgr %p assoc_len %zu", hdev->name, mgr, len); + + req = kzalloc(len, GFP_KERNEL); + if (!req) { + amp_mgr_put(mgr); + return; + } + + bredr_chan = mgr->bredr_chan; + if (!bredr_chan) + goto clean; + + req->local_id = hdev->id; + req->remote_id = bredr_chan->remote_amp_id; + memcpy(req->amp_assoc, loc_assoc->data, loc_assoc->len); + + a2mp_send(mgr, A2MP_CREATEPHYSLINK_REQ, __next_ident(mgr), len, req); + +clean: + amp_mgr_put(mgr); + kfree(req); +} + +void a2mp_send_create_phy_link_rsp(struct hci_dev *hdev, u8 status) +{ + struct amp_mgr *mgr; + struct a2mp_physlink_rsp rsp; + struct hci_conn *hs_hcon; + + mgr = amp_mgr_lookup_by_state(WRITE_REMOTE_AMP_ASSOC); + if (!mgr) + return; + + hs_hcon = hci_conn_hash_lookup_state(hdev, AMP_LINK, BT_CONNECT); + if (!hs_hcon) { + rsp.status = A2MP_STATUS_UNABLE_START_LINK_CREATION; + } else { + rsp.remote_id = hs_hcon->remote_id; + rsp.status = A2MP_STATUS_SUCCESS; + } + + BT_DBG("%s mgr %p hs_hcon %p status %u", hdev->name, mgr, hs_hcon, + status); + + rsp.local_id = hdev->id; + a2mp_send(mgr, A2MP_CREATEPHYSLINK_RSP, mgr->ident, sizeof(rsp), &rsp); + amp_mgr_put(mgr); +} + +void a2mp_discover_amp(struct l2cap_chan *chan) +{ + struct l2cap_conn *conn = chan->conn; + struct amp_mgr *mgr = conn->hcon->amp_mgr; + struct a2mp_discov_req req; + + BT_DBG("chan %p conn %p mgr %p", chan, conn, mgr); + + if (!mgr) { + mgr = amp_mgr_create(conn, true); + if (!mgr) + return; + } + + mgr->bredr_chan = chan; + + req.mtu = cpu_to_le16(L2CAP_A2MP_DEFAULT_MTU); + req.ext_feat = 0; + a2mp_send(mgr, A2MP_DISCOVER_REQ, 1, sizeof(req), &req); +} diff --git a/kernel/net/bluetooth/a2mp.h b/kernel/net/bluetooth/a2mp.h new file mode 100644 index 000000000..296f665ad --- /dev/null +++ b/kernel/net/bluetooth/a2mp.h @@ -0,0 +1,142 @@ +/* + Copyright (c) 2010,2011 Code Aurora Forum. All rights reserved. + Copyright (c) 2011,2012 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 __A2MP_H +#define __A2MP_H + +#include <net/bluetooth/l2cap.h> + +enum amp_mgr_state { + READ_LOC_AMP_INFO, + READ_LOC_AMP_ASSOC, + READ_LOC_AMP_ASSOC_FINAL, + WRITE_REMOTE_AMP_ASSOC, +}; + +struct amp_mgr { + struct list_head list; + struct l2cap_conn *l2cap_conn; + struct l2cap_chan *a2mp_chan; + struct l2cap_chan *bredr_chan; + struct kref kref; + __u8 ident; + __u8 handle; + unsigned long state; + unsigned long flags; + + struct list_head amp_ctrls; + struct mutex amp_ctrls_lock; +}; + +struct a2mp_cmd { + __u8 code; + __u8 ident; + __le16 len; + __u8 data[0]; +} __packed; + +/* A2MP command codes */ +#define A2MP_COMMAND_REJ 0x01 +struct a2mp_cmd_rej { + __le16 reason; + __u8 data[0]; +} __packed; + +#define A2MP_DISCOVER_REQ 0x02 +struct a2mp_discov_req { + __le16 mtu; + __le16 ext_feat; +} __packed; + +struct a2mp_cl { + __u8 id; + __u8 type; + __u8 status; +} __packed; + +#define A2MP_DISCOVER_RSP 0x03 +struct a2mp_discov_rsp { + __le16 mtu; + __le16 ext_feat; + struct a2mp_cl cl[0]; +} __packed; + +#define A2MP_CHANGE_NOTIFY 0x04 +#define A2MP_CHANGE_RSP 0x05 + +#define A2MP_GETINFO_REQ 0x06 +struct a2mp_info_req { + __u8 id; +} __packed; + +#define A2MP_GETINFO_RSP 0x07 +struct a2mp_info_rsp { + __u8 id; + __u8 status; + __le32 total_bw; + __le32 max_bw; + __le32 min_latency; + __le16 pal_cap; + __le16 assoc_size; +} __packed; + +#define A2MP_GETAMPASSOC_REQ 0x08 +struct a2mp_amp_assoc_req { + __u8 id; +} __packed; + +#define A2MP_GETAMPASSOC_RSP 0x09 +struct a2mp_amp_assoc_rsp { + __u8 id; + __u8 status; + __u8 amp_assoc[0]; +} __packed; + +#define A2MP_CREATEPHYSLINK_REQ 0x0A +#define A2MP_DISCONNPHYSLINK_REQ 0x0C +struct a2mp_physlink_req { + __u8 local_id; + __u8 remote_id; + __u8 amp_assoc[0]; +} __packed; + +#define A2MP_CREATEPHYSLINK_RSP 0x0B +#define A2MP_DISCONNPHYSLINK_RSP 0x0D +struct a2mp_physlink_rsp { + __u8 local_id; + __u8 remote_id; + __u8 status; +} __packed; + +/* A2MP response status */ +#define A2MP_STATUS_SUCCESS 0x00 +#define A2MP_STATUS_INVALID_CTRL_ID 0x01 +#define A2MP_STATUS_UNABLE_START_LINK_CREATION 0x02 +#define A2MP_STATUS_NO_PHYSICAL_LINK_EXISTS 0x02 +#define A2MP_STATUS_COLLISION_OCCURED 0x03 +#define A2MP_STATUS_DISCONN_REQ_RECVD 0x04 +#define A2MP_STATUS_PHYS_LINK_EXISTS 0x05 +#define A2MP_STATUS_SECURITY_VIOLATION 0x06 + +struct amp_mgr *amp_mgr_get(struct amp_mgr *mgr); +int amp_mgr_put(struct amp_mgr *mgr); +struct l2cap_chan *a2mp_channel_create(struct l2cap_conn *conn, + struct sk_buff *skb); +void a2mp_discover_amp(struct l2cap_chan *chan); +void a2mp_send_getinfo_rsp(struct hci_dev *hdev); +void a2mp_send_getampassoc_rsp(struct hci_dev *hdev, u8 status); +void a2mp_send_create_phy_link_req(struct hci_dev *hdev, u8 status); +void a2mp_send_create_phy_link_rsp(struct hci_dev *hdev, u8 status); + +#endif /* __A2MP_H */ diff --git a/kernel/net/bluetooth/af_bluetooth.c b/kernel/net/bluetooth/af_bluetooth.c new file mode 100644 index 000000000..70f9d945f --- /dev/null +++ b/kernel/net/bluetooth/af_bluetooth.c @@ -0,0 +1,795 @@ +/* + BlueZ - Bluetooth protocol stack for Linux + Copyright (C) 2000-2001 Qualcomm Incorporated + + Written 2000,2001 by Maxim Krasnyansky <maxk@qualcomm.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; + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS + OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT OF THIRD PARTY RIGHTS. + IN NO EVENT SHALL THE COPYRIGHT HOLDER(S) AND AUTHOR(S) BE LIABLE FOR ANY + CLAIM, OR ANY SPECIAL INDIRECT OR CONSEQUENTIAL DAMAGES, OR ANY DAMAGES + WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN + ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF + OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + + ALL LIABILITY, INCLUDING LIABILITY FOR INFRINGEMENT OF ANY PATENTS, + COPYRIGHTS, TRADEMARKS OR OTHER RIGHTS, RELATING TO USE OF THIS + SOFTWARE IS DISCLAIMED. +*/ + +/* Bluetooth address family and sockets. */ + +#include <linux/module.h> +#include <linux/debugfs.h> +#include <asm/ioctls.h> + +#include <net/bluetooth/bluetooth.h> +#include <linux/proc_fs.h> + +#include "selftest.h" + +#define VERSION "2.20" + +/* Bluetooth sockets */ +#define BT_MAX_PROTO 8 +static const struct net_proto_family *bt_proto[BT_MAX_PROTO]; +static DEFINE_RWLOCK(bt_proto_lock); + +static struct lock_class_key bt_lock_key[BT_MAX_PROTO]; +static const char *const bt_key_strings[BT_MAX_PROTO] = { + "sk_lock-AF_BLUETOOTH-BTPROTO_L2CAP", + "sk_lock-AF_BLUETOOTH-BTPROTO_HCI", + "sk_lock-AF_BLUETOOTH-BTPROTO_SCO", + "sk_lock-AF_BLUETOOTH-BTPROTO_RFCOMM", + "sk_lock-AF_BLUETOOTH-BTPROTO_BNEP", + "sk_lock-AF_BLUETOOTH-BTPROTO_CMTP", + "sk_lock-AF_BLUETOOTH-BTPROTO_HIDP", + "sk_lock-AF_BLUETOOTH-BTPROTO_AVDTP", +}; + +static struct lock_class_key bt_slock_key[BT_MAX_PROTO]; +static const char *const bt_slock_key_strings[BT_MAX_PROTO] = { + "slock-AF_BLUETOOTH-BTPROTO_L2CAP", + "slock-AF_BLUETOOTH-BTPROTO_HCI", + "slock-AF_BLUETOOTH-BTPROTO_SCO", + "slock-AF_BLUETOOTH-BTPROTO_RFCOMM", + "slock-AF_BLUETOOTH-BTPROTO_BNEP", + "slock-AF_BLUETOOTH-BTPROTO_CMTP", + "slock-AF_BLUETOOTH-BTPROTO_HIDP", + "slock-AF_BLUETOOTH-BTPROTO_AVDTP", +}; + +void bt_sock_reclassify_lock(struct sock *sk, int proto) +{ + BUG_ON(!sk); + BUG_ON(sock_owned_by_user(sk)); + + sock_lock_init_class_and_name(sk, + bt_slock_key_strings[proto], &bt_slock_key[proto], + bt_key_strings[proto], &bt_lock_key[proto]); +} +EXPORT_SYMBOL(bt_sock_reclassify_lock); + +int bt_sock_register(int proto, const struct net_proto_family *ops) +{ + int err = 0; + + if (proto < 0 || proto >= BT_MAX_PROTO) + return -EINVAL; + + write_lock(&bt_proto_lock); + + if (bt_proto[proto]) + err = -EEXIST; + else + bt_proto[proto] = ops; + + write_unlock(&bt_proto_lock); + + return err; +} +EXPORT_SYMBOL(bt_sock_register); + +void bt_sock_unregister(int proto) +{ + if (proto < 0 || proto >= BT_MAX_PROTO) + return; + + write_lock(&bt_proto_lock); + bt_proto[proto] = NULL; + write_unlock(&bt_proto_lock); +} +EXPORT_SYMBOL(bt_sock_unregister); + +static int bt_sock_create(struct net *net, struct socket *sock, int proto, + int kern) +{ + int err; + + if (net != &init_net) + return -EAFNOSUPPORT; + + if (proto < 0 || proto >= BT_MAX_PROTO) + return -EINVAL; + + if (!bt_proto[proto]) + request_module("bt-proto-%d", proto); + + err = -EPROTONOSUPPORT; + + read_lock(&bt_proto_lock); + + if (bt_proto[proto] && try_module_get(bt_proto[proto]->owner)) { + err = bt_proto[proto]->create(net, sock, proto, kern); + if (!err) + bt_sock_reclassify_lock(sock->sk, proto); + module_put(bt_proto[proto]->owner); + } + + read_unlock(&bt_proto_lock); + + return err; +} + +void bt_sock_link(struct bt_sock_list *l, struct sock *sk) +{ + write_lock(&l->lock); + sk_add_node(sk, &l->head); + write_unlock(&l->lock); +} +EXPORT_SYMBOL(bt_sock_link); + +void bt_sock_unlink(struct bt_sock_list *l, struct sock *sk) +{ + write_lock(&l->lock); + sk_del_node_init(sk); + write_unlock(&l->lock); +} +EXPORT_SYMBOL(bt_sock_unlink); + +void bt_accept_enqueue(struct sock *parent, struct sock *sk) +{ + BT_DBG("parent %p, sk %p", parent, sk); + + sock_hold(sk); + list_add_tail(&bt_sk(sk)->accept_q, &bt_sk(parent)->accept_q); + bt_sk(sk)->parent = parent; + parent->sk_ack_backlog++; +} +EXPORT_SYMBOL(bt_accept_enqueue); + +void bt_accept_unlink(struct sock *sk) +{ + BT_DBG("sk %p state %d", sk, sk->sk_state); + + list_del_init(&bt_sk(sk)->accept_q); + bt_sk(sk)->parent->sk_ack_backlog--; + bt_sk(sk)->parent = NULL; + sock_put(sk); +} +EXPORT_SYMBOL(bt_accept_unlink); + +struct sock *bt_accept_dequeue(struct sock *parent, struct socket *newsock) +{ + struct list_head *p, *n; + struct sock *sk; + + BT_DBG("parent %p", parent); + + list_for_each_safe(p, n, &bt_sk(parent)->accept_q) { + sk = (struct sock *) list_entry(p, struct bt_sock, accept_q); + + lock_sock(sk); + + /* FIXME: Is this check still needed */ + if (sk->sk_state == BT_CLOSED) { + release_sock(sk); + bt_accept_unlink(sk); + continue; + } + + if (sk->sk_state == BT_CONNECTED || !newsock || + test_bit(BT_SK_DEFER_SETUP, &bt_sk(parent)->flags)) { + bt_accept_unlink(sk); + if (newsock) + sock_graft(sk, newsock); + + release_sock(sk); + return sk; + } + + release_sock(sk); + } + + return NULL; +} +EXPORT_SYMBOL(bt_accept_dequeue); + +int bt_sock_recvmsg(struct socket *sock, struct msghdr *msg, size_t len, + int flags) +{ + int noblock = flags & MSG_DONTWAIT; + struct sock *sk = sock->sk; + struct sk_buff *skb; + size_t copied; + int err; + + BT_DBG("sock %p sk %p len %zu", sock, sk, len); + + if (flags & (MSG_OOB)) + return -EOPNOTSUPP; + + skb = skb_recv_datagram(sk, flags, noblock, &err); + if (!skb) { + if (sk->sk_shutdown & RCV_SHUTDOWN) + return 0; + + return err; + } + + copied = skb->len; + if (len < copied) { + msg->msg_flags |= MSG_TRUNC; + copied = len; + } + + skb_reset_transport_header(skb); + err = skb_copy_datagram_msg(skb, 0, msg, copied); + if (err == 0) { + sock_recv_ts_and_drops(msg, sk, skb); + + if (bt_sk(sk)->skb_msg_name) + bt_sk(sk)->skb_msg_name(skb, msg->msg_name, + &msg->msg_namelen); + } + + skb_free_datagram(sk, skb); + + return err ? : copied; +} +EXPORT_SYMBOL(bt_sock_recvmsg); + +static long bt_sock_data_wait(struct sock *sk, long timeo) +{ + DECLARE_WAITQUEUE(wait, current); + + add_wait_queue(sk_sleep(sk), &wait); + for (;;) { + set_current_state(TASK_INTERRUPTIBLE); + + if (!skb_queue_empty(&sk->sk_receive_queue)) + break; + + if (sk->sk_err || (sk->sk_shutdown & RCV_SHUTDOWN)) + break; + + if (signal_pending(current) || !timeo) + break; + + set_bit(SOCK_ASYNC_WAITDATA, &sk->sk_socket->flags); + release_sock(sk); + timeo = schedule_timeout(timeo); + lock_sock(sk); + clear_bit(SOCK_ASYNC_WAITDATA, &sk->sk_socket->flags); + } + + __set_current_state(TASK_RUNNING); + remove_wait_queue(sk_sleep(sk), &wait); + return timeo; +} + +int bt_sock_stream_recvmsg(struct socket *sock, struct msghdr *msg, + size_t size, int flags) +{ + struct sock *sk = sock->sk; + int err = 0; + size_t target, copied = 0; + long timeo; + + if (flags & MSG_OOB) + return -EOPNOTSUPP; + + BT_DBG("sk %p size %zu", sk, size); + + lock_sock(sk); + + target = sock_rcvlowat(sk, flags & MSG_WAITALL, size); + timeo = sock_rcvtimeo(sk, flags & MSG_DONTWAIT); + + do { + struct sk_buff *skb; + int chunk; + + skb = skb_dequeue(&sk->sk_receive_queue); + if (!skb) { + if (copied >= target) + break; + + err = sock_error(sk); + if (err) + break; + if (sk->sk_shutdown & RCV_SHUTDOWN) + break; + + err = -EAGAIN; + if (!timeo) + break; + + timeo = bt_sock_data_wait(sk, timeo); + + if (signal_pending(current)) { + err = sock_intr_errno(timeo); + goto out; + } + continue; + } + + chunk = min_t(unsigned int, skb->len, size); + if (skb_copy_datagram_msg(skb, 0, msg, chunk)) { + skb_queue_head(&sk->sk_receive_queue, skb); + if (!copied) + copied = -EFAULT; + break; + } + copied += chunk; + size -= chunk; + + sock_recv_ts_and_drops(msg, sk, skb); + + if (!(flags & MSG_PEEK)) { + int skb_len = skb_headlen(skb); + + if (chunk <= skb_len) { + __skb_pull(skb, chunk); + } else { + struct sk_buff *frag; + + __skb_pull(skb, skb_len); + chunk -= skb_len; + + skb_walk_frags(skb, frag) { + if (chunk <= frag->len) { + /* Pulling partial data */ + skb->len -= chunk; + skb->data_len -= chunk; + __skb_pull(frag, chunk); + break; + } else if (frag->len) { + /* Pulling all frag data */ + chunk -= frag->len; + skb->len -= frag->len; + skb->data_len -= frag->len; + __skb_pull(frag, frag->len); + } + } + } + + if (skb->len) { + skb_queue_head(&sk->sk_receive_queue, skb); + break; + } + kfree_skb(skb); + + } else { + /* put message back and return */ + skb_queue_head(&sk->sk_receive_queue, skb); + break; + } + } while (size); + +out: + release_sock(sk); + return copied ? : err; +} +EXPORT_SYMBOL(bt_sock_stream_recvmsg); + +static inline unsigned int bt_accept_poll(struct sock *parent) +{ + struct list_head *p, *n; + struct sock *sk; + + list_for_each_safe(p, n, &bt_sk(parent)->accept_q) { + sk = (struct sock *) list_entry(p, struct bt_sock, accept_q); + if (sk->sk_state == BT_CONNECTED || + (test_bit(BT_SK_DEFER_SETUP, &bt_sk(parent)->flags) && + sk->sk_state == BT_CONNECT2)) + return POLLIN | POLLRDNORM; + } + + return 0; +} + +unsigned int bt_sock_poll(struct file *file, struct socket *sock, + poll_table *wait) +{ + struct sock *sk = sock->sk; + unsigned int mask = 0; + + BT_DBG("sock %p, sk %p", sock, sk); + + poll_wait(file, sk_sleep(sk), wait); + + if (sk->sk_state == BT_LISTEN) + return bt_accept_poll(sk); + + if (sk->sk_err || !skb_queue_empty(&sk->sk_error_queue)) + mask |= POLLERR | + (sock_flag(sk, SOCK_SELECT_ERR_QUEUE) ? POLLPRI : 0); + + if (sk->sk_shutdown & RCV_SHUTDOWN) + mask |= POLLRDHUP | POLLIN | POLLRDNORM; + + if (sk->sk_shutdown == SHUTDOWN_MASK) + mask |= POLLHUP; + + if (!skb_queue_empty(&sk->sk_receive_queue)) + mask |= POLLIN | POLLRDNORM; + + if (sk->sk_state == BT_CLOSED) + mask |= POLLHUP; + + if (sk->sk_state == BT_CONNECT || + sk->sk_state == BT_CONNECT2 || + sk->sk_state == BT_CONFIG) + return mask; + + if (!test_bit(BT_SK_SUSPEND, &bt_sk(sk)->flags) && sock_writeable(sk)) + mask |= POLLOUT | POLLWRNORM | POLLWRBAND; + else + set_bit(SOCK_ASYNC_NOSPACE, &sk->sk_socket->flags); + + return mask; +} +EXPORT_SYMBOL(bt_sock_poll); + +int bt_sock_ioctl(struct socket *sock, unsigned int cmd, unsigned long arg) +{ + struct sock *sk = sock->sk; + struct sk_buff *skb; + long amount; + int err; + + BT_DBG("sk %p cmd %x arg %lx", sk, cmd, arg); + + switch (cmd) { + case TIOCOUTQ: + if (sk->sk_state == BT_LISTEN) + return -EINVAL; + + amount = sk->sk_sndbuf - sk_wmem_alloc_get(sk); + if (amount < 0) + amount = 0; + err = put_user(amount, (int __user *) arg); + break; + + case TIOCINQ: + if (sk->sk_state == BT_LISTEN) + return -EINVAL; + + lock_sock(sk); + skb = skb_peek(&sk->sk_receive_queue); + amount = skb ? skb->len : 0; + release_sock(sk); + err = put_user(amount, (int __user *) arg); + break; + + case SIOCGSTAMP: + err = sock_get_timestamp(sk, (struct timeval __user *) arg); + break; + + case SIOCGSTAMPNS: + err = sock_get_timestampns(sk, (struct timespec __user *) arg); + break; + + default: + err = -ENOIOCTLCMD; + break; + } + + return err; +} +EXPORT_SYMBOL(bt_sock_ioctl); + +/* This function expects the sk lock to be held when called */ +int bt_sock_wait_state(struct sock *sk, int state, unsigned long timeo) +{ + DECLARE_WAITQUEUE(wait, current); + int err = 0; + + BT_DBG("sk %p", sk); + + add_wait_queue(sk_sleep(sk), &wait); + set_current_state(TASK_INTERRUPTIBLE); + while (sk->sk_state != state) { + if (!timeo) { + err = -EINPROGRESS; + break; + } + + if (signal_pending(current)) { + err = sock_intr_errno(timeo); + break; + } + + release_sock(sk); + timeo = schedule_timeout(timeo); + lock_sock(sk); + set_current_state(TASK_INTERRUPTIBLE); + + err = sock_error(sk); + if (err) + break; + } + __set_current_state(TASK_RUNNING); + remove_wait_queue(sk_sleep(sk), &wait); + return err; +} +EXPORT_SYMBOL(bt_sock_wait_state); + +/* This function expects the sk lock to be held when called */ +int bt_sock_wait_ready(struct sock *sk, unsigned long flags) +{ + DECLARE_WAITQUEUE(wait, current); + unsigned long timeo; + int err = 0; + + BT_DBG("sk %p", sk); + + timeo = sock_sndtimeo(sk, flags & O_NONBLOCK); + + add_wait_queue(sk_sleep(sk), &wait); + set_current_state(TASK_INTERRUPTIBLE); + while (test_bit(BT_SK_SUSPEND, &bt_sk(sk)->flags)) { + if (!timeo) { + err = -EAGAIN; + break; + } + + if (signal_pending(current)) { + err = sock_intr_errno(timeo); + break; + } + + release_sock(sk); + timeo = schedule_timeout(timeo); + lock_sock(sk); + set_current_state(TASK_INTERRUPTIBLE); + + err = sock_error(sk); + if (err) + break; + } + __set_current_state(TASK_RUNNING); + remove_wait_queue(sk_sleep(sk), &wait); + + return err; +} +EXPORT_SYMBOL(bt_sock_wait_ready); + +#ifdef CONFIG_PROC_FS +struct bt_seq_state { + struct bt_sock_list *l; +}; + +static void *bt_seq_start(struct seq_file *seq, loff_t *pos) + __acquires(seq->private->l->lock) +{ + struct bt_seq_state *s = seq->private; + struct bt_sock_list *l = s->l; + + read_lock(&l->lock); + return seq_hlist_start_head(&l->head, *pos); +} + +static void *bt_seq_next(struct seq_file *seq, void *v, loff_t *pos) +{ + struct bt_seq_state *s = seq->private; + struct bt_sock_list *l = s->l; + + return seq_hlist_next(v, &l->head, pos); +} + +static void bt_seq_stop(struct seq_file *seq, void *v) + __releases(seq->private->l->lock) +{ + struct bt_seq_state *s = seq->private; + struct bt_sock_list *l = s->l; + + read_unlock(&l->lock); +} + +static int bt_seq_show(struct seq_file *seq, void *v) +{ + struct bt_seq_state *s = seq->private; + struct bt_sock_list *l = s->l; + + if (v == SEQ_START_TOKEN) { + seq_puts(seq ,"sk RefCnt Rmem Wmem User Inode Parent"); + + if (l->custom_seq_show) { + seq_putc(seq, ' '); + l->custom_seq_show(seq, v); + } + + seq_putc(seq, '\n'); + } else { + struct sock *sk = sk_entry(v); + struct bt_sock *bt = bt_sk(sk); + + seq_printf(seq, + "%pK %-6d %-6u %-6u %-6u %-6lu %-6lu", + sk, + atomic_read(&sk->sk_refcnt), + sk_rmem_alloc_get(sk), + sk_wmem_alloc_get(sk), + from_kuid(seq_user_ns(seq), sock_i_uid(sk)), + sock_i_ino(sk), + bt->parent? sock_i_ino(bt->parent): 0LU); + + if (l->custom_seq_show) { + seq_putc(seq, ' '); + l->custom_seq_show(seq, v); + } + + seq_putc(seq, '\n'); + } + return 0; +} + +static const struct seq_operations bt_seq_ops = { + .start = bt_seq_start, + .next = bt_seq_next, + .stop = bt_seq_stop, + .show = bt_seq_show, +}; + +static int bt_seq_open(struct inode *inode, struct file *file) +{ + struct bt_sock_list *sk_list; + struct bt_seq_state *s; + + sk_list = PDE_DATA(inode); + s = __seq_open_private(file, &bt_seq_ops, + sizeof(struct bt_seq_state)); + if (!s) + return -ENOMEM; + + s->l = sk_list; + return 0; +} + +static const struct file_operations bt_fops = { + .open = bt_seq_open, + .read = seq_read, + .llseek = seq_lseek, + .release = seq_release_private +}; + +int bt_procfs_init(struct net *net, const char *name, + struct bt_sock_list* sk_list, + int (* seq_show)(struct seq_file *, void *)) +{ + sk_list->custom_seq_show = seq_show; + + if (!proc_create_data(name, 0, net->proc_net, &bt_fops, sk_list)) + return -ENOMEM; + return 0; +} + +void bt_procfs_cleanup(struct net *net, const char *name) +{ + remove_proc_entry(name, net->proc_net); +} +#else +int bt_procfs_init(struct net *net, const char *name, + struct bt_sock_list* sk_list, + int (* seq_show)(struct seq_file *, void *)) +{ + return 0; +} + +void bt_procfs_cleanup(struct net *net, const char *name) +{ +} +#endif +EXPORT_SYMBOL(bt_procfs_init); +EXPORT_SYMBOL(bt_procfs_cleanup); + +static struct net_proto_family bt_sock_family_ops = { + .owner = THIS_MODULE, + .family = PF_BLUETOOTH, + .create = bt_sock_create, +}; + +struct dentry *bt_debugfs; +EXPORT_SYMBOL_GPL(bt_debugfs); + +static int __init bt_init(void) +{ + int err; + + sock_skb_cb_check_size(sizeof(struct bt_skb_cb)); + + BT_INFO("Core ver %s", VERSION); + + err = bt_selftest(); + if (err < 0) + return err; + + bt_debugfs = debugfs_create_dir("bluetooth", NULL); + + err = bt_sysfs_init(); + if (err < 0) + return err; + + err = sock_register(&bt_sock_family_ops); + if (err < 0) { + bt_sysfs_cleanup(); + return err; + } + + BT_INFO("HCI device and connection manager initialized"); + + err = hci_sock_init(); + if (err < 0) + goto error; + + err = l2cap_init(); + if (err < 0) + goto sock_err; + + err = sco_init(); + if (err < 0) { + l2cap_exit(); + goto sock_err; + } + + err = mgmt_init(); + if (err < 0) { + sco_exit(); + l2cap_exit(); + goto sock_err; + } + + return 0; + +sock_err: + hci_sock_cleanup(); + +error: + sock_unregister(PF_BLUETOOTH); + bt_sysfs_cleanup(); + + return err; +} + +static void __exit bt_exit(void) +{ + mgmt_exit(); + + sco_exit(); + + l2cap_exit(); + + hci_sock_cleanup(); + + sock_unregister(PF_BLUETOOTH); + + bt_sysfs_cleanup(); + + debugfs_remove_recursive(bt_debugfs); +} + +subsys_initcall(bt_init); +module_exit(bt_exit); + +MODULE_AUTHOR("Marcel Holtmann <marcel@holtmann.org>"); +MODULE_DESCRIPTION("Bluetooth Core ver " VERSION); +MODULE_VERSION(VERSION); +MODULE_LICENSE("GPL"); +MODULE_ALIAS_NETPROTO(PF_BLUETOOTH); diff --git a/kernel/net/bluetooth/amp.c b/kernel/net/bluetooth/amp.c new file mode 100644 index 000000000..ee016f039 --- /dev/null +++ b/kernel/net/bluetooth/amp.c @@ -0,0 +1,474 @@ +/* + Copyright (c) 2011,2012 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 <net/bluetooth/bluetooth.h> +#include <net/bluetooth/hci.h> +#include <net/bluetooth/hci_core.h> +#include <crypto/hash.h> + +#include "a2mp.h" +#include "amp.h" + +/* Remote AMP Controllers interface */ +void amp_ctrl_get(struct amp_ctrl *ctrl) +{ + BT_DBG("ctrl %p orig refcnt %d", ctrl, + atomic_read(&ctrl->kref.refcount)); + + kref_get(&ctrl->kref); +} + +static void amp_ctrl_destroy(struct kref *kref) +{ + struct amp_ctrl *ctrl = container_of(kref, struct amp_ctrl, kref); + + BT_DBG("ctrl %p", ctrl); + + kfree(ctrl->assoc); + kfree(ctrl); +} + +int amp_ctrl_put(struct amp_ctrl *ctrl) +{ + BT_DBG("ctrl %p orig refcnt %d", ctrl, + atomic_read(&ctrl->kref.refcount)); + + return kref_put(&ctrl->kref, &_ctrl_destroy); +} + +struct amp_ctrl *amp_ctrl_add(struct amp_mgr *mgr, u8 id) +{ + struct amp_ctrl *ctrl; + + ctrl = kzalloc(sizeof(*ctrl), GFP_KERNEL); + if (!ctrl) + return NULL; + + kref_init(&ctrl->kref); + ctrl->id = id; + + mutex_lock(&mgr->amp_ctrls_lock); + list_add(&ctrl->list, &mgr->amp_ctrls); + mutex_unlock(&mgr->amp_ctrls_lock); + + BT_DBG("mgr %p ctrl %p", mgr, ctrl); + + return ctrl; +} + +void amp_ctrl_list_flush(struct amp_mgr *mgr) +{ + struct amp_ctrl *ctrl, *n; + + BT_DBG("mgr %p", mgr); + + mutex_lock(&mgr->amp_ctrls_lock); + list_for_each_entry_safe(ctrl, n, &mgr->amp_ctrls, list) { + list_del(&ctrl->list); + amp_ctrl_put(ctrl); + } + mutex_unlock(&mgr->amp_ctrls_lock); +} + +struct amp_ctrl *amp_ctrl_lookup(struct amp_mgr *mgr, u8 id) +{ + struct amp_ctrl *ctrl; + + BT_DBG("mgr %p id %d", mgr, id); + + mutex_lock(&mgr->amp_ctrls_lock); + list_for_each_entry(ctrl, &mgr->amp_ctrls, list) { + if (ctrl->id == id) { + amp_ctrl_get(ctrl); + mutex_unlock(&mgr->amp_ctrls_lock); + return ctrl; + } + } + mutex_unlock(&mgr->amp_ctrls_lock); + + return NULL; +} + +/* Physical Link interface */ +static u8 __next_handle(struct amp_mgr *mgr) +{ + if (++mgr->handle == 0) + mgr->handle = 1; + + return mgr->handle; +} + +struct hci_conn *phylink_add(struct hci_dev *hdev, struct amp_mgr *mgr, + u8 remote_id, bool out) +{ + bdaddr_t *dst = &mgr->l2cap_conn->hcon->dst; + struct hci_conn *hcon; + u8 role = out ? HCI_ROLE_MASTER : HCI_ROLE_SLAVE; + + hcon = hci_conn_add(hdev, AMP_LINK, dst, role); + if (!hcon) + return NULL; + + BT_DBG("hcon %p dst %pMR", hcon, dst); + + hcon->state = BT_CONNECT; + hcon->attempt++; + hcon->handle = __next_handle(mgr); + hcon->remote_id = remote_id; + hcon->amp_mgr = amp_mgr_get(mgr); + + return hcon; +} + +/* AMP crypto key generation interface */ +static int hmac_sha256(u8 *key, u8 ksize, char *plaintext, u8 psize, u8 *output) +{ + struct crypto_shash *tfm; + struct shash_desc *shash; + int ret; + + if (!ksize) + return -EINVAL; + + tfm = crypto_alloc_shash("hmac(sha256)", 0, 0); + if (IS_ERR(tfm)) { + BT_DBG("crypto_alloc_ahash failed: err %ld", PTR_ERR(tfm)); + return PTR_ERR(tfm); + } + + ret = crypto_shash_setkey(tfm, key, ksize); + if (ret) { + BT_DBG("crypto_ahash_setkey failed: err %d", ret); + goto failed; + } + + shash = kzalloc(sizeof(*shash) + crypto_shash_descsize(tfm), + GFP_KERNEL); + if (!shash) { + ret = -ENOMEM; + goto failed; + } + + shash->tfm = tfm; + shash->flags = CRYPTO_TFM_REQ_MAY_SLEEP; + + ret = crypto_shash_digest(shash, plaintext, psize, output); + + kfree(shash); + +failed: + crypto_free_shash(tfm); + return ret; +} + +int phylink_gen_key(struct hci_conn *conn, u8 *data, u8 *len, u8 *type) +{ + struct hci_dev *hdev = conn->hdev; + struct link_key *key; + u8 keybuf[HCI_AMP_LINK_KEY_SIZE]; + u8 gamp_key[HCI_AMP_LINK_KEY_SIZE]; + int err; + + if (!hci_conn_check_link_mode(conn)) + return -EACCES; + + BT_DBG("conn %p key_type %d", conn, conn->key_type); + + /* Legacy key */ + if (conn->key_type < 3) { + BT_ERR("Legacy key type %d", conn->key_type); + return -EACCES; + } + + *type = conn->key_type; + *len = HCI_AMP_LINK_KEY_SIZE; + + key = hci_find_link_key(hdev, &conn->dst); + if (!key) { + BT_DBG("No Link key for conn %p dst %pMR", conn, &conn->dst); + return -EACCES; + } + + /* BR/EDR Link Key concatenated together with itself */ + memcpy(&keybuf[0], key->val, HCI_LINK_KEY_SIZE); + memcpy(&keybuf[HCI_LINK_KEY_SIZE], key->val, HCI_LINK_KEY_SIZE); + + /* Derive Generic AMP Link Key (gamp) */ + err = hmac_sha256(keybuf, HCI_AMP_LINK_KEY_SIZE, "gamp", 4, gamp_key); + if (err) { + BT_ERR("Could not derive Generic AMP Key: err %d", err); + return err; + } + + if (conn->key_type == HCI_LK_DEBUG_COMBINATION) { + BT_DBG("Use Generic AMP Key (gamp)"); + memcpy(data, gamp_key, HCI_AMP_LINK_KEY_SIZE); + return err; + } + + /* Derive Dedicated AMP Link Key: "802b" is 802.11 PAL keyID */ + return hmac_sha256(gamp_key, HCI_AMP_LINK_KEY_SIZE, "802b", 4, data); +} + +void amp_read_loc_assoc_frag(struct hci_dev *hdev, u8 phy_handle) +{ + struct hci_cp_read_local_amp_assoc cp; + struct amp_assoc *loc_assoc = &hdev->loc_assoc; + + BT_DBG("%s handle %d", hdev->name, phy_handle); + + cp.phy_handle = phy_handle; + cp.max_len = cpu_to_le16(hdev->amp_assoc_size); + cp.len_so_far = cpu_to_le16(loc_assoc->offset); + + hci_send_cmd(hdev, HCI_OP_READ_LOCAL_AMP_ASSOC, sizeof(cp), &cp); +} + +void amp_read_loc_assoc(struct hci_dev *hdev, struct amp_mgr *mgr) +{ + struct hci_cp_read_local_amp_assoc cp; + + memset(&hdev->loc_assoc, 0, sizeof(struct amp_assoc)); + memset(&cp, 0, sizeof(cp)); + + cp.max_len = cpu_to_le16(hdev->amp_assoc_size); + + set_bit(READ_LOC_AMP_ASSOC, &mgr->state); + hci_send_cmd(hdev, HCI_OP_READ_LOCAL_AMP_ASSOC, sizeof(cp), &cp); +} + +void amp_read_loc_assoc_final_data(struct hci_dev *hdev, + struct hci_conn *hcon) +{ + struct hci_cp_read_local_amp_assoc cp; + struct amp_mgr *mgr = hcon->amp_mgr; + + cp.phy_handle = hcon->handle; + cp.len_so_far = cpu_to_le16(0); + cp.max_len = cpu_to_le16(hdev->amp_assoc_size); + + set_bit(READ_LOC_AMP_ASSOC_FINAL, &mgr->state); + + /* Read Local AMP Assoc final link information data */ + hci_send_cmd(hdev, HCI_OP_READ_LOCAL_AMP_ASSOC, sizeof(cp), &cp); +} + +/* Write AMP Assoc data fragments, returns true with last fragment written*/ +static bool amp_write_rem_assoc_frag(struct hci_dev *hdev, + struct hci_conn *hcon) +{ + struct hci_cp_write_remote_amp_assoc *cp; + struct amp_mgr *mgr = hcon->amp_mgr; + struct amp_ctrl *ctrl; + u16 frag_len, len; + + ctrl = amp_ctrl_lookup(mgr, hcon->remote_id); + if (!ctrl) + return false; + + if (!ctrl->assoc_rem_len) { + BT_DBG("all fragments are written"); + ctrl->assoc_rem_len = ctrl->assoc_len; + ctrl->assoc_len_so_far = 0; + + amp_ctrl_put(ctrl); + return true; + } + + frag_len = min_t(u16, 248, ctrl->assoc_rem_len); + len = frag_len + sizeof(*cp); + + cp = kzalloc(len, GFP_KERNEL); + if (!cp) { + amp_ctrl_put(ctrl); + return false; + } + + BT_DBG("hcon %p ctrl %p frag_len %u assoc_len %u rem_len %u", + hcon, ctrl, frag_len, ctrl->assoc_len, ctrl->assoc_rem_len); + + cp->phy_handle = hcon->handle; + cp->len_so_far = cpu_to_le16(ctrl->assoc_len_so_far); + cp->rem_len = cpu_to_le16(ctrl->assoc_rem_len); + memcpy(cp->frag, ctrl->assoc, frag_len); + + ctrl->assoc_len_so_far += frag_len; + ctrl->assoc_rem_len -= frag_len; + + amp_ctrl_put(ctrl); + + hci_send_cmd(hdev, HCI_OP_WRITE_REMOTE_AMP_ASSOC, len, cp); + + kfree(cp); + + return false; +} + +void amp_write_rem_assoc_continue(struct hci_dev *hdev, u8 handle) +{ + struct hci_conn *hcon; + + BT_DBG("%s phy handle 0x%2.2x", hdev->name, handle); + + hcon = hci_conn_hash_lookup_handle(hdev, handle); + if (!hcon) + return; + + /* Send A2MP create phylink rsp when all fragments are written */ + if (amp_write_rem_assoc_frag(hdev, hcon)) + a2mp_send_create_phy_link_rsp(hdev, 0); +} + +void amp_write_remote_assoc(struct hci_dev *hdev, u8 handle) +{ + struct hci_conn *hcon; + + BT_DBG("%s phy handle 0x%2.2x", hdev->name, handle); + + hcon = hci_conn_hash_lookup_handle(hdev, handle); + if (!hcon) + return; + + BT_DBG("%s phy handle 0x%2.2x hcon %p", hdev->name, handle, hcon); + + amp_write_rem_assoc_frag(hdev, hcon); +} + +void amp_create_phylink(struct hci_dev *hdev, struct amp_mgr *mgr, + struct hci_conn *hcon) +{ + struct hci_cp_create_phy_link cp; + + cp.phy_handle = hcon->handle; + + BT_DBG("%s hcon %p phy handle 0x%2.2x", hdev->name, hcon, + hcon->handle); + + if (phylink_gen_key(mgr->l2cap_conn->hcon, cp.key, &cp.key_len, + &cp.key_type)) { + BT_DBG("Cannot create link key"); + return; + } + + hci_send_cmd(hdev, HCI_OP_CREATE_PHY_LINK, sizeof(cp), &cp); +} + +void amp_accept_phylink(struct hci_dev *hdev, struct amp_mgr *mgr, + struct hci_conn *hcon) +{ + struct hci_cp_accept_phy_link cp; + + cp.phy_handle = hcon->handle; + + BT_DBG("%s hcon %p phy handle 0x%2.2x", hdev->name, hcon, + hcon->handle); + + if (phylink_gen_key(mgr->l2cap_conn->hcon, cp.key, &cp.key_len, + &cp.key_type)) { + BT_DBG("Cannot create link key"); + return; + } + + hci_send_cmd(hdev, HCI_OP_ACCEPT_PHY_LINK, sizeof(cp), &cp); +} + +void amp_physical_cfm(struct hci_conn *bredr_hcon, struct hci_conn *hs_hcon) +{ + struct hci_dev *bredr_hdev = hci_dev_hold(bredr_hcon->hdev); + struct amp_mgr *mgr = hs_hcon->amp_mgr; + struct l2cap_chan *bredr_chan; + + BT_DBG("bredr_hcon %p hs_hcon %p mgr %p", bredr_hcon, hs_hcon, mgr); + + if (!bredr_hdev || !mgr || !mgr->bredr_chan) + return; + + bredr_chan = mgr->bredr_chan; + + l2cap_chan_lock(bredr_chan); + + set_bit(FLAG_EFS_ENABLE, &bredr_chan->flags); + bredr_chan->remote_amp_id = hs_hcon->remote_id; + bredr_chan->local_amp_id = hs_hcon->hdev->id; + bredr_chan->hs_hcon = hs_hcon; + bredr_chan->conn->mtu = hs_hcon->hdev->block_mtu; + + __l2cap_physical_cfm(bredr_chan, 0); + + l2cap_chan_unlock(bredr_chan); + + hci_dev_put(bredr_hdev); +} + +void amp_create_logical_link(struct l2cap_chan *chan) +{ + struct hci_conn *hs_hcon = chan->hs_hcon; + struct hci_cp_create_accept_logical_link cp; + struct hci_dev *hdev; + + BT_DBG("chan %p hs_hcon %p dst %pMR", chan, hs_hcon, + &chan->conn->hcon->dst); + + if (!hs_hcon) + return; + + hdev = hci_dev_hold(chan->hs_hcon->hdev); + if (!hdev) + return; + + cp.phy_handle = hs_hcon->handle; + + cp.tx_flow_spec.id = chan->local_id; + cp.tx_flow_spec.stype = chan->local_stype; + cp.tx_flow_spec.msdu = cpu_to_le16(chan->local_msdu); + cp.tx_flow_spec.sdu_itime = cpu_to_le32(chan->local_sdu_itime); + cp.tx_flow_spec.acc_lat = cpu_to_le32(chan->local_acc_lat); + cp.tx_flow_spec.flush_to = cpu_to_le32(chan->local_flush_to); + + cp.rx_flow_spec.id = chan->remote_id; + cp.rx_flow_spec.stype = chan->remote_stype; + cp.rx_flow_spec.msdu = cpu_to_le16(chan->remote_msdu); + cp.rx_flow_spec.sdu_itime = cpu_to_le32(chan->remote_sdu_itime); + cp.rx_flow_spec.acc_lat = cpu_to_le32(chan->remote_acc_lat); + cp.rx_flow_spec.flush_to = cpu_to_le32(chan->remote_flush_to); + + if (hs_hcon->out) + hci_send_cmd(hdev, HCI_OP_CREATE_LOGICAL_LINK, sizeof(cp), + &cp); + else + hci_send_cmd(hdev, HCI_OP_ACCEPT_LOGICAL_LINK, sizeof(cp), + &cp); + + hci_dev_put(hdev); +} + +void amp_disconnect_logical_link(struct hci_chan *hchan) +{ + struct hci_conn *hcon = hchan->conn; + struct hci_cp_disconn_logical_link cp; + + if (hcon->state != BT_CONNECTED) { + BT_DBG("hchan %p not connected", hchan); + return; + } + + cp.log_handle = cpu_to_le16(hchan->handle); + hci_send_cmd(hcon->hdev, HCI_OP_DISCONN_LOGICAL_LINK, sizeof(cp), &cp); +} + +void amp_destroy_logical_link(struct hci_chan *hchan, u8 reason) +{ + BT_DBG("hchan %p", hchan); + + hci_chan_del(hchan); +} diff --git a/kernel/net/bluetooth/amp.h b/kernel/net/bluetooth/amp.h new file mode 100644 index 000000000..7ea3db77b --- /dev/null +++ b/kernel/net/bluetooth/amp.h @@ -0,0 +1,54 @@ +/* + Copyright (c) 2011,2012 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 __AMP_H +#define __AMP_H + +struct amp_ctrl { + struct list_head list; + struct kref kref; + __u8 id; + __u16 assoc_len_so_far; + __u16 assoc_rem_len; + __u16 assoc_len; + __u8 *assoc; +}; + +int amp_ctrl_put(struct amp_ctrl *ctrl); +void amp_ctrl_get(struct amp_ctrl *ctrl); +struct amp_ctrl *amp_ctrl_add(struct amp_mgr *mgr, u8 id); +struct amp_ctrl *amp_ctrl_lookup(struct amp_mgr *mgr, u8 id); +void amp_ctrl_list_flush(struct amp_mgr *mgr); + +struct hci_conn *phylink_add(struct hci_dev *hdev, struct amp_mgr *mgr, + u8 remote_id, bool out); + +int phylink_gen_key(struct hci_conn *hcon, u8 *data, u8 *len, u8 *type); + +void amp_read_loc_info(struct hci_dev *hdev, struct amp_mgr *mgr); +void amp_read_loc_assoc_frag(struct hci_dev *hdev, u8 phy_handle); +void amp_read_loc_assoc(struct hci_dev *hdev, struct amp_mgr *mgr); +void amp_read_loc_assoc_final_data(struct hci_dev *hdev, + struct hci_conn *hcon); +void amp_create_phylink(struct hci_dev *hdev, struct amp_mgr *mgr, + struct hci_conn *hcon); +void amp_accept_phylink(struct hci_dev *hdev, struct amp_mgr *mgr, + struct hci_conn *hcon); +void amp_write_remote_assoc(struct hci_dev *hdev, u8 handle); +void amp_write_rem_assoc_continue(struct hci_dev *hdev, u8 handle); +void amp_physical_cfm(struct hci_conn *bredr_hcon, struct hci_conn *hs_hcon); +void amp_create_logical_link(struct l2cap_chan *chan); +void amp_disconnect_logical_link(struct hci_chan *hchan); +void amp_destroy_logical_link(struct hci_chan *hchan, u8 reason); + +#endif /* __AMP_H */ diff --git a/kernel/net/bluetooth/bnep/Kconfig b/kernel/net/bluetooth/bnep/Kconfig new file mode 100644 index 000000000..9b70317c4 --- /dev/null +++ b/kernel/net/bluetooth/bnep/Kconfig @@ -0,0 +1,24 @@ +config BT_BNEP + tristate "BNEP protocol support" + depends on BT_BREDR + select CRC32 + help + BNEP (Bluetooth Network Encapsulation Protocol) is Ethernet + emulation layer on top of Bluetooth. BNEP is required for + Bluetooth PAN (Personal Area Network). + + Say Y here to compile BNEP support into the kernel or say M to + compile it as module (bnep). + +config BT_BNEP_MC_FILTER + bool "Multicast filter support" + depends on BT_BNEP + help + This option enables the multicast filter support for BNEP. + +config BT_BNEP_PROTO_FILTER + bool "Protocol filter support" + depends on BT_BNEP + help + This option enables the protocol filter support for BNEP. + diff --git a/kernel/net/bluetooth/bnep/Makefile b/kernel/net/bluetooth/bnep/Makefile new file mode 100644 index 000000000..c7821e76c --- /dev/null +++ b/kernel/net/bluetooth/bnep/Makefile @@ -0,0 +1,7 @@ +# +# Makefile for the Linux Bluetooth BNEP layer. +# + +obj-$(CONFIG_BT_BNEP) += bnep.o + +bnep-objs := core.o sock.o netdev.o diff --git a/kernel/net/bluetooth/bnep/bnep.h b/kernel/net/bluetooth/bnep/bnep.h new file mode 100644 index 000000000..40854c99b --- /dev/null +++ b/kernel/net/bluetooth/bnep/bnep.h @@ -0,0 +1,183 @@ +/* + BNEP protocol definition for Linux Bluetooth stack (BlueZ). + Copyright (C) 2002 Maxim Krasnyansky <maxk@qualcomm.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, see <http://www.gnu.org/licenses/>. +*/ + +#ifndef _BNEP_H +#define _BNEP_H + +#include <linux/types.h> +#include <linux/crc32.h> +#include <net/bluetooth/bluetooth.h> + +/* Limits */ +#define BNEP_MAX_PROTO_FILTERS 5 +#define BNEP_MAX_MULTICAST_FILTERS 20 + +/* UUIDs */ +#define BNEP_BASE_UUID 0x0000000000001000800000805F9B34FB +#define BNEP_UUID16 0x02 +#define BNEP_UUID32 0x04 +#define BNEP_UUID128 0x16 + +#define BNEP_SVC_PANU 0x1115 +#define BNEP_SVC_NAP 0x1116 +#define BNEP_SVC_GN 0x1117 + +/* Packet types */ +#define BNEP_GENERAL 0x00 +#define BNEP_CONTROL 0x01 +#define BNEP_COMPRESSED 0x02 +#define BNEP_COMPRESSED_SRC_ONLY 0x03 +#define BNEP_COMPRESSED_DST_ONLY 0x04 + +/* Control types */ +#define BNEP_CMD_NOT_UNDERSTOOD 0x00 +#define BNEP_SETUP_CONN_REQ 0x01 +#define BNEP_SETUP_CONN_RSP 0x02 +#define BNEP_FILTER_NET_TYPE_SET 0x03 +#define BNEP_FILTER_NET_TYPE_RSP 0x04 +#define BNEP_FILTER_MULTI_ADDR_SET 0x05 +#define BNEP_FILTER_MULTI_ADDR_RSP 0x06 + +/* Extension types */ +#define BNEP_EXT_CONTROL 0x00 + +/* Response messages */ +#define BNEP_SUCCESS 0x00 + +#define BNEP_CONN_INVALID_DST 0x01 +#define BNEP_CONN_INVALID_SRC 0x02 +#define BNEP_CONN_INVALID_SVC 0x03 +#define BNEP_CONN_NOT_ALLOWED 0x04 + +#define BNEP_FILTER_UNSUPPORTED_REQ 0x01 +#define BNEP_FILTER_INVALID_RANGE 0x02 +#define BNEP_FILTER_INVALID_MCADDR 0x02 +#define BNEP_FILTER_LIMIT_REACHED 0x03 +#define BNEP_FILTER_DENIED_SECURITY 0x04 + +/* L2CAP settings */ +#define BNEP_MTU 1691 +#define BNEP_PSM 0x0f +#define BNEP_FLUSH_TO 0xffff +#define BNEP_CONNECT_TO 15 +#define BNEP_FILTER_TO 15 + +/* Headers */ +#define BNEP_TYPE_MASK 0x7f +#define BNEP_EXT_HEADER 0x80 + +struct bnep_setup_conn_req { + __u8 type; + __u8 ctrl; + __u8 uuid_size; + __u8 service[0]; +} __packed; + +struct bnep_set_filter_req { + __u8 type; + __u8 ctrl; + __be16 len; + __u8 list[0]; +} __packed; + +struct bnep_control_rsp { + __u8 type; + __u8 ctrl; + __be16 resp; +} __packed; + +struct bnep_ext_hdr { + __u8 type; + __u8 len; + __u8 data[0]; +} __packed; + +/* BNEP ioctl defines */ +#define BNEPCONNADD _IOW('B', 200, int) +#define BNEPCONNDEL _IOW('B', 201, int) +#define BNEPGETCONNLIST _IOR('B', 210, int) +#define BNEPGETCONNINFO _IOR('B', 211, int) +#define BNEPGETSUPPFEAT _IOR('B', 212, int) + +#define BNEP_SETUP_RESPONSE 0 +#define BNEP_SETUP_RSP_SENT 10 + +struct bnep_connadd_req { + int sock; /* Connected socket */ + __u32 flags; + __u16 role; + char device[16]; /* Name of the Ethernet device */ +}; + +struct bnep_conndel_req { + __u32 flags; + __u8 dst[ETH_ALEN]; +}; + +struct bnep_conninfo { + __u32 flags; + __u16 role; + __u16 state; + __u8 dst[ETH_ALEN]; + char device[16]; +}; + +struct bnep_connlist_req { + __u32 cnum; + struct bnep_conninfo __user *ci; +}; + +struct bnep_proto_filter { + __u16 start; + __u16 end; +}; + +int bnep_add_connection(struct bnep_connadd_req *req, struct socket *sock); +int bnep_del_connection(struct bnep_conndel_req *req); +int bnep_get_connlist(struct bnep_connlist_req *req); +int bnep_get_conninfo(struct bnep_conninfo *ci); + +/* BNEP sessions */ +struct bnep_session { + struct list_head list; + + unsigned int role; + unsigned long state; + unsigned long flags; + atomic_t terminate; + struct task_struct *task; + + struct ethhdr eh; + struct msghdr msg; + + struct bnep_proto_filter proto_filter[BNEP_MAX_PROTO_FILTERS]; + unsigned long long mc_filter; + + struct socket *sock; + struct net_device *dev; +}; + +void bnep_net_setup(struct net_device *dev); +int bnep_sock_init(void); +void bnep_sock_cleanup(void); + +static inline int bnep_mc_hash(__u8 *addr) +{ + return crc32_be(~0, addr, ETH_ALEN) >> 26; +} + +#endif diff --git a/kernel/net/bluetooth/bnep/core.c b/kernel/net/bluetooth/bnep/core.c new file mode 100644 index 000000000..1641367e5 --- /dev/null +++ b/kernel/net/bluetooth/bnep/core.c @@ -0,0 +1,769 @@ +/* + BNEP implementation for Linux Bluetooth stack (BlueZ). + Copyright (C) 2001-2002 Inventel Systemes + Written 2001-2002 by + Clément Moreau <clement.moreau@inventel.fr> + David Libault <david.libault@inventel.fr> + + Copyright (C) 2002 Maxim Krasnyansky <maxk@qualcomm.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; + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS + OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT OF THIRD PARTY RIGHTS. + IN NO EVENT SHALL THE COPYRIGHT HOLDER(S) AND AUTHOR(S) BE LIABLE FOR ANY + CLAIM, OR ANY SPECIAL INDIRECT OR CONSEQUENTIAL DAMAGES, OR ANY DAMAGES + WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN + ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF + OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + + ALL LIABILITY, INCLUDING LIABILITY FOR INFRINGEMENT OF ANY PATENTS, + COPYRIGHTS, TRADEMARKS OR OTHER RIGHTS, RELATING TO USE OF THIS + SOFTWARE IS DISCLAIMED. +*/ + +#include <linux/module.h> +#include <linux/kthread.h> +#include <linux/file.h> +#include <linux/etherdevice.h> +#include <asm/unaligned.h> + +#include <net/bluetooth/bluetooth.h> +#include <net/bluetooth/l2cap.h> +#include <net/bluetooth/hci_core.h> + +#include "bnep.h" + +#define VERSION "1.3" + +static bool compress_src = true; +static bool compress_dst = true; + +static LIST_HEAD(bnep_session_list); +static DECLARE_RWSEM(bnep_session_sem); + +static struct bnep_session *__bnep_get_session(u8 *dst) +{ + struct bnep_session *s; + + BT_DBG(""); + + list_for_each_entry(s, &bnep_session_list, list) + if (ether_addr_equal(dst, s->eh.h_source)) + return s; + + return NULL; +} + +static void __bnep_link_session(struct bnep_session *s) +{ + list_add(&s->list, &bnep_session_list); +} + +static void __bnep_unlink_session(struct bnep_session *s) +{ + list_del(&s->list); +} + +static int bnep_send(struct bnep_session *s, void *data, size_t len) +{ + struct socket *sock = s->sock; + struct kvec iv = { data, len }; + + return kernel_sendmsg(sock, &s->msg, &iv, 1, len); +} + +static int bnep_send_rsp(struct bnep_session *s, u8 ctrl, u16 resp) +{ + struct bnep_control_rsp rsp; + rsp.type = BNEP_CONTROL; + rsp.ctrl = ctrl; + rsp.resp = htons(resp); + return bnep_send(s, &rsp, sizeof(rsp)); +} + +#ifdef CONFIG_BT_BNEP_PROTO_FILTER +static inline void bnep_set_default_proto_filter(struct bnep_session *s) +{ + /* (IPv4, ARP) */ + s->proto_filter[0].start = ETH_P_IP; + s->proto_filter[0].end = ETH_P_ARP; + /* (RARP, AppleTalk) */ + s->proto_filter[1].start = ETH_P_RARP; + s->proto_filter[1].end = ETH_P_AARP; + /* (IPX, IPv6) */ + s->proto_filter[2].start = ETH_P_IPX; + s->proto_filter[2].end = ETH_P_IPV6; +} +#endif + +static int bnep_ctrl_set_netfilter(struct bnep_session *s, __be16 *data, int len) +{ + int n; + + if (len < 2) + return -EILSEQ; + + n = get_unaligned_be16(data); + data++; + len -= 2; + + if (len < n) + return -EILSEQ; + + BT_DBG("filter len %d", n); + +#ifdef CONFIG_BT_BNEP_PROTO_FILTER + n /= 4; + if (n <= BNEP_MAX_PROTO_FILTERS) { + struct bnep_proto_filter *f = s->proto_filter; + int i; + + for (i = 0; i < n; i++) { + f[i].start = get_unaligned_be16(data++); + f[i].end = get_unaligned_be16(data++); + + BT_DBG("proto filter start %d end %d", + f[i].start, f[i].end); + } + + if (i < BNEP_MAX_PROTO_FILTERS) + memset(f + i, 0, sizeof(*f)); + + if (n == 0) + bnep_set_default_proto_filter(s); + + bnep_send_rsp(s, BNEP_FILTER_NET_TYPE_RSP, BNEP_SUCCESS); + } else { + bnep_send_rsp(s, BNEP_FILTER_NET_TYPE_RSP, BNEP_FILTER_LIMIT_REACHED); + } +#else + bnep_send_rsp(s, BNEP_FILTER_NET_TYPE_RSP, BNEP_FILTER_UNSUPPORTED_REQ); +#endif + return 0; +} + +static int bnep_ctrl_set_mcfilter(struct bnep_session *s, u8 *data, int len) +{ + int n; + + if (len < 2) + return -EILSEQ; + + n = get_unaligned_be16(data); + data += 2; + len -= 2; + + if (len < n) + return -EILSEQ; + + BT_DBG("filter len %d", n); + +#ifdef CONFIG_BT_BNEP_MC_FILTER + n /= (ETH_ALEN * 2); + + if (n > 0) { + int i; + + s->mc_filter = 0; + + /* Always send broadcast */ + set_bit(bnep_mc_hash(s->dev->broadcast), (ulong *) &s->mc_filter); + + /* Add address ranges to the multicast hash */ + for (; n > 0; n--) { + u8 a1[6], *a2; + + memcpy(a1, data, ETH_ALEN); + data += ETH_ALEN; + a2 = data; + data += ETH_ALEN; + + BT_DBG("mc filter %pMR -> %pMR", a1, a2); + + /* Iterate from a1 to a2 */ + set_bit(bnep_mc_hash(a1), (ulong *) &s->mc_filter); + while (memcmp(a1, a2, 6) < 0 && s->mc_filter != ~0LL) { + /* Increment a1 */ + i = 5; + while (i >= 0 && ++a1[i--] == 0) + ; + + set_bit(bnep_mc_hash(a1), (ulong *) &s->mc_filter); + } + } + } + + BT_DBG("mc filter hash 0x%llx", s->mc_filter); + + bnep_send_rsp(s, BNEP_FILTER_MULTI_ADDR_RSP, BNEP_SUCCESS); +#else + bnep_send_rsp(s, BNEP_FILTER_MULTI_ADDR_RSP, BNEP_FILTER_UNSUPPORTED_REQ); +#endif + return 0; +} + +static int bnep_rx_control(struct bnep_session *s, void *data, int len) +{ + u8 cmd = *(u8 *)data; + int err = 0; + + data++; + len--; + + switch (cmd) { + case BNEP_CMD_NOT_UNDERSTOOD: + case BNEP_SETUP_CONN_RSP: + case BNEP_FILTER_NET_TYPE_RSP: + case BNEP_FILTER_MULTI_ADDR_RSP: + /* Ignore these for now */ + break; + + case BNEP_FILTER_NET_TYPE_SET: + err = bnep_ctrl_set_netfilter(s, data, len); + break; + + case BNEP_FILTER_MULTI_ADDR_SET: + err = bnep_ctrl_set_mcfilter(s, data, len); + break; + + case BNEP_SETUP_CONN_REQ: + /* Successful response should be sent only once */ + if (test_bit(BNEP_SETUP_RESPONSE, &s->flags) && + !test_and_set_bit(BNEP_SETUP_RSP_SENT, &s->flags)) + err = bnep_send_rsp(s, BNEP_SETUP_CONN_RSP, + BNEP_SUCCESS); + else + err = bnep_send_rsp(s, BNEP_SETUP_CONN_RSP, + BNEP_CONN_NOT_ALLOWED); + break; + + default: { + u8 pkt[3]; + pkt[0] = BNEP_CONTROL; + pkt[1] = BNEP_CMD_NOT_UNDERSTOOD; + pkt[2] = cmd; + err = bnep_send(s, pkt, sizeof(pkt)); + } + break; + } + + return err; +} + +static int bnep_rx_extension(struct bnep_session *s, struct sk_buff *skb) +{ + struct bnep_ext_hdr *h; + int err = 0; + + do { + h = (void *) skb->data; + if (!skb_pull(skb, sizeof(*h))) { + err = -EILSEQ; + break; + } + + BT_DBG("type 0x%x len %d", h->type, h->len); + + switch (h->type & BNEP_TYPE_MASK) { + case BNEP_EXT_CONTROL: + bnep_rx_control(s, skb->data, skb->len); + break; + + default: + /* Unknown extension, skip it. */ + break; + } + + if (!skb_pull(skb, h->len)) { + err = -EILSEQ; + break; + } + } while (!err && (h->type & BNEP_EXT_HEADER)); + + return err; +} + +static u8 __bnep_rx_hlen[] = { + ETH_HLEN, /* BNEP_GENERAL */ + 0, /* BNEP_CONTROL */ + 2, /* BNEP_COMPRESSED */ + ETH_ALEN + 2, /* BNEP_COMPRESSED_SRC_ONLY */ + ETH_ALEN + 2 /* BNEP_COMPRESSED_DST_ONLY */ +}; + +static int bnep_rx_frame(struct bnep_session *s, struct sk_buff *skb) +{ + struct net_device *dev = s->dev; + struct sk_buff *nskb; + u8 type, ctrl_type; + + dev->stats.rx_bytes += skb->len; + + type = *(u8 *) skb->data; + skb_pull(skb, 1); + ctrl_type = *(u8 *)skb->data; + + if ((type & BNEP_TYPE_MASK) >= sizeof(__bnep_rx_hlen)) + goto badframe; + + if ((type & BNEP_TYPE_MASK) == BNEP_CONTROL) { + if (bnep_rx_control(s, skb->data, skb->len) < 0) { + dev->stats.tx_errors++; + kfree_skb(skb); + return 0; + } + + if (!(type & BNEP_EXT_HEADER)) { + kfree_skb(skb); + return 0; + } + + /* Verify and pull ctrl message since it's already processed */ + switch (ctrl_type) { + case BNEP_SETUP_CONN_REQ: + /* Pull: ctrl type (1 b), len (1 b), data (len bytes) */ + if (!skb_pull(skb, 2 + *(u8 *)(skb->data + 1) * 2)) + goto badframe; + break; + case BNEP_FILTER_MULTI_ADDR_SET: + case BNEP_FILTER_NET_TYPE_SET: + /* Pull: ctrl type (1 b), len (2 b), data (len bytes) */ + if (!skb_pull(skb, 3 + *(u16 *)(skb->data + 1) * 2)) + goto badframe; + break; + default: + kfree_skb(skb); + return 0; + } + } else { + skb_reset_mac_header(skb); + + /* Verify and pull out header */ + if (!skb_pull(skb, __bnep_rx_hlen[type & BNEP_TYPE_MASK])) + goto badframe; + + s->eh.h_proto = get_unaligned((__be16 *) (skb->data - 2)); + } + + if (type & BNEP_EXT_HEADER) { + if (bnep_rx_extension(s, skb) < 0) + goto badframe; + } + + /* Strip 802.1p header */ + if (ntohs(s->eh.h_proto) == ETH_P_8021Q) { + if (!skb_pull(skb, 4)) + goto badframe; + s->eh.h_proto = get_unaligned((__be16 *) (skb->data - 2)); + } + + /* We have to alloc new skb and copy data here :(. Because original skb + * may not be modified and because of the alignment requirements. */ + nskb = alloc_skb(2 + ETH_HLEN + skb->len, GFP_KERNEL); + if (!nskb) { + dev->stats.rx_dropped++; + kfree_skb(skb); + return -ENOMEM; + } + skb_reserve(nskb, 2); + + /* Decompress header and construct ether frame */ + switch (type & BNEP_TYPE_MASK) { + case BNEP_COMPRESSED: + memcpy(__skb_put(nskb, ETH_HLEN), &s->eh, ETH_HLEN); + break; + + case BNEP_COMPRESSED_SRC_ONLY: + memcpy(__skb_put(nskb, ETH_ALEN), s->eh.h_dest, ETH_ALEN); + memcpy(__skb_put(nskb, ETH_ALEN), skb_mac_header(skb), ETH_ALEN); + put_unaligned(s->eh.h_proto, (__be16 *) __skb_put(nskb, 2)); + break; + + case BNEP_COMPRESSED_DST_ONLY: + memcpy(__skb_put(nskb, ETH_ALEN), skb_mac_header(skb), + ETH_ALEN); + memcpy(__skb_put(nskb, ETH_ALEN + 2), s->eh.h_source, + ETH_ALEN + 2); + break; + + case BNEP_GENERAL: + memcpy(__skb_put(nskb, ETH_ALEN * 2), skb_mac_header(skb), + ETH_ALEN * 2); + put_unaligned(s->eh.h_proto, (__be16 *) __skb_put(nskb, 2)); + break; + } + + skb_copy_from_linear_data(skb, __skb_put(nskb, skb->len), skb->len); + kfree_skb(skb); + + dev->stats.rx_packets++; + nskb->ip_summed = CHECKSUM_NONE; + nskb->protocol = eth_type_trans(nskb, dev); + netif_rx_ni(nskb); + return 0; + +badframe: + dev->stats.rx_errors++; + kfree_skb(skb); + return 0; +} + +static u8 __bnep_tx_types[] = { + BNEP_GENERAL, + BNEP_COMPRESSED_SRC_ONLY, + BNEP_COMPRESSED_DST_ONLY, + BNEP_COMPRESSED +}; + +static int bnep_tx_frame(struct bnep_session *s, struct sk_buff *skb) +{ + struct ethhdr *eh = (void *) skb->data; + struct socket *sock = s->sock; + struct kvec iv[3]; + int len = 0, il = 0; + u8 type = 0; + + BT_DBG("skb %p dev %p type %d", skb, skb->dev, skb->pkt_type); + + if (!skb->dev) { + /* Control frame sent by us */ + goto send; + } + + iv[il++] = (struct kvec) { &type, 1 }; + len++; + + if (compress_src && ether_addr_equal(eh->h_dest, s->eh.h_source)) + type |= 0x01; + + if (compress_dst && ether_addr_equal(eh->h_source, s->eh.h_dest)) + type |= 0x02; + + if (type) + skb_pull(skb, ETH_ALEN * 2); + + type = __bnep_tx_types[type]; + switch (type) { + case BNEP_COMPRESSED_SRC_ONLY: + iv[il++] = (struct kvec) { eh->h_source, ETH_ALEN }; + len += ETH_ALEN; + break; + + case BNEP_COMPRESSED_DST_ONLY: + iv[il++] = (struct kvec) { eh->h_dest, ETH_ALEN }; + len += ETH_ALEN; + break; + } + +send: + iv[il++] = (struct kvec) { skb->data, skb->len }; + len += skb->len; + + /* FIXME: linearize skb */ + { + len = kernel_sendmsg(sock, &s->msg, iv, il, len); + } + kfree_skb(skb); + + if (len > 0) { + s->dev->stats.tx_bytes += len; + s->dev->stats.tx_packets++; + return 0; + } + + return len; +} + +static int bnep_session(void *arg) +{ + struct bnep_session *s = arg; + struct net_device *dev = s->dev; + struct sock *sk = s->sock->sk; + struct sk_buff *skb; + wait_queue_t wait; + + BT_DBG(""); + + set_user_nice(current, -15); + + init_waitqueue_entry(&wait, current); + add_wait_queue(sk_sleep(sk), &wait); + while (1) { + set_current_state(TASK_INTERRUPTIBLE); + + if (atomic_read(&s->terminate)) + break; + /* RX */ + while ((skb = skb_dequeue(&sk->sk_receive_queue))) { + skb_orphan(skb); + if (!skb_linearize(skb)) + bnep_rx_frame(s, skb); + else + kfree_skb(skb); + } + + if (sk->sk_state != BT_CONNECTED) + break; + + /* TX */ + while ((skb = skb_dequeue(&sk->sk_write_queue))) + if (bnep_tx_frame(s, skb)) + break; + netif_wake_queue(dev); + + schedule(); + } + __set_current_state(TASK_RUNNING); + remove_wait_queue(sk_sleep(sk), &wait); + + /* Cleanup session */ + down_write(&bnep_session_sem); + + /* Delete network device */ + unregister_netdev(dev); + + /* Wakeup user-space polling for socket errors */ + s->sock->sk->sk_err = EUNATCH; + + wake_up_interruptible(sk_sleep(s->sock->sk)); + + /* Release the socket */ + fput(s->sock->file); + + __bnep_unlink_session(s); + + up_write(&bnep_session_sem); + free_netdev(dev); + module_put_and_exit(0); + return 0; +} + +static struct device *bnep_get_device(struct bnep_session *session) +{ + struct l2cap_conn *conn = l2cap_pi(session->sock->sk)->chan->conn; + + if (!conn || !conn->hcon) + return NULL; + + return &conn->hcon->dev; +} + +static struct device_type bnep_type = { + .name = "bluetooth", +}; + +int bnep_add_connection(struct bnep_connadd_req *req, struct socket *sock) +{ + u32 valid_flags = BIT(BNEP_SETUP_RESPONSE); + struct net_device *dev; + struct bnep_session *s, *ss; + u8 dst[ETH_ALEN], src[ETH_ALEN]; + int err; + + BT_DBG(""); + + if (!l2cap_is_socket(sock)) + return -EBADFD; + + if (req->flags & ~valid_flags) + return -EINVAL; + + baswap((void *) dst, &l2cap_pi(sock->sk)->chan->dst); + baswap((void *) src, &l2cap_pi(sock->sk)->chan->src); + + /* session struct allocated as private part of net_device */ + dev = alloc_netdev(sizeof(struct bnep_session), + (*req->device) ? req->device : "bnep%d", + NET_NAME_UNKNOWN, + bnep_net_setup); + if (!dev) + return -ENOMEM; + + down_write(&bnep_session_sem); + + ss = __bnep_get_session(dst); + if (ss && ss->state == BT_CONNECTED) { + err = -EEXIST; + goto failed; + } + + s = netdev_priv(dev); + + /* This is rx header therefore addresses are swapped. + * ie. eh.h_dest is our local address. */ + memcpy(s->eh.h_dest, &src, ETH_ALEN); + memcpy(s->eh.h_source, &dst, ETH_ALEN); + memcpy(dev->dev_addr, s->eh.h_dest, ETH_ALEN); + + s->dev = dev; + s->sock = sock; + s->role = req->role; + s->state = BT_CONNECTED; + s->flags = req->flags; + + s->msg.msg_flags = MSG_NOSIGNAL; + +#ifdef CONFIG_BT_BNEP_MC_FILTER + /* Set default mc filter */ + set_bit(bnep_mc_hash(dev->broadcast), (ulong *) &s->mc_filter); +#endif + +#ifdef CONFIG_BT_BNEP_PROTO_FILTER + /* Set default protocol filter */ + bnep_set_default_proto_filter(s); +#endif + + SET_NETDEV_DEV(dev, bnep_get_device(s)); + SET_NETDEV_DEVTYPE(dev, &bnep_type); + + err = register_netdev(dev); + if (err) + goto failed; + + __bnep_link_session(s); + + __module_get(THIS_MODULE); + s->task = kthread_run(bnep_session, s, "kbnepd %s", dev->name); + if (IS_ERR(s->task)) { + /* Session thread start failed, gotta cleanup. */ + module_put(THIS_MODULE); + unregister_netdev(dev); + __bnep_unlink_session(s); + err = PTR_ERR(s->task); + goto failed; + } + + up_write(&bnep_session_sem); + strcpy(req->device, dev->name); + return 0; + +failed: + up_write(&bnep_session_sem); + free_netdev(dev); + return err; +} + +int bnep_del_connection(struct bnep_conndel_req *req) +{ + u32 valid_flags = 0; + struct bnep_session *s; + int err = 0; + + BT_DBG(""); + + if (req->flags & ~valid_flags) + return -EINVAL; + + down_read(&bnep_session_sem); + + s = __bnep_get_session(req->dst); + if (s) { + atomic_inc(&s->terminate); + wake_up_process(s->task); + } else + err = -ENOENT; + + up_read(&bnep_session_sem); + return err; +} + +static void __bnep_copy_ci(struct bnep_conninfo *ci, struct bnep_session *s) +{ + u32 valid_flags = BIT(BNEP_SETUP_RESPONSE); + + memset(ci, 0, sizeof(*ci)); + memcpy(ci->dst, s->eh.h_source, ETH_ALEN); + strcpy(ci->device, s->dev->name); + ci->flags = s->flags & valid_flags; + ci->state = s->state; + ci->role = s->role; +} + +int bnep_get_connlist(struct bnep_connlist_req *req) +{ + struct bnep_session *s; + int err = 0, n = 0; + + down_read(&bnep_session_sem); + + list_for_each_entry(s, &bnep_session_list, list) { + struct bnep_conninfo ci; + + __bnep_copy_ci(&ci, s); + + if (copy_to_user(req->ci, &ci, sizeof(ci))) { + err = -EFAULT; + break; + } + + if (++n >= req->cnum) + break; + + req->ci++; + } + req->cnum = n; + + up_read(&bnep_session_sem); + return err; +} + +int bnep_get_conninfo(struct bnep_conninfo *ci) +{ + struct bnep_session *s; + int err = 0; + + down_read(&bnep_session_sem); + + s = __bnep_get_session(ci->dst); + if (s) + __bnep_copy_ci(ci, s); + else + err = -ENOENT; + + up_read(&bnep_session_sem); + return err; +} + +static int __init bnep_init(void) +{ + char flt[50] = ""; + +#ifdef CONFIG_BT_BNEP_PROTO_FILTER + strcat(flt, "protocol "); +#endif + +#ifdef CONFIG_BT_BNEP_MC_FILTER + strcat(flt, "multicast"); +#endif + + BT_INFO("BNEP (Ethernet Emulation) ver %s", VERSION); + if (flt[0]) + BT_INFO("BNEP filters: %s", flt); + + bnep_sock_init(); + return 0; +} + +static void __exit bnep_exit(void) +{ + bnep_sock_cleanup(); +} + +module_init(bnep_init); +module_exit(bnep_exit); + +module_param(compress_src, bool, 0644); +MODULE_PARM_DESC(compress_src, "Compress sources headers"); + +module_param(compress_dst, bool, 0644); +MODULE_PARM_DESC(compress_dst, "Compress destination headers"); + +MODULE_AUTHOR("Marcel Holtmann <marcel@holtmann.org>"); +MODULE_DESCRIPTION("Bluetooth BNEP ver " VERSION); +MODULE_VERSION(VERSION); +MODULE_LICENSE("GPL"); +MODULE_ALIAS("bt-proto-4"); diff --git a/kernel/net/bluetooth/bnep/netdev.c b/kernel/net/bluetooth/bnep/netdev.c new file mode 100644 index 000000000..6ceb5d36a --- /dev/null +++ b/kernel/net/bluetooth/bnep/netdev.c @@ -0,0 +1,229 @@ +/* + BNEP implementation for Linux Bluetooth stack (BlueZ). + Copyright (C) 2001-2002 Inventel Systemes + Written 2001-2002 by + Clément Moreau <clement.moreau@inventel.fr> + David Libault <david.libault@inventel.fr> + + Copyright (C) 2002 Maxim Krasnyansky <maxk@qualcomm.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; + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS + OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT OF THIRD PARTY RIGHTS. + IN NO EVENT SHALL THE COPYRIGHT HOLDER(S) AND AUTHOR(S) BE LIABLE FOR ANY + CLAIM, OR ANY SPECIAL INDIRECT OR CONSEQUENTIAL DAMAGES, OR ANY DAMAGES + WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN + ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF + OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + + ALL LIABILITY, INCLUDING LIABILITY FOR INFRINGEMENT OF ANY PATENTS, + COPYRIGHTS, TRADEMARKS OR OTHER RIGHTS, RELATING TO USE OF THIS + SOFTWARE IS DISCLAIMED. +*/ + +#include <linux/etherdevice.h> + +#include <net/bluetooth/bluetooth.h> +#include <net/bluetooth/hci_core.h> +#include <net/bluetooth/l2cap.h> + +#include "bnep.h" + +#define BNEP_TX_QUEUE_LEN 20 + +static int bnep_net_open(struct net_device *dev) +{ + netif_start_queue(dev); + return 0; +} + +static int bnep_net_close(struct net_device *dev) +{ + netif_stop_queue(dev); + return 0; +} + +static void bnep_net_set_mc_list(struct net_device *dev) +{ +#ifdef CONFIG_BT_BNEP_MC_FILTER + struct bnep_session *s = netdev_priv(dev); + struct sock *sk = s->sock->sk; + struct bnep_set_filter_req *r; + struct sk_buff *skb; + int size; + + BT_DBG("%s mc_count %d", dev->name, netdev_mc_count(dev)); + + size = sizeof(*r) + (BNEP_MAX_MULTICAST_FILTERS + 1) * ETH_ALEN * 2; + skb = alloc_skb(size, GFP_ATOMIC); + if (!skb) { + BT_ERR("%s Multicast list allocation failed", dev->name); + return; + } + + r = (void *) skb->data; + __skb_put(skb, sizeof(*r)); + + r->type = BNEP_CONTROL; + r->ctrl = BNEP_FILTER_MULTI_ADDR_SET; + + if (dev->flags & (IFF_PROMISC | IFF_ALLMULTI)) { + u8 start[ETH_ALEN] = { 0x01 }; + + /* Request all addresses */ + memcpy(__skb_put(skb, ETH_ALEN), start, ETH_ALEN); + memcpy(__skb_put(skb, ETH_ALEN), dev->broadcast, ETH_ALEN); + r->len = htons(ETH_ALEN * 2); + } else { + struct netdev_hw_addr *ha; + int i, len = skb->len; + + if (dev->flags & IFF_BROADCAST) { + memcpy(__skb_put(skb, ETH_ALEN), dev->broadcast, ETH_ALEN); + memcpy(__skb_put(skb, ETH_ALEN), dev->broadcast, ETH_ALEN); + } + + /* FIXME: We should group addresses here. */ + + i = 0; + netdev_for_each_mc_addr(ha, dev) { + if (i == BNEP_MAX_MULTICAST_FILTERS) + break; + memcpy(__skb_put(skb, ETH_ALEN), ha->addr, ETH_ALEN); + memcpy(__skb_put(skb, ETH_ALEN), ha->addr, ETH_ALEN); + + i++; + } + r->len = htons(skb->len - len); + } + + skb_queue_tail(&sk->sk_write_queue, skb); + wake_up_interruptible(sk_sleep(sk)); +#endif +} + +static int bnep_net_set_mac_addr(struct net_device *dev, void *arg) +{ + BT_DBG("%s", dev->name); + return 0; +} + +static void bnep_net_timeout(struct net_device *dev) +{ + BT_DBG("net_timeout"); + netif_wake_queue(dev); +} + +#ifdef CONFIG_BT_BNEP_MC_FILTER +static int bnep_net_mc_filter(struct sk_buff *skb, struct bnep_session *s) +{ + struct ethhdr *eh = (void *) skb->data; + + if ((eh->h_dest[0] & 1) && !test_bit(bnep_mc_hash(eh->h_dest), (ulong *) &s->mc_filter)) + return 1; + return 0; +} +#endif + +#ifdef CONFIG_BT_BNEP_PROTO_FILTER +/* Determine ether protocol. Based on eth_type_trans. */ +static u16 bnep_net_eth_proto(struct sk_buff *skb) +{ + struct ethhdr *eh = (void *) skb->data; + u16 proto = ntohs(eh->h_proto); + + if (proto >= ETH_P_802_3_MIN) + return proto; + + if (get_unaligned((__be16 *) skb->data) == htons(0xFFFF)) + return ETH_P_802_3; + + return ETH_P_802_2; +} + +static int bnep_net_proto_filter(struct sk_buff *skb, struct bnep_session *s) +{ + u16 proto = bnep_net_eth_proto(skb); + struct bnep_proto_filter *f = s->proto_filter; + int i; + + for (i = 0; i < BNEP_MAX_PROTO_FILTERS && f[i].end; i++) { + if (proto >= f[i].start && proto <= f[i].end) + return 0; + } + + BT_DBG("BNEP: filtered skb %p, proto 0x%.4x", skb, proto); + return 1; +} +#endif + +static netdev_tx_t bnep_net_xmit(struct sk_buff *skb, + struct net_device *dev) +{ + struct bnep_session *s = netdev_priv(dev); + struct sock *sk = s->sock->sk; + + BT_DBG("skb %p, dev %p", skb, dev); + +#ifdef CONFIG_BT_BNEP_MC_FILTER + if (bnep_net_mc_filter(skb, s)) { + kfree_skb(skb); + return NETDEV_TX_OK; + } +#endif + +#ifdef CONFIG_BT_BNEP_PROTO_FILTER + if (bnep_net_proto_filter(skb, s)) { + kfree_skb(skb); + return NETDEV_TX_OK; + } +#endif + + /* + * We cannot send L2CAP packets from here as we are potentially in a bh. + * So we have to queue them and wake up session thread which is sleeping + * on the sk_sleep(sk). + */ + dev->trans_start = jiffies; + skb_queue_tail(&sk->sk_write_queue, skb); + wake_up_interruptible(sk_sleep(sk)); + + if (skb_queue_len(&sk->sk_write_queue) >= BNEP_TX_QUEUE_LEN) { + BT_DBG("tx queue is full"); + + /* Stop queuing. + * Session thread will do netif_wake_queue() */ + netif_stop_queue(dev); + } + + return NETDEV_TX_OK; +} + +static const struct net_device_ops bnep_netdev_ops = { + .ndo_open = bnep_net_open, + .ndo_stop = bnep_net_close, + .ndo_start_xmit = bnep_net_xmit, + .ndo_validate_addr = eth_validate_addr, + .ndo_set_rx_mode = bnep_net_set_mc_list, + .ndo_set_mac_address = bnep_net_set_mac_addr, + .ndo_tx_timeout = bnep_net_timeout, + .ndo_change_mtu = eth_change_mtu, + +}; + +void bnep_net_setup(struct net_device *dev) +{ + + eth_broadcast_addr(dev->broadcast); + dev->addr_len = ETH_ALEN; + + ether_setup(dev); + dev->priv_flags &= ~IFF_TX_SKB_SHARING; + dev->netdev_ops = &bnep_netdev_ops; + + dev->watchdog_timeo = HZ * 2; +} diff --git a/kernel/net/bluetooth/bnep/sock.c b/kernel/net/bluetooth/bnep/sock.c new file mode 100644 index 000000000..bde2bdd9e --- /dev/null +++ b/kernel/net/bluetooth/bnep/sock.c @@ -0,0 +1,265 @@ +/* + BNEP implementation for Linux Bluetooth stack (BlueZ). + Copyright (C) 2001-2002 Inventel Systemes + Written 2001-2002 by + David Libault <david.libault@inventel.fr> + + Copyright (C) 2002 Maxim Krasnyansky <maxk@qualcomm.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; + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS + OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT OF THIRD PARTY RIGHTS. + IN NO EVENT SHALL THE COPYRIGHT HOLDER(S) AND AUTHOR(S) BE LIABLE FOR ANY + CLAIM, OR ANY SPECIAL INDIRECT OR CONSEQUENTIAL DAMAGES, OR ANY DAMAGES + WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN + ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF + OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + + ALL LIABILITY, INCLUDING LIABILITY FOR INFRINGEMENT OF ANY PATENTS, + COPYRIGHTS, TRADEMARKS OR OTHER RIGHTS, RELATING TO USE OF THIS + SOFTWARE IS DISCLAIMED. +*/ + +#include <linux/export.h> +#include <linux/file.h> + +#include "bnep.h" + +static struct bt_sock_list bnep_sk_list = { + .lock = __RW_LOCK_UNLOCKED(bnep_sk_list.lock) +}; + +static int bnep_sock_release(struct socket *sock) +{ + struct sock *sk = sock->sk; + + BT_DBG("sock %p sk %p", sock, sk); + + if (!sk) + return 0; + + bt_sock_unlink(&bnep_sk_list, sk); + + sock_orphan(sk); + sock_put(sk); + return 0; +} + +static int bnep_sock_ioctl(struct socket *sock, unsigned int cmd, unsigned long arg) +{ + struct bnep_connlist_req cl; + struct bnep_connadd_req ca; + struct bnep_conndel_req cd; + struct bnep_conninfo ci; + struct socket *nsock; + void __user *argp = (void __user *)arg; + __u32 supp_feat = BIT(BNEP_SETUP_RESPONSE); + int err; + + BT_DBG("cmd %x arg %lx", cmd, arg); + + switch (cmd) { + case BNEPCONNADD: + if (!capable(CAP_NET_ADMIN)) + return -EPERM; + + if (copy_from_user(&ca, argp, sizeof(ca))) + return -EFAULT; + + nsock = sockfd_lookup(ca.sock, &err); + if (!nsock) + return err; + + if (nsock->sk->sk_state != BT_CONNECTED) { + sockfd_put(nsock); + return -EBADFD; + } + ca.device[sizeof(ca.device)-1] = 0; + + err = bnep_add_connection(&ca, nsock); + if (!err) { + if (copy_to_user(argp, &ca, sizeof(ca))) + err = -EFAULT; + } else + sockfd_put(nsock); + + return err; + + case BNEPCONNDEL: + if (!capable(CAP_NET_ADMIN)) + return -EPERM; + + if (copy_from_user(&cd, argp, sizeof(cd))) + return -EFAULT; + + return bnep_del_connection(&cd); + + case BNEPGETCONNLIST: + if (copy_from_user(&cl, argp, sizeof(cl))) + return -EFAULT; + + if (cl.cnum <= 0) + return -EINVAL; + + err = bnep_get_connlist(&cl); + if (!err && copy_to_user(argp, &cl, sizeof(cl))) + return -EFAULT; + + return err; + + case BNEPGETCONNINFO: + if (copy_from_user(&ci, argp, sizeof(ci))) + return -EFAULT; + + err = bnep_get_conninfo(&ci); + if (!err && copy_to_user(argp, &ci, sizeof(ci))) + return -EFAULT; + + return err; + + case BNEPGETSUPPFEAT: + if (copy_to_user(argp, &supp_feat, sizeof(supp_feat))) + return -EFAULT; + + return 0; + + default: + return -EINVAL; + } + + return 0; +} + +#ifdef CONFIG_COMPAT +static int bnep_sock_compat_ioctl(struct socket *sock, unsigned int cmd, unsigned long arg) +{ + if (cmd == BNEPGETCONNLIST) { + struct bnep_connlist_req cl; + u32 uci; + int err; + + if (get_user(cl.cnum, (u32 __user *) arg) || + get_user(uci, (u32 __user *) (arg + 4))) + return -EFAULT; + + cl.ci = compat_ptr(uci); + + if (cl.cnum <= 0) + return -EINVAL; + + err = bnep_get_connlist(&cl); + + if (!err && put_user(cl.cnum, (u32 __user *) arg)) + err = -EFAULT; + + return err; + } + + return bnep_sock_ioctl(sock, cmd, arg); +} +#endif + +static const struct proto_ops bnep_sock_ops = { + .family = PF_BLUETOOTH, + .owner = THIS_MODULE, + .release = bnep_sock_release, + .ioctl = bnep_sock_ioctl, +#ifdef CONFIG_COMPAT + .compat_ioctl = bnep_sock_compat_ioctl, +#endif + .bind = sock_no_bind, + .getname = sock_no_getname, + .sendmsg = sock_no_sendmsg, + .recvmsg = sock_no_recvmsg, + .poll = sock_no_poll, + .listen = sock_no_listen, + .shutdown = sock_no_shutdown, + .setsockopt = sock_no_setsockopt, + .getsockopt = sock_no_getsockopt, + .connect = sock_no_connect, + .socketpair = sock_no_socketpair, + .accept = sock_no_accept, + .mmap = sock_no_mmap +}; + +static struct proto bnep_proto = { + .name = "BNEP", + .owner = THIS_MODULE, + .obj_size = sizeof(struct bt_sock) +}; + +static int bnep_sock_create(struct net *net, struct socket *sock, int protocol, + int kern) +{ + struct sock *sk; + + BT_DBG("sock %p", sock); + + if (sock->type != SOCK_RAW) + return -ESOCKTNOSUPPORT; + + sk = sk_alloc(net, PF_BLUETOOTH, GFP_ATOMIC, &bnep_proto); + if (!sk) + return -ENOMEM; + + sock_init_data(sock, sk); + + sock->ops = &bnep_sock_ops; + + sock->state = SS_UNCONNECTED; + + sock_reset_flag(sk, SOCK_ZAPPED); + + sk->sk_protocol = protocol; + sk->sk_state = BT_OPEN; + + bt_sock_link(&bnep_sk_list, sk); + return 0; +} + +static const struct net_proto_family bnep_sock_family_ops = { + .family = PF_BLUETOOTH, + .owner = THIS_MODULE, + .create = bnep_sock_create +}; + +int __init bnep_sock_init(void) +{ + int err; + + err = proto_register(&bnep_proto, 0); + if (err < 0) + return err; + + err = bt_sock_register(BTPROTO_BNEP, &bnep_sock_family_ops); + if (err < 0) { + BT_ERR("Can't register BNEP socket"); + goto error; + } + + err = bt_procfs_init(&init_net, "bnep", &bnep_sk_list, NULL); + if (err < 0) { + BT_ERR("Failed to create BNEP proc file"); + bt_sock_unregister(BTPROTO_BNEP); + goto error; + } + + BT_INFO("BNEP socket layer initialized"); + + return 0; + +error: + proto_unregister(&bnep_proto); + return err; +} + +void __exit bnep_sock_cleanup(void) +{ + bt_procfs_cleanup(&init_net, "bnep"); + bt_sock_unregister(BTPROTO_BNEP); + proto_unregister(&bnep_proto); +} diff --git a/kernel/net/bluetooth/cmtp/Kconfig b/kernel/net/bluetooth/cmtp/Kconfig new file mode 100644 index 000000000..939da0fbd --- /dev/null +++ b/kernel/net/bluetooth/cmtp/Kconfig @@ -0,0 +1,11 @@ +config BT_CMTP + tristate "CMTP protocol support" + depends on BT_BREDR && ISDN_CAPI + help + CMTP (CAPI Message Transport Protocol) is a transport layer + for CAPI messages. CMTP is required for the Bluetooth Common + ISDN Access Profile. + + Say Y here to compile CMTP support into the kernel or say M to + compile it as module (cmtp). + diff --git a/kernel/net/bluetooth/cmtp/Makefile b/kernel/net/bluetooth/cmtp/Makefile new file mode 100644 index 000000000..890a9a5a6 --- /dev/null +++ b/kernel/net/bluetooth/cmtp/Makefile @@ -0,0 +1,7 @@ +# +# Makefile for the Linux Bluetooth CMTP layer +# + +obj-$(CONFIG_BT_CMTP) += cmtp.o + +cmtp-objs := core.o sock.o capi.o diff --git a/kernel/net/bluetooth/cmtp/capi.c b/kernel/net/bluetooth/cmtp/capi.c new file mode 100644 index 000000000..b0c6c6af7 --- /dev/null +++ b/kernel/net/bluetooth/cmtp/capi.c @@ -0,0 +1,612 @@ +/* + CMTP implementation for Linux Bluetooth stack (BlueZ). + Copyright (C) 2002-2003 Marcel Holtmann <marcel@holtmann.org> + + 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; + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS + OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT OF THIRD PARTY RIGHTS. + IN NO EVENT SHALL THE COPYRIGHT HOLDER(S) AND AUTHOR(S) BE LIABLE FOR ANY + CLAIM, OR ANY SPECIAL INDIRECT OR CONSEQUENTIAL DAMAGES, OR ANY DAMAGES + WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN + ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF + OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + + ALL LIABILITY, INCLUDING LIABILITY FOR INFRINGEMENT OF ANY PATENTS, + COPYRIGHTS, TRADEMARKS OR OTHER RIGHTS, RELATING TO USE OF THIS + SOFTWARE IS DISCLAIMED. +*/ + +#include <linux/export.h> +#include <linux/proc_fs.h> +#include <linux/seq_file.h> +#include <linux/types.h> +#include <linux/errno.h> +#include <linux/kernel.h> +#include <linux/sched.h> +#include <linux/slab.h> +#include <linux/poll.h> +#include <linux/fcntl.h> +#include <linux/skbuff.h> +#include <linux/socket.h> +#include <linux/ioctl.h> +#include <linux/file.h> +#include <linux/wait.h> +#include <linux/kthread.h> +#include <net/sock.h> + +#include <linux/isdn/capilli.h> +#include <linux/isdn/capicmd.h> +#include <linux/isdn/capiutil.h> + +#include "cmtp.h" + +#define CAPI_INTEROPERABILITY 0x20 + +#define CAPI_INTEROPERABILITY_REQ CAPICMD(CAPI_INTEROPERABILITY, CAPI_REQ) +#define CAPI_INTEROPERABILITY_CONF CAPICMD(CAPI_INTEROPERABILITY, CAPI_CONF) +#define CAPI_INTEROPERABILITY_IND CAPICMD(CAPI_INTEROPERABILITY, CAPI_IND) +#define CAPI_INTEROPERABILITY_RESP CAPICMD(CAPI_INTEROPERABILITY, CAPI_RESP) + +#define CAPI_INTEROPERABILITY_REQ_LEN (CAPI_MSG_BASELEN + 2) +#define CAPI_INTEROPERABILITY_CONF_LEN (CAPI_MSG_BASELEN + 4) +#define CAPI_INTEROPERABILITY_IND_LEN (CAPI_MSG_BASELEN + 2) +#define CAPI_INTEROPERABILITY_RESP_LEN (CAPI_MSG_BASELEN + 2) + +#define CAPI_FUNCTION_REGISTER 0 +#define CAPI_FUNCTION_RELEASE 1 +#define CAPI_FUNCTION_GET_PROFILE 2 +#define CAPI_FUNCTION_GET_MANUFACTURER 3 +#define CAPI_FUNCTION_GET_VERSION 4 +#define CAPI_FUNCTION_GET_SERIAL_NUMBER 5 +#define CAPI_FUNCTION_MANUFACTURER 6 +#define CAPI_FUNCTION_LOOPBACK 7 + + +#define CMTP_MSGNUM 1 +#define CMTP_APPLID 2 +#define CMTP_MAPPING 3 + +static struct cmtp_application *cmtp_application_add(struct cmtp_session *session, __u16 appl) +{ + struct cmtp_application *app = kzalloc(sizeof(*app), GFP_KERNEL); + + BT_DBG("session %p application %p appl %d", session, app, appl); + + if (!app) + return NULL; + + app->state = BT_OPEN; + app->appl = appl; + + list_add_tail(&app->list, &session->applications); + + return app; +} + +static void cmtp_application_del(struct cmtp_session *session, struct cmtp_application *app) +{ + BT_DBG("session %p application %p", session, app); + + if (app) { + list_del(&app->list); + kfree(app); + } +} + +static struct cmtp_application *cmtp_application_get(struct cmtp_session *session, int pattern, __u16 value) +{ + struct cmtp_application *app; + struct list_head *p, *n; + + list_for_each_safe(p, n, &session->applications) { + app = list_entry(p, struct cmtp_application, list); + switch (pattern) { + case CMTP_MSGNUM: + if (app->msgnum == value) + return app; + break; + case CMTP_APPLID: + if (app->appl == value) + return app; + break; + case CMTP_MAPPING: + if (app->mapping == value) + return app; + break; + } + } + + return NULL; +} + +static int cmtp_msgnum_get(struct cmtp_session *session) +{ + session->msgnum++; + + if ((session->msgnum & 0xff) > 200) + session->msgnum = CMTP_INITIAL_MSGNUM + 1; + + return session->msgnum; +} + +static void cmtp_send_capimsg(struct cmtp_session *session, struct sk_buff *skb) +{ + struct cmtp_scb *scb = (void *) skb->cb; + + BT_DBG("session %p skb %p len %d", session, skb, skb->len); + + scb->id = -1; + scb->data = (CAPIMSG_COMMAND(skb->data) == CAPI_DATA_B3); + + skb_queue_tail(&session->transmit, skb); + + wake_up_interruptible(sk_sleep(session->sock->sk)); +} + +static void cmtp_send_interopmsg(struct cmtp_session *session, + __u8 subcmd, __u16 appl, __u16 msgnum, + __u16 function, unsigned char *buf, int len) +{ + struct sk_buff *skb; + unsigned char *s; + + BT_DBG("session %p subcmd 0x%02x appl %d msgnum %d", session, subcmd, appl, msgnum); + + skb = alloc_skb(CAPI_MSG_BASELEN + 6 + len, GFP_ATOMIC); + if (!skb) { + BT_ERR("Can't allocate memory for interoperability packet"); + return; + } + + s = skb_put(skb, CAPI_MSG_BASELEN + 6 + len); + + capimsg_setu16(s, 0, CAPI_MSG_BASELEN + 6 + len); + capimsg_setu16(s, 2, appl); + capimsg_setu8 (s, 4, CAPI_INTEROPERABILITY); + capimsg_setu8 (s, 5, subcmd); + capimsg_setu16(s, 6, msgnum); + + /* Interoperability selector (Bluetooth Device Management) */ + capimsg_setu16(s, 8, 0x0001); + + capimsg_setu8 (s, 10, 3 + len); + capimsg_setu16(s, 11, function); + capimsg_setu8 (s, 13, len); + + if (len > 0) + memcpy(s + 14, buf, len); + + cmtp_send_capimsg(session, skb); +} + +static void cmtp_recv_interopmsg(struct cmtp_session *session, struct sk_buff *skb) +{ + struct capi_ctr *ctrl = &session->ctrl; + struct cmtp_application *application; + __u16 appl, msgnum, func, info; + __u32 controller; + + BT_DBG("session %p skb %p len %d", session, skb, skb->len); + + switch (CAPIMSG_SUBCOMMAND(skb->data)) { + case CAPI_CONF: + if (skb->len < CAPI_MSG_BASELEN + 10) + break; + + func = CAPIMSG_U16(skb->data, CAPI_MSG_BASELEN + 5); + info = CAPIMSG_U16(skb->data, CAPI_MSG_BASELEN + 8); + + switch (func) { + case CAPI_FUNCTION_REGISTER: + msgnum = CAPIMSG_MSGID(skb->data); + + application = cmtp_application_get(session, CMTP_MSGNUM, msgnum); + if (application) { + application->state = BT_CONNECTED; + application->msgnum = 0; + application->mapping = CAPIMSG_APPID(skb->data); + wake_up_interruptible(&session->wait); + } + + break; + + case CAPI_FUNCTION_RELEASE: + appl = CAPIMSG_APPID(skb->data); + + application = cmtp_application_get(session, CMTP_MAPPING, appl); + if (application) { + application->state = BT_CLOSED; + application->msgnum = 0; + wake_up_interruptible(&session->wait); + } + + break; + + case CAPI_FUNCTION_GET_PROFILE: + if (skb->len < CAPI_MSG_BASELEN + 11 + sizeof(capi_profile)) + break; + + controller = CAPIMSG_U16(skb->data, CAPI_MSG_BASELEN + 11); + msgnum = CAPIMSG_MSGID(skb->data); + + if (!info && (msgnum == CMTP_INITIAL_MSGNUM)) { + session->ncontroller = controller; + wake_up_interruptible(&session->wait); + break; + } + + if (!info && ctrl) { + memcpy(&ctrl->profile, + skb->data + CAPI_MSG_BASELEN + 11, + sizeof(capi_profile)); + session->state = BT_CONNECTED; + capi_ctr_ready(ctrl); + } + + break; + + case CAPI_FUNCTION_GET_MANUFACTURER: + if (skb->len < CAPI_MSG_BASELEN + 15) + break; + + if (!info && ctrl) { + int len = min_t(uint, CAPI_MANUFACTURER_LEN, + skb->data[CAPI_MSG_BASELEN + 14]); + + memset(ctrl->manu, 0, CAPI_MANUFACTURER_LEN); + strncpy(ctrl->manu, + skb->data + CAPI_MSG_BASELEN + 15, len); + } + + break; + + case CAPI_FUNCTION_GET_VERSION: + if (skb->len < CAPI_MSG_BASELEN + 32) + break; + + if (!info && ctrl) { + ctrl->version.majorversion = CAPIMSG_U32(skb->data, CAPI_MSG_BASELEN + 16); + ctrl->version.minorversion = CAPIMSG_U32(skb->data, CAPI_MSG_BASELEN + 20); + ctrl->version.majormanuversion = CAPIMSG_U32(skb->data, CAPI_MSG_BASELEN + 24); + ctrl->version.minormanuversion = CAPIMSG_U32(skb->data, CAPI_MSG_BASELEN + 28); + } + + break; + + case CAPI_FUNCTION_GET_SERIAL_NUMBER: + if (skb->len < CAPI_MSG_BASELEN + 17) + break; + + if (!info && ctrl) { + int len = min_t(uint, CAPI_SERIAL_LEN, + skb->data[CAPI_MSG_BASELEN + 16]); + + memset(ctrl->serial, 0, CAPI_SERIAL_LEN); + strncpy(ctrl->serial, + skb->data + CAPI_MSG_BASELEN + 17, len); + } + + break; + } + + break; + + case CAPI_IND: + if (skb->len < CAPI_MSG_BASELEN + 6) + break; + + func = CAPIMSG_U16(skb->data, CAPI_MSG_BASELEN + 3); + + if (func == CAPI_FUNCTION_LOOPBACK) { + int len = min_t(uint, skb->len - CAPI_MSG_BASELEN - 6, + skb->data[CAPI_MSG_BASELEN + 5]); + appl = CAPIMSG_APPID(skb->data); + msgnum = CAPIMSG_MSGID(skb->data); + cmtp_send_interopmsg(session, CAPI_RESP, appl, msgnum, func, + skb->data + CAPI_MSG_BASELEN + 6, len); + } + + break; + } + + kfree_skb(skb); +} + +void cmtp_recv_capimsg(struct cmtp_session *session, struct sk_buff *skb) +{ + struct capi_ctr *ctrl = &session->ctrl; + struct cmtp_application *application; + __u16 appl; + __u32 contr; + + BT_DBG("session %p skb %p len %d", session, skb, skb->len); + + if (skb->len < CAPI_MSG_BASELEN) + return; + + if (CAPIMSG_COMMAND(skb->data) == CAPI_INTEROPERABILITY) { + cmtp_recv_interopmsg(session, skb); + return; + } + + if (session->flags & BIT(CMTP_LOOPBACK)) { + kfree_skb(skb); + return; + } + + appl = CAPIMSG_APPID(skb->data); + contr = CAPIMSG_CONTROL(skb->data); + + application = cmtp_application_get(session, CMTP_MAPPING, appl); + if (application) { + appl = application->appl; + CAPIMSG_SETAPPID(skb->data, appl); + } else { + BT_ERR("Can't find application with id %d", appl); + kfree_skb(skb); + return; + } + + if ((contr & 0x7f) == 0x01) { + contr = (contr & 0xffffff80) | session->num; + CAPIMSG_SETCONTROL(skb->data, contr); + } + + capi_ctr_handle_message(ctrl, appl, skb); +} + +static int cmtp_load_firmware(struct capi_ctr *ctrl, capiloaddata *data) +{ + BT_DBG("ctrl %p data %p", ctrl, data); + + return 0; +} + +static void cmtp_reset_ctr(struct capi_ctr *ctrl) +{ + struct cmtp_session *session = ctrl->driverdata; + + BT_DBG("ctrl %p", ctrl); + + capi_ctr_down(ctrl); + + atomic_inc(&session->terminate); + wake_up_process(session->task); +} + +static void cmtp_register_appl(struct capi_ctr *ctrl, __u16 appl, capi_register_params *rp) +{ + DECLARE_WAITQUEUE(wait, current); + struct cmtp_session *session = ctrl->driverdata; + struct cmtp_application *application; + unsigned long timeo = CMTP_INTEROP_TIMEOUT; + unsigned char buf[8]; + int err = 0, nconn, want = rp->level3cnt; + + BT_DBG("ctrl %p appl %d level3cnt %d datablkcnt %d datablklen %d", + ctrl, appl, rp->level3cnt, rp->datablkcnt, rp->datablklen); + + application = cmtp_application_add(session, appl); + if (!application) { + BT_ERR("Can't allocate memory for new application"); + return; + } + + if (want < 0) + nconn = ctrl->profile.nbchannel * -want; + else + nconn = want; + + if (nconn == 0) + nconn = ctrl->profile.nbchannel; + + capimsg_setu16(buf, 0, nconn); + capimsg_setu16(buf, 2, rp->datablkcnt); + capimsg_setu16(buf, 4, rp->datablklen); + + application->state = BT_CONFIG; + application->msgnum = cmtp_msgnum_get(session); + + cmtp_send_interopmsg(session, CAPI_REQ, 0x0000, application->msgnum, + CAPI_FUNCTION_REGISTER, buf, 6); + + add_wait_queue(&session->wait, &wait); + while (1) { + set_current_state(TASK_INTERRUPTIBLE); + + if (!timeo) { + err = -EAGAIN; + break; + } + + if (application->state == BT_CLOSED) { + err = -application->err; + break; + } + + if (application->state == BT_CONNECTED) + break; + + if (signal_pending(current)) { + err = -EINTR; + break; + } + + timeo = schedule_timeout(timeo); + } + set_current_state(TASK_RUNNING); + remove_wait_queue(&session->wait, &wait); + + if (err) { + cmtp_application_del(session, application); + return; + } +} + +static void cmtp_release_appl(struct capi_ctr *ctrl, __u16 appl) +{ + struct cmtp_session *session = ctrl->driverdata; + struct cmtp_application *application; + + BT_DBG("ctrl %p appl %d", ctrl, appl); + + application = cmtp_application_get(session, CMTP_APPLID, appl); + if (!application) { + BT_ERR("Can't find application"); + return; + } + + application->msgnum = cmtp_msgnum_get(session); + + cmtp_send_interopmsg(session, CAPI_REQ, application->mapping, application->msgnum, + CAPI_FUNCTION_RELEASE, NULL, 0); + + wait_event_interruptible_timeout(session->wait, + (application->state == BT_CLOSED), CMTP_INTEROP_TIMEOUT); + + cmtp_application_del(session, application); +} + +static u16 cmtp_send_message(struct capi_ctr *ctrl, struct sk_buff *skb) +{ + struct cmtp_session *session = ctrl->driverdata; + struct cmtp_application *application; + __u16 appl; + __u32 contr; + + BT_DBG("ctrl %p skb %p", ctrl, skb); + + appl = CAPIMSG_APPID(skb->data); + contr = CAPIMSG_CONTROL(skb->data); + + application = cmtp_application_get(session, CMTP_APPLID, appl); + if ((!application) || (application->state != BT_CONNECTED)) { + BT_ERR("Can't find application with id %d", appl); + return CAPI_ILLAPPNR; + } + + CAPIMSG_SETAPPID(skb->data, application->mapping); + + if ((contr & 0x7f) == session->num) { + contr = (contr & 0xffffff80) | 0x01; + CAPIMSG_SETCONTROL(skb->data, contr); + } + + cmtp_send_capimsg(session, skb); + + return CAPI_NOERROR; +} + +static char *cmtp_procinfo(struct capi_ctr *ctrl) +{ + return "CAPI Message Transport Protocol"; +} + +static int cmtp_proc_show(struct seq_file *m, void *v) +{ + struct capi_ctr *ctrl = m->private; + struct cmtp_session *session = ctrl->driverdata; + struct cmtp_application *app; + struct list_head *p, *n; + + seq_printf(m, "%s\n\n", cmtp_procinfo(ctrl)); + seq_printf(m, "addr %s\n", session->name); + seq_printf(m, "ctrl %d\n", session->num); + + list_for_each_safe(p, n, &session->applications) { + app = list_entry(p, struct cmtp_application, list); + seq_printf(m, "appl %d -> %d\n", app->appl, app->mapping); + } + + return 0; +} + +static int cmtp_proc_open(struct inode *inode, struct file *file) +{ + return single_open(file, cmtp_proc_show, PDE_DATA(inode)); +} + +static const struct file_operations cmtp_proc_fops = { + .owner = THIS_MODULE, + .open = cmtp_proc_open, + .read = seq_read, + .llseek = seq_lseek, + .release = single_release, +}; + +int cmtp_attach_device(struct cmtp_session *session) +{ + unsigned char buf[4]; + long ret; + + BT_DBG("session %p", session); + + capimsg_setu32(buf, 0, 0); + + cmtp_send_interopmsg(session, CAPI_REQ, 0xffff, CMTP_INITIAL_MSGNUM, + CAPI_FUNCTION_GET_PROFILE, buf, 4); + + ret = wait_event_interruptible_timeout(session->wait, + session->ncontroller, CMTP_INTEROP_TIMEOUT); + + BT_INFO("Found %d CAPI controller(s) on device %s", session->ncontroller, session->name); + + if (!ret) + return -ETIMEDOUT; + + if (!session->ncontroller) + return -ENODEV; + + if (session->ncontroller > 1) + BT_INFO("Setting up only CAPI controller 1"); + + session->ctrl.owner = THIS_MODULE; + session->ctrl.driverdata = session; + strcpy(session->ctrl.name, session->name); + + session->ctrl.driver_name = "cmtp"; + session->ctrl.load_firmware = cmtp_load_firmware; + session->ctrl.reset_ctr = cmtp_reset_ctr; + session->ctrl.register_appl = cmtp_register_appl; + session->ctrl.release_appl = cmtp_release_appl; + session->ctrl.send_message = cmtp_send_message; + + session->ctrl.procinfo = cmtp_procinfo; + session->ctrl.proc_fops = &cmtp_proc_fops; + + if (attach_capi_ctr(&session->ctrl) < 0) { + BT_ERR("Can't attach new controller"); + return -EBUSY; + } + + session->num = session->ctrl.cnr; + + BT_DBG("session %p num %d", session, session->num); + + capimsg_setu32(buf, 0, 1); + + cmtp_send_interopmsg(session, CAPI_REQ, 0xffff, cmtp_msgnum_get(session), + CAPI_FUNCTION_GET_MANUFACTURER, buf, 4); + + cmtp_send_interopmsg(session, CAPI_REQ, 0xffff, cmtp_msgnum_get(session), + CAPI_FUNCTION_GET_VERSION, buf, 4); + + cmtp_send_interopmsg(session, CAPI_REQ, 0xffff, cmtp_msgnum_get(session), + CAPI_FUNCTION_GET_SERIAL_NUMBER, buf, 4); + + cmtp_send_interopmsg(session, CAPI_REQ, 0xffff, cmtp_msgnum_get(session), + CAPI_FUNCTION_GET_PROFILE, buf, 4); + + return 0; +} + +void cmtp_detach_device(struct cmtp_session *session) +{ + BT_DBG("session %p", session); + + detach_capi_ctr(&session->ctrl); +} diff --git a/kernel/net/bluetooth/cmtp/cmtp.h b/kernel/net/bluetooth/cmtp/cmtp.h new file mode 100644 index 000000000..c32638ddd --- /dev/null +++ b/kernel/net/bluetooth/cmtp/cmtp.h @@ -0,0 +1,129 @@ +/* + CMTP implementation for Linux Bluetooth stack (BlueZ). + Copyright (C) 2002-2003 Marcel Holtmann <marcel@holtmann.org> + + 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; + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS + OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT OF THIRD PARTY RIGHTS. + IN NO EVENT SHALL THE COPYRIGHT HOLDER(S) AND AUTHOR(S) BE LIABLE FOR ANY + CLAIM, OR ANY SPECIAL INDIRECT OR CONSEQUENTIAL DAMAGES, OR ANY DAMAGES + WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN + ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF + OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + + ALL LIABILITY, INCLUDING LIABILITY FOR INFRINGEMENT OF ANY PATENTS, + COPYRIGHTS, TRADEMARKS OR OTHER RIGHTS, RELATING TO USE OF THIS + SOFTWARE IS DISCLAIMED. +*/ + +#ifndef __CMTP_H +#define __CMTP_H + +#include <linux/types.h> +#include <net/bluetooth/bluetooth.h> + +#define BTNAMSIZ 18 + +/* CMTP ioctl defines */ +#define CMTPCONNADD _IOW('C', 200, int) +#define CMTPCONNDEL _IOW('C', 201, int) +#define CMTPGETCONNLIST _IOR('C', 210, int) +#define CMTPGETCONNINFO _IOR('C', 211, int) + +#define CMTP_LOOPBACK 0 + +struct cmtp_connadd_req { + int sock; /* Connected socket */ + __u32 flags; +}; + +struct cmtp_conndel_req { + bdaddr_t bdaddr; + __u32 flags; +}; + +struct cmtp_conninfo { + bdaddr_t bdaddr; + __u32 flags; + __u16 state; + int num; +}; + +struct cmtp_connlist_req { + __u32 cnum; + struct cmtp_conninfo __user *ci; +}; + +int cmtp_add_connection(struct cmtp_connadd_req *req, struct socket *sock); +int cmtp_del_connection(struct cmtp_conndel_req *req); +int cmtp_get_connlist(struct cmtp_connlist_req *req); +int cmtp_get_conninfo(struct cmtp_conninfo *ci); + +/* CMTP session defines */ +#define CMTP_INTEROP_TIMEOUT (HZ * 5) +#define CMTP_INITIAL_MSGNUM 0xff00 + +struct cmtp_session { + struct list_head list; + + struct socket *sock; + + bdaddr_t bdaddr; + + unsigned long state; + unsigned long flags; + + uint mtu; + + char name[BTNAMSIZ]; + + atomic_t terminate; + struct task_struct *task; + + wait_queue_head_t wait; + + int ncontroller; + int num; + struct capi_ctr ctrl; + + struct list_head applications; + + unsigned long blockids; + int msgnum; + + struct sk_buff_head transmit; + + struct sk_buff *reassembly[16]; +}; + +struct cmtp_application { + struct list_head list; + + unsigned long state; + int err; + + __u16 appl; + __u16 mapping; + + __u16 msgnum; +}; + +struct cmtp_scb { + int id; + int data; +}; + +int cmtp_attach_device(struct cmtp_session *session); +void cmtp_detach_device(struct cmtp_session *session); + +void cmtp_recv_capimsg(struct cmtp_session *session, struct sk_buff *skb); + +/* CMTP init defines */ +int cmtp_init_sockets(void); +void cmtp_cleanup_sockets(void); + +#endif /* __CMTP_H */ diff --git a/kernel/net/bluetooth/cmtp/core.c b/kernel/net/bluetooth/cmtp/core.c new file mode 100644 index 000000000..298ed3701 --- /dev/null +++ b/kernel/net/bluetooth/cmtp/core.c @@ -0,0 +1,512 @@ +/* + CMTP implementation for Linux Bluetooth stack (BlueZ). + Copyright (C) 2002-2003 Marcel Holtmann <marcel@holtmann.org> + + 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; + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS + OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT OF THIRD PARTY RIGHTS. + IN NO EVENT SHALL THE COPYRIGHT HOLDER(S) AND AUTHOR(S) BE LIABLE FOR ANY + CLAIM, OR ANY SPECIAL INDIRECT OR CONSEQUENTIAL DAMAGES, OR ANY DAMAGES + WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN + ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF + OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + + ALL LIABILITY, INCLUDING LIABILITY FOR INFRINGEMENT OF ANY PATENTS, + COPYRIGHTS, TRADEMARKS OR OTHER RIGHTS, RELATING TO USE OF THIS + SOFTWARE IS DISCLAIMED. +*/ + +#include <linux/module.h> + +#include <linux/types.h> +#include <linux/errno.h> +#include <linux/kernel.h> +#include <linux/sched.h> +#include <linux/slab.h> +#include <linux/poll.h> +#include <linux/fcntl.h> +#include <linux/freezer.h> +#include <linux/skbuff.h> +#include <linux/socket.h> +#include <linux/ioctl.h> +#include <linux/file.h> +#include <linux/init.h> +#include <linux/kthread.h> +#include <net/sock.h> + +#include <linux/isdn/capilli.h> + +#include <net/bluetooth/bluetooth.h> +#include <net/bluetooth/l2cap.h> + +#include "cmtp.h" + +#define VERSION "1.0" + +static DECLARE_RWSEM(cmtp_session_sem); +static LIST_HEAD(cmtp_session_list); + +static struct cmtp_session *__cmtp_get_session(bdaddr_t *bdaddr) +{ + struct cmtp_session *session; + + BT_DBG(""); + + list_for_each_entry(session, &cmtp_session_list, list) + if (!bacmp(bdaddr, &session->bdaddr)) + return session; + + return NULL; +} + +static void __cmtp_link_session(struct cmtp_session *session) +{ + list_add(&session->list, &cmtp_session_list); +} + +static void __cmtp_unlink_session(struct cmtp_session *session) +{ + list_del(&session->list); +} + +static void __cmtp_copy_session(struct cmtp_session *session, struct cmtp_conninfo *ci) +{ + u32 valid_flags = BIT(CMTP_LOOPBACK); + memset(ci, 0, sizeof(*ci)); + bacpy(&ci->bdaddr, &session->bdaddr); + + ci->flags = session->flags & valid_flags; + ci->state = session->state; + + ci->num = session->num; +} + + +static inline int cmtp_alloc_block_id(struct cmtp_session *session) +{ + int i, id = -1; + + for (i = 0; i < 16; i++) + if (!test_and_set_bit(i, &session->blockids)) { + id = i; + break; + } + + return id; +} + +static inline void cmtp_free_block_id(struct cmtp_session *session, int id) +{ + clear_bit(id, &session->blockids); +} + +static inline void cmtp_add_msgpart(struct cmtp_session *session, int id, const unsigned char *buf, int count) +{ + struct sk_buff *skb = session->reassembly[id], *nskb; + int size; + + BT_DBG("session %p buf %p count %d", session, buf, count); + + size = (skb) ? skb->len + count : count; + + nskb = alloc_skb(size, GFP_ATOMIC); + if (!nskb) { + BT_ERR("Can't allocate memory for CAPI message"); + return; + } + + if (skb && (skb->len > 0)) + skb_copy_from_linear_data(skb, skb_put(nskb, skb->len), skb->len); + + memcpy(skb_put(nskb, count), buf, count); + + session->reassembly[id] = nskb; + + kfree_skb(skb); +} + +static inline int cmtp_recv_frame(struct cmtp_session *session, struct sk_buff *skb) +{ + __u8 hdr, hdrlen, id; + __u16 len; + + BT_DBG("session %p skb %p len %d", session, skb, skb->len); + + while (skb->len > 0) { + hdr = skb->data[0]; + + switch (hdr & 0xc0) { + case 0x40: + hdrlen = 2; + len = skb->data[1]; + break; + case 0x80: + hdrlen = 3; + len = skb->data[1] | (skb->data[2] << 8); + break; + default: + hdrlen = 1; + len = 0; + break; + } + + id = (hdr & 0x3c) >> 2; + + BT_DBG("hdr 0x%02x hdrlen %d len %d id %d", hdr, hdrlen, len, id); + + if (hdrlen + len > skb->len) { + BT_ERR("Wrong size or header information in CMTP frame"); + break; + } + + if (len == 0) { + skb_pull(skb, hdrlen); + continue; + } + + switch (hdr & 0x03) { + case 0x00: + cmtp_add_msgpart(session, id, skb->data + hdrlen, len); + cmtp_recv_capimsg(session, session->reassembly[id]); + session->reassembly[id] = NULL; + break; + case 0x01: + cmtp_add_msgpart(session, id, skb->data + hdrlen, len); + break; + default: + if (session->reassembly[id] != NULL) + kfree_skb(session->reassembly[id]); + session->reassembly[id] = NULL; + break; + } + + skb_pull(skb, hdrlen + len); + } + + kfree_skb(skb); + return 0; +} + +static int cmtp_send_frame(struct cmtp_session *session, unsigned char *data, int len) +{ + struct socket *sock = session->sock; + struct kvec iv = { data, len }; + struct msghdr msg; + + BT_DBG("session %p data %p len %d", session, data, len); + + if (!len) + return 0; + + memset(&msg, 0, sizeof(msg)); + + return kernel_sendmsg(sock, &msg, &iv, 1, len); +} + +static void cmtp_process_transmit(struct cmtp_session *session) +{ + struct sk_buff *skb, *nskb; + unsigned char *hdr; + unsigned int size, tail; + + BT_DBG("session %p", session); + + nskb = alloc_skb(session->mtu, GFP_ATOMIC); + if (!nskb) { + BT_ERR("Can't allocate memory for new frame"); + return; + } + + while ((skb = skb_dequeue(&session->transmit))) { + struct cmtp_scb *scb = (void *) skb->cb; + + tail = session->mtu - nskb->len; + if (tail < 5) { + cmtp_send_frame(session, nskb->data, nskb->len); + skb_trim(nskb, 0); + tail = session->mtu; + } + + size = min_t(uint, ((tail < 258) ? (tail - 2) : (tail - 3)), skb->len); + + if (scb->id < 0) { + scb->id = cmtp_alloc_block_id(session); + if (scb->id < 0) { + skb_queue_head(&session->transmit, skb); + break; + } + } + + if (size < 256) { + hdr = skb_put(nskb, 2); + hdr[0] = 0x40 + | ((scb->id << 2) & 0x3c) + | ((skb->len == size) ? 0x00 : 0x01); + hdr[1] = size; + } else { + hdr = skb_put(nskb, 3); + hdr[0] = 0x80 + | ((scb->id << 2) & 0x3c) + | ((skb->len == size) ? 0x00 : 0x01); + hdr[1] = size & 0xff; + hdr[2] = size >> 8; + } + + skb_copy_from_linear_data(skb, skb_put(nskb, size), size); + skb_pull(skb, size); + + if (skb->len > 0) { + skb_queue_head(&session->transmit, skb); + } else { + cmtp_free_block_id(session, scb->id); + if (scb->data) { + cmtp_send_frame(session, nskb->data, nskb->len); + skb_trim(nskb, 0); + } + kfree_skb(skb); + } + } + + cmtp_send_frame(session, nskb->data, nskb->len); + + kfree_skb(nskb); +} + +static int cmtp_session(void *arg) +{ + struct cmtp_session *session = arg; + struct sock *sk = session->sock->sk; + struct sk_buff *skb; + wait_queue_t wait; + + BT_DBG("session %p", session); + + set_user_nice(current, -15); + + init_waitqueue_entry(&wait, current); + add_wait_queue(sk_sleep(sk), &wait); + while (1) { + set_current_state(TASK_INTERRUPTIBLE); + + if (atomic_read(&session->terminate)) + break; + if (sk->sk_state != BT_CONNECTED) + break; + + while ((skb = skb_dequeue(&sk->sk_receive_queue))) { + skb_orphan(skb); + if (!skb_linearize(skb)) + cmtp_recv_frame(session, skb); + else + kfree_skb(skb); + } + + cmtp_process_transmit(session); + + schedule(); + } + __set_current_state(TASK_RUNNING); + remove_wait_queue(sk_sleep(sk), &wait); + + down_write(&cmtp_session_sem); + + if (!(session->flags & BIT(CMTP_LOOPBACK))) + cmtp_detach_device(session); + + fput(session->sock->file); + + __cmtp_unlink_session(session); + + up_write(&cmtp_session_sem); + + kfree(session); + module_put_and_exit(0); + return 0; +} + +int cmtp_add_connection(struct cmtp_connadd_req *req, struct socket *sock) +{ + u32 valid_flags = BIT(CMTP_LOOPBACK); + struct cmtp_session *session, *s; + int i, err; + + BT_DBG(""); + + if (!l2cap_is_socket(sock)) + return -EBADFD; + + if (req->flags & ~valid_flags) + return -EINVAL; + + session = kzalloc(sizeof(struct cmtp_session), GFP_KERNEL); + if (!session) + return -ENOMEM; + + down_write(&cmtp_session_sem); + + s = __cmtp_get_session(&l2cap_pi(sock->sk)->chan->dst); + if (s && s->state == BT_CONNECTED) { + err = -EEXIST; + goto failed; + } + + bacpy(&session->bdaddr, &l2cap_pi(sock->sk)->chan->dst); + + session->mtu = min_t(uint, l2cap_pi(sock->sk)->chan->omtu, + l2cap_pi(sock->sk)->chan->imtu); + + BT_DBG("mtu %d", session->mtu); + + sprintf(session->name, "%pMR", &session->bdaddr); + + session->sock = sock; + session->state = BT_CONFIG; + + init_waitqueue_head(&session->wait); + + session->msgnum = CMTP_INITIAL_MSGNUM; + + INIT_LIST_HEAD(&session->applications); + + skb_queue_head_init(&session->transmit); + + for (i = 0; i < 16; i++) + session->reassembly[i] = NULL; + + session->flags = req->flags; + + __cmtp_link_session(session); + + __module_get(THIS_MODULE); + session->task = kthread_run(cmtp_session, session, "kcmtpd_ctr_%d", + session->num); + if (IS_ERR(session->task)) { + module_put(THIS_MODULE); + err = PTR_ERR(session->task); + goto unlink; + } + + if (!(session->flags & BIT(CMTP_LOOPBACK))) { + err = cmtp_attach_device(session); + if (err < 0) { + atomic_inc(&session->terminate); + wake_up_process(session->task); + up_write(&cmtp_session_sem); + return err; + } + } + + up_write(&cmtp_session_sem); + return 0; + +unlink: + __cmtp_unlink_session(session); + +failed: + up_write(&cmtp_session_sem); + kfree(session); + return err; +} + +int cmtp_del_connection(struct cmtp_conndel_req *req) +{ + u32 valid_flags = 0; + struct cmtp_session *session; + int err = 0; + + BT_DBG(""); + + if (req->flags & ~valid_flags) + return -EINVAL; + + down_read(&cmtp_session_sem); + + session = __cmtp_get_session(&req->bdaddr); + if (session) { + /* Flush the transmit queue */ + skb_queue_purge(&session->transmit); + + /* Stop session thread */ + atomic_inc(&session->terminate); + wake_up_process(session->task); + } else + err = -ENOENT; + + up_read(&cmtp_session_sem); + return err; +} + +int cmtp_get_connlist(struct cmtp_connlist_req *req) +{ + struct cmtp_session *session; + int err = 0, n = 0; + + BT_DBG(""); + + down_read(&cmtp_session_sem); + + list_for_each_entry(session, &cmtp_session_list, list) { + struct cmtp_conninfo ci; + + __cmtp_copy_session(session, &ci); + + if (copy_to_user(req->ci, &ci, sizeof(ci))) { + err = -EFAULT; + break; + } + + if (++n >= req->cnum) + break; + + req->ci++; + } + req->cnum = n; + + up_read(&cmtp_session_sem); + return err; +} + +int cmtp_get_conninfo(struct cmtp_conninfo *ci) +{ + struct cmtp_session *session; + int err = 0; + + down_read(&cmtp_session_sem); + + session = __cmtp_get_session(&ci->bdaddr); + if (session) + __cmtp_copy_session(session, ci); + else + err = -ENOENT; + + up_read(&cmtp_session_sem); + return err; +} + + +static int __init cmtp_init(void) +{ + BT_INFO("CMTP (CAPI Emulation) ver %s", VERSION); + + cmtp_init_sockets(); + + return 0; +} + +static void __exit cmtp_exit(void) +{ + cmtp_cleanup_sockets(); +} + +module_init(cmtp_init); +module_exit(cmtp_exit); + +MODULE_AUTHOR("Marcel Holtmann <marcel@holtmann.org>"); +MODULE_DESCRIPTION("Bluetooth CMTP ver " VERSION); +MODULE_VERSION(VERSION); +MODULE_LICENSE("GPL"); +MODULE_ALIAS("bt-proto-5"); diff --git a/kernel/net/bluetooth/cmtp/sock.c b/kernel/net/bluetooth/cmtp/sock.c new file mode 100644 index 000000000..d82787d41 --- /dev/null +++ b/kernel/net/bluetooth/cmtp/sock.c @@ -0,0 +1,269 @@ +/* + CMTP implementation for Linux Bluetooth stack (BlueZ). + Copyright (C) 2002-2003 Marcel Holtmann <marcel@holtmann.org> + + 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; + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS + OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT OF THIRD PARTY RIGHTS. + IN NO EVENT SHALL THE COPYRIGHT HOLDER(S) AND AUTHOR(S) BE LIABLE FOR ANY + CLAIM, OR ANY SPECIAL INDIRECT OR CONSEQUENTIAL DAMAGES, OR ANY DAMAGES + WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN + ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF + OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + + ALL LIABILITY, INCLUDING LIABILITY FOR INFRINGEMENT OF ANY PATENTS, + COPYRIGHTS, TRADEMARKS OR OTHER RIGHTS, RELATING TO USE OF THIS + SOFTWARE IS DISCLAIMED. +*/ + +#include <linux/export.h> + +#include <linux/types.h> +#include <linux/capability.h> +#include <linux/errno.h> +#include <linux/kernel.h> +#include <linux/poll.h> +#include <linux/fcntl.h> +#include <linux/skbuff.h> +#include <linux/socket.h> +#include <linux/ioctl.h> +#include <linux/file.h> +#include <linux/compat.h> +#include <linux/gfp.h> +#include <linux/uaccess.h> +#include <net/sock.h> + +#include <linux/isdn/capilli.h> + + +#include "cmtp.h" + +static struct bt_sock_list cmtp_sk_list = { + .lock = __RW_LOCK_UNLOCKED(cmtp_sk_list.lock) +}; + +static int cmtp_sock_release(struct socket *sock) +{ + struct sock *sk = sock->sk; + + BT_DBG("sock %p sk %p", sock, sk); + + if (!sk) + return 0; + + bt_sock_unlink(&cmtp_sk_list, sk); + + sock_orphan(sk); + sock_put(sk); + + return 0; +} + +static int cmtp_sock_ioctl(struct socket *sock, unsigned int cmd, unsigned long arg) +{ + struct cmtp_connadd_req ca; + struct cmtp_conndel_req cd; + struct cmtp_connlist_req cl; + struct cmtp_conninfo ci; + struct socket *nsock; + void __user *argp = (void __user *)arg; + int err; + + BT_DBG("cmd %x arg %lx", cmd, arg); + + switch (cmd) { + case CMTPCONNADD: + if (!capable(CAP_NET_ADMIN)) + return -EPERM; + + if (copy_from_user(&ca, argp, sizeof(ca))) + return -EFAULT; + + nsock = sockfd_lookup(ca.sock, &err); + if (!nsock) + return err; + + if (nsock->sk->sk_state != BT_CONNECTED) { + sockfd_put(nsock); + return -EBADFD; + } + + err = cmtp_add_connection(&ca, nsock); + if (!err) { + if (copy_to_user(argp, &ca, sizeof(ca))) + err = -EFAULT; + } else + sockfd_put(nsock); + + return err; + + case CMTPCONNDEL: + if (!capable(CAP_NET_ADMIN)) + return -EPERM; + + if (copy_from_user(&cd, argp, sizeof(cd))) + return -EFAULT; + + return cmtp_del_connection(&cd); + + case CMTPGETCONNLIST: + if (copy_from_user(&cl, argp, sizeof(cl))) + return -EFAULT; + + if (cl.cnum <= 0) + return -EINVAL; + + err = cmtp_get_connlist(&cl); + if (!err && copy_to_user(argp, &cl, sizeof(cl))) + return -EFAULT; + + return err; + + case CMTPGETCONNINFO: + if (copy_from_user(&ci, argp, sizeof(ci))) + return -EFAULT; + + err = cmtp_get_conninfo(&ci); + if (!err && copy_to_user(argp, &ci, sizeof(ci))) + return -EFAULT; + + return err; + } + + return -EINVAL; +} + +#ifdef CONFIG_COMPAT +static int cmtp_sock_compat_ioctl(struct socket *sock, unsigned int cmd, unsigned long arg) +{ + if (cmd == CMTPGETCONNLIST) { + struct cmtp_connlist_req cl; + u32 uci; + int err; + + if (get_user(cl.cnum, (u32 __user *) arg) || + get_user(uci, (u32 __user *) (arg + 4))) + return -EFAULT; + + cl.ci = compat_ptr(uci); + + if (cl.cnum <= 0) + return -EINVAL; + + err = cmtp_get_connlist(&cl); + + if (!err && put_user(cl.cnum, (u32 __user *) arg)) + err = -EFAULT; + + return err; + } + + return cmtp_sock_ioctl(sock, cmd, arg); +} +#endif + +static const struct proto_ops cmtp_sock_ops = { + .family = PF_BLUETOOTH, + .owner = THIS_MODULE, + .release = cmtp_sock_release, + .ioctl = cmtp_sock_ioctl, +#ifdef CONFIG_COMPAT + .compat_ioctl = cmtp_sock_compat_ioctl, +#endif + .bind = sock_no_bind, + .getname = sock_no_getname, + .sendmsg = sock_no_sendmsg, + .recvmsg = sock_no_recvmsg, + .poll = sock_no_poll, + .listen = sock_no_listen, + .shutdown = sock_no_shutdown, + .setsockopt = sock_no_setsockopt, + .getsockopt = sock_no_getsockopt, + .connect = sock_no_connect, + .socketpair = sock_no_socketpair, + .accept = sock_no_accept, + .mmap = sock_no_mmap +}; + +static struct proto cmtp_proto = { + .name = "CMTP", + .owner = THIS_MODULE, + .obj_size = sizeof(struct bt_sock) +}; + +static int cmtp_sock_create(struct net *net, struct socket *sock, int protocol, + int kern) +{ + struct sock *sk; + + BT_DBG("sock %p", sock); + + if (sock->type != SOCK_RAW) + return -ESOCKTNOSUPPORT; + + sk = sk_alloc(net, PF_BLUETOOTH, GFP_ATOMIC, &cmtp_proto); + if (!sk) + return -ENOMEM; + + sock_init_data(sock, sk); + + sock->ops = &cmtp_sock_ops; + + sock->state = SS_UNCONNECTED; + + sock_reset_flag(sk, SOCK_ZAPPED); + + sk->sk_protocol = protocol; + sk->sk_state = BT_OPEN; + + bt_sock_link(&cmtp_sk_list, sk); + + return 0; +} + +static const struct net_proto_family cmtp_sock_family_ops = { + .family = PF_BLUETOOTH, + .owner = THIS_MODULE, + .create = cmtp_sock_create +}; + +int cmtp_init_sockets(void) +{ + int err; + + err = proto_register(&cmtp_proto, 0); + if (err < 0) + return err; + + err = bt_sock_register(BTPROTO_CMTP, &cmtp_sock_family_ops); + if (err < 0) { + BT_ERR("Can't register CMTP socket"); + goto error; + } + + err = bt_procfs_init(&init_net, "cmtp", &cmtp_sk_list, NULL); + if (err < 0) { + BT_ERR("Failed to create CMTP proc file"); + bt_sock_unregister(BTPROTO_HIDP); + goto error; + } + + BT_INFO("CMTP socket layer initialized"); + + return 0; + +error: + proto_unregister(&cmtp_proto); + return err; +} + +void cmtp_cleanup_sockets(void) +{ + bt_procfs_cleanup(&init_net, "cmtp"); + bt_sock_unregister(BTPROTO_CMTP); + proto_unregister(&cmtp_proto); +} diff --git a/kernel/net/bluetooth/ecc.c b/kernel/net/bluetooth/ecc.c new file mode 100644 index 000000000..e1709f846 --- /dev/null +++ b/kernel/net/bluetooth/ecc.c @@ -0,0 +1,816 @@ +/* + * Copyright (c) 2013, Kenneth MacKay + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are + * met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * 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. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS 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 COPYRIGHT + * HOLDER 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/random.h> + +#include "ecc.h" + +/* 256-bit curve */ +#define ECC_BYTES 32 + +#define MAX_TRIES 16 + +/* Number of u64's needed */ +#define NUM_ECC_DIGITS (ECC_BYTES / 8) + +struct ecc_point { + u64 x[NUM_ECC_DIGITS]; + u64 y[NUM_ECC_DIGITS]; +}; + +typedef struct { + u64 m_low; + u64 m_high; +} uint128_t; + +#define CURVE_P_32 { 0xFFFFFFFFFFFFFFFFull, 0x00000000FFFFFFFFull, \ + 0x0000000000000000ull, 0xFFFFFFFF00000001ull } + +#define CURVE_G_32 { \ + { 0xF4A13945D898C296ull, 0x77037D812DEB33A0ull, \ + 0xF8BCE6E563A440F2ull, 0x6B17D1F2E12C4247ull }, \ + { 0xCBB6406837BF51F5ull, 0x2BCE33576B315ECEull, \ + 0x8EE7EB4A7C0F9E16ull, 0x4FE342E2FE1A7F9Bull } \ +} + +#define CURVE_N_32 { 0xF3B9CAC2FC632551ull, 0xBCE6FAADA7179E84ull, \ + 0xFFFFFFFFFFFFFFFFull, 0xFFFFFFFF00000000ull } + +static u64 curve_p[NUM_ECC_DIGITS] = CURVE_P_32; +static struct ecc_point curve_g = CURVE_G_32; +static u64 curve_n[NUM_ECC_DIGITS] = CURVE_N_32; + +static void vli_clear(u64 *vli) +{ + int i; + + for (i = 0; i < NUM_ECC_DIGITS; i++) + vli[i] = 0; +} + +/* Returns true if vli == 0, false otherwise. */ +static bool vli_is_zero(const u64 *vli) +{ + int i; + + for (i = 0; i < NUM_ECC_DIGITS; i++) { + if (vli[i]) + return false; + } + + return true; +} + +/* Returns nonzero if bit bit of vli is set. */ +static u64 vli_test_bit(const u64 *vli, unsigned int bit) +{ + return (vli[bit / 64] & ((u64) 1 << (bit % 64))); +} + +/* Counts the number of 64-bit "digits" in vli. */ +static unsigned int vli_num_digits(const u64 *vli) +{ + int i; + + /* Search from the end until we find a non-zero digit. + * We do it in reverse because we expect that most digits will + * be nonzero. + */ + for (i = NUM_ECC_DIGITS - 1; i >= 0 && vli[i] == 0; i--); + + return (i + 1); +} + +/* Counts the number of bits required for vli. */ +static unsigned int vli_num_bits(const u64 *vli) +{ + unsigned int i, num_digits; + u64 digit; + + num_digits = vli_num_digits(vli); + if (num_digits == 0) + return 0; + + digit = vli[num_digits - 1]; + for (i = 0; digit; i++) + digit >>= 1; + + return ((num_digits - 1) * 64 + i); +} + +/* Sets dest = src. */ +static void vli_set(u64 *dest, const u64 *src) +{ + int i; + + for (i = 0; i < NUM_ECC_DIGITS; i++) + dest[i] = src[i]; +} + +/* Returns sign of left - right. */ +static int vli_cmp(const u64 *left, const u64 *right) +{ + int i; + + for (i = NUM_ECC_DIGITS - 1; i >= 0; i--) { + if (left[i] > right[i]) + return 1; + else if (left[i] < right[i]) + return -1; + } + + return 0; +} + +/* Computes result = in << c, returning carry. Can modify in place + * (if result == in). 0 < shift < 64. + */ +static u64 vli_lshift(u64 *result, const u64 *in, + unsigned int shift) +{ + u64 carry = 0; + int i; + + for (i = 0; i < NUM_ECC_DIGITS; i++) { + u64 temp = in[i]; + + result[i] = (temp << shift) | carry; + carry = temp >> (64 - shift); + } + + return carry; +} + +/* Computes vli = vli >> 1. */ +static void vli_rshift1(u64 *vli) +{ + u64 *end = vli; + u64 carry = 0; + + vli += NUM_ECC_DIGITS; + + while (vli-- > end) { + u64 temp = *vli; + *vli = (temp >> 1) | carry; + carry = temp << 63; + } +} + +/* Computes result = left + right, returning carry. Can modify in place. */ +static u64 vli_add(u64 *result, const u64 *left, + const u64 *right) +{ + u64 carry = 0; + int i; + + for (i = 0; i < NUM_ECC_DIGITS; i++) { + u64 sum; + + sum = left[i] + right[i] + carry; + if (sum != left[i]) + carry = (sum < left[i]); + + result[i] = sum; + } + + return carry; +} + +/* Computes result = left - right, returning borrow. Can modify in place. */ +static u64 vli_sub(u64 *result, const u64 *left, const u64 *right) +{ + u64 borrow = 0; + int i; + + for (i = 0; i < NUM_ECC_DIGITS; i++) { + u64 diff; + + diff = left[i] - right[i] - borrow; + if (diff != left[i]) + borrow = (diff > left[i]); + + result[i] = diff; + } + + return borrow; +} + +static uint128_t mul_64_64(u64 left, u64 right) +{ + u64 a0 = left & 0xffffffffull; + u64 a1 = left >> 32; + u64 b0 = right & 0xffffffffull; + u64 b1 = right >> 32; + u64 m0 = a0 * b0; + u64 m1 = a0 * b1; + u64 m2 = a1 * b0; + u64 m3 = a1 * b1; + uint128_t result; + + m2 += (m0 >> 32); + m2 += m1; + + /* Overflow */ + if (m2 < m1) + m3 += 0x100000000ull; + + result.m_low = (m0 & 0xffffffffull) | (m2 << 32); + result.m_high = m3 + (m2 >> 32); + + return result; +} + +static uint128_t add_128_128(uint128_t a, uint128_t b) +{ + uint128_t result; + + result.m_low = a.m_low + b.m_low; + result.m_high = a.m_high + b.m_high + (result.m_low < a.m_low); + + return result; +} + +static void vli_mult(u64 *result, const u64 *left, const u64 *right) +{ + uint128_t r01 = { 0, 0 }; + u64 r2 = 0; + unsigned int i, k; + + /* Compute each digit of result in sequence, maintaining the + * carries. + */ + for (k = 0; k < NUM_ECC_DIGITS * 2 - 1; k++) { + unsigned int min; + + if (k < NUM_ECC_DIGITS) + min = 0; + else + min = (k + 1) - NUM_ECC_DIGITS; + + for (i = min; i <= k && i < NUM_ECC_DIGITS; i++) { + uint128_t product; + + product = mul_64_64(left[i], right[k - i]); + + r01 = add_128_128(r01, product); + r2 += (r01.m_high < product.m_high); + } + + result[k] = r01.m_low; + r01.m_low = r01.m_high; + r01.m_high = r2; + r2 = 0; + } + + result[NUM_ECC_DIGITS * 2 - 1] = r01.m_low; +} + +static void vli_square(u64 *result, const u64 *left) +{ + uint128_t r01 = { 0, 0 }; + u64 r2 = 0; + int i, k; + + for (k = 0; k < NUM_ECC_DIGITS * 2 - 1; k++) { + unsigned int min; + + if (k < NUM_ECC_DIGITS) + min = 0; + else + min = (k + 1) - NUM_ECC_DIGITS; + + for (i = min; i <= k && i <= k - i; i++) { + uint128_t product; + + product = mul_64_64(left[i], left[k - i]); + + if (i < k - i) { + r2 += product.m_high >> 63; + product.m_high = (product.m_high << 1) | + (product.m_low >> 63); + product.m_low <<= 1; + } + + r01 = add_128_128(r01, product); + r2 += (r01.m_high < product.m_high); + } + + result[k] = r01.m_low; + r01.m_low = r01.m_high; + r01.m_high = r2; + r2 = 0; + } + + result[NUM_ECC_DIGITS * 2 - 1] = r01.m_low; +} + +/* Computes result = (left + right) % mod. + * Assumes that left < mod and right < mod, result != mod. + */ +static void vli_mod_add(u64 *result, const u64 *left, const u64 *right, + const u64 *mod) +{ + u64 carry; + + carry = vli_add(result, left, right); + + /* result > mod (result = mod + remainder), so subtract mod to + * get remainder. + */ + if (carry || vli_cmp(result, mod) >= 0) + vli_sub(result, result, mod); +} + +/* Computes result = (left - right) % mod. + * Assumes that left < mod and right < mod, result != mod. + */ +static void vli_mod_sub(u64 *result, const u64 *left, const u64 *right, + const u64 *mod) +{ + u64 borrow = vli_sub(result, left, right); + + /* In this case, p_result == -diff == (max int) - diff. + * Since -x % d == d - x, we can get the correct result from + * result + mod (with overflow). + */ + if (borrow) + vli_add(result, result, mod); +} + +/* Computes result = product % curve_p + from http://www.nsa.gov/ia/_files/nist-routines.pdf */ +static void vli_mmod_fast(u64 *result, const u64 *product) +{ + u64 tmp[NUM_ECC_DIGITS]; + int carry; + + /* t */ + vli_set(result, product); + + /* s1 */ + tmp[0] = 0; + tmp[1] = product[5] & 0xffffffff00000000ull; + tmp[2] = product[6]; + tmp[3] = product[7]; + carry = vli_lshift(tmp, tmp, 1); + carry += vli_add(result, result, tmp); + + /* s2 */ + tmp[1] = product[6] << 32; + tmp[2] = (product[6] >> 32) | (product[7] << 32); + tmp[3] = product[7] >> 32; + carry += vli_lshift(tmp, tmp, 1); + carry += vli_add(result, result, tmp); + + /* s3 */ + tmp[0] = product[4]; + tmp[1] = product[5] & 0xffffffff; + tmp[2] = 0; + tmp[3] = product[7]; + carry += vli_add(result, result, tmp); + + /* s4 */ + tmp[0] = (product[4] >> 32) | (product[5] << 32); + tmp[1] = (product[5] >> 32) | (product[6] & 0xffffffff00000000ull); + tmp[2] = product[7]; + tmp[3] = (product[6] >> 32) | (product[4] << 32); + carry += vli_add(result, result, tmp); + + /* d1 */ + tmp[0] = (product[5] >> 32) | (product[6] << 32); + tmp[1] = (product[6] >> 32); + tmp[2] = 0; + tmp[3] = (product[4] & 0xffffffff) | (product[5] << 32); + carry -= vli_sub(result, result, tmp); + + /* d2 */ + tmp[0] = product[6]; + tmp[1] = product[7]; + tmp[2] = 0; + tmp[3] = (product[4] >> 32) | (product[5] & 0xffffffff00000000ull); + carry -= vli_sub(result, result, tmp); + + /* d3 */ + tmp[0] = (product[6] >> 32) | (product[7] << 32); + tmp[1] = (product[7] >> 32) | (product[4] << 32); + tmp[2] = (product[4] >> 32) | (product[5] << 32); + tmp[3] = (product[6] << 32); + carry -= vli_sub(result, result, tmp); + + /* d4 */ + tmp[0] = product[7]; + tmp[1] = product[4] & 0xffffffff00000000ull; + tmp[2] = product[5]; + tmp[3] = product[6] & 0xffffffff00000000ull; + carry -= vli_sub(result, result, tmp); + + if (carry < 0) { + do { + carry += vli_add(result, result, curve_p); + } while (carry < 0); + } else { + while (carry || vli_cmp(curve_p, result) != 1) + carry -= vli_sub(result, result, curve_p); + } +} + +/* Computes result = (left * right) % curve_p. */ +static void vli_mod_mult_fast(u64 *result, const u64 *left, const u64 *right) +{ + u64 product[2 * NUM_ECC_DIGITS]; + + vli_mult(product, left, right); + vli_mmod_fast(result, product); +} + +/* Computes result = left^2 % curve_p. */ +static void vli_mod_square_fast(u64 *result, const u64 *left) +{ + u64 product[2 * NUM_ECC_DIGITS]; + + vli_square(product, left); + vli_mmod_fast(result, product); +} + +#define EVEN(vli) (!(vli[0] & 1)) +/* Computes result = (1 / p_input) % mod. All VLIs are the same size. + * See "From Euclid's GCD to Montgomery Multiplication to the Great Divide" + * https://labs.oracle.com/techrep/2001/smli_tr-2001-95.pdf + */ +static void vli_mod_inv(u64 *result, const u64 *input, const u64 *mod) +{ + u64 a[NUM_ECC_DIGITS], b[NUM_ECC_DIGITS]; + u64 u[NUM_ECC_DIGITS], v[NUM_ECC_DIGITS]; + u64 carry; + int cmp_result; + + if (vli_is_zero(input)) { + vli_clear(result); + return; + } + + vli_set(a, input); + vli_set(b, mod); + vli_clear(u); + u[0] = 1; + vli_clear(v); + + while ((cmp_result = vli_cmp(a, b)) != 0) { + carry = 0; + + if (EVEN(a)) { + vli_rshift1(a); + + if (!EVEN(u)) + carry = vli_add(u, u, mod); + + vli_rshift1(u); + if (carry) + u[NUM_ECC_DIGITS - 1] |= 0x8000000000000000ull; + } else if (EVEN(b)) { + vli_rshift1(b); + + if (!EVEN(v)) + carry = vli_add(v, v, mod); + + vli_rshift1(v); + if (carry) + v[NUM_ECC_DIGITS - 1] |= 0x8000000000000000ull; + } else if (cmp_result > 0) { + vli_sub(a, a, b); + vli_rshift1(a); + + if (vli_cmp(u, v) < 0) + vli_add(u, u, mod); + + vli_sub(u, u, v); + if (!EVEN(u)) + carry = vli_add(u, u, mod); + + vli_rshift1(u); + if (carry) + u[NUM_ECC_DIGITS - 1] |= 0x8000000000000000ull; + } else { + vli_sub(b, b, a); + vli_rshift1(b); + + if (vli_cmp(v, u) < 0) + vli_add(v, v, mod); + + vli_sub(v, v, u); + if (!EVEN(v)) + carry = vli_add(v, v, mod); + + vli_rshift1(v); + if (carry) + v[NUM_ECC_DIGITS - 1] |= 0x8000000000000000ull; + } + } + + vli_set(result, u); +} + +/* ------ Point operations ------ */ + +/* Returns true if p_point is the point at infinity, false otherwise. */ +static bool ecc_point_is_zero(const struct ecc_point *point) +{ + return (vli_is_zero(point->x) && vli_is_zero(point->y)); +} + +/* Point multiplication algorithm using Montgomery's ladder with co-Z + * coordinates. From http://eprint.iacr.org/2011/338.pdf + */ + +/* Double in place */ +static void ecc_point_double_jacobian(u64 *x1, u64 *y1, u64 *z1) +{ + /* t1 = x, t2 = y, t3 = z */ + u64 t4[NUM_ECC_DIGITS]; + u64 t5[NUM_ECC_DIGITS]; + + if (vli_is_zero(z1)) + return; + + vli_mod_square_fast(t4, y1); /* t4 = y1^2 */ + vli_mod_mult_fast(t5, x1, t4); /* t5 = x1*y1^2 = A */ + vli_mod_square_fast(t4, t4); /* t4 = y1^4 */ + vli_mod_mult_fast(y1, y1, z1); /* t2 = y1*z1 = z3 */ + vli_mod_square_fast(z1, z1); /* t3 = z1^2 */ + + vli_mod_add(x1, x1, z1, curve_p); /* t1 = x1 + z1^2 */ + vli_mod_add(z1, z1, z1, curve_p); /* t3 = 2*z1^2 */ + vli_mod_sub(z1, x1, z1, curve_p); /* t3 = x1 - z1^2 */ + vli_mod_mult_fast(x1, x1, z1); /* t1 = x1^2 - z1^4 */ + + vli_mod_add(z1, x1, x1, curve_p); /* t3 = 2*(x1^2 - z1^4) */ + vli_mod_add(x1, x1, z1, curve_p); /* t1 = 3*(x1^2 - z1^4) */ + if (vli_test_bit(x1, 0)) { + u64 carry = vli_add(x1, x1, curve_p); + vli_rshift1(x1); + x1[NUM_ECC_DIGITS - 1] |= carry << 63; + } else { + vli_rshift1(x1); + } + /* t1 = 3/2*(x1^2 - z1^4) = B */ + + vli_mod_square_fast(z1, x1); /* t3 = B^2 */ + vli_mod_sub(z1, z1, t5, curve_p); /* t3 = B^2 - A */ + vli_mod_sub(z1, z1, t5, curve_p); /* t3 = B^2 - 2A = x3 */ + vli_mod_sub(t5, t5, z1, curve_p); /* t5 = A - x3 */ + vli_mod_mult_fast(x1, x1, t5); /* t1 = B * (A - x3) */ + vli_mod_sub(t4, x1, t4, curve_p); /* t4 = B * (A - x3) - y1^4 = y3 */ + + vli_set(x1, z1); + vli_set(z1, y1); + vli_set(y1, t4); +} + +/* Modify (x1, y1) => (x1 * z^2, y1 * z^3) */ +static void apply_z(u64 *x1, u64 *y1, u64 *z) +{ + u64 t1[NUM_ECC_DIGITS]; + + vli_mod_square_fast(t1, z); /* z^2 */ + vli_mod_mult_fast(x1, x1, t1); /* x1 * z^2 */ + vli_mod_mult_fast(t1, t1, z); /* z^3 */ + vli_mod_mult_fast(y1, y1, t1); /* y1 * z^3 */ +} + +/* P = (x1, y1) => 2P, (x2, y2) => P' */ +static void xycz_initial_double(u64 *x1, u64 *y1, u64 *x2, u64 *y2, + u64 *p_initial_z) +{ + u64 z[NUM_ECC_DIGITS]; + + vli_set(x2, x1); + vli_set(y2, y1); + + vli_clear(z); + z[0] = 1; + + if (p_initial_z) + vli_set(z, p_initial_z); + + apply_z(x1, y1, z); + + ecc_point_double_jacobian(x1, y1, z); + + apply_z(x2, y2, z); +} + +/* Input P = (x1, y1, Z), Q = (x2, y2, Z) + * Output P' = (x1', y1', Z3), P + Q = (x3, y3, Z3) + * or P => P', Q => P + Q + */ +static void xycz_add(u64 *x1, u64 *y1, u64 *x2, u64 *y2) +{ + /* t1 = X1, t2 = Y1, t3 = X2, t4 = Y2 */ + u64 t5[NUM_ECC_DIGITS]; + + vli_mod_sub(t5, x2, x1, curve_p); /* t5 = x2 - x1 */ + vli_mod_square_fast(t5, t5); /* t5 = (x2 - x1)^2 = A */ + vli_mod_mult_fast(x1, x1, t5); /* t1 = x1*A = B */ + vli_mod_mult_fast(x2, x2, t5); /* t3 = x2*A = C */ + vli_mod_sub(y2, y2, y1, curve_p); /* t4 = y2 - y1 */ + vli_mod_square_fast(t5, y2); /* t5 = (y2 - y1)^2 = D */ + + vli_mod_sub(t5, t5, x1, curve_p); /* t5 = D - B */ + vli_mod_sub(t5, t5, x2, curve_p); /* t5 = D - B - C = x3 */ + vli_mod_sub(x2, x2, x1, curve_p); /* t3 = C - B */ + vli_mod_mult_fast(y1, y1, x2); /* t2 = y1*(C - B) */ + vli_mod_sub(x2, x1, t5, curve_p); /* t3 = B - x3 */ + vli_mod_mult_fast(y2, y2, x2); /* t4 = (y2 - y1)*(B - x3) */ + vli_mod_sub(y2, y2, y1, curve_p); /* t4 = y3 */ + + vli_set(x2, t5); +} + +/* Input P = (x1, y1, Z), Q = (x2, y2, Z) + * Output P + Q = (x3, y3, Z3), P - Q = (x3', y3', Z3) + * or P => P - Q, Q => P + Q + */ +static void xycz_add_c(u64 *x1, u64 *y1, u64 *x2, u64 *y2) +{ + /* t1 = X1, t2 = Y1, t3 = X2, t4 = Y2 */ + u64 t5[NUM_ECC_DIGITS]; + u64 t6[NUM_ECC_DIGITS]; + u64 t7[NUM_ECC_DIGITS]; + + vli_mod_sub(t5, x2, x1, curve_p); /* t5 = x2 - x1 */ + vli_mod_square_fast(t5, t5); /* t5 = (x2 - x1)^2 = A */ + vli_mod_mult_fast(x1, x1, t5); /* t1 = x1*A = B */ + vli_mod_mult_fast(x2, x2, t5); /* t3 = x2*A = C */ + vli_mod_add(t5, y2, y1, curve_p); /* t4 = y2 + y1 */ + vli_mod_sub(y2, y2, y1, curve_p); /* t4 = y2 - y1 */ + + vli_mod_sub(t6, x2, x1, curve_p); /* t6 = C - B */ + vli_mod_mult_fast(y1, y1, t6); /* t2 = y1 * (C - B) */ + vli_mod_add(t6, x1, x2, curve_p); /* t6 = B + C */ + vli_mod_square_fast(x2, y2); /* t3 = (y2 - y1)^2 */ + vli_mod_sub(x2, x2, t6, curve_p); /* t3 = x3 */ + + vli_mod_sub(t7, x1, x2, curve_p); /* t7 = B - x3 */ + vli_mod_mult_fast(y2, y2, t7); /* t4 = (y2 - y1)*(B - x3) */ + vli_mod_sub(y2, y2, y1, curve_p); /* t4 = y3 */ + + vli_mod_square_fast(t7, t5); /* t7 = (y2 + y1)^2 = F */ + vli_mod_sub(t7, t7, t6, curve_p); /* t7 = x3' */ + vli_mod_sub(t6, t7, x1, curve_p); /* t6 = x3' - B */ + vli_mod_mult_fast(t6, t6, t5); /* t6 = (y2 + y1)*(x3' - B) */ + vli_mod_sub(y1, t6, y1, curve_p); /* t2 = y3' */ + + vli_set(x1, t7); +} + +static void ecc_point_mult(struct ecc_point *result, + const struct ecc_point *point, u64 *scalar, + u64 *initial_z, int num_bits) +{ + /* R0 and R1 */ + u64 rx[2][NUM_ECC_DIGITS]; + u64 ry[2][NUM_ECC_DIGITS]; + u64 z[NUM_ECC_DIGITS]; + int i, nb; + + vli_set(rx[1], point->x); + vli_set(ry[1], point->y); + + xycz_initial_double(rx[1], ry[1], rx[0], ry[0], initial_z); + + for (i = num_bits - 2; i > 0; i--) { + nb = !vli_test_bit(scalar, i); + xycz_add_c(rx[1 - nb], ry[1 - nb], rx[nb], ry[nb]); + xycz_add(rx[nb], ry[nb], rx[1 - nb], ry[1 - nb]); + } + + nb = !vli_test_bit(scalar, 0); + xycz_add_c(rx[1 - nb], ry[1 - nb], rx[nb], ry[nb]); + + /* Find final 1/Z value. */ + vli_mod_sub(z, rx[1], rx[0], curve_p); /* X1 - X0 */ + vli_mod_mult_fast(z, z, ry[1 - nb]); /* Yb * (X1 - X0) */ + vli_mod_mult_fast(z, z, point->x); /* xP * Yb * (X1 - X0) */ + vli_mod_inv(z, z, curve_p); /* 1 / (xP * Yb * (X1 - X0)) */ + vli_mod_mult_fast(z, z, point->y); /* yP / (xP * Yb * (X1 - X0)) */ + vli_mod_mult_fast(z, z, rx[1 - nb]); /* Xb * yP / (xP * Yb * (X1 - X0)) */ + /* End 1/Z calculation */ + + xycz_add(rx[nb], ry[nb], rx[1 - nb], ry[1 - nb]); + + apply_z(rx[0], ry[0], z); + + vli_set(result->x, rx[0]); + vli_set(result->y, ry[0]); +} + +static void ecc_bytes2native(const u8 bytes[ECC_BYTES], + u64 native[NUM_ECC_DIGITS]) +{ + int i; + + for (i = 0; i < NUM_ECC_DIGITS; i++) { + const u8 *digit = bytes + 8 * (NUM_ECC_DIGITS - 1 - i); + + native[NUM_ECC_DIGITS - 1 - i] = + ((u64) digit[0] << 0) | + ((u64) digit[1] << 8) | + ((u64) digit[2] << 16) | + ((u64) digit[3] << 24) | + ((u64) digit[4] << 32) | + ((u64) digit[5] << 40) | + ((u64) digit[6] << 48) | + ((u64) digit[7] << 56); + } +} + +static void ecc_native2bytes(const u64 native[NUM_ECC_DIGITS], + u8 bytes[ECC_BYTES]) +{ + int i; + + for (i = 0; i < NUM_ECC_DIGITS; i++) { + u8 *digit = bytes + 8 * (NUM_ECC_DIGITS - 1 - i); + + digit[0] = native[NUM_ECC_DIGITS - 1 - i] >> 0; + digit[1] = native[NUM_ECC_DIGITS - 1 - i] >> 8; + digit[2] = native[NUM_ECC_DIGITS - 1 - i] >> 16; + digit[3] = native[NUM_ECC_DIGITS - 1 - i] >> 24; + digit[4] = native[NUM_ECC_DIGITS - 1 - i] >> 32; + digit[5] = native[NUM_ECC_DIGITS - 1 - i] >> 40; + digit[6] = native[NUM_ECC_DIGITS - 1 - i] >> 48; + digit[7] = native[NUM_ECC_DIGITS - 1 - i] >> 56; + } +} + +bool ecc_make_key(u8 public_key[64], u8 private_key[32]) +{ + struct ecc_point pk; + u64 priv[NUM_ECC_DIGITS]; + unsigned int tries = 0; + + do { + if (tries++ >= MAX_TRIES) + return false; + + get_random_bytes(priv, ECC_BYTES); + + if (vli_is_zero(priv)) + continue; + + /* Make sure the private key is in the range [1, n-1]. */ + if (vli_cmp(curve_n, priv) != 1) + continue; + + ecc_point_mult(&pk, &curve_g, priv, NULL, vli_num_bits(priv)); + } while (ecc_point_is_zero(&pk)); + + ecc_native2bytes(priv, private_key); + ecc_native2bytes(pk.x, public_key); + ecc_native2bytes(pk.y, &public_key[32]); + + return true; +} + +bool ecdh_shared_secret(const u8 public_key[64], const u8 private_key[32], + u8 secret[32]) +{ + u64 priv[NUM_ECC_DIGITS]; + u64 rand[NUM_ECC_DIGITS]; + struct ecc_point product, pk; + + get_random_bytes(rand, ECC_BYTES); + + ecc_bytes2native(public_key, pk.x); + ecc_bytes2native(&public_key[32], pk.y); + ecc_bytes2native(private_key, priv); + + ecc_point_mult(&product, &pk, priv, rand, vli_num_bits(priv)); + + ecc_native2bytes(product.x, secret); + + return !ecc_point_is_zero(&product); +} diff --git a/kernel/net/bluetooth/ecc.h b/kernel/net/bluetooth/ecc.h new file mode 100644 index 000000000..8d6a2f4d1 --- /dev/null +++ b/kernel/net/bluetooth/ecc.h @@ -0,0 +1,54 @@ +/* + * Copyright (c) 2013, Kenneth MacKay + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are + * met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * 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. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS 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 COPYRIGHT + * HOLDER 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. + */ + +/* Create a public/private key pair. + * Outputs: + * public_key - Will be filled in with the public key. + * private_key - Will be filled in with the private key. + * + * Returns true if the key pair was generated successfully, false + * if an error occurred. The keys are with the LSB first. + */ +bool ecc_make_key(u8 public_key[64], u8 private_key[32]); + +/* Compute a shared secret given your secret key and someone else's + * public key. + * Note: It is recommended that you hash the result of ecdh_shared_secret + * before using it for symmetric encryption or HMAC. + * + * Inputs: + * public_key - The public key of the remote party + * private_key - Your private key. + * + * Outputs: + * secret - Will be filled in with the shared secret value. + * + * Returns true if the shared secret was generated successfully, false + * if an error occurred. Both input and output parameters are with the + * LSB first. + */ +bool ecdh_shared_secret(const u8 public_key[64], const u8 private_key[32], + u8 secret[32]); diff --git a/kernel/net/bluetooth/hci_conn.c b/kernel/net/bluetooth/hci_conn.c new file mode 100644 index 000000000..ee5e59839 --- /dev/null +++ b/kernel/net/bluetooth/hci_conn.c @@ -0,0 +1,1383 @@ +/* + BlueZ - Bluetooth protocol stack for Linux + Copyright (c) 2000-2001, 2010, Code Aurora Forum. All rights reserved. + + Written 2000,2001 by Maxim Krasnyansky <maxk@qualcomm.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; + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS + OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT OF THIRD PARTY RIGHTS. + IN NO EVENT SHALL THE COPYRIGHT HOLDER(S) AND AUTHOR(S) BE LIABLE FOR ANY + CLAIM, OR ANY SPECIAL INDIRECT OR CONSEQUENTIAL DAMAGES, OR ANY DAMAGES + WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN + ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF + OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + + ALL LIABILITY, INCLUDING LIABILITY FOR INFRINGEMENT OF ANY PATENTS, + COPYRIGHTS, TRADEMARKS OR OTHER RIGHTS, RELATING TO USE OF THIS + SOFTWARE IS DISCLAIMED. +*/ + +/* Bluetooth HCI connection handling. */ + +#include <linux/export.h> +#include <linux/debugfs.h> + +#include <net/bluetooth/bluetooth.h> +#include <net/bluetooth/hci_core.h> +#include <net/bluetooth/l2cap.h> + +#include "hci_request.h" +#include "smp.h" +#include "a2mp.h" + +struct sco_param { + u16 pkt_type; + u16 max_latency; + u8 retrans_effort; +}; + +static const struct sco_param esco_param_cvsd[] = { + { EDR_ESCO_MASK & ~ESCO_2EV3, 0x000a, 0x01 }, /* S3 */ + { EDR_ESCO_MASK & ~ESCO_2EV3, 0x0007, 0x01 }, /* S2 */ + { EDR_ESCO_MASK | ESCO_EV3, 0x0007, 0x01 }, /* S1 */ + { EDR_ESCO_MASK | ESCO_HV3, 0xffff, 0x01 }, /* D1 */ + { EDR_ESCO_MASK | ESCO_HV1, 0xffff, 0x01 }, /* D0 */ +}; + +static const struct sco_param sco_param_cvsd[] = { + { EDR_ESCO_MASK | ESCO_HV3, 0xffff, 0xff }, /* D1 */ + { EDR_ESCO_MASK | ESCO_HV1, 0xffff, 0xff }, /* D0 */ +}; + +static const struct sco_param esco_param_msbc[] = { + { EDR_ESCO_MASK & ~ESCO_2EV3, 0x000d, 0x02 }, /* T2 */ + { EDR_ESCO_MASK | ESCO_EV3, 0x0008, 0x02 }, /* T1 */ +}; + +static void hci_le_create_connection_cancel(struct hci_conn *conn) +{ + hci_send_cmd(conn->hdev, HCI_OP_LE_CREATE_CONN_CANCEL, 0, NULL); +} + +static void hci_acl_create_connection(struct hci_conn *conn) +{ + struct hci_dev *hdev = conn->hdev; + struct inquiry_entry *ie; + struct hci_cp_create_conn cp; + + BT_DBG("hcon %p", conn); + + conn->state = BT_CONNECT; + conn->out = true; + conn->role = HCI_ROLE_MASTER; + + conn->attempt++; + + conn->link_policy = hdev->link_policy; + + memset(&cp, 0, sizeof(cp)); + bacpy(&cp.bdaddr, &conn->dst); + cp.pscan_rep_mode = 0x02; + + ie = hci_inquiry_cache_lookup(hdev, &conn->dst); + if (ie) { + if (inquiry_entry_age(ie) <= INQUIRY_ENTRY_AGE_MAX) { + cp.pscan_rep_mode = ie->data.pscan_rep_mode; + cp.pscan_mode = ie->data.pscan_mode; + cp.clock_offset = ie->data.clock_offset | + cpu_to_le16(0x8000); + } + + memcpy(conn->dev_class, ie->data.dev_class, 3); + if (ie->data.ssp_mode > 0) + set_bit(HCI_CONN_SSP_ENABLED, &conn->flags); + } + + cp.pkt_type = cpu_to_le16(conn->pkt_type); + if (lmp_rswitch_capable(hdev) && !(hdev->link_mode & HCI_LM_MASTER)) + cp.role_switch = 0x01; + else + cp.role_switch = 0x00; + + hci_send_cmd(hdev, HCI_OP_CREATE_CONN, sizeof(cp), &cp); +} + +static void hci_acl_create_connection_cancel(struct hci_conn *conn) +{ + struct hci_cp_create_conn_cancel cp; + + BT_DBG("hcon %p", conn); + + if (conn->hdev->hci_ver < BLUETOOTH_VER_1_2) + return; + + bacpy(&cp.bdaddr, &conn->dst); + hci_send_cmd(conn->hdev, HCI_OP_CREATE_CONN_CANCEL, sizeof(cp), &cp); +} + +static void hci_reject_sco(struct hci_conn *conn) +{ + struct hci_cp_reject_sync_conn_req cp; + + cp.reason = HCI_ERROR_REJ_LIMITED_RESOURCES; + bacpy(&cp.bdaddr, &conn->dst); + + hci_send_cmd(conn->hdev, HCI_OP_REJECT_SYNC_CONN_REQ, sizeof(cp), &cp); +} + +int hci_disconnect(struct hci_conn *conn, __u8 reason) +{ + struct hci_cp_disconnect cp; + + BT_DBG("hcon %p", conn); + + /* When we are master of an established connection and it enters + * the disconnect timeout, then go ahead and try to read the + * current clock offset. Processing of the result is done + * within the event handling and hci_clock_offset_evt function. + */ + if (conn->type == ACL_LINK && conn->role == HCI_ROLE_MASTER) { + struct hci_dev *hdev = conn->hdev; + struct hci_cp_read_clock_offset clkoff_cp; + + clkoff_cp.handle = cpu_to_le16(conn->handle); + hci_send_cmd(hdev, HCI_OP_READ_CLOCK_OFFSET, sizeof(clkoff_cp), + &clkoff_cp); + } + + conn->state = BT_DISCONN; + + cp.handle = cpu_to_le16(conn->handle); + cp.reason = reason; + return hci_send_cmd(conn->hdev, HCI_OP_DISCONNECT, sizeof(cp), &cp); +} + +static void hci_amp_disconn(struct hci_conn *conn) +{ + struct hci_cp_disconn_phy_link cp; + + BT_DBG("hcon %p", conn); + + conn->state = BT_DISCONN; + + cp.phy_handle = HCI_PHY_HANDLE(conn->handle); + cp.reason = hci_proto_disconn_ind(conn); + hci_send_cmd(conn->hdev, HCI_OP_DISCONN_PHY_LINK, + sizeof(cp), &cp); +} + +static void hci_add_sco(struct hci_conn *conn, __u16 handle) +{ + struct hci_dev *hdev = conn->hdev; + struct hci_cp_add_sco cp; + + BT_DBG("hcon %p", conn); + + conn->state = BT_CONNECT; + conn->out = true; + + conn->attempt++; + + cp.handle = cpu_to_le16(handle); + cp.pkt_type = cpu_to_le16(conn->pkt_type); + + hci_send_cmd(hdev, HCI_OP_ADD_SCO, sizeof(cp), &cp); +} + +bool hci_setup_sync(struct hci_conn *conn, __u16 handle) +{ + struct hci_dev *hdev = conn->hdev; + struct hci_cp_setup_sync_conn cp; + const struct sco_param *param; + + BT_DBG("hcon %p", conn); + + conn->state = BT_CONNECT; + conn->out = true; + + conn->attempt++; + + cp.handle = cpu_to_le16(handle); + + cp.tx_bandwidth = cpu_to_le32(0x00001f40); + cp.rx_bandwidth = cpu_to_le32(0x00001f40); + cp.voice_setting = cpu_to_le16(conn->setting); + + switch (conn->setting & SCO_AIRMODE_MASK) { + case SCO_AIRMODE_TRANSP: + if (conn->attempt > ARRAY_SIZE(esco_param_msbc)) + return false; + param = &esco_param_msbc[conn->attempt - 1]; + break; + case SCO_AIRMODE_CVSD: + if (lmp_esco_capable(conn->link)) { + if (conn->attempt > ARRAY_SIZE(esco_param_cvsd)) + return false; + param = &esco_param_cvsd[conn->attempt - 1]; + } else { + if (conn->attempt > ARRAY_SIZE(sco_param_cvsd)) + return false; + param = &sco_param_cvsd[conn->attempt - 1]; + } + break; + default: + return false; + } + + cp.retrans_effort = param->retrans_effort; + cp.pkt_type = __cpu_to_le16(param->pkt_type); + cp.max_latency = __cpu_to_le16(param->max_latency); + + if (hci_send_cmd(hdev, HCI_OP_SETUP_SYNC_CONN, sizeof(cp), &cp) < 0) + return false; + + return true; +} + +u8 hci_le_conn_update(struct hci_conn *conn, u16 min, u16 max, u16 latency, + u16 to_multiplier) +{ + struct hci_dev *hdev = conn->hdev; + struct hci_conn_params *params; + struct hci_cp_le_conn_update cp; + + hci_dev_lock(hdev); + + params = hci_conn_params_lookup(hdev, &conn->dst, conn->dst_type); + if (params) { + params->conn_min_interval = min; + params->conn_max_interval = max; + params->conn_latency = latency; + params->supervision_timeout = to_multiplier; + } + + hci_dev_unlock(hdev); + + memset(&cp, 0, sizeof(cp)); + cp.handle = cpu_to_le16(conn->handle); + cp.conn_interval_min = cpu_to_le16(min); + cp.conn_interval_max = cpu_to_le16(max); + cp.conn_latency = cpu_to_le16(latency); + cp.supervision_timeout = cpu_to_le16(to_multiplier); + cp.min_ce_len = cpu_to_le16(0x0000); + cp.max_ce_len = cpu_to_le16(0x0000); + + hci_send_cmd(hdev, HCI_OP_LE_CONN_UPDATE, sizeof(cp), &cp); + + if (params) + return 0x01; + + return 0x00; +} + +void hci_le_start_enc(struct hci_conn *conn, __le16 ediv, __le64 rand, + __u8 ltk[16]) +{ + struct hci_dev *hdev = conn->hdev; + struct hci_cp_le_start_enc cp; + + BT_DBG("hcon %p", conn); + + memset(&cp, 0, sizeof(cp)); + + cp.handle = cpu_to_le16(conn->handle); + cp.rand = rand; + cp.ediv = ediv; + memcpy(cp.ltk, ltk, sizeof(cp.ltk)); + + hci_send_cmd(hdev, HCI_OP_LE_START_ENC, sizeof(cp), &cp); +} + +/* Device _must_ be locked */ +void hci_sco_setup(struct hci_conn *conn, __u8 status) +{ + struct hci_conn *sco = conn->link; + + if (!sco) + return; + + BT_DBG("hcon %p", conn); + + if (!status) { + if (lmp_esco_capable(conn->hdev)) + hci_setup_sync(sco, conn->handle); + else + hci_add_sco(sco, conn->handle); + } else { + hci_connect_cfm(sco, status); + hci_conn_del(sco); + } +} + +static void hci_conn_timeout(struct work_struct *work) +{ + struct hci_conn *conn = container_of(work, struct hci_conn, + disc_work.work); + int refcnt = atomic_read(&conn->refcnt); + + BT_DBG("hcon %p state %s", conn, state_to_string(conn->state)); + + WARN_ON(refcnt < 0); + + /* FIXME: It was observed that in pairing failed scenario, refcnt + * drops below 0. Probably this is because l2cap_conn_del calls + * l2cap_chan_del for each channel, and inside l2cap_chan_del conn is + * dropped. After that loop hci_chan_del is called which also drops + * conn. For now make sure that ACL is alive if refcnt is higher then 0, + * otherwise drop it. + */ + if (refcnt > 0) + return; + + switch (conn->state) { + case BT_CONNECT: + case BT_CONNECT2: + if (conn->out) { + if (conn->type == ACL_LINK) + hci_acl_create_connection_cancel(conn); + else if (conn->type == LE_LINK) + hci_le_create_connection_cancel(conn); + } else if (conn->type == SCO_LINK || conn->type == ESCO_LINK) { + hci_reject_sco(conn); + } + break; + case BT_CONFIG: + case BT_CONNECTED: + if (conn->type == AMP_LINK) { + hci_amp_disconn(conn); + } else { + __u8 reason = hci_proto_disconn_ind(conn); + hci_disconnect(conn, reason); + } + break; + default: + conn->state = BT_CLOSED; + break; + } +} + +/* Enter sniff mode */ +static void hci_conn_idle(struct work_struct *work) +{ + struct hci_conn *conn = container_of(work, struct hci_conn, + idle_work.work); + struct hci_dev *hdev = conn->hdev; + + BT_DBG("hcon %p mode %d", conn, conn->mode); + + if (!lmp_sniff_capable(hdev) || !lmp_sniff_capable(conn)) + return; + + if (conn->mode != HCI_CM_ACTIVE || !(conn->link_policy & HCI_LP_SNIFF)) + return; + + if (lmp_sniffsubr_capable(hdev) && lmp_sniffsubr_capable(conn)) { + struct hci_cp_sniff_subrate cp; + cp.handle = cpu_to_le16(conn->handle); + cp.max_latency = cpu_to_le16(0); + cp.min_remote_timeout = cpu_to_le16(0); + cp.min_local_timeout = cpu_to_le16(0); + hci_send_cmd(hdev, HCI_OP_SNIFF_SUBRATE, sizeof(cp), &cp); + } + + if (!test_and_set_bit(HCI_CONN_MODE_CHANGE_PEND, &conn->flags)) { + struct hci_cp_sniff_mode cp; + cp.handle = cpu_to_le16(conn->handle); + cp.max_interval = cpu_to_le16(hdev->sniff_max_interval); + cp.min_interval = cpu_to_le16(hdev->sniff_min_interval); + cp.attempt = cpu_to_le16(4); + cp.timeout = cpu_to_le16(1); + hci_send_cmd(hdev, HCI_OP_SNIFF_MODE, sizeof(cp), &cp); + } +} + +static void hci_conn_auto_accept(struct work_struct *work) +{ + struct hci_conn *conn = container_of(work, struct hci_conn, + auto_accept_work.work); + + hci_send_cmd(conn->hdev, HCI_OP_USER_CONFIRM_REPLY, sizeof(conn->dst), + &conn->dst); +} + +static void le_conn_timeout(struct work_struct *work) +{ + struct hci_conn *conn = container_of(work, struct hci_conn, + le_conn_timeout.work); + struct hci_dev *hdev = conn->hdev; + + BT_DBG(""); + + /* We could end up here due to having done directed advertising, + * so clean up the state if necessary. This should however only + * happen with broken hardware or if low duty cycle was used + * (which doesn't have a timeout of its own). + */ + if (conn->role == HCI_ROLE_SLAVE) { + u8 enable = 0x00; + hci_send_cmd(hdev, HCI_OP_LE_SET_ADV_ENABLE, sizeof(enable), + &enable); + hci_le_conn_failed(conn, HCI_ERROR_ADVERTISING_TIMEOUT); + return; + } + + hci_le_create_connection_cancel(conn); +} + +struct hci_conn *hci_conn_add(struct hci_dev *hdev, int type, bdaddr_t *dst, + u8 role) +{ + struct hci_conn *conn; + + BT_DBG("%s dst %pMR", hdev->name, dst); + + conn = kzalloc(sizeof(*conn), GFP_KERNEL); + if (!conn) + return NULL; + + bacpy(&conn->dst, dst); + bacpy(&conn->src, &hdev->bdaddr); + conn->hdev = hdev; + conn->type = type; + conn->role = role; + conn->mode = HCI_CM_ACTIVE; + conn->state = BT_OPEN; + conn->auth_type = HCI_AT_GENERAL_BONDING; + conn->io_capability = hdev->io_capability; + conn->remote_auth = 0xff; + conn->key_type = 0xff; + conn->rssi = HCI_RSSI_INVALID; + conn->tx_power = HCI_TX_POWER_INVALID; + conn->max_tx_power = HCI_TX_POWER_INVALID; + + set_bit(HCI_CONN_POWER_SAVE, &conn->flags); + conn->disc_timeout = HCI_DISCONN_TIMEOUT; + + if (conn->role == HCI_ROLE_MASTER) + conn->out = true; + + switch (type) { + case ACL_LINK: + conn->pkt_type = hdev->pkt_type & ACL_PTYPE_MASK; + break; + case LE_LINK: + /* conn->src should reflect the local identity address */ + hci_copy_identity_address(hdev, &conn->src, &conn->src_type); + break; + case SCO_LINK: + if (lmp_esco_capable(hdev)) + conn->pkt_type = (hdev->esco_type & SCO_ESCO_MASK) | + (hdev->esco_type & EDR_ESCO_MASK); + else + conn->pkt_type = hdev->pkt_type & SCO_PTYPE_MASK; + break; + case ESCO_LINK: + conn->pkt_type = hdev->esco_type & ~EDR_ESCO_MASK; + break; + } + + skb_queue_head_init(&conn->data_q); + + INIT_LIST_HEAD(&conn->chan_list); + + INIT_DELAYED_WORK(&conn->disc_work, hci_conn_timeout); + INIT_DELAYED_WORK(&conn->auto_accept_work, hci_conn_auto_accept); + INIT_DELAYED_WORK(&conn->idle_work, hci_conn_idle); + INIT_DELAYED_WORK(&conn->le_conn_timeout, le_conn_timeout); + + atomic_set(&conn->refcnt, 0); + + hci_dev_hold(hdev); + + hci_conn_hash_add(hdev, conn); + if (hdev->notify) + hdev->notify(hdev, HCI_NOTIFY_CONN_ADD); + + hci_conn_init_sysfs(conn); + + return conn; +} + +int hci_conn_del(struct hci_conn *conn) +{ + struct hci_dev *hdev = conn->hdev; + + BT_DBG("%s hcon %p handle %d", hdev->name, conn, conn->handle); + + cancel_delayed_work_sync(&conn->disc_work); + cancel_delayed_work_sync(&conn->auto_accept_work); + cancel_delayed_work_sync(&conn->idle_work); + + if (conn->type == ACL_LINK) { + struct hci_conn *sco = conn->link; + if (sco) + sco->link = NULL; + + /* Unacked frames */ + hdev->acl_cnt += conn->sent; + } else if (conn->type == LE_LINK) { + cancel_delayed_work(&conn->le_conn_timeout); + + if (hdev->le_pkts) + hdev->le_cnt += conn->sent; + else + hdev->acl_cnt += conn->sent; + } else { + struct hci_conn *acl = conn->link; + if (acl) { + acl->link = NULL; + hci_conn_drop(acl); + } + } + + hci_chan_list_flush(conn); + + if (conn->amp_mgr) + amp_mgr_put(conn->amp_mgr); + + hci_conn_hash_del(hdev, conn); + if (hdev->notify) + hdev->notify(hdev, HCI_NOTIFY_CONN_DEL); + + skb_queue_purge(&conn->data_q); + + hci_conn_del_sysfs(conn); + + debugfs_remove_recursive(conn->debugfs); + + if (test_bit(HCI_CONN_PARAM_REMOVAL_PEND, &conn->flags)) + hci_conn_params_del(conn->hdev, &conn->dst, conn->dst_type); + + hci_dev_put(hdev); + + hci_conn_put(conn); + + return 0; +} + +struct hci_dev *hci_get_route(bdaddr_t *dst, bdaddr_t *src) +{ + int use_src = bacmp(src, BDADDR_ANY); + struct hci_dev *hdev = NULL, *d; + + BT_DBG("%pMR -> %pMR", src, dst); + + read_lock(&hci_dev_list_lock); + + list_for_each_entry(d, &hci_dev_list, list) { + if (!test_bit(HCI_UP, &d->flags) || + hci_dev_test_flag(d, HCI_USER_CHANNEL) || + d->dev_type != HCI_BREDR) + continue; + + /* Simple routing: + * No source address - find interface with bdaddr != dst + * Source address - find interface with bdaddr == src + */ + + if (use_src) { + if (!bacmp(&d->bdaddr, src)) { + hdev = d; break; + } + } else { + if (bacmp(&d->bdaddr, dst)) { + hdev = d; break; + } + } + } + + if (hdev) + hdev = hci_dev_hold(hdev); + + read_unlock(&hci_dev_list_lock); + return hdev; +} +EXPORT_SYMBOL(hci_get_route); + +/* This function requires the caller holds hdev->lock */ +void hci_le_conn_failed(struct hci_conn *conn, u8 status) +{ + struct hci_dev *hdev = conn->hdev; + struct hci_conn_params *params; + + params = hci_pend_le_action_lookup(&hdev->pend_le_conns, &conn->dst, + conn->dst_type); + if (params && params->conn) { + hci_conn_drop(params->conn); + hci_conn_put(params->conn); + params->conn = NULL; + } + + conn->state = BT_CLOSED; + + mgmt_connect_failed(hdev, &conn->dst, conn->type, conn->dst_type, + status); + + hci_connect_cfm(conn, status); + + hci_conn_del(conn); + + /* Since we may have temporarily stopped the background scanning in + * favor of connection establishment, we should restart it. + */ + hci_update_background_scan(hdev); + + /* Re-enable advertising in case this was a failed connection + * attempt as a peripheral. + */ + mgmt_reenable_advertising(hdev); +} + +static void create_le_conn_complete(struct hci_dev *hdev, u8 status, u16 opcode) +{ + struct hci_conn *conn; + + if (status == 0) + return; + + BT_ERR("HCI request failed to create LE connection: status 0x%2.2x", + status); + + hci_dev_lock(hdev); + + conn = hci_conn_hash_lookup_state(hdev, LE_LINK, BT_CONNECT); + if (!conn) + goto done; + + hci_le_conn_failed(conn, status); + +done: + hci_dev_unlock(hdev); +} + +static void hci_req_add_le_create_conn(struct hci_request *req, + struct hci_conn *conn) +{ + struct hci_cp_le_create_conn cp; + struct hci_dev *hdev = conn->hdev; + u8 own_addr_type; + + memset(&cp, 0, sizeof(cp)); + + /* Update random address, but set require_privacy to false so + * that we never connect with an non-resolvable address. + */ + if (hci_update_random_address(req, false, &own_addr_type)) + return; + + cp.scan_interval = cpu_to_le16(hdev->le_scan_interval); + cp.scan_window = cpu_to_le16(hdev->le_scan_window); + bacpy(&cp.peer_addr, &conn->dst); + cp.peer_addr_type = conn->dst_type; + cp.own_address_type = own_addr_type; + cp.conn_interval_min = cpu_to_le16(conn->le_conn_min_interval); + cp.conn_interval_max = cpu_to_le16(conn->le_conn_max_interval); + cp.conn_latency = cpu_to_le16(conn->le_conn_latency); + cp.supervision_timeout = cpu_to_le16(conn->le_supv_timeout); + cp.min_ce_len = cpu_to_le16(0x0000); + cp.max_ce_len = cpu_to_le16(0x0000); + + hci_req_add(req, HCI_OP_LE_CREATE_CONN, sizeof(cp), &cp); + + conn->state = BT_CONNECT; +} + +static void hci_req_directed_advertising(struct hci_request *req, + struct hci_conn *conn) +{ + struct hci_dev *hdev = req->hdev; + struct hci_cp_le_set_adv_param cp; + u8 own_addr_type; + u8 enable; + + /* Clear the HCI_LE_ADV bit temporarily so that the + * hci_update_random_address knows that it's safe to go ahead + * and write a new random address. The flag will be set back on + * as soon as the SET_ADV_ENABLE HCI command completes. + */ + hci_dev_clear_flag(hdev, HCI_LE_ADV); + + /* Set require_privacy to false so that the remote device has a + * chance of identifying us. + */ + if (hci_update_random_address(req, false, &own_addr_type) < 0) + return; + + memset(&cp, 0, sizeof(cp)); + cp.type = LE_ADV_DIRECT_IND; + cp.own_address_type = own_addr_type; + cp.direct_addr_type = conn->dst_type; + bacpy(&cp.direct_addr, &conn->dst); + cp.channel_map = hdev->le_adv_channel_map; + + hci_req_add(req, HCI_OP_LE_SET_ADV_PARAM, sizeof(cp), &cp); + + enable = 0x01; + hci_req_add(req, HCI_OP_LE_SET_ADV_ENABLE, sizeof(enable), &enable); + + conn->state = BT_CONNECT; +} + +struct hci_conn *hci_connect_le(struct hci_dev *hdev, bdaddr_t *dst, + u8 dst_type, u8 sec_level, u16 conn_timeout, + u8 role) +{ + struct hci_conn_params *params; + struct hci_conn *conn; + struct smp_irk *irk; + struct hci_request req; + int err; + + /* Let's make sure that le is enabled.*/ + if (!hci_dev_test_flag(hdev, HCI_LE_ENABLED)) { + if (lmp_le_capable(hdev)) + return ERR_PTR(-ECONNREFUSED); + + return ERR_PTR(-EOPNOTSUPP); + } + + /* Some devices send ATT messages as soon as the physical link is + * established. To be able to handle these ATT messages, the user- + * space first establishes the connection and then starts the pairing + * process. + * + * So if a hci_conn object already exists for the following connection + * attempt, we simply update pending_sec_level and auth_type fields + * and return the object found. + */ + conn = hci_conn_hash_lookup_ba(hdev, LE_LINK, dst); + if (conn) { + conn->pending_sec_level = sec_level; + goto done; + } + + /* Since the controller supports only one LE connection attempt at a + * time, we return -EBUSY if there is any connection attempt running. + */ + conn = hci_conn_hash_lookup_state(hdev, LE_LINK, BT_CONNECT); + if (conn) + return ERR_PTR(-EBUSY); + + /* When given an identity address with existing identity + * resolving key, the connection needs to be established + * to a resolvable random address. + * + * This uses the cached random resolvable address from + * a previous scan. When no cached address is available, + * try connecting to the identity address instead. + * + * Storing the resolvable random address is required here + * to handle connection failures. The address will later + * be resolved back into the original identity address + * from the connect request. + */ + irk = hci_find_irk_by_addr(hdev, dst, dst_type); + if (irk && bacmp(&irk->rpa, BDADDR_ANY)) { + dst = &irk->rpa; + dst_type = ADDR_LE_DEV_RANDOM; + } + + conn = hci_conn_add(hdev, LE_LINK, dst, role); + if (!conn) + return ERR_PTR(-ENOMEM); + + conn->dst_type = dst_type; + conn->sec_level = BT_SECURITY_LOW; + conn->pending_sec_level = sec_level; + conn->conn_timeout = conn_timeout; + + hci_req_init(&req, hdev); + + /* Disable advertising if we're active. For master role + * connections most controllers will refuse to connect if + * advertising is enabled, and for slave role connections we + * anyway have to disable it in order to start directed + * advertising. + */ + if (hci_dev_test_flag(hdev, HCI_LE_ADV)) { + u8 enable = 0x00; + hci_req_add(&req, HCI_OP_LE_SET_ADV_ENABLE, sizeof(enable), + &enable); + } + + /* If requested to connect as slave use directed advertising */ + if (conn->role == HCI_ROLE_SLAVE) { + /* If we're active scanning most controllers are unable + * to initiate advertising. Simply reject the attempt. + */ + if (hci_dev_test_flag(hdev, HCI_LE_SCAN) && + hdev->le_scan_type == LE_SCAN_ACTIVE) { + skb_queue_purge(&req.cmd_q); + hci_conn_del(conn); + return ERR_PTR(-EBUSY); + } + + hci_req_directed_advertising(&req, conn); + goto create_conn; + } + + params = hci_conn_params_lookup(hdev, &conn->dst, conn->dst_type); + if (params) { + conn->le_conn_min_interval = params->conn_min_interval; + conn->le_conn_max_interval = params->conn_max_interval; + conn->le_conn_latency = params->conn_latency; + conn->le_supv_timeout = params->supervision_timeout; + } else { + conn->le_conn_min_interval = hdev->le_conn_min_interval; + conn->le_conn_max_interval = hdev->le_conn_max_interval; + conn->le_conn_latency = hdev->le_conn_latency; + conn->le_supv_timeout = hdev->le_supv_timeout; + } + + /* If controller is scanning, we stop it since some controllers are + * not able to scan and connect at the same time. Also set the + * HCI_LE_SCAN_INTERRUPTED flag so that the command complete + * handler for scan disabling knows to set the correct discovery + * state. + */ + if (hci_dev_test_flag(hdev, HCI_LE_SCAN)) { + hci_req_add_le_scan_disable(&req); + hci_dev_set_flag(hdev, HCI_LE_SCAN_INTERRUPTED); + } + + hci_req_add_le_create_conn(&req, conn); + +create_conn: + err = hci_req_run(&req, create_le_conn_complete); + if (err) { + hci_conn_del(conn); + return ERR_PTR(err); + } + +done: + hci_conn_hold(conn); + return conn; +} + +struct hci_conn *hci_connect_acl(struct hci_dev *hdev, bdaddr_t *dst, + u8 sec_level, u8 auth_type) +{ + struct hci_conn *acl; + + if (!hci_dev_test_flag(hdev, HCI_BREDR_ENABLED)) { + if (lmp_bredr_capable(hdev)) + return ERR_PTR(-ECONNREFUSED); + + return ERR_PTR(-EOPNOTSUPP); + } + + acl = hci_conn_hash_lookup_ba(hdev, ACL_LINK, dst); + if (!acl) { + acl = hci_conn_add(hdev, ACL_LINK, dst, HCI_ROLE_MASTER); + if (!acl) + return ERR_PTR(-ENOMEM); + } + + hci_conn_hold(acl); + + if (acl->state == BT_OPEN || acl->state == BT_CLOSED) { + acl->sec_level = BT_SECURITY_LOW; + acl->pending_sec_level = sec_level; + acl->auth_type = auth_type; + hci_acl_create_connection(acl); + } + + return acl; +} + +struct hci_conn *hci_connect_sco(struct hci_dev *hdev, int type, bdaddr_t *dst, + __u16 setting) +{ + struct hci_conn *acl; + struct hci_conn *sco; + + acl = hci_connect_acl(hdev, dst, BT_SECURITY_LOW, HCI_AT_NO_BONDING); + if (IS_ERR(acl)) + return acl; + + sco = hci_conn_hash_lookup_ba(hdev, type, dst); + if (!sco) { + sco = hci_conn_add(hdev, type, dst, HCI_ROLE_MASTER); + if (!sco) { + hci_conn_drop(acl); + return ERR_PTR(-ENOMEM); + } + } + + acl->link = sco; + sco->link = acl; + + hci_conn_hold(sco); + + sco->setting = setting; + + if (acl->state == BT_CONNECTED && + (sco->state == BT_OPEN || sco->state == BT_CLOSED)) { + set_bit(HCI_CONN_POWER_SAVE, &acl->flags); + hci_conn_enter_active_mode(acl, BT_POWER_FORCE_ACTIVE_ON); + + if (test_bit(HCI_CONN_MODE_CHANGE_PEND, &acl->flags)) { + /* defer SCO setup until mode change completed */ + set_bit(HCI_CONN_SCO_SETUP_PEND, &acl->flags); + return sco; + } + + hci_sco_setup(acl, 0x00); + } + + return sco; +} + +/* Check link security requirement */ +int hci_conn_check_link_mode(struct hci_conn *conn) +{ + BT_DBG("hcon %p", conn); + + /* In Secure Connections Only mode, it is required that Secure + * Connections is used and the link is encrypted with AES-CCM + * using a P-256 authenticated combination key. + */ + if (hci_dev_test_flag(conn->hdev, HCI_SC_ONLY)) { + if (!hci_conn_sc_enabled(conn) || + !test_bit(HCI_CONN_AES_CCM, &conn->flags) || + conn->key_type != HCI_LK_AUTH_COMBINATION_P256) + return 0; + } + + if (hci_conn_ssp_enabled(conn) && + !test_bit(HCI_CONN_ENCRYPT, &conn->flags)) + return 0; + + return 1; +} + +/* Authenticate remote device */ +static int hci_conn_auth(struct hci_conn *conn, __u8 sec_level, __u8 auth_type) +{ + BT_DBG("hcon %p", conn); + + if (conn->pending_sec_level > sec_level) + sec_level = conn->pending_sec_level; + + if (sec_level > conn->sec_level) + conn->pending_sec_level = sec_level; + else if (test_bit(HCI_CONN_AUTH, &conn->flags)) + return 1; + + /* Make sure we preserve an existing MITM requirement*/ + auth_type |= (conn->auth_type & 0x01); + + conn->auth_type = auth_type; + + if (!test_and_set_bit(HCI_CONN_AUTH_PEND, &conn->flags)) { + struct hci_cp_auth_requested cp; + + cp.handle = cpu_to_le16(conn->handle); + hci_send_cmd(conn->hdev, HCI_OP_AUTH_REQUESTED, + sizeof(cp), &cp); + + /* If we're already encrypted set the REAUTH_PEND flag, + * otherwise set the ENCRYPT_PEND. + */ + if (test_bit(HCI_CONN_ENCRYPT, &conn->flags)) + set_bit(HCI_CONN_REAUTH_PEND, &conn->flags); + else + set_bit(HCI_CONN_ENCRYPT_PEND, &conn->flags); + } + + return 0; +} + +/* Encrypt the the link */ +static void hci_conn_encrypt(struct hci_conn *conn) +{ + BT_DBG("hcon %p", conn); + + if (!test_and_set_bit(HCI_CONN_ENCRYPT_PEND, &conn->flags)) { + struct hci_cp_set_conn_encrypt cp; + cp.handle = cpu_to_le16(conn->handle); + cp.encrypt = 0x01; + hci_send_cmd(conn->hdev, HCI_OP_SET_CONN_ENCRYPT, sizeof(cp), + &cp); + } +} + +/* Enable security */ +int hci_conn_security(struct hci_conn *conn, __u8 sec_level, __u8 auth_type, + bool initiator) +{ + BT_DBG("hcon %p", conn); + + if (conn->type == LE_LINK) + return smp_conn_security(conn, sec_level); + + /* For sdp we don't need the link key. */ + if (sec_level == BT_SECURITY_SDP) + return 1; + + /* For non 2.1 devices and low security level we don't need the link + key. */ + if (sec_level == BT_SECURITY_LOW && !hci_conn_ssp_enabled(conn)) + return 1; + + /* For other security levels we need the link key. */ + if (!test_bit(HCI_CONN_AUTH, &conn->flags)) + goto auth; + + /* An authenticated FIPS approved combination key has sufficient + * security for security level 4. */ + if (conn->key_type == HCI_LK_AUTH_COMBINATION_P256 && + sec_level == BT_SECURITY_FIPS) + goto encrypt; + + /* An authenticated combination key has sufficient security for + security level 3. */ + if ((conn->key_type == HCI_LK_AUTH_COMBINATION_P192 || + conn->key_type == HCI_LK_AUTH_COMBINATION_P256) && + sec_level == BT_SECURITY_HIGH) + goto encrypt; + + /* An unauthenticated combination key has sufficient security for + security level 1 and 2. */ + if ((conn->key_type == HCI_LK_UNAUTH_COMBINATION_P192 || + conn->key_type == HCI_LK_UNAUTH_COMBINATION_P256) && + (sec_level == BT_SECURITY_MEDIUM || sec_level == BT_SECURITY_LOW)) + goto encrypt; + + /* A combination key has always sufficient security for the security + levels 1 or 2. High security level requires the combination key + is generated using maximum PIN code length (16). + For pre 2.1 units. */ + if (conn->key_type == HCI_LK_COMBINATION && + (sec_level == BT_SECURITY_MEDIUM || sec_level == BT_SECURITY_LOW || + conn->pin_length == 16)) + goto encrypt; + +auth: + if (test_bit(HCI_CONN_ENCRYPT_PEND, &conn->flags)) + return 0; + + if (initiator) + set_bit(HCI_CONN_AUTH_INITIATOR, &conn->flags); + + if (!hci_conn_auth(conn, sec_level, auth_type)) + return 0; + +encrypt: + if (test_bit(HCI_CONN_ENCRYPT, &conn->flags)) + return 1; + + hci_conn_encrypt(conn); + return 0; +} +EXPORT_SYMBOL(hci_conn_security); + +/* Check secure link requirement */ +int hci_conn_check_secure(struct hci_conn *conn, __u8 sec_level) +{ + BT_DBG("hcon %p", conn); + + /* Accept if non-secure or higher security level is required */ + if (sec_level != BT_SECURITY_HIGH && sec_level != BT_SECURITY_FIPS) + return 1; + + /* Accept if secure or higher security level is already present */ + if (conn->sec_level == BT_SECURITY_HIGH || + conn->sec_level == BT_SECURITY_FIPS) + return 1; + + /* Reject not secure link */ + return 0; +} +EXPORT_SYMBOL(hci_conn_check_secure); + +/* Switch role */ +int hci_conn_switch_role(struct hci_conn *conn, __u8 role) +{ + BT_DBG("hcon %p", conn); + + if (role == conn->role) + return 1; + + if (!test_and_set_bit(HCI_CONN_RSWITCH_PEND, &conn->flags)) { + struct hci_cp_switch_role cp; + bacpy(&cp.bdaddr, &conn->dst); + cp.role = role; + hci_send_cmd(conn->hdev, HCI_OP_SWITCH_ROLE, sizeof(cp), &cp); + } + + return 0; +} +EXPORT_SYMBOL(hci_conn_switch_role); + +/* Enter active mode */ +void hci_conn_enter_active_mode(struct hci_conn *conn, __u8 force_active) +{ + struct hci_dev *hdev = conn->hdev; + + BT_DBG("hcon %p mode %d", conn, conn->mode); + + if (conn->mode != HCI_CM_SNIFF) + goto timer; + + if (!test_bit(HCI_CONN_POWER_SAVE, &conn->flags) && !force_active) + goto timer; + + if (!test_and_set_bit(HCI_CONN_MODE_CHANGE_PEND, &conn->flags)) { + struct hci_cp_exit_sniff_mode cp; + cp.handle = cpu_to_le16(conn->handle); + hci_send_cmd(hdev, HCI_OP_EXIT_SNIFF_MODE, sizeof(cp), &cp); + } + +timer: + if (hdev->idle_timeout > 0) + queue_delayed_work(hdev->workqueue, &conn->idle_work, + msecs_to_jiffies(hdev->idle_timeout)); +} + +/* Drop all connection on the device */ +void hci_conn_hash_flush(struct hci_dev *hdev) +{ + struct hci_conn_hash *h = &hdev->conn_hash; + struct hci_conn *c, *n; + + BT_DBG("hdev %s", hdev->name); + + list_for_each_entry_safe(c, n, &h->list, list) { + c->state = BT_CLOSED; + + hci_disconn_cfm(c, HCI_ERROR_LOCAL_HOST_TERM); + hci_conn_del(c); + } +} + +/* Check pending connect attempts */ +void hci_conn_check_pending(struct hci_dev *hdev) +{ + struct hci_conn *conn; + + BT_DBG("hdev %s", hdev->name); + + hci_dev_lock(hdev); + + conn = hci_conn_hash_lookup_state(hdev, ACL_LINK, BT_CONNECT2); + if (conn) + hci_acl_create_connection(conn); + + hci_dev_unlock(hdev); +} + +static u32 get_link_mode(struct hci_conn *conn) +{ + u32 link_mode = 0; + + if (conn->role == HCI_ROLE_MASTER) + link_mode |= HCI_LM_MASTER; + + if (test_bit(HCI_CONN_ENCRYPT, &conn->flags)) + link_mode |= HCI_LM_ENCRYPT; + + if (test_bit(HCI_CONN_AUTH, &conn->flags)) + link_mode |= HCI_LM_AUTH; + + if (test_bit(HCI_CONN_SECURE, &conn->flags)) + link_mode |= HCI_LM_SECURE; + + if (test_bit(HCI_CONN_FIPS, &conn->flags)) + link_mode |= HCI_LM_FIPS; + + return link_mode; +} + +int hci_get_conn_list(void __user *arg) +{ + struct hci_conn *c; + struct hci_conn_list_req req, *cl; + struct hci_conn_info *ci; + struct hci_dev *hdev; + int n = 0, size, err; + + if (copy_from_user(&req, arg, sizeof(req))) + return -EFAULT; + + if (!req.conn_num || req.conn_num > (PAGE_SIZE * 2) / sizeof(*ci)) + return -EINVAL; + + size = sizeof(req) + req.conn_num * sizeof(*ci); + + cl = kmalloc(size, GFP_KERNEL); + if (!cl) + return -ENOMEM; + + hdev = hci_dev_get(req.dev_id); + if (!hdev) { + kfree(cl); + return -ENODEV; + } + + ci = cl->conn_info; + + hci_dev_lock(hdev); + list_for_each_entry(c, &hdev->conn_hash.list, list) { + bacpy(&(ci + n)->bdaddr, &c->dst); + (ci + n)->handle = c->handle; + (ci + n)->type = c->type; + (ci + n)->out = c->out; + (ci + n)->state = c->state; + (ci + n)->link_mode = get_link_mode(c); + if (++n >= req.conn_num) + break; + } + hci_dev_unlock(hdev); + + cl->dev_id = hdev->id; + cl->conn_num = n; + size = sizeof(req) + n * sizeof(*ci); + + hci_dev_put(hdev); + + err = copy_to_user(arg, cl, size); + kfree(cl); + + return err ? -EFAULT : 0; +} + +int hci_get_conn_info(struct hci_dev *hdev, void __user *arg) +{ + struct hci_conn_info_req req; + struct hci_conn_info ci; + struct hci_conn *conn; + char __user *ptr = arg + sizeof(req); + + if (copy_from_user(&req, arg, sizeof(req))) + return -EFAULT; + + hci_dev_lock(hdev); + conn = hci_conn_hash_lookup_ba(hdev, req.type, &req.bdaddr); + if (conn) { + bacpy(&ci.bdaddr, &conn->dst); + ci.handle = conn->handle; + ci.type = conn->type; + ci.out = conn->out; + ci.state = conn->state; + ci.link_mode = get_link_mode(conn); + } + hci_dev_unlock(hdev); + + if (!conn) + return -ENOENT; + + return copy_to_user(ptr, &ci, sizeof(ci)) ? -EFAULT : 0; +} + +int hci_get_auth_info(struct hci_dev *hdev, void __user *arg) +{ + struct hci_auth_info_req req; + struct hci_conn *conn; + + if (copy_from_user(&req, arg, sizeof(req))) + return -EFAULT; + + hci_dev_lock(hdev); + conn = hci_conn_hash_lookup_ba(hdev, ACL_LINK, &req.bdaddr); + if (conn) + req.type = conn->auth_type; + hci_dev_unlock(hdev); + + if (!conn) + return -ENOENT; + + return copy_to_user(arg, &req, sizeof(req)) ? -EFAULT : 0; +} + +struct hci_chan *hci_chan_create(struct hci_conn *conn) +{ + struct hci_dev *hdev = conn->hdev; + struct hci_chan *chan; + + BT_DBG("%s hcon %p", hdev->name, conn); + + if (test_bit(HCI_CONN_DROP, &conn->flags)) { + BT_DBG("Refusing to create new hci_chan"); + return NULL; + } + + chan = kzalloc(sizeof(*chan), GFP_KERNEL); + if (!chan) + return NULL; + + chan->conn = hci_conn_get(conn); + skb_queue_head_init(&chan->data_q); + chan->state = BT_CONNECTED; + + list_add_rcu(&chan->list, &conn->chan_list); + + return chan; +} + +void hci_chan_del(struct hci_chan *chan) +{ + struct hci_conn *conn = chan->conn; + struct hci_dev *hdev = conn->hdev; + + BT_DBG("%s hcon %p chan %p", hdev->name, conn, chan); + + list_del_rcu(&chan->list); + + synchronize_rcu(); + + /* Prevent new hci_chan's to be created for this hci_conn */ + set_bit(HCI_CONN_DROP, &conn->flags); + + hci_conn_put(conn); + + skb_queue_purge(&chan->data_q); + kfree(chan); +} + +void hci_chan_list_flush(struct hci_conn *conn) +{ + struct hci_chan *chan, *n; + + BT_DBG("hcon %p", conn); + + list_for_each_entry_safe(chan, n, &conn->chan_list, list) + hci_chan_del(chan); +} + +static struct hci_chan *__hci_chan_lookup_handle(struct hci_conn *hcon, + __u16 handle) +{ + struct hci_chan *hchan; + + list_for_each_entry(hchan, &hcon->chan_list, list) { + if (hchan->handle == handle) + return hchan; + } + + return NULL; +} + +struct hci_chan *hci_chan_lookup_handle(struct hci_dev *hdev, __u16 handle) +{ + struct hci_conn_hash *h = &hdev->conn_hash; + struct hci_conn *hcon; + struct hci_chan *hchan = NULL; + + rcu_read_lock(); + + list_for_each_entry_rcu(hcon, &h->list, list) { + hchan = __hci_chan_lookup_handle(hcon, handle); + if (hchan) + break; + } + + rcu_read_unlock(); + + return hchan; +} diff --git a/kernel/net/bluetooth/hci_core.c b/kernel/net/bluetooth/hci_core.c new file mode 100644 index 000000000..c4802f3bd --- /dev/null +++ b/kernel/net/bluetooth/hci_core.c @@ -0,0 +1,4241 @@ +/* + BlueZ - Bluetooth protocol stack for Linux + Copyright (C) 2000-2001 Qualcomm Incorporated + Copyright (C) 2011 ProFUSION Embedded Systems + + Written 2000,2001 by Maxim Krasnyansky <maxk@qualcomm.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; + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS + OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT OF THIRD PARTY RIGHTS. + IN NO EVENT SHALL THE COPYRIGHT HOLDER(S) AND AUTHOR(S) BE LIABLE FOR ANY + CLAIM, OR ANY SPECIAL INDIRECT OR CONSEQUENTIAL DAMAGES, OR ANY DAMAGES + WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN + ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF + OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + + ALL LIABILITY, INCLUDING LIABILITY FOR INFRINGEMENT OF ANY PATENTS, + COPYRIGHTS, TRADEMARKS OR OTHER RIGHTS, RELATING TO USE OF THIS + SOFTWARE IS DISCLAIMED. +*/ + +/* Bluetooth HCI core. */ + +#include <linux/export.h> +#include <linux/idr.h> +#include <linux/rfkill.h> +#include <linux/debugfs.h> +#include <linux/crypto.h> +#include <asm/unaligned.h> + +#include <net/bluetooth/bluetooth.h> +#include <net/bluetooth/hci_core.h> +#include <net/bluetooth/l2cap.h> +#include <net/bluetooth/mgmt.h> + +#include "hci_request.h" +#include "hci_debugfs.h" +#include "smp.h" + +static void hci_rx_work(struct work_struct *work); +static void hci_cmd_work(struct work_struct *work); +static void hci_tx_work(struct work_struct *work); + +/* HCI device list */ +LIST_HEAD(hci_dev_list); +DEFINE_RWLOCK(hci_dev_list_lock); + +/* HCI callback list */ +LIST_HEAD(hci_cb_list); +DEFINE_MUTEX(hci_cb_list_lock); + +/* HCI ID Numbering */ +static DEFINE_IDA(hci_index_ida); + +/* ----- HCI requests ----- */ + +#define HCI_REQ_DONE 0 +#define HCI_REQ_PEND 1 +#define HCI_REQ_CANCELED 2 + +#define hci_req_lock(d) mutex_lock(&d->req_lock) +#define hci_req_unlock(d) mutex_unlock(&d->req_lock) + +/* ---- HCI notifications ---- */ + +static void hci_notify(struct hci_dev *hdev, int event) +{ + hci_sock_dev_event(hdev, event); +} + +/* ---- HCI debugfs entries ---- */ + +static ssize_t dut_mode_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] = hci_dev_test_flag(hdev, HCI_DUT_MODE) ? 'Y': 'N'; + buf[1] = '\n'; + buf[2] = '\0'; + return simple_read_from_buffer(user_buf, count, ppos, buf, 2); +} + +static ssize_t dut_mode_write(struct file *file, const char __user *user_buf, + size_t count, loff_t *ppos) +{ + struct hci_dev *hdev = file->private_data; + struct sk_buff *skb; + char buf[32]; + size_t buf_size = min(count, (sizeof(buf)-1)); + bool enable; + int err; + + if (!test_bit(HCI_UP, &hdev->flags)) + return -ENETDOWN; + + if (copy_from_user(buf, user_buf, buf_size)) + return -EFAULT; + + buf[buf_size] = '\0'; + if (strtobool(buf, &enable)) + return -EINVAL; + + if (enable == hci_dev_test_flag(hdev, HCI_DUT_MODE)) + return -EALREADY; + + hci_req_lock(hdev); + if (enable) + skb = __hci_cmd_sync(hdev, HCI_OP_ENABLE_DUT_MODE, 0, NULL, + HCI_CMD_TIMEOUT); + else + skb = __hci_cmd_sync(hdev, HCI_OP_RESET, 0, NULL, + HCI_CMD_TIMEOUT); + hci_req_unlock(hdev); + + if (IS_ERR(skb)) + return PTR_ERR(skb); + + err = -bt_to_errno(skb->data[0]); + kfree_skb(skb); + + if (err < 0) + return err; + + hci_dev_change_flag(hdev, HCI_DUT_MODE); + + return count; +} + +static const struct file_operations dut_mode_fops = { + .open = simple_open, + .read = dut_mode_read, + .write = dut_mode_write, + .llseek = default_llseek, +}; + +/* ---- HCI requests ---- */ + +static void hci_req_sync_complete(struct hci_dev *hdev, u8 result, u16 opcode, + struct sk_buff *skb) +{ + BT_DBG("%s result 0x%2.2x", hdev->name, result); + + if (hdev->req_status == HCI_REQ_PEND) { + hdev->req_result = result; + hdev->req_status = HCI_REQ_DONE; + if (skb) + hdev->req_skb = skb_get(skb); + wake_up_interruptible(&hdev->req_wait_q); + } +} + +static void hci_req_cancel(struct hci_dev *hdev, int err) +{ + BT_DBG("%s err 0x%2.2x", hdev->name, err); + + if (hdev->req_status == HCI_REQ_PEND) { + hdev->req_result = err; + hdev->req_status = HCI_REQ_CANCELED; + wake_up_interruptible(&hdev->req_wait_q); + } +} + +struct sk_buff *__hci_cmd_sync_ev(struct hci_dev *hdev, u16 opcode, u32 plen, + const void *param, u8 event, u32 timeout) +{ + DECLARE_WAITQUEUE(wait, current); + struct hci_request req; + struct sk_buff *skb; + int err = 0; + + BT_DBG("%s", hdev->name); + + hci_req_init(&req, hdev); + + hci_req_add_ev(&req, opcode, plen, param, event); + + hdev->req_status = HCI_REQ_PEND; + + add_wait_queue(&hdev->req_wait_q, &wait); + set_current_state(TASK_INTERRUPTIBLE); + + err = hci_req_run_skb(&req, hci_req_sync_complete); + if (err < 0) { + remove_wait_queue(&hdev->req_wait_q, &wait); + set_current_state(TASK_RUNNING); + return ERR_PTR(err); + } + + schedule_timeout(timeout); + + remove_wait_queue(&hdev->req_wait_q, &wait); + + if (signal_pending(current)) + return ERR_PTR(-EINTR); + + switch (hdev->req_status) { + case HCI_REQ_DONE: + err = -bt_to_errno(hdev->req_result); + break; + + case HCI_REQ_CANCELED: + err = -hdev->req_result; + break; + + default: + err = -ETIMEDOUT; + break; + } + + hdev->req_status = hdev->req_result = 0; + skb = hdev->req_skb; + hdev->req_skb = NULL; + + BT_DBG("%s end: err %d", hdev->name, err); + + if (err < 0) { + kfree_skb(skb); + return ERR_PTR(err); + } + + if (!skb) + return ERR_PTR(-ENODATA); + + return skb; +} +EXPORT_SYMBOL(__hci_cmd_sync_ev); + +struct sk_buff *__hci_cmd_sync(struct hci_dev *hdev, u16 opcode, u32 plen, + const void *param, u32 timeout) +{ + return __hci_cmd_sync_ev(hdev, opcode, plen, param, 0, timeout); +} +EXPORT_SYMBOL(__hci_cmd_sync); + +/* Execute request and wait for completion. */ +static int __hci_req_sync(struct hci_dev *hdev, + void (*func)(struct hci_request *req, + unsigned long opt), + unsigned long opt, __u32 timeout) +{ + struct hci_request req; + DECLARE_WAITQUEUE(wait, current); + int err = 0; + + BT_DBG("%s start", hdev->name); + + hci_req_init(&req, hdev); + + hdev->req_status = HCI_REQ_PEND; + + func(&req, opt); + + add_wait_queue(&hdev->req_wait_q, &wait); + set_current_state(TASK_INTERRUPTIBLE); + + err = hci_req_run_skb(&req, hci_req_sync_complete); + if (err < 0) { + hdev->req_status = 0; + + remove_wait_queue(&hdev->req_wait_q, &wait); + set_current_state(TASK_RUNNING); + + /* ENODATA means the HCI request command queue is empty. + * This can happen when a request with conditionals doesn't + * trigger any commands to be sent. This is normal behavior + * and should not trigger an error return. + */ + if (err == -ENODATA) + return 0; + + return err; + } + + schedule_timeout(timeout); + + remove_wait_queue(&hdev->req_wait_q, &wait); + + if (signal_pending(current)) + return -EINTR; + + switch (hdev->req_status) { + case HCI_REQ_DONE: + err = -bt_to_errno(hdev->req_result); + break; + + case HCI_REQ_CANCELED: + err = -hdev->req_result; + break; + + default: + err = -ETIMEDOUT; + break; + } + + hdev->req_status = hdev->req_result = 0; + + BT_DBG("%s end: err %d", hdev->name, err); + + return err; +} + +static int hci_req_sync(struct hci_dev *hdev, + void (*req)(struct hci_request *req, + unsigned long opt), + unsigned long opt, __u32 timeout) +{ + int ret; + + if (!test_bit(HCI_UP, &hdev->flags)) + return -ENETDOWN; + + /* Serialize all requests */ + hci_req_lock(hdev); + ret = __hci_req_sync(hdev, req, opt, timeout); + hci_req_unlock(hdev); + + return ret; +} + +static void hci_reset_req(struct hci_request *req, unsigned long opt) +{ + BT_DBG("%s %ld", req->hdev->name, opt); + + /* Reset device */ + set_bit(HCI_RESET, &req->hdev->flags); + hci_req_add(req, HCI_OP_RESET, 0, NULL); +} + +static void bredr_init(struct hci_request *req) +{ + req->hdev->flow_ctl_mode = HCI_FLOW_CTL_MODE_PACKET_BASED; + + /* Read Local Supported Features */ + hci_req_add(req, HCI_OP_READ_LOCAL_FEATURES, 0, NULL); + + /* Read Local Version */ + hci_req_add(req, HCI_OP_READ_LOCAL_VERSION, 0, NULL); + + /* Read BD Address */ + hci_req_add(req, HCI_OP_READ_BD_ADDR, 0, NULL); +} + +static void amp_init1(struct hci_request *req) +{ + req->hdev->flow_ctl_mode = HCI_FLOW_CTL_MODE_BLOCK_BASED; + + /* Read Local Version */ + hci_req_add(req, HCI_OP_READ_LOCAL_VERSION, 0, NULL); + + /* Read Local Supported Commands */ + hci_req_add(req, HCI_OP_READ_LOCAL_COMMANDS, 0, NULL); + + /* Read Local AMP Info */ + hci_req_add(req, HCI_OP_READ_LOCAL_AMP_INFO, 0, NULL); + + /* Read Data Blk size */ + hci_req_add(req, HCI_OP_READ_DATA_BLOCK_SIZE, 0, NULL); + + /* Read Flow Control Mode */ + hci_req_add(req, HCI_OP_READ_FLOW_CONTROL_MODE, 0, NULL); + + /* Read Location Data */ + hci_req_add(req, HCI_OP_READ_LOCATION_DATA, 0, NULL); +} + +static void amp_init2(struct hci_request *req) +{ + /* Read Local Supported Features. Not all AMP controllers + * support this so it's placed conditionally in the second + * stage init. + */ + if (req->hdev->commands[14] & 0x20) + hci_req_add(req, HCI_OP_READ_LOCAL_FEATURES, 0, NULL); +} + +static void hci_init1_req(struct hci_request *req, unsigned long opt) +{ + struct hci_dev *hdev = req->hdev; + + BT_DBG("%s %ld", hdev->name, opt); + + /* Reset */ + if (!test_bit(HCI_QUIRK_RESET_ON_CLOSE, &hdev->quirks)) + hci_reset_req(req, 0); + + switch (hdev->dev_type) { + case HCI_BREDR: + bredr_init(req); + break; + + case HCI_AMP: + amp_init1(req); + break; + + default: + BT_ERR("Unknown device type %d", hdev->dev_type); + break; + } +} + +static void bredr_setup(struct hci_request *req) +{ + __le16 param; + __u8 flt_type; + + /* Read Buffer Size (ACL mtu, max pkt, etc.) */ + hci_req_add(req, HCI_OP_READ_BUFFER_SIZE, 0, NULL); + + /* Read Class of Device */ + hci_req_add(req, HCI_OP_READ_CLASS_OF_DEV, 0, NULL); + + /* Read Local Name */ + hci_req_add(req, HCI_OP_READ_LOCAL_NAME, 0, NULL); + + /* Read Voice Setting */ + hci_req_add(req, HCI_OP_READ_VOICE_SETTING, 0, NULL); + + /* Read Number of Supported IAC */ + hci_req_add(req, HCI_OP_READ_NUM_SUPPORTED_IAC, 0, NULL); + + /* Read Current IAC LAP */ + hci_req_add(req, HCI_OP_READ_CURRENT_IAC_LAP, 0, NULL); + + /* Clear Event Filters */ + flt_type = HCI_FLT_CLEAR_ALL; + hci_req_add(req, HCI_OP_SET_EVENT_FLT, 1, &flt_type); + + /* Connection accept timeout ~20 secs */ + param = cpu_to_le16(0x7d00); + hci_req_add(req, HCI_OP_WRITE_CA_TIMEOUT, 2, ¶m); +} + +static void le_setup(struct hci_request *req) +{ + struct hci_dev *hdev = req->hdev; + + /* Read LE Buffer Size */ + hci_req_add(req, HCI_OP_LE_READ_BUFFER_SIZE, 0, NULL); + + /* Read LE Local Supported Features */ + hci_req_add(req, HCI_OP_LE_READ_LOCAL_FEATURES, 0, NULL); + + /* Read LE Supported States */ + hci_req_add(req, HCI_OP_LE_READ_SUPPORTED_STATES, 0, NULL); + + /* Read LE White List Size */ + hci_req_add(req, HCI_OP_LE_READ_WHITE_LIST_SIZE, 0, NULL); + + /* Clear LE White List */ + hci_req_add(req, HCI_OP_LE_CLEAR_WHITE_LIST, 0, NULL); + + /* LE-only controllers have LE implicitly enabled */ + if (!lmp_bredr_capable(hdev)) + hci_dev_set_flag(hdev, HCI_LE_ENABLED); +} + +static void hci_setup_event_mask(struct hci_request *req) +{ + struct hci_dev *hdev = req->hdev; + + /* The second byte is 0xff instead of 0x9f (two reserved bits + * disabled) since a Broadcom 1.2 dongle doesn't respond to the + * command otherwise. + */ + u8 events[8] = { 0xff, 0xff, 0xfb, 0xff, 0x00, 0x00, 0x00, 0x00 }; + + /* CSR 1.1 dongles does not accept any bitfield so don't try to set + * any event mask for pre 1.2 devices. + */ + if (hdev->hci_ver < BLUETOOTH_VER_1_2) + return; + + if (lmp_bredr_capable(hdev)) { + events[4] |= 0x01; /* Flow Specification Complete */ + events[4] |= 0x02; /* Inquiry Result with RSSI */ + events[4] |= 0x04; /* Read Remote Extended Features Complete */ + events[5] |= 0x08; /* Synchronous Connection Complete */ + events[5] |= 0x10; /* Synchronous Connection Changed */ + } else { + /* Use a different default for LE-only devices */ + memset(events, 0, sizeof(events)); + events[0] |= 0x10; /* Disconnection Complete */ + events[1] |= 0x08; /* Read Remote Version Information Complete */ + events[1] |= 0x20; /* Command Complete */ + events[1] |= 0x40; /* Command Status */ + events[1] |= 0x80; /* Hardware Error */ + events[2] |= 0x04; /* Number of Completed Packets */ + events[3] |= 0x02; /* Data Buffer Overflow */ + + if (hdev->le_features[0] & HCI_LE_ENCRYPTION) { + events[0] |= 0x80; /* Encryption Change */ + events[5] |= 0x80; /* Encryption Key Refresh Complete */ + } + } + + if (lmp_inq_rssi_capable(hdev)) + events[4] |= 0x02; /* Inquiry Result with RSSI */ + + if (lmp_sniffsubr_capable(hdev)) + events[5] |= 0x20; /* Sniff Subrating */ + + if (lmp_pause_enc_capable(hdev)) + events[5] |= 0x80; /* Encryption Key Refresh Complete */ + + if (lmp_ext_inq_capable(hdev)) + events[5] |= 0x40; /* Extended Inquiry Result */ + + if (lmp_no_flush_capable(hdev)) + events[7] |= 0x01; /* Enhanced Flush Complete */ + + if (lmp_lsto_capable(hdev)) + events[6] |= 0x80; /* Link Supervision Timeout Changed */ + + if (lmp_ssp_capable(hdev)) { + events[6] |= 0x01; /* IO Capability Request */ + events[6] |= 0x02; /* IO Capability Response */ + events[6] |= 0x04; /* User Confirmation Request */ + events[6] |= 0x08; /* User Passkey Request */ + events[6] |= 0x10; /* Remote OOB Data Request */ + events[6] |= 0x20; /* Simple Pairing Complete */ + events[7] |= 0x04; /* User Passkey Notification */ + events[7] |= 0x08; /* Keypress Notification */ + events[7] |= 0x10; /* Remote Host Supported + * Features Notification + */ + } + + if (lmp_le_capable(hdev)) + events[7] |= 0x20; /* LE Meta-Event */ + + hci_req_add(req, HCI_OP_SET_EVENT_MASK, sizeof(events), events); +} + +static void hci_init2_req(struct hci_request *req, unsigned long opt) +{ + struct hci_dev *hdev = req->hdev; + + if (hdev->dev_type == HCI_AMP) + return amp_init2(req); + + if (lmp_bredr_capable(hdev)) + bredr_setup(req); + else + hci_dev_clear_flag(hdev, HCI_BREDR_ENABLED); + + if (lmp_le_capable(hdev)) + le_setup(req); + + /* All Bluetooth 1.2 and later controllers should support the + * HCI command for reading the local supported commands. + * + * Unfortunately some controllers indicate Bluetooth 1.2 support, + * but do not have support for this command. If that is the case, + * the driver can quirk the behavior and skip reading the local + * supported commands. + */ + if (hdev->hci_ver > BLUETOOTH_VER_1_1 && + !test_bit(HCI_QUIRK_BROKEN_LOCAL_COMMANDS, &hdev->quirks)) + hci_req_add(req, HCI_OP_READ_LOCAL_COMMANDS, 0, NULL); + + if (lmp_ssp_capable(hdev)) { + /* When SSP is available, then the host features page + * should also be available as well. However some + * controllers list the max_page as 0 as long as SSP + * has not been enabled. To achieve proper debugging + * output, force the minimum max_page to 1 at least. + */ + hdev->max_page = 0x01; + + if (hci_dev_test_flag(hdev, HCI_SSP_ENABLED)) { + u8 mode = 0x01; + + hci_req_add(req, HCI_OP_WRITE_SSP_MODE, + sizeof(mode), &mode); + } else { + struct hci_cp_write_eir cp; + + memset(hdev->eir, 0, sizeof(hdev->eir)); + memset(&cp, 0, sizeof(cp)); + + hci_req_add(req, HCI_OP_WRITE_EIR, sizeof(cp), &cp); + } + } + + if (lmp_inq_rssi_capable(hdev) || + test_bit(HCI_QUIRK_FIXUP_INQUIRY_MODE, &hdev->quirks)) { + u8 mode; + + /* If Extended Inquiry Result events are supported, then + * they are clearly preferred over Inquiry Result with RSSI + * events. + */ + mode = lmp_ext_inq_capable(hdev) ? 0x02 : 0x01; + + hci_req_add(req, HCI_OP_WRITE_INQUIRY_MODE, 1, &mode); + } + + if (lmp_inq_tx_pwr_capable(hdev)) + hci_req_add(req, HCI_OP_READ_INQ_RSP_TX_POWER, 0, NULL); + + if (lmp_ext_feat_capable(hdev)) { + struct hci_cp_read_local_ext_features cp; + + cp.page = 0x01; + hci_req_add(req, HCI_OP_READ_LOCAL_EXT_FEATURES, + sizeof(cp), &cp); + } + + if (hci_dev_test_flag(hdev, HCI_LINK_SECURITY)) { + u8 enable = 1; + hci_req_add(req, HCI_OP_WRITE_AUTH_ENABLE, sizeof(enable), + &enable); + } +} + +static void hci_setup_link_policy(struct hci_request *req) +{ + struct hci_dev *hdev = req->hdev; + struct hci_cp_write_def_link_policy cp; + u16 link_policy = 0; + + if (lmp_rswitch_capable(hdev)) + link_policy |= HCI_LP_RSWITCH; + if (lmp_hold_capable(hdev)) + link_policy |= HCI_LP_HOLD; + if (lmp_sniff_capable(hdev)) + link_policy |= HCI_LP_SNIFF; + if (lmp_park_capable(hdev)) + link_policy |= HCI_LP_PARK; + + cp.policy = cpu_to_le16(link_policy); + hci_req_add(req, HCI_OP_WRITE_DEF_LINK_POLICY, sizeof(cp), &cp); +} + +static void hci_set_le_support(struct hci_request *req) +{ + struct hci_dev *hdev = req->hdev; + struct hci_cp_write_le_host_supported cp; + + /* LE-only devices do not support explicit enablement */ + if (!lmp_bredr_capable(hdev)) + return; + + memset(&cp, 0, sizeof(cp)); + + if (hci_dev_test_flag(hdev, HCI_LE_ENABLED)) { + cp.le = 0x01; + cp.simul = 0x00; + } + + if (cp.le != lmp_host_le_capable(hdev)) + hci_req_add(req, HCI_OP_WRITE_LE_HOST_SUPPORTED, sizeof(cp), + &cp); +} + +static void hci_set_event_mask_page_2(struct hci_request *req) +{ + struct hci_dev *hdev = req->hdev; + u8 events[8] = { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 }; + + /* If Connectionless Slave Broadcast master role is supported + * enable all necessary events for it. + */ + if (lmp_csb_master_capable(hdev)) { + events[1] |= 0x40; /* Triggered Clock Capture */ + events[1] |= 0x80; /* Synchronization Train Complete */ + events[2] |= 0x10; /* Slave Page Response Timeout */ + events[2] |= 0x20; /* CSB Channel Map Change */ + } + + /* If Connectionless Slave Broadcast slave role is supported + * enable all necessary events for it. + */ + if (lmp_csb_slave_capable(hdev)) { + events[2] |= 0x01; /* Synchronization Train Received */ + events[2] |= 0x02; /* CSB Receive */ + events[2] |= 0x04; /* CSB Timeout */ + events[2] |= 0x08; /* Truncated Page Complete */ + } + + /* Enable Authenticated Payload Timeout Expired event if supported */ + if (lmp_ping_capable(hdev) || hdev->le_features[0] & HCI_LE_PING) + events[2] |= 0x80; + + hci_req_add(req, HCI_OP_SET_EVENT_MASK_PAGE_2, sizeof(events), events); +} + +static void hci_init3_req(struct hci_request *req, unsigned long opt) +{ + struct hci_dev *hdev = req->hdev; + u8 p; + + hci_setup_event_mask(req); + + if (hdev->commands[6] & 0x20) { + struct hci_cp_read_stored_link_key cp; + + bacpy(&cp.bdaddr, BDADDR_ANY); + cp.read_all = 0x01; + hci_req_add(req, HCI_OP_READ_STORED_LINK_KEY, sizeof(cp), &cp); + } + + if (hdev->commands[5] & 0x10) + hci_setup_link_policy(req); + + if (hdev->commands[8] & 0x01) + hci_req_add(req, HCI_OP_READ_PAGE_SCAN_ACTIVITY, 0, NULL); + + /* Some older Broadcom based Bluetooth 1.2 controllers do not + * support the Read Page Scan Type command. Check support for + * this command in the bit mask of supported commands. + */ + if (hdev->commands[13] & 0x01) + hci_req_add(req, HCI_OP_READ_PAGE_SCAN_TYPE, 0, NULL); + + if (lmp_le_capable(hdev)) { + u8 events[8]; + + memset(events, 0, sizeof(events)); + events[0] = 0x0f; + + if (hdev->le_features[0] & HCI_LE_ENCRYPTION) + events[0] |= 0x10; /* LE Long Term Key Request */ + + /* If controller supports the Connection Parameters Request + * Link Layer Procedure, enable the corresponding event. + */ + if (hdev->le_features[0] & HCI_LE_CONN_PARAM_REQ_PROC) + events[0] |= 0x20; /* LE Remote Connection + * Parameter Request + */ + + /* If the controller supports the Data Length Extension + * feature, enable the corresponding event. + */ + if (hdev->le_features[0] & HCI_LE_DATA_LEN_EXT) + events[0] |= 0x40; /* LE Data Length Change */ + + /* If the controller supports Extended Scanner Filter + * Policies, enable the correspondig event. + */ + if (hdev->le_features[0] & HCI_LE_EXT_SCAN_POLICY) + events[1] |= 0x04; /* LE Direct Advertising + * Report + */ + + /* If the controller supports the LE Read Local P-256 + * Public Key command, enable the corresponding event. + */ + if (hdev->commands[34] & 0x02) + events[0] |= 0x80; /* LE Read Local P-256 + * Public Key Complete + */ + + /* If the controller supports the LE Generate DHKey + * command, enable the corresponding event. + */ + if (hdev->commands[34] & 0x04) + events[1] |= 0x01; /* LE Generate DHKey Complete */ + + hci_req_add(req, HCI_OP_LE_SET_EVENT_MASK, sizeof(events), + events); + + if (hdev->commands[25] & 0x40) { + /* Read LE Advertising Channel TX Power */ + hci_req_add(req, HCI_OP_LE_READ_ADV_TX_POWER, 0, NULL); + } + + if (hdev->le_features[0] & HCI_LE_DATA_LEN_EXT) { + /* Read LE Maximum Data Length */ + hci_req_add(req, HCI_OP_LE_READ_MAX_DATA_LEN, 0, NULL); + + /* Read LE Suggested Default Data Length */ + hci_req_add(req, HCI_OP_LE_READ_DEF_DATA_LEN, 0, NULL); + } + + hci_set_le_support(req); + } + + /* Read features beyond page 1 if available */ + for (p = 2; p < HCI_MAX_PAGES && p <= hdev->max_page; p++) { + struct hci_cp_read_local_ext_features cp; + + cp.page = p; + hci_req_add(req, HCI_OP_READ_LOCAL_EXT_FEATURES, + sizeof(cp), &cp); + } +} + +static void hci_init4_req(struct hci_request *req, unsigned long opt) +{ + struct hci_dev *hdev = req->hdev; + + /* Some Broadcom based Bluetooth controllers do not support the + * Delete Stored Link Key command. They are clearly indicating its + * absence in the bit mask of supported commands. + * + * Check the supported commands and only if the the command is marked + * 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 && + !test_bit(HCI_QUIRK_BROKEN_STORED_LINK_KEY, &hdev->quirks)) { + struct hci_cp_delete_stored_link_key cp; + + bacpy(&cp.bdaddr, BDADDR_ANY); + cp.delete_all = 0x01; + hci_req_add(req, HCI_OP_DELETE_STORED_LINK_KEY, + sizeof(cp), &cp); + } + + /* Set event mask page 2 if the HCI command for it is supported */ + if (hdev->commands[22] & 0x04) + hci_set_event_mask_page_2(req); + + /* Read local codec list if the HCI command is supported */ + if (hdev->commands[29] & 0x20) + hci_req_add(req, HCI_OP_READ_LOCAL_CODECS, 0, NULL); + + /* Get MWS transport configuration if the HCI command is supported */ + if (hdev->commands[30] & 0x08) + hci_req_add(req, HCI_OP_GET_MWS_TRANSPORT_CONFIG, 0, NULL); + + /* Check for Synchronization Train support */ + if (lmp_sync_train_capable(hdev)) + hci_req_add(req, HCI_OP_READ_SYNC_TRAIN_PARAMS, 0, NULL); + + /* Enable Secure Connections if supported and configured */ + if (hci_dev_test_flag(hdev, HCI_SSP_ENABLED) && + bredr_sc_enabled(hdev)) { + u8 support = 0x01; + + hci_req_add(req, HCI_OP_WRITE_SC_SUPPORT, + sizeof(support), &support); + } +} + +static int __hci_init(struct hci_dev *hdev) +{ + int err; + + err = __hci_req_sync(hdev, hci_init1_req, 0, HCI_INIT_TIMEOUT); + if (err < 0) + return err; + + /* The Device Under Test (DUT) mode is special and available for + * all controller types. So just create it early on. + */ + if (hci_dev_test_flag(hdev, HCI_SETUP)) { + debugfs_create_file("dut_mode", 0644, hdev->debugfs, hdev, + &dut_mode_fops); + } + + err = __hci_req_sync(hdev, hci_init2_req, 0, HCI_INIT_TIMEOUT); + if (err < 0) + return err; + + /* HCI_BREDR covers both single-mode LE, BR/EDR and dual-mode + * BR/EDR/LE type controllers. AMP controllers only need the + * first two stages of init. + */ + if (hdev->dev_type != HCI_BREDR) + return 0; + + err = __hci_req_sync(hdev, hci_init3_req, 0, HCI_INIT_TIMEOUT); + if (err < 0) + return err; + + err = __hci_req_sync(hdev, hci_init4_req, 0, HCI_INIT_TIMEOUT); + if (err < 0) + return err; + + /* This function is only called when the controller is actually in + * configured state. When the controller is marked as unconfigured, + * this initialization procedure is not run. + * + * It means that it is possible that a controller runs through its + * setup phase and then discovers missing settings. If that is the + * case, then this function will not be called. It then will only + * be called during the config phase. + * + * So only when in setup phase or config phase, create the debugfs + * entries and register the SMP channels. + */ + if (!hci_dev_test_flag(hdev, HCI_SETUP) && + !hci_dev_test_flag(hdev, HCI_CONFIG)) + return 0; + + hci_debugfs_create_common(hdev); + + if (lmp_bredr_capable(hdev)) + hci_debugfs_create_bredr(hdev); + + if (lmp_le_capable(hdev)) + hci_debugfs_create_le(hdev); + + return 0; +} + +static void hci_init0_req(struct hci_request *req, unsigned long opt) +{ + struct hci_dev *hdev = req->hdev; + + BT_DBG("%s %ld", hdev->name, opt); + + /* Reset */ + if (!test_bit(HCI_QUIRK_RESET_ON_CLOSE, &hdev->quirks)) + hci_reset_req(req, 0); + + /* Read Local Version */ + hci_req_add(req, HCI_OP_READ_LOCAL_VERSION, 0, NULL); + + /* Read BD Address */ + if (hdev->set_bdaddr) + hci_req_add(req, HCI_OP_READ_BD_ADDR, 0, NULL); +} + +static int __hci_unconf_init(struct hci_dev *hdev) +{ + int err; + + if (test_bit(HCI_QUIRK_RAW_DEVICE, &hdev->quirks)) + return 0; + + err = __hci_req_sync(hdev, hci_init0_req, 0, HCI_INIT_TIMEOUT); + if (err < 0) + return err; + + return 0; +} + +static void hci_scan_req(struct hci_request *req, unsigned long opt) +{ + __u8 scan = opt; + + BT_DBG("%s %x", req->hdev->name, scan); + + /* Inquiry and Page scans */ + hci_req_add(req, HCI_OP_WRITE_SCAN_ENABLE, 1, &scan); +} + +static void hci_auth_req(struct hci_request *req, unsigned long opt) +{ + __u8 auth = opt; + + BT_DBG("%s %x", req->hdev->name, auth); + + /* Authentication */ + hci_req_add(req, HCI_OP_WRITE_AUTH_ENABLE, 1, &auth); +} + +static void hci_encrypt_req(struct hci_request *req, unsigned long opt) +{ + __u8 encrypt = opt; + + BT_DBG("%s %x", req->hdev->name, encrypt); + + /* Encryption */ + hci_req_add(req, HCI_OP_WRITE_ENCRYPT_MODE, 1, &encrypt); +} + +static void hci_linkpol_req(struct hci_request *req, unsigned long opt) +{ + __le16 policy = cpu_to_le16(opt); + + BT_DBG("%s %x", req->hdev->name, policy); + + /* Default link policy */ + hci_req_add(req, HCI_OP_WRITE_DEF_LINK_POLICY, 2, &policy); +} + +/* Get HCI device by index. + * Device is held on return. */ +struct hci_dev *hci_dev_get(int index) +{ + struct hci_dev *hdev = NULL, *d; + + BT_DBG("%d", index); + + if (index < 0) + return NULL; + + read_lock(&hci_dev_list_lock); + list_for_each_entry(d, &hci_dev_list, list) { + if (d->id == index) { + hdev = hci_dev_hold(d); + break; + } + } + read_unlock(&hci_dev_list_lock); + return hdev; +} + +/* ---- Inquiry support ---- */ + +bool hci_discovery_active(struct hci_dev *hdev) +{ + struct discovery_state *discov = &hdev->discovery; + + switch (discov->state) { + case DISCOVERY_FINDING: + case DISCOVERY_RESOLVING: + return true; + + default: + return false; + } +} + +void hci_discovery_set_state(struct hci_dev *hdev, int state) +{ + int old_state = hdev->discovery.state; + + BT_DBG("%s state %u -> %u", hdev->name, hdev->discovery.state, state); + + if (old_state == state) + return; + + hdev->discovery.state = state; + + switch (state) { + case DISCOVERY_STOPPED: + hci_update_background_scan(hdev); + + if (old_state != DISCOVERY_STARTING) + mgmt_discovering(hdev, 0); + break; + case DISCOVERY_STARTING: + break; + case DISCOVERY_FINDING: + mgmt_discovering(hdev, 1); + break; + case DISCOVERY_RESOLVING: + break; + case DISCOVERY_STOPPING: + break; + } +} + +void hci_inquiry_cache_flush(struct hci_dev *hdev) +{ + struct discovery_state *cache = &hdev->discovery; + struct inquiry_entry *p, *n; + + list_for_each_entry_safe(p, n, &cache->all, all) { + list_del(&p->all); + kfree(p); + } + + INIT_LIST_HEAD(&cache->unknown); + INIT_LIST_HEAD(&cache->resolve); +} + +struct inquiry_entry *hci_inquiry_cache_lookup(struct hci_dev *hdev, + bdaddr_t *bdaddr) +{ + struct discovery_state *cache = &hdev->discovery; + struct inquiry_entry *e; + + BT_DBG("cache %p, %pMR", cache, bdaddr); + + list_for_each_entry(e, &cache->all, all) { + if (!bacmp(&e->data.bdaddr, bdaddr)) + return e; + } + + return NULL; +} + +struct inquiry_entry *hci_inquiry_cache_lookup_unknown(struct hci_dev *hdev, + bdaddr_t *bdaddr) +{ + struct discovery_state *cache = &hdev->discovery; + struct inquiry_entry *e; + + BT_DBG("cache %p, %pMR", cache, bdaddr); + + list_for_each_entry(e, &cache->unknown, list) { + if (!bacmp(&e->data.bdaddr, bdaddr)) + return e; + } + + return NULL; +} + +struct inquiry_entry *hci_inquiry_cache_lookup_resolve(struct hci_dev *hdev, + bdaddr_t *bdaddr, + int state) +{ + struct discovery_state *cache = &hdev->discovery; + struct inquiry_entry *e; + + BT_DBG("cache %p bdaddr %pMR state %d", cache, bdaddr, state); + + list_for_each_entry(e, &cache->resolve, list) { + if (!bacmp(bdaddr, BDADDR_ANY) && e->name_state == state) + return e; + if (!bacmp(&e->data.bdaddr, bdaddr)) + return e; + } + + return NULL; +} + +void hci_inquiry_cache_update_resolve(struct hci_dev *hdev, + struct inquiry_entry *ie) +{ + struct discovery_state *cache = &hdev->discovery; + struct list_head *pos = &cache->resolve; + struct inquiry_entry *p; + + list_del(&ie->list); + + list_for_each_entry(p, &cache->resolve, list) { + if (p->name_state != NAME_PENDING && + abs(p->data.rssi) >= abs(ie->data.rssi)) + break; + pos = &p->list; + } + + list_add(&ie->list, pos); +} + +u32 hci_inquiry_cache_update(struct hci_dev *hdev, struct inquiry_data *data, + bool name_known) +{ + struct discovery_state *cache = &hdev->discovery; + struct inquiry_entry *ie; + u32 flags = 0; + + BT_DBG("cache %p, %pMR", cache, &data->bdaddr); + + hci_remove_remote_oob_data(hdev, &data->bdaddr, BDADDR_BREDR); + + if (!data->ssp_mode) + flags |= MGMT_DEV_FOUND_LEGACY_PAIRING; + + ie = hci_inquiry_cache_lookup(hdev, &data->bdaddr); + if (ie) { + if (!ie->data.ssp_mode) + flags |= MGMT_DEV_FOUND_LEGACY_PAIRING; + + if (ie->name_state == NAME_NEEDED && + data->rssi != ie->data.rssi) { + ie->data.rssi = data->rssi; + hci_inquiry_cache_update_resolve(hdev, ie); + } + + goto update; + } + + /* Entry not in the cache. Add new one. */ + ie = kzalloc(sizeof(*ie), GFP_KERNEL); + if (!ie) { + flags |= MGMT_DEV_FOUND_CONFIRM_NAME; + goto done; + } + + list_add(&ie->all, &cache->all); + + if (name_known) { + ie->name_state = NAME_KNOWN; + } else { + ie->name_state = NAME_NOT_KNOWN; + list_add(&ie->list, &cache->unknown); + } + +update: + if (name_known && ie->name_state != NAME_KNOWN && + ie->name_state != NAME_PENDING) { + ie->name_state = NAME_KNOWN; + list_del(&ie->list); + } + + memcpy(&ie->data, data, sizeof(*data)); + ie->timestamp = jiffies; + cache->timestamp = jiffies; + + if (ie->name_state == NAME_NOT_KNOWN) + flags |= MGMT_DEV_FOUND_CONFIRM_NAME; + +done: + return flags; +} + +static int inquiry_cache_dump(struct hci_dev *hdev, int num, __u8 *buf) +{ + struct discovery_state *cache = &hdev->discovery; + struct inquiry_info *info = (struct inquiry_info *) buf; + struct inquiry_entry *e; + int copied = 0; + + list_for_each_entry(e, &cache->all, all) { + struct inquiry_data *data = &e->data; + + if (copied >= num) + break; + + bacpy(&info->bdaddr, &data->bdaddr); + info->pscan_rep_mode = data->pscan_rep_mode; + info->pscan_period_mode = data->pscan_period_mode; + info->pscan_mode = data->pscan_mode; + memcpy(info->dev_class, data->dev_class, 3); + info->clock_offset = data->clock_offset; + + info++; + copied++; + } + + BT_DBG("cache %p, copied %d", cache, copied); + return copied; +} + +static void hci_inq_req(struct hci_request *req, unsigned long opt) +{ + struct hci_inquiry_req *ir = (struct hci_inquiry_req *) opt; + struct hci_dev *hdev = req->hdev; + struct hci_cp_inquiry cp; + + BT_DBG("%s", hdev->name); + + if (test_bit(HCI_INQUIRY, &hdev->flags)) + return; + + /* Start Inquiry */ + memcpy(&cp.lap, &ir->lap, 3); + cp.length = ir->length; + cp.num_rsp = ir->num_rsp; + hci_req_add(req, HCI_OP_INQUIRY, sizeof(cp), &cp); +} + +int hci_inquiry(void __user *arg) +{ + __u8 __user *ptr = arg; + struct hci_inquiry_req ir; + struct hci_dev *hdev; + int err = 0, do_inquiry = 0, max_rsp; + long timeo; + __u8 *buf; + + if (copy_from_user(&ir, ptr, sizeof(ir))) + return -EFAULT; + + hdev = hci_dev_get(ir.dev_id); + if (!hdev) + return -ENODEV; + + if (hci_dev_test_flag(hdev, HCI_USER_CHANNEL)) { + err = -EBUSY; + goto done; + } + + if (hci_dev_test_flag(hdev, HCI_UNCONFIGURED)) { + err = -EOPNOTSUPP; + goto done; + } + + if (hdev->dev_type != HCI_BREDR) { + err = -EOPNOTSUPP; + goto done; + } + + if (!hci_dev_test_flag(hdev, HCI_BREDR_ENABLED)) { + err = -EOPNOTSUPP; + goto done; + } + + hci_dev_lock(hdev); + if (inquiry_cache_age(hdev) > INQUIRY_CACHE_AGE_MAX || + inquiry_cache_empty(hdev) || ir.flags & IREQ_CACHE_FLUSH) { + hci_inquiry_cache_flush(hdev); + do_inquiry = 1; + } + hci_dev_unlock(hdev); + + timeo = ir.length * msecs_to_jiffies(2000); + + if (do_inquiry) { + err = hci_req_sync(hdev, hci_inq_req, (unsigned long) &ir, + timeo); + if (err < 0) + goto done; + + /* Wait until Inquiry procedure finishes (HCI_INQUIRY flag is + * cleared). If it is interrupted by a signal, return -EINTR. + */ + if (wait_on_bit(&hdev->flags, HCI_INQUIRY, + TASK_INTERRUPTIBLE)) + return -EINTR; + } + + /* for unlimited number of responses we will use buffer with + * 255 entries + */ + max_rsp = (ir.num_rsp == 0) ? 255 : ir.num_rsp; + + /* cache_dump can't sleep. Therefore we allocate temp buffer and then + * copy it to the user space. + */ + buf = kmalloc(sizeof(struct inquiry_info) * max_rsp, GFP_KERNEL); + if (!buf) { + err = -ENOMEM; + goto done; + } + + hci_dev_lock(hdev); + ir.num_rsp = inquiry_cache_dump(hdev, max_rsp, buf); + hci_dev_unlock(hdev); + + BT_DBG("num_rsp %d", ir.num_rsp); + + if (!copy_to_user(ptr, &ir, sizeof(ir))) { + ptr += sizeof(ir); + if (copy_to_user(ptr, buf, sizeof(struct inquiry_info) * + ir.num_rsp)) + err = -EFAULT; + } else + err = -EFAULT; + + kfree(buf); + +done: + hci_dev_put(hdev); + return err; +} + +static int hci_dev_do_open(struct hci_dev *hdev) +{ + int ret = 0; + + BT_DBG("%s %p", hdev->name, hdev); + + hci_req_lock(hdev); + + if (hci_dev_test_flag(hdev, HCI_UNREGISTER)) { + ret = -ENODEV; + goto done; + } + + if (!hci_dev_test_flag(hdev, HCI_SETUP) && + !hci_dev_test_flag(hdev, HCI_CONFIG)) { + /* Check for rfkill but allow the HCI setup stage to + * proceed (which in itself doesn't cause any RF activity). + */ + if (hci_dev_test_flag(hdev, HCI_RFKILLED)) { + ret = -ERFKILL; + goto done; + } + + /* Check for valid public address or a configured static + * random adddress, but let the HCI setup proceed to + * be able to determine if there is a public address + * or not. + * + * In case of user channel usage, it is not important + * if a public address or static random address is + * available. + * + * This check is only valid for BR/EDR controllers + * since AMP controllers do not have an address. + */ + if (!hci_dev_test_flag(hdev, HCI_USER_CHANNEL) && + hdev->dev_type == HCI_BREDR && + !bacmp(&hdev->bdaddr, BDADDR_ANY) && + !bacmp(&hdev->static_addr, BDADDR_ANY)) { + ret = -EADDRNOTAVAIL; + goto done; + } + } + + if (test_bit(HCI_UP, &hdev->flags)) { + ret = -EALREADY; + goto done; + } + + if (hdev->open(hdev)) { + ret = -EIO; + goto done; + } + + atomic_set(&hdev->cmd_cnt, 1); + set_bit(HCI_INIT, &hdev->flags); + + if (hci_dev_test_flag(hdev, HCI_SETUP)) { + if (hdev->setup) + ret = hdev->setup(hdev); + + /* The transport driver can set these quirks before + * creating the HCI device or in its setup callback. + * + * In case any of them is set, the controller has to + * start up as unconfigured. + */ + if (test_bit(HCI_QUIRK_EXTERNAL_CONFIG, &hdev->quirks) || + test_bit(HCI_QUIRK_INVALID_BDADDR, &hdev->quirks)) + hci_dev_set_flag(hdev, HCI_UNCONFIGURED); + + /* For an unconfigured controller it is required to + * read at least the version information provided by + * the Read Local Version Information command. + * + * If the set_bdaddr driver callback is provided, then + * also the original Bluetooth public device address + * will be read using the Read BD Address command. + */ + if (hci_dev_test_flag(hdev, HCI_UNCONFIGURED)) + ret = __hci_unconf_init(hdev); + } + + if (hci_dev_test_flag(hdev, HCI_CONFIG)) { + /* If public address change is configured, ensure that + * the address gets programmed. If the driver does not + * support changing the public address, fail the power + * on procedure. + */ + if (bacmp(&hdev->public_addr, BDADDR_ANY) && + hdev->set_bdaddr) + ret = hdev->set_bdaddr(hdev, &hdev->public_addr); + else + ret = -EADDRNOTAVAIL; + } + + if (!ret) { + if (!hci_dev_test_flag(hdev, HCI_UNCONFIGURED) && + !hci_dev_test_flag(hdev, HCI_USER_CHANNEL)) + ret = __hci_init(hdev); + } + + clear_bit(HCI_INIT, &hdev->flags); + + if (!ret) { + hci_dev_hold(hdev); + hci_dev_set_flag(hdev, HCI_RPA_EXPIRED); + set_bit(HCI_UP, &hdev->flags); + hci_notify(hdev, HCI_DEV_UP); + if (!hci_dev_test_flag(hdev, HCI_SETUP) && + !hci_dev_test_flag(hdev, HCI_CONFIG) && + !hci_dev_test_flag(hdev, HCI_UNCONFIGURED) && + !hci_dev_test_flag(hdev, HCI_USER_CHANNEL) && + hdev->dev_type == HCI_BREDR) { + hci_dev_lock(hdev); + mgmt_powered(hdev, 1); + hci_dev_unlock(hdev); + } + } else { + /* Init failed, cleanup */ + flush_work(&hdev->tx_work); + flush_work(&hdev->cmd_work); + flush_work(&hdev->rx_work); + + skb_queue_purge(&hdev->cmd_q); + skb_queue_purge(&hdev->rx_q); + + if (hdev->flush) + hdev->flush(hdev); + + if (hdev->sent_cmd) { + kfree_skb(hdev->sent_cmd); + hdev->sent_cmd = NULL; + } + + hdev->close(hdev); + hdev->flags &= BIT(HCI_RAW); + } + +done: + hci_req_unlock(hdev); + return ret; +} + +/* ---- HCI ioctl helpers ---- */ + +int hci_dev_open(__u16 dev) +{ + struct hci_dev *hdev; + int err; + + hdev = hci_dev_get(dev); + if (!hdev) + return -ENODEV; + + /* Devices that are marked as unconfigured can only be powered + * up as user channel. Trying to bring them up as normal devices + * will result into a failure. Only user channel operation is + * possible. + * + * When this function is called for a user channel, the flag + * HCI_USER_CHANNEL will be set first before attempting to + * open the device. + */ + if (hci_dev_test_flag(hdev, HCI_UNCONFIGURED) && + !hci_dev_test_flag(hdev, HCI_USER_CHANNEL)) { + err = -EOPNOTSUPP; + goto done; + } + + /* We need to ensure that no other power on/off work is pending + * before proceeding to call hci_dev_do_open. This is + * particularly important if the setup procedure has not yet + * completed. + */ + if (hci_dev_test_and_clear_flag(hdev, HCI_AUTO_OFF)) + cancel_delayed_work(&hdev->power_off); + + /* After this call it is guaranteed that the setup procedure + * has finished. This means that error conditions like RFKILL + * or no valid public or static random address apply. + */ + flush_workqueue(hdev->req_workqueue); + + /* For controllers not using the management interface and that + * are brought up using legacy ioctl, set the HCI_BONDABLE bit + * so that pairing works for them. Once the management interface + * is in use this bit will be cleared again and userspace has + * to explicitly enable it. + */ + if (!hci_dev_test_flag(hdev, HCI_USER_CHANNEL) && + !hci_dev_test_flag(hdev, HCI_MGMT)) + hci_dev_set_flag(hdev, HCI_BONDABLE); + + err = hci_dev_do_open(hdev); + +done: + hci_dev_put(hdev); + return err; +} + +/* This function requires the caller holds hdev->lock */ +static void hci_pend_le_actions_clear(struct hci_dev *hdev) +{ + struct hci_conn_params *p; + + list_for_each_entry(p, &hdev->le_conn_params, list) { + if (p->conn) { + hci_conn_drop(p->conn); + hci_conn_put(p->conn); + p->conn = NULL; + } + list_del_init(&p->action); + } + + BT_DBG("All LE pending actions cleared"); +} + +static int hci_dev_do_close(struct hci_dev *hdev) +{ + BT_DBG("%s %p", hdev->name, hdev); + + if (!hci_dev_test_flag(hdev, HCI_UNREGISTER) && + test_bit(HCI_UP, &hdev->flags)) { + /* Execute vendor specific shutdown routine */ + if (hdev->shutdown) + hdev->shutdown(hdev); + } + + cancel_delayed_work(&hdev->power_off); + + hci_req_cancel(hdev, ENODEV); + hci_req_lock(hdev); + + if (!test_and_clear_bit(HCI_UP, &hdev->flags)) { + cancel_delayed_work_sync(&hdev->cmd_timer); + hci_req_unlock(hdev); + return 0; + } + + /* Flush RX and TX works */ + flush_work(&hdev->tx_work); + flush_work(&hdev->rx_work); + + if (hdev->discov_timeout > 0) { + cancel_delayed_work(&hdev->discov_off); + hdev->discov_timeout = 0; + hci_dev_clear_flag(hdev, HCI_DISCOVERABLE); + hci_dev_clear_flag(hdev, HCI_LIMITED_DISCOVERABLE); + } + + if (hci_dev_test_and_clear_flag(hdev, HCI_SERVICE_CACHE)) + cancel_delayed_work(&hdev->service_cache); + + cancel_delayed_work_sync(&hdev->le_scan_disable); + cancel_delayed_work_sync(&hdev->le_scan_restart); + + if (hci_dev_test_flag(hdev, HCI_MGMT)) + cancel_delayed_work_sync(&hdev->rpa_expired); + + /* Avoid potential lockdep warnings from the *_flush() calls by + * ensuring the workqueue is empty up front. + */ + drain_workqueue(hdev->workqueue); + + hci_dev_lock(hdev); + + hci_discovery_set_state(hdev, DISCOVERY_STOPPED); + + if (!hci_dev_test_and_clear_flag(hdev, HCI_AUTO_OFF)) { + if (hdev->dev_type == HCI_BREDR) + mgmt_powered(hdev, 0); + } + + hci_inquiry_cache_flush(hdev); + hci_pend_le_actions_clear(hdev); + hci_conn_hash_flush(hdev); + hci_dev_unlock(hdev); + + smp_unregister(hdev); + + hci_notify(hdev, HCI_DEV_DOWN); + + if (hdev->flush) + hdev->flush(hdev); + + /* Reset device */ + skb_queue_purge(&hdev->cmd_q); + atomic_set(&hdev->cmd_cnt, 1); + if (!hci_dev_test_flag(hdev, HCI_AUTO_OFF) && + !hci_dev_test_flag(hdev, HCI_UNCONFIGURED) && + test_bit(HCI_QUIRK_RESET_ON_CLOSE, &hdev->quirks)) { + set_bit(HCI_INIT, &hdev->flags); + __hci_req_sync(hdev, hci_reset_req, 0, HCI_CMD_TIMEOUT); + clear_bit(HCI_INIT, &hdev->flags); + } + + /* flush cmd work */ + flush_work(&hdev->cmd_work); + + /* Drop queues */ + skb_queue_purge(&hdev->rx_q); + skb_queue_purge(&hdev->cmd_q); + skb_queue_purge(&hdev->raw_q); + + /* Drop last sent command */ + if (hdev->sent_cmd) { + cancel_delayed_work_sync(&hdev->cmd_timer); + kfree_skb(hdev->sent_cmd); + hdev->sent_cmd = NULL; + } + + /* After this point our queues are empty + * and no tasks are scheduled. */ + hdev->close(hdev); + + /* Clear flags */ + hdev->flags &= BIT(HCI_RAW); + hci_dev_clear_volatile_flags(hdev); + + /* Controller radio is available but is currently powered down */ + hdev->amp_status = AMP_STATUS_POWERED_DOWN; + + memset(hdev->eir, 0, sizeof(hdev->eir)); + memset(hdev->dev_class, 0, sizeof(hdev->dev_class)); + bacpy(&hdev->random_addr, BDADDR_ANY); + + hci_req_unlock(hdev); + + hci_dev_put(hdev); + return 0; +} + +int hci_dev_close(__u16 dev) +{ + struct hci_dev *hdev; + int err; + + hdev = hci_dev_get(dev); + if (!hdev) + return -ENODEV; + + if (hci_dev_test_flag(hdev, HCI_USER_CHANNEL)) { + err = -EBUSY; + goto done; + } + + if (hci_dev_test_and_clear_flag(hdev, HCI_AUTO_OFF)) + cancel_delayed_work(&hdev->power_off); + + err = hci_dev_do_close(hdev); + +done: + hci_dev_put(hdev); + return err; +} + +static int hci_dev_do_reset(struct hci_dev *hdev) +{ + int ret; + + BT_DBG("%s %p", hdev->name, hdev); + + hci_req_lock(hdev); + + /* Drop queues */ + skb_queue_purge(&hdev->rx_q); + skb_queue_purge(&hdev->cmd_q); + + /* Avoid potential lockdep warnings from the *_flush() calls by + * ensuring the workqueue is empty up front. + */ + drain_workqueue(hdev->workqueue); + + hci_dev_lock(hdev); + hci_inquiry_cache_flush(hdev); + hci_conn_hash_flush(hdev); + hci_dev_unlock(hdev); + + if (hdev->flush) + hdev->flush(hdev); + + atomic_set(&hdev->cmd_cnt, 1); + hdev->acl_cnt = 0; hdev->sco_cnt = 0; hdev->le_cnt = 0; + + ret = __hci_req_sync(hdev, hci_reset_req, 0, HCI_INIT_TIMEOUT); + + hci_req_unlock(hdev); + return ret; +} + +int hci_dev_reset(__u16 dev) +{ + struct hci_dev *hdev; + int err; + + hdev = hci_dev_get(dev); + if (!hdev) + return -ENODEV; + + if (!test_bit(HCI_UP, &hdev->flags)) { + err = -ENETDOWN; + goto done; + } + + if (hci_dev_test_flag(hdev, HCI_USER_CHANNEL)) { + err = -EBUSY; + goto done; + } + + if (hci_dev_test_flag(hdev, HCI_UNCONFIGURED)) { + err = -EOPNOTSUPP; + goto done; + } + + err = hci_dev_do_reset(hdev); + +done: + hci_dev_put(hdev); + return err; +} + +int hci_dev_reset_stat(__u16 dev) +{ + struct hci_dev *hdev; + int ret = 0; + + hdev = hci_dev_get(dev); + if (!hdev) + return -ENODEV; + + if (hci_dev_test_flag(hdev, HCI_USER_CHANNEL)) { + ret = -EBUSY; + goto done; + } + + if (hci_dev_test_flag(hdev, HCI_UNCONFIGURED)) { + ret = -EOPNOTSUPP; + goto done; + } + + memset(&hdev->stat, 0, sizeof(struct hci_dev_stats)); + +done: + hci_dev_put(hdev); + return ret; +} + +static void hci_update_scan_state(struct hci_dev *hdev, u8 scan) +{ + bool conn_changed, discov_changed; + + BT_DBG("%s scan 0x%02x", hdev->name, scan); + + if ((scan & SCAN_PAGE)) + conn_changed = !hci_dev_test_and_set_flag(hdev, + HCI_CONNECTABLE); + else + conn_changed = hci_dev_test_and_clear_flag(hdev, + HCI_CONNECTABLE); + + if ((scan & SCAN_INQUIRY)) { + discov_changed = !hci_dev_test_and_set_flag(hdev, + HCI_DISCOVERABLE); + } else { + hci_dev_clear_flag(hdev, HCI_LIMITED_DISCOVERABLE); + discov_changed = hci_dev_test_and_clear_flag(hdev, + HCI_DISCOVERABLE); + } + + if (!hci_dev_test_flag(hdev, HCI_MGMT)) + return; + + if (conn_changed || discov_changed) { + /* In case this was disabled through mgmt */ + hci_dev_set_flag(hdev, HCI_BREDR_ENABLED); + + if (hci_dev_test_flag(hdev, HCI_LE_ENABLED)) + mgmt_update_adv_data(hdev); + + mgmt_new_settings(hdev); + } +} + +int hci_dev_cmd(unsigned int cmd, void __user *arg) +{ + struct hci_dev *hdev; + struct hci_dev_req dr; + int err = 0; + + if (copy_from_user(&dr, arg, sizeof(dr))) + return -EFAULT; + + hdev = hci_dev_get(dr.dev_id); + if (!hdev) + return -ENODEV; + + if (hci_dev_test_flag(hdev, HCI_USER_CHANNEL)) { + err = -EBUSY; + goto done; + } + + if (hci_dev_test_flag(hdev, HCI_UNCONFIGURED)) { + err = -EOPNOTSUPP; + goto done; + } + + if (hdev->dev_type != HCI_BREDR) { + err = -EOPNOTSUPP; + goto done; + } + + if (!hci_dev_test_flag(hdev, HCI_BREDR_ENABLED)) { + err = -EOPNOTSUPP; + goto done; + } + + switch (cmd) { + case HCISETAUTH: + err = hci_req_sync(hdev, hci_auth_req, dr.dev_opt, + HCI_INIT_TIMEOUT); + break; + + case HCISETENCRYPT: + if (!lmp_encrypt_capable(hdev)) { + err = -EOPNOTSUPP; + break; + } + + if (!test_bit(HCI_AUTH, &hdev->flags)) { + /* Auth must be enabled first */ + err = hci_req_sync(hdev, hci_auth_req, dr.dev_opt, + HCI_INIT_TIMEOUT); + if (err) + break; + } + + err = hci_req_sync(hdev, hci_encrypt_req, dr.dev_opt, + HCI_INIT_TIMEOUT); + break; + + case HCISETSCAN: + err = hci_req_sync(hdev, hci_scan_req, dr.dev_opt, + HCI_INIT_TIMEOUT); + + /* Ensure that the connectable and discoverable states + * get correctly modified as this was a non-mgmt change. + */ + if (!err) + hci_update_scan_state(hdev, dr.dev_opt); + break; + + case HCISETLINKPOL: + err = hci_req_sync(hdev, hci_linkpol_req, dr.dev_opt, + HCI_INIT_TIMEOUT); + break; + + case HCISETLINKMODE: + hdev->link_mode = ((__u16) dr.dev_opt) & + (HCI_LM_MASTER | HCI_LM_ACCEPT); + break; + + case HCISETPTYPE: + hdev->pkt_type = (__u16) dr.dev_opt; + break; + + case HCISETACLMTU: + hdev->acl_mtu = *((__u16 *) &dr.dev_opt + 1); + hdev->acl_pkts = *((__u16 *) &dr.dev_opt + 0); + break; + + case HCISETSCOMTU: + hdev->sco_mtu = *((__u16 *) &dr.dev_opt + 1); + hdev->sco_pkts = *((__u16 *) &dr.dev_opt + 0); + break; + + default: + err = -EINVAL; + break; + } + +done: + hci_dev_put(hdev); + return err; +} + +int hci_get_dev_list(void __user *arg) +{ + struct hci_dev *hdev; + struct hci_dev_list_req *dl; + struct hci_dev_req *dr; + int n = 0, size, err; + __u16 dev_num; + + if (get_user(dev_num, (__u16 __user *) arg)) + return -EFAULT; + + if (!dev_num || dev_num > (PAGE_SIZE * 2) / sizeof(*dr)) + return -EINVAL; + + size = sizeof(*dl) + dev_num * sizeof(*dr); + + dl = kzalloc(size, GFP_KERNEL); + if (!dl) + return -ENOMEM; + + dr = dl->dev_req; + + read_lock(&hci_dev_list_lock); + list_for_each_entry(hdev, &hci_dev_list, list) { + unsigned long flags = hdev->flags; + + /* When the auto-off is configured it means the transport + * is running, but in that case still indicate that the + * device is actually down. + */ + if (hci_dev_test_flag(hdev, HCI_AUTO_OFF)) + flags &= ~BIT(HCI_UP); + + (dr + n)->dev_id = hdev->id; + (dr + n)->dev_opt = flags; + + if (++n >= dev_num) + break; + } + read_unlock(&hci_dev_list_lock); + + dl->dev_num = n; + size = sizeof(*dl) + n * sizeof(*dr); + + err = copy_to_user(arg, dl, size); + kfree(dl); + + return err ? -EFAULT : 0; +} + +int hci_get_dev_info(void __user *arg) +{ + struct hci_dev *hdev; + struct hci_dev_info di; + unsigned long flags; + int err = 0; + + if (copy_from_user(&di, arg, sizeof(di))) + return -EFAULT; + + hdev = hci_dev_get(di.dev_id); + if (!hdev) + return -ENODEV; + + /* When the auto-off is configured it means the transport + * is running, but in that case still indicate that the + * device is actually down. + */ + if (hci_dev_test_flag(hdev, HCI_AUTO_OFF)) + flags = hdev->flags & ~BIT(HCI_UP); + else + flags = hdev->flags; + + strcpy(di.name, hdev->name); + di.bdaddr = hdev->bdaddr; + di.type = (hdev->bus & 0x0f) | ((hdev->dev_type & 0x03) << 4); + di.flags = flags; + di.pkt_type = hdev->pkt_type; + if (lmp_bredr_capable(hdev)) { + di.acl_mtu = hdev->acl_mtu; + di.acl_pkts = hdev->acl_pkts; + di.sco_mtu = hdev->sco_mtu; + di.sco_pkts = hdev->sco_pkts; + } else { + di.acl_mtu = hdev->le_mtu; + di.acl_pkts = hdev->le_pkts; + di.sco_mtu = 0; + di.sco_pkts = 0; + } + di.link_policy = hdev->link_policy; + di.link_mode = hdev->link_mode; + + memcpy(&di.stat, &hdev->stat, sizeof(di.stat)); + memcpy(&di.features, &hdev->features, sizeof(di.features)); + + if (copy_to_user(arg, &di, sizeof(di))) + err = -EFAULT; + + hci_dev_put(hdev); + + return err; +} + +/* ---- Interface to HCI drivers ---- */ + +static int hci_rfkill_set_block(void *data, bool blocked) +{ + struct hci_dev *hdev = data; + + BT_DBG("%p name %s blocked %d", hdev, hdev->name, blocked); + + if (hci_dev_test_flag(hdev, HCI_USER_CHANNEL)) + return -EBUSY; + + if (blocked) { + hci_dev_set_flag(hdev, HCI_RFKILLED); + if (!hci_dev_test_flag(hdev, HCI_SETUP) && + !hci_dev_test_flag(hdev, HCI_CONFIG)) + hci_dev_do_close(hdev); + } else { + hci_dev_clear_flag(hdev, HCI_RFKILLED); + } + + return 0; +} + +static const struct rfkill_ops hci_rfkill_ops = { + .set_block = hci_rfkill_set_block, +}; + +static void hci_power_on(struct work_struct *work) +{ + struct hci_dev *hdev = container_of(work, struct hci_dev, power_on); + int err; + + BT_DBG("%s", hdev->name); + + err = hci_dev_do_open(hdev); + if (err < 0) { + hci_dev_lock(hdev); + mgmt_set_powered_failed(hdev, err); + hci_dev_unlock(hdev); + return; + } + + /* During the HCI setup phase, a few error conditions are + * ignored and they need to be checked now. If they are still + * valid, it is important to turn the device back off. + */ + if (hci_dev_test_flag(hdev, HCI_RFKILLED) || + hci_dev_test_flag(hdev, HCI_UNCONFIGURED) || + (hdev->dev_type == HCI_BREDR && + !bacmp(&hdev->bdaddr, BDADDR_ANY) && + !bacmp(&hdev->static_addr, BDADDR_ANY))) { + hci_dev_clear_flag(hdev, HCI_AUTO_OFF); + hci_dev_do_close(hdev); + } else if (hci_dev_test_flag(hdev, HCI_AUTO_OFF)) { + queue_delayed_work(hdev->req_workqueue, &hdev->power_off, + HCI_AUTO_OFF_TIMEOUT); + } + + if (hci_dev_test_and_clear_flag(hdev, HCI_SETUP)) { + /* For unconfigured devices, set the HCI_RAW flag + * so that userspace can easily identify them. + */ + if (hci_dev_test_flag(hdev, HCI_UNCONFIGURED)) + set_bit(HCI_RAW, &hdev->flags); + + /* For fully configured devices, this will send + * the Index Added event. For unconfigured devices, + * it will send Unconfigued Index Added event. + * + * Devices with HCI_QUIRK_RAW_DEVICE are ignored + * and no event will be send. + */ + mgmt_index_added(hdev); + } else if (hci_dev_test_and_clear_flag(hdev, HCI_CONFIG)) { + /* When the controller is now configured, then it + * is important to clear the HCI_RAW flag. + */ + if (!hci_dev_test_flag(hdev, HCI_UNCONFIGURED)) + clear_bit(HCI_RAW, &hdev->flags); + + /* Powering on the controller with HCI_CONFIG set only + * happens with the transition from unconfigured to + * configured. This will send the Index Added event. + */ + mgmt_index_added(hdev); + } +} + +static void hci_power_off(struct work_struct *work) +{ + struct hci_dev *hdev = container_of(work, struct hci_dev, + power_off.work); + + BT_DBG("%s", hdev->name); + + hci_dev_do_close(hdev); +} + +static void hci_error_reset(struct work_struct *work) +{ + struct hci_dev *hdev = container_of(work, struct hci_dev, error_reset); + + BT_DBG("%s", hdev->name); + + if (hdev->hw_error) + hdev->hw_error(hdev, hdev->hw_error_code); + else + BT_ERR("%s hardware error 0x%2.2x", hdev->name, + hdev->hw_error_code); + + if (hci_dev_do_close(hdev)) + return; + + hci_dev_do_open(hdev); +} + +static void hci_discov_off(struct work_struct *work) +{ + struct hci_dev *hdev; + + hdev = container_of(work, struct hci_dev, discov_off.work); + + BT_DBG("%s", hdev->name); + + mgmt_discoverable_timeout(hdev); +} + +void hci_uuids_clear(struct hci_dev *hdev) +{ + struct bt_uuid *uuid, *tmp; + + list_for_each_entry_safe(uuid, tmp, &hdev->uuids, list) { + list_del(&uuid->list); + kfree(uuid); + } +} + +void hci_link_keys_clear(struct hci_dev *hdev) +{ + struct link_key *key; + + list_for_each_entry_rcu(key, &hdev->link_keys, list) { + list_del_rcu(&key->list); + kfree_rcu(key, rcu); + } +} + +void hci_smp_ltks_clear(struct hci_dev *hdev) +{ + struct smp_ltk *k; + + list_for_each_entry_rcu(k, &hdev->long_term_keys, list) { + list_del_rcu(&k->list); + kfree_rcu(k, rcu); + } +} + +void hci_smp_irks_clear(struct hci_dev *hdev) +{ + struct smp_irk *k; + + list_for_each_entry_rcu(k, &hdev->identity_resolving_keys, list) { + list_del_rcu(&k->list); + kfree_rcu(k, rcu); + } +} + +struct link_key *hci_find_link_key(struct hci_dev *hdev, bdaddr_t *bdaddr) +{ + struct link_key *k; + + rcu_read_lock(); + list_for_each_entry_rcu(k, &hdev->link_keys, list) { + if (bacmp(bdaddr, &k->bdaddr) == 0) { + rcu_read_unlock(); + return k; + } + } + rcu_read_unlock(); + + return NULL; +} + +static bool hci_persistent_key(struct hci_dev *hdev, struct hci_conn *conn, + u8 key_type, u8 old_key_type) +{ + /* Legacy key */ + if (key_type < 0x03) + return true; + + /* Debug keys are insecure so don't store them persistently */ + if (key_type == HCI_LK_DEBUG_COMBINATION) + return false; + + /* Changed combination key and there's no previous one */ + if (key_type == HCI_LK_CHANGED_COMBINATION && old_key_type == 0xff) + return false; + + /* Security mode 3 case */ + if (!conn) + return true; + + /* BR/EDR key derived using SC from an LE link */ + if (conn->type == LE_LINK) + return true; + + /* Neither local nor remote side had no-bonding as requirement */ + if (conn->auth_type > 0x01 && conn->remote_auth > 0x01) + return true; + + /* Local side had dedicated bonding as requirement */ + if (conn->auth_type == 0x02 || conn->auth_type == 0x03) + return true; + + /* Remote side had dedicated bonding as requirement */ + if (conn->remote_auth == 0x02 || conn->remote_auth == 0x03) + return true; + + /* If none of the above criteria match, then don't store the key + * persistently */ + return false; +} + +static u8 ltk_role(u8 type) +{ + if (type == SMP_LTK) + return HCI_ROLE_MASTER; + + return HCI_ROLE_SLAVE; +} + +struct smp_ltk *hci_find_ltk(struct hci_dev *hdev, bdaddr_t *bdaddr, + u8 addr_type, u8 role) +{ + struct smp_ltk *k; + + rcu_read_lock(); + list_for_each_entry_rcu(k, &hdev->long_term_keys, list) { + if (addr_type != k->bdaddr_type || bacmp(bdaddr, &k->bdaddr)) + continue; + + if (smp_ltk_is_sc(k) || ltk_role(k->type) == role) { + rcu_read_unlock(); + return k; + } + } + rcu_read_unlock(); + + return NULL; +} + +struct smp_irk *hci_find_irk_by_rpa(struct hci_dev *hdev, bdaddr_t *rpa) +{ + struct smp_irk *irk; + + rcu_read_lock(); + list_for_each_entry_rcu(irk, &hdev->identity_resolving_keys, list) { + if (!bacmp(&irk->rpa, rpa)) { + rcu_read_unlock(); + return irk; + } + } + + list_for_each_entry_rcu(irk, &hdev->identity_resolving_keys, list) { + if (smp_irk_matches(hdev, irk->val, rpa)) { + bacpy(&irk->rpa, rpa); + rcu_read_unlock(); + return irk; + } + } + rcu_read_unlock(); + + return NULL; +} + +struct smp_irk *hci_find_irk_by_addr(struct hci_dev *hdev, bdaddr_t *bdaddr, + u8 addr_type) +{ + struct smp_irk *irk; + + /* Identity Address must be public or static random */ + if (addr_type == ADDR_LE_DEV_RANDOM && (bdaddr->b[5] & 0xc0) != 0xc0) + return NULL; + + rcu_read_lock(); + list_for_each_entry_rcu(irk, &hdev->identity_resolving_keys, list) { + if (addr_type == irk->addr_type && + bacmp(bdaddr, &irk->bdaddr) == 0) { + rcu_read_unlock(); + return irk; + } + } + rcu_read_unlock(); + + return NULL; +} + +struct link_key *hci_add_link_key(struct hci_dev *hdev, struct hci_conn *conn, + bdaddr_t *bdaddr, u8 *val, u8 type, + u8 pin_len, bool *persistent) +{ + struct link_key *key, *old_key; + u8 old_key_type; + + old_key = hci_find_link_key(hdev, bdaddr); + if (old_key) { + old_key_type = old_key->type; + key = old_key; + } else { + old_key_type = conn ? conn->key_type : 0xff; + key = kzalloc(sizeof(*key), GFP_KERNEL); + if (!key) + return NULL; + list_add_rcu(&key->list, &hdev->link_keys); + } + + BT_DBG("%s key for %pMR type %u", hdev->name, bdaddr, type); + + /* Some buggy controller combinations generate a changed + * combination key for legacy pairing even when there's no + * previous key */ + if (type == HCI_LK_CHANGED_COMBINATION && + (!conn || conn->remote_auth == 0xff) && old_key_type == 0xff) { + type = HCI_LK_COMBINATION; + if (conn) + conn->key_type = type; + } + + bacpy(&key->bdaddr, bdaddr); + memcpy(key->val, val, HCI_LINK_KEY_SIZE); + key->pin_len = pin_len; + + if (type == HCI_LK_CHANGED_COMBINATION) + key->type = old_key_type; + else + key->type = type; + + if (persistent) + *persistent = hci_persistent_key(hdev, conn, type, + old_key_type); + + return key; +} + +struct smp_ltk *hci_add_ltk(struct hci_dev *hdev, bdaddr_t *bdaddr, + u8 addr_type, u8 type, u8 authenticated, + u8 tk[16], u8 enc_size, __le16 ediv, __le64 rand) +{ + struct smp_ltk *key, *old_key; + u8 role = ltk_role(type); + + old_key = hci_find_ltk(hdev, bdaddr, addr_type, role); + if (old_key) + key = old_key; + else { + key = kzalloc(sizeof(*key), GFP_KERNEL); + if (!key) + return NULL; + list_add_rcu(&key->list, &hdev->long_term_keys); + } + + bacpy(&key->bdaddr, bdaddr); + key->bdaddr_type = addr_type; + memcpy(key->val, tk, sizeof(key->val)); + key->authenticated = authenticated; + key->ediv = ediv; + key->rand = rand; + key->enc_size = enc_size; + key->type = type; + + return key; +} + +struct smp_irk *hci_add_irk(struct hci_dev *hdev, bdaddr_t *bdaddr, + u8 addr_type, u8 val[16], bdaddr_t *rpa) +{ + struct smp_irk *irk; + + irk = hci_find_irk_by_addr(hdev, bdaddr, addr_type); + if (!irk) { + irk = kzalloc(sizeof(*irk), GFP_KERNEL); + if (!irk) + return NULL; + + bacpy(&irk->bdaddr, bdaddr); + irk->addr_type = addr_type; + + list_add_rcu(&irk->list, &hdev->identity_resolving_keys); + } + + memcpy(irk->val, val, 16); + bacpy(&irk->rpa, rpa); + + return irk; +} + +int hci_remove_link_key(struct hci_dev *hdev, bdaddr_t *bdaddr) +{ + struct link_key *key; + + key = hci_find_link_key(hdev, bdaddr); + if (!key) + return -ENOENT; + + BT_DBG("%s removing %pMR", hdev->name, bdaddr); + + list_del_rcu(&key->list); + kfree_rcu(key, rcu); + + return 0; +} + +int hci_remove_ltk(struct hci_dev *hdev, bdaddr_t *bdaddr, u8 bdaddr_type) +{ + struct smp_ltk *k; + int removed = 0; + + list_for_each_entry_rcu(k, &hdev->long_term_keys, list) { + if (bacmp(bdaddr, &k->bdaddr) || k->bdaddr_type != bdaddr_type) + continue; + + BT_DBG("%s removing %pMR", hdev->name, bdaddr); + + list_del_rcu(&k->list); + kfree_rcu(k, rcu); + removed++; + } + + return removed ? 0 : -ENOENT; +} + +void hci_remove_irk(struct hci_dev *hdev, bdaddr_t *bdaddr, u8 addr_type) +{ + struct smp_irk *k; + + list_for_each_entry_rcu(k, &hdev->identity_resolving_keys, list) { + if (bacmp(bdaddr, &k->bdaddr) || k->addr_type != addr_type) + continue; + + BT_DBG("%s removing %pMR", hdev->name, bdaddr); + + list_del_rcu(&k->list); + kfree_rcu(k, rcu); + } +} + +bool hci_bdaddr_is_paired(struct hci_dev *hdev, bdaddr_t *bdaddr, u8 type) +{ + struct smp_ltk *k; + struct smp_irk *irk; + u8 addr_type; + + if (type == BDADDR_BREDR) { + if (hci_find_link_key(hdev, bdaddr)) + return true; + return false; + } + + /* Convert to HCI addr type which struct smp_ltk uses */ + if (type == BDADDR_LE_PUBLIC) + addr_type = ADDR_LE_DEV_PUBLIC; + else + addr_type = ADDR_LE_DEV_RANDOM; + + irk = hci_get_irk(hdev, bdaddr, addr_type); + if (irk) { + bdaddr = &irk->bdaddr; + addr_type = irk->addr_type; + } + + rcu_read_lock(); + list_for_each_entry_rcu(k, &hdev->long_term_keys, list) { + if (k->bdaddr_type == addr_type && !bacmp(bdaddr, &k->bdaddr)) { + rcu_read_unlock(); + return true; + } + } + rcu_read_unlock(); + + return false; +} + +/* HCI command timer function */ +static void hci_cmd_timeout(struct work_struct *work) +{ + struct hci_dev *hdev = container_of(work, struct hci_dev, + cmd_timer.work); + + if (hdev->sent_cmd) { + struct hci_command_hdr *sent = (void *) hdev->sent_cmd->data; + u16 opcode = __le16_to_cpu(sent->opcode); + + BT_ERR("%s command 0x%4.4x tx timeout", hdev->name, opcode); + } else { + BT_ERR("%s command tx timeout", hdev->name); + } + + atomic_set(&hdev->cmd_cnt, 1); + queue_work(hdev->workqueue, &hdev->cmd_work); +} + +struct oob_data *hci_find_remote_oob_data(struct hci_dev *hdev, + bdaddr_t *bdaddr, u8 bdaddr_type) +{ + struct oob_data *data; + + list_for_each_entry(data, &hdev->remote_oob_data, list) { + if (bacmp(bdaddr, &data->bdaddr) != 0) + continue; + if (data->bdaddr_type != bdaddr_type) + continue; + return data; + } + + return NULL; +} + +int hci_remove_remote_oob_data(struct hci_dev *hdev, bdaddr_t *bdaddr, + u8 bdaddr_type) +{ + struct oob_data *data; + + data = hci_find_remote_oob_data(hdev, bdaddr, bdaddr_type); + if (!data) + return -ENOENT; + + BT_DBG("%s removing %pMR (%u)", hdev->name, bdaddr, bdaddr_type); + + list_del(&data->list); + kfree(data); + + return 0; +} + +void hci_remote_oob_data_clear(struct hci_dev *hdev) +{ + struct oob_data *data, *n; + + list_for_each_entry_safe(data, n, &hdev->remote_oob_data, list) { + list_del(&data->list); + kfree(data); + } +} + +int hci_add_remote_oob_data(struct hci_dev *hdev, bdaddr_t *bdaddr, + u8 bdaddr_type, u8 *hash192, u8 *rand192, + u8 *hash256, u8 *rand256) +{ + struct oob_data *data; + + data = hci_find_remote_oob_data(hdev, bdaddr, bdaddr_type); + if (!data) { + data = kmalloc(sizeof(*data), GFP_KERNEL); + if (!data) + return -ENOMEM; + + bacpy(&data->bdaddr, bdaddr); + data->bdaddr_type = bdaddr_type; + list_add(&data->list, &hdev->remote_oob_data); + } + + if (hash192 && rand192) { + memcpy(data->hash192, hash192, sizeof(data->hash192)); + memcpy(data->rand192, rand192, sizeof(data->rand192)); + if (hash256 && rand256) + data->present = 0x03; + } else { + memset(data->hash192, 0, sizeof(data->hash192)); + memset(data->rand192, 0, sizeof(data->rand192)); + if (hash256 && rand256) + data->present = 0x02; + else + data->present = 0x00; + } + + if (hash256 && rand256) { + memcpy(data->hash256, hash256, sizeof(data->hash256)); + memcpy(data->rand256, rand256, sizeof(data->rand256)); + } else { + memset(data->hash256, 0, sizeof(data->hash256)); + memset(data->rand256, 0, sizeof(data->rand256)); + if (hash192 && rand192) + data->present = 0x01; + } + + BT_DBG("%s for %pMR", hdev->name, bdaddr); + + return 0; +} + +struct bdaddr_list *hci_bdaddr_list_lookup(struct list_head *bdaddr_list, + bdaddr_t *bdaddr, u8 type) +{ + struct bdaddr_list *b; + + list_for_each_entry(b, bdaddr_list, list) { + if (!bacmp(&b->bdaddr, bdaddr) && b->bdaddr_type == type) + return b; + } + + return NULL; +} + +void hci_bdaddr_list_clear(struct list_head *bdaddr_list) +{ + struct list_head *p, *n; + + list_for_each_safe(p, n, bdaddr_list) { + struct bdaddr_list *b = list_entry(p, struct bdaddr_list, list); + + list_del(p); + kfree(b); + } +} + +int hci_bdaddr_list_add(struct list_head *list, bdaddr_t *bdaddr, u8 type) +{ + struct bdaddr_list *entry; + + if (!bacmp(bdaddr, BDADDR_ANY)) + return -EBADF; + + if (hci_bdaddr_list_lookup(list, bdaddr, type)) + return -EEXIST; + + entry = kzalloc(sizeof(*entry), GFP_KERNEL); + if (!entry) + return -ENOMEM; + + bacpy(&entry->bdaddr, bdaddr); + entry->bdaddr_type = type; + + list_add(&entry->list, list); + + return 0; +} + +int hci_bdaddr_list_del(struct list_head *list, bdaddr_t *bdaddr, u8 type) +{ + struct bdaddr_list *entry; + + if (!bacmp(bdaddr, BDADDR_ANY)) { + hci_bdaddr_list_clear(list); + return 0; + } + + entry = hci_bdaddr_list_lookup(list, bdaddr, type); + if (!entry) + return -ENOENT; + + list_del(&entry->list); + kfree(entry); + + return 0; +} + +/* This function requires the caller holds hdev->lock */ +struct hci_conn_params *hci_conn_params_lookup(struct hci_dev *hdev, + bdaddr_t *addr, u8 addr_type) +{ + struct hci_conn_params *params; + + /* The conn params list only contains identity addresses */ + if (!hci_is_identity_address(addr, addr_type)) + return NULL; + + list_for_each_entry(params, &hdev->le_conn_params, list) { + if (bacmp(¶ms->addr, addr) == 0 && + params->addr_type == addr_type) { + return params; + } + } + + return NULL; +} + +/* This function requires the caller holds hdev->lock */ +struct hci_conn_params *hci_pend_le_action_lookup(struct list_head *list, + bdaddr_t *addr, u8 addr_type) +{ + struct hci_conn_params *param; + + /* The list only contains identity addresses */ + if (!hci_is_identity_address(addr, addr_type)) + return NULL; + + list_for_each_entry(param, list, action) { + if (bacmp(¶m->addr, addr) == 0 && + param->addr_type == addr_type) + return param; + } + + return NULL; +} + +/* This function requires the caller holds hdev->lock */ +struct hci_conn_params *hci_conn_params_add(struct hci_dev *hdev, + bdaddr_t *addr, u8 addr_type) +{ + struct hci_conn_params *params; + + if (!hci_is_identity_address(addr, addr_type)) + return NULL; + + params = hci_conn_params_lookup(hdev, addr, addr_type); + if (params) + return params; + + params = kzalloc(sizeof(*params), GFP_KERNEL); + if (!params) { + BT_ERR("Out of memory"); + return NULL; + } + + bacpy(¶ms->addr, addr); + params->addr_type = addr_type; + + list_add(¶ms->list, &hdev->le_conn_params); + INIT_LIST_HEAD(¶ms->action); + + params->conn_min_interval = hdev->le_conn_min_interval; + params->conn_max_interval = hdev->le_conn_max_interval; + params->conn_latency = hdev->le_conn_latency; + params->supervision_timeout = hdev->le_supv_timeout; + params->auto_connect = HCI_AUTO_CONN_DISABLED; + + BT_DBG("addr %pMR (type %u)", addr, addr_type); + + return params; +} + +static void hci_conn_params_free(struct hci_conn_params *params) +{ + if (params->conn) { + hci_conn_drop(params->conn); + hci_conn_put(params->conn); + } + + list_del(¶ms->action); + list_del(¶ms->list); + kfree(params); +} + +/* This function requires the caller holds hdev->lock */ +void hci_conn_params_del(struct hci_dev *hdev, bdaddr_t *addr, u8 addr_type) +{ + struct hci_conn_params *params; + + params = hci_conn_params_lookup(hdev, addr, addr_type); + if (!params) + return; + + hci_conn_params_free(params); + + hci_update_background_scan(hdev); + + BT_DBG("addr %pMR (type %u)", addr, addr_type); +} + +/* This function requires the caller holds hdev->lock */ +void hci_conn_params_clear_disabled(struct hci_dev *hdev) +{ + struct hci_conn_params *params, *tmp; + + list_for_each_entry_safe(params, tmp, &hdev->le_conn_params, list) { + if (params->auto_connect != HCI_AUTO_CONN_DISABLED) + continue; + list_del(¶ms->list); + kfree(params); + } + + BT_DBG("All LE disabled connection parameters were removed"); +} + +/* This function requires the caller holds hdev->lock */ +void hci_conn_params_clear_all(struct hci_dev *hdev) +{ + struct hci_conn_params *params, *tmp; + + list_for_each_entry_safe(params, tmp, &hdev->le_conn_params, list) + hci_conn_params_free(params); + + hci_update_background_scan(hdev); + + BT_DBG("All LE connection parameters were removed"); +} + +static void inquiry_complete(struct hci_dev *hdev, u8 status, u16 opcode) +{ + if (status) { + BT_ERR("Failed to start inquiry: status %d", status); + + hci_dev_lock(hdev); + hci_discovery_set_state(hdev, DISCOVERY_STOPPED); + hci_dev_unlock(hdev); + return; + } +} + +static void le_scan_disable_work_complete(struct hci_dev *hdev, u8 status, + u16 opcode) +{ + /* General inquiry access code (GIAC) */ + u8 lap[3] = { 0x33, 0x8b, 0x9e }; + struct hci_cp_inquiry cp; + int err; + + if (status) { + BT_ERR("Failed to disable LE scanning: status %d", status); + return; + } + + hdev->discovery.scan_start = 0; + + switch (hdev->discovery.type) { + case DISCOV_TYPE_LE: + hci_dev_lock(hdev); + hci_discovery_set_state(hdev, DISCOVERY_STOPPED); + hci_dev_unlock(hdev); + break; + + case DISCOV_TYPE_INTERLEAVED: + hci_dev_lock(hdev); + + if (test_bit(HCI_QUIRK_SIMULTANEOUS_DISCOVERY, + &hdev->quirks)) { + /* If we were running LE only scan, change discovery + * state. If we were running both LE and BR/EDR inquiry + * simultaneously, and BR/EDR inquiry is already + * finished, stop discovery, otherwise BR/EDR inquiry + * will stop discovery when finished. If we will resolve + * remote device name, do not change discovery state. + */ + if (!test_bit(HCI_INQUIRY, &hdev->flags) && + hdev->discovery.state != DISCOVERY_RESOLVING) + hci_discovery_set_state(hdev, + DISCOVERY_STOPPED); + } else { + struct hci_request req; + + hci_inquiry_cache_flush(hdev); + + hci_req_init(&req, hdev); + + memset(&cp, 0, sizeof(cp)); + memcpy(&cp.lap, lap, sizeof(cp.lap)); + cp.length = DISCOV_INTERLEAVED_INQUIRY_LEN; + hci_req_add(&req, HCI_OP_INQUIRY, sizeof(cp), &cp); + + err = hci_req_run(&req, inquiry_complete); + if (err) { + BT_ERR("Inquiry request failed: err %d", err); + hci_discovery_set_state(hdev, + DISCOVERY_STOPPED); + } + } + + hci_dev_unlock(hdev); + break; + } +} + +static void le_scan_disable_work(struct work_struct *work) +{ + struct hci_dev *hdev = container_of(work, struct hci_dev, + le_scan_disable.work); + struct hci_request req; + int err; + + BT_DBG("%s", hdev->name); + + cancel_delayed_work_sync(&hdev->le_scan_restart); + + hci_req_init(&req, hdev); + + hci_req_add_le_scan_disable(&req); + + err = hci_req_run(&req, le_scan_disable_work_complete); + if (err) + BT_ERR("Disable LE scanning request failed: err %d", err); +} + +static void le_scan_restart_work_complete(struct hci_dev *hdev, u8 status, + u16 opcode) +{ + unsigned long timeout, duration, scan_start, now; + + BT_DBG("%s", hdev->name); + + if (status) { + BT_ERR("Failed to restart LE scan: status %d", status); + return; + } + + if (!test_bit(HCI_QUIRK_STRICT_DUPLICATE_FILTER, &hdev->quirks) || + !hdev->discovery.scan_start) + return; + + /* When the scan was started, hdev->le_scan_disable has been queued + * after duration from scan_start. During scan restart this job + * has been canceled, and we need to queue it again after proper + * timeout, to make sure that scan does not run indefinitely. + */ + duration = hdev->discovery.scan_duration; + scan_start = hdev->discovery.scan_start; + now = jiffies; + if (now - scan_start <= duration) { + int elapsed; + + if (now >= scan_start) + elapsed = now - scan_start; + else + elapsed = ULONG_MAX - scan_start + now; + + timeout = duration - elapsed; + } else { + timeout = 0; + } + queue_delayed_work(hdev->workqueue, + &hdev->le_scan_disable, timeout); +} + +static void le_scan_restart_work(struct work_struct *work) +{ + struct hci_dev *hdev = container_of(work, struct hci_dev, + le_scan_restart.work); + struct hci_request req; + struct hci_cp_le_set_scan_enable cp; + int err; + + BT_DBG("%s", hdev->name); + + /* If controller is not scanning we are done. */ + if (!hci_dev_test_flag(hdev, HCI_LE_SCAN)) + return; + + hci_req_init(&req, hdev); + + hci_req_add_le_scan_disable(&req); + + memset(&cp, 0, sizeof(cp)); + cp.enable = LE_SCAN_ENABLE; + cp.filter_dup = LE_SCAN_FILTER_DUP_ENABLE; + hci_req_add(&req, HCI_OP_LE_SET_SCAN_ENABLE, sizeof(cp), &cp); + + err = hci_req_run(&req, le_scan_restart_work_complete); + if (err) + BT_ERR("Restart LE scan request failed: err %d", err); +} + +/* Copy the Identity Address of the controller. + * + * If the controller has a public BD_ADDR, then by default use that one. + * If this is a LE only controller without a public address, default to + * the static random address. + * + * For debugging purposes it is possible to force controllers with a + * public address to use the static random address instead. + * + * In case BR/EDR has been disabled on a dual-mode controller and + * userspace has configured a static address, then that address + * becomes the identity address instead of the public BR/EDR address. + */ +void hci_copy_identity_address(struct hci_dev *hdev, bdaddr_t *bdaddr, + u8 *bdaddr_type) +{ + if (hci_dev_test_flag(hdev, HCI_FORCE_STATIC_ADDR) || + !bacmp(&hdev->bdaddr, BDADDR_ANY) || + (!hci_dev_test_flag(hdev, HCI_BREDR_ENABLED) && + bacmp(&hdev->static_addr, BDADDR_ANY))) { + bacpy(bdaddr, &hdev->static_addr); + *bdaddr_type = ADDR_LE_DEV_RANDOM; + } else { + bacpy(bdaddr, &hdev->bdaddr); + *bdaddr_type = ADDR_LE_DEV_PUBLIC; + } +} + +/* Alloc HCI device */ +struct hci_dev *hci_alloc_dev(void) +{ + struct hci_dev *hdev; + + hdev = kzalloc(sizeof(*hdev), GFP_KERNEL); + if (!hdev) + return NULL; + + hdev->pkt_type = (HCI_DM1 | HCI_DH1 | HCI_HV1); + hdev->esco_type = (ESCO_HV1); + hdev->link_mode = (HCI_LM_ACCEPT); + hdev->num_iac = 0x01; /* One IAC support is mandatory */ + hdev->io_capability = 0x03; /* No Input No Output */ + hdev->manufacturer = 0xffff; /* Default to internal use */ + hdev->inq_tx_power = HCI_TX_POWER_INVALID; + hdev->adv_tx_power = HCI_TX_POWER_INVALID; + + hdev->sniff_max_interval = 800; + hdev->sniff_min_interval = 80; + + hdev->le_adv_channel_map = 0x07; + hdev->le_adv_min_interval = 0x0800; + hdev->le_adv_max_interval = 0x0800; + hdev->le_scan_interval = 0x0060; + hdev->le_scan_window = 0x0030; + hdev->le_conn_min_interval = 0x0028; + hdev->le_conn_max_interval = 0x0038; + hdev->le_conn_latency = 0x0000; + hdev->le_supv_timeout = 0x002a; + hdev->le_def_tx_len = 0x001b; + hdev->le_def_tx_time = 0x0148; + hdev->le_max_tx_len = 0x001b; + hdev->le_max_tx_time = 0x0148; + hdev->le_max_rx_len = 0x001b; + hdev->le_max_rx_time = 0x0148; + + hdev->rpa_timeout = HCI_DEFAULT_RPA_TIMEOUT; + hdev->discov_interleaved_timeout = DISCOV_INTERLEAVED_TIMEOUT; + hdev->conn_info_min_age = DEFAULT_CONN_INFO_MIN_AGE; + hdev->conn_info_max_age = DEFAULT_CONN_INFO_MAX_AGE; + + mutex_init(&hdev->lock); + mutex_init(&hdev->req_lock); + + INIT_LIST_HEAD(&hdev->mgmt_pending); + INIT_LIST_HEAD(&hdev->blacklist); + INIT_LIST_HEAD(&hdev->whitelist); + INIT_LIST_HEAD(&hdev->uuids); + INIT_LIST_HEAD(&hdev->link_keys); + INIT_LIST_HEAD(&hdev->long_term_keys); + INIT_LIST_HEAD(&hdev->identity_resolving_keys); + INIT_LIST_HEAD(&hdev->remote_oob_data); + INIT_LIST_HEAD(&hdev->le_white_list); + INIT_LIST_HEAD(&hdev->le_conn_params); + INIT_LIST_HEAD(&hdev->pend_le_conns); + INIT_LIST_HEAD(&hdev->pend_le_reports); + INIT_LIST_HEAD(&hdev->conn_hash.list); + + INIT_WORK(&hdev->rx_work, hci_rx_work); + INIT_WORK(&hdev->cmd_work, hci_cmd_work); + INIT_WORK(&hdev->tx_work, hci_tx_work); + INIT_WORK(&hdev->power_on, hci_power_on); + INIT_WORK(&hdev->error_reset, hci_error_reset); + + INIT_DELAYED_WORK(&hdev->power_off, hci_power_off); + INIT_DELAYED_WORK(&hdev->discov_off, hci_discov_off); + INIT_DELAYED_WORK(&hdev->le_scan_disable, le_scan_disable_work); + INIT_DELAYED_WORK(&hdev->le_scan_restart, le_scan_restart_work); + + skb_queue_head_init(&hdev->rx_q); + skb_queue_head_init(&hdev->cmd_q); + skb_queue_head_init(&hdev->raw_q); + + init_waitqueue_head(&hdev->req_wait_q); + + INIT_DELAYED_WORK(&hdev->cmd_timer, hci_cmd_timeout); + + hci_init_sysfs(hdev); + discovery_init(hdev); + adv_info_init(hdev); + + return hdev; +} +EXPORT_SYMBOL(hci_alloc_dev); + +/* Free HCI device */ +void hci_free_dev(struct hci_dev *hdev) +{ + /* will free via device release */ + put_device(&hdev->dev); +} +EXPORT_SYMBOL(hci_free_dev); + +/* Register HCI device */ +int hci_register_dev(struct hci_dev *hdev) +{ + int id, error; + + if (!hdev->open || !hdev->close || !hdev->send) + return -EINVAL; + + /* Do not allow HCI_AMP devices to register at index 0, + * so the index can be used as the AMP controller ID. + */ + switch (hdev->dev_type) { + case HCI_BREDR: + id = ida_simple_get(&hci_index_ida, 0, 0, GFP_KERNEL); + break; + case HCI_AMP: + id = ida_simple_get(&hci_index_ida, 1, 0, GFP_KERNEL); + break; + default: + return -EINVAL; + } + + if (id < 0) + return id; + + sprintf(hdev->name, "hci%d", id); + hdev->id = id; + + BT_DBG("%p name %s bus %d", hdev, hdev->name, hdev->bus); + + hdev->workqueue = alloc_workqueue("%s", WQ_HIGHPRI | WQ_UNBOUND | + WQ_MEM_RECLAIM, 1, hdev->name); + if (!hdev->workqueue) { + error = -ENOMEM; + goto err; + } + + hdev->req_workqueue = alloc_workqueue("%s", WQ_HIGHPRI | WQ_UNBOUND | + WQ_MEM_RECLAIM, 1, hdev->name); + if (!hdev->req_workqueue) { + destroy_workqueue(hdev->workqueue); + error = -ENOMEM; + goto err; + } + + if (!IS_ERR_OR_NULL(bt_debugfs)) + hdev->debugfs = debugfs_create_dir(hdev->name, bt_debugfs); + + dev_set_name(&hdev->dev, "%s", hdev->name); + + error = device_add(&hdev->dev); + if (error < 0) + goto err_wqueue; + + hdev->rfkill = rfkill_alloc(hdev->name, &hdev->dev, + RFKILL_TYPE_BLUETOOTH, &hci_rfkill_ops, + hdev); + if (hdev->rfkill) { + if (rfkill_register(hdev->rfkill) < 0) { + rfkill_destroy(hdev->rfkill); + hdev->rfkill = NULL; + } + } + + if (hdev->rfkill && rfkill_blocked(hdev->rfkill)) + hci_dev_set_flag(hdev, HCI_RFKILLED); + + hci_dev_set_flag(hdev, HCI_SETUP); + hci_dev_set_flag(hdev, HCI_AUTO_OFF); + + if (hdev->dev_type == HCI_BREDR) { + /* Assume BR/EDR support until proven otherwise (such as + * through reading supported features during init. + */ + hci_dev_set_flag(hdev, HCI_BREDR_ENABLED); + } + + write_lock(&hci_dev_list_lock); + list_add(&hdev->list, &hci_dev_list); + write_unlock(&hci_dev_list_lock); + + /* Devices that are marked for raw-only usage are unconfigured + * and should not be included in normal operation. + */ + if (test_bit(HCI_QUIRK_RAW_DEVICE, &hdev->quirks)) + hci_dev_set_flag(hdev, HCI_UNCONFIGURED); + + hci_notify(hdev, HCI_DEV_REG); + hci_dev_hold(hdev); + + queue_work(hdev->req_workqueue, &hdev->power_on); + + return id; + +err_wqueue: + destroy_workqueue(hdev->workqueue); + destroy_workqueue(hdev->req_workqueue); +err: + ida_simple_remove(&hci_index_ida, hdev->id); + + return error; +} +EXPORT_SYMBOL(hci_register_dev); + +/* Unregister HCI device */ +void hci_unregister_dev(struct hci_dev *hdev) +{ + int id; + + BT_DBG("%p name %s bus %d", hdev, hdev->name, hdev->bus); + + hci_dev_set_flag(hdev, HCI_UNREGISTER); + + id = hdev->id; + + write_lock(&hci_dev_list_lock); + list_del(&hdev->list); + write_unlock(&hci_dev_list_lock); + + hci_dev_do_close(hdev); + + cancel_work_sync(&hdev->power_on); + + if (!test_bit(HCI_INIT, &hdev->flags) && + !hci_dev_test_flag(hdev, HCI_SETUP) && + !hci_dev_test_flag(hdev, HCI_CONFIG)) { + hci_dev_lock(hdev); + mgmt_index_removed(hdev); + hci_dev_unlock(hdev); + } + + /* mgmt_index_removed should take care of emptying the + * pending list */ + BUG_ON(!list_empty(&hdev->mgmt_pending)); + + hci_notify(hdev, HCI_DEV_UNREG); + + if (hdev->rfkill) { + rfkill_unregister(hdev->rfkill); + rfkill_destroy(hdev->rfkill); + } + + device_del(&hdev->dev); + + debugfs_remove_recursive(hdev->debugfs); + + destroy_workqueue(hdev->workqueue); + destroy_workqueue(hdev->req_workqueue); + + hci_dev_lock(hdev); + hci_bdaddr_list_clear(&hdev->blacklist); + hci_bdaddr_list_clear(&hdev->whitelist); + hci_uuids_clear(hdev); + hci_link_keys_clear(hdev); + hci_smp_ltks_clear(hdev); + hci_smp_irks_clear(hdev); + hci_remote_oob_data_clear(hdev); + hci_bdaddr_list_clear(&hdev->le_white_list); + hci_conn_params_clear_all(hdev); + hci_discovery_filter_clear(hdev); + hci_dev_unlock(hdev); + + hci_dev_put(hdev); + + ida_simple_remove(&hci_index_ida, id); +} +EXPORT_SYMBOL(hci_unregister_dev); + +/* Suspend HCI device */ +int hci_suspend_dev(struct hci_dev *hdev) +{ + hci_notify(hdev, HCI_DEV_SUSPEND); + return 0; +} +EXPORT_SYMBOL(hci_suspend_dev); + +/* Resume HCI device */ +int hci_resume_dev(struct hci_dev *hdev) +{ + hci_notify(hdev, HCI_DEV_RESUME); + return 0; +} +EXPORT_SYMBOL(hci_resume_dev); + +/* Reset HCI device */ +int hci_reset_dev(struct hci_dev *hdev) +{ + const u8 hw_err[] = { HCI_EV_HARDWARE_ERROR, 0x01, 0x00 }; + struct sk_buff *skb; + + skb = bt_skb_alloc(3, GFP_ATOMIC); + if (!skb) + return -ENOMEM; + + bt_cb(skb)->pkt_type = HCI_EVENT_PKT; + memcpy(skb_put(skb, 3), hw_err, 3); + + /* Send Hardware Error to upper stack */ + return hci_recv_frame(hdev, skb); +} +EXPORT_SYMBOL(hci_reset_dev); + +/* Receive frame from HCI drivers */ +int hci_recv_frame(struct hci_dev *hdev, struct sk_buff *skb) +{ + if (!hdev || (!test_bit(HCI_UP, &hdev->flags) + && !test_bit(HCI_INIT, &hdev->flags))) { + kfree_skb(skb); + return -ENXIO; + } + + /* Incoming skb */ + bt_cb(skb)->incoming = 1; + + /* Time stamp */ + __net_timestamp(skb); + + skb_queue_tail(&hdev->rx_q, skb); + queue_work(hdev->workqueue, &hdev->rx_work); + + return 0; +} +EXPORT_SYMBOL(hci_recv_frame); + +/* ---- Interface to upper protocols ---- */ + +int hci_register_cb(struct hci_cb *cb) +{ + BT_DBG("%p name %s", cb, cb->name); + + mutex_lock(&hci_cb_list_lock); + list_add_tail(&cb->list, &hci_cb_list); + mutex_unlock(&hci_cb_list_lock); + + return 0; +} +EXPORT_SYMBOL(hci_register_cb); + +int hci_unregister_cb(struct hci_cb *cb) +{ + BT_DBG("%p name %s", cb, cb->name); + + mutex_lock(&hci_cb_list_lock); + list_del(&cb->list); + mutex_unlock(&hci_cb_list_lock); + + return 0; +} +EXPORT_SYMBOL(hci_unregister_cb); + +static void hci_send_frame(struct hci_dev *hdev, struct sk_buff *skb) +{ + int err; + + BT_DBG("%s type %d len %d", hdev->name, bt_cb(skb)->pkt_type, skb->len); + + /* Time stamp */ + __net_timestamp(skb); + + /* Send copy to monitor */ + hci_send_to_monitor(hdev, skb); + + if (atomic_read(&hdev->promisc)) { + /* Send copy to the sockets */ + hci_send_to_sock(hdev, skb); + } + + /* Get rid of skb owner, prior to sending to the driver. */ + skb_orphan(skb); + + err = hdev->send(hdev, skb); + if (err < 0) { + BT_ERR("%s sending frame failed (%d)", hdev->name, err); + kfree_skb(skb); + } +} + +/* Send HCI command */ +int hci_send_cmd(struct hci_dev *hdev, __u16 opcode, __u32 plen, + const void *param) +{ + struct sk_buff *skb; + + BT_DBG("%s opcode 0x%4.4x plen %d", hdev->name, opcode, plen); + + skb = hci_prepare_cmd(hdev, opcode, plen, param); + if (!skb) { + BT_ERR("%s no memory for command", hdev->name); + return -ENOMEM; + } + + /* Stand-alone HCI commands must be flagged as + * single-command requests. + */ + bt_cb(skb)->req.start = true; + + skb_queue_tail(&hdev->cmd_q, skb); + queue_work(hdev->workqueue, &hdev->cmd_work); + + return 0; +} + +/* Get data from the previously sent command */ +void *hci_sent_cmd_data(struct hci_dev *hdev, __u16 opcode) +{ + struct hci_command_hdr *hdr; + + if (!hdev->sent_cmd) + return NULL; + + hdr = (void *) hdev->sent_cmd->data; + + if (hdr->opcode != cpu_to_le16(opcode)) + return NULL; + + BT_DBG("%s opcode 0x%4.4x", hdev->name, opcode); + + return hdev->sent_cmd->data + HCI_COMMAND_HDR_SIZE; +} + +/* Send ACL data */ +static void hci_add_acl_hdr(struct sk_buff *skb, __u16 handle, __u16 flags) +{ + struct hci_acl_hdr *hdr; + int len = skb->len; + + skb_push(skb, HCI_ACL_HDR_SIZE); + skb_reset_transport_header(skb); + hdr = (struct hci_acl_hdr *)skb_transport_header(skb); + hdr->handle = cpu_to_le16(hci_handle_pack(handle, flags)); + hdr->dlen = cpu_to_le16(len); +} + +static void hci_queue_acl(struct hci_chan *chan, struct sk_buff_head *queue, + struct sk_buff *skb, __u16 flags) +{ + struct hci_conn *conn = chan->conn; + struct hci_dev *hdev = conn->hdev; + struct sk_buff *list; + + skb->len = skb_headlen(skb); + skb->data_len = 0; + + bt_cb(skb)->pkt_type = HCI_ACLDATA_PKT; + + switch (hdev->dev_type) { + case HCI_BREDR: + hci_add_acl_hdr(skb, conn->handle, flags); + break; + case HCI_AMP: + hci_add_acl_hdr(skb, chan->handle, flags); + break; + default: + BT_ERR("%s unknown dev_type %d", hdev->name, hdev->dev_type); + return; + } + + list = skb_shinfo(skb)->frag_list; + if (!list) { + /* Non fragmented */ + BT_DBG("%s nonfrag skb %p len %d", hdev->name, skb, skb->len); + + skb_queue_tail(queue, skb); + } else { + /* Fragmented */ + BT_DBG("%s frag %p len %d", hdev->name, skb, skb->len); + + skb_shinfo(skb)->frag_list = NULL; + + /* Queue all fragments atomically. We need to use spin_lock_bh + * here because of 6LoWPAN links, as there this function is + * called from softirq and using normal spin lock could cause + * deadlocks. + */ + spin_lock_bh(&queue->lock); + + __skb_queue_tail(queue, skb); + + flags &= ~ACL_START; + flags |= ACL_CONT; + do { + skb = list; list = list->next; + + bt_cb(skb)->pkt_type = HCI_ACLDATA_PKT; + hci_add_acl_hdr(skb, conn->handle, flags); + + BT_DBG("%s frag %p len %d", hdev->name, skb, skb->len); + + __skb_queue_tail(queue, skb); + } while (list); + + spin_unlock_bh(&queue->lock); + } +} + +void hci_send_acl(struct hci_chan *chan, struct sk_buff *skb, __u16 flags) +{ + struct hci_dev *hdev = chan->conn->hdev; + + BT_DBG("%s chan %p flags 0x%4.4x", hdev->name, chan, flags); + + hci_queue_acl(chan, &chan->data_q, skb, flags); + + queue_work(hdev->workqueue, &hdev->tx_work); +} + +/* Send SCO data */ +void hci_send_sco(struct hci_conn *conn, struct sk_buff *skb) +{ + struct hci_dev *hdev = conn->hdev; + struct hci_sco_hdr hdr; + + BT_DBG("%s len %d", hdev->name, skb->len); + + hdr.handle = cpu_to_le16(conn->handle); + hdr.dlen = skb->len; + + skb_push(skb, HCI_SCO_HDR_SIZE); + skb_reset_transport_header(skb); + memcpy(skb_transport_header(skb), &hdr, HCI_SCO_HDR_SIZE); + + bt_cb(skb)->pkt_type = HCI_SCODATA_PKT; + + skb_queue_tail(&conn->data_q, skb); + queue_work(hdev->workqueue, &hdev->tx_work); +} + +/* ---- HCI TX task (outgoing data) ---- */ + +/* HCI Connection scheduler */ +static struct hci_conn *hci_low_sent(struct hci_dev *hdev, __u8 type, + int *quote) +{ + struct hci_conn_hash *h = &hdev->conn_hash; + struct hci_conn *conn = NULL, *c; + unsigned int num = 0, min = ~0; + + /* We don't have to lock device here. Connections are always + * added and removed with TX task disabled. */ + + rcu_read_lock(); + + list_for_each_entry_rcu(c, &h->list, list) { + if (c->type != type || skb_queue_empty(&c->data_q)) + continue; + + if (c->state != BT_CONNECTED && c->state != BT_CONFIG) + continue; + + num++; + + if (c->sent < min) { + min = c->sent; + conn = c; + } + + if (hci_conn_num(hdev, type) == num) + break; + } + + rcu_read_unlock(); + + if (conn) { + int cnt, q; + + switch (conn->type) { + case ACL_LINK: + cnt = hdev->acl_cnt; + break; + case SCO_LINK: + case ESCO_LINK: + cnt = hdev->sco_cnt; + break; + case LE_LINK: + cnt = hdev->le_mtu ? hdev->le_cnt : hdev->acl_cnt; + break; + default: + cnt = 0; + BT_ERR("Unknown link type"); + } + + q = cnt / num; + *quote = q ? q : 1; + } else + *quote = 0; + + BT_DBG("conn %p quote %d", conn, *quote); + return conn; +} + +static void hci_link_tx_to(struct hci_dev *hdev, __u8 type) +{ + struct hci_conn_hash *h = &hdev->conn_hash; + struct hci_conn *c; + + BT_ERR("%s link tx timeout", hdev->name); + + rcu_read_lock(); + + /* Kill stalled connections */ + list_for_each_entry_rcu(c, &h->list, list) { + if (c->type == type && c->sent) { + BT_ERR("%s killing stalled connection %pMR", + hdev->name, &c->dst); + hci_disconnect(c, HCI_ERROR_REMOTE_USER_TERM); + } + } + + rcu_read_unlock(); +} + +static struct hci_chan *hci_chan_sent(struct hci_dev *hdev, __u8 type, + int *quote) +{ + struct hci_conn_hash *h = &hdev->conn_hash; + struct hci_chan *chan = NULL; + unsigned int num = 0, min = ~0, cur_prio = 0; + struct hci_conn *conn; + int cnt, q, conn_num = 0; + + BT_DBG("%s", hdev->name); + + rcu_read_lock(); + + list_for_each_entry_rcu(conn, &h->list, list) { + struct hci_chan *tmp; + + if (conn->type != type) + continue; + + if (conn->state != BT_CONNECTED && conn->state != BT_CONFIG) + continue; + + conn_num++; + + list_for_each_entry_rcu(tmp, &conn->chan_list, list) { + struct sk_buff *skb; + + if (skb_queue_empty(&tmp->data_q)) + continue; + + skb = skb_peek(&tmp->data_q); + if (skb->priority < cur_prio) + continue; + + if (skb->priority > cur_prio) { + num = 0; + min = ~0; + cur_prio = skb->priority; + } + + num++; + + if (conn->sent < min) { + min = conn->sent; + chan = tmp; + } + } + + if (hci_conn_num(hdev, type) == conn_num) + break; + } + + rcu_read_unlock(); + + if (!chan) + return NULL; + + switch (chan->conn->type) { + case ACL_LINK: + cnt = hdev->acl_cnt; + break; + case AMP_LINK: + cnt = hdev->block_cnt; + break; + case SCO_LINK: + case ESCO_LINK: + cnt = hdev->sco_cnt; + break; + case LE_LINK: + cnt = hdev->le_mtu ? hdev->le_cnt : hdev->acl_cnt; + break; + default: + cnt = 0; + BT_ERR("Unknown link type"); + } + + q = cnt / num; + *quote = q ? q : 1; + BT_DBG("chan %p quote %d", chan, *quote); + return chan; +} + +static void hci_prio_recalculate(struct hci_dev *hdev, __u8 type) +{ + struct hci_conn_hash *h = &hdev->conn_hash; + struct hci_conn *conn; + int num = 0; + + BT_DBG("%s", hdev->name); + + rcu_read_lock(); + + list_for_each_entry_rcu(conn, &h->list, list) { + struct hci_chan *chan; + + if (conn->type != type) + continue; + + if (conn->state != BT_CONNECTED && conn->state != BT_CONFIG) + continue; + + num++; + + list_for_each_entry_rcu(chan, &conn->chan_list, list) { + struct sk_buff *skb; + + if (chan->sent) { + chan->sent = 0; + continue; + } + + if (skb_queue_empty(&chan->data_q)) + continue; + + skb = skb_peek(&chan->data_q); + if (skb->priority >= HCI_PRIO_MAX - 1) + continue; + + skb->priority = HCI_PRIO_MAX - 1; + + BT_DBG("chan %p skb %p promoted to %d", chan, skb, + skb->priority); + } + + if (hci_conn_num(hdev, type) == num) + break; + } + + rcu_read_unlock(); + +} + +static inline int __get_blocks(struct hci_dev *hdev, struct sk_buff *skb) +{ + /* Calculate count of blocks used by this packet */ + return DIV_ROUND_UP(skb->len - HCI_ACL_HDR_SIZE, hdev->block_len); +} + +static void __check_timeout(struct hci_dev *hdev, unsigned int cnt) +{ + if (!hci_dev_test_flag(hdev, HCI_UNCONFIGURED)) { + /* ACL tx timeout must be longer than maximum + * link supervision timeout (40.9 seconds) */ + if (!cnt && time_after(jiffies, hdev->acl_last_tx + + HCI_ACL_TX_TIMEOUT)) + hci_link_tx_to(hdev, ACL_LINK); + } +} + +static void hci_sched_acl_pkt(struct hci_dev *hdev) +{ + unsigned int cnt = hdev->acl_cnt; + struct hci_chan *chan; + struct sk_buff *skb; + int quote; + + __check_timeout(hdev, cnt); + + while (hdev->acl_cnt && + (chan = hci_chan_sent(hdev, ACL_LINK, "e))) { + u32 priority = (skb_peek(&chan->data_q))->priority; + while (quote-- && (skb = skb_peek(&chan->data_q))) { + BT_DBG("chan %p skb %p len %d priority %u", chan, skb, + skb->len, skb->priority); + + /* Stop if priority has changed */ + if (skb->priority < priority) + break; + + skb = skb_dequeue(&chan->data_q); + + hci_conn_enter_active_mode(chan->conn, + bt_cb(skb)->force_active); + + hci_send_frame(hdev, skb); + hdev->acl_last_tx = jiffies; + + hdev->acl_cnt--; + chan->sent++; + chan->conn->sent++; + } + } + + if (cnt != hdev->acl_cnt) + hci_prio_recalculate(hdev, ACL_LINK); +} + +static void hci_sched_acl_blk(struct hci_dev *hdev) +{ + unsigned int cnt = hdev->block_cnt; + struct hci_chan *chan; + struct sk_buff *skb; + int quote; + u8 type; + + __check_timeout(hdev, cnt); + + BT_DBG("%s", hdev->name); + + if (hdev->dev_type == HCI_AMP) + type = AMP_LINK; + else + type = ACL_LINK; + + while (hdev->block_cnt > 0 && + (chan = hci_chan_sent(hdev, type, "e))) { + u32 priority = (skb_peek(&chan->data_q))->priority; + while (quote > 0 && (skb = skb_peek(&chan->data_q))) { + int blocks; + + BT_DBG("chan %p skb %p len %d priority %u", chan, skb, + skb->len, skb->priority); + + /* Stop if priority has changed */ + if (skb->priority < priority) + break; + + skb = skb_dequeue(&chan->data_q); + + blocks = __get_blocks(hdev, skb); + if (blocks > hdev->block_cnt) + return; + + hci_conn_enter_active_mode(chan->conn, + bt_cb(skb)->force_active); + + hci_send_frame(hdev, skb); + hdev->acl_last_tx = jiffies; + + hdev->block_cnt -= blocks; + quote -= blocks; + + chan->sent += blocks; + chan->conn->sent += blocks; + } + } + + if (cnt != hdev->block_cnt) + hci_prio_recalculate(hdev, type); +} + +static void hci_sched_acl(struct hci_dev *hdev) +{ + BT_DBG("%s", hdev->name); + + /* No ACL link over BR/EDR controller */ + if (!hci_conn_num(hdev, ACL_LINK) && hdev->dev_type == HCI_BREDR) + return; + + /* No AMP link over AMP controller */ + if (!hci_conn_num(hdev, AMP_LINK) && hdev->dev_type == HCI_AMP) + return; + + switch (hdev->flow_ctl_mode) { + case HCI_FLOW_CTL_MODE_PACKET_BASED: + hci_sched_acl_pkt(hdev); + break; + + case HCI_FLOW_CTL_MODE_BLOCK_BASED: + hci_sched_acl_blk(hdev); + break; + } +} + +/* Schedule SCO */ +static void hci_sched_sco(struct hci_dev *hdev) +{ + struct hci_conn *conn; + struct sk_buff *skb; + int quote; + + BT_DBG("%s", hdev->name); + + if (!hci_conn_num(hdev, SCO_LINK)) + return; + + while (hdev->sco_cnt && (conn = hci_low_sent(hdev, SCO_LINK, "e))) { + while (quote-- && (skb = skb_dequeue(&conn->data_q))) { + BT_DBG("skb %p len %d", skb, skb->len); + hci_send_frame(hdev, skb); + + conn->sent++; + if (conn->sent == ~0) + conn->sent = 0; + } + } +} + +static void hci_sched_esco(struct hci_dev *hdev) +{ + struct hci_conn *conn; + struct sk_buff *skb; + int quote; + + BT_DBG("%s", hdev->name); + + if (!hci_conn_num(hdev, ESCO_LINK)) + return; + + while (hdev->sco_cnt && (conn = hci_low_sent(hdev, ESCO_LINK, + "e))) { + while (quote-- && (skb = skb_dequeue(&conn->data_q))) { + BT_DBG("skb %p len %d", skb, skb->len); + hci_send_frame(hdev, skb); + + conn->sent++; + if (conn->sent == ~0) + conn->sent = 0; + } + } +} + +static void hci_sched_le(struct hci_dev *hdev) +{ + struct hci_chan *chan; + struct sk_buff *skb; + int quote, cnt, tmp; + + BT_DBG("%s", hdev->name); + + if (!hci_conn_num(hdev, LE_LINK)) + return; + + if (!hci_dev_test_flag(hdev, HCI_UNCONFIGURED)) { + /* LE tx timeout must be longer than maximum + * link supervision timeout (40.9 seconds) */ + if (!hdev->le_cnt && hdev->le_pkts && + time_after(jiffies, hdev->le_last_tx + HZ * 45)) + hci_link_tx_to(hdev, LE_LINK); + } + + cnt = hdev->le_pkts ? hdev->le_cnt : hdev->acl_cnt; + tmp = cnt; + while (cnt && (chan = hci_chan_sent(hdev, LE_LINK, "e))) { + u32 priority = (skb_peek(&chan->data_q))->priority; + while (quote-- && (skb = skb_peek(&chan->data_q))) { + BT_DBG("chan %p skb %p len %d priority %u", chan, skb, + skb->len, skb->priority); + + /* Stop if priority has changed */ + if (skb->priority < priority) + break; + + skb = skb_dequeue(&chan->data_q); + + hci_send_frame(hdev, skb); + hdev->le_last_tx = jiffies; + + cnt--; + chan->sent++; + chan->conn->sent++; + } + } + + if (hdev->le_pkts) + hdev->le_cnt = cnt; + else + hdev->acl_cnt = cnt; + + if (cnt != tmp) + hci_prio_recalculate(hdev, LE_LINK); +} + +static void hci_tx_work(struct work_struct *work) +{ + struct hci_dev *hdev = container_of(work, struct hci_dev, tx_work); + struct sk_buff *skb; + + BT_DBG("%s acl %d sco %d le %d", hdev->name, hdev->acl_cnt, + hdev->sco_cnt, hdev->le_cnt); + + if (!hci_dev_test_flag(hdev, HCI_USER_CHANNEL)) { + /* Schedule queues and send stuff to HCI driver */ + hci_sched_acl(hdev); + hci_sched_sco(hdev); + hci_sched_esco(hdev); + hci_sched_le(hdev); + } + + /* Send next queued raw (unknown type) packet */ + while ((skb = skb_dequeue(&hdev->raw_q))) + hci_send_frame(hdev, skb); +} + +/* ----- HCI RX task (incoming data processing) ----- */ + +/* ACL data packet */ +static void hci_acldata_packet(struct hci_dev *hdev, struct sk_buff *skb) +{ + struct hci_acl_hdr *hdr = (void *) skb->data; + struct hci_conn *conn; + __u16 handle, flags; + + skb_pull(skb, HCI_ACL_HDR_SIZE); + + handle = __le16_to_cpu(hdr->handle); + flags = hci_flags(handle); + handle = hci_handle(handle); + + BT_DBG("%s len %d handle 0x%4.4x flags 0x%4.4x", hdev->name, skb->len, + handle, flags); + + hdev->stat.acl_rx++; + + hci_dev_lock(hdev); + conn = hci_conn_hash_lookup_handle(hdev, handle); + hci_dev_unlock(hdev); + + if (conn) { + hci_conn_enter_active_mode(conn, BT_POWER_FORCE_ACTIVE_OFF); + + /* Send to upper protocol */ + l2cap_recv_acldata(conn, skb, flags); + return; + } else { + BT_ERR("%s ACL packet for unknown connection handle %d", + hdev->name, handle); + } + + kfree_skb(skb); +} + +/* SCO data packet */ +static void hci_scodata_packet(struct hci_dev *hdev, struct sk_buff *skb) +{ + struct hci_sco_hdr *hdr = (void *) skb->data; + struct hci_conn *conn; + __u16 handle; + + skb_pull(skb, HCI_SCO_HDR_SIZE); + + handle = __le16_to_cpu(hdr->handle); + + BT_DBG("%s len %d handle 0x%4.4x", hdev->name, skb->len, handle); + + hdev->stat.sco_rx++; + + hci_dev_lock(hdev); + conn = hci_conn_hash_lookup_handle(hdev, handle); + hci_dev_unlock(hdev); + + if (conn) { + /* Send to upper protocol */ + sco_recv_scodata(conn, skb); + return; + } else { + BT_ERR("%s SCO packet for unknown connection handle %d", + hdev->name, handle); + } + + kfree_skb(skb); +} + +static bool hci_req_is_complete(struct hci_dev *hdev) +{ + struct sk_buff *skb; + + skb = skb_peek(&hdev->cmd_q); + if (!skb) + return true; + + return bt_cb(skb)->req.start; +} + +static void hci_resend_last(struct hci_dev *hdev) +{ + struct hci_command_hdr *sent; + struct sk_buff *skb; + u16 opcode; + + if (!hdev->sent_cmd) + return; + + sent = (void *) hdev->sent_cmd->data; + opcode = __le16_to_cpu(sent->opcode); + if (opcode == HCI_OP_RESET) + return; + + skb = skb_clone(hdev->sent_cmd, GFP_KERNEL); + if (!skb) + return; + + skb_queue_head(&hdev->cmd_q, skb); + queue_work(hdev->workqueue, &hdev->cmd_work); +} + +void hci_req_cmd_complete(struct hci_dev *hdev, u16 opcode, u8 status, + hci_req_complete_t *req_complete, + hci_req_complete_skb_t *req_complete_skb) +{ + struct sk_buff *skb; + unsigned long flags; + + BT_DBG("opcode 0x%04x status 0x%02x", opcode, status); + + /* If the completed command doesn't match the last one that was + * sent we need to do special handling of it. + */ + if (!hci_sent_cmd_data(hdev, opcode)) { + /* Some CSR based controllers generate a spontaneous + * reset complete event during init and any pending + * command will never be completed. In such a case we + * need to resend whatever was the last sent + * command. + */ + if (test_bit(HCI_INIT, &hdev->flags) && opcode == HCI_OP_RESET) + hci_resend_last(hdev); + + return; + } + + /* If the command succeeded and there's still more commands in + * this request the request is not yet complete. + */ + if (!status && !hci_req_is_complete(hdev)) + return; + + /* If this was the last command in a request the complete + * callback would be found in hdev->sent_cmd instead of the + * command queue (hdev->cmd_q). + */ + if (bt_cb(hdev->sent_cmd)->req.complete) { + *req_complete = bt_cb(hdev->sent_cmd)->req.complete; + return; + } + + if (bt_cb(hdev->sent_cmd)->req.complete_skb) { + *req_complete_skb = bt_cb(hdev->sent_cmd)->req.complete_skb; + return; + } + + /* Remove all pending commands belonging to this request */ + spin_lock_irqsave(&hdev->cmd_q.lock, flags); + while ((skb = __skb_dequeue(&hdev->cmd_q))) { + if (bt_cb(skb)->req.start) { + __skb_queue_head(&hdev->cmd_q, skb); + break; + } + + *req_complete = bt_cb(skb)->req.complete; + *req_complete_skb = bt_cb(skb)->req.complete_skb; + kfree_skb(skb); + } + spin_unlock_irqrestore(&hdev->cmd_q.lock, flags); +} + +static void hci_rx_work(struct work_struct *work) +{ + struct hci_dev *hdev = container_of(work, struct hci_dev, rx_work); + struct sk_buff *skb; + + BT_DBG("%s", hdev->name); + + while ((skb = skb_dequeue(&hdev->rx_q))) { + /* Send copy to monitor */ + hci_send_to_monitor(hdev, skb); + + if (atomic_read(&hdev->promisc)) { + /* Send copy to the sockets */ + hci_send_to_sock(hdev, skb); + } + + if (hci_dev_test_flag(hdev, HCI_USER_CHANNEL)) { + kfree_skb(skb); + continue; + } + + if (test_bit(HCI_INIT, &hdev->flags)) { + /* Don't process data packets in this states. */ + switch (bt_cb(skb)->pkt_type) { + case HCI_ACLDATA_PKT: + case HCI_SCODATA_PKT: + kfree_skb(skb); + continue; + } + } + + /* Process frame */ + switch (bt_cb(skb)->pkt_type) { + case HCI_EVENT_PKT: + BT_DBG("%s Event packet", hdev->name); + hci_event_packet(hdev, skb); + break; + + case HCI_ACLDATA_PKT: + BT_DBG("%s ACL data packet", hdev->name); + hci_acldata_packet(hdev, skb); + break; + + case HCI_SCODATA_PKT: + BT_DBG("%s SCO data packet", hdev->name); + hci_scodata_packet(hdev, skb); + break; + + default: + kfree_skb(skb); + break; + } + } +} + +static void hci_cmd_work(struct work_struct *work) +{ + struct hci_dev *hdev = container_of(work, struct hci_dev, cmd_work); + struct sk_buff *skb; + + BT_DBG("%s cmd_cnt %d cmd queued %d", hdev->name, + atomic_read(&hdev->cmd_cnt), skb_queue_len(&hdev->cmd_q)); + + /* Send queued commands */ + if (atomic_read(&hdev->cmd_cnt)) { + skb = skb_dequeue(&hdev->cmd_q); + if (!skb) + return; + + kfree_skb(hdev->sent_cmd); + + hdev->sent_cmd = skb_clone(skb, GFP_KERNEL); + if (hdev->sent_cmd) { + atomic_dec(&hdev->cmd_cnt); + hci_send_frame(hdev, skb); + if (test_bit(HCI_RESET, &hdev->flags)) + cancel_delayed_work(&hdev->cmd_timer); + else + schedule_delayed_work(&hdev->cmd_timer, + HCI_CMD_TIMEOUT); + } else { + skb_queue_head(&hdev->cmd_q, skb); + queue_work(hdev->workqueue, &hdev->cmd_work); + } + } +} diff --git a/kernel/net/bluetooth/hci_debugfs.c b/kernel/net/bluetooth/hci_debugfs.c new file mode 100644 index 000000000..7db422094 --- /dev/null +++ b/kernel/net/bluetooth/hci_debugfs.c @@ -0,0 +1,1142 @@ +/* + BlueZ - Bluetooth protocol stack for Linux + + Copyright (C) 2014 Intel Corporation + + 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; + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS + OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT OF THIRD PARTY RIGHTS. + IN NO EVENT SHALL THE COPYRIGHT HOLDER(S) AND AUTHOR(S) BE LIABLE FOR ANY + CLAIM, OR ANY SPECIAL INDIRECT OR CONSEQUENTIAL DAMAGES, OR ANY DAMAGES + WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN + ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF + OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + + ALL LIABILITY, INCLUDING LIABILITY FOR INFRINGEMENT OF ANY PATENTS, + COPYRIGHTS, TRADEMARKS OR OTHER RIGHTS, RELATING TO USE OF THIS + SOFTWARE IS DISCLAIMED. +*/ + +#include <linux/debugfs.h> + +#include <net/bluetooth/bluetooth.h> +#include <net/bluetooth/hci_core.h> + +#include "hci_debugfs.h" + +#define DEFINE_QUIRK_ATTRIBUTE(__name, __quirk) \ +static ssize_t __name ## _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(__quirk, &hdev->quirks) ? 'Y' : 'N'; \ + buf[1] = '\n'; \ + buf[2] = '\0'; \ + return simple_read_from_buffer(user_buf, count, ppos, buf, 2); \ +} \ + \ +static ssize_t __name ## _write(struct file *file, \ + const char __user *user_buf, \ + size_t count, loff_t *ppos) \ +{ \ + struct hci_dev *hdev = file->private_data; \ + char buf[32]; \ + size_t buf_size = min(count, (sizeof(buf) - 1)); \ + bool enable; \ + \ + if (test_bit(HCI_UP, &hdev->flags)) \ + return -EBUSY; \ + \ + if (copy_from_user(buf, user_buf, buf_size)) \ + return -EFAULT; \ + \ + buf[buf_size] = '\0'; \ + if (strtobool(buf, &enable)) \ + return -EINVAL; \ + \ + if (enable == test_bit(__quirk, &hdev->quirks)) \ + return -EALREADY; \ + \ + change_bit(__quirk, &hdev->quirks); \ + \ + return count; \ +} \ + \ +static const struct file_operations __name ## _fops = { \ + .open = simple_open, \ + .read = __name ## _read, \ + .write = __name ## _write, \ + .llseek = default_llseek, \ +} \ + +static int features_show(struct seq_file *f, void *ptr) +{ + struct hci_dev *hdev = f->private; + u8 p; + + hci_dev_lock(hdev); + for (p = 0; p < HCI_MAX_PAGES && p <= hdev->max_page; p++) { + seq_printf(f, "%2u: 0x%2.2x 0x%2.2x 0x%2.2x 0x%2.2x " + "0x%2.2x 0x%2.2x 0x%2.2x 0x%2.2x\n", p, + hdev->features[p][0], hdev->features[p][1], + hdev->features[p][2], hdev->features[p][3], + hdev->features[p][4], hdev->features[p][5], + hdev->features[p][6], hdev->features[p][7]); + } + if (lmp_le_capable(hdev)) + seq_printf(f, "LE: 0x%2.2x 0x%2.2x 0x%2.2x 0x%2.2x " + "0x%2.2x 0x%2.2x 0x%2.2x 0x%2.2x\n", + hdev->le_features[0], hdev->le_features[1], + hdev->le_features[2], hdev->le_features[3], + hdev->le_features[4], hdev->le_features[5], + hdev->le_features[6], hdev->le_features[7]); + hci_dev_unlock(hdev); + + return 0; +} + +static int features_open(struct inode *inode, struct file *file) +{ + return single_open(file, features_show, inode->i_private); +} + +static const struct file_operations features_fops = { + .open = features_open, + .read = seq_read, + .llseek = seq_lseek, + .release = single_release, +}; + +static int device_id_show(struct seq_file *f, void *ptr) +{ + struct hci_dev *hdev = f->private; + + hci_dev_lock(hdev); + seq_printf(f, "%4.4x:%4.4x:%4.4x:%4.4x\n", hdev->devid_source, + hdev->devid_vendor, hdev->devid_product, hdev->devid_version); + hci_dev_unlock(hdev); + + return 0; +} + +static int device_id_open(struct inode *inode, struct file *file) +{ + return single_open(file, device_id_show, inode->i_private); +} + +static const struct file_operations device_id_fops = { + .open = device_id_open, + .read = seq_read, + .llseek = seq_lseek, + .release = single_release, +}; + +static int device_list_show(struct seq_file *f, void *ptr) +{ + struct hci_dev *hdev = f->private; + struct hci_conn_params *p; + struct bdaddr_list *b; + + hci_dev_lock(hdev); + list_for_each_entry(b, &hdev->whitelist, list) + seq_printf(f, "%pMR (type %u)\n", &b->bdaddr, b->bdaddr_type); + list_for_each_entry(p, &hdev->le_conn_params, list) { + seq_printf(f, "%pMR (type %u) %u\n", &p->addr, p->addr_type, + p->auto_connect); + } + hci_dev_unlock(hdev); + + return 0; +} + +static int device_list_open(struct inode *inode, struct file *file) +{ + return single_open(file, device_list_show, inode->i_private); +} + +static const struct file_operations device_list_fops = { + .open = device_list_open, + .read = seq_read, + .llseek = seq_lseek, + .release = single_release, +}; + +static int blacklist_show(struct seq_file *f, void *p) +{ + struct hci_dev *hdev = f->private; + struct bdaddr_list *b; + + hci_dev_lock(hdev); + list_for_each_entry(b, &hdev->blacklist, list) + seq_printf(f, "%pMR (type %u)\n", &b->bdaddr, b->bdaddr_type); + hci_dev_unlock(hdev); + + return 0; +} + +static int blacklist_open(struct inode *inode, struct file *file) +{ + return single_open(file, blacklist_show, inode->i_private); +} + +static const struct file_operations blacklist_fops = { + .open = blacklist_open, + .read = seq_read, + .llseek = seq_lseek, + .release = single_release, +}; + +static int uuids_show(struct seq_file *f, void *p) +{ + struct hci_dev *hdev = f->private; + struct bt_uuid *uuid; + + hci_dev_lock(hdev); + list_for_each_entry(uuid, &hdev->uuids, list) { + u8 i, val[16]; + + /* The Bluetooth UUID values are stored in big endian, + * but with reversed byte order. So convert them into + * the right order for the %pUb modifier. + */ + for (i = 0; i < 16; i++) + val[i] = uuid->uuid[15 - i]; + + seq_printf(f, "%pUb\n", val); + } + hci_dev_unlock(hdev); + + return 0; +} + +static int uuids_open(struct inode *inode, struct file *file) +{ + return single_open(file, uuids_show, inode->i_private); +} + +static const struct file_operations uuids_fops = { + .open = uuids_open, + .read = seq_read, + .llseek = seq_lseek, + .release = single_release, +}; + +static int remote_oob_show(struct seq_file *f, void *ptr) +{ + struct hci_dev *hdev = f->private; + struct oob_data *data; + + hci_dev_lock(hdev); + list_for_each_entry(data, &hdev->remote_oob_data, list) { + seq_printf(f, "%pMR (type %u) %u %*phN %*phN %*phN %*phN\n", + &data->bdaddr, data->bdaddr_type, data->present, + 16, data->hash192, 16, data->rand192, + 16, data->hash256, 16, data->rand256); + } + hci_dev_unlock(hdev); + + return 0; +} + +static int remote_oob_open(struct inode *inode, struct file *file) +{ + return single_open(file, remote_oob_show, inode->i_private); +} + +static const struct file_operations remote_oob_fops = { + .open = remote_oob_open, + .read = seq_read, + .llseek = seq_lseek, + .release = single_release, +}; + +static int conn_info_min_age_set(void *data, u64 val) +{ + struct hci_dev *hdev = data; + + if (val == 0 || val > hdev->conn_info_max_age) + return -EINVAL; + + hci_dev_lock(hdev); + hdev->conn_info_min_age = val; + hci_dev_unlock(hdev); + + return 0; +} + +static int conn_info_min_age_get(void *data, u64 *val) +{ + struct hci_dev *hdev = data; + + hci_dev_lock(hdev); + *val = hdev->conn_info_min_age; + hci_dev_unlock(hdev); + + return 0; +} + +DEFINE_SIMPLE_ATTRIBUTE(conn_info_min_age_fops, conn_info_min_age_get, + conn_info_min_age_set, "%llu\n"); + +static int conn_info_max_age_set(void *data, u64 val) +{ + struct hci_dev *hdev = data; + + if (val == 0 || val < hdev->conn_info_min_age) + return -EINVAL; + + hci_dev_lock(hdev); + hdev->conn_info_max_age = val; + hci_dev_unlock(hdev); + + return 0; +} + +static int conn_info_max_age_get(void *data, u64 *val) +{ + struct hci_dev *hdev = data; + + hci_dev_lock(hdev); + *val = hdev->conn_info_max_age; + hci_dev_unlock(hdev); + + return 0; +} + +DEFINE_SIMPLE_ATTRIBUTE(conn_info_max_age_fops, conn_info_max_age_get, + conn_info_max_age_set, "%llu\n"); + +static ssize_t use_debug_keys_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] = hci_dev_test_flag(hdev, HCI_USE_DEBUG_KEYS) ? 'Y': 'N'; + buf[1] = '\n'; + buf[2] = '\0'; + return simple_read_from_buffer(user_buf, count, ppos, buf, 2); +} + +static const struct file_operations use_debug_keys_fops = { + .open = simple_open, + .read = use_debug_keys_read, + .llseek = default_llseek, +}; + +static ssize_t sc_only_mode_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] = hci_dev_test_flag(hdev, HCI_SC_ONLY) ? 'Y': 'N'; + buf[1] = '\n'; + buf[2] = '\0'; + return simple_read_from_buffer(user_buf, count, ppos, buf, 2); +} + +static const struct file_operations sc_only_mode_fops = { + .open = simple_open, + .read = sc_only_mode_read, + .llseek = default_llseek, +}; + +void hci_debugfs_create_common(struct hci_dev *hdev) +{ + debugfs_create_file("features", 0444, hdev->debugfs, hdev, + &features_fops); + debugfs_create_u16("manufacturer", 0444, hdev->debugfs, + &hdev->manufacturer); + debugfs_create_u8("hci_version", 0444, hdev->debugfs, &hdev->hci_ver); + debugfs_create_u16("hci_revision", 0444, hdev->debugfs, &hdev->hci_rev); + debugfs_create_u8("hardware_error", 0444, hdev->debugfs, + &hdev->hw_error_code); + debugfs_create_file("device_id", 0444, hdev->debugfs, hdev, + &device_id_fops); + + debugfs_create_file("device_list", 0444, hdev->debugfs, hdev, + &device_list_fops); + debugfs_create_file("blacklist", 0444, hdev->debugfs, hdev, + &blacklist_fops); + debugfs_create_file("uuids", 0444, hdev->debugfs, hdev, &uuids_fops); + debugfs_create_file("remote_oob", 0400, hdev->debugfs, hdev, + &remote_oob_fops); + + debugfs_create_file("conn_info_min_age", 0644, hdev->debugfs, hdev, + &conn_info_min_age_fops); + debugfs_create_file("conn_info_max_age", 0644, hdev->debugfs, hdev, + &conn_info_max_age_fops); + + if (lmp_ssp_capable(hdev) || lmp_le_capable(hdev)) + debugfs_create_file("use_debug_keys", 0444, hdev->debugfs, + hdev, &use_debug_keys_fops); + + if (lmp_sc_capable(hdev) || lmp_le_capable(hdev)) + debugfs_create_file("sc_only_mode", 0444, hdev->debugfs, + hdev, &sc_only_mode_fops); +} + +static int inquiry_cache_show(struct seq_file *f, void *p) +{ + struct hci_dev *hdev = f->private; + struct discovery_state *cache = &hdev->discovery; + struct inquiry_entry *e; + + hci_dev_lock(hdev); + + list_for_each_entry(e, &cache->all, all) { + struct inquiry_data *data = &e->data; + seq_printf(f, "%pMR %d %d %d 0x%.2x%.2x%.2x 0x%.4x %d %d %u\n", + &data->bdaddr, + data->pscan_rep_mode, data->pscan_period_mode, + data->pscan_mode, data->dev_class[2], + data->dev_class[1], data->dev_class[0], + __le16_to_cpu(data->clock_offset), + data->rssi, data->ssp_mode, e->timestamp); + } + + hci_dev_unlock(hdev); + + return 0; +} + +static int inquiry_cache_open(struct inode *inode, struct file *file) +{ + return single_open(file, inquiry_cache_show, inode->i_private); +} + +static const struct file_operations inquiry_cache_fops = { + .open = inquiry_cache_open, + .read = seq_read, + .llseek = seq_lseek, + .release = single_release, +}; + +static int link_keys_show(struct seq_file *f, void *ptr) +{ + struct hci_dev *hdev = f->private; + struct link_key *key; + + rcu_read_lock(); + list_for_each_entry_rcu(key, &hdev->link_keys, list) + seq_printf(f, "%pMR %u %*phN %u\n", &key->bdaddr, key->type, + HCI_LINK_KEY_SIZE, key->val, key->pin_len); + rcu_read_unlock(); + + return 0; +} + +static int link_keys_open(struct inode *inode, struct file *file) +{ + return single_open(file, link_keys_show, inode->i_private); +} + +static const struct file_operations link_keys_fops = { + .open = link_keys_open, + .read = seq_read, + .llseek = seq_lseek, + .release = single_release, +}; + +static int dev_class_show(struct seq_file *f, void *ptr) +{ + struct hci_dev *hdev = f->private; + + hci_dev_lock(hdev); + seq_printf(f, "0x%.2x%.2x%.2x\n", hdev->dev_class[2], + hdev->dev_class[1], hdev->dev_class[0]); + hci_dev_unlock(hdev); + + return 0; +} + +static int dev_class_open(struct inode *inode, struct file *file) +{ + return single_open(file, dev_class_show, inode->i_private); +} + +static const struct file_operations dev_class_fops = { + .open = dev_class_open, + .read = seq_read, + .llseek = seq_lseek, + .release = single_release, +}; + +static int voice_setting_get(void *data, u64 *val) +{ + struct hci_dev *hdev = data; + + hci_dev_lock(hdev); + *val = hdev->voice_setting; + hci_dev_unlock(hdev); + + return 0; +} + +DEFINE_SIMPLE_ATTRIBUTE(voice_setting_fops, voice_setting_get, + NULL, "0x%4.4llx\n"); + +static ssize_t ssp_debug_mode_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] = hdev->ssp_debug_mode ? 'Y': 'N'; + buf[1] = '\n'; + buf[2] = '\0'; + return simple_read_from_buffer(user_buf, count, ppos, buf, 2); +} + +static const struct file_operations ssp_debug_mode_fops = { + .open = simple_open, + .read = ssp_debug_mode_read, + .llseek = default_llseek, +}; + +static int auto_accept_delay_set(void *data, u64 val) +{ + struct hci_dev *hdev = data; + + hci_dev_lock(hdev); + hdev->auto_accept_delay = val; + hci_dev_unlock(hdev); + + return 0; +} + +static int auto_accept_delay_get(void *data, u64 *val) +{ + struct hci_dev *hdev = data; + + hci_dev_lock(hdev); + *val = hdev->auto_accept_delay; + hci_dev_unlock(hdev); + + return 0; +} + +DEFINE_SIMPLE_ATTRIBUTE(auto_accept_delay_fops, auto_accept_delay_get, + auto_accept_delay_set, "%llu\n"); + +static int idle_timeout_set(void *data, u64 val) +{ + struct hci_dev *hdev = data; + + if (val != 0 && (val < 500 || val > 3600000)) + return -EINVAL; + + hci_dev_lock(hdev); + hdev->idle_timeout = val; + hci_dev_unlock(hdev); + + return 0; +} + +static int idle_timeout_get(void *data, u64 *val) +{ + struct hci_dev *hdev = data; + + hci_dev_lock(hdev); + *val = hdev->idle_timeout; + hci_dev_unlock(hdev); + + return 0; +} + +DEFINE_SIMPLE_ATTRIBUTE(idle_timeout_fops, idle_timeout_get, + idle_timeout_set, "%llu\n"); + +static int sniff_min_interval_set(void *data, u64 val) +{ + struct hci_dev *hdev = data; + + if (val == 0 || val % 2 || val > hdev->sniff_max_interval) + return -EINVAL; + + hci_dev_lock(hdev); + hdev->sniff_min_interval = val; + hci_dev_unlock(hdev); + + return 0; +} + +static int sniff_min_interval_get(void *data, u64 *val) +{ + struct hci_dev *hdev = data; + + hci_dev_lock(hdev); + *val = hdev->sniff_min_interval; + hci_dev_unlock(hdev); + + return 0; +} + +DEFINE_SIMPLE_ATTRIBUTE(sniff_min_interval_fops, sniff_min_interval_get, + sniff_min_interval_set, "%llu\n"); + +static int sniff_max_interval_set(void *data, u64 val) +{ + struct hci_dev *hdev = data; + + if (val == 0 || val % 2 || val < hdev->sniff_min_interval) + return -EINVAL; + + hci_dev_lock(hdev); + hdev->sniff_max_interval = val; + hci_dev_unlock(hdev); + + return 0; +} + +static int sniff_max_interval_get(void *data, u64 *val) +{ + struct hci_dev *hdev = data; + + hci_dev_lock(hdev); + *val = hdev->sniff_max_interval; + hci_dev_unlock(hdev); + + return 0; +} + +DEFINE_SIMPLE_ATTRIBUTE(sniff_max_interval_fops, sniff_max_interval_get, + sniff_max_interval_set, "%llu\n"); + +void hci_debugfs_create_bredr(struct hci_dev *hdev) +{ + debugfs_create_file("inquiry_cache", 0444, hdev->debugfs, hdev, + &inquiry_cache_fops); + debugfs_create_file("link_keys", 0400, hdev->debugfs, hdev, + &link_keys_fops); + debugfs_create_file("dev_class", 0444, hdev->debugfs, hdev, + &dev_class_fops); + debugfs_create_file("voice_setting", 0444, hdev->debugfs, hdev, + &voice_setting_fops); + + if (lmp_ssp_capable(hdev)) { + debugfs_create_file("ssp_debug_mode", 0444, hdev->debugfs, + hdev, &ssp_debug_mode_fops); + debugfs_create_file("auto_accept_delay", 0644, hdev->debugfs, + hdev, &auto_accept_delay_fops); + } + + if (lmp_sniff_capable(hdev)) { + debugfs_create_file("idle_timeout", 0644, hdev->debugfs, + hdev, &idle_timeout_fops); + debugfs_create_file("sniff_min_interval", 0644, hdev->debugfs, + hdev, &sniff_min_interval_fops); + debugfs_create_file("sniff_max_interval", 0644, hdev->debugfs, + hdev, &sniff_max_interval_fops); + } +} + +static int identity_show(struct seq_file *f, void *p) +{ + struct hci_dev *hdev = f->private; + bdaddr_t addr; + u8 addr_type; + + hci_dev_lock(hdev); + + hci_copy_identity_address(hdev, &addr, &addr_type); + + seq_printf(f, "%pMR (type %u) %*phN %pMR\n", &addr, addr_type, + 16, hdev->irk, &hdev->rpa); + + hci_dev_unlock(hdev); + + return 0; +} + +static int identity_open(struct inode *inode, struct file *file) +{ + return single_open(file, identity_show, inode->i_private); +} + +static const struct file_operations identity_fops = { + .open = identity_open, + .read = seq_read, + .llseek = seq_lseek, + .release = single_release, +}; + +static int rpa_timeout_set(void *data, u64 val) +{ + struct hci_dev *hdev = data; + + /* Require the RPA timeout to be at least 30 seconds and at most + * 24 hours. + */ + if (val < 30 || val > (60 * 60 * 24)) + return -EINVAL; + + hci_dev_lock(hdev); + hdev->rpa_timeout = val; + hci_dev_unlock(hdev); + + return 0; +} + +static int rpa_timeout_get(void *data, u64 *val) +{ + struct hci_dev *hdev = data; + + hci_dev_lock(hdev); + *val = hdev->rpa_timeout; + hci_dev_unlock(hdev); + + return 0; +} + +DEFINE_SIMPLE_ATTRIBUTE(rpa_timeout_fops, rpa_timeout_get, + rpa_timeout_set, "%llu\n"); + +static int random_address_show(struct seq_file *f, void *p) +{ + struct hci_dev *hdev = f->private; + + hci_dev_lock(hdev); + seq_printf(f, "%pMR\n", &hdev->random_addr); + hci_dev_unlock(hdev); + + return 0; +} + +static int random_address_open(struct inode *inode, struct file *file) +{ + return single_open(file, random_address_show, inode->i_private); +} + +static const struct file_operations random_address_fops = { + .open = random_address_open, + .read = seq_read, + .llseek = seq_lseek, + .release = single_release, +}; + +static int static_address_show(struct seq_file *f, void *p) +{ + struct hci_dev *hdev = f->private; + + hci_dev_lock(hdev); + seq_printf(f, "%pMR\n", &hdev->static_addr); + hci_dev_unlock(hdev); + + return 0; +} + +static int static_address_open(struct inode *inode, struct file *file) +{ + return single_open(file, static_address_show, inode->i_private); +} + +static const struct file_operations static_address_fops = { + .open = static_address_open, + .read = seq_read, + .llseek = seq_lseek, + .release = single_release, +}; + +static ssize_t force_static_address_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] = hci_dev_test_flag(hdev, HCI_FORCE_STATIC_ADDR) ? 'Y': 'N'; + buf[1] = '\n'; + buf[2] = '\0'; + return simple_read_from_buffer(user_buf, count, ppos, buf, 2); +} + +static ssize_t force_static_address_write(struct file *file, + const char __user *user_buf, + size_t count, loff_t *ppos) +{ + struct hci_dev *hdev = file->private_data; + char buf[32]; + size_t buf_size = min(count, (sizeof(buf)-1)); + bool enable; + + if (test_bit(HCI_UP, &hdev->flags)) + return -EBUSY; + + if (copy_from_user(buf, user_buf, buf_size)) + return -EFAULT; + + buf[buf_size] = '\0'; + if (strtobool(buf, &enable)) + return -EINVAL; + + if (enable == hci_dev_test_flag(hdev, HCI_FORCE_STATIC_ADDR)) + return -EALREADY; + + hci_dev_change_flag(hdev, HCI_FORCE_STATIC_ADDR); + + return count; +} + +static const struct file_operations force_static_address_fops = { + .open = simple_open, + .read = force_static_address_read, + .write = force_static_address_write, + .llseek = default_llseek, +}; + +static int white_list_show(struct seq_file *f, void *ptr) +{ + struct hci_dev *hdev = f->private; + struct bdaddr_list *b; + + hci_dev_lock(hdev); + list_for_each_entry(b, &hdev->le_white_list, list) + seq_printf(f, "%pMR (type %u)\n", &b->bdaddr, b->bdaddr_type); + hci_dev_unlock(hdev); + + return 0; +} + +static int white_list_open(struct inode *inode, struct file *file) +{ + return single_open(file, white_list_show, inode->i_private); +} + +static const struct file_operations white_list_fops = { + .open = white_list_open, + .read = seq_read, + .llseek = seq_lseek, + .release = single_release, +}; + +static int identity_resolving_keys_show(struct seq_file *f, void *ptr) +{ + struct hci_dev *hdev = f->private; + struct smp_irk *irk; + + rcu_read_lock(); + list_for_each_entry_rcu(irk, &hdev->identity_resolving_keys, list) { + seq_printf(f, "%pMR (type %u) %*phN %pMR\n", + &irk->bdaddr, irk->addr_type, + 16, irk->val, &irk->rpa); + } + rcu_read_unlock(); + + return 0; +} + +static int identity_resolving_keys_open(struct inode *inode, struct file *file) +{ + return single_open(file, identity_resolving_keys_show, + inode->i_private); +} + +static const struct file_operations identity_resolving_keys_fops = { + .open = identity_resolving_keys_open, + .read = seq_read, + .llseek = seq_lseek, + .release = single_release, +}; + +static int long_term_keys_show(struct seq_file *f, void *ptr) +{ + struct hci_dev *hdev = f->private; + struct smp_ltk *ltk; + + rcu_read_lock(); + list_for_each_entry_rcu(ltk, &hdev->long_term_keys, list) + seq_printf(f, "%pMR (type %u) %u 0x%02x %u %.4x %.16llx %*phN\n", + <k->bdaddr, ltk->bdaddr_type, ltk->authenticated, + ltk->type, ltk->enc_size, __le16_to_cpu(ltk->ediv), + __le64_to_cpu(ltk->rand), 16, ltk->val); + rcu_read_unlock(); + + return 0; +} + +static int long_term_keys_open(struct inode *inode, struct file *file) +{ + return single_open(file, long_term_keys_show, inode->i_private); +} + +static const struct file_operations long_term_keys_fops = { + .open = long_term_keys_open, + .read = seq_read, + .llseek = seq_lseek, + .release = single_release, +}; + +static int conn_min_interval_set(void *data, u64 val) +{ + struct hci_dev *hdev = data; + + if (val < 0x0006 || val > 0x0c80 || val > hdev->le_conn_max_interval) + return -EINVAL; + + hci_dev_lock(hdev); + hdev->le_conn_min_interval = val; + hci_dev_unlock(hdev); + + return 0; +} + +static int conn_min_interval_get(void *data, u64 *val) +{ + struct hci_dev *hdev = data; + + hci_dev_lock(hdev); + *val = hdev->le_conn_min_interval; + hci_dev_unlock(hdev); + + return 0; +} + +DEFINE_SIMPLE_ATTRIBUTE(conn_min_interval_fops, conn_min_interval_get, + conn_min_interval_set, "%llu\n"); + +static int conn_max_interval_set(void *data, u64 val) +{ + struct hci_dev *hdev = data; + + if (val < 0x0006 || val > 0x0c80 || val < hdev->le_conn_min_interval) + return -EINVAL; + + hci_dev_lock(hdev); + hdev->le_conn_max_interval = val; + hci_dev_unlock(hdev); + + return 0; +} + +static int conn_max_interval_get(void *data, u64 *val) +{ + struct hci_dev *hdev = data; + + hci_dev_lock(hdev); + *val = hdev->le_conn_max_interval; + hci_dev_unlock(hdev); + + return 0; +} + +DEFINE_SIMPLE_ATTRIBUTE(conn_max_interval_fops, conn_max_interval_get, + conn_max_interval_set, "%llu\n"); + +static int conn_latency_set(void *data, u64 val) +{ + struct hci_dev *hdev = data; + + if (val > 0x01f3) + return -EINVAL; + + hci_dev_lock(hdev); + hdev->le_conn_latency = val; + hci_dev_unlock(hdev); + + return 0; +} + +static int conn_latency_get(void *data, u64 *val) +{ + struct hci_dev *hdev = data; + + hci_dev_lock(hdev); + *val = hdev->le_conn_latency; + hci_dev_unlock(hdev); + + return 0; +} + +DEFINE_SIMPLE_ATTRIBUTE(conn_latency_fops, conn_latency_get, + conn_latency_set, "%llu\n"); + +static int supervision_timeout_set(void *data, u64 val) +{ + struct hci_dev *hdev = data; + + if (val < 0x000a || val > 0x0c80) + return -EINVAL; + + hci_dev_lock(hdev); + hdev->le_supv_timeout = val; + hci_dev_unlock(hdev); + + return 0; +} + +static int supervision_timeout_get(void *data, u64 *val) +{ + struct hci_dev *hdev = data; + + hci_dev_lock(hdev); + *val = hdev->le_supv_timeout; + hci_dev_unlock(hdev); + + return 0; +} + +DEFINE_SIMPLE_ATTRIBUTE(supervision_timeout_fops, supervision_timeout_get, + supervision_timeout_set, "%llu\n"); + +static int adv_channel_map_set(void *data, u64 val) +{ + struct hci_dev *hdev = data; + + if (val < 0x01 || val > 0x07) + return -EINVAL; + + hci_dev_lock(hdev); + hdev->le_adv_channel_map = val; + hci_dev_unlock(hdev); + + return 0; +} + +static int adv_channel_map_get(void *data, u64 *val) +{ + struct hci_dev *hdev = data; + + hci_dev_lock(hdev); + *val = hdev->le_adv_channel_map; + hci_dev_unlock(hdev); + + return 0; +} + +DEFINE_SIMPLE_ATTRIBUTE(adv_channel_map_fops, adv_channel_map_get, + adv_channel_map_set, "%llu\n"); + +static int adv_min_interval_set(void *data, u64 val) +{ + struct hci_dev *hdev = data; + + if (val < 0x0020 || val > 0x4000 || val > hdev->le_adv_max_interval) + return -EINVAL; + + hci_dev_lock(hdev); + hdev->le_adv_min_interval = val; + hci_dev_unlock(hdev); + + return 0; +} + +static int adv_min_interval_get(void *data, u64 *val) +{ + struct hci_dev *hdev = data; + + hci_dev_lock(hdev); + *val = hdev->le_adv_min_interval; + hci_dev_unlock(hdev); + + return 0; +} + +DEFINE_SIMPLE_ATTRIBUTE(adv_min_interval_fops, adv_min_interval_get, + adv_min_interval_set, "%llu\n"); + +static int adv_max_interval_set(void *data, u64 val) +{ + struct hci_dev *hdev = data; + + if (val < 0x0020 || val > 0x4000 || val < hdev->le_adv_min_interval) + return -EINVAL; + + hci_dev_lock(hdev); + hdev->le_adv_max_interval = val; + hci_dev_unlock(hdev); + + return 0; +} + +static int adv_max_interval_get(void *data, u64 *val) +{ + struct hci_dev *hdev = data; + + hci_dev_lock(hdev); + *val = hdev->le_adv_max_interval; + hci_dev_unlock(hdev); + + return 0; +} + +DEFINE_SIMPLE_ATTRIBUTE(adv_max_interval_fops, adv_max_interval_get, + adv_max_interval_set, "%llu\n"); + +DEFINE_QUIRK_ATTRIBUTE(quirk_strict_duplicate_filter, + HCI_QUIRK_STRICT_DUPLICATE_FILTER); +DEFINE_QUIRK_ATTRIBUTE(quirk_simultaneous_discovery, + HCI_QUIRK_SIMULTANEOUS_DISCOVERY); + +void hci_debugfs_create_le(struct hci_dev *hdev) +{ + debugfs_create_file("identity", 0400, hdev->debugfs, hdev, + &identity_fops); + debugfs_create_file("rpa_timeout", 0644, hdev->debugfs, hdev, + &rpa_timeout_fops); + debugfs_create_file("random_address", 0444, hdev->debugfs, hdev, + &random_address_fops); + debugfs_create_file("static_address", 0444, hdev->debugfs, hdev, + &static_address_fops); + + /* For controllers with a public address, provide a debug + * option to force the usage of the configured static + * address. By default the public address is used. + */ + if (bacmp(&hdev->bdaddr, BDADDR_ANY)) + debugfs_create_file("force_static_address", 0644, + hdev->debugfs, hdev, + &force_static_address_fops); + + debugfs_create_u8("white_list_size", 0444, hdev->debugfs, + &hdev->le_white_list_size); + debugfs_create_file("white_list", 0444, hdev->debugfs, hdev, + &white_list_fops); + debugfs_create_file("identity_resolving_keys", 0400, hdev->debugfs, + hdev, &identity_resolving_keys_fops); + debugfs_create_file("long_term_keys", 0400, hdev->debugfs, hdev, + &long_term_keys_fops); + debugfs_create_file("conn_min_interval", 0644, hdev->debugfs, hdev, + &conn_min_interval_fops); + debugfs_create_file("conn_max_interval", 0644, hdev->debugfs, hdev, + &conn_max_interval_fops); + debugfs_create_file("conn_latency", 0644, hdev->debugfs, hdev, + &conn_latency_fops); + debugfs_create_file("supervision_timeout", 0644, hdev->debugfs, hdev, + &supervision_timeout_fops); + debugfs_create_file("adv_channel_map", 0644, hdev->debugfs, hdev, + &adv_channel_map_fops); + debugfs_create_file("adv_min_interval", 0644, hdev->debugfs, hdev, + &adv_min_interval_fops); + debugfs_create_file("adv_max_interval", 0644, hdev->debugfs, hdev, + &adv_max_interval_fops); + debugfs_create_u16("discov_interleaved_timeout", 0644, hdev->debugfs, + &hdev->discov_interleaved_timeout); + + debugfs_create_file("quirk_strict_duplicate_filter", 0644, + hdev->debugfs, hdev, + &quirk_strict_duplicate_filter_fops); + debugfs_create_file("quirk_simultaneous_discovery", 0644, + hdev->debugfs, hdev, + &quirk_simultaneous_discovery_fops); +} + +void hci_debugfs_create_conn(struct hci_conn *conn) +{ + struct hci_dev *hdev = conn->hdev; + char name[6]; + + if (IS_ERR_OR_NULL(hdev->debugfs)) + return; + + snprintf(name, sizeof(name), "%u", conn->handle); + conn->debugfs = debugfs_create_dir(name, hdev->debugfs); +} diff --git a/kernel/net/bluetooth/hci_debugfs.h b/kernel/net/bluetooth/hci_debugfs.h new file mode 100644 index 000000000..4444dc8ce --- /dev/null +++ b/kernel/net/bluetooth/hci_debugfs.h @@ -0,0 +1,48 @@ +/* + BlueZ - Bluetooth protocol stack for Linux + Copyright (C) 2014 Intel Corporation + + 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; + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS + OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT OF THIRD PARTY RIGHTS. + IN NO EVENT SHALL THE COPYRIGHT HOLDER(S) AND AUTHOR(S) BE LIABLE FOR ANY + CLAIM, OR ANY SPECIAL INDIRECT OR CONSEQUENTIAL DAMAGES, OR ANY DAMAGES + WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN + ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF + OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + + ALL LIABILITY, INCLUDING LIABILITY FOR INFRINGEMENT OF ANY PATENTS, + COPYRIGHTS, TRADEMARKS OR OTHER RIGHTS, RELATING TO USE OF THIS + SOFTWARE IS DISCLAIMED. +*/ + +#if IS_ENABLED(CONFIG_BT_DEBUGFS) + +void hci_debugfs_create_common(struct hci_dev *hdev); +void hci_debugfs_create_bredr(struct hci_dev *hdev); +void hci_debugfs_create_le(struct hci_dev *hdev); +void hci_debugfs_create_conn(struct hci_conn *conn); + +#else + +static inline void hci_debugfs_create_common(struct hci_dev *hdev) +{ +} + +static inline void hci_debugfs_create_bredr(struct hci_dev *hdev) +{ +} + +static inline void hci_debugfs_create_le(struct hci_dev *hdev) +{ +} + +static inline void hci_debugfs_create_conn(struct hci_conn *conn) +{ +} + +#endif diff --git a/kernel/net/bluetooth/hci_event.c b/kernel/net/bluetooth/hci_event.c new file mode 100644 index 000000000..7b61be736 --- /dev/null +++ b/kernel/net/bluetooth/hci_event.c @@ -0,0 +1,5404 @@ +/* + BlueZ - Bluetooth protocol stack for Linux + Copyright (c) 2000-2001, 2010, Code Aurora Forum. All rights reserved. + + Written 2000,2001 by Maxim Krasnyansky <maxk@qualcomm.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; + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS + OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT OF THIRD PARTY RIGHTS. + IN NO EVENT SHALL THE COPYRIGHT HOLDER(S) AND AUTHOR(S) BE LIABLE FOR ANY + CLAIM, OR ANY SPECIAL INDIRECT OR CONSEQUENTIAL DAMAGES, OR ANY DAMAGES + WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN + ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF + OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + + ALL LIABILITY, INCLUDING LIABILITY FOR INFRINGEMENT OF ANY PATENTS, + COPYRIGHTS, TRADEMARKS OR OTHER RIGHTS, RELATING TO USE OF THIS + SOFTWARE IS DISCLAIMED. +*/ + +/* Bluetooth HCI event handling. */ + +#include <asm/unaligned.h> + +#include <net/bluetooth/bluetooth.h> +#include <net/bluetooth/hci_core.h> +#include <net/bluetooth/mgmt.h> + +#include "hci_request.h" +#include "hci_debugfs.h" +#include "a2mp.h" +#include "amp.h" +#include "smp.h" + +#define ZERO_KEY "\x00\x00\x00\x00\x00\x00\x00\x00" \ + "\x00\x00\x00\x00\x00\x00\x00\x00" + +/* Handle HCI Event packets */ + +static void hci_cc_inquiry_cancel(struct hci_dev *hdev, struct sk_buff *skb) +{ + __u8 status = *((__u8 *) skb->data); + + BT_DBG("%s status 0x%2.2x", hdev->name, status); + + if (status) + return; + + clear_bit(HCI_INQUIRY, &hdev->flags); + smp_mb__after_atomic(); /* wake_up_bit advises about this barrier */ + wake_up_bit(&hdev->flags, HCI_INQUIRY); + + hci_dev_lock(hdev); + hci_discovery_set_state(hdev, DISCOVERY_STOPPED); + hci_dev_unlock(hdev); + + hci_conn_check_pending(hdev); +} + +static void hci_cc_periodic_inq(struct hci_dev *hdev, struct sk_buff *skb) +{ + __u8 status = *((__u8 *) skb->data); + + BT_DBG("%s status 0x%2.2x", hdev->name, status); + + if (status) + return; + + hci_dev_set_flag(hdev, HCI_PERIODIC_INQ); +} + +static void hci_cc_exit_periodic_inq(struct hci_dev *hdev, struct sk_buff *skb) +{ + __u8 status = *((__u8 *) skb->data); + + BT_DBG("%s status 0x%2.2x", hdev->name, status); + + if (status) + return; + + hci_dev_clear_flag(hdev, HCI_PERIODIC_INQ); + + hci_conn_check_pending(hdev); +} + +static void hci_cc_remote_name_req_cancel(struct hci_dev *hdev, + struct sk_buff *skb) +{ + BT_DBG("%s", hdev->name); +} + +static void hci_cc_role_discovery(struct hci_dev *hdev, struct sk_buff *skb) +{ + struct hci_rp_role_discovery *rp = (void *) skb->data; + struct hci_conn *conn; + + BT_DBG("%s status 0x%2.2x", hdev->name, rp->status); + + if (rp->status) + return; + + hci_dev_lock(hdev); + + conn = hci_conn_hash_lookup_handle(hdev, __le16_to_cpu(rp->handle)); + if (conn) + conn->role = rp->role; + + hci_dev_unlock(hdev); +} + +static void hci_cc_read_link_policy(struct hci_dev *hdev, struct sk_buff *skb) +{ + struct hci_rp_read_link_policy *rp = (void *) skb->data; + struct hci_conn *conn; + + BT_DBG("%s status 0x%2.2x", hdev->name, rp->status); + + if (rp->status) + return; + + hci_dev_lock(hdev); + + conn = hci_conn_hash_lookup_handle(hdev, __le16_to_cpu(rp->handle)); + if (conn) + conn->link_policy = __le16_to_cpu(rp->policy); + + hci_dev_unlock(hdev); +} + +static void hci_cc_write_link_policy(struct hci_dev *hdev, struct sk_buff *skb) +{ + struct hci_rp_write_link_policy *rp = (void *) skb->data; + struct hci_conn *conn; + void *sent; + + BT_DBG("%s status 0x%2.2x", hdev->name, rp->status); + + if (rp->status) + return; + + sent = hci_sent_cmd_data(hdev, HCI_OP_WRITE_LINK_POLICY); + if (!sent) + return; + + hci_dev_lock(hdev); + + conn = hci_conn_hash_lookup_handle(hdev, __le16_to_cpu(rp->handle)); + if (conn) + conn->link_policy = get_unaligned_le16(sent + 2); + + hci_dev_unlock(hdev); +} + +static void hci_cc_read_def_link_policy(struct hci_dev *hdev, + struct sk_buff *skb) +{ + struct hci_rp_read_def_link_policy *rp = (void *) skb->data; + + BT_DBG("%s status 0x%2.2x", hdev->name, rp->status); + + if (rp->status) + return; + + hdev->link_policy = __le16_to_cpu(rp->policy); +} + +static void hci_cc_write_def_link_policy(struct hci_dev *hdev, + struct sk_buff *skb) +{ + __u8 status = *((__u8 *) skb->data); + void *sent; + + BT_DBG("%s status 0x%2.2x", hdev->name, status); + + if (status) + return; + + sent = hci_sent_cmd_data(hdev, HCI_OP_WRITE_DEF_LINK_POLICY); + if (!sent) + return; + + hdev->link_policy = get_unaligned_le16(sent); +} + +static void hci_cc_reset(struct hci_dev *hdev, struct sk_buff *skb) +{ + __u8 status = *((__u8 *) skb->data); + + BT_DBG("%s status 0x%2.2x", hdev->name, status); + + clear_bit(HCI_RESET, &hdev->flags); + + if (status) + return; + + /* Reset all non-persistent flags */ + hci_dev_clear_volatile_flags(hdev); + + hci_discovery_set_state(hdev, DISCOVERY_STOPPED); + + hdev->inq_tx_power = HCI_TX_POWER_INVALID; + hdev->adv_tx_power = HCI_TX_POWER_INVALID; + + memset(hdev->adv_data, 0, sizeof(hdev->adv_data)); + hdev->adv_data_len = 0; + + memset(hdev->scan_rsp_data, 0, sizeof(hdev->scan_rsp_data)); + hdev->scan_rsp_data_len = 0; + + hdev->le_scan_type = LE_SCAN_PASSIVE; + + hdev->ssp_debug_mode = 0; + + hci_bdaddr_list_clear(&hdev->le_white_list); +} + +static void hci_cc_read_stored_link_key(struct hci_dev *hdev, + struct sk_buff *skb) +{ + struct hci_rp_read_stored_link_key *rp = (void *)skb->data; + struct hci_cp_read_stored_link_key *sent; + + BT_DBG("%s status 0x%2.2x", hdev->name, rp->status); + + sent = hci_sent_cmd_data(hdev, HCI_OP_READ_STORED_LINK_KEY); + if (!sent) + return; + + if (!rp->status && sent->read_all == 0x01) { + hdev->stored_max_keys = rp->max_keys; + hdev->stored_num_keys = rp->num_keys; + } +} + +static void hci_cc_delete_stored_link_key(struct hci_dev *hdev, + struct sk_buff *skb) +{ + struct hci_rp_delete_stored_link_key *rp = (void *)skb->data; + + BT_DBG("%s status 0x%2.2x", hdev->name, rp->status); + + if (rp->status) + return; + + if (rp->num_keys <= hdev->stored_num_keys) + hdev->stored_num_keys -= rp->num_keys; + else + hdev->stored_num_keys = 0; +} + +static void hci_cc_write_local_name(struct hci_dev *hdev, struct sk_buff *skb) +{ + __u8 status = *((__u8 *) skb->data); + void *sent; + + BT_DBG("%s status 0x%2.2x", hdev->name, status); + + sent = hci_sent_cmd_data(hdev, HCI_OP_WRITE_LOCAL_NAME); + if (!sent) + return; + + hci_dev_lock(hdev); + + if (hci_dev_test_flag(hdev, HCI_MGMT)) + mgmt_set_local_name_complete(hdev, sent, status); + else if (!status) + memcpy(hdev->dev_name, sent, HCI_MAX_NAME_LENGTH); + + hci_dev_unlock(hdev); +} + +static void hci_cc_read_local_name(struct hci_dev *hdev, struct sk_buff *skb) +{ + struct hci_rp_read_local_name *rp = (void *) skb->data; + + BT_DBG("%s status 0x%2.2x", hdev->name, rp->status); + + if (rp->status) + return; + + if (hci_dev_test_flag(hdev, HCI_SETUP) || + hci_dev_test_flag(hdev, HCI_CONFIG)) + memcpy(hdev->dev_name, rp->name, HCI_MAX_NAME_LENGTH); +} + +static void hci_cc_write_auth_enable(struct hci_dev *hdev, struct sk_buff *skb) +{ + __u8 status = *((__u8 *) skb->data); + void *sent; + + BT_DBG("%s status 0x%2.2x", hdev->name, status); + + sent = hci_sent_cmd_data(hdev, HCI_OP_WRITE_AUTH_ENABLE); + if (!sent) + return; + + hci_dev_lock(hdev); + + if (!status) { + __u8 param = *((__u8 *) sent); + + if (param == AUTH_ENABLED) + set_bit(HCI_AUTH, &hdev->flags); + else + clear_bit(HCI_AUTH, &hdev->flags); + } + + if (hci_dev_test_flag(hdev, HCI_MGMT)) + mgmt_auth_enable_complete(hdev, status); + + hci_dev_unlock(hdev); +} + +static void hci_cc_write_encrypt_mode(struct hci_dev *hdev, struct sk_buff *skb) +{ + __u8 status = *((__u8 *) skb->data); + __u8 param; + void *sent; + + BT_DBG("%s status 0x%2.2x", hdev->name, status); + + if (status) + return; + + sent = hci_sent_cmd_data(hdev, HCI_OP_WRITE_ENCRYPT_MODE); + if (!sent) + return; + + param = *((__u8 *) sent); + + if (param) + set_bit(HCI_ENCRYPT, &hdev->flags); + else + clear_bit(HCI_ENCRYPT, &hdev->flags); +} + +static void hci_cc_write_scan_enable(struct hci_dev *hdev, struct sk_buff *skb) +{ + __u8 status = *((__u8 *) skb->data); + __u8 param; + void *sent; + + BT_DBG("%s status 0x%2.2x", hdev->name, status); + + sent = hci_sent_cmd_data(hdev, HCI_OP_WRITE_SCAN_ENABLE); + if (!sent) + return; + + param = *((__u8 *) sent); + + hci_dev_lock(hdev); + + if (status) { + hdev->discov_timeout = 0; + goto done; + } + + if (param & SCAN_INQUIRY) + set_bit(HCI_ISCAN, &hdev->flags); + else + clear_bit(HCI_ISCAN, &hdev->flags); + + if (param & SCAN_PAGE) + set_bit(HCI_PSCAN, &hdev->flags); + else + clear_bit(HCI_PSCAN, &hdev->flags); + +done: + hci_dev_unlock(hdev); +} + +static void hci_cc_read_class_of_dev(struct hci_dev *hdev, struct sk_buff *skb) +{ + struct hci_rp_read_class_of_dev *rp = (void *) skb->data; + + BT_DBG("%s status 0x%2.2x", hdev->name, rp->status); + + if (rp->status) + return; + + memcpy(hdev->dev_class, rp->dev_class, 3); + + BT_DBG("%s class 0x%.2x%.2x%.2x", hdev->name, + hdev->dev_class[2], hdev->dev_class[1], hdev->dev_class[0]); +} + +static void hci_cc_write_class_of_dev(struct hci_dev *hdev, struct sk_buff *skb) +{ + __u8 status = *((__u8 *) skb->data); + void *sent; + + BT_DBG("%s status 0x%2.2x", hdev->name, status); + + sent = hci_sent_cmd_data(hdev, HCI_OP_WRITE_CLASS_OF_DEV); + if (!sent) + return; + + hci_dev_lock(hdev); + + if (status == 0) + memcpy(hdev->dev_class, sent, 3); + + if (hci_dev_test_flag(hdev, HCI_MGMT)) + mgmt_set_class_of_dev_complete(hdev, sent, status); + + hci_dev_unlock(hdev); +} + +static void hci_cc_read_voice_setting(struct hci_dev *hdev, struct sk_buff *skb) +{ + struct hci_rp_read_voice_setting *rp = (void *) skb->data; + __u16 setting; + + BT_DBG("%s status 0x%2.2x", hdev->name, rp->status); + + if (rp->status) + return; + + setting = __le16_to_cpu(rp->voice_setting); + + if (hdev->voice_setting == setting) + return; + + hdev->voice_setting = setting; + + BT_DBG("%s voice setting 0x%4.4x", hdev->name, setting); + + if (hdev->notify) + hdev->notify(hdev, HCI_NOTIFY_VOICE_SETTING); +} + +static void hci_cc_write_voice_setting(struct hci_dev *hdev, + struct sk_buff *skb) +{ + __u8 status = *((__u8 *) skb->data); + __u16 setting; + void *sent; + + BT_DBG("%s status 0x%2.2x", hdev->name, status); + + if (status) + return; + + sent = hci_sent_cmd_data(hdev, HCI_OP_WRITE_VOICE_SETTING); + if (!sent) + return; + + setting = get_unaligned_le16(sent); + + if (hdev->voice_setting == setting) + return; + + hdev->voice_setting = setting; + + BT_DBG("%s voice setting 0x%4.4x", hdev->name, setting); + + if (hdev->notify) + hdev->notify(hdev, HCI_NOTIFY_VOICE_SETTING); +} + +static void hci_cc_read_num_supported_iac(struct hci_dev *hdev, + struct sk_buff *skb) +{ + struct hci_rp_read_num_supported_iac *rp = (void *) skb->data; + + BT_DBG("%s status 0x%2.2x", hdev->name, rp->status); + + if (rp->status) + return; + + hdev->num_iac = rp->num_iac; + + BT_DBG("%s num iac %d", hdev->name, hdev->num_iac); +} + +static void hci_cc_write_ssp_mode(struct hci_dev *hdev, struct sk_buff *skb) +{ + __u8 status = *((__u8 *) skb->data); + struct hci_cp_write_ssp_mode *sent; + + BT_DBG("%s status 0x%2.2x", hdev->name, status); + + sent = hci_sent_cmd_data(hdev, HCI_OP_WRITE_SSP_MODE); + if (!sent) + return; + + hci_dev_lock(hdev); + + if (!status) { + if (sent->mode) + hdev->features[1][0] |= LMP_HOST_SSP; + else + hdev->features[1][0] &= ~LMP_HOST_SSP; + } + + if (hci_dev_test_flag(hdev, HCI_MGMT)) + mgmt_ssp_enable_complete(hdev, sent->mode, status); + else if (!status) { + if (sent->mode) + hci_dev_set_flag(hdev, HCI_SSP_ENABLED); + else + hci_dev_clear_flag(hdev, HCI_SSP_ENABLED); + } + + hci_dev_unlock(hdev); +} + +static void hci_cc_write_sc_support(struct hci_dev *hdev, struct sk_buff *skb) +{ + u8 status = *((u8 *) skb->data); + struct hci_cp_write_sc_support *sent; + + BT_DBG("%s status 0x%2.2x", hdev->name, status); + + sent = hci_sent_cmd_data(hdev, HCI_OP_WRITE_SC_SUPPORT); + if (!sent) + return; + + hci_dev_lock(hdev); + + if (!status) { + if (sent->support) + hdev->features[1][0] |= LMP_HOST_SC; + else + hdev->features[1][0] &= ~LMP_HOST_SC; + } + + if (!hci_dev_test_flag(hdev, HCI_MGMT) && !status) { + if (sent->support) + hci_dev_set_flag(hdev, HCI_SC_ENABLED); + else + hci_dev_clear_flag(hdev, HCI_SC_ENABLED); + } + + hci_dev_unlock(hdev); +} + +static void hci_cc_read_local_version(struct hci_dev *hdev, struct sk_buff *skb) +{ + struct hci_rp_read_local_version *rp = (void *) skb->data; + + BT_DBG("%s status 0x%2.2x", hdev->name, rp->status); + + if (rp->status) + return; + + if (hci_dev_test_flag(hdev, HCI_SETUP) || + hci_dev_test_flag(hdev, HCI_CONFIG)) { + hdev->hci_ver = rp->hci_ver; + hdev->hci_rev = __le16_to_cpu(rp->hci_rev); + hdev->lmp_ver = rp->lmp_ver; + hdev->manufacturer = __le16_to_cpu(rp->manufacturer); + hdev->lmp_subver = __le16_to_cpu(rp->lmp_subver); + } +} + +static void hci_cc_read_local_commands(struct hci_dev *hdev, + struct sk_buff *skb) +{ + struct hci_rp_read_local_commands *rp = (void *) skb->data; + + BT_DBG("%s status 0x%2.2x", hdev->name, rp->status); + + if (rp->status) + return; + + if (hci_dev_test_flag(hdev, HCI_SETUP) || + hci_dev_test_flag(hdev, HCI_CONFIG)) + memcpy(hdev->commands, rp->commands, sizeof(hdev->commands)); +} + +static void hci_cc_read_local_features(struct hci_dev *hdev, + struct sk_buff *skb) +{ + struct hci_rp_read_local_features *rp = (void *) skb->data; + + BT_DBG("%s status 0x%2.2x", hdev->name, rp->status); + + if (rp->status) + return; + + memcpy(hdev->features, rp->features, 8); + + /* Adjust default settings according to features + * supported by device. */ + + if (hdev->features[0][0] & LMP_3SLOT) + hdev->pkt_type |= (HCI_DM3 | HCI_DH3); + + if (hdev->features[0][0] & LMP_5SLOT) + hdev->pkt_type |= (HCI_DM5 | HCI_DH5); + + if (hdev->features[0][1] & LMP_HV2) { + hdev->pkt_type |= (HCI_HV2); + hdev->esco_type |= (ESCO_HV2); + } + + if (hdev->features[0][1] & LMP_HV3) { + hdev->pkt_type |= (HCI_HV3); + hdev->esco_type |= (ESCO_HV3); + } + + if (lmp_esco_capable(hdev)) + hdev->esco_type |= (ESCO_EV3); + + if (hdev->features[0][4] & LMP_EV4) + hdev->esco_type |= (ESCO_EV4); + + if (hdev->features[0][4] & LMP_EV5) + hdev->esco_type |= (ESCO_EV5); + + if (hdev->features[0][5] & LMP_EDR_ESCO_2M) + hdev->esco_type |= (ESCO_2EV3); + + if (hdev->features[0][5] & LMP_EDR_ESCO_3M) + hdev->esco_type |= (ESCO_3EV3); + + if (hdev->features[0][5] & LMP_EDR_3S_ESCO) + hdev->esco_type |= (ESCO_2EV5 | ESCO_3EV5); +} + +static void hci_cc_read_local_ext_features(struct hci_dev *hdev, + struct sk_buff *skb) +{ + struct hci_rp_read_local_ext_features *rp = (void *) skb->data; + + BT_DBG("%s status 0x%2.2x", hdev->name, rp->status); + + if (rp->status) + return; + + if (hdev->max_page < rp->max_page) + hdev->max_page = rp->max_page; + + if (rp->page < HCI_MAX_PAGES) + memcpy(hdev->features[rp->page], rp->features, 8); +} + +static void hci_cc_read_flow_control_mode(struct hci_dev *hdev, + struct sk_buff *skb) +{ + struct hci_rp_read_flow_control_mode *rp = (void *) skb->data; + + BT_DBG("%s status 0x%2.2x", hdev->name, rp->status); + + if (rp->status) + return; + + hdev->flow_ctl_mode = rp->mode; +} + +static void hci_cc_read_buffer_size(struct hci_dev *hdev, struct sk_buff *skb) +{ + struct hci_rp_read_buffer_size *rp = (void *) skb->data; + + BT_DBG("%s status 0x%2.2x", hdev->name, rp->status); + + if (rp->status) + return; + + hdev->acl_mtu = __le16_to_cpu(rp->acl_mtu); + hdev->sco_mtu = rp->sco_mtu; + hdev->acl_pkts = __le16_to_cpu(rp->acl_max_pkt); + hdev->sco_pkts = __le16_to_cpu(rp->sco_max_pkt); + + if (test_bit(HCI_QUIRK_FIXUP_BUFFER_SIZE, &hdev->quirks)) { + hdev->sco_mtu = 64; + hdev->sco_pkts = 8; + } + + hdev->acl_cnt = hdev->acl_pkts; + hdev->sco_cnt = hdev->sco_pkts; + + BT_DBG("%s acl mtu %d:%d sco mtu %d:%d", hdev->name, hdev->acl_mtu, + hdev->acl_pkts, hdev->sco_mtu, hdev->sco_pkts); +} + +static void hci_cc_read_bd_addr(struct hci_dev *hdev, struct sk_buff *skb) +{ + struct hci_rp_read_bd_addr *rp = (void *) skb->data; + + BT_DBG("%s status 0x%2.2x", hdev->name, rp->status); + + if (rp->status) + return; + + if (test_bit(HCI_INIT, &hdev->flags)) + bacpy(&hdev->bdaddr, &rp->bdaddr); + + if (hci_dev_test_flag(hdev, HCI_SETUP)) + bacpy(&hdev->setup_addr, &rp->bdaddr); +} + +static void hci_cc_read_page_scan_activity(struct hci_dev *hdev, + struct sk_buff *skb) +{ + struct hci_rp_read_page_scan_activity *rp = (void *) skb->data; + + BT_DBG("%s status 0x%2.2x", hdev->name, rp->status); + + if (rp->status) + return; + + if (test_bit(HCI_INIT, &hdev->flags)) { + hdev->page_scan_interval = __le16_to_cpu(rp->interval); + hdev->page_scan_window = __le16_to_cpu(rp->window); + } +} + +static void hci_cc_write_page_scan_activity(struct hci_dev *hdev, + struct sk_buff *skb) +{ + u8 status = *((u8 *) skb->data); + struct hci_cp_write_page_scan_activity *sent; + + BT_DBG("%s status 0x%2.2x", hdev->name, status); + + if (status) + return; + + sent = hci_sent_cmd_data(hdev, HCI_OP_WRITE_PAGE_SCAN_ACTIVITY); + if (!sent) + return; + + hdev->page_scan_interval = __le16_to_cpu(sent->interval); + hdev->page_scan_window = __le16_to_cpu(sent->window); +} + +static void hci_cc_read_page_scan_type(struct hci_dev *hdev, + struct sk_buff *skb) +{ + struct hci_rp_read_page_scan_type *rp = (void *) skb->data; + + BT_DBG("%s status 0x%2.2x", hdev->name, rp->status); + + if (rp->status) + return; + + if (test_bit(HCI_INIT, &hdev->flags)) + hdev->page_scan_type = rp->type; +} + +static void hci_cc_write_page_scan_type(struct hci_dev *hdev, + struct sk_buff *skb) +{ + u8 status = *((u8 *) skb->data); + u8 *type; + + BT_DBG("%s status 0x%2.2x", hdev->name, status); + + if (status) + return; + + type = hci_sent_cmd_data(hdev, HCI_OP_WRITE_PAGE_SCAN_TYPE); + if (type) + hdev->page_scan_type = *type; +} + +static void hci_cc_read_data_block_size(struct hci_dev *hdev, + struct sk_buff *skb) +{ + struct hci_rp_read_data_block_size *rp = (void *) skb->data; + + BT_DBG("%s status 0x%2.2x", hdev->name, rp->status); + + if (rp->status) + return; + + hdev->block_mtu = __le16_to_cpu(rp->max_acl_len); + hdev->block_len = __le16_to_cpu(rp->block_len); + hdev->num_blocks = __le16_to_cpu(rp->num_blocks); + + hdev->block_cnt = hdev->num_blocks; + + BT_DBG("%s blk mtu %d cnt %d len %d", hdev->name, hdev->block_mtu, + hdev->block_cnt, hdev->block_len); +} + +static void hci_cc_read_clock(struct hci_dev *hdev, struct sk_buff *skb) +{ + struct hci_rp_read_clock *rp = (void *) skb->data; + struct hci_cp_read_clock *cp; + struct hci_conn *conn; + + BT_DBG("%s", hdev->name); + + if (skb->len < sizeof(*rp)) + return; + + if (rp->status) + return; + + hci_dev_lock(hdev); + + cp = hci_sent_cmd_data(hdev, HCI_OP_READ_CLOCK); + if (!cp) + goto unlock; + + if (cp->which == 0x00) { + hdev->clock = le32_to_cpu(rp->clock); + goto unlock; + } + + conn = hci_conn_hash_lookup_handle(hdev, __le16_to_cpu(rp->handle)); + if (conn) { + conn->clock = le32_to_cpu(rp->clock); + conn->clock_accuracy = le16_to_cpu(rp->accuracy); + } + +unlock: + hci_dev_unlock(hdev); +} + +static void hci_cc_read_local_amp_info(struct hci_dev *hdev, + struct sk_buff *skb) +{ + struct hci_rp_read_local_amp_info *rp = (void *) skb->data; + + BT_DBG("%s status 0x%2.2x", hdev->name, rp->status); + + if (rp->status) + goto a2mp_rsp; + + hdev->amp_status = rp->amp_status; + hdev->amp_total_bw = __le32_to_cpu(rp->total_bw); + hdev->amp_max_bw = __le32_to_cpu(rp->max_bw); + hdev->amp_min_latency = __le32_to_cpu(rp->min_latency); + hdev->amp_max_pdu = __le32_to_cpu(rp->max_pdu); + hdev->amp_type = rp->amp_type; + hdev->amp_pal_cap = __le16_to_cpu(rp->pal_cap); + hdev->amp_assoc_size = __le16_to_cpu(rp->max_assoc_size); + hdev->amp_be_flush_to = __le32_to_cpu(rp->be_flush_to); + hdev->amp_max_flush_to = __le32_to_cpu(rp->max_flush_to); + +a2mp_rsp: + a2mp_send_getinfo_rsp(hdev); +} + +static void hci_cc_read_local_amp_assoc(struct hci_dev *hdev, + struct sk_buff *skb) +{ + struct hci_rp_read_local_amp_assoc *rp = (void *) skb->data; + struct amp_assoc *assoc = &hdev->loc_assoc; + size_t rem_len, frag_len; + + BT_DBG("%s status 0x%2.2x", hdev->name, rp->status); + + if (rp->status) + goto a2mp_rsp; + + frag_len = skb->len - sizeof(*rp); + rem_len = __le16_to_cpu(rp->rem_len); + + if (rem_len > frag_len) { + BT_DBG("frag_len %zu rem_len %zu", frag_len, rem_len); + + memcpy(assoc->data + assoc->offset, rp->frag, frag_len); + assoc->offset += frag_len; + + /* Read other fragments */ + amp_read_loc_assoc_frag(hdev, rp->phy_handle); + + return; + } + + memcpy(assoc->data + assoc->offset, rp->frag, rem_len); + assoc->len = assoc->offset + rem_len; + assoc->offset = 0; + +a2mp_rsp: + /* Send A2MP Rsp when all fragments are received */ + a2mp_send_getampassoc_rsp(hdev, rp->status); + a2mp_send_create_phy_link_req(hdev, rp->status); +} + +static void hci_cc_read_inq_rsp_tx_power(struct hci_dev *hdev, + struct sk_buff *skb) +{ + struct hci_rp_read_inq_rsp_tx_power *rp = (void *) skb->data; + + BT_DBG("%s status 0x%2.2x", hdev->name, rp->status); + + if (rp->status) + return; + + hdev->inq_tx_power = rp->tx_power; +} + +static void hci_cc_pin_code_reply(struct hci_dev *hdev, struct sk_buff *skb) +{ + struct hci_rp_pin_code_reply *rp = (void *) skb->data; + struct hci_cp_pin_code_reply *cp; + struct hci_conn *conn; + + BT_DBG("%s status 0x%2.2x", hdev->name, rp->status); + + hci_dev_lock(hdev); + + if (hci_dev_test_flag(hdev, HCI_MGMT)) + mgmt_pin_code_reply_complete(hdev, &rp->bdaddr, rp->status); + + if (rp->status) + goto unlock; + + cp = hci_sent_cmd_data(hdev, HCI_OP_PIN_CODE_REPLY); + if (!cp) + goto unlock; + + conn = hci_conn_hash_lookup_ba(hdev, ACL_LINK, &cp->bdaddr); + if (conn) + conn->pin_length = cp->pin_len; + +unlock: + hci_dev_unlock(hdev); +} + +static void hci_cc_pin_code_neg_reply(struct hci_dev *hdev, struct sk_buff *skb) +{ + struct hci_rp_pin_code_neg_reply *rp = (void *) skb->data; + + BT_DBG("%s status 0x%2.2x", hdev->name, rp->status); + + hci_dev_lock(hdev); + + if (hci_dev_test_flag(hdev, HCI_MGMT)) + mgmt_pin_code_neg_reply_complete(hdev, &rp->bdaddr, + rp->status); + + hci_dev_unlock(hdev); +} + +static void hci_cc_le_read_buffer_size(struct hci_dev *hdev, + struct sk_buff *skb) +{ + struct hci_rp_le_read_buffer_size *rp = (void *) skb->data; + + BT_DBG("%s status 0x%2.2x", hdev->name, rp->status); + + if (rp->status) + return; + + hdev->le_mtu = __le16_to_cpu(rp->le_mtu); + hdev->le_pkts = rp->le_max_pkt; + + hdev->le_cnt = hdev->le_pkts; + + BT_DBG("%s le mtu %d:%d", hdev->name, hdev->le_mtu, hdev->le_pkts); +} + +static void hci_cc_le_read_local_features(struct hci_dev *hdev, + struct sk_buff *skb) +{ + struct hci_rp_le_read_local_features *rp = (void *) skb->data; + + BT_DBG("%s status 0x%2.2x", hdev->name, rp->status); + + if (rp->status) + return; + + memcpy(hdev->le_features, rp->features, 8); +} + +static void hci_cc_le_read_adv_tx_power(struct hci_dev *hdev, + struct sk_buff *skb) +{ + struct hci_rp_le_read_adv_tx_power *rp = (void *) skb->data; + + BT_DBG("%s status 0x%2.2x", hdev->name, rp->status); + + if (rp->status) + return; + + hdev->adv_tx_power = rp->tx_power; +} + +static void hci_cc_user_confirm_reply(struct hci_dev *hdev, struct sk_buff *skb) +{ + struct hci_rp_user_confirm_reply *rp = (void *) skb->data; + + BT_DBG("%s status 0x%2.2x", hdev->name, rp->status); + + hci_dev_lock(hdev); + + if (hci_dev_test_flag(hdev, HCI_MGMT)) + mgmt_user_confirm_reply_complete(hdev, &rp->bdaddr, ACL_LINK, 0, + rp->status); + + hci_dev_unlock(hdev); +} + +static void hci_cc_user_confirm_neg_reply(struct hci_dev *hdev, + struct sk_buff *skb) +{ + struct hci_rp_user_confirm_reply *rp = (void *) skb->data; + + BT_DBG("%s status 0x%2.2x", hdev->name, rp->status); + + hci_dev_lock(hdev); + + if (hci_dev_test_flag(hdev, HCI_MGMT)) + mgmt_user_confirm_neg_reply_complete(hdev, &rp->bdaddr, + ACL_LINK, 0, rp->status); + + hci_dev_unlock(hdev); +} + +static void hci_cc_user_passkey_reply(struct hci_dev *hdev, struct sk_buff *skb) +{ + struct hci_rp_user_confirm_reply *rp = (void *) skb->data; + + BT_DBG("%s status 0x%2.2x", hdev->name, rp->status); + + hci_dev_lock(hdev); + + if (hci_dev_test_flag(hdev, HCI_MGMT)) + mgmt_user_passkey_reply_complete(hdev, &rp->bdaddr, ACL_LINK, + 0, rp->status); + + hci_dev_unlock(hdev); +} + +static void hci_cc_user_passkey_neg_reply(struct hci_dev *hdev, + struct sk_buff *skb) +{ + struct hci_rp_user_confirm_reply *rp = (void *) skb->data; + + BT_DBG("%s status 0x%2.2x", hdev->name, rp->status); + + hci_dev_lock(hdev); + + if (hci_dev_test_flag(hdev, HCI_MGMT)) + mgmt_user_passkey_neg_reply_complete(hdev, &rp->bdaddr, + ACL_LINK, 0, rp->status); + + hci_dev_unlock(hdev); +} + +static void hci_cc_read_local_oob_data(struct hci_dev *hdev, + struct sk_buff *skb) +{ + struct hci_rp_read_local_oob_data *rp = (void *) skb->data; + + BT_DBG("%s status 0x%2.2x", hdev->name, rp->status); +} + +static void hci_cc_read_local_oob_ext_data(struct hci_dev *hdev, + struct sk_buff *skb) +{ + struct hci_rp_read_local_oob_ext_data *rp = (void *) skb->data; + + BT_DBG("%s status 0x%2.2x", hdev->name, rp->status); +} + +static void hci_cc_le_set_random_addr(struct hci_dev *hdev, struct sk_buff *skb) +{ + __u8 status = *((__u8 *) skb->data); + bdaddr_t *sent; + + BT_DBG("%s status 0x%2.2x", hdev->name, status); + + if (status) + return; + + sent = hci_sent_cmd_data(hdev, HCI_OP_LE_SET_RANDOM_ADDR); + if (!sent) + return; + + hci_dev_lock(hdev); + + bacpy(&hdev->random_addr, sent); + + hci_dev_unlock(hdev); +} + +static void hci_cc_le_set_adv_enable(struct hci_dev *hdev, struct sk_buff *skb) +{ + __u8 *sent, status = *((__u8 *) skb->data); + + BT_DBG("%s status 0x%2.2x", hdev->name, status); + + if (status) + return; + + sent = hci_sent_cmd_data(hdev, HCI_OP_LE_SET_ADV_ENABLE); + if (!sent) + return; + + hci_dev_lock(hdev); + + /* If we're doing connection initiation as peripheral. Set a + * timeout in case something goes wrong. + */ + if (*sent) { + struct hci_conn *conn; + + hci_dev_set_flag(hdev, HCI_LE_ADV); + + conn = hci_conn_hash_lookup_state(hdev, LE_LINK, BT_CONNECT); + if (conn) + queue_delayed_work(hdev->workqueue, + &conn->le_conn_timeout, + conn->conn_timeout); + } else { + hci_dev_clear_flag(hdev, HCI_LE_ADV); + } + + hci_dev_unlock(hdev); +} + +static void hci_cc_le_set_scan_param(struct hci_dev *hdev, struct sk_buff *skb) +{ + struct hci_cp_le_set_scan_param *cp; + __u8 status = *((__u8 *) skb->data); + + BT_DBG("%s status 0x%2.2x", hdev->name, status); + + if (status) + return; + + cp = hci_sent_cmd_data(hdev, HCI_OP_LE_SET_SCAN_PARAM); + if (!cp) + return; + + hci_dev_lock(hdev); + + hdev->le_scan_type = cp->type; + + hci_dev_unlock(hdev); +} + +static bool has_pending_adv_report(struct hci_dev *hdev) +{ + struct discovery_state *d = &hdev->discovery; + + return bacmp(&d->last_adv_addr, BDADDR_ANY); +} + +static void clear_pending_adv_report(struct hci_dev *hdev) +{ + struct discovery_state *d = &hdev->discovery; + + bacpy(&d->last_adv_addr, BDADDR_ANY); + d->last_adv_data_len = 0; +} + +static void store_pending_adv_report(struct hci_dev *hdev, bdaddr_t *bdaddr, + u8 bdaddr_type, s8 rssi, u32 flags, + u8 *data, u8 len) +{ + struct discovery_state *d = &hdev->discovery; + + bacpy(&d->last_adv_addr, bdaddr); + d->last_adv_addr_type = bdaddr_type; + d->last_adv_rssi = rssi; + d->last_adv_flags = flags; + memcpy(d->last_adv_data, data, len); + d->last_adv_data_len = len; +} + +static void hci_cc_le_set_scan_enable(struct hci_dev *hdev, + struct sk_buff *skb) +{ + struct hci_cp_le_set_scan_enable *cp; + __u8 status = *((__u8 *) skb->data); + + BT_DBG("%s status 0x%2.2x", hdev->name, status); + + if (status) + return; + + cp = hci_sent_cmd_data(hdev, HCI_OP_LE_SET_SCAN_ENABLE); + if (!cp) + return; + + hci_dev_lock(hdev); + + switch (cp->enable) { + case LE_SCAN_ENABLE: + hci_dev_set_flag(hdev, HCI_LE_SCAN); + if (hdev->le_scan_type == LE_SCAN_ACTIVE) + clear_pending_adv_report(hdev); + break; + + case LE_SCAN_DISABLE: + /* We do this here instead of when setting DISCOVERY_STOPPED + * since the latter would potentially require waiting for + * inquiry to stop too. + */ + if (has_pending_adv_report(hdev)) { + struct discovery_state *d = &hdev->discovery; + + mgmt_device_found(hdev, &d->last_adv_addr, LE_LINK, + d->last_adv_addr_type, NULL, + d->last_adv_rssi, d->last_adv_flags, + d->last_adv_data, + d->last_adv_data_len, NULL, 0); + } + + /* Cancel this timer so that we don't try to disable scanning + * when it's already disabled. + */ + cancel_delayed_work(&hdev->le_scan_disable); + + hci_dev_clear_flag(hdev, HCI_LE_SCAN); + + /* The HCI_LE_SCAN_INTERRUPTED flag indicates that we + * interrupted scanning due to a connect request. Mark + * therefore discovery as stopped. If this was not + * because of a connect request advertising might have + * been disabled because of active scanning, so + * re-enable it again if necessary. + */ + if (hci_dev_test_and_clear_flag(hdev, HCI_LE_SCAN_INTERRUPTED)) + hci_discovery_set_state(hdev, DISCOVERY_STOPPED); + else if (!hci_dev_test_flag(hdev, HCI_LE_ADV) && + hdev->discovery.state == DISCOVERY_FINDING) + mgmt_reenable_advertising(hdev); + + break; + + default: + BT_ERR("Used reserved LE_Scan_Enable param %d", cp->enable); + break; + } + + hci_dev_unlock(hdev); +} + +static void hci_cc_le_read_white_list_size(struct hci_dev *hdev, + struct sk_buff *skb) +{ + struct hci_rp_le_read_white_list_size *rp = (void *) skb->data; + + BT_DBG("%s status 0x%2.2x size %u", hdev->name, rp->status, rp->size); + + if (rp->status) + return; + + hdev->le_white_list_size = rp->size; +} + +static void hci_cc_le_clear_white_list(struct hci_dev *hdev, + struct sk_buff *skb) +{ + __u8 status = *((__u8 *) skb->data); + + BT_DBG("%s status 0x%2.2x", hdev->name, status); + + if (status) + return; + + hci_bdaddr_list_clear(&hdev->le_white_list); +} + +static void hci_cc_le_add_to_white_list(struct hci_dev *hdev, + struct sk_buff *skb) +{ + struct hci_cp_le_add_to_white_list *sent; + __u8 status = *((__u8 *) skb->data); + + BT_DBG("%s status 0x%2.2x", hdev->name, status); + + if (status) + return; + + sent = hci_sent_cmd_data(hdev, HCI_OP_LE_ADD_TO_WHITE_LIST); + if (!sent) + return; + + hci_bdaddr_list_add(&hdev->le_white_list, &sent->bdaddr, + sent->bdaddr_type); +} + +static void hci_cc_le_del_from_white_list(struct hci_dev *hdev, + struct sk_buff *skb) +{ + struct hci_cp_le_del_from_white_list *sent; + __u8 status = *((__u8 *) skb->data); + + BT_DBG("%s status 0x%2.2x", hdev->name, status); + + if (status) + return; + + sent = hci_sent_cmd_data(hdev, HCI_OP_LE_DEL_FROM_WHITE_LIST); + if (!sent) + return; + + hci_bdaddr_list_del(&hdev->le_white_list, &sent->bdaddr, + sent->bdaddr_type); +} + +static void hci_cc_le_read_supported_states(struct hci_dev *hdev, + struct sk_buff *skb) +{ + struct hci_rp_le_read_supported_states *rp = (void *) skb->data; + + BT_DBG("%s status 0x%2.2x", hdev->name, rp->status); + + if (rp->status) + return; + + memcpy(hdev->le_states, rp->le_states, 8); +} + +static void hci_cc_le_read_def_data_len(struct hci_dev *hdev, + struct sk_buff *skb) +{ + struct hci_rp_le_read_def_data_len *rp = (void *) skb->data; + + BT_DBG("%s status 0x%2.2x", hdev->name, rp->status); + + if (rp->status) + return; + + hdev->le_def_tx_len = le16_to_cpu(rp->tx_len); + hdev->le_def_tx_time = le16_to_cpu(rp->tx_time); +} + +static void hci_cc_le_write_def_data_len(struct hci_dev *hdev, + struct sk_buff *skb) +{ + struct hci_cp_le_write_def_data_len *sent; + __u8 status = *((__u8 *) skb->data); + + BT_DBG("%s status 0x%2.2x", hdev->name, status); + + if (status) + return; + + sent = hci_sent_cmd_data(hdev, HCI_OP_LE_WRITE_DEF_DATA_LEN); + if (!sent) + return; + + hdev->le_def_tx_len = le16_to_cpu(sent->tx_len); + hdev->le_def_tx_time = le16_to_cpu(sent->tx_time); +} + +static void hci_cc_le_read_max_data_len(struct hci_dev *hdev, + struct sk_buff *skb) +{ + struct hci_rp_le_read_max_data_len *rp = (void *) skb->data; + + BT_DBG("%s status 0x%2.2x", hdev->name, rp->status); + + if (rp->status) + return; + + hdev->le_max_tx_len = le16_to_cpu(rp->tx_len); + hdev->le_max_tx_time = le16_to_cpu(rp->tx_time); + hdev->le_max_rx_len = le16_to_cpu(rp->rx_len); + hdev->le_max_rx_time = le16_to_cpu(rp->rx_time); +} + +static void hci_cc_write_le_host_supported(struct hci_dev *hdev, + struct sk_buff *skb) +{ + struct hci_cp_write_le_host_supported *sent; + __u8 status = *((__u8 *) skb->data); + + BT_DBG("%s status 0x%2.2x", hdev->name, status); + + if (status) + return; + + sent = hci_sent_cmd_data(hdev, HCI_OP_WRITE_LE_HOST_SUPPORTED); + if (!sent) + return; + + hci_dev_lock(hdev); + + if (sent->le) { + hdev->features[1][0] |= LMP_HOST_LE; + hci_dev_set_flag(hdev, HCI_LE_ENABLED); + } else { + hdev->features[1][0] &= ~LMP_HOST_LE; + hci_dev_clear_flag(hdev, HCI_LE_ENABLED); + hci_dev_clear_flag(hdev, HCI_ADVERTISING); + } + + if (sent->simul) + hdev->features[1][0] |= LMP_HOST_LE_BREDR; + else + hdev->features[1][0] &= ~LMP_HOST_LE_BREDR; + + hci_dev_unlock(hdev); +} + +static void hci_cc_set_adv_param(struct hci_dev *hdev, struct sk_buff *skb) +{ + struct hci_cp_le_set_adv_param *cp; + u8 status = *((u8 *) skb->data); + + BT_DBG("%s status 0x%2.2x", hdev->name, status); + + if (status) + return; + + cp = hci_sent_cmd_data(hdev, HCI_OP_LE_SET_ADV_PARAM); + if (!cp) + return; + + hci_dev_lock(hdev); + hdev->adv_addr_type = cp->own_address_type; + hci_dev_unlock(hdev); +} + +static void hci_cc_write_remote_amp_assoc(struct hci_dev *hdev, + struct sk_buff *skb) +{ + struct hci_rp_write_remote_amp_assoc *rp = (void *) skb->data; + + BT_DBG("%s status 0x%2.2x phy_handle 0x%2.2x", + hdev->name, rp->status, rp->phy_handle); + + if (rp->status) + return; + + amp_write_rem_assoc_continue(hdev, rp->phy_handle); +} + +static void hci_cc_read_rssi(struct hci_dev *hdev, struct sk_buff *skb) +{ + struct hci_rp_read_rssi *rp = (void *) skb->data; + struct hci_conn *conn; + + BT_DBG("%s status 0x%2.2x", hdev->name, rp->status); + + if (rp->status) + return; + + hci_dev_lock(hdev); + + conn = hci_conn_hash_lookup_handle(hdev, __le16_to_cpu(rp->handle)); + if (conn) + conn->rssi = rp->rssi; + + hci_dev_unlock(hdev); +} + +static void hci_cc_read_tx_power(struct hci_dev *hdev, struct sk_buff *skb) +{ + struct hci_cp_read_tx_power *sent; + struct hci_rp_read_tx_power *rp = (void *) skb->data; + struct hci_conn *conn; + + BT_DBG("%s status 0x%2.2x", hdev->name, rp->status); + + if (rp->status) + return; + + sent = hci_sent_cmd_data(hdev, HCI_OP_READ_TX_POWER); + if (!sent) + return; + + hci_dev_lock(hdev); + + conn = hci_conn_hash_lookup_handle(hdev, __le16_to_cpu(rp->handle)); + if (!conn) + goto unlock; + + switch (sent->type) { + case 0x00: + conn->tx_power = rp->tx_power; + break; + case 0x01: + conn->max_tx_power = rp->tx_power; + break; + } + +unlock: + hci_dev_unlock(hdev); +} + +static void hci_cc_write_ssp_debug_mode(struct hci_dev *hdev, struct sk_buff *skb) +{ + u8 status = *((u8 *) skb->data); + u8 *mode; + + BT_DBG("%s status 0x%2.2x", hdev->name, status); + + if (status) + return; + + mode = hci_sent_cmd_data(hdev, HCI_OP_WRITE_SSP_DEBUG_MODE); + if (mode) + hdev->ssp_debug_mode = *mode; +} + +static void hci_cs_inquiry(struct hci_dev *hdev, __u8 status) +{ + BT_DBG("%s status 0x%2.2x", hdev->name, status); + + if (status) { + hci_conn_check_pending(hdev); + return; + } + + set_bit(HCI_INQUIRY, &hdev->flags); +} + +static void hci_cs_create_conn(struct hci_dev *hdev, __u8 status) +{ + struct hci_cp_create_conn *cp; + struct hci_conn *conn; + + BT_DBG("%s status 0x%2.2x", hdev->name, status); + + cp = hci_sent_cmd_data(hdev, HCI_OP_CREATE_CONN); + if (!cp) + return; + + hci_dev_lock(hdev); + + conn = hci_conn_hash_lookup_ba(hdev, ACL_LINK, &cp->bdaddr); + + BT_DBG("%s bdaddr %pMR hcon %p", hdev->name, &cp->bdaddr, conn); + + if (status) { + if (conn && conn->state == BT_CONNECT) { + if (status != 0x0c || conn->attempt > 2) { + conn->state = BT_CLOSED; + hci_connect_cfm(conn, status); + hci_conn_del(conn); + } else + conn->state = BT_CONNECT2; + } + } else { + if (!conn) { + conn = hci_conn_add(hdev, ACL_LINK, &cp->bdaddr, + HCI_ROLE_MASTER); + if (!conn) + BT_ERR("No memory for new connection"); + } + } + + hci_dev_unlock(hdev); +} + +static void hci_cs_add_sco(struct hci_dev *hdev, __u8 status) +{ + struct hci_cp_add_sco *cp; + struct hci_conn *acl, *sco; + __u16 handle; + + BT_DBG("%s status 0x%2.2x", hdev->name, status); + + if (!status) + return; + + cp = hci_sent_cmd_data(hdev, HCI_OP_ADD_SCO); + if (!cp) + return; + + handle = __le16_to_cpu(cp->handle); + + BT_DBG("%s handle 0x%4.4x", hdev->name, handle); + + hci_dev_lock(hdev); + + acl = hci_conn_hash_lookup_handle(hdev, handle); + if (acl) { + sco = acl->link; + if (sco) { + sco->state = BT_CLOSED; + + hci_connect_cfm(sco, status); + hci_conn_del(sco); + } + } + + hci_dev_unlock(hdev); +} + +static void hci_cs_auth_requested(struct hci_dev *hdev, __u8 status) +{ + struct hci_cp_auth_requested *cp; + struct hci_conn *conn; + + BT_DBG("%s status 0x%2.2x", hdev->name, status); + + if (!status) + return; + + cp = hci_sent_cmd_data(hdev, HCI_OP_AUTH_REQUESTED); + if (!cp) + return; + + hci_dev_lock(hdev); + + conn = hci_conn_hash_lookup_handle(hdev, __le16_to_cpu(cp->handle)); + if (conn) { + if (conn->state == BT_CONFIG) { + hci_connect_cfm(conn, status); + hci_conn_drop(conn); + } + } + + hci_dev_unlock(hdev); +} + +static void hci_cs_set_conn_encrypt(struct hci_dev *hdev, __u8 status) +{ + struct hci_cp_set_conn_encrypt *cp; + struct hci_conn *conn; + + BT_DBG("%s status 0x%2.2x", hdev->name, status); + + if (!status) + return; + + cp = hci_sent_cmd_data(hdev, HCI_OP_SET_CONN_ENCRYPT); + if (!cp) + return; + + hci_dev_lock(hdev); + + conn = hci_conn_hash_lookup_handle(hdev, __le16_to_cpu(cp->handle)); + if (conn) { + if (conn->state == BT_CONFIG) { + hci_connect_cfm(conn, status); + hci_conn_drop(conn); + } + } + + hci_dev_unlock(hdev); +} + +static int hci_outgoing_auth_needed(struct hci_dev *hdev, + struct hci_conn *conn) +{ + if (conn->state != BT_CONFIG || !conn->out) + return 0; + + if (conn->pending_sec_level == BT_SECURITY_SDP) + return 0; + + /* Only request authentication for SSP connections or non-SSP + * devices with sec_level MEDIUM or HIGH or if MITM protection + * is requested. + */ + if (!hci_conn_ssp_enabled(conn) && !(conn->auth_type & 0x01) && + conn->pending_sec_level != BT_SECURITY_FIPS && + conn->pending_sec_level != BT_SECURITY_HIGH && + conn->pending_sec_level != BT_SECURITY_MEDIUM) + return 0; + + return 1; +} + +static int hci_resolve_name(struct hci_dev *hdev, + struct inquiry_entry *e) +{ + struct hci_cp_remote_name_req cp; + + memset(&cp, 0, sizeof(cp)); + + bacpy(&cp.bdaddr, &e->data.bdaddr); + cp.pscan_rep_mode = e->data.pscan_rep_mode; + cp.pscan_mode = e->data.pscan_mode; + cp.clock_offset = e->data.clock_offset; + + return hci_send_cmd(hdev, HCI_OP_REMOTE_NAME_REQ, sizeof(cp), &cp); +} + +static bool hci_resolve_next_name(struct hci_dev *hdev) +{ + struct discovery_state *discov = &hdev->discovery; + struct inquiry_entry *e; + + if (list_empty(&discov->resolve)) + return false; + + e = hci_inquiry_cache_lookup_resolve(hdev, BDADDR_ANY, NAME_NEEDED); + if (!e) + return false; + + if (hci_resolve_name(hdev, e) == 0) { + e->name_state = NAME_PENDING; + return true; + } + + return false; +} + +static void hci_check_pending_name(struct hci_dev *hdev, struct hci_conn *conn, + bdaddr_t *bdaddr, u8 *name, u8 name_len) +{ + struct discovery_state *discov = &hdev->discovery; + struct inquiry_entry *e; + + /* Update the mgmt connected state if necessary. Be careful with + * conn objects that exist but are not (yet) connected however. + * Only those in BT_CONFIG or BT_CONNECTED states can be + * considered connected. + */ + if (conn && + (conn->state == BT_CONFIG || conn->state == BT_CONNECTED) && + !test_and_set_bit(HCI_CONN_MGMT_CONNECTED, &conn->flags)) + mgmt_device_connected(hdev, conn, 0, name, name_len); + + if (discov->state == DISCOVERY_STOPPED) + return; + + if (discov->state == DISCOVERY_STOPPING) + goto discov_complete; + + if (discov->state != DISCOVERY_RESOLVING) + return; + + e = hci_inquiry_cache_lookup_resolve(hdev, bdaddr, NAME_PENDING); + /* If the device was not found in a list of found devices names of which + * are pending. there is no need to continue resolving a next name as it + * will be done upon receiving another Remote Name Request Complete + * Event */ + if (!e) + return; + + list_del(&e->list); + if (name) { + e->name_state = NAME_KNOWN; + mgmt_remote_name(hdev, bdaddr, ACL_LINK, 0x00, + e->data.rssi, name, name_len); + } else { + e->name_state = NAME_NOT_KNOWN; + } + + if (hci_resolve_next_name(hdev)) + return; + +discov_complete: + hci_discovery_set_state(hdev, DISCOVERY_STOPPED); +} + +static void hci_cs_remote_name_req(struct hci_dev *hdev, __u8 status) +{ + struct hci_cp_remote_name_req *cp; + struct hci_conn *conn; + + BT_DBG("%s status 0x%2.2x", hdev->name, status); + + /* If successful wait for the name req complete event before + * checking for the need to do authentication */ + if (!status) + return; + + cp = hci_sent_cmd_data(hdev, HCI_OP_REMOTE_NAME_REQ); + if (!cp) + return; + + hci_dev_lock(hdev); + + conn = hci_conn_hash_lookup_ba(hdev, ACL_LINK, &cp->bdaddr); + + if (hci_dev_test_flag(hdev, HCI_MGMT)) + hci_check_pending_name(hdev, conn, &cp->bdaddr, NULL, 0); + + if (!conn) + goto unlock; + + if (!hci_outgoing_auth_needed(hdev, conn)) + goto unlock; + + if (!test_and_set_bit(HCI_CONN_AUTH_PEND, &conn->flags)) { + struct hci_cp_auth_requested auth_cp; + + set_bit(HCI_CONN_AUTH_INITIATOR, &conn->flags); + + auth_cp.handle = __cpu_to_le16(conn->handle); + hci_send_cmd(hdev, HCI_OP_AUTH_REQUESTED, + sizeof(auth_cp), &auth_cp); + } + +unlock: + hci_dev_unlock(hdev); +} + +static void hci_cs_read_remote_features(struct hci_dev *hdev, __u8 status) +{ + struct hci_cp_read_remote_features *cp; + struct hci_conn *conn; + + BT_DBG("%s status 0x%2.2x", hdev->name, status); + + if (!status) + return; + + cp = hci_sent_cmd_data(hdev, HCI_OP_READ_REMOTE_FEATURES); + if (!cp) + return; + + hci_dev_lock(hdev); + + conn = hci_conn_hash_lookup_handle(hdev, __le16_to_cpu(cp->handle)); + if (conn) { + if (conn->state == BT_CONFIG) { + hci_connect_cfm(conn, status); + hci_conn_drop(conn); + } + } + + hci_dev_unlock(hdev); +} + +static void hci_cs_read_remote_ext_features(struct hci_dev *hdev, __u8 status) +{ + struct hci_cp_read_remote_ext_features *cp; + struct hci_conn *conn; + + BT_DBG("%s status 0x%2.2x", hdev->name, status); + + if (!status) + return; + + cp = hci_sent_cmd_data(hdev, HCI_OP_READ_REMOTE_EXT_FEATURES); + if (!cp) + return; + + hci_dev_lock(hdev); + + conn = hci_conn_hash_lookup_handle(hdev, __le16_to_cpu(cp->handle)); + if (conn) { + if (conn->state == BT_CONFIG) { + hci_connect_cfm(conn, status); + hci_conn_drop(conn); + } + } + + hci_dev_unlock(hdev); +} + +static void hci_cs_setup_sync_conn(struct hci_dev *hdev, __u8 status) +{ + struct hci_cp_setup_sync_conn *cp; + struct hci_conn *acl, *sco; + __u16 handle; + + BT_DBG("%s status 0x%2.2x", hdev->name, status); + + if (!status) + return; + + cp = hci_sent_cmd_data(hdev, HCI_OP_SETUP_SYNC_CONN); + if (!cp) + return; + + handle = __le16_to_cpu(cp->handle); + + BT_DBG("%s handle 0x%4.4x", hdev->name, handle); + + hci_dev_lock(hdev); + + acl = hci_conn_hash_lookup_handle(hdev, handle); + if (acl) { + sco = acl->link; + if (sco) { + sco->state = BT_CLOSED; + + hci_connect_cfm(sco, status); + hci_conn_del(sco); + } + } + + hci_dev_unlock(hdev); +} + +static void hci_cs_sniff_mode(struct hci_dev *hdev, __u8 status) +{ + struct hci_cp_sniff_mode *cp; + struct hci_conn *conn; + + BT_DBG("%s status 0x%2.2x", hdev->name, status); + + if (!status) + return; + + cp = hci_sent_cmd_data(hdev, HCI_OP_SNIFF_MODE); + if (!cp) + return; + + hci_dev_lock(hdev); + + conn = hci_conn_hash_lookup_handle(hdev, __le16_to_cpu(cp->handle)); + if (conn) { + clear_bit(HCI_CONN_MODE_CHANGE_PEND, &conn->flags); + + if (test_and_clear_bit(HCI_CONN_SCO_SETUP_PEND, &conn->flags)) + hci_sco_setup(conn, status); + } + + hci_dev_unlock(hdev); +} + +static void hci_cs_exit_sniff_mode(struct hci_dev *hdev, __u8 status) +{ + struct hci_cp_exit_sniff_mode *cp; + struct hci_conn *conn; + + BT_DBG("%s status 0x%2.2x", hdev->name, status); + + if (!status) + return; + + cp = hci_sent_cmd_data(hdev, HCI_OP_EXIT_SNIFF_MODE); + if (!cp) + return; + + hci_dev_lock(hdev); + + conn = hci_conn_hash_lookup_handle(hdev, __le16_to_cpu(cp->handle)); + if (conn) { + clear_bit(HCI_CONN_MODE_CHANGE_PEND, &conn->flags); + + if (test_and_clear_bit(HCI_CONN_SCO_SETUP_PEND, &conn->flags)) + hci_sco_setup(conn, status); + } + + hci_dev_unlock(hdev); +} + +static void hci_cs_disconnect(struct hci_dev *hdev, u8 status) +{ + struct hci_cp_disconnect *cp; + struct hci_conn *conn; + + if (!status) + return; + + cp = hci_sent_cmd_data(hdev, HCI_OP_DISCONNECT); + if (!cp) + return; + + hci_dev_lock(hdev); + + conn = hci_conn_hash_lookup_handle(hdev, __le16_to_cpu(cp->handle)); + if (conn) + mgmt_disconnect_failed(hdev, &conn->dst, conn->type, + conn->dst_type, status); + + hci_dev_unlock(hdev); +} + +static void hci_cs_create_phylink(struct hci_dev *hdev, u8 status) +{ + struct hci_cp_create_phy_link *cp; + + BT_DBG("%s status 0x%2.2x", hdev->name, status); + + cp = hci_sent_cmd_data(hdev, HCI_OP_CREATE_PHY_LINK); + if (!cp) + return; + + hci_dev_lock(hdev); + + if (status) { + struct hci_conn *hcon; + + hcon = hci_conn_hash_lookup_handle(hdev, cp->phy_handle); + if (hcon) + hci_conn_del(hcon); + } else { + amp_write_remote_assoc(hdev, cp->phy_handle); + } + + hci_dev_unlock(hdev); +} + +static void hci_cs_accept_phylink(struct hci_dev *hdev, u8 status) +{ + struct hci_cp_accept_phy_link *cp; + + BT_DBG("%s status 0x%2.2x", hdev->name, status); + + if (status) + return; + + cp = hci_sent_cmd_data(hdev, HCI_OP_ACCEPT_PHY_LINK); + if (!cp) + return; + + amp_write_remote_assoc(hdev, cp->phy_handle); +} + +static void hci_cs_le_create_conn(struct hci_dev *hdev, u8 status) +{ + struct hci_cp_le_create_conn *cp; + struct hci_conn *conn; + + BT_DBG("%s status 0x%2.2x", hdev->name, status); + + /* All connection failure handling is taken care of by the + * hci_le_conn_failed function which is triggered by the HCI + * request completion callbacks used for connecting. + */ + if (status) + return; + + cp = hci_sent_cmd_data(hdev, HCI_OP_LE_CREATE_CONN); + if (!cp) + return; + + hci_dev_lock(hdev); + + conn = hci_conn_hash_lookup_ba(hdev, LE_LINK, &cp->peer_addr); + if (!conn) + goto unlock; + + /* Store the initiator and responder address information which + * is needed for SMP. These values will not change during the + * lifetime of the connection. + */ + conn->init_addr_type = cp->own_address_type; + if (cp->own_address_type == ADDR_LE_DEV_RANDOM) + bacpy(&conn->init_addr, &hdev->random_addr); + else + bacpy(&conn->init_addr, &hdev->bdaddr); + + conn->resp_addr_type = cp->peer_addr_type; + bacpy(&conn->resp_addr, &cp->peer_addr); + + /* We don't want the connection attempt to stick around + * indefinitely since LE doesn't have a page timeout concept + * like BR/EDR. Set a timer for any connection that doesn't use + * the white list for connecting. + */ + if (cp->filter_policy == HCI_LE_USE_PEER_ADDR) + queue_delayed_work(conn->hdev->workqueue, + &conn->le_conn_timeout, + conn->conn_timeout); + +unlock: + hci_dev_unlock(hdev); +} + +static void hci_cs_le_read_remote_features(struct hci_dev *hdev, u8 status) +{ + struct hci_cp_le_read_remote_features *cp; + struct hci_conn *conn; + + BT_DBG("%s status 0x%2.2x", hdev->name, status); + + if (!status) + return; + + cp = hci_sent_cmd_data(hdev, HCI_OP_LE_READ_REMOTE_FEATURES); + if (!cp) + return; + + hci_dev_lock(hdev); + + conn = hci_conn_hash_lookup_handle(hdev, __le16_to_cpu(cp->handle)); + if (conn) { + if (conn->state == BT_CONFIG) { + hci_connect_cfm(conn, status); + hci_conn_drop(conn); + } + } + + hci_dev_unlock(hdev); +} + +static void hci_cs_le_start_enc(struct hci_dev *hdev, u8 status) +{ + struct hci_cp_le_start_enc *cp; + struct hci_conn *conn; + + BT_DBG("%s status 0x%2.2x", hdev->name, status); + + if (!status) + return; + + hci_dev_lock(hdev); + + cp = hci_sent_cmd_data(hdev, HCI_OP_LE_START_ENC); + if (!cp) + goto unlock; + + conn = hci_conn_hash_lookup_handle(hdev, __le16_to_cpu(cp->handle)); + if (!conn) + goto unlock; + + if (conn->state != BT_CONNECTED) + goto unlock; + + hci_disconnect(conn, HCI_ERROR_AUTH_FAILURE); + hci_conn_drop(conn); + +unlock: + hci_dev_unlock(hdev); +} + +static void hci_cs_switch_role(struct hci_dev *hdev, u8 status) +{ + struct hci_cp_switch_role *cp; + struct hci_conn *conn; + + BT_DBG("%s status 0x%2.2x", hdev->name, status); + + if (!status) + return; + + cp = hci_sent_cmd_data(hdev, HCI_OP_SWITCH_ROLE); + if (!cp) + return; + + hci_dev_lock(hdev); + + conn = hci_conn_hash_lookup_ba(hdev, ACL_LINK, &cp->bdaddr); + if (conn) + clear_bit(HCI_CONN_RSWITCH_PEND, &conn->flags); + + hci_dev_unlock(hdev); +} + +static void hci_inquiry_complete_evt(struct hci_dev *hdev, struct sk_buff *skb) +{ + __u8 status = *((__u8 *) skb->data); + struct discovery_state *discov = &hdev->discovery; + struct inquiry_entry *e; + + BT_DBG("%s status 0x%2.2x", hdev->name, status); + + hci_conn_check_pending(hdev); + + if (!test_and_clear_bit(HCI_INQUIRY, &hdev->flags)) + return; + + smp_mb__after_atomic(); /* wake_up_bit advises about this barrier */ + wake_up_bit(&hdev->flags, HCI_INQUIRY); + + if (!hci_dev_test_flag(hdev, HCI_MGMT)) + return; + + hci_dev_lock(hdev); + + if (discov->state != DISCOVERY_FINDING) + goto unlock; + + if (list_empty(&discov->resolve)) { + /* When BR/EDR inquiry is active and no LE scanning is in + * progress, then change discovery state to indicate completion. + * + * When running LE scanning and BR/EDR inquiry simultaneously + * and the LE scan already finished, then change the discovery + * state to indicate completion. + */ + if (!hci_dev_test_flag(hdev, HCI_LE_SCAN) || + !test_bit(HCI_QUIRK_SIMULTANEOUS_DISCOVERY, &hdev->quirks)) + hci_discovery_set_state(hdev, DISCOVERY_STOPPED); + goto unlock; + } + + e = hci_inquiry_cache_lookup_resolve(hdev, BDADDR_ANY, NAME_NEEDED); + if (e && hci_resolve_name(hdev, e) == 0) { + e->name_state = NAME_PENDING; + hci_discovery_set_state(hdev, DISCOVERY_RESOLVING); + } else { + /* When BR/EDR inquiry is active and no LE scanning is in + * progress, then change discovery state to indicate completion. + * + * When running LE scanning and BR/EDR inquiry simultaneously + * and the LE scan already finished, then change the discovery + * state to indicate completion. + */ + if (!hci_dev_test_flag(hdev, HCI_LE_SCAN) || + !test_bit(HCI_QUIRK_SIMULTANEOUS_DISCOVERY, &hdev->quirks)) + hci_discovery_set_state(hdev, DISCOVERY_STOPPED); + } + +unlock: + hci_dev_unlock(hdev); +} + +static void hci_inquiry_result_evt(struct hci_dev *hdev, struct sk_buff *skb) +{ + struct inquiry_data data; + struct inquiry_info *info = (void *) (skb->data + 1); + int num_rsp = *((__u8 *) skb->data); + + BT_DBG("%s num_rsp %d", hdev->name, num_rsp); + + if (!num_rsp) + return; + + if (hci_dev_test_flag(hdev, HCI_PERIODIC_INQ)) + return; + + hci_dev_lock(hdev); + + for (; num_rsp; num_rsp--, info++) { + u32 flags; + + bacpy(&data.bdaddr, &info->bdaddr); + data.pscan_rep_mode = info->pscan_rep_mode; + data.pscan_period_mode = info->pscan_period_mode; + data.pscan_mode = info->pscan_mode; + memcpy(data.dev_class, info->dev_class, 3); + data.clock_offset = info->clock_offset; + data.rssi = HCI_RSSI_INVALID; + data.ssp_mode = 0x00; + + flags = hci_inquiry_cache_update(hdev, &data, false); + + mgmt_device_found(hdev, &info->bdaddr, ACL_LINK, 0x00, + info->dev_class, HCI_RSSI_INVALID, + flags, NULL, 0, NULL, 0); + } + + hci_dev_unlock(hdev); +} + +static void hci_conn_complete_evt(struct hci_dev *hdev, struct sk_buff *skb) +{ + struct hci_ev_conn_complete *ev = (void *) skb->data; + struct hci_conn *conn; + + BT_DBG("%s", hdev->name); + + hci_dev_lock(hdev); + + conn = hci_conn_hash_lookup_ba(hdev, ev->link_type, &ev->bdaddr); + if (!conn) { + if (ev->link_type != SCO_LINK) + goto unlock; + + conn = hci_conn_hash_lookup_ba(hdev, ESCO_LINK, &ev->bdaddr); + if (!conn) + goto unlock; + + conn->type = SCO_LINK; + } + + if (!ev->status) { + conn->handle = __le16_to_cpu(ev->handle); + + if (conn->type == ACL_LINK) { + conn->state = BT_CONFIG; + hci_conn_hold(conn); + + if (!conn->out && !hci_conn_ssp_enabled(conn) && + !hci_find_link_key(hdev, &ev->bdaddr)) + conn->disc_timeout = HCI_PAIRING_TIMEOUT; + else + conn->disc_timeout = HCI_DISCONN_TIMEOUT; + } else + conn->state = BT_CONNECTED; + + hci_debugfs_create_conn(conn); + hci_conn_add_sysfs(conn); + + if (test_bit(HCI_AUTH, &hdev->flags)) + set_bit(HCI_CONN_AUTH, &conn->flags); + + if (test_bit(HCI_ENCRYPT, &hdev->flags)) + set_bit(HCI_CONN_ENCRYPT, &conn->flags); + + /* Get remote features */ + if (conn->type == ACL_LINK) { + struct hci_cp_read_remote_features cp; + cp.handle = ev->handle; + hci_send_cmd(hdev, HCI_OP_READ_REMOTE_FEATURES, + sizeof(cp), &cp); + + hci_update_page_scan(hdev); + } + + /* Set packet type for incoming connection */ + if (!conn->out && hdev->hci_ver < BLUETOOTH_VER_2_0) { + struct hci_cp_change_conn_ptype cp; + cp.handle = ev->handle; + cp.pkt_type = cpu_to_le16(conn->pkt_type); + hci_send_cmd(hdev, HCI_OP_CHANGE_CONN_PTYPE, sizeof(cp), + &cp); + } + } else { + conn->state = BT_CLOSED; + if (conn->type == ACL_LINK) + mgmt_connect_failed(hdev, &conn->dst, conn->type, + conn->dst_type, ev->status); + } + + if (conn->type == ACL_LINK) + hci_sco_setup(conn, ev->status); + + if (ev->status) { + hci_connect_cfm(conn, ev->status); + hci_conn_del(conn); + } else if (ev->link_type != ACL_LINK) + hci_connect_cfm(conn, ev->status); + +unlock: + hci_dev_unlock(hdev); + + hci_conn_check_pending(hdev); +} + +static void hci_reject_conn(struct hci_dev *hdev, bdaddr_t *bdaddr) +{ + struct hci_cp_reject_conn_req cp; + + bacpy(&cp.bdaddr, bdaddr); + cp.reason = HCI_ERROR_REJ_BAD_ADDR; + hci_send_cmd(hdev, HCI_OP_REJECT_CONN_REQ, sizeof(cp), &cp); +} + +static void hci_conn_request_evt(struct hci_dev *hdev, struct sk_buff *skb) +{ + struct hci_ev_conn_request *ev = (void *) skb->data; + int mask = hdev->link_mode; + struct inquiry_entry *ie; + struct hci_conn *conn; + __u8 flags = 0; + + BT_DBG("%s bdaddr %pMR type 0x%x", hdev->name, &ev->bdaddr, + ev->link_type); + + mask |= hci_proto_connect_ind(hdev, &ev->bdaddr, ev->link_type, + &flags); + + if (!(mask & HCI_LM_ACCEPT)) { + hci_reject_conn(hdev, &ev->bdaddr); + return; + } + + if (hci_bdaddr_list_lookup(&hdev->blacklist, &ev->bdaddr, + BDADDR_BREDR)) { + hci_reject_conn(hdev, &ev->bdaddr); + return; + } + + /* Require HCI_CONNECTABLE or a whitelist entry to accept the + * connection. These features are only touched through mgmt so + * only do the checks if HCI_MGMT is set. + */ + if (hci_dev_test_flag(hdev, HCI_MGMT) && + !hci_dev_test_flag(hdev, HCI_CONNECTABLE) && + !hci_bdaddr_list_lookup(&hdev->whitelist, &ev->bdaddr, + BDADDR_BREDR)) { + hci_reject_conn(hdev, &ev->bdaddr); + return; + } + + /* Connection accepted */ + + hci_dev_lock(hdev); + + ie = hci_inquiry_cache_lookup(hdev, &ev->bdaddr); + if (ie) + memcpy(ie->data.dev_class, ev->dev_class, 3); + + conn = hci_conn_hash_lookup_ba(hdev, ev->link_type, + &ev->bdaddr); + if (!conn) { + conn = hci_conn_add(hdev, ev->link_type, &ev->bdaddr, + HCI_ROLE_SLAVE); + if (!conn) { + BT_ERR("No memory for new connection"); + hci_dev_unlock(hdev); + return; + } + } + + memcpy(conn->dev_class, ev->dev_class, 3); + + hci_dev_unlock(hdev); + + if (ev->link_type == ACL_LINK || + (!(flags & HCI_PROTO_DEFER) && !lmp_esco_capable(hdev))) { + struct hci_cp_accept_conn_req cp; + conn->state = BT_CONNECT; + + bacpy(&cp.bdaddr, &ev->bdaddr); + + if (lmp_rswitch_capable(hdev) && (mask & HCI_LM_MASTER)) + cp.role = 0x00; /* Become master */ + else + cp.role = 0x01; /* Remain slave */ + + hci_send_cmd(hdev, HCI_OP_ACCEPT_CONN_REQ, sizeof(cp), &cp); + } else if (!(flags & HCI_PROTO_DEFER)) { + struct hci_cp_accept_sync_conn_req cp; + conn->state = BT_CONNECT; + + bacpy(&cp.bdaddr, &ev->bdaddr); + cp.pkt_type = cpu_to_le16(conn->pkt_type); + + cp.tx_bandwidth = cpu_to_le32(0x00001f40); + cp.rx_bandwidth = cpu_to_le32(0x00001f40); + cp.max_latency = cpu_to_le16(0xffff); + cp.content_format = cpu_to_le16(hdev->voice_setting); + cp.retrans_effort = 0xff; + + hci_send_cmd(hdev, HCI_OP_ACCEPT_SYNC_CONN_REQ, sizeof(cp), + &cp); + } else { + conn->state = BT_CONNECT2; + hci_connect_cfm(conn, 0); + } +} + +static u8 hci_to_mgmt_reason(u8 err) +{ + switch (err) { + case HCI_ERROR_CONNECTION_TIMEOUT: + return MGMT_DEV_DISCONN_TIMEOUT; + case HCI_ERROR_REMOTE_USER_TERM: + case HCI_ERROR_REMOTE_LOW_RESOURCES: + case HCI_ERROR_REMOTE_POWER_OFF: + return MGMT_DEV_DISCONN_REMOTE; + case HCI_ERROR_LOCAL_HOST_TERM: + return MGMT_DEV_DISCONN_LOCAL_HOST; + default: + return MGMT_DEV_DISCONN_UNKNOWN; + } +} + +static void hci_disconn_complete_evt(struct hci_dev *hdev, struct sk_buff *skb) +{ + struct hci_ev_disconn_complete *ev = (void *) skb->data; + u8 reason = hci_to_mgmt_reason(ev->reason); + struct hci_conn_params *params; + struct hci_conn *conn; + bool mgmt_connected; + u8 type; + + BT_DBG("%s status 0x%2.2x", hdev->name, ev->status); + + hci_dev_lock(hdev); + + conn = hci_conn_hash_lookup_handle(hdev, __le16_to_cpu(ev->handle)); + if (!conn) + goto unlock; + + if (ev->status) { + mgmt_disconnect_failed(hdev, &conn->dst, conn->type, + conn->dst_type, ev->status); + goto unlock; + } + + conn->state = BT_CLOSED; + + mgmt_connected = test_and_clear_bit(HCI_CONN_MGMT_CONNECTED, &conn->flags); + mgmt_device_disconnected(hdev, &conn->dst, conn->type, conn->dst_type, + reason, mgmt_connected); + + if (conn->type == ACL_LINK) { + if (test_bit(HCI_CONN_FLUSH_KEY, &conn->flags)) + hci_remove_link_key(hdev, &conn->dst); + + hci_update_page_scan(hdev); + } + + params = hci_conn_params_lookup(hdev, &conn->dst, conn->dst_type); + if (params) { + switch (params->auto_connect) { + case HCI_AUTO_CONN_LINK_LOSS: + if (ev->reason != HCI_ERROR_CONNECTION_TIMEOUT) + break; + /* Fall through */ + + case HCI_AUTO_CONN_DIRECT: + case HCI_AUTO_CONN_ALWAYS: + list_del_init(¶ms->action); + list_add(¶ms->action, &hdev->pend_le_conns); + hci_update_background_scan(hdev); + break; + + default: + break; + } + } + + type = conn->type; + + hci_disconn_cfm(conn, ev->reason); + hci_conn_del(conn); + + /* Re-enable advertising if necessary, since it might + * have been disabled by the connection. From the + * HCI_LE_Set_Advertise_Enable command description in + * the core specification (v4.0): + * "The Controller shall continue advertising until the Host + * issues an LE_Set_Advertise_Enable command with + * Advertising_Enable set to 0x00 (Advertising is disabled) + * or until a connection is created or until the Advertising + * is timed out due to Directed Advertising." + */ + if (type == LE_LINK) + mgmt_reenable_advertising(hdev); + +unlock: + hci_dev_unlock(hdev); +} + +static void hci_auth_complete_evt(struct hci_dev *hdev, struct sk_buff *skb) +{ + struct hci_ev_auth_complete *ev = (void *) skb->data; + struct hci_conn *conn; + + BT_DBG("%s status 0x%2.2x", hdev->name, ev->status); + + hci_dev_lock(hdev); + + conn = hci_conn_hash_lookup_handle(hdev, __le16_to_cpu(ev->handle)); + if (!conn) + goto unlock; + + if (!ev->status) { + if (!hci_conn_ssp_enabled(conn) && + test_bit(HCI_CONN_REAUTH_PEND, &conn->flags)) { + BT_INFO("re-auth of legacy device is not possible."); + } else { + set_bit(HCI_CONN_AUTH, &conn->flags); + conn->sec_level = conn->pending_sec_level; + } + } else { + mgmt_auth_failed(conn, ev->status); + } + + clear_bit(HCI_CONN_AUTH_PEND, &conn->flags); + clear_bit(HCI_CONN_REAUTH_PEND, &conn->flags); + + if (conn->state == BT_CONFIG) { + if (!ev->status && hci_conn_ssp_enabled(conn)) { + struct hci_cp_set_conn_encrypt cp; + cp.handle = ev->handle; + cp.encrypt = 0x01; + hci_send_cmd(hdev, HCI_OP_SET_CONN_ENCRYPT, sizeof(cp), + &cp); + } else { + conn->state = BT_CONNECTED; + hci_connect_cfm(conn, ev->status); + hci_conn_drop(conn); + } + } else { + hci_auth_cfm(conn, ev->status); + + hci_conn_hold(conn); + conn->disc_timeout = HCI_DISCONN_TIMEOUT; + hci_conn_drop(conn); + } + + if (test_bit(HCI_CONN_ENCRYPT_PEND, &conn->flags)) { + if (!ev->status) { + struct hci_cp_set_conn_encrypt cp; + cp.handle = ev->handle; + cp.encrypt = 0x01; + hci_send_cmd(hdev, HCI_OP_SET_CONN_ENCRYPT, sizeof(cp), + &cp); + } else { + clear_bit(HCI_CONN_ENCRYPT_PEND, &conn->flags); + hci_encrypt_cfm(conn, ev->status, 0x00); + } + } + +unlock: + hci_dev_unlock(hdev); +} + +static void hci_remote_name_evt(struct hci_dev *hdev, struct sk_buff *skb) +{ + struct hci_ev_remote_name *ev = (void *) skb->data; + struct hci_conn *conn; + + BT_DBG("%s", hdev->name); + + hci_conn_check_pending(hdev); + + hci_dev_lock(hdev); + + conn = hci_conn_hash_lookup_ba(hdev, ACL_LINK, &ev->bdaddr); + + if (!hci_dev_test_flag(hdev, HCI_MGMT)) + goto check_auth; + + if (ev->status == 0) + hci_check_pending_name(hdev, conn, &ev->bdaddr, ev->name, + strnlen(ev->name, HCI_MAX_NAME_LENGTH)); + else + hci_check_pending_name(hdev, conn, &ev->bdaddr, NULL, 0); + +check_auth: + if (!conn) + goto unlock; + + if (!hci_outgoing_auth_needed(hdev, conn)) + goto unlock; + + if (!test_and_set_bit(HCI_CONN_AUTH_PEND, &conn->flags)) { + struct hci_cp_auth_requested cp; + + set_bit(HCI_CONN_AUTH_INITIATOR, &conn->flags); + + cp.handle = __cpu_to_le16(conn->handle); + hci_send_cmd(hdev, HCI_OP_AUTH_REQUESTED, sizeof(cp), &cp); + } + +unlock: + hci_dev_unlock(hdev); +} + +static void hci_encrypt_change_evt(struct hci_dev *hdev, struct sk_buff *skb) +{ + struct hci_ev_encrypt_change *ev = (void *) skb->data; + struct hci_conn *conn; + + BT_DBG("%s status 0x%2.2x", hdev->name, ev->status); + + hci_dev_lock(hdev); + + conn = hci_conn_hash_lookup_handle(hdev, __le16_to_cpu(ev->handle)); + if (!conn) + goto unlock; + + if (!ev->status) { + if (ev->encrypt) { + /* Encryption implies authentication */ + set_bit(HCI_CONN_AUTH, &conn->flags); + set_bit(HCI_CONN_ENCRYPT, &conn->flags); + conn->sec_level = conn->pending_sec_level; + + /* P-256 authentication key implies FIPS */ + if (conn->key_type == HCI_LK_AUTH_COMBINATION_P256) + set_bit(HCI_CONN_FIPS, &conn->flags); + + if ((conn->type == ACL_LINK && ev->encrypt == 0x02) || + conn->type == LE_LINK) + set_bit(HCI_CONN_AES_CCM, &conn->flags); + } else { + clear_bit(HCI_CONN_ENCRYPT, &conn->flags); + clear_bit(HCI_CONN_AES_CCM, &conn->flags); + } + } + + /* We should disregard the current RPA and generate a new one + * whenever the encryption procedure fails. + */ + if (ev->status && conn->type == LE_LINK) + hci_dev_set_flag(hdev, HCI_RPA_EXPIRED); + + clear_bit(HCI_CONN_ENCRYPT_PEND, &conn->flags); + + if (ev->status && conn->state == BT_CONNECTED) { + hci_disconnect(conn, HCI_ERROR_AUTH_FAILURE); + hci_conn_drop(conn); + goto unlock; + } + + if (conn->state == BT_CONFIG) { + if (!ev->status) + conn->state = BT_CONNECTED; + + /* In Secure Connections Only mode, do not allow any + * connections that are not encrypted with AES-CCM + * using a P-256 authenticated combination key. + */ + if (hci_dev_test_flag(hdev, HCI_SC_ONLY) && + (!test_bit(HCI_CONN_AES_CCM, &conn->flags) || + conn->key_type != HCI_LK_AUTH_COMBINATION_P256)) { + hci_connect_cfm(conn, HCI_ERROR_AUTH_FAILURE); + hci_conn_drop(conn); + goto unlock; + } + + hci_connect_cfm(conn, ev->status); + hci_conn_drop(conn); + } else + hci_encrypt_cfm(conn, ev->status, ev->encrypt); + +unlock: + hci_dev_unlock(hdev); +} + +static void hci_change_link_key_complete_evt(struct hci_dev *hdev, + struct sk_buff *skb) +{ + struct hci_ev_change_link_key_complete *ev = (void *) skb->data; + struct hci_conn *conn; + + BT_DBG("%s status 0x%2.2x", hdev->name, ev->status); + + hci_dev_lock(hdev); + + conn = hci_conn_hash_lookup_handle(hdev, __le16_to_cpu(ev->handle)); + if (conn) { + if (!ev->status) + set_bit(HCI_CONN_SECURE, &conn->flags); + + clear_bit(HCI_CONN_AUTH_PEND, &conn->flags); + + hci_key_change_cfm(conn, ev->status); + } + + hci_dev_unlock(hdev); +} + +static void hci_remote_features_evt(struct hci_dev *hdev, + struct sk_buff *skb) +{ + struct hci_ev_remote_features *ev = (void *) skb->data; + struct hci_conn *conn; + + BT_DBG("%s status 0x%2.2x", hdev->name, ev->status); + + hci_dev_lock(hdev); + + conn = hci_conn_hash_lookup_handle(hdev, __le16_to_cpu(ev->handle)); + if (!conn) + goto unlock; + + if (!ev->status) + memcpy(conn->features[0], ev->features, 8); + + if (conn->state != BT_CONFIG) + goto unlock; + + if (!ev->status && lmp_ext_feat_capable(hdev) && + lmp_ext_feat_capable(conn)) { + struct hci_cp_read_remote_ext_features cp; + cp.handle = ev->handle; + cp.page = 0x01; + hci_send_cmd(hdev, HCI_OP_READ_REMOTE_EXT_FEATURES, + sizeof(cp), &cp); + goto unlock; + } + + if (!ev->status && !test_bit(HCI_CONN_MGMT_CONNECTED, &conn->flags)) { + struct hci_cp_remote_name_req cp; + memset(&cp, 0, sizeof(cp)); + bacpy(&cp.bdaddr, &conn->dst); + cp.pscan_rep_mode = 0x02; + hci_send_cmd(hdev, HCI_OP_REMOTE_NAME_REQ, sizeof(cp), &cp); + } else if (!test_and_set_bit(HCI_CONN_MGMT_CONNECTED, &conn->flags)) + mgmt_device_connected(hdev, conn, 0, NULL, 0); + + if (!hci_outgoing_auth_needed(hdev, conn)) { + conn->state = BT_CONNECTED; + hci_connect_cfm(conn, ev->status); + hci_conn_drop(conn); + } + +unlock: + hci_dev_unlock(hdev); +} + +static void hci_cmd_complete_evt(struct hci_dev *hdev, struct sk_buff *skb, + u16 *opcode, u8 *status, + hci_req_complete_t *req_complete, + hci_req_complete_skb_t *req_complete_skb) +{ + struct hci_ev_cmd_complete *ev = (void *) skb->data; + + *opcode = __le16_to_cpu(ev->opcode); + *status = skb->data[sizeof(*ev)]; + + skb_pull(skb, sizeof(*ev)); + + switch (*opcode) { + case HCI_OP_INQUIRY_CANCEL: + hci_cc_inquiry_cancel(hdev, skb); + break; + + case HCI_OP_PERIODIC_INQ: + hci_cc_periodic_inq(hdev, skb); + break; + + case HCI_OP_EXIT_PERIODIC_INQ: + hci_cc_exit_periodic_inq(hdev, skb); + break; + + case HCI_OP_REMOTE_NAME_REQ_CANCEL: + hci_cc_remote_name_req_cancel(hdev, skb); + break; + + case HCI_OP_ROLE_DISCOVERY: + hci_cc_role_discovery(hdev, skb); + break; + + case HCI_OP_READ_LINK_POLICY: + hci_cc_read_link_policy(hdev, skb); + break; + + case HCI_OP_WRITE_LINK_POLICY: + hci_cc_write_link_policy(hdev, skb); + break; + + case HCI_OP_READ_DEF_LINK_POLICY: + hci_cc_read_def_link_policy(hdev, skb); + break; + + case HCI_OP_WRITE_DEF_LINK_POLICY: + hci_cc_write_def_link_policy(hdev, skb); + break; + + case HCI_OP_RESET: + hci_cc_reset(hdev, skb); + break; + + case HCI_OP_READ_STORED_LINK_KEY: + hci_cc_read_stored_link_key(hdev, skb); + break; + + case HCI_OP_DELETE_STORED_LINK_KEY: + hci_cc_delete_stored_link_key(hdev, skb); + break; + + case HCI_OP_WRITE_LOCAL_NAME: + hci_cc_write_local_name(hdev, skb); + break; + + case HCI_OP_READ_LOCAL_NAME: + hci_cc_read_local_name(hdev, skb); + break; + + case HCI_OP_WRITE_AUTH_ENABLE: + hci_cc_write_auth_enable(hdev, skb); + break; + + case HCI_OP_WRITE_ENCRYPT_MODE: + hci_cc_write_encrypt_mode(hdev, skb); + break; + + case HCI_OP_WRITE_SCAN_ENABLE: + hci_cc_write_scan_enable(hdev, skb); + break; + + case HCI_OP_READ_CLASS_OF_DEV: + hci_cc_read_class_of_dev(hdev, skb); + break; + + case HCI_OP_WRITE_CLASS_OF_DEV: + hci_cc_write_class_of_dev(hdev, skb); + break; + + case HCI_OP_READ_VOICE_SETTING: + hci_cc_read_voice_setting(hdev, skb); + break; + + case HCI_OP_WRITE_VOICE_SETTING: + hci_cc_write_voice_setting(hdev, skb); + break; + + case HCI_OP_READ_NUM_SUPPORTED_IAC: + hci_cc_read_num_supported_iac(hdev, skb); + break; + + case HCI_OP_WRITE_SSP_MODE: + hci_cc_write_ssp_mode(hdev, skb); + break; + + case HCI_OP_WRITE_SC_SUPPORT: + hci_cc_write_sc_support(hdev, skb); + break; + + case HCI_OP_READ_LOCAL_VERSION: + hci_cc_read_local_version(hdev, skb); + break; + + case HCI_OP_READ_LOCAL_COMMANDS: + hci_cc_read_local_commands(hdev, skb); + break; + + case HCI_OP_READ_LOCAL_FEATURES: + hci_cc_read_local_features(hdev, skb); + break; + + case HCI_OP_READ_LOCAL_EXT_FEATURES: + hci_cc_read_local_ext_features(hdev, skb); + break; + + case HCI_OP_READ_BUFFER_SIZE: + hci_cc_read_buffer_size(hdev, skb); + break; + + case HCI_OP_READ_BD_ADDR: + hci_cc_read_bd_addr(hdev, skb); + break; + + case HCI_OP_READ_PAGE_SCAN_ACTIVITY: + hci_cc_read_page_scan_activity(hdev, skb); + break; + + case HCI_OP_WRITE_PAGE_SCAN_ACTIVITY: + hci_cc_write_page_scan_activity(hdev, skb); + break; + + case HCI_OP_READ_PAGE_SCAN_TYPE: + hci_cc_read_page_scan_type(hdev, skb); + break; + + case HCI_OP_WRITE_PAGE_SCAN_TYPE: + hci_cc_write_page_scan_type(hdev, skb); + break; + + case HCI_OP_READ_DATA_BLOCK_SIZE: + hci_cc_read_data_block_size(hdev, skb); + break; + + case HCI_OP_READ_FLOW_CONTROL_MODE: + hci_cc_read_flow_control_mode(hdev, skb); + break; + + case HCI_OP_READ_LOCAL_AMP_INFO: + hci_cc_read_local_amp_info(hdev, skb); + break; + + case HCI_OP_READ_CLOCK: + hci_cc_read_clock(hdev, skb); + break; + + case HCI_OP_READ_LOCAL_AMP_ASSOC: + hci_cc_read_local_amp_assoc(hdev, skb); + break; + + case HCI_OP_READ_INQ_RSP_TX_POWER: + hci_cc_read_inq_rsp_tx_power(hdev, skb); + break; + + case HCI_OP_PIN_CODE_REPLY: + hci_cc_pin_code_reply(hdev, skb); + break; + + case HCI_OP_PIN_CODE_NEG_REPLY: + hci_cc_pin_code_neg_reply(hdev, skb); + break; + + case HCI_OP_READ_LOCAL_OOB_DATA: + hci_cc_read_local_oob_data(hdev, skb); + break; + + case HCI_OP_READ_LOCAL_OOB_EXT_DATA: + hci_cc_read_local_oob_ext_data(hdev, skb); + break; + + case HCI_OP_LE_READ_BUFFER_SIZE: + hci_cc_le_read_buffer_size(hdev, skb); + break; + + case HCI_OP_LE_READ_LOCAL_FEATURES: + hci_cc_le_read_local_features(hdev, skb); + break; + + case HCI_OP_LE_READ_ADV_TX_POWER: + hci_cc_le_read_adv_tx_power(hdev, skb); + break; + + case HCI_OP_USER_CONFIRM_REPLY: + hci_cc_user_confirm_reply(hdev, skb); + break; + + case HCI_OP_USER_CONFIRM_NEG_REPLY: + hci_cc_user_confirm_neg_reply(hdev, skb); + break; + + case HCI_OP_USER_PASSKEY_REPLY: + hci_cc_user_passkey_reply(hdev, skb); + break; + + case HCI_OP_USER_PASSKEY_NEG_REPLY: + hci_cc_user_passkey_neg_reply(hdev, skb); + break; + + case HCI_OP_LE_SET_RANDOM_ADDR: + hci_cc_le_set_random_addr(hdev, skb); + break; + + case HCI_OP_LE_SET_ADV_ENABLE: + hci_cc_le_set_adv_enable(hdev, skb); + break; + + case HCI_OP_LE_SET_SCAN_PARAM: + hci_cc_le_set_scan_param(hdev, skb); + break; + + case HCI_OP_LE_SET_SCAN_ENABLE: + hci_cc_le_set_scan_enable(hdev, skb); + break; + + case HCI_OP_LE_READ_WHITE_LIST_SIZE: + hci_cc_le_read_white_list_size(hdev, skb); + break; + + case HCI_OP_LE_CLEAR_WHITE_LIST: + hci_cc_le_clear_white_list(hdev, skb); + break; + + case HCI_OP_LE_ADD_TO_WHITE_LIST: + hci_cc_le_add_to_white_list(hdev, skb); + break; + + case HCI_OP_LE_DEL_FROM_WHITE_LIST: + hci_cc_le_del_from_white_list(hdev, skb); + break; + + case HCI_OP_LE_READ_SUPPORTED_STATES: + hci_cc_le_read_supported_states(hdev, skb); + break; + + case HCI_OP_LE_READ_DEF_DATA_LEN: + hci_cc_le_read_def_data_len(hdev, skb); + break; + + case HCI_OP_LE_WRITE_DEF_DATA_LEN: + hci_cc_le_write_def_data_len(hdev, skb); + break; + + case HCI_OP_LE_READ_MAX_DATA_LEN: + hci_cc_le_read_max_data_len(hdev, skb); + break; + + case HCI_OP_WRITE_LE_HOST_SUPPORTED: + hci_cc_write_le_host_supported(hdev, skb); + break; + + case HCI_OP_LE_SET_ADV_PARAM: + hci_cc_set_adv_param(hdev, skb); + break; + + case HCI_OP_WRITE_REMOTE_AMP_ASSOC: + hci_cc_write_remote_amp_assoc(hdev, skb); + break; + + case HCI_OP_READ_RSSI: + hci_cc_read_rssi(hdev, skb); + break; + + case HCI_OP_READ_TX_POWER: + hci_cc_read_tx_power(hdev, skb); + break; + + case HCI_OP_WRITE_SSP_DEBUG_MODE: + hci_cc_write_ssp_debug_mode(hdev, skb); + break; + + default: + BT_DBG("%s opcode 0x%4.4x", hdev->name, *opcode); + break; + } + + if (*opcode != HCI_OP_NOP) + cancel_delayed_work(&hdev->cmd_timer); + + if (ev->ncmd && !test_bit(HCI_RESET, &hdev->flags)) + atomic_set(&hdev->cmd_cnt, 1); + + hci_req_cmd_complete(hdev, *opcode, *status, req_complete, + req_complete_skb); + + if (atomic_read(&hdev->cmd_cnt) && !skb_queue_empty(&hdev->cmd_q)) + queue_work(hdev->workqueue, &hdev->cmd_work); +} + +static void hci_cmd_status_evt(struct hci_dev *hdev, struct sk_buff *skb, + u16 *opcode, u8 *status, + hci_req_complete_t *req_complete, + hci_req_complete_skb_t *req_complete_skb) +{ + struct hci_ev_cmd_status *ev = (void *) skb->data; + + skb_pull(skb, sizeof(*ev)); + + *opcode = __le16_to_cpu(ev->opcode); + *status = ev->status; + + switch (*opcode) { + case HCI_OP_INQUIRY: + hci_cs_inquiry(hdev, ev->status); + break; + + case HCI_OP_CREATE_CONN: + hci_cs_create_conn(hdev, ev->status); + break; + + case HCI_OP_DISCONNECT: + hci_cs_disconnect(hdev, ev->status); + break; + + case HCI_OP_ADD_SCO: + hci_cs_add_sco(hdev, ev->status); + break; + + case HCI_OP_AUTH_REQUESTED: + hci_cs_auth_requested(hdev, ev->status); + break; + + case HCI_OP_SET_CONN_ENCRYPT: + hci_cs_set_conn_encrypt(hdev, ev->status); + break; + + case HCI_OP_REMOTE_NAME_REQ: + hci_cs_remote_name_req(hdev, ev->status); + break; + + case HCI_OP_READ_REMOTE_FEATURES: + hci_cs_read_remote_features(hdev, ev->status); + break; + + case HCI_OP_READ_REMOTE_EXT_FEATURES: + hci_cs_read_remote_ext_features(hdev, ev->status); + break; + + case HCI_OP_SETUP_SYNC_CONN: + hci_cs_setup_sync_conn(hdev, ev->status); + break; + + case HCI_OP_CREATE_PHY_LINK: + hci_cs_create_phylink(hdev, ev->status); + break; + + case HCI_OP_ACCEPT_PHY_LINK: + hci_cs_accept_phylink(hdev, ev->status); + break; + + case HCI_OP_SNIFF_MODE: + hci_cs_sniff_mode(hdev, ev->status); + break; + + case HCI_OP_EXIT_SNIFF_MODE: + hci_cs_exit_sniff_mode(hdev, ev->status); + break; + + case HCI_OP_SWITCH_ROLE: + hci_cs_switch_role(hdev, ev->status); + break; + + case HCI_OP_LE_CREATE_CONN: + hci_cs_le_create_conn(hdev, ev->status); + break; + + case HCI_OP_LE_READ_REMOTE_FEATURES: + hci_cs_le_read_remote_features(hdev, ev->status); + break; + + case HCI_OP_LE_START_ENC: + hci_cs_le_start_enc(hdev, ev->status); + break; + + default: + BT_DBG("%s opcode 0x%4.4x", hdev->name, *opcode); + break; + } + + if (*opcode != HCI_OP_NOP) + cancel_delayed_work(&hdev->cmd_timer); + + if (ev->ncmd && !test_bit(HCI_RESET, &hdev->flags)) + atomic_set(&hdev->cmd_cnt, 1); + + /* Indicate request completion if the command failed. Also, if + * we're not waiting for a special event and we get a success + * command status we should try to flag the request as completed + * (since for this kind of commands there will not be a command + * complete event). + */ + if (ev->status || + (hdev->sent_cmd && !bt_cb(hdev->sent_cmd)->req.event)) + hci_req_cmd_complete(hdev, *opcode, ev->status, req_complete, + req_complete_skb); + + if (atomic_read(&hdev->cmd_cnt) && !skb_queue_empty(&hdev->cmd_q)) + queue_work(hdev->workqueue, &hdev->cmd_work); +} + +static void hci_hardware_error_evt(struct hci_dev *hdev, struct sk_buff *skb) +{ + struct hci_ev_hardware_error *ev = (void *) skb->data; + + hdev->hw_error_code = ev->code; + + queue_work(hdev->req_workqueue, &hdev->error_reset); +} + +static void hci_role_change_evt(struct hci_dev *hdev, struct sk_buff *skb) +{ + struct hci_ev_role_change *ev = (void *) skb->data; + struct hci_conn *conn; + + BT_DBG("%s status 0x%2.2x", hdev->name, ev->status); + + hci_dev_lock(hdev); + + conn = hci_conn_hash_lookup_ba(hdev, ACL_LINK, &ev->bdaddr); + if (conn) { + if (!ev->status) + conn->role = ev->role; + + clear_bit(HCI_CONN_RSWITCH_PEND, &conn->flags); + + hci_role_switch_cfm(conn, ev->status, ev->role); + } + + hci_dev_unlock(hdev); +} + +static void hci_num_comp_pkts_evt(struct hci_dev *hdev, struct sk_buff *skb) +{ + struct hci_ev_num_comp_pkts *ev = (void *) skb->data; + int i; + + if (hdev->flow_ctl_mode != HCI_FLOW_CTL_MODE_PACKET_BASED) { + BT_ERR("Wrong event for mode %d", hdev->flow_ctl_mode); + return; + } + + if (skb->len < sizeof(*ev) || skb->len < sizeof(*ev) + + ev->num_hndl * sizeof(struct hci_comp_pkts_info)) { + BT_DBG("%s bad parameters", hdev->name); + return; + } + + BT_DBG("%s num_hndl %d", hdev->name, ev->num_hndl); + + for (i = 0; i < ev->num_hndl; i++) { + struct hci_comp_pkts_info *info = &ev->handles[i]; + struct hci_conn *conn; + __u16 handle, count; + + handle = __le16_to_cpu(info->handle); + count = __le16_to_cpu(info->count); + + conn = hci_conn_hash_lookup_handle(hdev, handle); + if (!conn) + continue; + + conn->sent -= count; + + switch (conn->type) { + case ACL_LINK: + hdev->acl_cnt += count; + if (hdev->acl_cnt > hdev->acl_pkts) + hdev->acl_cnt = hdev->acl_pkts; + break; + + case LE_LINK: + if (hdev->le_pkts) { + hdev->le_cnt += count; + if (hdev->le_cnt > hdev->le_pkts) + hdev->le_cnt = hdev->le_pkts; + } else { + hdev->acl_cnt += count; + if (hdev->acl_cnt > hdev->acl_pkts) + hdev->acl_cnt = hdev->acl_pkts; + } + break; + + case SCO_LINK: + hdev->sco_cnt += count; + if (hdev->sco_cnt > hdev->sco_pkts) + hdev->sco_cnt = hdev->sco_pkts; + break; + + default: + BT_ERR("Unknown type %d conn %p", conn->type, conn); + break; + } + } + + queue_work(hdev->workqueue, &hdev->tx_work); +} + +static struct hci_conn *__hci_conn_lookup_handle(struct hci_dev *hdev, + __u16 handle) +{ + struct hci_chan *chan; + + switch (hdev->dev_type) { + case HCI_BREDR: + return hci_conn_hash_lookup_handle(hdev, handle); + case HCI_AMP: + chan = hci_chan_lookup_handle(hdev, handle); + if (chan) + return chan->conn; + break; + default: + BT_ERR("%s unknown dev_type %d", hdev->name, hdev->dev_type); + break; + } + + return NULL; +} + +static void hci_num_comp_blocks_evt(struct hci_dev *hdev, struct sk_buff *skb) +{ + struct hci_ev_num_comp_blocks *ev = (void *) skb->data; + int i; + + if (hdev->flow_ctl_mode != HCI_FLOW_CTL_MODE_BLOCK_BASED) { + BT_ERR("Wrong event for mode %d", hdev->flow_ctl_mode); + return; + } + + if (skb->len < sizeof(*ev) || skb->len < sizeof(*ev) + + ev->num_hndl * sizeof(struct hci_comp_blocks_info)) { + BT_DBG("%s bad parameters", hdev->name); + return; + } + + BT_DBG("%s num_blocks %d num_hndl %d", hdev->name, ev->num_blocks, + ev->num_hndl); + + for (i = 0; i < ev->num_hndl; i++) { + struct hci_comp_blocks_info *info = &ev->handles[i]; + struct hci_conn *conn = NULL; + __u16 handle, block_count; + + handle = __le16_to_cpu(info->handle); + block_count = __le16_to_cpu(info->blocks); + + conn = __hci_conn_lookup_handle(hdev, handle); + if (!conn) + continue; + + conn->sent -= block_count; + + switch (conn->type) { + case ACL_LINK: + case AMP_LINK: + hdev->block_cnt += block_count; + if (hdev->block_cnt > hdev->num_blocks) + hdev->block_cnt = hdev->num_blocks; + break; + + default: + BT_ERR("Unknown type %d conn %p", conn->type, conn); + break; + } + } + + queue_work(hdev->workqueue, &hdev->tx_work); +} + +static void hci_mode_change_evt(struct hci_dev *hdev, struct sk_buff *skb) +{ + struct hci_ev_mode_change *ev = (void *) skb->data; + struct hci_conn *conn; + + BT_DBG("%s status 0x%2.2x", hdev->name, ev->status); + + hci_dev_lock(hdev); + + conn = hci_conn_hash_lookup_handle(hdev, __le16_to_cpu(ev->handle)); + if (conn) { + conn->mode = ev->mode; + + if (!test_and_clear_bit(HCI_CONN_MODE_CHANGE_PEND, + &conn->flags)) { + if (conn->mode == HCI_CM_ACTIVE) + set_bit(HCI_CONN_POWER_SAVE, &conn->flags); + else + clear_bit(HCI_CONN_POWER_SAVE, &conn->flags); + } + + if (test_and_clear_bit(HCI_CONN_SCO_SETUP_PEND, &conn->flags)) + hci_sco_setup(conn, ev->status); + } + + hci_dev_unlock(hdev); +} + +static void hci_pin_code_request_evt(struct hci_dev *hdev, struct sk_buff *skb) +{ + struct hci_ev_pin_code_req *ev = (void *) skb->data; + struct hci_conn *conn; + + BT_DBG("%s", hdev->name); + + hci_dev_lock(hdev); + + conn = hci_conn_hash_lookup_ba(hdev, ACL_LINK, &ev->bdaddr); + if (!conn) + goto unlock; + + if (conn->state == BT_CONNECTED) { + hci_conn_hold(conn); + conn->disc_timeout = HCI_PAIRING_TIMEOUT; + hci_conn_drop(conn); + } + + if (!hci_dev_test_flag(hdev, HCI_BONDABLE) && + !test_bit(HCI_CONN_AUTH_INITIATOR, &conn->flags)) { + hci_send_cmd(hdev, HCI_OP_PIN_CODE_NEG_REPLY, + sizeof(ev->bdaddr), &ev->bdaddr); + } else if (hci_dev_test_flag(hdev, HCI_MGMT)) { + u8 secure; + + if (conn->pending_sec_level == BT_SECURITY_HIGH) + secure = 1; + else + secure = 0; + + mgmt_pin_code_request(hdev, &ev->bdaddr, secure); + } + +unlock: + hci_dev_unlock(hdev); +} + +static void conn_set_key(struct hci_conn *conn, u8 key_type, u8 pin_len) +{ + if (key_type == HCI_LK_CHANGED_COMBINATION) + return; + + conn->pin_length = pin_len; + conn->key_type = key_type; + + switch (key_type) { + case HCI_LK_LOCAL_UNIT: + case HCI_LK_REMOTE_UNIT: + case HCI_LK_DEBUG_COMBINATION: + return; + case HCI_LK_COMBINATION: + if (pin_len == 16) + conn->pending_sec_level = BT_SECURITY_HIGH; + else + conn->pending_sec_level = BT_SECURITY_MEDIUM; + break; + case HCI_LK_UNAUTH_COMBINATION_P192: + case HCI_LK_UNAUTH_COMBINATION_P256: + conn->pending_sec_level = BT_SECURITY_MEDIUM; + break; + case HCI_LK_AUTH_COMBINATION_P192: + conn->pending_sec_level = BT_SECURITY_HIGH; + break; + case HCI_LK_AUTH_COMBINATION_P256: + conn->pending_sec_level = BT_SECURITY_FIPS; + break; + } +} + +static void hci_link_key_request_evt(struct hci_dev *hdev, struct sk_buff *skb) +{ + struct hci_ev_link_key_req *ev = (void *) skb->data; + struct hci_cp_link_key_reply cp; + struct hci_conn *conn; + struct link_key *key; + + BT_DBG("%s", hdev->name); + + if (!hci_dev_test_flag(hdev, HCI_MGMT)) + return; + + hci_dev_lock(hdev); + + key = hci_find_link_key(hdev, &ev->bdaddr); + if (!key) { + BT_DBG("%s link key not found for %pMR", hdev->name, + &ev->bdaddr); + goto not_found; + } + + BT_DBG("%s found key type %u for %pMR", hdev->name, key->type, + &ev->bdaddr); + + conn = hci_conn_hash_lookup_ba(hdev, ACL_LINK, &ev->bdaddr); + if (conn) { + clear_bit(HCI_CONN_NEW_LINK_KEY, &conn->flags); + + if ((key->type == HCI_LK_UNAUTH_COMBINATION_P192 || + key->type == HCI_LK_UNAUTH_COMBINATION_P256) && + conn->auth_type != 0xff && (conn->auth_type & 0x01)) { + BT_DBG("%s ignoring unauthenticated key", hdev->name); + goto not_found; + } + + if (key->type == HCI_LK_COMBINATION && key->pin_len < 16 && + (conn->pending_sec_level == BT_SECURITY_HIGH || + conn->pending_sec_level == BT_SECURITY_FIPS)) { + BT_DBG("%s ignoring key unauthenticated for high security", + hdev->name); + goto not_found; + } + + conn_set_key(conn, key->type, key->pin_len); + } + + bacpy(&cp.bdaddr, &ev->bdaddr); + memcpy(cp.link_key, key->val, HCI_LINK_KEY_SIZE); + + hci_send_cmd(hdev, HCI_OP_LINK_KEY_REPLY, sizeof(cp), &cp); + + hci_dev_unlock(hdev); + + return; + +not_found: + hci_send_cmd(hdev, HCI_OP_LINK_KEY_NEG_REPLY, 6, &ev->bdaddr); + hci_dev_unlock(hdev); +} + +static void hci_link_key_notify_evt(struct hci_dev *hdev, struct sk_buff *skb) +{ + struct hci_ev_link_key_notify *ev = (void *) skb->data; + struct hci_conn *conn; + struct link_key *key; + bool persistent; + u8 pin_len = 0; + + BT_DBG("%s", hdev->name); + + hci_dev_lock(hdev); + + conn = hci_conn_hash_lookup_ba(hdev, ACL_LINK, &ev->bdaddr); + if (!conn) + goto unlock; + + hci_conn_hold(conn); + conn->disc_timeout = HCI_DISCONN_TIMEOUT; + hci_conn_drop(conn); + + set_bit(HCI_CONN_NEW_LINK_KEY, &conn->flags); + conn_set_key(conn, ev->key_type, conn->pin_length); + + if (!hci_dev_test_flag(hdev, HCI_MGMT)) + goto unlock; + + key = hci_add_link_key(hdev, conn, &ev->bdaddr, ev->link_key, + ev->key_type, pin_len, &persistent); + if (!key) + goto unlock; + + /* Update connection information since adding the key will have + * fixed up the type in the case of changed combination keys. + */ + if (ev->key_type == HCI_LK_CHANGED_COMBINATION) + conn_set_key(conn, key->type, key->pin_len); + + mgmt_new_link_key(hdev, key, persistent); + + /* Keep debug keys around only if the HCI_KEEP_DEBUG_KEYS flag + * is set. If it's not set simply remove the key from the kernel + * list (we've still notified user space about it but with + * store_hint being 0). + */ + if (key->type == HCI_LK_DEBUG_COMBINATION && + !hci_dev_test_flag(hdev, HCI_KEEP_DEBUG_KEYS)) { + list_del_rcu(&key->list); + kfree_rcu(key, rcu); + goto unlock; + } + + if (persistent) + clear_bit(HCI_CONN_FLUSH_KEY, &conn->flags); + else + set_bit(HCI_CONN_FLUSH_KEY, &conn->flags); + +unlock: + hci_dev_unlock(hdev); +} + +static void hci_clock_offset_evt(struct hci_dev *hdev, struct sk_buff *skb) +{ + struct hci_ev_clock_offset *ev = (void *) skb->data; + struct hci_conn *conn; + + BT_DBG("%s status 0x%2.2x", hdev->name, ev->status); + + hci_dev_lock(hdev); + + conn = hci_conn_hash_lookup_handle(hdev, __le16_to_cpu(ev->handle)); + if (conn && !ev->status) { + struct inquiry_entry *ie; + + ie = hci_inquiry_cache_lookup(hdev, &conn->dst); + if (ie) { + ie->data.clock_offset = ev->clock_offset; + ie->timestamp = jiffies; + } + } + + hci_dev_unlock(hdev); +} + +static void hci_pkt_type_change_evt(struct hci_dev *hdev, struct sk_buff *skb) +{ + struct hci_ev_pkt_type_change *ev = (void *) skb->data; + struct hci_conn *conn; + + BT_DBG("%s status 0x%2.2x", hdev->name, ev->status); + + hci_dev_lock(hdev); + + conn = hci_conn_hash_lookup_handle(hdev, __le16_to_cpu(ev->handle)); + if (conn && !ev->status) + conn->pkt_type = __le16_to_cpu(ev->pkt_type); + + hci_dev_unlock(hdev); +} + +static void hci_pscan_rep_mode_evt(struct hci_dev *hdev, struct sk_buff *skb) +{ + struct hci_ev_pscan_rep_mode *ev = (void *) skb->data; + struct inquiry_entry *ie; + + BT_DBG("%s", hdev->name); + + hci_dev_lock(hdev); + + ie = hci_inquiry_cache_lookup(hdev, &ev->bdaddr); + if (ie) { + ie->data.pscan_rep_mode = ev->pscan_rep_mode; + ie->timestamp = jiffies; + } + + hci_dev_unlock(hdev); +} + +static void hci_inquiry_result_with_rssi_evt(struct hci_dev *hdev, + struct sk_buff *skb) +{ + struct inquiry_data data; + int num_rsp = *((__u8 *) skb->data); + + BT_DBG("%s num_rsp %d", hdev->name, num_rsp); + + if (!num_rsp) + return; + + if (hci_dev_test_flag(hdev, HCI_PERIODIC_INQ)) + return; + + hci_dev_lock(hdev); + + if ((skb->len - 1) / num_rsp != sizeof(struct inquiry_info_with_rssi)) { + struct inquiry_info_with_rssi_and_pscan_mode *info; + info = (void *) (skb->data + 1); + + for (; num_rsp; num_rsp--, info++) { + u32 flags; + + bacpy(&data.bdaddr, &info->bdaddr); + data.pscan_rep_mode = info->pscan_rep_mode; + data.pscan_period_mode = info->pscan_period_mode; + data.pscan_mode = info->pscan_mode; + memcpy(data.dev_class, info->dev_class, 3); + data.clock_offset = info->clock_offset; + data.rssi = info->rssi; + data.ssp_mode = 0x00; + + flags = hci_inquiry_cache_update(hdev, &data, false); + + mgmt_device_found(hdev, &info->bdaddr, ACL_LINK, 0x00, + info->dev_class, info->rssi, + flags, NULL, 0, NULL, 0); + } + } else { + struct inquiry_info_with_rssi *info = (void *) (skb->data + 1); + + for (; num_rsp; num_rsp--, info++) { + u32 flags; + + bacpy(&data.bdaddr, &info->bdaddr); + data.pscan_rep_mode = info->pscan_rep_mode; + data.pscan_period_mode = info->pscan_period_mode; + data.pscan_mode = 0x00; + memcpy(data.dev_class, info->dev_class, 3); + data.clock_offset = info->clock_offset; + data.rssi = info->rssi; + data.ssp_mode = 0x00; + + flags = hci_inquiry_cache_update(hdev, &data, false); + + mgmt_device_found(hdev, &info->bdaddr, ACL_LINK, 0x00, + info->dev_class, info->rssi, + flags, NULL, 0, NULL, 0); + } + } + + hci_dev_unlock(hdev); +} + +static void hci_remote_ext_features_evt(struct hci_dev *hdev, + struct sk_buff *skb) +{ + struct hci_ev_remote_ext_features *ev = (void *) skb->data; + struct hci_conn *conn; + + BT_DBG("%s", hdev->name); + + hci_dev_lock(hdev); + + conn = hci_conn_hash_lookup_handle(hdev, __le16_to_cpu(ev->handle)); + if (!conn) + goto unlock; + + if (ev->page < HCI_MAX_PAGES) + memcpy(conn->features[ev->page], ev->features, 8); + + if (!ev->status && ev->page == 0x01) { + struct inquiry_entry *ie; + + ie = hci_inquiry_cache_lookup(hdev, &conn->dst); + if (ie) + ie->data.ssp_mode = (ev->features[0] & LMP_HOST_SSP); + + if (ev->features[0] & LMP_HOST_SSP) { + set_bit(HCI_CONN_SSP_ENABLED, &conn->flags); + } else { + /* It is mandatory by the Bluetooth specification that + * Extended Inquiry Results are only used when Secure + * Simple Pairing is enabled, but some devices violate + * this. + * + * To make these devices work, the internal SSP + * enabled flag needs to be cleared if the remote host + * features do not indicate SSP support */ + clear_bit(HCI_CONN_SSP_ENABLED, &conn->flags); + } + + if (ev->features[0] & LMP_HOST_SC) + set_bit(HCI_CONN_SC_ENABLED, &conn->flags); + } + + if (conn->state != BT_CONFIG) + goto unlock; + + if (!ev->status && !test_bit(HCI_CONN_MGMT_CONNECTED, &conn->flags)) { + struct hci_cp_remote_name_req cp; + memset(&cp, 0, sizeof(cp)); + bacpy(&cp.bdaddr, &conn->dst); + cp.pscan_rep_mode = 0x02; + hci_send_cmd(hdev, HCI_OP_REMOTE_NAME_REQ, sizeof(cp), &cp); + } else if (!test_and_set_bit(HCI_CONN_MGMT_CONNECTED, &conn->flags)) + mgmt_device_connected(hdev, conn, 0, NULL, 0); + + if (!hci_outgoing_auth_needed(hdev, conn)) { + conn->state = BT_CONNECTED; + hci_connect_cfm(conn, ev->status); + hci_conn_drop(conn); + } + +unlock: + hci_dev_unlock(hdev); +} + +static void hci_sync_conn_complete_evt(struct hci_dev *hdev, + struct sk_buff *skb) +{ + struct hci_ev_sync_conn_complete *ev = (void *) skb->data; + struct hci_conn *conn; + + BT_DBG("%s status 0x%2.2x", hdev->name, ev->status); + + hci_dev_lock(hdev); + + conn = hci_conn_hash_lookup_ba(hdev, ev->link_type, &ev->bdaddr); + if (!conn) { + if (ev->link_type == ESCO_LINK) + goto unlock; + + conn = hci_conn_hash_lookup_ba(hdev, ESCO_LINK, &ev->bdaddr); + if (!conn) + goto unlock; + + conn->type = SCO_LINK; + } + + switch (ev->status) { + case 0x00: + conn->handle = __le16_to_cpu(ev->handle); + conn->state = BT_CONNECTED; + + hci_debugfs_create_conn(conn); + hci_conn_add_sysfs(conn); + break; + + case 0x10: /* Connection Accept Timeout */ + case 0x0d: /* Connection Rejected due to Limited Resources */ + case 0x11: /* Unsupported Feature or Parameter Value */ + case 0x1c: /* SCO interval rejected */ + case 0x1a: /* Unsupported Remote Feature */ + case 0x1f: /* Unspecified error */ + case 0x20: /* Unsupported LMP Parameter value */ + if (conn->out) { + conn->pkt_type = (hdev->esco_type & SCO_ESCO_MASK) | + (hdev->esco_type & EDR_ESCO_MASK); + if (hci_setup_sync(conn, conn->link->handle)) + goto unlock; + } + /* fall through */ + + default: + conn->state = BT_CLOSED; + break; + } + + hci_connect_cfm(conn, ev->status); + if (ev->status) + hci_conn_del(conn); + +unlock: + hci_dev_unlock(hdev); +} + +static inline size_t eir_get_length(u8 *eir, size_t eir_len) +{ + size_t parsed = 0; + + while (parsed < eir_len) { + u8 field_len = eir[0]; + + if (field_len == 0) + return parsed; + + parsed += field_len + 1; + eir += field_len + 1; + } + + return eir_len; +} + +static void hci_extended_inquiry_result_evt(struct hci_dev *hdev, + struct sk_buff *skb) +{ + struct inquiry_data data; + struct extended_inquiry_info *info = (void *) (skb->data + 1); + int num_rsp = *((__u8 *) skb->data); + size_t eir_len; + + BT_DBG("%s num_rsp %d", hdev->name, num_rsp); + + if (!num_rsp) + return; + + if (hci_dev_test_flag(hdev, HCI_PERIODIC_INQ)) + return; + + hci_dev_lock(hdev); + + for (; num_rsp; num_rsp--, info++) { + u32 flags; + bool name_known; + + bacpy(&data.bdaddr, &info->bdaddr); + data.pscan_rep_mode = info->pscan_rep_mode; + data.pscan_period_mode = info->pscan_period_mode; + data.pscan_mode = 0x00; + memcpy(data.dev_class, info->dev_class, 3); + data.clock_offset = info->clock_offset; + data.rssi = info->rssi; + data.ssp_mode = 0x01; + + if (hci_dev_test_flag(hdev, HCI_MGMT)) + name_known = eir_has_data_type(info->data, + sizeof(info->data), + EIR_NAME_COMPLETE); + else + name_known = true; + + flags = hci_inquiry_cache_update(hdev, &data, name_known); + + eir_len = eir_get_length(info->data, sizeof(info->data)); + + mgmt_device_found(hdev, &info->bdaddr, ACL_LINK, 0x00, + info->dev_class, info->rssi, + flags, info->data, eir_len, NULL, 0); + } + + hci_dev_unlock(hdev); +} + +static void hci_key_refresh_complete_evt(struct hci_dev *hdev, + struct sk_buff *skb) +{ + struct hci_ev_key_refresh_complete *ev = (void *) skb->data; + struct hci_conn *conn; + + BT_DBG("%s status 0x%2.2x handle 0x%4.4x", hdev->name, ev->status, + __le16_to_cpu(ev->handle)); + + hci_dev_lock(hdev); + + conn = hci_conn_hash_lookup_handle(hdev, __le16_to_cpu(ev->handle)); + if (!conn) + goto unlock; + + /* For BR/EDR the necessary steps are taken through the + * auth_complete event. + */ + if (conn->type != LE_LINK) + goto unlock; + + if (!ev->status) + conn->sec_level = conn->pending_sec_level; + + clear_bit(HCI_CONN_ENCRYPT_PEND, &conn->flags); + + if (ev->status && conn->state == BT_CONNECTED) { + hci_disconnect(conn, HCI_ERROR_AUTH_FAILURE); + hci_conn_drop(conn); + goto unlock; + } + + if (conn->state == BT_CONFIG) { + if (!ev->status) + conn->state = BT_CONNECTED; + + hci_connect_cfm(conn, ev->status); + hci_conn_drop(conn); + } else { + hci_auth_cfm(conn, ev->status); + + hci_conn_hold(conn); + conn->disc_timeout = HCI_DISCONN_TIMEOUT; + hci_conn_drop(conn); + } + +unlock: + hci_dev_unlock(hdev); +} + +static u8 hci_get_auth_req(struct hci_conn *conn) +{ + /* If remote requests no-bonding follow that lead */ + if (conn->remote_auth == HCI_AT_NO_BONDING || + conn->remote_auth == HCI_AT_NO_BONDING_MITM) + return conn->remote_auth | (conn->auth_type & 0x01); + + /* If both remote and local have enough IO capabilities, require + * MITM protection + */ + if (conn->remote_cap != HCI_IO_NO_INPUT_OUTPUT && + conn->io_capability != HCI_IO_NO_INPUT_OUTPUT) + return conn->remote_auth | 0x01; + + /* No MITM protection possible so ignore remote requirement */ + return (conn->remote_auth & ~0x01) | (conn->auth_type & 0x01); +} + +static u8 bredr_oob_data_present(struct hci_conn *conn) +{ + struct hci_dev *hdev = conn->hdev; + struct oob_data *data; + + data = hci_find_remote_oob_data(hdev, &conn->dst, BDADDR_BREDR); + if (!data) + return 0x00; + + if (bredr_sc_enabled(hdev)) { + /* When Secure Connections is enabled, then just + * return the present value stored with the OOB + * data. The stored value contains the right present + * information. However it can only be trusted when + * not in Secure Connection Only mode. + */ + if (!hci_dev_test_flag(hdev, HCI_SC_ONLY)) + return data->present; + + /* When Secure Connections Only mode is enabled, then + * the P-256 values are required. If they are not + * available, then do not declare that OOB data is + * present. + */ + if (!memcmp(data->rand256, ZERO_KEY, 16) || + !memcmp(data->hash256, ZERO_KEY, 16)) + return 0x00; + + return 0x02; + } + + /* When Secure Connections is not enabled or actually + * not supported by the hardware, then check that if + * P-192 data values are present. + */ + if (!memcmp(data->rand192, ZERO_KEY, 16) || + !memcmp(data->hash192, ZERO_KEY, 16)) + return 0x00; + + return 0x01; +} + +static void hci_io_capa_request_evt(struct hci_dev *hdev, struct sk_buff *skb) +{ + struct hci_ev_io_capa_request *ev = (void *) skb->data; + struct hci_conn *conn; + + BT_DBG("%s", hdev->name); + + hci_dev_lock(hdev); + + conn = hci_conn_hash_lookup_ba(hdev, ACL_LINK, &ev->bdaddr); + if (!conn) + goto unlock; + + hci_conn_hold(conn); + + if (!hci_dev_test_flag(hdev, HCI_MGMT)) + goto unlock; + + /* Allow pairing if we're pairable, the initiators of the + * pairing or if the remote is not requesting bonding. + */ + if (hci_dev_test_flag(hdev, HCI_BONDABLE) || + test_bit(HCI_CONN_AUTH_INITIATOR, &conn->flags) || + (conn->remote_auth & ~0x01) == HCI_AT_NO_BONDING) { + struct hci_cp_io_capability_reply cp; + + bacpy(&cp.bdaddr, &ev->bdaddr); + /* Change the IO capability from KeyboardDisplay + * to DisplayYesNo as it is not supported by BT spec. */ + cp.capability = (conn->io_capability == 0x04) ? + HCI_IO_DISPLAY_YESNO : conn->io_capability; + + /* If we are initiators, there is no remote information yet */ + if (conn->remote_auth == 0xff) { + /* Request MITM protection if our IO caps allow it + * except for the no-bonding case. + */ + if (conn->io_capability != HCI_IO_NO_INPUT_OUTPUT && + conn->auth_type != HCI_AT_NO_BONDING) + conn->auth_type |= 0x01; + } else { + conn->auth_type = hci_get_auth_req(conn); + } + + /* If we're not bondable, force one of the non-bondable + * authentication requirement values. + */ + if (!hci_dev_test_flag(hdev, HCI_BONDABLE)) + conn->auth_type &= HCI_AT_NO_BONDING_MITM; + + cp.authentication = conn->auth_type; + cp.oob_data = bredr_oob_data_present(conn); + + hci_send_cmd(hdev, HCI_OP_IO_CAPABILITY_REPLY, + sizeof(cp), &cp); + } else { + struct hci_cp_io_capability_neg_reply cp; + + bacpy(&cp.bdaddr, &ev->bdaddr); + cp.reason = HCI_ERROR_PAIRING_NOT_ALLOWED; + + hci_send_cmd(hdev, HCI_OP_IO_CAPABILITY_NEG_REPLY, + sizeof(cp), &cp); + } + +unlock: + hci_dev_unlock(hdev); +} + +static void hci_io_capa_reply_evt(struct hci_dev *hdev, struct sk_buff *skb) +{ + struct hci_ev_io_capa_reply *ev = (void *) skb->data; + struct hci_conn *conn; + + BT_DBG("%s", hdev->name); + + hci_dev_lock(hdev); + + conn = hci_conn_hash_lookup_ba(hdev, ACL_LINK, &ev->bdaddr); + if (!conn) + goto unlock; + + conn->remote_cap = ev->capability; + conn->remote_auth = ev->authentication; + +unlock: + hci_dev_unlock(hdev); +} + +static void hci_user_confirm_request_evt(struct hci_dev *hdev, + struct sk_buff *skb) +{ + struct hci_ev_user_confirm_req *ev = (void *) skb->data; + int loc_mitm, rem_mitm, confirm_hint = 0; + struct hci_conn *conn; + + BT_DBG("%s", hdev->name); + + hci_dev_lock(hdev); + + if (!hci_dev_test_flag(hdev, HCI_MGMT)) + goto unlock; + + conn = hci_conn_hash_lookup_ba(hdev, ACL_LINK, &ev->bdaddr); + if (!conn) + goto unlock; + + loc_mitm = (conn->auth_type & 0x01); + rem_mitm = (conn->remote_auth & 0x01); + + /* If we require MITM but the remote device can't provide that + * (it has NoInputNoOutput) then reject the confirmation + * request. We check the security level here since it doesn't + * necessarily match conn->auth_type. + */ + if (conn->pending_sec_level > BT_SECURITY_MEDIUM && + conn->remote_cap == HCI_IO_NO_INPUT_OUTPUT) { + BT_DBG("Rejecting request: remote device can't provide MITM"); + hci_send_cmd(hdev, HCI_OP_USER_CONFIRM_NEG_REPLY, + sizeof(ev->bdaddr), &ev->bdaddr); + goto unlock; + } + + /* If no side requires MITM protection; auto-accept */ + if ((!loc_mitm || conn->remote_cap == HCI_IO_NO_INPUT_OUTPUT) && + (!rem_mitm || conn->io_capability == HCI_IO_NO_INPUT_OUTPUT)) { + + /* If we're not the initiators request authorization to + * proceed from user space (mgmt_user_confirm with + * confirm_hint set to 1). The exception is if neither + * side had MITM or if the local IO capability is + * NoInputNoOutput, in which case we do auto-accept + */ + if (!test_bit(HCI_CONN_AUTH_PEND, &conn->flags) && + conn->io_capability != HCI_IO_NO_INPUT_OUTPUT && + (loc_mitm || rem_mitm)) { + BT_DBG("Confirming auto-accept as acceptor"); + confirm_hint = 1; + goto confirm; + } + + BT_DBG("Auto-accept of user confirmation with %ums delay", + hdev->auto_accept_delay); + + if (hdev->auto_accept_delay > 0) { + int delay = msecs_to_jiffies(hdev->auto_accept_delay); + queue_delayed_work(conn->hdev->workqueue, + &conn->auto_accept_work, delay); + goto unlock; + } + + hci_send_cmd(hdev, HCI_OP_USER_CONFIRM_REPLY, + sizeof(ev->bdaddr), &ev->bdaddr); + goto unlock; + } + +confirm: + mgmt_user_confirm_request(hdev, &ev->bdaddr, ACL_LINK, 0, + le32_to_cpu(ev->passkey), confirm_hint); + +unlock: + hci_dev_unlock(hdev); +} + +static void hci_user_passkey_request_evt(struct hci_dev *hdev, + struct sk_buff *skb) +{ + struct hci_ev_user_passkey_req *ev = (void *) skb->data; + + BT_DBG("%s", hdev->name); + + if (hci_dev_test_flag(hdev, HCI_MGMT)) + mgmt_user_passkey_request(hdev, &ev->bdaddr, ACL_LINK, 0); +} + +static void hci_user_passkey_notify_evt(struct hci_dev *hdev, + struct sk_buff *skb) +{ + struct hci_ev_user_passkey_notify *ev = (void *) skb->data; + struct hci_conn *conn; + + BT_DBG("%s", hdev->name); + + conn = hci_conn_hash_lookup_ba(hdev, ACL_LINK, &ev->bdaddr); + if (!conn) + return; + + conn->passkey_notify = __le32_to_cpu(ev->passkey); + conn->passkey_entered = 0; + + if (hci_dev_test_flag(hdev, HCI_MGMT)) + mgmt_user_passkey_notify(hdev, &conn->dst, conn->type, + conn->dst_type, conn->passkey_notify, + conn->passkey_entered); +} + +static void hci_keypress_notify_evt(struct hci_dev *hdev, struct sk_buff *skb) +{ + struct hci_ev_keypress_notify *ev = (void *) skb->data; + struct hci_conn *conn; + + BT_DBG("%s", hdev->name); + + conn = hci_conn_hash_lookup_ba(hdev, ACL_LINK, &ev->bdaddr); + if (!conn) + return; + + switch (ev->type) { + case HCI_KEYPRESS_STARTED: + conn->passkey_entered = 0; + return; + + case HCI_KEYPRESS_ENTERED: + conn->passkey_entered++; + break; + + case HCI_KEYPRESS_ERASED: + conn->passkey_entered--; + break; + + case HCI_KEYPRESS_CLEARED: + conn->passkey_entered = 0; + break; + + case HCI_KEYPRESS_COMPLETED: + return; + } + + if (hci_dev_test_flag(hdev, HCI_MGMT)) + mgmt_user_passkey_notify(hdev, &conn->dst, conn->type, + conn->dst_type, conn->passkey_notify, + conn->passkey_entered); +} + +static void hci_simple_pair_complete_evt(struct hci_dev *hdev, + struct sk_buff *skb) +{ + struct hci_ev_simple_pair_complete *ev = (void *) skb->data; + struct hci_conn *conn; + + BT_DBG("%s", hdev->name); + + hci_dev_lock(hdev); + + conn = hci_conn_hash_lookup_ba(hdev, ACL_LINK, &ev->bdaddr); + if (!conn) + goto unlock; + + /* Reset the authentication requirement to unknown */ + conn->remote_auth = 0xff; + + /* To avoid duplicate auth_failed events to user space we check + * the HCI_CONN_AUTH_PEND flag which will be set if we + * initiated the authentication. A traditional auth_complete + * event gets always produced as initiator and is also mapped to + * the mgmt_auth_failed event */ + if (!test_bit(HCI_CONN_AUTH_PEND, &conn->flags) && ev->status) + mgmt_auth_failed(conn, ev->status); + + hci_conn_drop(conn); + +unlock: + hci_dev_unlock(hdev); +} + +static void hci_remote_host_features_evt(struct hci_dev *hdev, + struct sk_buff *skb) +{ + struct hci_ev_remote_host_features *ev = (void *) skb->data; + struct inquiry_entry *ie; + struct hci_conn *conn; + + BT_DBG("%s", hdev->name); + + hci_dev_lock(hdev); + + conn = hci_conn_hash_lookup_ba(hdev, ACL_LINK, &ev->bdaddr); + if (conn) + memcpy(conn->features[1], ev->features, 8); + + ie = hci_inquiry_cache_lookup(hdev, &ev->bdaddr); + if (ie) + ie->data.ssp_mode = (ev->features[0] & LMP_HOST_SSP); + + hci_dev_unlock(hdev); +} + +static void hci_remote_oob_data_request_evt(struct hci_dev *hdev, + struct sk_buff *skb) +{ + struct hci_ev_remote_oob_data_request *ev = (void *) skb->data; + struct oob_data *data; + + BT_DBG("%s", hdev->name); + + hci_dev_lock(hdev); + + if (!hci_dev_test_flag(hdev, HCI_MGMT)) + goto unlock; + + data = hci_find_remote_oob_data(hdev, &ev->bdaddr, BDADDR_BREDR); + if (!data) { + struct hci_cp_remote_oob_data_neg_reply cp; + + bacpy(&cp.bdaddr, &ev->bdaddr); + hci_send_cmd(hdev, HCI_OP_REMOTE_OOB_DATA_NEG_REPLY, + sizeof(cp), &cp); + goto unlock; + } + + if (bredr_sc_enabled(hdev)) { + struct hci_cp_remote_oob_ext_data_reply cp; + + bacpy(&cp.bdaddr, &ev->bdaddr); + if (hci_dev_test_flag(hdev, HCI_SC_ONLY)) { + memset(cp.hash192, 0, sizeof(cp.hash192)); + memset(cp.rand192, 0, sizeof(cp.rand192)); + } else { + memcpy(cp.hash192, data->hash192, sizeof(cp.hash192)); + memcpy(cp.rand192, data->rand192, sizeof(cp.rand192)); + } + memcpy(cp.hash256, data->hash256, sizeof(cp.hash256)); + memcpy(cp.rand256, data->rand256, sizeof(cp.rand256)); + + hci_send_cmd(hdev, HCI_OP_REMOTE_OOB_EXT_DATA_REPLY, + sizeof(cp), &cp); + } else { + struct hci_cp_remote_oob_data_reply cp; + + bacpy(&cp.bdaddr, &ev->bdaddr); + memcpy(cp.hash, data->hash192, sizeof(cp.hash)); + memcpy(cp.rand, data->rand192, sizeof(cp.rand)); + + hci_send_cmd(hdev, HCI_OP_REMOTE_OOB_DATA_REPLY, + sizeof(cp), &cp); + } + +unlock: + hci_dev_unlock(hdev); +} + +static void hci_phy_link_complete_evt(struct hci_dev *hdev, + struct sk_buff *skb) +{ + struct hci_ev_phy_link_complete *ev = (void *) skb->data; + struct hci_conn *hcon, *bredr_hcon; + + BT_DBG("%s handle 0x%2.2x status 0x%2.2x", hdev->name, ev->phy_handle, + ev->status); + + hci_dev_lock(hdev); + + hcon = hci_conn_hash_lookup_handle(hdev, ev->phy_handle); + if (!hcon) { + hci_dev_unlock(hdev); + return; + } + + if (ev->status) { + hci_conn_del(hcon); + hci_dev_unlock(hdev); + return; + } + + bredr_hcon = hcon->amp_mgr->l2cap_conn->hcon; + + hcon->state = BT_CONNECTED; + bacpy(&hcon->dst, &bredr_hcon->dst); + + hci_conn_hold(hcon); + hcon->disc_timeout = HCI_DISCONN_TIMEOUT; + hci_conn_drop(hcon); + + hci_debugfs_create_conn(hcon); + hci_conn_add_sysfs(hcon); + + amp_physical_cfm(bredr_hcon, hcon); + + hci_dev_unlock(hdev); +} + +static void hci_loglink_complete_evt(struct hci_dev *hdev, struct sk_buff *skb) +{ + struct hci_ev_logical_link_complete *ev = (void *) skb->data; + struct hci_conn *hcon; + struct hci_chan *hchan; + struct amp_mgr *mgr; + + BT_DBG("%s log_handle 0x%4.4x phy_handle 0x%2.2x status 0x%2.2x", + hdev->name, le16_to_cpu(ev->handle), ev->phy_handle, + ev->status); + + hcon = hci_conn_hash_lookup_handle(hdev, ev->phy_handle); + if (!hcon) + return; + + /* Create AMP hchan */ + hchan = hci_chan_create(hcon); + if (!hchan) + return; + + hchan->handle = le16_to_cpu(ev->handle); + + BT_DBG("hcon %p mgr %p hchan %p", hcon, hcon->amp_mgr, hchan); + + mgr = hcon->amp_mgr; + if (mgr && mgr->bredr_chan) { + struct l2cap_chan *bredr_chan = mgr->bredr_chan; + + l2cap_chan_lock(bredr_chan); + + bredr_chan->conn->mtu = hdev->block_mtu; + l2cap_logical_cfm(bredr_chan, hchan, 0); + hci_conn_hold(hcon); + + l2cap_chan_unlock(bredr_chan); + } +} + +static void hci_disconn_loglink_complete_evt(struct hci_dev *hdev, + struct sk_buff *skb) +{ + struct hci_ev_disconn_logical_link_complete *ev = (void *) skb->data; + struct hci_chan *hchan; + + BT_DBG("%s log handle 0x%4.4x status 0x%2.2x", hdev->name, + le16_to_cpu(ev->handle), ev->status); + + if (ev->status) + return; + + hci_dev_lock(hdev); + + hchan = hci_chan_lookup_handle(hdev, le16_to_cpu(ev->handle)); + if (!hchan) + goto unlock; + + amp_destroy_logical_link(hchan, ev->reason); + +unlock: + hci_dev_unlock(hdev); +} + +static void hci_disconn_phylink_complete_evt(struct hci_dev *hdev, + struct sk_buff *skb) +{ + struct hci_ev_disconn_phy_link_complete *ev = (void *) skb->data; + struct hci_conn *hcon; + + BT_DBG("%s status 0x%2.2x", hdev->name, ev->status); + + if (ev->status) + return; + + hci_dev_lock(hdev); + + hcon = hci_conn_hash_lookup_handle(hdev, ev->phy_handle); + if (hcon) { + hcon->state = BT_CLOSED; + hci_conn_del(hcon); + } + + hci_dev_unlock(hdev); +} + +static void hci_le_conn_complete_evt(struct hci_dev *hdev, struct sk_buff *skb) +{ + struct hci_ev_le_conn_complete *ev = (void *) skb->data; + struct hci_conn_params *params; + struct hci_conn *conn; + struct smp_irk *irk; + u8 addr_type; + + BT_DBG("%s status 0x%2.2x", hdev->name, ev->status); + + hci_dev_lock(hdev); + + /* All controllers implicitly stop advertising in the event of a + * connection, so ensure that the state bit is cleared. + */ + hci_dev_clear_flag(hdev, HCI_LE_ADV); + + conn = hci_conn_hash_lookup_state(hdev, LE_LINK, BT_CONNECT); + if (!conn) { + conn = hci_conn_add(hdev, LE_LINK, &ev->bdaddr, ev->role); + if (!conn) { + BT_ERR("No memory for new connection"); + goto unlock; + } + + conn->dst_type = ev->bdaddr_type; + + /* If we didn't have a hci_conn object previously + * but we're in master role this must be something + * initiated using a white list. Since white list based + * connections are not "first class citizens" we don't + * have full tracking of them. Therefore, we go ahead + * with a "best effort" approach of determining the + * initiator address based on the HCI_PRIVACY flag. + */ + if (conn->out) { + conn->resp_addr_type = ev->bdaddr_type; + bacpy(&conn->resp_addr, &ev->bdaddr); + if (hci_dev_test_flag(hdev, HCI_PRIVACY)) { + conn->init_addr_type = ADDR_LE_DEV_RANDOM; + bacpy(&conn->init_addr, &hdev->rpa); + } else { + hci_copy_identity_address(hdev, + &conn->init_addr, + &conn->init_addr_type); + } + } + } else { + cancel_delayed_work(&conn->le_conn_timeout); + } + + if (!conn->out) { + /* Set the responder (our side) address type based on + * the advertising address type. + */ + conn->resp_addr_type = hdev->adv_addr_type; + if (hdev->adv_addr_type == ADDR_LE_DEV_RANDOM) + bacpy(&conn->resp_addr, &hdev->random_addr); + else + bacpy(&conn->resp_addr, &hdev->bdaddr); + + conn->init_addr_type = ev->bdaddr_type; + bacpy(&conn->init_addr, &ev->bdaddr); + + /* For incoming connections, set the default minimum + * and maximum connection interval. They will be used + * to check if the parameters are in range and if not + * trigger the connection update procedure. + */ + conn->le_conn_min_interval = hdev->le_conn_min_interval; + conn->le_conn_max_interval = hdev->le_conn_max_interval; + } + + /* Lookup the identity address from the stored connection + * address and address type. + * + * When establishing connections to an identity address, the + * connection procedure will store the resolvable random + * address first. Now if it can be converted back into the + * identity address, start using the identity address from + * now on. + */ + irk = hci_get_irk(hdev, &conn->dst, conn->dst_type); + if (irk) { + bacpy(&conn->dst, &irk->bdaddr); + conn->dst_type = irk->addr_type; + } + + if (ev->status) { + hci_le_conn_failed(conn, ev->status); + goto unlock; + } + + if (conn->dst_type == ADDR_LE_DEV_PUBLIC) + addr_type = BDADDR_LE_PUBLIC; + else + addr_type = BDADDR_LE_RANDOM; + + /* Drop the connection if the device is blocked */ + if (hci_bdaddr_list_lookup(&hdev->blacklist, &conn->dst, addr_type)) { + hci_conn_drop(conn); + goto unlock; + } + + if (!test_and_set_bit(HCI_CONN_MGMT_CONNECTED, &conn->flags)) + mgmt_device_connected(hdev, conn, 0, NULL, 0); + + conn->sec_level = BT_SECURITY_LOW; + conn->handle = __le16_to_cpu(ev->handle); + conn->state = BT_CONFIG; + + conn->le_conn_interval = le16_to_cpu(ev->interval); + conn->le_conn_latency = le16_to_cpu(ev->latency); + conn->le_supv_timeout = le16_to_cpu(ev->supervision_timeout); + + hci_debugfs_create_conn(conn); + hci_conn_add_sysfs(conn); + + if (!ev->status) { + /* The remote features procedure is defined for master + * role only. So only in case of an initiated connection + * request the remote features. + * + * If the local controller supports slave-initiated features + * exchange, then requesting the remote features in slave + * role is possible. Otherwise just transition into the + * connected state without requesting the remote features. + */ + if (conn->out || + (hdev->le_features[0] & HCI_LE_SLAVE_FEATURES)) { + struct hci_cp_le_read_remote_features cp; + + cp.handle = __cpu_to_le16(conn->handle); + + hci_send_cmd(hdev, HCI_OP_LE_READ_REMOTE_FEATURES, + sizeof(cp), &cp); + + hci_conn_hold(conn); + } else { + conn->state = BT_CONNECTED; + hci_connect_cfm(conn, ev->status); + } + } else { + hci_connect_cfm(conn, ev->status); + } + + params = hci_pend_le_action_lookup(&hdev->pend_le_conns, &conn->dst, + conn->dst_type); + if (params) { + list_del_init(¶ms->action); + if (params->conn) { + hci_conn_drop(params->conn); + hci_conn_put(params->conn); + params->conn = NULL; + } + } + +unlock: + hci_update_background_scan(hdev); + hci_dev_unlock(hdev); +} + +static void hci_le_conn_update_complete_evt(struct hci_dev *hdev, + struct sk_buff *skb) +{ + struct hci_ev_le_conn_update_complete *ev = (void *) skb->data; + struct hci_conn *conn; + + BT_DBG("%s status 0x%2.2x", hdev->name, ev->status); + + if (ev->status) + return; + + hci_dev_lock(hdev); + + conn = hci_conn_hash_lookup_handle(hdev, __le16_to_cpu(ev->handle)); + if (conn) { + conn->le_conn_interval = le16_to_cpu(ev->interval); + conn->le_conn_latency = le16_to_cpu(ev->latency); + conn->le_supv_timeout = le16_to_cpu(ev->supervision_timeout); + } + + hci_dev_unlock(hdev); +} + +/* This function requires the caller holds hdev->lock */ +static struct hci_conn *check_pending_le_conn(struct hci_dev *hdev, + bdaddr_t *addr, + u8 addr_type, u8 adv_type) +{ + struct hci_conn *conn; + struct hci_conn_params *params; + + /* If the event is not connectable don't proceed further */ + if (adv_type != LE_ADV_IND && adv_type != LE_ADV_DIRECT_IND) + return NULL; + + /* Ignore if the device is blocked */ + if (hci_bdaddr_list_lookup(&hdev->blacklist, addr, addr_type)) + return NULL; + + /* Most controller will fail if we try to create new connections + * while we have an existing one in slave role. + */ + if (hdev->conn_hash.le_num_slave > 0) + return NULL; + + /* If we're not connectable only connect devices that we have in + * our pend_le_conns list. + */ + params = hci_pend_le_action_lookup(&hdev->pend_le_conns, + addr, addr_type); + if (!params) + return NULL; + + switch (params->auto_connect) { + case HCI_AUTO_CONN_DIRECT: + /* Only devices advertising with ADV_DIRECT_IND are + * triggering a connection attempt. This is allowing + * incoming connections from slave devices. + */ + if (adv_type != LE_ADV_DIRECT_IND) + return NULL; + break; + case HCI_AUTO_CONN_ALWAYS: + /* Devices advertising with ADV_IND or ADV_DIRECT_IND + * are triggering a connection attempt. This means + * that incoming connectioms from slave device are + * accepted and also outgoing connections to slave + * devices are established when found. + */ + break; + default: + return NULL; + } + + conn = hci_connect_le(hdev, addr, addr_type, BT_SECURITY_LOW, + HCI_LE_AUTOCONN_TIMEOUT, HCI_ROLE_MASTER); + if (!IS_ERR(conn)) { + /* Store the pointer since we don't really have any + * other owner of the object besides the params that + * triggered it. This way we can abort the connection if + * the parameters get removed and keep the reference + * count consistent once the connection is established. + */ + params->conn = hci_conn_get(conn); + return conn; + } + + switch (PTR_ERR(conn)) { + case -EBUSY: + /* If hci_connect() returns -EBUSY it means there is already + * an LE connection attempt going on. Since controllers don't + * support more than one connection attempt at the time, we + * don't consider this an error case. + */ + break; + default: + BT_DBG("Failed to connect: err %ld", PTR_ERR(conn)); + return NULL; + } + + return NULL; +} + +static void process_adv_report(struct hci_dev *hdev, u8 type, bdaddr_t *bdaddr, + u8 bdaddr_type, bdaddr_t *direct_addr, + u8 direct_addr_type, s8 rssi, u8 *data, u8 len) +{ + struct discovery_state *d = &hdev->discovery; + struct smp_irk *irk; + struct hci_conn *conn; + bool match; + u32 flags; + + /* If the direct address is present, then this report is from + * a LE Direct Advertising Report event. In that case it is + * important to see if the address is matching the local + * controller address. + */ + if (direct_addr) { + /* Only resolvable random addresses are valid for these + * kind of reports and others can be ignored. + */ + if (!hci_bdaddr_is_rpa(direct_addr, direct_addr_type)) + return; + + /* If the controller is not using resolvable random + * addresses, then this report can be ignored. + */ + if (!hci_dev_test_flag(hdev, HCI_PRIVACY)) + return; + + /* If the local IRK of the controller does not match + * with the resolvable random address provided, then + * this report can be ignored. + */ + if (!smp_irk_matches(hdev, hdev->irk, direct_addr)) + return; + } + + /* Check if we need to convert to identity address */ + irk = hci_get_irk(hdev, bdaddr, bdaddr_type); + if (irk) { + bdaddr = &irk->bdaddr; + bdaddr_type = irk->addr_type; + } + + /* Check if we have been requested to connect to this device */ + conn = check_pending_le_conn(hdev, bdaddr, bdaddr_type, type); + if (conn && type == LE_ADV_IND) { + /* Store report for later inclusion by + * mgmt_device_connected + */ + memcpy(conn->le_adv_data, data, len); + conn->le_adv_data_len = len; + } + + /* Passive scanning shouldn't trigger any device found events, + * except for devices marked as CONN_REPORT for which we do send + * device found events. + */ + if (hdev->le_scan_type == LE_SCAN_PASSIVE) { + if (type == LE_ADV_DIRECT_IND) + return; + + if (!hci_pend_le_action_lookup(&hdev->pend_le_reports, + bdaddr, bdaddr_type)) + return; + + if (type == LE_ADV_NONCONN_IND || type == LE_ADV_SCAN_IND) + flags = MGMT_DEV_FOUND_NOT_CONNECTABLE; + else + flags = 0; + mgmt_device_found(hdev, bdaddr, LE_LINK, bdaddr_type, NULL, + rssi, flags, data, len, NULL, 0); + return; + } + + /* When receiving non-connectable or scannable undirected + * advertising reports, this means that the remote device is + * not connectable and then clearly indicate this in the + * device found event. + * + * When receiving a scan response, then there is no way to + * know if the remote device is connectable or not. However + * since scan responses are merged with a previously seen + * advertising report, the flags field from that report + * will be used. + * + * In the really unlikely case that a controller get confused + * and just sends a scan response event, then it is marked as + * not connectable as well. + */ + if (type == LE_ADV_NONCONN_IND || type == LE_ADV_SCAN_IND || + type == LE_ADV_SCAN_RSP) + flags = MGMT_DEV_FOUND_NOT_CONNECTABLE; + else + flags = 0; + + /* If there's nothing pending either store the data from this + * event or send an immediate device found event if the data + * should not be stored for later. + */ + if (!has_pending_adv_report(hdev)) { + /* If the report will trigger a SCAN_REQ store it for + * later merging. + */ + if (type == LE_ADV_IND || type == LE_ADV_SCAN_IND) { + store_pending_adv_report(hdev, bdaddr, bdaddr_type, + rssi, flags, data, len); + return; + } + + mgmt_device_found(hdev, bdaddr, LE_LINK, bdaddr_type, NULL, + rssi, flags, data, len, NULL, 0); + return; + } + + /* Check if the pending report is for the same device as the new one */ + match = (!bacmp(bdaddr, &d->last_adv_addr) && + bdaddr_type == d->last_adv_addr_type); + + /* If the pending data doesn't match this report or this isn't a + * scan response (e.g. we got a duplicate ADV_IND) then force + * sending of the pending data. + */ + if (type != LE_ADV_SCAN_RSP || !match) { + /* Send out whatever is in the cache, but skip duplicates */ + if (!match) + mgmt_device_found(hdev, &d->last_adv_addr, LE_LINK, + d->last_adv_addr_type, NULL, + d->last_adv_rssi, d->last_adv_flags, + d->last_adv_data, + d->last_adv_data_len, NULL, 0); + + /* If the new report will trigger a SCAN_REQ store it for + * later merging. + */ + if (type == LE_ADV_IND || type == LE_ADV_SCAN_IND) { + store_pending_adv_report(hdev, bdaddr, bdaddr_type, + rssi, flags, data, len); + return; + } + + /* The advertising reports cannot be merged, so clear + * the pending report and send out a device found event. + */ + clear_pending_adv_report(hdev); + mgmt_device_found(hdev, bdaddr, LE_LINK, bdaddr_type, NULL, + rssi, flags, data, len, NULL, 0); + return; + } + + /* If we get here we've got a pending ADV_IND or ADV_SCAN_IND and + * the new event is a SCAN_RSP. We can therefore proceed with + * sending a merged device found event. + */ + mgmt_device_found(hdev, &d->last_adv_addr, LE_LINK, + d->last_adv_addr_type, NULL, rssi, d->last_adv_flags, + d->last_adv_data, d->last_adv_data_len, data, len); + clear_pending_adv_report(hdev); +} + +static void hci_le_adv_report_evt(struct hci_dev *hdev, struct sk_buff *skb) +{ + u8 num_reports = skb->data[0]; + void *ptr = &skb->data[1]; + + hci_dev_lock(hdev); + + while (num_reports--) { + struct hci_ev_le_advertising_info *ev = ptr; + s8 rssi; + + rssi = ev->data[ev->length]; + process_adv_report(hdev, ev->evt_type, &ev->bdaddr, + ev->bdaddr_type, NULL, 0, rssi, + ev->data, ev->length); + + ptr += sizeof(*ev) + ev->length + 1; + } + + hci_dev_unlock(hdev); +} + +static void hci_le_remote_feat_complete_evt(struct hci_dev *hdev, + struct sk_buff *skb) +{ + struct hci_ev_le_remote_feat_complete *ev = (void *)skb->data; + struct hci_conn *conn; + + BT_DBG("%s status 0x%2.2x", hdev->name, ev->status); + + hci_dev_lock(hdev); + + conn = hci_conn_hash_lookup_handle(hdev, __le16_to_cpu(ev->handle)); + if (conn) { + if (!ev->status) + memcpy(conn->features[0], ev->features, 8); + + if (conn->state == BT_CONFIG) { + __u8 status; + + /* If the local controller supports slave-initiated + * features exchange, but the remote controller does + * not, then it is possible that the error code 0x1a + * for unsupported remote feature gets returned. + * + * In this specific case, allow the connection to + * transition into connected state and mark it as + * successful. + */ + if ((hdev->le_features[0] & HCI_LE_SLAVE_FEATURES) && + !conn->out && ev->status == 0x1a) + status = 0x00; + else + status = ev->status; + + conn->state = BT_CONNECTED; + hci_connect_cfm(conn, status); + hci_conn_drop(conn); + } + } + + hci_dev_unlock(hdev); +} + +static void hci_le_ltk_request_evt(struct hci_dev *hdev, struct sk_buff *skb) +{ + struct hci_ev_le_ltk_req *ev = (void *) skb->data; + struct hci_cp_le_ltk_reply cp; + struct hci_cp_le_ltk_neg_reply neg; + struct hci_conn *conn; + struct smp_ltk *ltk; + + BT_DBG("%s handle 0x%4.4x", hdev->name, __le16_to_cpu(ev->handle)); + + hci_dev_lock(hdev); + + conn = hci_conn_hash_lookup_handle(hdev, __le16_to_cpu(ev->handle)); + if (conn == NULL) + goto not_found; + + ltk = hci_find_ltk(hdev, &conn->dst, conn->dst_type, conn->role); + if (!ltk) + goto not_found; + + if (smp_ltk_is_sc(ltk)) { + /* With SC both EDiv and Rand are set to zero */ + if (ev->ediv || ev->rand) + goto not_found; + } else { + /* For non-SC keys check that EDiv and Rand match */ + if (ev->ediv != ltk->ediv || ev->rand != ltk->rand) + goto not_found; + } + + memcpy(cp.ltk, ltk->val, sizeof(ltk->val)); + cp.handle = cpu_to_le16(conn->handle); + + conn->pending_sec_level = smp_ltk_sec_level(ltk); + + conn->enc_key_size = ltk->enc_size; + + hci_send_cmd(hdev, HCI_OP_LE_LTK_REPLY, sizeof(cp), &cp); + + /* Ref. Bluetooth Core SPEC pages 1975 and 2004. STK is a + * temporary key used to encrypt a connection following + * pairing. It is used during the Encrypted Session Setup to + * distribute the keys. Later, security can be re-established + * using a distributed LTK. + */ + if (ltk->type == SMP_STK) { + set_bit(HCI_CONN_STK_ENCRYPT, &conn->flags); + list_del_rcu(<k->list); + kfree_rcu(ltk, rcu); + } else { + clear_bit(HCI_CONN_STK_ENCRYPT, &conn->flags); + } + + hci_dev_unlock(hdev); + + return; + +not_found: + neg.handle = ev->handle; + hci_send_cmd(hdev, HCI_OP_LE_LTK_NEG_REPLY, sizeof(neg), &neg); + hci_dev_unlock(hdev); +} + +static void send_conn_param_neg_reply(struct hci_dev *hdev, u16 handle, + u8 reason) +{ + struct hci_cp_le_conn_param_req_neg_reply cp; + + cp.handle = cpu_to_le16(handle); + cp.reason = reason; + + hci_send_cmd(hdev, HCI_OP_LE_CONN_PARAM_REQ_NEG_REPLY, sizeof(cp), + &cp); +} + +static void hci_le_remote_conn_param_req_evt(struct hci_dev *hdev, + struct sk_buff *skb) +{ + struct hci_ev_le_remote_conn_param_req *ev = (void *) skb->data; + struct hci_cp_le_conn_param_req_reply cp; + struct hci_conn *hcon; + u16 handle, min, max, latency, timeout; + + handle = le16_to_cpu(ev->handle); + min = le16_to_cpu(ev->interval_min); + max = le16_to_cpu(ev->interval_max); + latency = le16_to_cpu(ev->latency); + timeout = le16_to_cpu(ev->timeout); + + hcon = hci_conn_hash_lookup_handle(hdev, handle); + if (!hcon || hcon->state != BT_CONNECTED) + return send_conn_param_neg_reply(hdev, handle, + HCI_ERROR_UNKNOWN_CONN_ID); + + if (hci_check_conn_params(min, max, latency, timeout)) + return send_conn_param_neg_reply(hdev, handle, + HCI_ERROR_INVALID_LL_PARAMS); + + if (hcon->role == HCI_ROLE_MASTER) { + struct hci_conn_params *params; + u8 store_hint; + + hci_dev_lock(hdev); + + params = hci_conn_params_lookup(hdev, &hcon->dst, + hcon->dst_type); + if (params) { + params->conn_min_interval = min; + params->conn_max_interval = max; + params->conn_latency = latency; + params->supervision_timeout = timeout; + store_hint = 0x01; + } else{ + store_hint = 0x00; + } + + hci_dev_unlock(hdev); + + mgmt_new_conn_param(hdev, &hcon->dst, hcon->dst_type, + store_hint, min, max, latency, timeout); + } + + cp.handle = ev->handle; + cp.interval_min = ev->interval_min; + cp.interval_max = ev->interval_max; + cp.latency = ev->latency; + cp.timeout = ev->timeout; + cp.min_ce_len = 0; + cp.max_ce_len = 0; + + hci_send_cmd(hdev, HCI_OP_LE_CONN_PARAM_REQ_REPLY, sizeof(cp), &cp); +} + +static void hci_le_direct_adv_report_evt(struct hci_dev *hdev, + struct sk_buff *skb) +{ + u8 num_reports = skb->data[0]; + void *ptr = &skb->data[1]; + + hci_dev_lock(hdev); + + while (num_reports--) { + struct hci_ev_le_direct_adv_info *ev = ptr; + + process_adv_report(hdev, ev->evt_type, &ev->bdaddr, + ev->bdaddr_type, &ev->direct_addr, + ev->direct_addr_type, ev->rssi, NULL, 0); + + ptr += sizeof(*ev); + } + + hci_dev_unlock(hdev); +} + +static void hci_le_meta_evt(struct hci_dev *hdev, struct sk_buff *skb) +{ + struct hci_ev_le_meta *le_ev = (void *) skb->data; + + skb_pull(skb, sizeof(*le_ev)); + + switch (le_ev->subevent) { + case HCI_EV_LE_CONN_COMPLETE: + hci_le_conn_complete_evt(hdev, skb); + break; + + case HCI_EV_LE_CONN_UPDATE_COMPLETE: + hci_le_conn_update_complete_evt(hdev, skb); + break; + + case HCI_EV_LE_ADVERTISING_REPORT: + hci_le_adv_report_evt(hdev, skb); + break; + + case HCI_EV_LE_REMOTE_FEAT_COMPLETE: + hci_le_remote_feat_complete_evt(hdev, skb); + break; + + case HCI_EV_LE_LTK_REQ: + hci_le_ltk_request_evt(hdev, skb); + break; + + case HCI_EV_LE_REMOTE_CONN_PARAM_REQ: + hci_le_remote_conn_param_req_evt(hdev, skb); + break; + + case HCI_EV_LE_DIRECT_ADV_REPORT: + hci_le_direct_adv_report_evt(hdev, skb); + break; + + default: + break; + } +} + +static void hci_chan_selected_evt(struct hci_dev *hdev, struct sk_buff *skb) +{ + struct hci_ev_channel_selected *ev = (void *) skb->data; + struct hci_conn *hcon; + + BT_DBG("%s handle 0x%2.2x", hdev->name, ev->phy_handle); + + skb_pull(skb, sizeof(*ev)); + + hcon = hci_conn_hash_lookup_handle(hdev, ev->phy_handle); + if (!hcon) + return; + + amp_read_loc_assoc_final_data(hdev, hcon); +} + +static bool hci_get_cmd_complete(struct hci_dev *hdev, u16 opcode, + u8 event, struct sk_buff *skb) +{ + struct hci_ev_cmd_complete *ev; + struct hci_event_hdr *hdr; + + if (!skb) + return false; + + if (skb->len < sizeof(*hdr)) { + BT_ERR("Too short HCI event"); + return false; + } + + hdr = (void *) skb->data; + skb_pull(skb, HCI_EVENT_HDR_SIZE); + + if (event) { + if (hdr->evt != event) + return false; + return true; + } + + if (hdr->evt != HCI_EV_CMD_COMPLETE) { + BT_DBG("Last event is not cmd complete (0x%2.2x)", hdr->evt); + return false; + } + + if (skb->len < sizeof(*ev)) { + BT_ERR("Too short cmd_complete event"); + return false; + } + + ev = (void *) skb->data; + skb_pull(skb, sizeof(*ev)); + + if (opcode != __le16_to_cpu(ev->opcode)) { + BT_DBG("opcode doesn't match (0x%2.2x != 0x%2.2x)", opcode, + __le16_to_cpu(ev->opcode)); + return false; + } + + return true; +} + +void hci_event_packet(struct hci_dev *hdev, struct sk_buff *skb) +{ + struct hci_event_hdr *hdr = (void *) skb->data; + hci_req_complete_t req_complete = NULL; + hci_req_complete_skb_t req_complete_skb = NULL; + struct sk_buff *orig_skb = NULL; + u8 status = 0, event = hdr->evt, req_evt = 0; + u16 opcode = HCI_OP_NOP; + + if (hdev->sent_cmd && bt_cb(hdev->sent_cmd)->req.event == event) { + struct hci_command_hdr *cmd_hdr = (void *) hdev->sent_cmd->data; + opcode = __le16_to_cpu(cmd_hdr->opcode); + hci_req_cmd_complete(hdev, opcode, status, &req_complete, + &req_complete_skb); + req_evt = event; + } + + /* If it looks like we might end up having to call + * req_complete_skb, store a pristine copy of the skb since the + * various handlers may modify the original one through + * skb_pull() calls, etc. + */ + if (req_complete_skb || event == HCI_EV_CMD_STATUS || + event == HCI_EV_CMD_COMPLETE) + orig_skb = skb_clone(skb, GFP_KERNEL); + + skb_pull(skb, HCI_EVENT_HDR_SIZE); + + switch (event) { + case HCI_EV_INQUIRY_COMPLETE: + hci_inquiry_complete_evt(hdev, skb); + break; + + case HCI_EV_INQUIRY_RESULT: + hci_inquiry_result_evt(hdev, skb); + break; + + case HCI_EV_CONN_COMPLETE: + hci_conn_complete_evt(hdev, skb); + break; + + case HCI_EV_CONN_REQUEST: + hci_conn_request_evt(hdev, skb); + break; + + case HCI_EV_DISCONN_COMPLETE: + hci_disconn_complete_evt(hdev, skb); + break; + + case HCI_EV_AUTH_COMPLETE: + hci_auth_complete_evt(hdev, skb); + break; + + case HCI_EV_REMOTE_NAME: + hci_remote_name_evt(hdev, skb); + break; + + case HCI_EV_ENCRYPT_CHANGE: + hci_encrypt_change_evt(hdev, skb); + break; + + case HCI_EV_CHANGE_LINK_KEY_COMPLETE: + hci_change_link_key_complete_evt(hdev, skb); + break; + + case HCI_EV_REMOTE_FEATURES: + hci_remote_features_evt(hdev, skb); + break; + + case HCI_EV_CMD_COMPLETE: + hci_cmd_complete_evt(hdev, skb, &opcode, &status, + &req_complete, &req_complete_skb); + break; + + case HCI_EV_CMD_STATUS: + hci_cmd_status_evt(hdev, skb, &opcode, &status, &req_complete, + &req_complete_skb); + break; + + case HCI_EV_HARDWARE_ERROR: + hci_hardware_error_evt(hdev, skb); + break; + + case HCI_EV_ROLE_CHANGE: + hci_role_change_evt(hdev, skb); + break; + + case HCI_EV_NUM_COMP_PKTS: + hci_num_comp_pkts_evt(hdev, skb); + break; + + case HCI_EV_MODE_CHANGE: + hci_mode_change_evt(hdev, skb); + break; + + case HCI_EV_PIN_CODE_REQ: + hci_pin_code_request_evt(hdev, skb); + break; + + case HCI_EV_LINK_KEY_REQ: + hci_link_key_request_evt(hdev, skb); + break; + + case HCI_EV_LINK_KEY_NOTIFY: + hci_link_key_notify_evt(hdev, skb); + break; + + case HCI_EV_CLOCK_OFFSET: + hci_clock_offset_evt(hdev, skb); + break; + + case HCI_EV_PKT_TYPE_CHANGE: + hci_pkt_type_change_evt(hdev, skb); + break; + + case HCI_EV_PSCAN_REP_MODE: + hci_pscan_rep_mode_evt(hdev, skb); + break; + + case HCI_EV_INQUIRY_RESULT_WITH_RSSI: + hci_inquiry_result_with_rssi_evt(hdev, skb); + break; + + case HCI_EV_REMOTE_EXT_FEATURES: + hci_remote_ext_features_evt(hdev, skb); + break; + + case HCI_EV_SYNC_CONN_COMPLETE: + hci_sync_conn_complete_evt(hdev, skb); + break; + + case HCI_EV_EXTENDED_INQUIRY_RESULT: + hci_extended_inquiry_result_evt(hdev, skb); + break; + + case HCI_EV_KEY_REFRESH_COMPLETE: + hci_key_refresh_complete_evt(hdev, skb); + break; + + case HCI_EV_IO_CAPA_REQUEST: + hci_io_capa_request_evt(hdev, skb); + break; + + case HCI_EV_IO_CAPA_REPLY: + hci_io_capa_reply_evt(hdev, skb); + break; + + case HCI_EV_USER_CONFIRM_REQUEST: + hci_user_confirm_request_evt(hdev, skb); + break; + + case HCI_EV_USER_PASSKEY_REQUEST: + hci_user_passkey_request_evt(hdev, skb); + break; + + case HCI_EV_USER_PASSKEY_NOTIFY: + hci_user_passkey_notify_evt(hdev, skb); + break; + + case HCI_EV_KEYPRESS_NOTIFY: + hci_keypress_notify_evt(hdev, skb); + break; + + case HCI_EV_SIMPLE_PAIR_COMPLETE: + hci_simple_pair_complete_evt(hdev, skb); + break; + + case HCI_EV_REMOTE_HOST_FEATURES: + hci_remote_host_features_evt(hdev, skb); + break; + + case HCI_EV_LE_META: + hci_le_meta_evt(hdev, skb); + break; + + case HCI_EV_CHANNEL_SELECTED: + hci_chan_selected_evt(hdev, skb); + break; + + case HCI_EV_REMOTE_OOB_DATA_REQUEST: + hci_remote_oob_data_request_evt(hdev, skb); + break; + + case HCI_EV_PHY_LINK_COMPLETE: + hci_phy_link_complete_evt(hdev, skb); + break; + + case HCI_EV_LOGICAL_LINK_COMPLETE: + hci_loglink_complete_evt(hdev, skb); + break; + + case HCI_EV_DISCONN_LOGICAL_LINK_COMPLETE: + hci_disconn_loglink_complete_evt(hdev, skb); + break; + + case HCI_EV_DISCONN_PHY_LINK_COMPLETE: + hci_disconn_phylink_complete_evt(hdev, skb); + break; + + case HCI_EV_NUM_COMP_BLOCKS: + hci_num_comp_blocks_evt(hdev, skb); + break; + + default: + BT_DBG("%s event 0x%2.2x", hdev->name, event); + break; + } + + if (req_complete) { + req_complete(hdev, status, opcode); + } else if (req_complete_skb) { + if (!hci_get_cmd_complete(hdev, opcode, req_evt, orig_skb)) { + kfree_skb(orig_skb); + orig_skb = NULL; + } + req_complete_skb(hdev, status, opcode, orig_skb); + } + + kfree_skb(orig_skb); + kfree_skb(skb); + hdev->stat.evt_rx++; +} diff --git a/kernel/net/bluetooth/hci_request.c b/kernel/net/bluetooth/hci_request.c new file mode 100644 index 000000000..d6025d6e6 --- /dev/null +++ b/kernel/net/bluetooth/hci_request.c @@ -0,0 +1,568 @@ +/* + BlueZ - Bluetooth protocol stack for Linux + + Copyright (C) 2014 Intel Corporation + + 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; + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS + OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT OF THIRD PARTY RIGHTS. + IN NO EVENT SHALL THE COPYRIGHT HOLDER(S) AND AUTHOR(S) BE LIABLE FOR ANY + CLAIM, OR ANY SPECIAL INDIRECT OR CONSEQUENTIAL DAMAGES, OR ANY DAMAGES + WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN + ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF + OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + + ALL LIABILITY, INCLUDING LIABILITY FOR INFRINGEMENT OF ANY PATENTS, + COPYRIGHTS, TRADEMARKS OR OTHER RIGHTS, RELATING TO USE OF THIS + SOFTWARE IS DISCLAIMED. +*/ + +#include <net/bluetooth/bluetooth.h> +#include <net/bluetooth/hci_core.h> + +#include "smp.h" +#include "hci_request.h" + +void hci_req_init(struct hci_request *req, struct hci_dev *hdev) +{ + skb_queue_head_init(&req->cmd_q); + req->hdev = hdev; + req->err = 0; +} + +static int req_run(struct hci_request *req, hci_req_complete_t complete, + hci_req_complete_skb_t complete_skb) +{ + struct hci_dev *hdev = req->hdev; + struct sk_buff *skb; + unsigned long flags; + + BT_DBG("length %u", skb_queue_len(&req->cmd_q)); + + /* If an error occurred during request building, remove all HCI + * commands queued on the HCI request queue. + */ + if (req->err) { + skb_queue_purge(&req->cmd_q); + return req->err; + } + + /* Do not allow empty requests */ + if (skb_queue_empty(&req->cmd_q)) + return -ENODATA; + + skb = skb_peek_tail(&req->cmd_q); + bt_cb(skb)->req.complete = complete; + bt_cb(skb)->req.complete_skb = complete_skb; + + spin_lock_irqsave(&hdev->cmd_q.lock, flags); + skb_queue_splice_tail(&req->cmd_q, &hdev->cmd_q); + spin_unlock_irqrestore(&hdev->cmd_q.lock, flags); + + queue_work(hdev->workqueue, &hdev->cmd_work); + + return 0; +} + +int hci_req_run(struct hci_request *req, hci_req_complete_t complete) +{ + return req_run(req, complete, NULL); +} + +int hci_req_run_skb(struct hci_request *req, hci_req_complete_skb_t complete) +{ + return req_run(req, NULL, complete); +} + +struct sk_buff *hci_prepare_cmd(struct hci_dev *hdev, u16 opcode, u32 plen, + const void *param) +{ + int len = HCI_COMMAND_HDR_SIZE + plen; + struct hci_command_hdr *hdr; + struct sk_buff *skb; + + skb = bt_skb_alloc(len, GFP_ATOMIC); + if (!skb) + return NULL; + + hdr = (struct hci_command_hdr *) skb_put(skb, HCI_COMMAND_HDR_SIZE); + hdr->opcode = cpu_to_le16(opcode); + hdr->plen = plen; + + if (plen) + memcpy(skb_put(skb, plen), param, plen); + + BT_DBG("skb len %d", skb->len); + + bt_cb(skb)->pkt_type = HCI_COMMAND_PKT; + bt_cb(skb)->opcode = opcode; + + return skb; +} + +/* Queue a command to an asynchronous HCI request */ +void hci_req_add_ev(struct hci_request *req, u16 opcode, u32 plen, + const void *param, u8 event) +{ + struct hci_dev *hdev = req->hdev; + struct sk_buff *skb; + + BT_DBG("%s opcode 0x%4.4x plen %d", hdev->name, opcode, plen); + + /* If an error occurred during request building, there is no point in + * queueing the HCI command. We can simply return. + */ + if (req->err) + return; + + skb = hci_prepare_cmd(hdev, opcode, plen, param); + if (!skb) { + BT_ERR("%s no memory for command (opcode 0x%4.4x)", + hdev->name, opcode); + req->err = -ENOMEM; + return; + } + + if (skb_queue_empty(&req->cmd_q)) + bt_cb(skb)->req.start = true; + + bt_cb(skb)->req.event = event; + + skb_queue_tail(&req->cmd_q, skb); +} + +void hci_req_add(struct hci_request *req, u16 opcode, u32 plen, + const void *param) +{ + hci_req_add_ev(req, opcode, plen, param, 0); +} + +void hci_req_add_le_scan_disable(struct hci_request *req) +{ + struct hci_cp_le_set_scan_enable cp; + + memset(&cp, 0, sizeof(cp)); + cp.enable = LE_SCAN_DISABLE; + hci_req_add(req, HCI_OP_LE_SET_SCAN_ENABLE, sizeof(cp), &cp); +} + +static void add_to_white_list(struct hci_request *req, + struct hci_conn_params *params) +{ + struct hci_cp_le_add_to_white_list cp; + + cp.bdaddr_type = params->addr_type; + bacpy(&cp.bdaddr, ¶ms->addr); + + hci_req_add(req, HCI_OP_LE_ADD_TO_WHITE_LIST, sizeof(cp), &cp); +} + +static u8 update_white_list(struct hci_request *req) +{ + struct hci_dev *hdev = req->hdev; + struct hci_conn_params *params; + struct bdaddr_list *b; + uint8_t white_list_entries = 0; + + /* Go through the current white list programmed into the + * controller one by one and check if that address is still + * in the list of pending connections or list of devices to + * report. If not present in either list, then queue the + * command to remove it from the controller. + */ + list_for_each_entry(b, &hdev->le_white_list, list) { + struct hci_cp_le_del_from_white_list cp; + + if (hci_pend_le_action_lookup(&hdev->pend_le_conns, + &b->bdaddr, b->bdaddr_type) || + hci_pend_le_action_lookup(&hdev->pend_le_reports, + &b->bdaddr, b->bdaddr_type)) { + white_list_entries++; + continue; + } + + cp.bdaddr_type = b->bdaddr_type; + bacpy(&cp.bdaddr, &b->bdaddr); + + hci_req_add(req, HCI_OP_LE_DEL_FROM_WHITE_LIST, + sizeof(cp), &cp); + } + + /* Since all no longer valid white list entries have been + * removed, walk through the list of pending connections + * and ensure that any new device gets programmed into + * the controller. + * + * If the list of the devices is larger than the list of + * available white list entries in the controller, then + * just abort and return filer policy value to not use the + * white list. + */ + list_for_each_entry(params, &hdev->pend_le_conns, action) { + if (hci_bdaddr_list_lookup(&hdev->le_white_list, + ¶ms->addr, params->addr_type)) + continue; + + if (white_list_entries >= hdev->le_white_list_size) { + /* Select filter policy to accept all advertising */ + return 0x00; + } + + if (hci_find_irk_by_addr(hdev, ¶ms->addr, + params->addr_type)) { + /* White list can not be used with RPAs */ + return 0x00; + } + + white_list_entries++; + add_to_white_list(req, params); + } + + /* After adding all new pending connections, walk through + * the list of pending reports and also add these to the + * white list if there is still space. + */ + list_for_each_entry(params, &hdev->pend_le_reports, action) { + if (hci_bdaddr_list_lookup(&hdev->le_white_list, + ¶ms->addr, params->addr_type)) + continue; + + if (white_list_entries >= hdev->le_white_list_size) { + /* Select filter policy to accept all advertising */ + return 0x00; + } + + if (hci_find_irk_by_addr(hdev, ¶ms->addr, + params->addr_type)) { + /* White list can not be used with RPAs */ + return 0x00; + } + + white_list_entries++; + add_to_white_list(req, params); + } + + /* Select filter policy to use white list */ + return 0x01; +} + +void hci_req_add_le_passive_scan(struct hci_request *req) +{ + struct hci_cp_le_set_scan_param param_cp; + struct hci_cp_le_set_scan_enable enable_cp; + struct hci_dev *hdev = req->hdev; + u8 own_addr_type; + u8 filter_policy; + + /* Set require_privacy to false since no SCAN_REQ are send + * during passive scanning. Not using an non-resolvable address + * here is important so that peer devices using direct + * advertising with our address will be correctly reported + * by the controller. + */ + if (hci_update_random_address(req, false, &own_addr_type)) + return; + + /* Adding or removing entries from the white list must + * happen before enabling scanning. The controller does + * not allow white list modification while scanning. + */ + filter_policy = update_white_list(req); + + /* When the controller is using random resolvable addresses and + * with that having LE privacy enabled, then controllers with + * Extended Scanner Filter Policies support can now enable support + * for handling directed advertising. + * + * So instead of using filter polices 0x00 (no whitelist) + * and 0x01 (whitelist enabled) use the new filter policies + * 0x02 (no whitelist) and 0x03 (whitelist enabled). + */ + if (hci_dev_test_flag(hdev, HCI_PRIVACY) && + (hdev->le_features[0] & HCI_LE_EXT_SCAN_POLICY)) + filter_policy |= 0x02; + + memset(¶m_cp, 0, sizeof(param_cp)); + param_cp.type = LE_SCAN_PASSIVE; + param_cp.interval = cpu_to_le16(hdev->le_scan_interval); + param_cp.window = cpu_to_le16(hdev->le_scan_window); + param_cp.own_address_type = own_addr_type; + param_cp.filter_policy = filter_policy; + hci_req_add(req, HCI_OP_LE_SET_SCAN_PARAM, sizeof(param_cp), + ¶m_cp); + + memset(&enable_cp, 0, sizeof(enable_cp)); + enable_cp.enable = LE_SCAN_ENABLE; + enable_cp.filter_dup = LE_SCAN_FILTER_DUP_ENABLE; + hci_req_add(req, HCI_OP_LE_SET_SCAN_ENABLE, sizeof(enable_cp), + &enable_cp); +} + +static void set_random_addr(struct hci_request *req, bdaddr_t *rpa) +{ + struct hci_dev *hdev = req->hdev; + + /* If we're advertising or initiating an LE connection we can't + * go ahead and change the random address at this time. This is + * because the eventual initiator address used for the + * subsequently created connection will be undefined (some + * controllers use the new address and others the one we had + * when the operation started). + * + * In this kind of scenario skip the update and let the random + * address be updated at the next cycle. + */ + if (hci_dev_test_flag(hdev, HCI_LE_ADV) || + hci_conn_hash_lookup_state(hdev, LE_LINK, BT_CONNECT)) { + BT_DBG("Deferring random address update"); + hci_dev_set_flag(hdev, HCI_RPA_EXPIRED); + return; + } + + hci_req_add(req, HCI_OP_LE_SET_RANDOM_ADDR, 6, rpa); +} + +int hci_update_random_address(struct hci_request *req, bool require_privacy, + u8 *own_addr_type) +{ + struct hci_dev *hdev = req->hdev; + int err; + + /* If privacy is enabled use a resolvable private address. If + * current RPA has expired or there is something else than + * the current RPA in use, then generate a new one. + */ + if (hci_dev_test_flag(hdev, HCI_PRIVACY)) { + int to; + + *own_addr_type = ADDR_LE_DEV_RANDOM; + + if (!hci_dev_test_and_clear_flag(hdev, HCI_RPA_EXPIRED) && + !bacmp(&hdev->random_addr, &hdev->rpa)) + return 0; + + err = smp_generate_rpa(hdev, hdev->irk, &hdev->rpa); + if (err < 0) { + BT_ERR("%s failed to generate new RPA", hdev->name); + return err; + } + + set_random_addr(req, &hdev->rpa); + + to = msecs_to_jiffies(hdev->rpa_timeout * 1000); + queue_delayed_work(hdev->workqueue, &hdev->rpa_expired, to); + + return 0; + } + + /* In case of required privacy without resolvable private address, + * use an non-resolvable private address. This is useful for active + * scanning and non-connectable advertising. + */ + if (require_privacy) { + bdaddr_t nrpa; + + while (true) { + /* The non-resolvable private address is generated + * from random six bytes with the two most significant + * bits cleared. + */ + get_random_bytes(&nrpa, 6); + nrpa.b[5] &= 0x3f; + + /* The non-resolvable private address shall not be + * equal to the public address. + */ + if (bacmp(&hdev->bdaddr, &nrpa)) + break; + } + + *own_addr_type = ADDR_LE_DEV_RANDOM; + set_random_addr(req, &nrpa); + return 0; + } + + /* If forcing static address is in use or there is no public + * address use the static address as random address (but skip + * the HCI command if the current random address is already the + * static one. + * + * In case BR/EDR has been disabled on a dual-mode controller + * and a static address has been configured, then use that + * address instead of the public BR/EDR address. + */ + if (hci_dev_test_flag(hdev, HCI_FORCE_STATIC_ADDR) || + !bacmp(&hdev->bdaddr, BDADDR_ANY) || + (!hci_dev_test_flag(hdev, HCI_BREDR_ENABLED) && + bacmp(&hdev->static_addr, BDADDR_ANY))) { + *own_addr_type = ADDR_LE_DEV_RANDOM; + if (bacmp(&hdev->static_addr, &hdev->random_addr)) + hci_req_add(req, HCI_OP_LE_SET_RANDOM_ADDR, 6, + &hdev->static_addr); + return 0; + } + + /* Neither privacy nor static address is being used so use a + * public address. + */ + *own_addr_type = ADDR_LE_DEV_PUBLIC; + + return 0; +} + +static bool disconnected_whitelist_entries(struct hci_dev *hdev) +{ + struct bdaddr_list *b; + + list_for_each_entry(b, &hdev->whitelist, list) { + struct hci_conn *conn; + + conn = hci_conn_hash_lookup_ba(hdev, ACL_LINK, &b->bdaddr); + if (!conn) + return true; + + if (conn->state != BT_CONNECTED && conn->state != BT_CONFIG) + return true; + } + + return false; +} + +void __hci_update_page_scan(struct hci_request *req) +{ + struct hci_dev *hdev = req->hdev; + u8 scan; + + if (!hci_dev_test_flag(hdev, HCI_BREDR_ENABLED)) + return; + + if (!hdev_is_powered(hdev)) + return; + + if (mgmt_powering_down(hdev)) + return; + + if (hci_dev_test_flag(hdev, HCI_CONNECTABLE) || + disconnected_whitelist_entries(hdev)) + scan = SCAN_PAGE; + else + scan = SCAN_DISABLED; + + if (test_bit(HCI_PSCAN, &hdev->flags) == !!(scan & SCAN_PAGE)) + return; + + if (hci_dev_test_flag(hdev, HCI_DISCOVERABLE)) + scan |= SCAN_INQUIRY; + + hci_req_add(req, HCI_OP_WRITE_SCAN_ENABLE, 1, &scan); +} + +void hci_update_page_scan(struct hci_dev *hdev) +{ + struct hci_request req; + + hci_req_init(&req, hdev); + __hci_update_page_scan(&req); + hci_req_run(&req, NULL); +} + +/* This function controls the background scanning based on hdev->pend_le_conns + * list. If there are pending LE connection we start the background scanning, + * otherwise we stop it. + * + * This function requires the caller holds hdev->lock. + */ +void __hci_update_background_scan(struct hci_request *req) +{ + struct hci_dev *hdev = req->hdev; + struct hci_conn *conn; + + if (!test_bit(HCI_UP, &hdev->flags) || + test_bit(HCI_INIT, &hdev->flags) || + hci_dev_test_flag(hdev, HCI_SETUP) || + hci_dev_test_flag(hdev, HCI_CONFIG) || + hci_dev_test_flag(hdev, HCI_AUTO_OFF) || + hci_dev_test_flag(hdev, HCI_UNREGISTER)) + return; + + /* No point in doing scanning if LE support hasn't been enabled */ + if (!hci_dev_test_flag(hdev, HCI_LE_ENABLED)) + return; + + /* If discovery is active don't interfere with it */ + if (hdev->discovery.state != DISCOVERY_STOPPED) + return; + + /* Reset RSSI and UUID filters when starting background scanning + * since these filters are meant for service discovery only. + * + * The Start Discovery and Start Service Discovery operations + * ensure to set proper values for RSSI threshold and UUID + * filter list. So it is safe to just reset them here. + */ + hci_discovery_filter_clear(hdev); + + if (list_empty(&hdev->pend_le_conns) && + list_empty(&hdev->pend_le_reports)) { + /* If there is no pending LE connections or devices + * to be scanned for, we should stop the background + * scanning. + */ + + /* If controller is not scanning we are done. */ + if (!hci_dev_test_flag(hdev, HCI_LE_SCAN)) + return; + + hci_req_add_le_scan_disable(req); + + BT_DBG("%s stopping background scanning", hdev->name); + } else { + /* If there is at least one pending LE connection, we should + * keep the background scan running. + */ + + /* If controller is connecting, we should not start scanning + * since some controllers are not able to scan and connect at + * the same time. + */ + conn = hci_conn_hash_lookup_state(hdev, LE_LINK, BT_CONNECT); + if (conn) + return; + + /* If controller is currently scanning, we stop it to ensure we + * don't miss any advertising (due to duplicates filter). + */ + if (hci_dev_test_flag(hdev, HCI_LE_SCAN)) + hci_req_add_le_scan_disable(req); + + hci_req_add_le_passive_scan(req); + + BT_DBG("%s starting background scanning", hdev->name); + } +} + +static void update_background_scan_complete(struct hci_dev *hdev, u8 status, + u16 opcode) +{ + if (status) + BT_DBG("HCI request failed to update background scanning: " + "status 0x%2.2x", status); +} + +void hci_update_background_scan(struct hci_dev *hdev) +{ + int err; + struct hci_request req; + + hci_req_init(&req, hdev); + + __hci_update_background_scan(&req); + + err = hci_req_run(&req, update_background_scan_complete); + if (err && err != -ENODATA) + BT_ERR("Failed to run HCI request: err %d", err); +} diff --git a/kernel/net/bluetooth/hci_request.h b/kernel/net/bluetooth/hci_request.h new file mode 100644 index 000000000..bf6df92f4 --- /dev/null +++ b/kernel/net/bluetooth/hci_request.h @@ -0,0 +1,57 @@ +/* + BlueZ - Bluetooth protocol stack for Linux + Copyright (C) 2014 Intel Corporation + + 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; + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS + OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT OF THIRD PARTY RIGHTS. + IN NO EVENT SHALL THE COPYRIGHT HOLDER(S) AND AUTHOR(S) BE LIABLE FOR ANY + CLAIM, OR ANY SPECIAL INDIRECT OR CONSEQUENTIAL DAMAGES, OR ANY DAMAGES + WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN + ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF + OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + + ALL LIABILITY, INCLUDING LIABILITY FOR INFRINGEMENT OF ANY PATENTS, + COPYRIGHTS, TRADEMARKS OR OTHER RIGHTS, RELATING TO USE OF THIS + SOFTWARE IS DISCLAIMED. +*/ + +struct hci_request { + struct hci_dev *hdev; + struct sk_buff_head cmd_q; + + /* If something goes wrong when building the HCI request, the error + * value is stored in this field. + */ + int err; +}; + +void hci_req_init(struct hci_request *req, struct hci_dev *hdev); +int hci_req_run(struct hci_request *req, hci_req_complete_t complete); +int hci_req_run_skb(struct hci_request *req, hci_req_complete_skb_t complete); +void hci_req_add(struct hci_request *req, u16 opcode, u32 plen, + const void *param); +void hci_req_add_ev(struct hci_request *req, u16 opcode, u32 plen, + const void *param, u8 event); +void hci_req_cmd_complete(struct hci_dev *hdev, u16 opcode, u8 status, + hci_req_complete_t *req_complete, + hci_req_complete_skb_t *req_complete_skb); + +struct sk_buff *hci_prepare_cmd(struct hci_dev *hdev, u16 opcode, u32 plen, + const void *param); + +void hci_req_add_le_scan_disable(struct hci_request *req); +void hci_req_add_le_passive_scan(struct hci_request *req); + +void hci_update_page_scan(struct hci_dev *hdev); +void __hci_update_page_scan(struct hci_request *req); + +int hci_update_random_address(struct hci_request *req, bool require_privacy, + u8 *own_addr_type); + +void hci_update_background_scan(struct hci_dev *hdev); +void __hci_update_background_scan(struct hci_request *req); diff --git a/kernel/net/bluetooth/hci_sock.c b/kernel/net/bluetooth/hci_sock.c new file mode 100644 index 000000000..56f9edbf3 --- /dev/null +++ b/kernel/net/bluetooth/hci_sock.c @@ -0,0 +1,1440 @@ +/* + BlueZ - Bluetooth protocol stack for Linux + Copyright (C) 2000-2001 Qualcomm Incorporated + + Written 2000,2001 by Maxim Krasnyansky <maxk@qualcomm.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; + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS + OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT OF THIRD PARTY RIGHTS. + IN NO EVENT SHALL THE COPYRIGHT HOLDER(S) AND AUTHOR(S) BE LIABLE FOR ANY + CLAIM, OR ANY SPECIAL INDIRECT OR CONSEQUENTIAL DAMAGES, OR ANY DAMAGES + WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN + ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF + OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + + ALL LIABILITY, INCLUDING LIABILITY FOR INFRINGEMENT OF ANY PATENTS, + COPYRIGHTS, TRADEMARKS OR OTHER RIGHTS, RELATING TO USE OF THIS + SOFTWARE IS DISCLAIMED. +*/ + +/* Bluetooth HCI sockets. */ + +#include <linux/export.h> +#include <asm/unaligned.h> + +#include <net/bluetooth/bluetooth.h> +#include <net/bluetooth/hci_core.h> +#include <net/bluetooth/hci_mon.h> +#include <net/bluetooth/mgmt.h> + +#include "mgmt_util.h" + +static LIST_HEAD(mgmt_chan_list); +static DEFINE_MUTEX(mgmt_chan_list_lock); + +static atomic_t monitor_promisc = ATOMIC_INIT(0); + +/* ----- HCI socket interface ----- */ + +/* Socket info */ +#define hci_pi(sk) ((struct hci_pinfo *) sk) + +struct hci_pinfo { + struct bt_sock bt; + struct hci_dev *hdev; + struct hci_filter filter; + __u32 cmsg_mask; + unsigned short channel; + unsigned long flags; +}; + +void hci_sock_set_flag(struct sock *sk, int nr) +{ + set_bit(nr, &hci_pi(sk)->flags); +} + +void hci_sock_clear_flag(struct sock *sk, int nr) +{ + clear_bit(nr, &hci_pi(sk)->flags); +} + +int hci_sock_test_flag(struct sock *sk, int nr) +{ + return test_bit(nr, &hci_pi(sk)->flags); +} + +unsigned short hci_sock_get_channel(struct sock *sk) +{ + return hci_pi(sk)->channel; +} + +static inline int hci_test_bit(int nr, const void *addr) +{ + return *((const __u32 *) addr + (nr >> 5)) & ((__u32) 1 << (nr & 31)); +} + +/* Security filter */ +#define HCI_SFLT_MAX_OGF 5 + +struct hci_sec_filter { + __u32 type_mask; + __u32 event_mask[2]; + __u32 ocf_mask[HCI_SFLT_MAX_OGF + 1][4]; +}; + +static const struct hci_sec_filter hci_sec_filter = { + /* Packet types */ + 0x10, + /* Events */ + { 0x1000d9fe, 0x0000b00c }, + /* Commands */ + { + { 0x0 }, + /* OGF_LINK_CTL */ + { 0xbe000006, 0x00000001, 0x00000000, 0x00 }, + /* OGF_LINK_POLICY */ + { 0x00005200, 0x00000000, 0x00000000, 0x00 }, + /* OGF_HOST_CTL */ + { 0xaab00200, 0x2b402aaa, 0x05220154, 0x00 }, + /* OGF_INFO_PARAM */ + { 0x000002be, 0x00000000, 0x00000000, 0x00 }, + /* OGF_STATUS_PARAM */ + { 0x000000ea, 0x00000000, 0x00000000, 0x00 } + } +}; + +static struct bt_sock_list hci_sk_list = { + .lock = __RW_LOCK_UNLOCKED(hci_sk_list.lock) +}; + +static bool is_filtered_packet(struct sock *sk, struct sk_buff *skb) +{ + struct hci_filter *flt; + int flt_type, flt_event; + + /* Apply filter */ + flt = &hci_pi(sk)->filter; + + if (bt_cb(skb)->pkt_type == HCI_VENDOR_PKT) + flt_type = 0; + else + flt_type = bt_cb(skb)->pkt_type & HCI_FLT_TYPE_BITS; + + if (!test_bit(flt_type, &flt->type_mask)) + return true; + + /* Extra filter for event packets only */ + if (bt_cb(skb)->pkt_type != HCI_EVENT_PKT) + return false; + + flt_event = (*(__u8 *)skb->data & HCI_FLT_EVENT_BITS); + + if (!hci_test_bit(flt_event, &flt->event_mask)) + return true; + + /* Check filter only when opcode is set */ + if (!flt->opcode) + return false; + + if (flt_event == HCI_EV_CMD_COMPLETE && + flt->opcode != get_unaligned((__le16 *)(skb->data + 3))) + return true; + + if (flt_event == HCI_EV_CMD_STATUS && + flt->opcode != get_unaligned((__le16 *)(skb->data + 4))) + return true; + + return false; +} + +/* Send frame to RAW socket */ +void hci_send_to_sock(struct hci_dev *hdev, struct sk_buff *skb) +{ + struct sock *sk; + struct sk_buff *skb_copy = NULL; + + BT_DBG("hdev %p len %d", hdev, skb->len); + + read_lock(&hci_sk_list.lock); + + sk_for_each(sk, &hci_sk_list.head) { + struct sk_buff *nskb; + + if (sk->sk_state != BT_BOUND || hci_pi(sk)->hdev != hdev) + continue; + + /* Don't send frame to the socket it came from */ + if (skb->sk == sk) + continue; + + if (hci_pi(sk)->channel == HCI_CHANNEL_RAW) { + if (is_filtered_packet(sk, skb)) + continue; + } else if (hci_pi(sk)->channel == HCI_CHANNEL_USER) { + if (!bt_cb(skb)->incoming) + continue; + if (bt_cb(skb)->pkt_type != HCI_EVENT_PKT && + bt_cb(skb)->pkt_type != HCI_ACLDATA_PKT && + bt_cb(skb)->pkt_type != HCI_SCODATA_PKT) + continue; + } else { + /* Don't send frame to other channel types */ + continue; + } + + if (!skb_copy) { + /* Create a private copy with headroom */ + skb_copy = __pskb_copy_fclone(skb, 1, GFP_ATOMIC, true); + if (!skb_copy) + continue; + + /* Put type byte before the data */ + memcpy(skb_push(skb_copy, 1), &bt_cb(skb)->pkt_type, 1); + } + + nskb = skb_clone(skb_copy, GFP_ATOMIC); + if (!nskb) + continue; + + if (sock_queue_rcv_skb(sk, nskb)) + kfree_skb(nskb); + } + + read_unlock(&hci_sk_list.lock); + + kfree_skb(skb_copy); +} + +/* Send frame to sockets with specific channel */ +void hci_send_to_channel(unsigned short channel, struct sk_buff *skb, + int flag, struct sock *skip_sk) +{ + struct sock *sk; + + BT_DBG("channel %u len %d", channel, skb->len); + + read_lock(&hci_sk_list.lock); + + sk_for_each(sk, &hci_sk_list.head) { + struct sk_buff *nskb; + + /* Ignore socket without the flag set */ + if (!hci_sock_test_flag(sk, flag)) + continue; + + /* Skip the original socket */ + if (sk == skip_sk) + continue; + + if (sk->sk_state != BT_BOUND) + continue; + + if (hci_pi(sk)->channel != channel) + continue; + + nskb = skb_clone(skb, GFP_ATOMIC); + if (!nskb) + continue; + + if (sock_queue_rcv_skb(sk, nskb)) + kfree_skb(nskb); + } + + read_unlock(&hci_sk_list.lock); +} + +/* Send frame to monitor socket */ +void hci_send_to_monitor(struct hci_dev *hdev, struct sk_buff *skb) +{ + struct sk_buff *skb_copy = NULL; + struct hci_mon_hdr *hdr; + __le16 opcode; + + if (!atomic_read(&monitor_promisc)) + return; + + BT_DBG("hdev %p len %d", hdev, skb->len); + + switch (bt_cb(skb)->pkt_type) { + case HCI_COMMAND_PKT: + opcode = cpu_to_le16(HCI_MON_COMMAND_PKT); + break; + case HCI_EVENT_PKT: + opcode = cpu_to_le16(HCI_MON_EVENT_PKT); + break; + case HCI_ACLDATA_PKT: + if (bt_cb(skb)->incoming) + opcode = cpu_to_le16(HCI_MON_ACL_RX_PKT); + else + opcode = cpu_to_le16(HCI_MON_ACL_TX_PKT); + break; + case HCI_SCODATA_PKT: + if (bt_cb(skb)->incoming) + opcode = cpu_to_le16(HCI_MON_SCO_RX_PKT); + else + opcode = cpu_to_le16(HCI_MON_SCO_TX_PKT); + break; + default: + return; + } + + /* Create a private copy with headroom */ + skb_copy = __pskb_copy_fclone(skb, HCI_MON_HDR_SIZE, GFP_ATOMIC, true); + if (!skb_copy) + return; + + /* Put header before the data */ + hdr = (void *) skb_push(skb_copy, HCI_MON_HDR_SIZE); + hdr->opcode = opcode; + hdr->index = cpu_to_le16(hdev->id); + hdr->len = cpu_to_le16(skb->len); + + hci_send_to_channel(HCI_CHANNEL_MONITOR, skb_copy, + HCI_SOCK_TRUSTED, NULL); + kfree_skb(skb_copy); +} + +static struct sk_buff *create_monitor_event(struct hci_dev *hdev, int event) +{ + struct hci_mon_hdr *hdr; + struct hci_mon_new_index *ni; + struct sk_buff *skb; + __le16 opcode; + + switch (event) { + case HCI_DEV_REG: + skb = bt_skb_alloc(HCI_MON_NEW_INDEX_SIZE, GFP_ATOMIC); + if (!skb) + return NULL; + + ni = (void *) skb_put(skb, HCI_MON_NEW_INDEX_SIZE); + ni->type = hdev->dev_type; + ni->bus = hdev->bus; + bacpy(&ni->bdaddr, &hdev->bdaddr); + memcpy(ni->name, hdev->name, 8); + + opcode = cpu_to_le16(HCI_MON_NEW_INDEX); + break; + + case HCI_DEV_UNREG: + skb = bt_skb_alloc(0, GFP_ATOMIC); + if (!skb) + return NULL; + + opcode = cpu_to_le16(HCI_MON_DEL_INDEX); + break; + + default: + return NULL; + } + + __net_timestamp(skb); + + hdr = (void *) skb_push(skb, HCI_MON_HDR_SIZE); + hdr->opcode = opcode; + hdr->index = cpu_to_le16(hdev->id); + hdr->len = cpu_to_le16(skb->len - HCI_MON_HDR_SIZE); + + return skb; +} + +static void send_monitor_replay(struct sock *sk) +{ + struct hci_dev *hdev; + + read_lock(&hci_dev_list_lock); + + list_for_each_entry(hdev, &hci_dev_list, list) { + struct sk_buff *skb; + + skb = create_monitor_event(hdev, HCI_DEV_REG); + if (!skb) + continue; + + if (sock_queue_rcv_skb(sk, skb)) + kfree_skb(skb); + } + + read_unlock(&hci_dev_list_lock); +} + +/* Generate internal stack event */ +static void hci_si_event(struct hci_dev *hdev, int type, int dlen, void *data) +{ + struct hci_event_hdr *hdr; + struct hci_ev_stack_internal *ev; + struct sk_buff *skb; + + skb = bt_skb_alloc(HCI_EVENT_HDR_SIZE + sizeof(*ev) + dlen, GFP_ATOMIC); + if (!skb) + return; + + hdr = (void *) skb_put(skb, HCI_EVENT_HDR_SIZE); + hdr->evt = HCI_EV_STACK_INTERNAL; + hdr->plen = sizeof(*ev) + dlen; + + ev = (void *) skb_put(skb, sizeof(*ev) + dlen); + ev->type = type; + memcpy(ev->data, data, dlen); + + bt_cb(skb)->incoming = 1; + __net_timestamp(skb); + + bt_cb(skb)->pkt_type = HCI_EVENT_PKT; + hci_send_to_sock(hdev, skb); + kfree_skb(skb); +} + +void hci_sock_dev_event(struct hci_dev *hdev, int event) +{ + struct hci_ev_si_device ev; + + BT_DBG("hdev %s event %d", hdev->name, event); + + /* Send event to monitor */ + if (atomic_read(&monitor_promisc)) { + struct sk_buff *skb; + + skb = create_monitor_event(hdev, event); + if (skb) { + hci_send_to_channel(HCI_CHANNEL_MONITOR, skb, + HCI_SOCK_TRUSTED, NULL); + kfree_skb(skb); + } + } + + /* Send event to sockets */ + ev.event = event; + ev.dev_id = hdev->id; + hci_si_event(NULL, HCI_EV_SI_DEVICE, sizeof(ev), &ev); + + if (event == HCI_DEV_UNREG) { + struct sock *sk; + + /* Detach sockets from device */ + read_lock(&hci_sk_list.lock); + sk_for_each(sk, &hci_sk_list.head) { + bh_lock_sock_nested(sk); + if (hci_pi(sk)->hdev == hdev) { + hci_pi(sk)->hdev = NULL; + sk->sk_err = EPIPE; + sk->sk_state = BT_OPEN; + sk->sk_state_change(sk); + + hci_dev_put(hdev); + } + bh_unlock_sock(sk); + } + read_unlock(&hci_sk_list.lock); + } +} + +static struct hci_mgmt_chan *__hci_mgmt_chan_find(unsigned short channel) +{ + struct hci_mgmt_chan *c; + + list_for_each_entry(c, &mgmt_chan_list, list) { + if (c->channel == channel) + return c; + } + + return NULL; +} + +static struct hci_mgmt_chan *hci_mgmt_chan_find(unsigned short channel) +{ + struct hci_mgmt_chan *c; + + mutex_lock(&mgmt_chan_list_lock); + c = __hci_mgmt_chan_find(channel); + mutex_unlock(&mgmt_chan_list_lock); + + return c; +} + +int hci_mgmt_chan_register(struct hci_mgmt_chan *c) +{ + if (c->channel < HCI_CHANNEL_CONTROL) + return -EINVAL; + + mutex_lock(&mgmt_chan_list_lock); + if (__hci_mgmt_chan_find(c->channel)) { + mutex_unlock(&mgmt_chan_list_lock); + return -EALREADY; + } + + list_add_tail(&c->list, &mgmt_chan_list); + + mutex_unlock(&mgmt_chan_list_lock); + + return 0; +} +EXPORT_SYMBOL(hci_mgmt_chan_register); + +void hci_mgmt_chan_unregister(struct hci_mgmt_chan *c) +{ + mutex_lock(&mgmt_chan_list_lock); + list_del(&c->list); + mutex_unlock(&mgmt_chan_list_lock); +} +EXPORT_SYMBOL(hci_mgmt_chan_unregister); + +static int hci_sock_release(struct socket *sock) +{ + struct sock *sk = sock->sk; + struct hci_dev *hdev; + + BT_DBG("sock %p sk %p", sock, sk); + + if (!sk) + return 0; + + hdev = hci_pi(sk)->hdev; + + if (hci_pi(sk)->channel == HCI_CHANNEL_MONITOR) + atomic_dec(&monitor_promisc); + + bt_sock_unlink(&hci_sk_list, sk); + + if (hdev) { + if (hci_pi(sk)->channel == HCI_CHANNEL_USER) { + mgmt_index_added(hdev); + hci_dev_clear_flag(hdev, HCI_USER_CHANNEL); + hci_dev_close(hdev->id); + } + + atomic_dec(&hdev->promisc); + hci_dev_put(hdev); + } + + sock_orphan(sk); + + skb_queue_purge(&sk->sk_receive_queue); + skb_queue_purge(&sk->sk_write_queue); + + sock_put(sk); + return 0; +} + +static int hci_sock_blacklist_add(struct hci_dev *hdev, void __user *arg) +{ + bdaddr_t bdaddr; + int err; + + if (copy_from_user(&bdaddr, arg, sizeof(bdaddr))) + return -EFAULT; + + hci_dev_lock(hdev); + + err = hci_bdaddr_list_add(&hdev->blacklist, &bdaddr, BDADDR_BREDR); + + hci_dev_unlock(hdev); + + return err; +} + +static int hci_sock_blacklist_del(struct hci_dev *hdev, void __user *arg) +{ + bdaddr_t bdaddr; + int err; + + if (copy_from_user(&bdaddr, arg, sizeof(bdaddr))) + return -EFAULT; + + hci_dev_lock(hdev); + + err = hci_bdaddr_list_del(&hdev->blacklist, &bdaddr, BDADDR_BREDR); + + hci_dev_unlock(hdev); + + return err; +} + +/* Ioctls that require bound socket */ +static int hci_sock_bound_ioctl(struct sock *sk, unsigned int cmd, + unsigned long arg) +{ + struct hci_dev *hdev = hci_pi(sk)->hdev; + + if (!hdev) + return -EBADFD; + + if (hci_dev_test_flag(hdev, HCI_USER_CHANNEL)) + return -EBUSY; + + if (hci_dev_test_flag(hdev, HCI_UNCONFIGURED)) + return -EOPNOTSUPP; + + if (hdev->dev_type != HCI_BREDR) + return -EOPNOTSUPP; + + switch (cmd) { + case HCISETRAW: + if (!capable(CAP_NET_ADMIN)) + return -EPERM; + return -EOPNOTSUPP; + + case HCIGETCONNINFO: + return hci_get_conn_info(hdev, (void __user *) arg); + + case HCIGETAUTHINFO: + return hci_get_auth_info(hdev, (void __user *) arg); + + case HCIBLOCKADDR: + if (!capable(CAP_NET_ADMIN)) + return -EPERM; + return hci_sock_blacklist_add(hdev, (void __user *) arg); + + case HCIUNBLOCKADDR: + if (!capable(CAP_NET_ADMIN)) + return -EPERM; + return hci_sock_blacklist_del(hdev, (void __user *) arg); + } + + return -ENOIOCTLCMD; +} + +static int hci_sock_ioctl(struct socket *sock, unsigned int cmd, + unsigned long arg) +{ + void __user *argp = (void __user *) arg; + struct sock *sk = sock->sk; + int err; + + BT_DBG("cmd %x arg %lx", cmd, arg); + + lock_sock(sk); + + if (hci_pi(sk)->channel != HCI_CHANNEL_RAW) { + err = -EBADFD; + goto done; + } + + release_sock(sk); + + switch (cmd) { + case HCIGETDEVLIST: + return hci_get_dev_list(argp); + + case HCIGETDEVINFO: + return hci_get_dev_info(argp); + + case HCIGETCONNLIST: + return hci_get_conn_list(argp); + + case HCIDEVUP: + if (!capable(CAP_NET_ADMIN)) + return -EPERM; + return hci_dev_open(arg); + + case HCIDEVDOWN: + if (!capable(CAP_NET_ADMIN)) + return -EPERM; + return hci_dev_close(arg); + + case HCIDEVRESET: + if (!capable(CAP_NET_ADMIN)) + return -EPERM; + return hci_dev_reset(arg); + + case HCIDEVRESTAT: + if (!capable(CAP_NET_ADMIN)) + return -EPERM; + return hci_dev_reset_stat(arg); + + case HCISETSCAN: + case HCISETAUTH: + case HCISETENCRYPT: + case HCISETPTYPE: + case HCISETLINKPOL: + case HCISETLINKMODE: + case HCISETACLMTU: + case HCISETSCOMTU: + if (!capable(CAP_NET_ADMIN)) + return -EPERM; + return hci_dev_cmd(cmd, argp); + + case HCIINQUIRY: + return hci_inquiry(argp); + } + + lock_sock(sk); + + err = hci_sock_bound_ioctl(sk, cmd, arg); + +done: + release_sock(sk); + return err; +} + +static int hci_sock_bind(struct socket *sock, struct sockaddr *addr, + int addr_len) +{ + struct sockaddr_hci haddr; + struct sock *sk = sock->sk; + struct hci_dev *hdev = NULL; + int len, err = 0; + + BT_DBG("sock %p sk %p", sock, sk); + + if (!addr) + return -EINVAL; + + memset(&haddr, 0, sizeof(haddr)); + len = min_t(unsigned int, sizeof(haddr), addr_len); + memcpy(&haddr, addr, len); + + if (haddr.hci_family != AF_BLUETOOTH) + return -EINVAL; + + lock_sock(sk); + + if (sk->sk_state == BT_BOUND) { + err = -EALREADY; + goto done; + } + + switch (haddr.hci_channel) { + case HCI_CHANNEL_RAW: + if (hci_pi(sk)->hdev) { + err = -EALREADY; + goto done; + } + + if (haddr.hci_dev != HCI_DEV_NONE) { + hdev = hci_dev_get(haddr.hci_dev); + if (!hdev) { + err = -ENODEV; + goto done; + } + + atomic_inc(&hdev->promisc); + } + + hci_pi(sk)->hdev = hdev; + break; + + case HCI_CHANNEL_USER: + if (hci_pi(sk)->hdev) { + err = -EALREADY; + goto done; + } + + if (haddr.hci_dev == HCI_DEV_NONE) { + err = -EINVAL; + goto done; + } + + if (!capable(CAP_NET_ADMIN)) { + err = -EPERM; + goto done; + } + + hdev = hci_dev_get(haddr.hci_dev); + if (!hdev) { + err = -ENODEV; + goto done; + } + + if (test_bit(HCI_UP, &hdev->flags) || + test_bit(HCI_INIT, &hdev->flags) || + hci_dev_test_flag(hdev, HCI_SETUP) || + hci_dev_test_flag(hdev, HCI_CONFIG)) { + err = -EBUSY; + hci_dev_put(hdev); + goto done; + } + + if (hci_dev_test_and_set_flag(hdev, HCI_USER_CHANNEL)) { + err = -EUSERS; + hci_dev_put(hdev); + goto done; + } + + mgmt_index_removed(hdev); + + err = hci_dev_open(hdev->id); + if (err) { + hci_dev_clear_flag(hdev, HCI_USER_CHANNEL); + mgmt_index_added(hdev); + hci_dev_put(hdev); + goto done; + } + + atomic_inc(&hdev->promisc); + + hci_pi(sk)->hdev = hdev; + break; + + case HCI_CHANNEL_MONITOR: + if (haddr.hci_dev != HCI_DEV_NONE) { + err = -EINVAL; + goto done; + } + + if (!capable(CAP_NET_RAW)) { + err = -EPERM; + goto done; + } + + /* The monitor interface is restricted to CAP_NET_RAW + * capabilities and with that implicitly trusted. + */ + hci_sock_set_flag(sk, HCI_SOCK_TRUSTED); + + send_monitor_replay(sk); + + atomic_inc(&monitor_promisc); + break; + + default: + if (!hci_mgmt_chan_find(haddr.hci_channel)) { + err = -EINVAL; + goto done; + } + + if (haddr.hci_dev != HCI_DEV_NONE) { + err = -EINVAL; + goto done; + } + + /* Users with CAP_NET_ADMIN capabilities are allowed + * access to all management commands and events. For + * untrusted users the interface is restricted and + * also only untrusted events are sent. + */ + if (capable(CAP_NET_ADMIN)) + hci_sock_set_flag(sk, HCI_SOCK_TRUSTED); + + /* At the moment the index and unconfigured index events + * are enabled unconditionally. Setting them on each + * socket when binding keeps this functionality. They + * however might be cleared later and then sending of these + * events will be disabled, but that is then intentional. + * + * This also enables generic events that are safe to be + * received by untrusted users. Example for such events + * are changes to settings, class of device, name etc. + */ + if (haddr.hci_channel == HCI_CHANNEL_CONTROL) { + hci_sock_set_flag(sk, HCI_MGMT_INDEX_EVENTS); + hci_sock_set_flag(sk, HCI_MGMT_UNCONF_INDEX_EVENTS); + hci_sock_set_flag(sk, HCI_MGMT_GENERIC_EVENTS); + } + break; + } + + + hci_pi(sk)->channel = haddr.hci_channel; + sk->sk_state = BT_BOUND; + +done: + release_sock(sk); + return err; +} + +static int hci_sock_getname(struct socket *sock, struct sockaddr *addr, + int *addr_len, int peer) +{ + struct sockaddr_hci *haddr = (struct sockaddr_hci *) addr; + struct sock *sk = sock->sk; + struct hci_dev *hdev; + int err = 0; + + BT_DBG("sock %p sk %p", sock, sk); + + if (peer) + return -EOPNOTSUPP; + + lock_sock(sk); + + hdev = hci_pi(sk)->hdev; + if (!hdev) { + err = -EBADFD; + goto done; + } + + *addr_len = sizeof(*haddr); + haddr->hci_family = AF_BLUETOOTH; + haddr->hci_dev = hdev->id; + haddr->hci_channel= hci_pi(sk)->channel; + +done: + release_sock(sk); + return err; +} + +static void hci_sock_cmsg(struct sock *sk, struct msghdr *msg, + struct sk_buff *skb) +{ + __u32 mask = hci_pi(sk)->cmsg_mask; + + if (mask & HCI_CMSG_DIR) { + int incoming = bt_cb(skb)->incoming; + put_cmsg(msg, SOL_HCI, HCI_CMSG_DIR, sizeof(incoming), + &incoming); + } + + if (mask & HCI_CMSG_TSTAMP) { +#ifdef CONFIG_COMPAT + struct compat_timeval ctv; +#endif + struct timeval tv; + void *data; + int len; + + skb_get_timestamp(skb, &tv); + + data = &tv; + len = sizeof(tv); +#ifdef CONFIG_COMPAT + if (!COMPAT_USE_64BIT_TIME && + (msg->msg_flags & MSG_CMSG_COMPAT)) { + ctv.tv_sec = tv.tv_sec; + ctv.tv_usec = tv.tv_usec; + data = &ctv; + len = sizeof(ctv); + } +#endif + + put_cmsg(msg, SOL_HCI, HCI_CMSG_TSTAMP, len, data); + } +} + +static int hci_sock_recvmsg(struct socket *sock, struct msghdr *msg, size_t len, + int flags) +{ + int noblock = flags & MSG_DONTWAIT; + struct sock *sk = sock->sk; + struct sk_buff *skb; + int copied, err; + + BT_DBG("sock %p, sk %p", sock, sk); + + if (flags & (MSG_OOB)) + return -EOPNOTSUPP; + + if (sk->sk_state == BT_CLOSED) + return 0; + + skb = skb_recv_datagram(sk, flags, noblock, &err); + if (!skb) + return err; + + copied = skb->len; + if (len < copied) { + msg->msg_flags |= MSG_TRUNC; + copied = len; + } + + skb_reset_transport_header(skb); + err = skb_copy_datagram_msg(skb, 0, msg, copied); + + switch (hci_pi(sk)->channel) { + case HCI_CHANNEL_RAW: + hci_sock_cmsg(sk, msg, skb); + break; + case HCI_CHANNEL_USER: + case HCI_CHANNEL_MONITOR: + sock_recv_timestamp(msg, sk, skb); + break; + default: + if (hci_mgmt_chan_find(hci_pi(sk)->channel)) + sock_recv_timestamp(msg, sk, skb); + break; + } + + skb_free_datagram(sk, skb); + + return err ? : copied; +} + +static int hci_mgmt_cmd(struct hci_mgmt_chan *chan, struct sock *sk, + struct msghdr *msg, size_t msglen) +{ + void *buf; + u8 *cp; + struct mgmt_hdr *hdr; + u16 opcode, index, len; + struct hci_dev *hdev = NULL; + const struct hci_mgmt_handler *handler; + bool var_len, no_hdev; + int err; + + BT_DBG("got %zu bytes", msglen); + + if (msglen < sizeof(*hdr)) + return -EINVAL; + + buf = kmalloc(msglen, GFP_KERNEL); + if (!buf) + return -ENOMEM; + + if (memcpy_from_msg(buf, msg, msglen)) { + err = -EFAULT; + goto done; + } + + hdr = buf; + opcode = __le16_to_cpu(hdr->opcode); + index = __le16_to_cpu(hdr->index); + len = __le16_to_cpu(hdr->len); + + if (len != msglen - sizeof(*hdr)) { + err = -EINVAL; + goto done; + } + + if (opcode >= chan->handler_count || + chan->handlers[opcode].func == NULL) { + BT_DBG("Unknown op %u", opcode); + err = mgmt_cmd_status(sk, index, opcode, + MGMT_STATUS_UNKNOWN_COMMAND); + goto done; + } + + handler = &chan->handlers[opcode]; + + if (!hci_sock_test_flag(sk, HCI_SOCK_TRUSTED) && + !(handler->flags & HCI_MGMT_UNTRUSTED)) { + err = mgmt_cmd_status(sk, index, opcode, + MGMT_STATUS_PERMISSION_DENIED); + goto done; + } + + if (index != MGMT_INDEX_NONE) { + hdev = hci_dev_get(index); + if (!hdev) { + err = mgmt_cmd_status(sk, index, opcode, + MGMT_STATUS_INVALID_INDEX); + goto done; + } + + if (hci_dev_test_flag(hdev, HCI_SETUP) || + hci_dev_test_flag(hdev, HCI_CONFIG) || + hci_dev_test_flag(hdev, HCI_USER_CHANNEL)) { + err = mgmt_cmd_status(sk, index, opcode, + MGMT_STATUS_INVALID_INDEX); + goto done; + } + + if (hci_dev_test_flag(hdev, HCI_UNCONFIGURED) && + !(handler->flags & HCI_MGMT_UNCONFIGURED)) { + err = mgmt_cmd_status(sk, index, opcode, + MGMT_STATUS_INVALID_INDEX); + goto done; + } + } + + no_hdev = (handler->flags & HCI_MGMT_NO_HDEV); + if (no_hdev != !hdev) { + err = mgmt_cmd_status(sk, index, opcode, + MGMT_STATUS_INVALID_INDEX); + goto done; + } + + var_len = (handler->flags & HCI_MGMT_VAR_LEN); + if ((var_len && len < handler->data_len) || + (!var_len && len != handler->data_len)) { + err = mgmt_cmd_status(sk, index, opcode, + MGMT_STATUS_INVALID_PARAMS); + goto done; + } + + if (hdev && chan->hdev_init) + chan->hdev_init(sk, hdev); + + cp = buf + sizeof(*hdr); + + err = handler->func(sk, hdev, cp, len); + if (err < 0) + goto done; + + err = msglen; + +done: + if (hdev) + hci_dev_put(hdev); + + kfree(buf); + return err; +} + +static int hci_sock_sendmsg(struct socket *sock, struct msghdr *msg, + size_t len) +{ + struct sock *sk = sock->sk; + struct hci_mgmt_chan *chan; + struct hci_dev *hdev; + struct sk_buff *skb; + int err; + + BT_DBG("sock %p sk %p", sock, sk); + + if (msg->msg_flags & MSG_OOB) + return -EOPNOTSUPP; + + if (msg->msg_flags & ~(MSG_DONTWAIT|MSG_NOSIGNAL|MSG_ERRQUEUE)) + return -EINVAL; + + if (len < 4 || len > HCI_MAX_FRAME_SIZE) + return -EINVAL; + + lock_sock(sk); + + switch (hci_pi(sk)->channel) { + case HCI_CHANNEL_RAW: + case HCI_CHANNEL_USER: + break; + case HCI_CHANNEL_MONITOR: + err = -EOPNOTSUPP; + goto done; + default: + mutex_lock(&mgmt_chan_list_lock); + chan = __hci_mgmt_chan_find(hci_pi(sk)->channel); + if (chan) + err = hci_mgmt_cmd(chan, sk, msg, len); + else + err = -EINVAL; + + mutex_unlock(&mgmt_chan_list_lock); + goto done; + } + + hdev = hci_pi(sk)->hdev; + if (!hdev) { + err = -EBADFD; + goto done; + } + + if (!test_bit(HCI_UP, &hdev->flags)) { + err = -ENETDOWN; + goto done; + } + + skb = bt_skb_send_alloc(sk, len, msg->msg_flags & MSG_DONTWAIT, &err); + if (!skb) + goto done; + + if (memcpy_from_msg(skb_put(skb, len), msg, len)) { + err = -EFAULT; + goto drop; + } + + bt_cb(skb)->pkt_type = *((unsigned char *) skb->data); + skb_pull(skb, 1); + + if (hci_pi(sk)->channel == HCI_CHANNEL_USER) { + /* No permission check is needed for user channel + * since that gets enforced when binding the socket. + * + * However check that the packet type is valid. + */ + if (bt_cb(skb)->pkt_type != HCI_COMMAND_PKT && + bt_cb(skb)->pkt_type != HCI_ACLDATA_PKT && + bt_cb(skb)->pkt_type != HCI_SCODATA_PKT) { + err = -EINVAL; + goto drop; + } + + skb_queue_tail(&hdev->raw_q, skb); + queue_work(hdev->workqueue, &hdev->tx_work); + } else if (bt_cb(skb)->pkt_type == HCI_COMMAND_PKT) { + u16 opcode = get_unaligned_le16(skb->data); + u16 ogf = hci_opcode_ogf(opcode); + u16 ocf = hci_opcode_ocf(opcode); + + if (((ogf > HCI_SFLT_MAX_OGF) || + !hci_test_bit(ocf & HCI_FLT_OCF_BITS, + &hci_sec_filter.ocf_mask[ogf])) && + !capable(CAP_NET_RAW)) { + err = -EPERM; + goto drop; + } + + if (ogf == 0x3f) { + skb_queue_tail(&hdev->raw_q, skb); + queue_work(hdev->workqueue, &hdev->tx_work); + } else { + /* Stand-alone HCI commands must be flagged as + * single-command requests. + */ + bt_cb(skb)->req.start = true; + + skb_queue_tail(&hdev->cmd_q, skb); + queue_work(hdev->workqueue, &hdev->cmd_work); + } + } else { + if (!capable(CAP_NET_RAW)) { + err = -EPERM; + goto drop; + } + + skb_queue_tail(&hdev->raw_q, skb); + queue_work(hdev->workqueue, &hdev->tx_work); + } + + err = len; + +done: + release_sock(sk); + return err; + +drop: + kfree_skb(skb); + goto done; +} + +static int hci_sock_setsockopt(struct socket *sock, int level, int optname, + char __user *optval, unsigned int len) +{ + struct hci_ufilter uf = { .opcode = 0 }; + struct sock *sk = sock->sk; + int err = 0, opt = 0; + + BT_DBG("sk %p, opt %d", sk, optname); + + lock_sock(sk); + + if (hci_pi(sk)->channel != HCI_CHANNEL_RAW) { + err = -EBADFD; + goto done; + } + + switch (optname) { + case HCI_DATA_DIR: + if (get_user(opt, (int __user *)optval)) { + err = -EFAULT; + break; + } + + if (opt) + hci_pi(sk)->cmsg_mask |= HCI_CMSG_DIR; + else + hci_pi(sk)->cmsg_mask &= ~HCI_CMSG_DIR; + break; + + case HCI_TIME_STAMP: + if (get_user(opt, (int __user *)optval)) { + err = -EFAULT; + break; + } + + if (opt) + hci_pi(sk)->cmsg_mask |= HCI_CMSG_TSTAMP; + else + hci_pi(sk)->cmsg_mask &= ~HCI_CMSG_TSTAMP; + break; + + case HCI_FILTER: + { + struct hci_filter *f = &hci_pi(sk)->filter; + + uf.type_mask = f->type_mask; + uf.opcode = f->opcode; + uf.event_mask[0] = *((u32 *) f->event_mask + 0); + uf.event_mask[1] = *((u32 *) f->event_mask + 1); + } + + len = min_t(unsigned int, len, sizeof(uf)); + if (copy_from_user(&uf, optval, len)) { + err = -EFAULT; + break; + } + + if (!capable(CAP_NET_RAW)) { + uf.type_mask &= hci_sec_filter.type_mask; + uf.event_mask[0] &= *((u32 *) hci_sec_filter.event_mask + 0); + uf.event_mask[1] &= *((u32 *) hci_sec_filter.event_mask + 1); + } + + { + struct hci_filter *f = &hci_pi(sk)->filter; + + f->type_mask = uf.type_mask; + f->opcode = uf.opcode; + *((u32 *) f->event_mask + 0) = uf.event_mask[0]; + *((u32 *) f->event_mask + 1) = uf.event_mask[1]; + } + break; + + default: + err = -ENOPROTOOPT; + break; + } + +done: + release_sock(sk); + return err; +} + +static int hci_sock_getsockopt(struct socket *sock, int level, int optname, + char __user *optval, int __user *optlen) +{ + struct hci_ufilter uf; + struct sock *sk = sock->sk; + int len, opt, err = 0; + + BT_DBG("sk %p, opt %d", sk, optname); + + if (get_user(len, optlen)) + return -EFAULT; + + lock_sock(sk); + + if (hci_pi(sk)->channel != HCI_CHANNEL_RAW) { + err = -EBADFD; + goto done; + } + + switch (optname) { + case HCI_DATA_DIR: + if (hci_pi(sk)->cmsg_mask & HCI_CMSG_DIR) + opt = 1; + else + opt = 0; + + if (put_user(opt, optval)) + err = -EFAULT; + break; + + case HCI_TIME_STAMP: + if (hci_pi(sk)->cmsg_mask & HCI_CMSG_TSTAMP) + opt = 1; + else + opt = 0; + + if (put_user(opt, optval)) + err = -EFAULT; + break; + + case HCI_FILTER: + { + struct hci_filter *f = &hci_pi(sk)->filter; + + memset(&uf, 0, sizeof(uf)); + uf.type_mask = f->type_mask; + uf.opcode = f->opcode; + uf.event_mask[0] = *((u32 *) f->event_mask + 0); + uf.event_mask[1] = *((u32 *) f->event_mask + 1); + } + + len = min_t(unsigned int, len, sizeof(uf)); + if (copy_to_user(optval, &uf, len)) + err = -EFAULT; + break; + + default: + err = -ENOPROTOOPT; + break; + } + +done: + release_sock(sk); + return err; +} + +static const struct proto_ops hci_sock_ops = { + .family = PF_BLUETOOTH, + .owner = THIS_MODULE, + .release = hci_sock_release, + .bind = hci_sock_bind, + .getname = hci_sock_getname, + .sendmsg = hci_sock_sendmsg, + .recvmsg = hci_sock_recvmsg, + .ioctl = hci_sock_ioctl, + .poll = datagram_poll, + .listen = sock_no_listen, + .shutdown = sock_no_shutdown, + .setsockopt = hci_sock_setsockopt, + .getsockopt = hci_sock_getsockopt, + .connect = sock_no_connect, + .socketpair = sock_no_socketpair, + .accept = sock_no_accept, + .mmap = sock_no_mmap +}; + +static struct proto hci_sk_proto = { + .name = "HCI", + .owner = THIS_MODULE, + .obj_size = sizeof(struct hci_pinfo) +}; + +static int hci_sock_create(struct net *net, struct socket *sock, int protocol, + int kern) +{ + struct sock *sk; + + BT_DBG("sock %p", sock); + + if (sock->type != SOCK_RAW) + return -ESOCKTNOSUPPORT; + + sock->ops = &hci_sock_ops; + + sk = sk_alloc(net, PF_BLUETOOTH, GFP_ATOMIC, &hci_sk_proto); + if (!sk) + return -ENOMEM; + + sock_init_data(sock, sk); + + sock_reset_flag(sk, SOCK_ZAPPED); + + sk->sk_protocol = protocol; + + sock->state = SS_UNCONNECTED; + sk->sk_state = BT_OPEN; + + bt_sock_link(&hci_sk_list, sk); + return 0; +} + +static const struct net_proto_family hci_sock_family_ops = { + .family = PF_BLUETOOTH, + .owner = THIS_MODULE, + .create = hci_sock_create, +}; + +int __init hci_sock_init(void) +{ + int err; + + BUILD_BUG_ON(sizeof(struct sockaddr_hci) > sizeof(struct sockaddr)); + + err = proto_register(&hci_sk_proto, 0); + if (err < 0) + return err; + + err = bt_sock_register(BTPROTO_HCI, &hci_sock_family_ops); + if (err < 0) { + BT_ERR("HCI socket registration failed"); + goto error; + } + + err = bt_procfs_init(&init_net, "hci", &hci_sk_list, NULL); + if (err < 0) { + BT_ERR("Failed to create HCI proc file"); + bt_sock_unregister(BTPROTO_HCI); + goto error; + } + + BT_INFO("HCI socket layer initialized"); + + return 0; + +error: + proto_unregister(&hci_sk_proto); + return err; +} + +void hci_sock_cleanup(void) +{ + bt_procfs_cleanup(&init_net, "hci"); + bt_sock_unregister(BTPROTO_HCI); + proto_unregister(&hci_sk_proto); +} diff --git a/kernel/net/bluetooth/hci_sysfs.c b/kernel/net/bluetooth/hci_sysfs.c new file mode 100644 index 000000000..555982a78 --- /dev/null +++ b/kernel/net/bluetooth/hci_sysfs.c @@ -0,0 +1,214 @@ +/* Bluetooth HCI driver model support. */ + +#include <linux/module.h> + +#include <net/bluetooth/bluetooth.h> +#include <net/bluetooth/hci_core.h> + +static struct class *bt_class; + +static inline char *link_typetostr(int type) +{ + switch (type) { + case ACL_LINK: + return "ACL"; + case SCO_LINK: + return "SCO"; + case ESCO_LINK: + return "eSCO"; + case LE_LINK: + return "LE"; + default: + return "UNKNOWN"; + } +} + +static ssize_t show_link_type(struct device *dev, + struct device_attribute *attr, char *buf) +{ + struct hci_conn *conn = to_hci_conn(dev); + return sprintf(buf, "%s\n", link_typetostr(conn->type)); +} + +static ssize_t show_link_address(struct device *dev, + struct device_attribute *attr, char *buf) +{ + struct hci_conn *conn = to_hci_conn(dev); + return sprintf(buf, "%pMR\n", &conn->dst); +} + +#define LINK_ATTR(_name, _mode, _show, _store) \ +struct device_attribute link_attr_##_name = __ATTR(_name, _mode, _show, _store) + +static LINK_ATTR(type, S_IRUGO, show_link_type, NULL); +static LINK_ATTR(address, S_IRUGO, show_link_address, NULL); + +static struct attribute *bt_link_attrs[] = { + &link_attr_type.attr, + &link_attr_address.attr, + NULL +}; + +ATTRIBUTE_GROUPS(bt_link); + +static void bt_link_release(struct device *dev) +{ + struct hci_conn *conn = to_hci_conn(dev); + kfree(conn); +} + +static struct device_type bt_link = { + .name = "link", + .groups = bt_link_groups, + .release = bt_link_release, +}; + +/* + * The rfcomm tty device will possibly retain even when conn + * is down, and sysfs doesn't support move zombie device, + * so we should move the device before conn device is destroyed. + */ +static int __match_tty(struct device *dev, void *data) +{ + return !strncmp(dev_name(dev), "rfcomm", 6); +} + +void hci_conn_init_sysfs(struct hci_conn *conn) +{ + struct hci_dev *hdev = conn->hdev; + + BT_DBG("conn %p", conn); + + conn->dev.type = &bt_link; + conn->dev.class = bt_class; + conn->dev.parent = &hdev->dev; + + device_initialize(&conn->dev); +} + +void hci_conn_add_sysfs(struct hci_conn *conn) +{ + struct hci_dev *hdev = conn->hdev; + + BT_DBG("conn %p", conn); + + dev_set_name(&conn->dev, "%s:%d", hdev->name, conn->handle); + + if (device_add(&conn->dev) < 0) { + BT_ERR("Failed to register connection device"); + return; + } + + hci_dev_hold(hdev); +} + +void hci_conn_del_sysfs(struct hci_conn *conn) +{ + struct hci_dev *hdev = conn->hdev; + + if (!device_is_registered(&conn->dev)) + return; + + while (1) { + struct device *dev; + + dev = device_find_child(&conn->dev, NULL, __match_tty); + if (!dev) + break; + device_move(dev, NULL, DPM_ORDER_DEV_LAST); + put_device(dev); + } + + device_del(&conn->dev); + + hci_dev_put(hdev); +} + +static inline char *host_typetostr(int type) +{ + switch (type) { + case HCI_BREDR: + return "BR/EDR"; + case HCI_AMP: + return "AMP"; + default: + return "UNKNOWN"; + } +} + +static ssize_t show_type(struct device *dev, + struct device_attribute *attr, char *buf) +{ + struct hci_dev *hdev = to_hci_dev(dev); + return sprintf(buf, "%s\n", host_typetostr(hdev->dev_type)); +} + +static ssize_t show_name(struct device *dev, + struct device_attribute *attr, char *buf) +{ + struct hci_dev *hdev = to_hci_dev(dev); + char name[HCI_MAX_NAME_LENGTH + 1]; + int i; + + for (i = 0; i < HCI_MAX_NAME_LENGTH; i++) + name[i] = hdev->dev_name[i]; + + name[HCI_MAX_NAME_LENGTH] = '\0'; + return sprintf(buf, "%s\n", name); +} + +static ssize_t show_address(struct device *dev, + struct device_attribute *attr, char *buf) +{ + struct hci_dev *hdev = to_hci_dev(dev); + return sprintf(buf, "%pMR\n", &hdev->bdaddr); +} + +static DEVICE_ATTR(type, S_IRUGO, show_type, NULL); +static DEVICE_ATTR(name, S_IRUGO, show_name, NULL); +static DEVICE_ATTR(address, S_IRUGO, show_address, NULL); + +static struct attribute *bt_host_attrs[] = { + &dev_attr_type.attr, + &dev_attr_name.attr, + &dev_attr_address.attr, + NULL +}; + +ATTRIBUTE_GROUPS(bt_host); + +static void bt_host_release(struct device *dev) +{ + struct hci_dev *hdev = to_hci_dev(dev); + kfree(hdev); + module_put(THIS_MODULE); +} + +static struct device_type bt_host = { + .name = "host", + .groups = bt_host_groups, + .release = bt_host_release, +}; + +void hci_init_sysfs(struct hci_dev *hdev) +{ + struct device *dev = &hdev->dev; + + dev->type = &bt_host; + dev->class = bt_class; + + __module_get(THIS_MODULE); + device_initialize(dev); +} + +int __init bt_sysfs_init(void) +{ + bt_class = class_create(THIS_MODULE, "bluetooth"); + + return PTR_ERR_OR_ZERO(bt_class); +} + +void bt_sysfs_cleanup(void) +{ + class_destroy(bt_class); +} diff --git a/kernel/net/bluetooth/hidp/Kconfig b/kernel/net/bluetooth/hidp/Kconfig new file mode 100644 index 000000000..bc8610b24 --- /dev/null +++ b/kernel/net/bluetooth/hidp/Kconfig @@ -0,0 +1,12 @@ +config BT_HIDP + tristate "HIDP protocol support" + depends on BT_BREDR && INPUT + select HID + help + HIDP (Human Interface Device Protocol) is a transport layer + for HID reports. HIDP is required for the Bluetooth Human + Interface Device Profile. + + Say Y here to compile HIDP support into the kernel or say M to + compile it as module (hidp). + diff --git a/kernel/net/bluetooth/hidp/Makefile b/kernel/net/bluetooth/hidp/Makefile new file mode 100644 index 000000000..a9ee11569 --- /dev/null +++ b/kernel/net/bluetooth/hidp/Makefile @@ -0,0 +1,7 @@ +# +# Makefile for the Linux Bluetooth HIDP layer +# + +obj-$(CONFIG_BT_HIDP) += hidp.o + +hidp-objs := core.o sock.o diff --git a/kernel/net/bluetooth/hidp/core.c b/kernel/net/bluetooth/hidp/core.c new file mode 100644 index 000000000..9070dfd6b --- /dev/null +++ b/kernel/net/bluetooth/hidp/core.c @@ -0,0 +1,1446 @@ +/* + HIDP implementation for Linux Bluetooth stack (BlueZ). + Copyright (C) 2003-2004 Marcel Holtmann <marcel@holtmann.org> + Copyright (C) 2013 David Herrmann <dh.herrmann@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; + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS + OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT OF THIRD PARTY RIGHTS. + IN NO EVENT SHALL THE COPYRIGHT HOLDER(S) AND AUTHOR(S) BE LIABLE FOR ANY + CLAIM, OR ANY SPECIAL INDIRECT OR CONSEQUENTIAL DAMAGES, OR ANY DAMAGES + WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN + ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF + OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + + ALL LIABILITY, INCLUDING LIABILITY FOR INFRINGEMENT OF ANY PATENTS, + COPYRIGHTS, TRADEMARKS OR OTHER RIGHTS, RELATING TO USE OF THIS + SOFTWARE IS DISCLAIMED. +*/ + +#include <linux/kref.h> +#include <linux/module.h> +#include <linux/file.h> +#include <linux/kthread.h> +#include <linux/hidraw.h> + +#include <net/bluetooth/bluetooth.h> +#include <net/bluetooth/hci_core.h> +#include <net/bluetooth/l2cap.h> + +#include "hidp.h" + +#define VERSION "1.2" + +static DECLARE_RWSEM(hidp_session_sem); +static LIST_HEAD(hidp_session_list); + +static unsigned char hidp_keycode[256] = { + 0, 0, 0, 0, 30, 48, 46, 32, 18, 33, 34, 35, 23, 36, + 37, 38, 50, 49, 24, 25, 16, 19, 31, 20, 22, 47, 17, 45, + 21, 44, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 28, 1, + 14, 15, 57, 12, 13, 26, 27, 43, 43, 39, 40, 41, 51, 52, + 53, 58, 59, 60, 61, 62, 63, 64, 65, 66, 67, 68, 87, 88, + 99, 70, 119, 110, 102, 104, 111, 107, 109, 106, 105, 108, 103, 69, + 98, 55, 74, 78, 96, 79, 80, 81, 75, 76, 77, 71, 72, 73, + 82, 83, 86, 127, 116, 117, 183, 184, 185, 186, 187, 188, 189, 190, + 191, 192, 193, 194, 134, 138, 130, 132, 128, 129, 131, 137, 133, 135, + 136, 113, 115, 114, 0, 0, 0, 121, 0, 89, 93, 124, 92, 94, + 95, 0, 0, 0, 122, 123, 90, 91, 85, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 29, 42, 56, 125, 97, 54, 100, 126, 164, 166, 165, 163, 161, 115, + 114, 113, 150, 158, 159, 128, 136, 177, 178, 176, 142, 152, 173, 140 +}; + +static unsigned char hidp_mkeyspat[] = { 0x01, 0x01, 0x01, 0x01, 0x01, 0x01 }; + +static int hidp_session_probe(struct l2cap_conn *conn, + struct l2cap_user *user); +static void hidp_session_remove(struct l2cap_conn *conn, + struct l2cap_user *user); +static int hidp_session_thread(void *arg); +static void hidp_session_terminate(struct hidp_session *s); + +static void hidp_copy_session(struct hidp_session *session, struct hidp_conninfo *ci) +{ + u32 valid_flags = 0; + memset(ci, 0, sizeof(*ci)); + bacpy(&ci->bdaddr, &session->bdaddr); + + ci->flags = session->flags & valid_flags; + ci->state = BT_CONNECTED; + + if (session->input) { + ci->vendor = session->input->id.vendor; + ci->product = session->input->id.product; + ci->version = session->input->id.version; + if (session->input->name) + strlcpy(ci->name, session->input->name, 128); + else + strlcpy(ci->name, "HID Boot Device", 128); + } else if (session->hid) { + ci->vendor = session->hid->vendor; + ci->product = session->hid->product; + ci->version = session->hid->version; + strlcpy(ci->name, session->hid->name, 128); + } +} + +/* assemble skb, queue message on @transmit and wake up the session thread */ +static int hidp_send_message(struct hidp_session *session, struct socket *sock, + struct sk_buff_head *transmit, unsigned char hdr, + const unsigned char *data, int size) +{ + struct sk_buff *skb; + struct sock *sk = sock->sk; + + BT_DBG("session %p data %p size %d", session, data, size); + + if (atomic_read(&session->terminate)) + return -EIO; + + skb = alloc_skb(size + 1, GFP_ATOMIC); + if (!skb) { + BT_ERR("Can't allocate memory for new frame"); + return -ENOMEM; + } + + *skb_put(skb, 1) = hdr; + if (data && size > 0) + memcpy(skb_put(skb, size), data, size); + + skb_queue_tail(transmit, skb); + wake_up_interruptible(sk_sleep(sk)); + + return 0; +} + +static int hidp_send_ctrl_message(struct hidp_session *session, + unsigned char hdr, const unsigned char *data, + int size) +{ + return hidp_send_message(session, session->ctrl_sock, + &session->ctrl_transmit, hdr, data, size); +} + +static int hidp_send_intr_message(struct hidp_session *session, + unsigned char hdr, const unsigned char *data, + int size) +{ + return hidp_send_message(session, session->intr_sock, + &session->intr_transmit, hdr, data, size); +} + +static int hidp_input_event(struct input_dev *dev, unsigned int type, + unsigned int code, int value) +{ + struct hidp_session *session = input_get_drvdata(dev); + unsigned char newleds; + unsigned char hdr, data[2]; + + BT_DBG("session %p type %d code %d value %d", + session, type, code, value); + + if (type != EV_LED) + return -1; + + newleds = (!!test_bit(LED_KANA, dev->led) << 3) | + (!!test_bit(LED_COMPOSE, dev->led) << 3) | + (!!test_bit(LED_SCROLLL, dev->led) << 2) | + (!!test_bit(LED_CAPSL, dev->led) << 1) | + (!!test_bit(LED_NUML, dev->led) << 0); + + if (session->leds == newleds) + return 0; + + session->leds = newleds; + + hdr = HIDP_TRANS_DATA | HIDP_DATA_RTYPE_OUPUT; + data[0] = 0x01; + data[1] = newleds; + + return hidp_send_intr_message(session, hdr, data, 2); +} + +static void hidp_input_report(struct hidp_session *session, struct sk_buff *skb) +{ + struct input_dev *dev = session->input; + unsigned char *keys = session->keys; + unsigned char *udata = skb->data + 1; + signed char *sdata = skb->data + 1; + int i, size = skb->len - 1; + + switch (skb->data[0]) { + case 0x01: /* Keyboard report */ + for (i = 0; i < 8; i++) + input_report_key(dev, hidp_keycode[i + 224], (udata[0] >> i) & 1); + + /* If all the key codes have been set to 0x01, it means + * too many keys were pressed at the same time. */ + if (!memcmp(udata + 2, hidp_mkeyspat, 6)) + break; + + for (i = 2; i < 8; i++) { + if (keys[i] > 3 && memscan(udata + 2, keys[i], 6) == udata + 8) { + if (hidp_keycode[keys[i]]) + input_report_key(dev, hidp_keycode[keys[i]], 0); + else + BT_ERR("Unknown key (scancode %#x) released.", keys[i]); + } + + if (udata[i] > 3 && memscan(keys + 2, udata[i], 6) == keys + 8) { + if (hidp_keycode[udata[i]]) + input_report_key(dev, hidp_keycode[udata[i]], 1); + else + BT_ERR("Unknown key (scancode %#x) pressed.", udata[i]); + } + } + + memcpy(keys, udata, 8); + break; + + case 0x02: /* Mouse report */ + input_report_key(dev, BTN_LEFT, sdata[0] & 0x01); + input_report_key(dev, BTN_RIGHT, sdata[0] & 0x02); + input_report_key(dev, BTN_MIDDLE, sdata[0] & 0x04); + input_report_key(dev, BTN_SIDE, sdata[0] & 0x08); + input_report_key(dev, BTN_EXTRA, sdata[0] & 0x10); + + input_report_rel(dev, REL_X, sdata[1]); + input_report_rel(dev, REL_Y, sdata[2]); + + if (size > 3) + input_report_rel(dev, REL_WHEEL, sdata[3]); + break; + } + + input_sync(dev); +} + +static int hidp_get_raw_report(struct hid_device *hid, + unsigned char report_number, + unsigned char *data, size_t count, + unsigned char report_type) +{ + struct hidp_session *session = hid->driver_data; + struct sk_buff *skb; + size_t len; + int numbered_reports = hid->report_enum[report_type].numbered; + int ret; + + if (atomic_read(&session->terminate)) + return -EIO; + + switch (report_type) { + case HID_FEATURE_REPORT: + report_type = HIDP_TRANS_GET_REPORT | HIDP_DATA_RTYPE_FEATURE; + break; + case HID_INPUT_REPORT: + report_type = HIDP_TRANS_GET_REPORT | HIDP_DATA_RTYPE_INPUT; + break; + case HID_OUTPUT_REPORT: + report_type = HIDP_TRANS_GET_REPORT | HIDP_DATA_RTYPE_OUPUT; + break; + default: + return -EINVAL; + } + + if (mutex_lock_interruptible(&session->report_mutex)) + return -ERESTARTSYS; + + /* Set up our wait, and send the report request to the device. */ + session->waiting_report_type = report_type & HIDP_DATA_RTYPE_MASK; + session->waiting_report_number = numbered_reports ? report_number : -1; + set_bit(HIDP_WAITING_FOR_RETURN, &session->flags); + data[0] = report_number; + ret = hidp_send_ctrl_message(session, report_type, data, 1); + if (ret) + goto err; + + /* Wait for the return of the report. The returned report + gets put in session->report_return. */ + while (test_bit(HIDP_WAITING_FOR_RETURN, &session->flags) && + !atomic_read(&session->terminate)) { + int res; + + res = wait_event_interruptible_timeout(session->report_queue, + !test_bit(HIDP_WAITING_FOR_RETURN, &session->flags) + || atomic_read(&session->terminate), + 5*HZ); + if (res == 0) { + /* timeout */ + ret = -EIO; + goto err; + } + if (res < 0) { + /* signal */ + ret = -ERESTARTSYS; + goto err; + } + } + + skb = session->report_return; + if (skb) { + len = skb->len < count ? skb->len : count; + memcpy(data, skb->data, len); + + kfree_skb(skb); + session->report_return = NULL; + } else { + /* Device returned a HANDSHAKE, indicating protocol error. */ + len = -EIO; + } + + clear_bit(HIDP_WAITING_FOR_RETURN, &session->flags); + mutex_unlock(&session->report_mutex); + + return len; + +err: + clear_bit(HIDP_WAITING_FOR_RETURN, &session->flags); + mutex_unlock(&session->report_mutex); + return ret; +} + +static int hidp_set_raw_report(struct hid_device *hid, unsigned char reportnum, + unsigned char *data, size_t count, + unsigned char report_type) +{ + struct hidp_session *session = hid->driver_data; + int ret; + + switch (report_type) { + case HID_FEATURE_REPORT: + report_type = HIDP_TRANS_SET_REPORT | HIDP_DATA_RTYPE_FEATURE; + break; + case HID_INPUT_REPORT: + report_type = HIDP_TRANS_SET_REPORT | HIDP_DATA_RTYPE_INPUT; + break; + case HID_OUTPUT_REPORT: + report_type = HIDP_TRANS_SET_REPORT | HIDP_DATA_RTYPE_OUPUT; + break; + default: + return -EINVAL; + } + + if (mutex_lock_interruptible(&session->report_mutex)) + return -ERESTARTSYS; + + /* Set up our wait, and send the report request to the device. */ + data[0] = reportnum; + set_bit(HIDP_WAITING_FOR_SEND_ACK, &session->flags); + ret = hidp_send_ctrl_message(session, report_type, data, count); + if (ret) + goto err; + + /* Wait for the ACK from the device. */ + while (test_bit(HIDP_WAITING_FOR_SEND_ACK, &session->flags) && + !atomic_read(&session->terminate)) { + int res; + + res = wait_event_interruptible_timeout(session->report_queue, + !test_bit(HIDP_WAITING_FOR_SEND_ACK, &session->flags) + || atomic_read(&session->terminate), + 10*HZ); + if (res == 0) { + /* timeout */ + ret = -EIO; + goto err; + } + if (res < 0) { + /* signal */ + ret = -ERESTARTSYS; + goto err; + } + } + + if (!session->output_report_success) { + ret = -EIO; + goto err; + } + + ret = count; + +err: + clear_bit(HIDP_WAITING_FOR_SEND_ACK, &session->flags); + mutex_unlock(&session->report_mutex); + return ret; +} + +static int hidp_output_report(struct hid_device *hid, __u8 *data, size_t count) +{ + struct hidp_session *session = hid->driver_data; + + return hidp_send_intr_message(session, + HIDP_TRANS_DATA | HIDP_DATA_RTYPE_OUPUT, + data, count); +} + +static int hidp_raw_request(struct hid_device *hid, unsigned char reportnum, + __u8 *buf, size_t len, unsigned char rtype, + int reqtype) +{ + switch (reqtype) { + case HID_REQ_GET_REPORT: + return hidp_get_raw_report(hid, reportnum, buf, len, rtype); + case HID_REQ_SET_REPORT: + return hidp_set_raw_report(hid, reportnum, buf, len, rtype); + default: + return -EIO; + } +} + +static void hidp_idle_timeout(unsigned long arg) +{ + struct hidp_session *session = (struct hidp_session *) arg; + + hidp_session_terminate(session); +} + +static void hidp_set_timer(struct hidp_session *session) +{ + if (session->idle_to > 0) + mod_timer(&session->timer, jiffies + HZ * session->idle_to); +} + +static void hidp_del_timer(struct hidp_session *session) +{ + if (session->idle_to > 0) + del_timer(&session->timer); +} + +static void hidp_process_report(struct hidp_session *session, + int type, const u8 *data, int len, int intr) +{ + if (len > HID_MAX_BUFFER_SIZE) + len = HID_MAX_BUFFER_SIZE; + + memcpy(session->input_buf, data, len); + hid_input_report(session->hid, type, session->input_buf, len, intr); +} + +static void hidp_process_handshake(struct hidp_session *session, + unsigned char param) +{ + BT_DBG("session %p param 0x%02x", session, param); + session->output_report_success = 0; /* default condition */ + + switch (param) { + case HIDP_HSHK_SUCCESSFUL: + /* FIXME: Call into SET_ GET_ handlers here */ + session->output_report_success = 1; + break; + + case HIDP_HSHK_NOT_READY: + case HIDP_HSHK_ERR_INVALID_REPORT_ID: + case HIDP_HSHK_ERR_UNSUPPORTED_REQUEST: + case HIDP_HSHK_ERR_INVALID_PARAMETER: + if (test_and_clear_bit(HIDP_WAITING_FOR_RETURN, &session->flags)) + wake_up_interruptible(&session->report_queue); + + /* FIXME: Call into SET_ GET_ handlers here */ + break; + + case HIDP_HSHK_ERR_UNKNOWN: + break; + + case HIDP_HSHK_ERR_FATAL: + /* Device requests a reboot, as this is the only way this error + * can be recovered. */ + hidp_send_ctrl_message(session, + HIDP_TRANS_HID_CONTROL | HIDP_CTRL_SOFT_RESET, NULL, 0); + break; + + default: + hidp_send_ctrl_message(session, + HIDP_TRANS_HANDSHAKE | HIDP_HSHK_ERR_INVALID_PARAMETER, NULL, 0); + break; + } + + /* Wake up the waiting thread. */ + if (test_and_clear_bit(HIDP_WAITING_FOR_SEND_ACK, &session->flags)) + wake_up_interruptible(&session->report_queue); +} + +static void hidp_process_hid_control(struct hidp_session *session, + unsigned char param) +{ + BT_DBG("session %p param 0x%02x", session, param); + + if (param == HIDP_CTRL_VIRTUAL_CABLE_UNPLUG) { + /* Flush the transmit queues */ + skb_queue_purge(&session->ctrl_transmit); + skb_queue_purge(&session->intr_transmit); + + hidp_session_terminate(session); + } +} + +/* Returns true if the passed-in skb should be freed by the caller. */ +static int hidp_process_data(struct hidp_session *session, struct sk_buff *skb, + unsigned char param) +{ + int done_with_skb = 1; + BT_DBG("session %p skb %p len %d param 0x%02x", session, skb, skb->len, param); + + switch (param) { + case HIDP_DATA_RTYPE_INPUT: + hidp_set_timer(session); + + if (session->input) + hidp_input_report(session, skb); + + if (session->hid) + hidp_process_report(session, HID_INPUT_REPORT, + skb->data, skb->len, 0); + break; + + case HIDP_DATA_RTYPE_OTHER: + case HIDP_DATA_RTYPE_OUPUT: + case HIDP_DATA_RTYPE_FEATURE: + break; + + default: + hidp_send_ctrl_message(session, + HIDP_TRANS_HANDSHAKE | HIDP_HSHK_ERR_INVALID_PARAMETER, NULL, 0); + } + + if (test_bit(HIDP_WAITING_FOR_RETURN, &session->flags) && + param == session->waiting_report_type) { + if (session->waiting_report_number < 0 || + session->waiting_report_number == skb->data[0]) { + /* hidp_get_raw_report() is waiting on this report. */ + session->report_return = skb; + done_with_skb = 0; + clear_bit(HIDP_WAITING_FOR_RETURN, &session->flags); + wake_up_interruptible(&session->report_queue); + } + } + + return done_with_skb; +} + +static void hidp_recv_ctrl_frame(struct hidp_session *session, + struct sk_buff *skb) +{ + unsigned char hdr, type, param; + int free_skb = 1; + + BT_DBG("session %p skb %p len %d", session, skb, skb->len); + + hdr = skb->data[0]; + skb_pull(skb, 1); + + type = hdr & HIDP_HEADER_TRANS_MASK; + param = hdr & HIDP_HEADER_PARAM_MASK; + + switch (type) { + case HIDP_TRANS_HANDSHAKE: + hidp_process_handshake(session, param); + break; + + case HIDP_TRANS_HID_CONTROL: + hidp_process_hid_control(session, param); + break; + + case HIDP_TRANS_DATA: + free_skb = hidp_process_data(session, skb, param); + break; + + default: + hidp_send_ctrl_message(session, + HIDP_TRANS_HANDSHAKE | HIDP_HSHK_ERR_UNSUPPORTED_REQUEST, NULL, 0); + break; + } + + if (free_skb) + kfree_skb(skb); +} + +static void hidp_recv_intr_frame(struct hidp_session *session, + struct sk_buff *skb) +{ + unsigned char hdr; + + BT_DBG("session %p skb %p len %d", session, skb, skb->len); + + hdr = skb->data[0]; + skb_pull(skb, 1); + + if (hdr == (HIDP_TRANS_DATA | HIDP_DATA_RTYPE_INPUT)) { + hidp_set_timer(session); + + if (session->input) + hidp_input_report(session, skb); + + if (session->hid) { + hidp_process_report(session, HID_INPUT_REPORT, + skb->data, skb->len, 1); + BT_DBG("report len %d", skb->len); + } + } else { + BT_DBG("Unsupported protocol header 0x%02x", hdr); + } + + kfree_skb(skb); +} + +static int hidp_send_frame(struct socket *sock, unsigned char *data, int len) +{ + struct kvec iv = { data, len }; + struct msghdr msg; + + BT_DBG("sock %p data %p len %d", sock, data, len); + + if (!len) + return 0; + + memset(&msg, 0, sizeof(msg)); + + return kernel_sendmsg(sock, &msg, &iv, 1, len); +} + +/* dequeue message from @transmit and send via @sock */ +static void hidp_process_transmit(struct hidp_session *session, + struct sk_buff_head *transmit, + struct socket *sock) +{ + struct sk_buff *skb; + int ret; + + BT_DBG("session %p", session); + + while ((skb = skb_dequeue(transmit))) { + ret = hidp_send_frame(sock, skb->data, skb->len); + if (ret == -EAGAIN) { + skb_queue_head(transmit, skb); + break; + } else if (ret < 0) { + hidp_session_terminate(session); + kfree_skb(skb); + break; + } + + hidp_set_timer(session); + kfree_skb(skb); + } +} + +static int hidp_setup_input(struct hidp_session *session, + struct hidp_connadd_req *req) +{ + struct input_dev *input; + int i; + + input = input_allocate_device(); + if (!input) + return -ENOMEM; + + session->input = input; + + input_set_drvdata(input, session); + + input->name = "Bluetooth HID Boot Protocol Device"; + + input->id.bustype = BUS_BLUETOOTH; + input->id.vendor = req->vendor; + input->id.product = req->product; + input->id.version = req->version; + + if (req->subclass & 0x40) { + set_bit(EV_KEY, input->evbit); + set_bit(EV_LED, input->evbit); + set_bit(EV_REP, input->evbit); + + set_bit(LED_NUML, input->ledbit); + set_bit(LED_CAPSL, input->ledbit); + set_bit(LED_SCROLLL, input->ledbit); + set_bit(LED_COMPOSE, input->ledbit); + set_bit(LED_KANA, input->ledbit); + + for (i = 0; i < sizeof(hidp_keycode); i++) + set_bit(hidp_keycode[i], input->keybit); + clear_bit(0, input->keybit); + } + + if (req->subclass & 0x80) { + input->evbit[0] = BIT_MASK(EV_KEY) | BIT_MASK(EV_REL); + input->keybit[BIT_WORD(BTN_MOUSE)] = BIT_MASK(BTN_LEFT) | + BIT_MASK(BTN_RIGHT) | BIT_MASK(BTN_MIDDLE); + input->relbit[0] = BIT_MASK(REL_X) | BIT_MASK(REL_Y); + input->keybit[BIT_WORD(BTN_MOUSE)] |= BIT_MASK(BTN_SIDE) | + BIT_MASK(BTN_EXTRA); + input->relbit[0] |= BIT_MASK(REL_WHEEL); + } + + input->dev.parent = &session->conn->hcon->dev; + + input->event = hidp_input_event; + + return 0; +} + +static int hidp_open(struct hid_device *hid) +{ + return 0; +} + +static void hidp_close(struct hid_device *hid) +{ +} + +static int hidp_parse(struct hid_device *hid) +{ + struct hidp_session *session = hid->driver_data; + + return hid_parse_report(session->hid, session->rd_data, + session->rd_size); +} + +static int hidp_start(struct hid_device *hid) +{ + return 0; +} + +static void hidp_stop(struct hid_device *hid) +{ + struct hidp_session *session = hid->driver_data; + + skb_queue_purge(&session->ctrl_transmit); + skb_queue_purge(&session->intr_transmit); + + hid->claimed = 0; +} + +static struct hid_ll_driver hidp_hid_driver = { + .parse = hidp_parse, + .start = hidp_start, + .stop = hidp_stop, + .open = hidp_open, + .close = hidp_close, + .raw_request = hidp_raw_request, + .output_report = hidp_output_report, +}; + +/* This function sets up the hid device. It does not add it + to the HID system. That is done in hidp_add_connection(). */ +static int hidp_setup_hid(struct hidp_session *session, + struct hidp_connadd_req *req) +{ + struct hid_device *hid; + int err; + + session->rd_data = memdup_user(req->rd_data, req->rd_size); + if (IS_ERR(session->rd_data)) + return PTR_ERR(session->rd_data); + + session->rd_size = req->rd_size; + + hid = hid_allocate_device(); + if (IS_ERR(hid)) { + err = PTR_ERR(hid); + goto fault; + } + + session->hid = hid; + + hid->driver_data = session; + + hid->bus = BUS_BLUETOOTH; + hid->vendor = req->vendor; + hid->product = req->product; + hid->version = req->version; + hid->country = req->country; + + strncpy(hid->name, req->name, sizeof(req->name) - 1); + + snprintf(hid->phys, sizeof(hid->phys), "%pMR", + &l2cap_pi(session->ctrl_sock->sk)->chan->src); + + /* NOTE: Some device modules depend on the dst address being stored in + * uniq. Please be aware of this before making changes to this behavior. + */ + snprintf(hid->uniq, sizeof(hid->uniq), "%pMR", + &l2cap_pi(session->ctrl_sock->sk)->chan->dst); + + hid->dev.parent = &session->conn->hcon->dev; + hid->ll_driver = &hidp_hid_driver; + + /* True if device is blacklisted in drivers/hid/hid-core.c */ + if (hid_ignore(hid)) { + hid_destroy_device(session->hid); + session->hid = NULL; + return -ENODEV; + } + + return 0; + +fault: + kfree(session->rd_data); + session->rd_data = NULL; + + return err; +} + +/* initialize session devices */ +static int hidp_session_dev_init(struct hidp_session *session, + struct hidp_connadd_req *req) +{ + int ret; + + if (req->rd_size > 0) { + ret = hidp_setup_hid(session, req); + if (ret && ret != -ENODEV) + return ret; + } + + if (!session->hid) { + ret = hidp_setup_input(session, req); + if (ret < 0) + return ret; + } + + return 0; +} + +/* destroy session devices */ +static void hidp_session_dev_destroy(struct hidp_session *session) +{ + if (session->hid) + put_device(&session->hid->dev); + else if (session->input) + input_put_device(session->input); + + kfree(session->rd_data); + session->rd_data = NULL; +} + +/* add HID/input devices to their underlying bus systems */ +static int hidp_session_dev_add(struct hidp_session *session) +{ + int ret; + + /* Both HID and input systems drop a ref-count when unregistering the + * device but they don't take a ref-count when registering them. Work + * around this by explicitly taking a refcount during registration + * which is dropped automatically by unregistering the devices. */ + + if (session->hid) { + ret = hid_add_device(session->hid); + if (ret) + return ret; + get_device(&session->hid->dev); + } else if (session->input) { + ret = input_register_device(session->input); + if (ret) + return ret; + input_get_device(session->input); + } + + return 0; +} + +/* remove HID/input devices from their bus systems */ +static void hidp_session_dev_del(struct hidp_session *session) +{ + if (session->hid) + hid_destroy_device(session->hid); + else if (session->input) + input_unregister_device(session->input); +} + +/* + * Asynchronous device registration + * HID device drivers might want to perform I/O during initialization to + * detect device types. Therefore, call device registration in a separate + * worker so the HIDP thread can schedule I/O operations. + * Note that this must be called after the worker thread was initialized + * successfully. This will then add the devices and increase session state + * on success, otherwise it will terminate the session thread. + */ +static void hidp_session_dev_work(struct work_struct *work) +{ + struct hidp_session *session = container_of(work, + struct hidp_session, + dev_init); + int ret; + + ret = hidp_session_dev_add(session); + if (!ret) + atomic_inc(&session->state); + else + hidp_session_terminate(session); +} + +/* + * Create new session object + * Allocate session object, initialize static fields, copy input data into the + * object and take a reference to all sub-objects. + * This returns 0 on success and puts a pointer to the new session object in + * \out. Otherwise, an error code is returned. + * The new session object has an initial ref-count of 1. + */ +static int hidp_session_new(struct hidp_session **out, const bdaddr_t *bdaddr, + struct socket *ctrl_sock, + struct socket *intr_sock, + struct hidp_connadd_req *req, + struct l2cap_conn *conn) +{ + struct hidp_session *session; + int ret; + struct bt_sock *ctrl, *intr; + + ctrl = bt_sk(ctrl_sock->sk); + intr = bt_sk(intr_sock->sk); + + session = kzalloc(sizeof(*session), GFP_KERNEL); + if (!session) + return -ENOMEM; + + /* object and runtime management */ + kref_init(&session->ref); + atomic_set(&session->state, HIDP_SESSION_IDLING); + init_waitqueue_head(&session->state_queue); + session->flags = req->flags & BIT(HIDP_BLUETOOTH_VENDOR_ID); + + /* connection management */ + bacpy(&session->bdaddr, bdaddr); + session->conn = l2cap_conn_get(conn); + session->user.probe = hidp_session_probe; + session->user.remove = hidp_session_remove; + session->ctrl_sock = ctrl_sock; + session->intr_sock = intr_sock; + skb_queue_head_init(&session->ctrl_transmit); + skb_queue_head_init(&session->intr_transmit); + session->ctrl_mtu = min_t(uint, l2cap_pi(ctrl)->chan->omtu, + l2cap_pi(ctrl)->chan->imtu); + session->intr_mtu = min_t(uint, l2cap_pi(intr)->chan->omtu, + l2cap_pi(intr)->chan->imtu); + session->idle_to = req->idle_to; + + /* device management */ + INIT_WORK(&session->dev_init, hidp_session_dev_work); + setup_timer(&session->timer, hidp_idle_timeout, + (unsigned long)session); + + /* session data */ + mutex_init(&session->report_mutex); + init_waitqueue_head(&session->report_queue); + + ret = hidp_session_dev_init(session, req); + if (ret) + goto err_free; + + get_file(session->intr_sock->file); + get_file(session->ctrl_sock->file); + *out = session; + return 0; + +err_free: + l2cap_conn_put(session->conn); + kfree(session); + return ret; +} + +/* increase ref-count of the given session by one */ +static void hidp_session_get(struct hidp_session *session) +{ + kref_get(&session->ref); +} + +/* release callback */ +static void session_free(struct kref *ref) +{ + struct hidp_session *session = container_of(ref, struct hidp_session, + ref); + + hidp_session_dev_destroy(session); + skb_queue_purge(&session->ctrl_transmit); + skb_queue_purge(&session->intr_transmit); + fput(session->intr_sock->file); + fput(session->ctrl_sock->file); + l2cap_conn_put(session->conn); + kfree(session); +} + +/* decrease ref-count of the given session by one */ +static void hidp_session_put(struct hidp_session *session) +{ + kref_put(&session->ref, session_free); +} + +/* + * Search the list of active sessions for a session with target address + * \bdaddr. You must hold at least a read-lock on \hidp_session_sem. As long as + * you do not release this lock, the session objects cannot vanish and you can + * safely take a reference to the session yourself. + */ +static struct hidp_session *__hidp_session_find(const bdaddr_t *bdaddr) +{ + struct hidp_session *session; + + list_for_each_entry(session, &hidp_session_list, list) { + if (!bacmp(bdaddr, &session->bdaddr)) + return session; + } + + return NULL; +} + +/* + * Same as __hidp_session_find() but no locks must be held. This also takes a + * reference of the returned session (if non-NULL) so you must drop this + * reference if you no longer use the object. + */ +static struct hidp_session *hidp_session_find(const bdaddr_t *bdaddr) +{ + struct hidp_session *session; + + down_read(&hidp_session_sem); + + session = __hidp_session_find(bdaddr); + if (session) + hidp_session_get(session); + + up_read(&hidp_session_sem); + + return session; +} + +/* + * Start session synchronously + * This starts a session thread and waits until initialization + * is done or returns an error if it couldn't be started. + * If this returns 0 the session thread is up and running. You must call + * hipd_session_stop_sync() before deleting any runtime resources. + */ +static int hidp_session_start_sync(struct hidp_session *session) +{ + unsigned int vendor, product; + + if (session->hid) { + vendor = session->hid->vendor; + product = session->hid->product; + } else if (session->input) { + vendor = session->input->id.vendor; + product = session->input->id.product; + } else { + vendor = 0x0000; + product = 0x0000; + } + + session->task = kthread_run(hidp_session_thread, session, + "khidpd_%04x%04x", vendor, product); + if (IS_ERR(session->task)) + return PTR_ERR(session->task); + + while (atomic_read(&session->state) <= HIDP_SESSION_IDLING) + wait_event(session->state_queue, + atomic_read(&session->state) > HIDP_SESSION_IDLING); + + return 0; +} + +/* + * Terminate session thread + * Wake up session thread and notify it to stop. This is asynchronous and + * returns immediately. Call this whenever a runtime error occurs and you want + * the session to stop. + * Note: wake_up_process() performs any necessary memory-barriers for us. + */ +static void hidp_session_terminate(struct hidp_session *session) +{ + atomic_inc(&session->terminate); + wake_up_process(session->task); +} + +/* + * Probe HIDP session + * This is called from the l2cap_conn core when our l2cap_user object is bound + * to the hci-connection. We get the session via the \user object and can now + * start the session thread, link it into the global session list and + * schedule HID/input device registration. + * The global session-list owns its own reference to the session object so you + * can drop your own reference after registering the l2cap_user object. + */ +static int hidp_session_probe(struct l2cap_conn *conn, + struct l2cap_user *user) +{ + struct hidp_session *session = container_of(user, + struct hidp_session, + user); + struct hidp_session *s; + int ret; + + down_write(&hidp_session_sem); + + /* check that no other session for this device exists */ + s = __hidp_session_find(&session->bdaddr); + if (s) { + ret = -EEXIST; + goto out_unlock; + } + + if (session->input) { + ret = hidp_session_dev_add(session); + if (ret) + goto out_unlock; + } + + ret = hidp_session_start_sync(session); + if (ret) + goto out_del; + + /* HID device registration is async to allow I/O during probe */ + if (session->input) + atomic_inc(&session->state); + else + schedule_work(&session->dev_init); + + hidp_session_get(session); + list_add(&session->list, &hidp_session_list); + ret = 0; + goto out_unlock; + +out_del: + if (session->input) + hidp_session_dev_del(session); +out_unlock: + up_write(&hidp_session_sem); + return ret; +} + +/* + * Remove HIDP session + * Called from the l2cap_conn core when either we explicitly unregistered + * the l2cap_user object or if the underlying connection is shut down. + * We signal the hidp-session thread to shut down, unregister the HID/input + * devices and unlink the session from the global list. + * This drops the reference to the session that is owned by the global + * session-list. + * Note: We _must_ not synchronosly wait for the session-thread to shut down. + * This is, because the session-thread might be waiting for an HCI lock that is + * held while we are called. Therefore, we only unregister the devices and + * notify the session-thread to terminate. The thread itself owns a reference + * to the session object so it can safely shut down. + */ +static void hidp_session_remove(struct l2cap_conn *conn, + struct l2cap_user *user) +{ + struct hidp_session *session = container_of(user, + struct hidp_session, + user); + + down_write(&hidp_session_sem); + + hidp_session_terminate(session); + + cancel_work_sync(&session->dev_init); + if (session->input || + atomic_read(&session->state) > HIDP_SESSION_PREPARING) + hidp_session_dev_del(session); + + list_del(&session->list); + + up_write(&hidp_session_sem); + + hidp_session_put(session); +} + +/* + * Session Worker + * This performs the actual main-loop of the HIDP worker. We first check + * whether the underlying connection is still alive, then parse all pending + * messages and finally send all outstanding messages. + */ +static void hidp_session_run(struct hidp_session *session) +{ + struct sock *ctrl_sk = session->ctrl_sock->sk; + struct sock *intr_sk = session->intr_sock->sk; + struct sk_buff *skb; + + for (;;) { + /* + * This thread can be woken up two ways: + * - You call hidp_session_terminate() which sets the + * session->terminate flag and wakes this thread up. + * - Via modifying the socket state of ctrl/intr_sock. This + * thread is woken up by ->sk_state_changed(). + * + * Note: set_current_state() performs any necessary + * memory-barriers for us. + */ + set_current_state(TASK_INTERRUPTIBLE); + + if (atomic_read(&session->terminate)) + break; + + if (ctrl_sk->sk_state != BT_CONNECTED || + intr_sk->sk_state != BT_CONNECTED) + break; + + /* parse incoming intr-skbs */ + while ((skb = skb_dequeue(&intr_sk->sk_receive_queue))) { + skb_orphan(skb); + if (!skb_linearize(skb)) + hidp_recv_intr_frame(session, skb); + else + kfree_skb(skb); + } + + /* send pending intr-skbs */ + hidp_process_transmit(session, &session->intr_transmit, + session->intr_sock); + + /* parse incoming ctrl-skbs */ + while ((skb = skb_dequeue(&ctrl_sk->sk_receive_queue))) { + skb_orphan(skb); + if (!skb_linearize(skb)) + hidp_recv_ctrl_frame(session, skb); + else + kfree_skb(skb); + } + + /* send pending ctrl-skbs */ + hidp_process_transmit(session, &session->ctrl_transmit, + session->ctrl_sock); + + schedule(); + } + + atomic_inc(&session->terminate); + set_current_state(TASK_RUNNING); +} + +/* + * HIDP session thread + * This thread runs the I/O for a single HIDP session. Startup is synchronous + * which allows us to take references to ourself here instead of doing that in + * the caller. + * When we are ready to run we notify the caller and call hidp_session_run(). + */ +static int hidp_session_thread(void *arg) +{ + struct hidp_session *session = arg; + wait_queue_t ctrl_wait, intr_wait; + + BT_DBG("session %p", session); + + /* initialize runtime environment */ + hidp_session_get(session); + __module_get(THIS_MODULE); + set_user_nice(current, -15); + hidp_set_timer(session); + + init_waitqueue_entry(&ctrl_wait, current); + init_waitqueue_entry(&intr_wait, current); + add_wait_queue(sk_sleep(session->ctrl_sock->sk), &ctrl_wait); + add_wait_queue(sk_sleep(session->intr_sock->sk), &intr_wait); + /* This memory barrier is paired with wq_has_sleeper(). See + * sock_poll_wait() for more information why this is needed. */ + smp_mb(); + + /* notify synchronous startup that we're ready */ + atomic_inc(&session->state); + wake_up(&session->state_queue); + + /* run session */ + hidp_session_run(session); + + /* cleanup runtime environment */ + remove_wait_queue(sk_sleep(session->intr_sock->sk), &intr_wait); + remove_wait_queue(sk_sleep(session->intr_sock->sk), &ctrl_wait); + wake_up_interruptible(&session->report_queue); + hidp_del_timer(session); + + /* + * If we stopped ourself due to any internal signal, we should try to + * unregister our own session here to avoid having it linger until the + * parent l2cap_conn dies or user-space cleans it up. + * This does not deadlock as we don't do any synchronous shutdown. + * Instead, this call has the same semantics as if user-space tried to + * delete the session. + */ + l2cap_unregister_user(session->conn, &session->user); + hidp_session_put(session); + + module_put_and_exit(0); + return 0; +} + +static int hidp_verify_sockets(struct socket *ctrl_sock, + struct socket *intr_sock) +{ + struct l2cap_chan *ctrl_chan, *intr_chan; + struct bt_sock *ctrl, *intr; + struct hidp_session *session; + + if (!l2cap_is_socket(ctrl_sock) || !l2cap_is_socket(intr_sock)) + return -EINVAL; + + ctrl_chan = l2cap_pi(ctrl_sock->sk)->chan; + intr_chan = l2cap_pi(intr_sock->sk)->chan; + + if (bacmp(&ctrl_chan->src, &intr_chan->src) || + bacmp(&ctrl_chan->dst, &intr_chan->dst)) + return -ENOTUNIQ; + + ctrl = bt_sk(ctrl_sock->sk); + intr = bt_sk(intr_sock->sk); + + if (ctrl->sk.sk_state != BT_CONNECTED || + intr->sk.sk_state != BT_CONNECTED) + return -EBADFD; + + /* early session check, we check again during session registration */ + session = hidp_session_find(&ctrl_chan->dst); + if (session) { + hidp_session_put(session); + return -EEXIST; + } + + return 0; +} + +int hidp_connection_add(struct hidp_connadd_req *req, + struct socket *ctrl_sock, + struct socket *intr_sock) +{ + u32 valid_flags = BIT(HIDP_VIRTUAL_CABLE_UNPLUG) | + BIT(HIDP_BOOT_PROTOCOL_MODE); + struct hidp_session *session; + struct l2cap_conn *conn; + struct l2cap_chan *chan; + int ret; + + ret = hidp_verify_sockets(ctrl_sock, intr_sock); + if (ret) + return ret; + + if (req->flags & ~valid_flags) + return -EINVAL; + + chan = l2cap_pi(ctrl_sock->sk)->chan; + conn = NULL; + l2cap_chan_lock(chan); + if (chan->conn) + conn = l2cap_conn_get(chan->conn); + l2cap_chan_unlock(chan); + + if (!conn) + return -EBADFD; + + ret = hidp_session_new(&session, &chan->dst, ctrl_sock, + intr_sock, req, conn); + if (ret) + goto out_conn; + + ret = l2cap_register_user(conn, &session->user); + if (ret) + goto out_session; + + ret = 0; + +out_session: + hidp_session_put(session); +out_conn: + l2cap_conn_put(conn); + return ret; +} + +int hidp_connection_del(struct hidp_conndel_req *req) +{ + u32 valid_flags = BIT(HIDP_VIRTUAL_CABLE_UNPLUG); + struct hidp_session *session; + + if (req->flags & ~valid_flags) + return -EINVAL; + + session = hidp_session_find(&req->bdaddr); + if (!session) + return -ENOENT; + + if (req->flags & BIT(HIDP_VIRTUAL_CABLE_UNPLUG)) + hidp_send_ctrl_message(session, + HIDP_TRANS_HID_CONTROL | + HIDP_CTRL_VIRTUAL_CABLE_UNPLUG, + NULL, 0); + else + l2cap_unregister_user(session->conn, &session->user); + + hidp_session_put(session); + + return 0; +} + +int hidp_get_connlist(struct hidp_connlist_req *req) +{ + struct hidp_session *session; + int err = 0, n = 0; + + BT_DBG(""); + + down_read(&hidp_session_sem); + + list_for_each_entry(session, &hidp_session_list, list) { + struct hidp_conninfo ci; + + hidp_copy_session(session, &ci); + + if (copy_to_user(req->ci, &ci, sizeof(ci))) { + err = -EFAULT; + break; + } + + if (++n >= req->cnum) + break; + + req->ci++; + } + req->cnum = n; + + up_read(&hidp_session_sem); + return err; +} + +int hidp_get_conninfo(struct hidp_conninfo *ci) +{ + struct hidp_session *session; + + session = hidp_session_find(&ci->bdaddr); + if (session) { + hidp_copy_session(session, ci); + hidp_session_put(session); + } + + return session ? 0 : -ENOENT; +} + +static int __init hidp_init(void) +{ + BT_INFO("HIDP (Human Interface Emulation) ver %s", VERSION); + + return hidp_init_sockets(); +} + +static void __exit hidp_exit(void) +{ + hidp_cleanup_sockets(); +} + +module_init(hidp_init); +module_exit(hidp_exit); + +MODULE_AUTHOR("Marcel Holtmann <marcel@holtmann.org>"); +MODULE_AUTHOR("David Herrmann <dh.herrmann@gmail.com>"); +MODULE_DESCRIPTION("Bluetooth HIDP ver " VERSION); +MODULE_VERSION(VERSION); +MODULE_LICENSE("GPL"); +MODULE_ALIAS("bt-proto-6"); diff --git a/kernel/net/bluetooth/hidp/hidp.h b/kernel/net/bluetooth/hidp/hidp.h new file mode 100644 index 000000000..8798492a6 --- /dev/null +++ b/kernel/net/bluetooth/hidp/hidp.h @@ -0,0 +1,192 @@ +/* + HIDP implementation for Linux Bluetooth stack (BlueZ). + Copyright (C) 2003-2004 Marcel Holtmann <marcel@holtmann.org> + + 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; + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS + OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT OF THIRD PARTY RIGHTS. + IN NO EVENT SHALL THE COPYRIGHT HOLDER(S) AND AUTHOR(S) BE LIABLE FOR ANY + CLAIM, OR ANY SPECIAL INDIRECT OR CONSEQUENTIAL DAMAGES, OR ANY DAMAGES + WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN + ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF + OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + + ALL LIABILITY, INCLUDING LIABILITY FOR INFRINGEMENT OF ANY PATENTS, + COPYRIGHTS, TRADEMARKS OR OTHER RIGHTS, RELATING TO USE OF THIS + SOFTWARE IS DISCLAIMED. +*/ + +#ifndef __HIDP_H +#define __HIDP_H + +#include <linux/types.h> +#include <linux/hid.h> +#include <linux/kref.h> +#include <net/bluetooth/bluetooth.h> +#include <net/bluetooth/l2cap.h> + +/* HIDP header masks */ +#define HIDP_HEADER_TRANS_MASK 0xf0 +#define HIDP_HEADER_PARAM_MASK 0x0f + +/* HIDP transaction types */ +#define HIDP_TRANS_HANDSHAKE 0x00 +#define HIDP_TRANS_HID_CONTROL 0x10 +#define HIDP_TRANS_GET_REPORT 0x40 +#define HIDP_TRANS_SET_REPORT 0x50 +#define HIDP_TRANS_GET_PROTOCOL 0x60 +#define HIDP_TRANS_SET_PROTOCOL 0x70 +#define HIDP_TRANS_GET_IDLE 0x80 +#define HIDP_TRANS_SET_IDLE 0x90 +#define HIDP_TRANS_DATA 0xa0 +#define HIDP_TRANS_DATC 0xb0 + +/* HIDP handshake results */ +#define HIDP_HSHK_SUCCESSFUL 0x00 +#define HIDP_HSHK_NOT_READY 0x01 +#define HIDP_HSHK_ERR_INVALID_REPORT_ID 0x02 +#define HIDP_HSHK_ERR_UNSUPPORTED_REQUEST 0x03 +#define HIDP_HSHK_ERR_INVALID_PARAMETER 0x04 +#define HIDP_HSHK_ERR_UNKNOWN 0x0e +#define HIDP_HSHK_ERR_FATAL 0x0f + +/* HIDP control operation parameters */ +#define HIDP_CTRL_NOP 0x00 +#define HIDP_CTRL_HARD_RESET 0x01 +#define HIDP_CTRL_SOFT_RESET 0x02 +#define HIDP_CTRL_SUSPEND 0x03 +#define HIDP_CTRL_EXIT_SUSPEND 0x04 +#define HIDP_CTRL_VIRTUAL_CABLE_UNPLUG 0x05 + +/* HIDP data transaction headers */ +#define HIDP_DATA_RTYPE_MASK 0x03 +#define HIDP_DATA_RSRVD_MASK 0x0c +#define HIDP_DATA_RTYPE_OTHER 0x00 +#define HIDP_DATA_RTYPE_INPUT 0x01 +#define HIDP_DATA_RTYPE_OUPUT 0x02 +#define HIDP_DATA_RTYPE_FEATURE 0x03 + +/* HIDP protocol header parameters */ +#define HIDP_PROTO_BOOT 0x00 +#define HIDP_PROTO_REPORT 0x01 + +/* HIDP ioctl defines */ +#define HIDPCONNADD _IOW('H', 200, int) +#define HIDPCONNDEL _IOW('H', 201, int) +#define HIDPGETCONNLIST _IOR('H', 210, int) +#define HIDPGETCONNINFO _IOR('H', 211, int) + +#define HIDP_VIRTUAL_CABLE_UNPLUG 0 +#define HIDP_BOOT_PROTOCOL_MODE 1 +#define HIDP_BLUETOOTH_VENDOR_ID 9 +#define HIDP_WAITING_FOR_RETURN 10 +#define HIDP_WAITING_FOR_SEND_ACK 11 + +struct hidp_connadd_req { + int ctrl_sock; /* Connected control socket */ + int intr_sock; /* Connected interrupt socket */ + __u16 parser; + __u16 rd_size; + __u8 __user *rd_data; + __u8 country; + __u8 subclass; + __u16 vendor; + __u16 product; + __u16 version; + __u32 flags; + __u32 idle_to; + char name[128]; +}; + +struct hidp_conndel_req { + bdaddr_t bdaddr; + __u32 flags; +}; + +struct hidp_conninfo { + bdaddr_t bdaddr; + __u32 flags; + __u16 state; + __u16 vendor; + __u16 product; + __u16 version; + char name[128]; +}; + +struct hidp_connlist_req { + __u32 cnum; + struct hidp_conninfo __user *ci; +}; + +int hidp_connection_add(struct hidp_connadd_req *req, struct socket *ctrl_sock, struct socket *intr_sock); +int hidp_connection_del(struct hidp_conndel_req *req); +int hidp_get_connlist(struct hidp_connlist_req *req); +int hidp_get_conninfo(struct hidp_conninfo *ci); + +enum hidp_session_state { + HIDP_SESSION_IDLING, + HIDP_SESSION_PREPARING, + HIDP_SESSION_RUNNING, +}; + +/* HIDP session defines */ +struct hidp_session { + struct list_head list; + struct kref ref; + + /* runtime management */ + atomic_t state; + wait_queue_head_t state_queue; + atomic_t terminate; + struct task_struct *task; + unsigned long flags; + + /* connection management */ + bdaddr_t bdaddr; + struct l2cap_conn *conn; + struct l2cap_user user; + struct socket *ctrl_sock; + struct socket *intr_sock; + struct sk_buff_head ctrl_transmit; + struct sk_buff_head intr_transmit; + uint ctrl_mtu; + uint intr_mtu; + unsigned long idle_to; + + /* device management */ + struct work_struct dev_init; + struct input_dev *input; + struct hid_device *hid; + struct timer_list timer; + + /* Report descriptor */ + __u8 *rd_data; + uint rd_size; + + /* session data */ + unsigned char keys[8]; + unsigned char leds; + + /* Used in hidp_get_raw_report() */ + int waiting_report_type; /* HIDP_DATA_RTYPE_* */ + int waiting_report_number; /* -1 for not numbered */ + struct mutex report_mutex; + struct sk_buff *report_return; + wait_queue_head_t report_queue; + + /* Used in hidp_output_raw_report() */ + int output_report_success; /* boolean */ + + /* temporary input buffer */ + u8 input_buf[HID_MAX_BUFFER_SIZE]; +}; + +/* HIDP init defines */ +int __init hidp_init_sockets(void); +void __exit hidp_cleanup_sockets(void); + +#endif /* __HIDP_H */ diff --git a/kernel/net/bluetooth/hidp/sock.c b/kernel/net/bluetooth/hidp/sock.c new file mode 100644 index 000000000..cb3fdde19 --- /dev/null +++ b/kernel/net/bluetooth/hidp/sock.c @@ -0,0 +1,299 @@ +/* + HIDP implementation for Linux Bluetooth stack (BlueZ). + Copyright (C) 2003-2004 Marcel Holtmann <marcel@holtmann.org> + + 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; + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS + OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT OF THIRD PARTY RIGHTS. + IN NO EVENT SHALL THE COPYRIGHT HOLDER(S) AND AUTHOR(S) BE LIABLE FOR ANY + CLAIM, OR ANY SPECIAL INDIRECT OR CONSEQUENTIAL DAMAGES, OR ANY DAMAGES + WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN + ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF + OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + + ALL LIABILITY, INCLUDING LIABILITY FOR INFRINGEMENT OF ANY PATENTS, + COPYRIGHTS, TRADEMARKS OR OTHER RIGHTS, RELATING TO USE OF THIS + SOFTWARE IS DISCLAIMED. +*/ + +#include <linux/export.h> +#include <linux/file.h> + +#include "hidp.h" + +static struct bt_sock_list hidp_sk_list = { + .lock = __RW_LOCK_UNLOCKED(hidp_sk_list.lock) +}; + +static int hidp_sock_release(struct socket *sock) +{ + struct sock *sk = sock->sk; + + BT_DBG("sock %p sk %p", sock, sk); + + if (!sk) + return 0; + + bt_sock_unlink(&hidp_sk_list, sk); + + sock_orphan(sk); + sock_put(sk); + + return 0; +} + +static int hidp_sock_ioctl(struct socket *sock, unsigned int cmd, unsigned long arg) +{ + void __user *argp = (void __user *) arg; + struct hidp_connadd_req ca; + struct hidp_conndel_req cd; + struct hidp_connlist_req cl; + struct hidp_conninfo ci; + struct socket *csock; + struct socket *isock; + int err; + + BT_DBG("cmd %x arg %lx", cmd, arg); + + switch (cmd) { + case HIDPCONNADD: + if (!capable(CAP_NET_ADMIN)) + return -EPERM; + + if (copy_from_user(&ca, argp, sizeof(ca))) + return -EFAULT; + + csock = sockfd_lookup(ca.ctrl_sock, &err); + if (!csock) + return err; + + isock = sockfd_lookup(ca.intr_sock, &err); + if (!isock) { + sockfd_put(csock); + return err; + } + + err = hidp_connection_add(&ca, csock, isock); + if (!err && copy_to_user(argp, &ca, sizeof(ca))) + err = -EFAULT; + + sockfd_put(csock); + sockfd_put(isock); + + return err; + + case HIDPCONNDEL: + if (!capable(CAP_NET_ADMIN)) + return -EPERM; + + if (copy_from_user(&cd, argp, sizeof(cd))) + return -EFAULT; + + return hidp_connection_del(&cd); + + case HIDPGETCONNLIST: + if (copy_from_user(&cl, argp, sizeof(cl))) + return -EFAULT; + + if (cl.cnum <= 0) + return -EINVAL; + + err = hidp_get_connlist(&cl); + if (!err && copy_to_user(argp, &cl, sizeof(cl))) + return -EFAULT; + + return err; + + case HIDPGETCONNINFO: + if (copy_from_user(&ci, argp, sizeof(ci))) + return -EFAULT; + + err = hidp_get_conninfo(&ci); + if (!err && copy_to_user(argp, &ci, sizeof(ci))) + return -EFAULT; + + return err; + } + + return -EINVAL; +} + +#ifdef CONFIG_COMPAT +struct compat_hidp_connadd_req { + int ctrl_sock; /* Connected control socket */ + int intr_sock; /* Connected interrupt socket */ + __u16 parser; + __u16 rd_size; + compat_uptr_t rd_data; + __u8 country; + __u8 subclass; + __u16 vendor; + __u16 product; + __u16 version; + __u32 flags; + __u32 idle_to; + char name[128]; +}; + +static int hidp_sock_compat_ioctl(struct socket *sock, unsigned int cmd, unsigned long arg) +{ + if (cmd == HIDPGETCONNLIST) { + struct hidp_connlist_req cl; + u32 uci; + int err; + + if (get_user(cl.cnum, (u32 __user *) arg) || + get_user(uci, (u32 __user *) (arg + 4))) + return -EFAULT; + + cl.ci = compat_ptr(uci); + + if (cl.cnum <= 0) + return -EINVAL; + + err = hidp_get_connlist(&cl); + + if (!err && put_user(cl.cnum, (u32 __user *) arg)) + err = -EFAULT; + + return err; + } else if (cmd == HIDPCONNADD) { + struct compat_hidp_connadd_req ca; + struct hidp_connadd_req __user *uca; + + uca = compat_alloc_user_space(sizeof(*uca)); + + if (copy_from_user(&ca, (void __user *) arg, sizeof(ca))) + return -EFAULT; + + if (put_user(ca.ctrl_sock, &uca->ctrl_sock) || + put_user(ca.intr_sock, &uca->intr_sock) || + put_user(ca.parser, &uca->parser) || + put_user(ca.rd_size, &uca->rd_size) || + put_user(compat_ptr(ca.rd_data), &uca->rd_data) || + put_user(ca.country, &uca->country) || + put_user(ca.subclass, &uca->subclass) || + put_user(ca.vendor, &uca->vendor) || + put_user(ca.product, &uca->product) || + put_user(ca.version, &uca->version) || + put_user(ca.flags, &uca->flags) || + put_user(ca.idle_to, &uca->idle_to) || + copy_to_user(&uca->name[0], &ca.name[0], 128)) + return -EFAULT; + + arg = (unsigned long) uca; + + /* Fall through. We don't actually write back any _changes_ + to the structure anyway, so there's no need to copy back + into the original compat version */ + } + + return hidp_sock_ioctl(sock, cmd, arg); +} +#endif + +static const struct proto_ops hidp_sock_ops = { + .family = PF_BLUETOOTH, + .owner = THIS_MODULE, + .release = hidp_sock_release, + .ioctl = hidp_sock_ioctl, +#ifdef CONFIG_COMPAT + .compat_ioctl = hidp_sock_compat_ioctl, +#endif + .bind = sock_no_bind, + .getname = sock_no_getname, + .sendmsg = sock_no_sendmsg, + .recvmsg = sock_no_recvmsg, + .poll = sock_no_poll, + .listen = sock_no_listen, + .shutdown = sock_no_shutdown, + .setsockopt = sock_no_setsockopt, + .getsockopt = sock_no_getsockopt, + .connect = sock_no_connect, + .socketpair = sock_no_socketpair, + .accept = sock_no_accept, + .mmap = sock_no_mmap +}; + +static struct proto hidp_proto = { + .name = "HIDP", + .owner = THIS_MODULE, + .obj_size = sizeof(struct bt_sock) +}; + +static int hidp_sock_create(struct net *net, struct socket *sock, int protocol, + int kern) +{ + struct sock *sk; + + BT_DBG("sock %p", sock); + + if (sock->type != SOCK_RAW) + return -ESOCKTNOSUPPORT; + + sk = sk_alloc(net, PF_BLUETOOTH, GFP_ATOMIC, &hidp_proto); + if (!sk) + return -ENOMEM; + + sock_init_data(sock, sk); + + sock->ops = &hidp_sock_ops; + + sock->state = SS_UNCONNECTED; + + sock_reset_flag(sk, SOCK_ZAPPED); + + sk->sk_protocol = protocol; + sk->sk_state = BT_OPEN; + + bt_sock_link(&hidp_sk_list, sk); + + return 0; +} + +static const struct net_proto_family hidp_sock_family_ops = { + .family = PF_BLUETOOTH, + .owner = THIS_MODULE, + .create = hidp_sock_create +}; + +int __init hidp_init_sockets(void) +{ + int err; + + err = proto_register(&hidp_proto, 0); + if (err < 0) + return err; + + err = bt_sock_register(BTPROTO_HIDP, &hidp_sock_family_ops); + if (err < 0) { + BT_ERR("Can't register HIDP socket"); + goto error; + } + + err = bt_procfs_init(&init_net, "hidp", &hidp_sk_list, NULL); + if (err < 0) { + BT_ERR("Failed to create HIDP proc file"); + bt_sock_unregister(BTPROTO_HIDP); + goto error; + } + + BT_INFO("HIDP socket layer initialized"); + + return 0; + +error: + proto_unregister(&hidp_proto); + return err; +} + +void __exit hidp_cleanup_sockets(void) +{ + bt_procfs_cleanup(&init_net, "hidp"); + bt_sock_unregister(BTPROTO_HIDP); + proto_unregister(&hidp_proto); +} diff --git a/kernel/net/bluetooth/l2cap_core.c b/kernel/net/bluetooth/l2cap_core.c new file mode 100644 index 000000000..dad419782 --- /dev/null +++ b/kernel/net/bluetooth/l2cap_core.c @@ -0,0 +1,7622 @@ +/* + BlueZ - Bluetooth protocol stack for Linux + Copyright (C) 2000-2001 Qualcomm Incorporated + Copyright (C) 2009-2010 Gustavo F. Padovan <gustavo@padovan.org> + Copyright (C) 2010 Google Inc. + Copyright (C) 2011 ProFUSION Embedded Systems + Copyright (c) 2012 Code Aurora Forum. All rights reserved. + + Written 2000,2001 by Maxim Krasnyansky <maxk@qualcomm.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; + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS + OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT OF THIRD PARTY RIGHTS. + IN NO EVENT SHALL THE COPYRIGHT HOLDER(S) AND AUTHOR(S) BE LIABLE FOR ANY + CLAIM, OR ANY SPECIAL INDIRECT OR CONSEQUENTIAL DAMAGES, OR ANY DAMAGES + WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN + ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF + OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + + ALL LIABILITY, INCLUDING LIABILITY FOR INFRINGEMENT OF ANY PATENTS, + COPYRIGHTS, TRADEMARKS OR OTHER RIGHTS, RELATING TO USE OF THIS + SOFTWARE IS DISCLAIMED. +*/ + +/* Bluetooth L2CAP core. */ + +#include <linux/module.h> + +#include <linux/debugfs.h> +#include <linux/crc16.h> + +#include <net/bluetooth/bluetooth.h> +#include <net/bluetooth/hci_core.h> +#include <net/bluetooth/l2cap.h> + +#include "smp.h" +#include "a2mp.h" +#include "amp.h" + +#define LE_FLOWCTL_MAX_CREDITS 65535 + +bool disable_ertm; + +static u32 l2cap_feat_mask = L2CAP_FEAT_FIXED_CHAN | L2CAP_FEAT_UCD; + +static LIST_HEAD(chan_list); +static DEFINE_RWLOCK(chan_list_lock); + +static u16 le_max_credits = L2CAP_LE_MAX_CREDITS; +static u16 le_default_mps = L2CAP_LE_DEFAULT_MPS; + +static struct sk_buff *l2cap_build_cmd(struct l2cap_conn *conn, + u8 code, u8 ident, u16 dlen, void *data); +static void l2cap_send_cmd(struct l2cap_conn *conn, u8 ident, u8 code, u16 len, + void *data); +static int l2cap_build_conf_req(struct l2cap_chan *chan, void *data); +static void l2cap_send_disconn_req(struct l2cap_chan *chan, int err); + +static void l2cap_tx(struct l2cap_chan *chan, struct l2cap_ctrl *control, + struct sk_buff_head *skbs, u8 event); + +static inline u8 bdaddr_type(u8 link_type, u8 bdaddr_type) +{ + if (link_type == LE_LINK) { + if (bdaddr_type == ADDR_LE_DEV_PUBLIC) + return BDADDR_LE_PUBLIC; + else + return BDADDR_LE_RANDOM; + } + + return BDADDR_BREDR; +} + +static inline u8 bdaddr_src_type(struct hci_conn *hcon) +{ + return bdaddr_type(hcon->type, hcon->src_type); +} + +static inline u8 bdaddr_dst_type(struct hci_conn *hcon) +{ + return bdaddr_type(hcon->type, hcon->dst_type); +} + +/* ---- L2CAP channels ---- */ + +static struct l2cap_chan *__l2cap_get_chan_by_dcid(struct l2cap_conn *conn, + u16 cid) +{ + struct l2cap_chan *c; + + list_for_each_entry(c, &conn->chan_l, list) { + if (c->dcid == cid) + return c; + } + return NULL; +} + +static struct l2cap_chan *__l2cap_get_chan_by_scid(struct l2cap_conn *conn, + u16 cid) +{ + struct l2cap_chan *c; + + list_for_each_entry(c, &conn->chan_l, list) { + if (c->scid == cid) + return c; + } + return NULL; +} + +/* Find channel with given SCID. + * Returns locked channel. */ +static struct l2cap_chan *l2cap_get_chan_by_scid(struct l2cap_conn *conn, + u16 cid) +{ + struct l2cap_chan *c; + + mutex_lock(&conn->chan_lock); + c = __l2cap_get_chan_by_scid(conn, cid); + if (c) + l2cap_chan_lock(c); + mutex_unlock(&conn->chan_lock); + + return c; +} + +/* Find channel with given DCID. + * Returns locked channel. + */ +static struct l2cap_chan *l2cap_get_chan_by_dcid(struct l2cap_conn *conn, + u16 cid) +{ + struct l2cap_chan *c; + + mutex_lock(&conn->chan_lock); + c = __l2cap_get_chan_by_dcid(conn, cid); + if (c) + l2cap_chan_lock(c); + mutex_unlock(&conn->chan_lock); + + return c; +} + +static struct l2cap_chan *__l2cap_get_chan_by_ident(struct l2cap_conn *conn, + u8 ident) +{ + struct l2cap_chan *c; + + list_for_each_entry(c, &conn->chan_l, list) { + if (c->ident == ident) + return c; + } + return NULL; +} + +static struct l2cap_chan *l2cap_get_chan_by_ident(struct l2cap_conn *conn, + u8 ident) +{ + struct l2cap_chan *c; + + mutex_lock(&conn->chan_lock); + c = __l2cap_get_chan_by_ident(conn, ident); + if (c) + l2cap_chan_lock(c); + mutex_unlock(&conn->chan_lock); + + return c; +} + +static struct l2cap_chan *__l2cap_global_chan_by_addr(__le16 psm, bdaddr_t *src) +{ + struct l2cap_chan *c; + + list_for_each_entry(c, &chan_list, global_l) { + if (c->sport == psm && !bacmp(&c->src, src)) + return c; + } + return NULL; +} + +int l2cap_add_psm(struct l2cap_chan *chan, bdaddr_t *src, __le16 psm) +{ + int err; + + write_lock(&chan_list_lock); + + if (psm && __l2cap_global_chan_by_addr(psm, src)) { + err = -EADDRINUSE; + goto done; + } + + if (psm) { + chan->psm = psm; + chan->sport = psm; + err = 0; + } else { + u16 p; + + err = -EINVAL; + for (p = 0x1001; p < 0x1100; p += 2) + if (!__l2cap_global_chan_by_addr(cpu_to_le16(p), src)) { + chan->psm = cpu_to_le16(p); + chan->sport = cpu_to_le16(p); + err = 0; + break; + } + } + +done: + write_unlock(&chan_list_lock); + return err; +} +EXPORT_SYMBOL_GPL(l2cap_add_psm); + +int l2cap_add_scid(struct l2cap_chan *chan, __u16 scid) +{ + write_lock(&chan_list_lock); + + /* Override the defaults (which are for conn-oriented) */ + chan->omtu = L2CAP_DEFAULT_MTU; + chan->chan_type = L2CAP_CHAN_FIXED; + + chan->scid = scid; + + write_unlock(&chan_list_lock); + + return 0; +} + +static u16 l2cap_alloc_cid(struct l2cap_conn *conn) +{ + u16 cid, dyn_end; + + if (conn->hcon->type == LE_LINK) + dyn_end = L2CAP_CID_LE_DYN_END; + else + dyn_end = L2CAP_CID_DYN_END; + + for (cid = L2CAP_CID_DYN_START; cid < dyn_end; cid++) { + if (!__l2cap_get_chan_by_scid(conn, cid)) + return cid; + } + + return 0; +} + +static void l2cap_state_change(struct l2cap_chan *chan, int state) +{ + BT_DBG("chan %p %s -> %s", chan, state_to_string(chan->state), + state_to_string(state)); + + chan->state = state; + chan->ops->state_change(chan, state, 0); +} + +static inline void l2cap_state_change_and_error(struct l2cap_chan *chan, + int state, int err) +{ + chan->state = state; + chan->ops->state_change(chan, chan->state, err); +} + +static inline void l2cap_chan_set_err(struct l2cap_chan *chan, int err) +{ + chan->ops->state_change(chan, chan->state, err); +} + +static void __set_retrans_timer(struct l2cap_chan *chan) +{ + if (!delayed_work_pending(&chan->monitor_timer) && + chan->retrans_timeout) { + l2cap_set_timer(chan, &chan->retrans_timer, + msecs_to_jiffies(chan->retrans_timeout)); + } +} + +static void __set_monitor_timer(struct l2cap_chan *chan) +{ + __clear_retrans_timer(chan); + if (chan->monitor_timeout) { + l2cap_set_timer(chan, &chan->monitor_timer, + msecs_to_jiffies(chan->monitor_timeout)); + } +} + +static struct sk_buff *l2cap_ertm_seq_in_queue(struct sk_buff_head *head, + u16 seq) +{ + struct sk_buff *skb; + + skb_queue_walk(head, skb) { + if (bt_cb(skb)->l2cap.txseq == seq) + return skb; + } + + return NULL; +} + +/* ---- L2CAP sequence number lists ---- */ + +/* For ERTM, ordered lists of sequence numbers must be tracked for + * SREJ requests that are received and for frames that are to be + * retransmitted. These seq_list functions implement a singly-linked + * list in an array, where membership in the list can also be checked + * in constant time. Items can also be added to the tail of the list + * and removed from the head in constant time, without further memory + * allocs or frees. + */ + +static int l2cap_seq_list_init(struct l2cap_seq_list *seq_list, u16 size) +{ + size_t alloc_size, i; + + /* Allocated size is a power of 2 to map sequence numbers + * (which may be up to 14 bits) in to a smaller array that is + * sized for the negotiated ERTM transmit windows. + */ + alloc_size = roundup_pow_of_two(size); + + seq_list->list = kmalloc(sizeof(u16) * alloc_size, GFP_KERNEL); + if (!seq_list->list) + return -ENOMEM; + + seq_list->mask = alloc_size - 1; + seq_list->head = L2CAP_SEQ_LIST_CLEAR; + seq_list->tail = L2CAP_SEQ_LIST_CLEAR; + for (i = 0; i < alloc_size; i++) + seq_list->list[i] = L2CAP_SEQ_LIST_CLEAR; + + return 0; +} + +static inline void l2cap_seq_list_free(struct l2cap_seq_list *seq_list) +{ + kfree(seq_list->list); +} + +static inline bool l2cap_seq_list_contains(struct l2cap_seq_list *seq_list, + u16 seq) +{ + /* Constant-time check for list membership */ + return seq_list->list[seq & seq_list->mask] != L2CAP_SEQ_LIST_CLEAR; +} + +static inline u16 l2cap_seq_list_pop(struct l2cap_seq_list *seq_list) +{ + u16 seq = seq_list->head; + u16 mask = seq_list->mask; + + seq_list->head = seq_list->list[seq & mask]; + seq_list->list[seq & mask] = L2CAP_SEQ_LIST_CLEAR; + + if (seq_list->head == L2CAP_SEQ_LIST_TAIL) { + seq_list->head = L2CAP_SEQ_LIST_CLEAR; + seq_list->tail = L2CAP_SEQ_LIST_CLEAR; + } + + return seq; +} + +static void l2cap_seq_list_clear(struct l2cap_seq_list *seq_list) +{ + u16 i; + + if (seq_list->head == L2CAP_SEQ_LIST_CLEAR) + return; + + for (i = 0; i <= seq_list->mask; i++) + seq_list->list[i] = L2CAP_SEQ_LIST_CLEAR; + + seq_list->head = L2CAP_SEQ_LIST_CLEAR; + seq_list->tail = L2CAP_SEQ_LIST_CLEAR; +} + +static void l2cap_seq_list_append(struct l2cap_seq_list *seq_list, u16 seq) +{ + u16 mask = seq_list->mask; + + /* All appends happen in constant time */ + + if (seq_list->list[seq & mask] != L2CAP_SEQ_LIST_CLEAR) + return; + + if (seq_list->tail == L2CAP_SEQ_LIST_CLEAR) + seq_list->head = seq; + else + seq_list->list[seq_list->tail & mask] = seq; + + seq_list->tail = seq; + seq_list->list[seq & mask] = L2CAP_SEQ_LIST_TAIL; +} + +static void l2cap_chan_timeout(struct work_struct *work) +{ + struct l2cap_chan *chan = container_of(work, struct l2cap_chan, + chan_timer.work); + struct l2cap_conn *conn = chan->conn; + int reason; + + BT_DBG("chan %p state %s", chan, state_to_string(chan->state)); + + mutex_lock(&conn->chan_lock); + l2cap_chan_lock(chan); + + if (chan->state == BT_CONNECTED || chan->state == BT_CONFIG) + reason = ECONNREFUSED; + else if (chan->state == BT_CONNECT && + chan->sec_level != BT_SECURITY_SDP) + reason = ECONNREFUSED; + else + reason = ETIMEDOUT; + + l2cap_chan_close(chan, reason); + + l2cap_chan_unlock(chan); + + chan->ops->close(chan); + mutex_unlock(&conn->chan_lock); + + l2cap_chan_put(chan); +} + +struct l2cap_chan *l2cap_chan_create(void) +{ + struct l2cap_chan *chan; + + chan = kzalloc(sizeof(*chan), GFP_ATOMIC); + if (!chan) + return NULL; + + mutex_init(&chan->lock); + + /* Set default lock nesting level */ + atomic_set(&chan->nesting, L2CAP_NESTING_NORMAL); + + write_lock(&chan_list_lock); + list_add(&chan->global_l, &chan_list); + write_unlock(&chan_list_lock); + + INIT_DELAYED_WORK(&chan->chan_timer, l2cap_chan_timeout); + + chan->state = BT_OPEN; + + kref_init(&chan->kref); + + /* This flag is cleared in l2cap_chan_ready() */ + set_bit(CONF_NOT_COMPLETE, &chan->conf_state); + + BT_DBG("chan %p", chan); + + return chan; +} +EXPORT_SYMBOL_GPL(l2cap_chan_create); + +static void l2cap_chan_destroy(struct kref *kref) +{ + struct l2cap_chan *chan = container_of(kref, struct l2cap_chan, kref); + + BT_DBG("chan %p", chan); + + write_lock(&chan_list_lock); + list_del(&chan->global_l); + write_unlock(&chan_list_lock); + + kfree(chan); +} + +void l2cap_chan_hold(struct l2cap_chan *c) +{ + BT_DBG("chan %p orig refcnt %d", c, atomic_read(&c->kref.refcount)); + + kref_get(&c->kref); +} + +void l2cap_chan_put(struct l2cap_chan *c) +{ + BT_DBG("chan %p orig refcnt %d", c, atomic_read(&c->kref.refcount)); + + kref_put(&c->kref, l2cap_chan_destroy); +} +EXPORT_SYMBOL_GPL(l2cap_chan_put); + +void l2cap_chan_set_defaults(struct l2cap_chan *chan) +{ + chan->fcs = L2CAP_FCS_CRC16; + chan->max_tx = L2CAP_DEFAULT_MAX_TX; + chan->tx_win = L2CAP_DEFAULT_TX_WINDOW; + chan->tx_win_max = L2CAP_DEFAULT_TX_WINDOW; + chan->remote_max_tx = chan->max_tx; + chan->remote_tx_win = chan->tx_win; + chan->ack_win = L2CAP_DEFAULT_TX_WINDOW; + chan->sec_level = BT_SECURITY_LOW; + chan->flush_to = L2CAP_DEFAULT_FLUSH_TO; + chan->retrans_timeout = L2CAP_DEFAULT_RETRANS_TO; + chan->monitor_timeout = L2CAP_DEFAULT_MONITOR_TO; + chan->conf_state = 0; + + set_bit(FLAG_FORCE_ACTIVE, &chan->flags); +} +EXPORT_SYMBOL_GPL(l2cap_chan_set_defaults); + +static void l2cap_le_flowctl_init(struct l2cap_chan *chan) +{ + chan->sdu = NULL; + chan->sdu_last_frag = NULL; + chan->sdu_len = 0; + chan->tx_credits = 0; + chan->rx_credits = le_max_credits; + chan->mps = min_t(u16, chan->imtu, le_default_mps); + + skb_queue_head_init(&chan->tx_q); +} + +void __l2cap_chan_add(struct l2cap_conn *conn, struct l2cap_chan *chan) +{ + BT_DBG("conn %p, psm 0x%2.2x, dcid 0x%4.4x", conn, + __le16_to_cpu(chan->psm), chan->dcid); + + conn->disc_reason = HCI_ERROR_REMOTE_USER_TERM; + + chan->conn = conn; + + switch (chan->chan_type) { + case L2CAP_CHAN_CONN_ORIENTED: + /* Alloc CID for connection-oriented socket */ + chan->scid = l2cap_alloc_cid(conn); + if (conn->hcon->type == ACL_LINK) + chan->omtu = L2CAP_DEFAULT_MTU; + break; + + case L2CAP_CHAN_CONN_LESS: + /* Connectionless socket */ + chan->scid = L2CAP_CID_CONN_LESS; + chan->dcid = L2CAP_CID_CONN_LESS; + chan->omtu = L2CAP_DEFAULT_MTU; + break; + + case L2CAP_CHAN_FIXED: + /* Caller will set CID and CID specific MTU values */ + break; + + default: + /* Raw socket can send/recv signalling messages only */ + chan->scid = L2CAP_CID_SIGNALING; + chan->dcid = L2CAP_CID_SIGNALING; + chan->omtu = L2CAP_DEFAULT_MTU; + } + + chan->local_id = L2CAP_BESTEFFORT_ID; + chan->local_stype = L2CAP_SERV_BESTEFFORT; + chan->local_msdu = L2CAP_DEFAULT_MAX_SDU_SIZE; + chan->local_sdu_itime = L2CAP_DEFAULT_SDU_ITIME; + chan->local_acc_lat = L2CAP_DEFAULT_ACC_LAT; + chan->local_flush_to = L2CAP_EFS_DEFAULT_FLUSH_TO; + + l2cap_chan_hold(chan); + + /* Only keep a reference for fixed channels if they requested it */ + if (chan->chan_type != L2CAP_CHAN_FIXED || + test_bit(FLAG_HOLD_HCI_CONN, &chan->flags)) + hci_conn_hold(conn->hcon); + + list_add(&chan->list, &conn->chan_l); +} + +void l2cap_chan_add(struct l2cap_conn *conn, struct l2cap_chan *chan) +{ + mutex_lock(&conn->chan_lock); + __l2cap_chan_add(conn, chan); + mutex_unlock(&conn->chan_lock); +} + +void l2cap_chan_del(struct l2cap_chan *chan, int err) +{ + struct l2cap_conn *conn = chan->conn; + + __clear_chan_timer(chan); + + BT_DBG("chan %p, conn %p, err %d, state %s", chan, conn, err, + state_to_string(chan->state)); + + chan->ops->teardown(chan, err); + + if (conn) { + struct amp_mgr *mgr = conn->hcon->amp_mgr; + /* Delete from channel list */ + list_del(&chan->list); + + l2cap_chan_put(chan); + + chan->conn = NULL; + + /* Reference was only held for non-fixed channels or + * fixed channels that explicitly requested it using the + * FLAG_HOLD_HCI_CONN flag. + */ + if (chan->chan_type != L2CAP_CHAN_FIXED || + test_bit(FLAG_HOLD_HCI_CONN, &chan->flags)) + hci_conn_drop(conn->hcon); + + if (mgr && mgr->bredr_chan == chan) + mgr->bredr_chan = NULL; + } + + if (chan->hs_hchan) { + struct hci_chan *hs_hchan = chan->hs_hchan; + + BT_DBG("chan %p disconnect hs_hchan %p", chan, hs_hchan); + amp_disconnect_logical_link(hs_hchan); + } + + if (test_bit(CONF_NOT_COMPLETE, &chan->conf_state)) + return; + + switch(chan->mode) { + case L2CAP_MODE_BASIC: + break; + + case L2CAP_MODE_LE_FLOWCTL: + skb_queue_purge(&chan->tx_q); + break; + + case L2CAP_MODE_ERTM: + __clear_retrans_timer(chan); + __clear_monitor_timer(chan); + __clear_ack_timer(chan); + + skb_queue_purge(&chan->srej_q); + + l2cap_seq_list_free(&chan->srej_list); + l2cap_seq_list_free(&chan->retrans_list); + + /* fall through */ + + case L2CAP_MODE_STREAMING: + skb_queue_purge(&chan->tx_q); + break; + } + + return; +} +EXPORT_SYMBOL_GPL(l2cap_chan_del); + +static void l2cap_conn_update_id_addr(struct work_struct *work) +{ + struct l2cap_conn *conn = container_of(work, struct l2cap_conn, + id_addr_update_work); + struct hci_conn *hcon = conn->hcon; + struct l2cap_chan *chan; + + mutex_lock(&conn->chan_lock); + + list_for_each_entry(chan, &conn->chan_l, list) { + l2cap_chan_lock(chan); + bacpy(&chan->dst, &hcon->dst); + chan->dst_type = bdaddr_dst_type(hcon); + l2cap_chan_unlock(chan); + } + + mutex_unlock(&conn->chan_lock); +} + +static void l2cap_chan_le_connect_reject(struct l2cap_chan *chan) +{ + struct l2cap_conn *conn = chan->conn; + struct l2cap_le_conn_rsp rsp; + u16 result; + + if (test_bit(FLAG_DEFER_SETUP, &chan->flags)) + result = L2CAP_CR_AUTHORIZATION; + else + result = L2CAP_CR_BAD_PSM; + + l2cap_state_change(chan, BT_DISCONN); + + rsp.dcid = cpu_to_le16(chan->scid); + rsp.mtu = cpu_to_le16(chan->imtu); + rsp.mps = cpu_to_le16(chan->mps); + rsp.credits = cpu_to_le16(chan->rx_credits); + rsp.result = cpu_to_le16(result); + + l2cap_send_cmd(conn, chan->ident, L2CAP_LE_CONN_RSP, sizeof(rsp), + &rsp); +} + +static void l2cap_chan_connect_reject(struct l2cap_chan *chan) +{ + struct l2cap_conn *conn = chan->conn; + struct l2cap_conn_rsp rsp; + u16 result; + + if (test_bit(FLAG_DEFER_SETUP, &chan->flags)) + result = L2CAP_CR_SEC_BLOCK; + else + result = L2CAP_CR_BAD_PSM; + + l2cap_state_change(chan, BT_DISCONN); + + rsp.scid = cpu_to_le16(chan->dcid); + rsp.dcid = cpu_to_le16(chan->scid); + rsp.result = cpu_to_le16(result); + rsp.status = cpu_to_le16(L2CAP_CS_NO_INFO); + + l2cap_send_cmd(conn, chan->ident, L2CAP_CONN_RSP, sizeof(rsp), &rsp); +} + +void l2cap_chan_close(struct l2cap_chan *chan, int reason) +{ + struct l2cap_conn *conn = chan->conn; + + BT_DBG("chan %p state %s", chan, state_to_string(chan->state)); + + switch (chan->state) { + case BT_LISTEN: + chan->ops->teardown(chan, 0); + break; + + case BT_CONNECTED: + case BT_CONFIG: + if (chan->chan_type == L2CAP_CHAN_CONN_ORIENTED) { + __set_chan_timer(chan, chan->ops->get_sndtimeo(chan)); + l2cap_send_disconn_req(chan, reason); + } else + l2cap_chan_del(chan, reason); + break; + + case BT_CONNECT2: + if (chan->chan_type == L2CAP_CHAN_CONN_ORIENTED) { + if (conn->hcon->type == ACL_LINK) + l2cap_chan_connect_reject(chan); + else if (conn->hcon->type == LE_LINK) + l2cap_chan_le_connect_reject(chan); + } + + l2cap_chan_del(chan, reason); + break; + + case BT_CONNECT: + case BT_DISCONN: + l2cap_chan_del(chan, reason); + break; + + default: + chan->ops->teardown(chan, 0); + break; + } +} +EXPORT_SYMBOL(l2cap_chan_close); + +static inline u8 l2cap_get_auth_type(struct l2cap_chan *chan) +{ + switch (chan->chan_type) { + case L2CAP_CHAN_RAW: + switch (chan->sec_level) { + case BT_SECURITY_HIGH: + case BT_SECURITY_FIPS: + return HCI_AT_DEDICATED_BONDING_MITM; + case BT_SECURITY_MEDIUM: + return HCI_AT_DEDICATED_BONDING; + default: + return HCI_AT_NO_BONDING; + } + break; + case L2CAP_CHAN_CONN_LESS: + if (chan->psm == cpu_to_le16(L2CAP_PSM_3DSP)) { + if (chan->sec_level == BT_SECURITY_LOW) + chan->sec_level = BT_SECURITY_SDP; + } + if (chan->sec_level == BT_SECURITY_HIGH || + chan->sec_level == BT_SECURITY_FIPS) + return HCI_AT_NO_BONDING_MITM; + else + return HCI_AT_NO_BONDING; + break; + case L2CAP_CHAN_CONN_ORIENTED: + if (chan->psm == cpu_to_le16(L2CAP_PSM_SDP)) { + if (chan->sec_level == BT_SECURITY_LOW) + chan->sec_level = BT_SECURITY_SDP; + + if (chan->sec_level == BT_SECURITY_HIGH || + chan->sec_level == BT_SECURITY_FIPS) + return HCI_AT_NO_BONDING_MITM; + else + return HCI_AT_NO_BONDING; + } + /* fall through */ + default: + switch (chan->sec_level) { + case BT_SECURITY_HIGH: + case BT_SECURITY_FIPS: + return HCI_AT_GENERAL_BONDING_MITM; + case BT_SECURITY_MEDIUM: + return HCI_AT_GENERAL_BONDING; + default: + return HCI_AT_NO_BONDING; + } + break; + } +} + +/* Service level security */ +int l2cap_chan_check_security(struct l2cap_chan *chan, bool initiator) +{ + struct l2cap_conn *conn = chan->conn; + __u8 auth_type; + + if (conn->hcon->type == LE_LINK) + return smp_conn_security(conn->hcon, chan->sec_level); + + auth_type = l2cap_get_auth_type(chan); + + return hci_conn_security(conn->hcon, chan->sec_level, auth_type, + initiator); +} + +static u8 l2cap_get_ident(struct l2cap_conn *conn) +{ + u8 id; + + /* Get next available identificator. + * 1 - 128 are used by kernel. + * 129 - 199 are reserved. + * 200 - 254 are used by utilities like l2ping, etc. + */ + + mutex_lock(&conn->ident_lock); + + if (++conn->tx_ident > 128) + conn->tx_ident = 1; + + id = conn->tx_ident; + + mutex_unlock(&conn->ident_lock); + + return id; +} + +static void l2cap_send_cmd(struct l2cap_conn *conn, u8 ident, u8 code, u16 len, + void *data) +{ + struct sk_buff *skb = l2cap_build_cmd(conn, code, ident, len, data); + u8 flags; + + BT_DBG("code 0x%2.2x", code); + + if (!skb) + return; + + /* Use NO_FLUSH if supported or we have an LE link (which does + * not support auto-flushing packets) */ + if (lmp_no_flush_capable(conn->hcon->hdev) || + conn->hcon->type == LE_LINK) + flags = ACL_START_NO_FLUSH; + else + flags = ACL_START; + + bt_cb(skb)->force_active = BT_POWER_FORCE_ACTIVE_ON; + skb->priority = HCI_PRIO_MAX; + + hci_send_acl(conn->hchan, skb, flags); +} + +static bool __chan_is_moving(struct l2cap_chan *chan) +{ + return chan->move_state != L2CAP_MOVE_STABLE && + chan->move_state != L2CAP_MOVE_WAIT_PREPARE; +} + +static void l2cap_do_send(struct l2cap_chan *chan, struct sk_buff *skb) +{ + struct hci_conn *hcon = chan->conn->hcon; + u16 flags; + + BT_DBG("chan %p, skb %p len %d priority %u", chan, skb, skb->len, + skb->priority); + + if (chan->hs_hcon && !__chan_is_moving(chan)) { + if (chan->hs_hchan) + hci_send_acl(chan->hs_hchan, skb, ACL_COMPLETE); + else + kfree_skb(skb); + + return; + } + + /* Use NO_FLUSH for LE links (where this is the only option) or + * if the BR/EDR link supports it and flushing has not been + * explicitly requested (through FLAG_FLUSHABLE). + */ + if (hcon->type == LE_LINK || + (!test_bit(FLAG_FLUSHABLE, &chan->flags) && + lmp_no_flush_capable(hcon->hdev))) + flags = ACL_START_NO_FLUSH; + else + flags = ACL_START; + + bt_cb(skb)->force_active = test_bit(FLAG_FORCE_ACTIVE, &chan->flags); + hci_send_acl(chan->conn->hchan, skb, flags); +} + +static void __unpack_enhanced_control(u16 enh, struct l2cap_ctrl *control) +{ + control->reqseq = (enh & L2CAP_CTRL_REQSEQ) >> L2CAP_CTRL_REQSEQ_SHIFT; + control->final = (enh & L2CAP_CTRL_FINAL) >> L2CAP_CTRL_FINAL_SHIFT; + + if (enh & L2CAP_CTRL_FRAME_TYPE) { + /* S-Frame */ + control->sframe = 1; + control->poll = (enh & L2CAP_CTRL_POLL) >> L2CAP_CTRL_POLL_SHIFT; + control->super = (enh & L2CAP_CTRL_SUPERVISE) >> L2CAP_CTRL_SUPER_SHIFT; + + control->sar = 0; + control->txseq = 0; + } else { + /* I-Frame */ + control->sframe = 0; + control->sar = (enh & L2CAP_CTRL_SAR) >> L2CAP_CTRL_SAR_SHIFT; + control->txseq = (enh & L2CAP_CTRL_TXSEQ) >> L2CAP_CTRL_TXSEQ_SHIFT; + + control->poll = 0; + control->super = 0; + } +} + +static void __unpack_extended_control(u32 ext, struct l2cap_ctrl *control) +{ + control->reqseq = (ext & L2CAP_EXT_CTRL_REQSEQ) >> L2CAP_EXT_CTRL_REQSEQ_SHIFT; + control->final = (ext & L2CAP_EXT_CTRL_FINAL) >> L2CAP_EXT_CTRL_FINAL_SHIFT; + + if (ext & L2CAP_EXT_CTRL_FRAME_TYPE) { + /* S-Frame */ + control->sframe = 1; + control->poll = (ext & L2CAP_EXT_CTRL_POLL) >> L2CAP_EXT_CTRL_POLL_SHIFT; + control->super = (ext & L2CAP_EXT_CTRL_SUPERVISE) >> L2CAP_EXT_CTRL_SUPER_SHIFT; + + control->sar = 0; + control->txseq = 0; + } else { + /* I-Frame */ + control->sframe = 0; + control->sar = (ext & L2CAP_EXT_CTRL_SAR) >> L2CAP_EXT_CTRL_SAR_SHIFT; + control->txseq = (ext & L2CAP_EXT_CTRL_TXSEQ) >> L2CAP_EXT_CTRL_TXSEQ_SHIFT; + + control->poll = 0; + control->super = 0; + } +} + +static inline void __unpack_control(struct l2cap_chan *chan, + struct sk_buff *skb) +{ + if (test_bit(FLAG_EXT_CTRL, &chan->flags)) { + __unpack_extended_control(get_unaligned_le32(skb->data), + &bt_cb(skb)->l2cap); + skb_pull(skb, L2CAP_EXT_CTRL_SIZE); + } else { + __unpack_enhanced_control(get_unaligned_le16(skb->data), + &bt_cb(skb)->l2cap); + skb_pull(skb, L2CAP_ENH_CTRL_SIZE); + } +} + +static u32 __pack_extended_control(struct l2cap_ctrl *control) +{ + u32 packed; + + packed = control->reqseq << L2CAP_EXT_CTRL_REQSEQ_SHIFT; + packed |= control->final << L2CAP_EXT_CTRL_FINAL_SHIFT; + + if (control->sframe) { + packed |= control->poll << L2CAP_EXT_CTRL_POLL_SHIFT; + packed |= control->super << L2CAP_EXT_CTRL_SUPER_SHIFT; + packed |= L2CAP_EXT_CTRL_FRAME_TYPE; + } else { + packed |= control->sar << L2CAP_EXT_CTRL_SAR_SHIFT; + packed |= control->txseq << L2CAP_EXT_CTRL_TXSEQ_SHIFT; + } + + return packed; +} + +static u16 __pack_enhanced_control(struct l2cap_ctrl *control) +{ + u16 packed; + + packed = control->reqseq << L2CAP_CTRL_REQSEQ_SHIFT; + packed |= control->final << L2CAP_CTRL_FINAL_SHIFT; + + if (control->sframe) { + packed |= control->poll << L2CAP_CTRL_POLL_SHIFT; + packed |= control->super << L2CAP_CTRL_SUPER_SHIFT; + packed |= L2CAP_CTRL_FRAME_TYPE; + } else { + packed |= control->sar << L2CAP_CTRL_SAR_SHIFT; + packed |= control->txseq << L2CAP_CTRL_TXSEQ_SHIFT; + } + + return packed; +} + +static inline void __pack_control(struct l2cap_chan *chan, + struct l2cap_ctrl *control, + struct sk_buff *skb) +{ + if (test_bit(FLAG_EXT_CTRL, &chan->flags)) { + put_unaligned_le32(__pack_extended_control(control), + skb->data + L2CAP_HDR_SIZE); + } else { + put_unaligned_le16(__pack_enhanced_control(control), + skb->data + L2CAP_HDR_SIZE); + } +} + +static inline unsigned int __ertm_hdr_size(struct l2cap_chan *chan) +{ + if (test_bit(FLAG_EXT_CTRL, &chan->flags)) + return L2CAP_EXT_HDR_SIZE; + else + return L2CAP_ENH_HDR_SIZE; +} + +static struct sk_buff *l2cap_create_sframe_pdu(struct l2cap_chan *chan, + u32 control) +{ + struct sk_buff *skb; + struct l2cap_hdr *lh; + int hlen = __ertm_hdr_size(chan); + + if (chan->fcs == L2CAP_FCS_CRC16) + hlen += L2CAP_FCS_SIZE; + + skb = bt_skb_alloc(hlen, GFP_KERNEL); + + if (!skb) + return ERR_PTR(-ENOMEM); + + lh = (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE); + lh->len = cpu_to_le16(hlen - L2CAP_HDR_SIZE); + lh->cid = cpu_to_le16(chan->dcid); + + if (test_bit(FLAG_EXT_CTRL, &chan->flags)) + put_unaligned_le32(control, skb_put(skb, L2CAP_EXT_CTRL_SIZE)); + else + put_unaligned_le16(control, skb_put(skb, L2CAP_ENH_CTRL_SIZE)); + + if (chan->fcs == L2CAP_FCS_CRC16) { + u16 fcs = crc16(0, (u8 *)skb->data, skb->len); + put_unaligned_le16(fcs, skb_put(skb, L2CAP_FCS_SIZE)); + } + + skb->priority = HCI_PRIO_MAX; + return skb; +} + +static void l2cap_send_sframe(struct l2cap_chan *chan, + struct l2cap_ctrl *control) +{ + struct sk_buff *skb; + u32 control_field; + + BT_DBG("chan %p, control %p", chan, control); + + if (!control->sframe) + return; + + if (__chan_is_moving(chan)) + return; + + if (test_and_clear_bit(CONN_SEND_FBIT, &chan->conn_state) && + !control->poll) + control->final = 1; + + if (control->super == L2CAP_SUPER_RR) + clear_bit(CONN_RNR_SENT, &chan->conn_state); + else if (control->super == L2CAP_SUPER_RNR) + set_bit(CONN_RNR_SENT, &chan->conn_state); + + if (control->super != L2CAP_SUPER_SREJ) { + chan->last_acked_seq = control->reqseq; + __clear_ack_timer(chan); + } + + BT_DBG("reqseq %d, final %d, poll %d, super %d", control->reqseq, + control->final, control->poll, control->super); + + if (test_bit(FLAG_EXT_CTRL, &chan->flags)) + control_field = __pack_extended_control(control); + else + control_field = __pack_enhanced_control(control); + + skb = l2cap_create_sframe_pdu(chan, control_field); + if (!IS_ERR(skb)) + l2cap_do_send(chan, skb); +} + +static void l2cap_send_rr_or_rnr(struct l2cap_chan *chan, bool poll) +{ + struct l2cap_ctrl control; + + BT_DBG("chan %p, poll %d", chan, poll); + + memset(&control, 0, sizeof(control)); + control.sframe = 1; + control.poll = poll; + + if (test_bit(CONN_LOCAL_BUSY, &chan->conn_state)) + control.super = L2CAP_SUPER_RNR; + else + control.super = L2CAP_SUPER_RR; + + control.reqseq = chan->buffer_seq; + l2cap_send_sframe(chan, &control); +} + +static inline int __l2cap_no_conn_pending(struct l2cap_chan *chan) +{ + if (chan->chan_type != L2CAP_CHAN_CONN_ORIENTED) + return true; + + return !test_bit(CONF_CONNECT_PEND, &chan->conf_state); +} + +static bool __amp_capable(struct l2cap_chan *chan) +{ + struct l2cap_conn *conn = chan->conn; + struct hci_dev *hdev; + bool amp_available = false; + + if (!(conn->local_fixed_chan & L2CAP_FC_A2MP)) + return false; + + if (!(conn->remote_fixed_chan & L2CAP_FC_A2MP)) + return false; + + read_lock(&hci_dev_list_lock); + list_for_each_entry(hdev, &hci_dev_list, list) { + if (hdev->amp_type != AMP_TYPE_BREDR && + test_bit(HCI_UP, &hdev->flags)) { + amp_available = true; + break; + } + } + read_unlock(&hci_dev_list_lock); + + if (chan->chan_policy == BT_CHANNEL_POLICY_AMP_PREFERRED) + return amp_available; + + return false; +} + +static bool l2cap_check_efs(struct l2cap_chan *chan) +{ + /* Check EFS parameters */ + return true; +} + +void l2cap_send_conn_req(struct l2cap_chan *chan) +{ + struct l2cap_conn *conn = chan->conn; + struct l2cap_conn_req req; + + req.scid = cpu_to_le16(chan->scid); + req.psm = chan->psm; + + chan->ident = l2cap_get_ident(conn); + + set_bit(CONF_CONNECT_PEND, &chan->conf_state); + + l2cap_send_cmd(conn, chan->ident, L2CAP_CONN_REQ, sizeof(req), &req); +} + +static void l2cap_send_create_chan_req(struct l2cap_chan *chan, u8 amp_id) +{ + struct l2cap_create_chan_req req; + req.scid = cpu_to_le16(chan->scid); + req.psm = chan->psm; + req.amp_id = amp_id; + + chan->ident = l2cap_get_ident(chan->conn); + + l2cap_send_cmd(chan->conn, chan->ident, L2CAP_CREATE_CHAN_REQ, + sizeof(req), &req); +} + +static void l2cap_move_setup(struct l2cap_chan *chan) +{ + struct sk_buff *skb; + + BT_DBG("chan %p", chan); + + if (chan->mode != L2CAP_MODE_ERTM) + return; + + __clear_retrans_timer(chan); + __clear_monitor_timer(chan); + __clear_ack_timer(chan); + + chan->retry_count = 0; + skb_queue_walk(&chan->tx_q, skb) { + if (bt_cb(skb)->l2cap.retries) + bt_cb(skb)->l2cap.retries = 1; + else + break; + } + + chan->expected_tx_seq = chan->buffer_seq; + + clear_bit(CONN_REJ_ACT, &chan->conn_state); + clear_bit(CONN_SREJ_ACT, &chan->conn_state); + l2cap_seq_list_clear(&chan->retrans_list); + l2cap_seq_list_clear(&chan->srej_list); + skb_queue_purge(&chan->srej_q); + + chan->tx_state = L2CAP_TX_STATE_XMIT; + chan->rx_state = L2CAP_RX_STATE_MOVE; + + set_bit(CONN_REMOTE_BUSY, &chan->conn_state); +} + +static void l2cap_move_done(struct l2cap_chan *chan) +{ + u8 move_role = chan->move_role; + BT_DBG("chan %p", chan); + + chan->move_state = L2CAP_MOVE_STABLE; + chan->move_role = L2CAP_MOVE_ROLE_NONE; + + if (chan->mode != L2CAP_MODE_ERTM) + return; + + switch (move_role) { + case L2CAP_MOVE_ROLE_INITIATOR: + l2cap_tx(chan, NULL, NULL, L2CAP_EV_EXPLICIT_POLL); + chan->rx_state = L2CAP_RX_STATE_WAIT_F; + break; + case L2CAP_MOVE_ROLE_RESPONDER: + chan->rx_state = L2CAP_RX_STATE_WAIT_P; + break; + } +} + +static void l2cap_chan_ready(struct l2cap_chan *chan) +{ + /* The channel may have already been flagged as connected in + * case of receiving data before the L2CAP info req/rsp + * procedure is complete. + */ + if (chan->state == BT_CONNECTED) + return; + + /* This clears all conf flags, including CONF_NOT_COMPLETE */ + chan->conf_state = 0; + __clear_chan_timer(chan); + + if (chan->mode == L2CAP_MODE_LE_FLOWCTL && !chan->tx_credits) + chan->ops->suspend(chan); + + chan->state = BT_CONNECTED; + + chan->ops->ready(chan); +} + +static void l2cap_le_connect(struct l2cap_chan *chan) +{ + struct l2cap_conn *conn = chan->conn; + struct l2cap_le_conn_req req; + + if (test_and_set_bit(FLAG_LE_CONN_REQ_SENT, &chan->flags)) + return; + + req.psm = chan->psm; + req.scid = cpu_to_le16(chan->scid); + req.mtu = cpu_to_le16(chan->imtu); + req.mps = cpu_to_le16(chan->mps); + req.credits = cpu_to_le16(chan->rx_credits); + + chan->ident = l2cap_get_ident(conn); + + l2cap_send_cmd(conn, chan->ident, L2CAP_LE_CONN_REQ, + sizeof(req), &req); +} + +static void l2cap_le_start(struct l2cap_chan *chan) +{ + struct l2cap_conn *conn = chan->conn; + + if (!smp_conn_security(conn->hcon, chan->sec_level)) + return; + + if (!chan->psm) { + l2cap_chan_ready(chan); + return; + } + + if (chan->state == BT_CONNECT) + l2cap_le_connect(chan); +} + +static void l2cap_start_connection(struct l2cap_chan *chan) +{ + if (__amp_capable(chan)) { + BT_DBG("chan %p AMP capable: discover AMPs", chan); + a2mp_discover_amp(chan); + } else if (chan->conn->hcon->type == LE_LINK) { + l2cap_le_start(chan); + } else { + l2cap_send_conn_req(chan); + } +} + +static void l2cap_request_info(struct l2cap_conn *conn) +{ + struct l2cap_info_req req; + + if (conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_SENT) + return; + + req.type = cpu_to_le16(L2CAP_IT_FEAT_MASK); + + conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_SENT; + conn->info_ident = l2cap_get_ident(conn); + + schedule_delayed_work(&conn->info_timer, L2CAP_INFO_TIMEOUT); + + l2cap_send_cmd(conn, conn->info_ident, L2CAP_INFO_REQ, + sizeof(req), &req); +} + +static void l2cap_do_start(struct l2cap_chan *chan) +{ + struct l2cap_conn *conn = chan->conn; + + if (conn->hcon->type == LE_LINK) { + l2cap_le_start(chan); + return; + } + + if (!(conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_SENT)) { + l2cap_request_info(conn); + return; + } + + if (!(conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_DONE)) + return; + + if (l2cap_chan_check_security(chan, true) && + __l2cap_no_conn_pending(chan)) + l2cap_start_connection(chan); +} + +static inline int l2cap_mode_supported(__u8 mode, __u32 feat_mask) +{ + u32 local_feat_mask = l2cap_feat_mask; + if (!disable_ertm) + local_feat_mask |= L2CAP_FEAT_ERTM | L2CAP_FEAT_STREAMING; + + switch (mode) { + case L2CAP_MODE_ERTM: + return L2CAP_FEAT_ERTM & feat_mask & local_feat_mask; + case L2CAP_MODE_STREAMING: + return L2CAP_FEAT_STREAMING & feat_mask & local_feat_mask; + default: + return 0x00; + } +} + +static void l2cap_send_disconn_req(struct l2cap_chan *chan, int err) +{ + struct l2cap_conn *conn = chan->conn; + struct l2cap_disconn_req req; + + if (!conn) + return; + + if (chan->mode == L2CAP_MODE_ERTM && chan->state == BT_CONNECTED) { + __clear_retrans_timer(chan); + __clear_monitor_timer(chan); + __clear_ack_timer(chan); + } + + if (chan->scid == L2CAP_CID_A2MP) { + l2cap_state_change(chan, BT_DISCONN); + return; + } + + req.dcid = cpu_to_le16(chan->dcid); + req.scid = cpu_to_le16(chan->scid); + l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_DISCONN_REQ, + sizeof(req), &req); + + l2cap_state_change_and_error(chan, BT_DISCONN, err); +} + +/* ---- L2CAP connections ---- */ +static void l2cap_conn_start(struct l2cap_conn *conn) +{ + struct l2cap_chan *chan, *tmp; + + BT_DBG("conn %p", conn); + + mutex_lock(&conn->chan_lock); + + list_for_each_entry_safe(chan, tmp, &conn->chan_l, list) { + l2cap_chan_lock(chan); + + if (chan->chan_type != L2CAP_CHAN_CONN_ORIENTED) { + l2cap_chan_ready(chan); + l2cap_chan_unlock(chan); + continue; + } + + if (chan->state == BT_CONNECT) { + if (!l2cap_chan_check_security(chan, true) || + !__l2cap_no_conn_pending(chan)) { + l2cap_chan_unlock(chan); + continue; + } + + if (!l2cap_mode_supported(chan->mode, conn->feat_mask) + && test_bit(CONF_STATE2_DEVICE, + &chan->conf_state)) { + l2cap_chan_close(chan, ECONNRESET); + l2cap_chan_unlock(chan); + continue; + } + + l2cap_start_connection(chan); + + } else if (chan->state == BT_CONNECT2) { + struct l2cap_conn_rsp rsp; + char buf[128]; + rsp.scid = cpu_to_le16(chan->dcid); + rsp.dcid = cpu_to_le16(chan->scid); + + if (l2cap_chan_check_security(chan, false)) { + if (test_bit(FLAG_DEFER_SETUP, &chan->flags)) { + rsp.result = cpu_to_le16(L2CAP_CR_PEND); + rsp.status = cpu_to_le16(L2CAP_CS_AUTHOR_PEND); + chan->ops->defer(chan); + + } else { + l2cap_state_change(chan, BT_CONFIG); + rsp.result = cpu_to_le16(L2CAP_CR_SUCCESS); + rsp.status = cpu_to_le16(L2CAP_CS_NO_INFO); + } + } else { + rsp.result = cpu_to_le16(L2CAP_CR_PEND); + rsp.status = cpu_to_le16(L2CAP_CS_AUTHEN_PEND); + } + + l2cap_send_cmd(conn, chan->ident, L2CAP_CONN_RSP, + sizeof(rsp), &rsp); + + if (test_bit(CONF_REQ_SENT, &chan->conf_state) || + rsp.result != L2CAP_CR_SUCCESS) { + l2cap_chan_unlock(chan); + continue; + } + + set_bit(CONF_REQ_SENT, &chan->conf_state); + l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ, + l2cap_build_conf_req(chan, buf), buf); + chan->num_conf_req++; + } + + l2cap_chan_unlock(chan); + } + + mutex_unlock(&conn->chan_lock); +} + +static void l2cap_le_conn_ready(struct l2cap_conn *conn) +{ + struct hci_conn *hcon = conn->hcon; + struct hci_dev *hdev = hcon->hdev; + + BT_DBG("%s conn %p", hdev->name, conn); + + /* For outgoing pairing which doesn't necessarily have an + * associated socket (e.g. mgmt_pair_device). + */ + if (hcon->out) + smp_conn_security(hcon, hcon->pending_sec_level); + + /* For LE slave connections, make sure the connection interval + * is in the range of the minium and maximum interval that has + * been configured for this connection. If not, then trigger + * the connection update procedure. + */ + if (hcon->role == HCI_ROLE_SLAVE && + (hcon->le_conn_interval < hcon->le_conn_min_interval || + hcon->le_conn_interval > hcon->le_conn_max_interval)) { + struct l2cap_conn_param_update_req req; + + req.min = cpu_to_le16(hcon->le_conn_min_interval); + req.max = cpu_to_le16(hcon->le_conn_max_interval); + req.latency = cpu_to_le16(hcon->le_conn_latency); + req.to_multiplier = cpu_to_le16(hcon->le_supv_timeout); + + l2cap_send_cmd(conn, l2cap_get_ident(conn), + L2CAP_CONN_PARAM_UPDATE_REQ, sizeof(req), &req); + } +} + +static void l2cap_conn_ready(struct l2cap_conn *conn) +{ + struct l2cap_chan *chan; + struct hci_conn *hcon = conn->hcon; + + BT_DBG("conn %p", conn); + + if (hcon->type == ACL_LINK) + l2cap_request_info(conn); + + mutex_lock(&conn->chan_lock); + + list_for_each_entry(chan, &conn->chan_l, list) { + + l2cap_chan_lock(chan); + + if (chan->scid == L2CAP_CID_A2MP) { + l2cap_chan_unlock(chan); + continue; + } + + if (hcon->type == LE_LINK) { + l2cap_le_start(chan); + } else if (chan->chan_type != L2CAP_CHAN_CONN_ORIENTED) { + if (conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_DONE) + l2cap_chan_ready(chan); + } else if (chan->state == BT_CONNECT) { + l2cap_do_start(chan); + } + + l2cap_chan_unlock(chan); + } + + mutex_unlock(&conn->chan_lock); + + if (hcon->type == LE_LINK) + l2cap_le_conn_ready(conn); + + queue_work(hcon->hdev->workqueue, &conn->pending_rx_work); +} + +/* Notify sockets that we cannot guaranty reliability anymore */ +static void l2cap_conn_unreliable(struct l2cap_conn *conn, int err) +{ + struct l2cap_chan *chan; + + BT_DBG("conn %p", conn); + + mutex_lock(&conn->chan_lock); + + list_for_each_entry(chan, &conn->chan_l, list) { + if (test_bit(FLAG_FORCE_RELIABLE, &chan->flags)) + l2cap_chan_set_err(chan, err); + } + + mutex_unlock(&conn->chan_lock); +} + +static void l2cap_info_timeout(struct work_struct *work) +{ + struct l2cap_conn *conn = container_of(work, struct l2cap_conn, + info_timer.work); + + conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_DONE; + conn->info_ident = 0; + + l2cap_conn_start(conn); +} + +/* + * l2cap_user + * External modules can register l2cap_user objects on l2cap_conn. The ->probe + * callback is called during registration. The ->remove callback is called + * during unregistration. + * An l2cap_user object can either be explicitly unregistered or when the + * underlying l2cap_conn object is deleted. This guarantees that l2cap->hcon, + * l2cap->hchan, .. are valid as long as the remove callback hasn't been called. + * External modules must own a reference to the l2cap_conn object if they intend + * to call l2cap_unregister_user(). The l2cap_conn object might get destroyed at + * any time if they don't. + */ + +int l2cap_register_user(struct l2cap_conn *conn, struct l2cap_user *user) +{ + struct hci_dev *hdev = conn->hcon->hdev; + int ret; + + /* We need to check whether l2cap_conn is registered. If it is not, we + * must not register the l2cap_user. l2cap_conn_del() is unregisters + * l2cap_conn objects, but doesn't provide its own locking. Instead, it + * relies on the parent hci_conn object to be locked. This itself relies + * on the hci_dev object to be locked. So we must lock the hci device + * here, too. */ + + hci_dev_lock(hdev); + + if (user->list.next || user->list.prev) { + ret = -EINVAL; + goto out_unlock; + } + + /* conn->hchan is NULL after l2cap_conn_del() was called */ + if (!conn->hchan) { + ret = -ENODEV; + goto out_unlock; + } + + ret = user->probe(conn, user); + if (ret) + goto out_unlock; + + list_add(&user->list, &conn->users); + ret = 0; + +out_unlock: + hci_dev_unlock(hdev); + return ret; +} +EXPORT_SYMBOL(l2cap_register_user); + +void l2cap_unregister_user(struct l2cap_conn *conn, struct l2cap_user *user) +{ + struct hci_dev *hdev = conn->hcon->hdev; + + hci_dev_lock(hdev); + + if (!user->list.next || !user->list.prev) + goto out_unlock; + + list_del(&user->list); + user->list.next = NULL; + user->list.prev = NULL; + user->remove(conn, user); + +out_unlock: + hci_dev_unlock(hdev); +} +EXPORT_SYMBOL(l2cap_unregister_user); + +static void l2cap_unregister_all_users(struct l2cap_conn *conn) +{ + struct l2cap_user *user; + + while (!list_empty(&conn->users)) { + user = list_first_entry(&conn->users, struct l2cap_user, list); + list_del(&user->list); + user->list.next = NULL; + user->list.prev = NULL; + user->remove(conn, user); + } +} + +static void l2cap_conn_del(struct hci_conn *hcon, int err) +{ + struct l2cap_conn *conn = hcon->l2cap_data; + struct l2cap_chan *chan, *l; + + if (!conn) + return; + + BT_DBG("hcon %p conn %p, err %d", hcon, conn, err); + + kfree_skb(conn->rx_skb); + + skb_queue_purge(&conn->pending_rx); + + /* We can not call flush_work(&conn->pending_rx_work) here since we + * might block if we are running on a worker from the same workqueue + * pending_rx_work is waiting on. + */ + if (work_pending(&conn->pending_rx_work)) + cancel_work_sync(&conn->pending_rx_work); + + if (work_pending(&conn->id_addr_update_work)) + cancel_work_sync(&conn->id_addr_update_work); + + l2cap_unregister_all_users(conn); + + /* Force the connection to be immediately dropped */ + hcon->disc_timeout = 0; + + mutex_lock(&conn->chan_lock); + + /* Kill channels */ + list_for_each_entry_safe(chan, l, &conn->chan_l, list) { + l2cap_chan_hold(chan); + l2cap_chan_lock(chan); + + l2cap_chan_del(chan, err); + + l2cap_chan_unlock(chan); + + chan->ops->close(chan); + l2cap_chan_put(chan); + } + + mutex_unlock(&conn->chan_lock); + + hci_chan_del(conn->hchan); + + if (conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_SENT) + cancel_delayed_work_sync(&conn->info_timer); + + hcon->l2cap_data = NULL; + conn->hchan = NULL; + l2cap_conn_put(conn); +} + +static void l2cap_conn_free(struct kref *ref) +{ + struct l2cap_conn *conn = container_of(ref, struct l2cap_conn, ref); + + hci_conn_put(conn->hcon); + kfree(conn); +} + +struct l2cap_conn *l2cap_conn_get(struct l2cap_conn *conn) +{ + kref_get(&conn->ref); + return conn; +} +EXPORT_SYMBOL(l2cap_conn_get); + +void l2cap_conn_put(struct l2cap_conn *conn) +{ + kref_put(&conn->ref, l2cap_conn_free); +} +EXPORT_SYMBOL(l2cap_conn_put); + +/* ---- Socket interface ---- */ + +/* Find socket with psm and source / destination bdaddr. + * Returns closest match. + */ +static struct l2cap_chan *l2cap_global_chan_by_psm(int state, __le16 psm, + bdaddr_t *src, + bdaddr_t *dst, + u8 link_type) +{ + struct l2cap_chan *c, *c1 = NULL; + + read_lock(&chan_list_lock); + + list_for_each_entry(c, &chan_list, global_l) { + if (state && c->state != state) + continue; + + if (link_type == ACL_LINK && c->src_type != BDADDR_BREDR) + continue; + + if (link_type == LE_LINK && c->src_type == BDADDR_BREDR) + continue; + + if (c->psm == psm) { + int src_match, dst_match; + int src_any, dst_any; + + /* Exact match. */ + src_match = !bacmp(&c->src, src); + dst_match = !bacmp(&c->dst, dst); + if (src_match && dst_match) { + l2cap_chan_hold(c); + read_unlock(&chan_list_lock); + return c; + } + + /* Closest match */ + src_any = !bacmp(&c->src, BDADDR_ANY); + dst_any = !bacmp(&c->dst, BDADDR_ANY); + if ((src_match && dst_any) || (src_any && dst_match) || + (src_any && dst_any)) + c1 = c; + } + } + + if (c1) + l2cap_chan_hold(c1); + + read_unlock(&chan_list_lock); + + return c1; +} + +static void l2cap_monitor_timeout(struct work_struct *work) +{ + struct l2cap_chan *chan = container_of(work, struct l2cap_chan, + monitor_timer.work); + + BT_DBG("chan %p", chan); + + l2cap_chan_lock(chan); + + if (!chan->conn) { + l2cap_chan_unlock(chan); + l2cap_chan_put(chan); + return; + } + + l2cap_tx(chan, NULL, NULL, L2CAP_EV_MONITOR_TO); + + l2cap_chan_unlock(chan); + l2cap_chan_put(chan); +} + +static void l2cap_retrans_timeout(struct work_struct *work) +{ + struct l2cap_chan *chan = container_of(work, struct l2cap_chan, + retrans_timer.work); + + BT_DBG("chan %p", chan); + + l2cap_chan_lock(chan); + + if (!chan->conn) { + l2cap_chan_unlock(chan); + l2cap_chan_put(chan); + return; + } + + l2cap_tx(chan, NULL, NULL, L2CAP_EV_RETRANS_TO); + l2cap_chan_unlock(chan); + l2cap_chan_put(chan); +} + +static void l2cap_streaming_send(struct l2cap_chan *chan, + struct sk_buff_head *skbs) +{ + struct sk_buff *skb; + struct l2cap_ctrl *control; + + BT_DBG("chan %p, skbs %p", chan, skbs); + + if (__chan_is_moving(chan)) + return; + + skb_queue_splice_tail_init(skbs, &chan->tx_q); + + while (!skb_queue_empty(&chan->tx_q)) { + + skb = skb_dequeue(&chan->tx_q); + + bt_cb(skb)->l2cap.retries = 1; + control = &bt_cb(skb)->l2cap; + + control->reqseq = 0; + control->txseq = chan->next_tx_seq; + + __pack_control(chan, control, skb); + + if (chan->fcs == L2CAP_FCS_CRC16) { + u16 fcs = crc16(0, (u8 *) skb->data, skb->len); + put_unaligned_le16(fcs, skb_put(skb, L2CAP_FCS_SIZE)); + } + + l2cap_do_send(chan, skb); + + BT_DBG("Sent txseq %u", control->txseq); + + chan->next_tx_seq = __next_seq(chan, chan->next_tx_seq); + chan->frames_sent++; + } +} + +static int l2cap_ertm_send(struct l2cap_chan *chan) +{ + struct sk_buff *skb, *tx_skb; + struct l2cap_ctrl *control; + int sent = 0; + + BT_DBG("chan %p", chan); + + if (chan->state != BT_CONNECTED) + return -ENOTCONN; + + if (test_bit(CONN_REMOTE_BUSY, &chan->conn_state)) + return 0; + + if (__chan_is_moving(chan)) + return 0; + + while (chan->tx_send_head && + chan->unacked_frames < chan->remote_tx_win && + chan->tx_state == L2CAP_TX_STATE_XMIT) { + + skb = chan->tx_send_head; + + bt_cb(skb)->l2cap.retries = 1; + control = &bt_cb(skb)->l2cap; + + if (test_and_clear_bit(CONN_SEND_FBIT, &chan->conn_state)) + control->final = 1; + + control->reqseq = chan->buffer_seq; + chan->last_acked_seq = chan->buffer_seq; + control->txseq = chan->next_tx_seq; + + __pack_control(chan, control, skb); + + if (chan->fcs == L2CAP_FCS_CRC16) { + u16 fcs = crc16(0, (u8 *) skb->data, skb->len); + put_unaligned_le16(fcs, skb_put(skb, L2CAP_FCS_SIZE)); + } + + /* Clone after data has been modified. Data is assumed to be + read-only (for locking purposes) on cloned sk_buffs. + */ + tx_skb = skb_clone(skb, GFP_KERNEL); + + if (!tx_skb) + break; + + __set_retrans_timer(chan); + + chan->next_tx_seq = __next_seq(chan, chan->next_tx_seq); + chan->unacked_frames++; + chan->frames_sent++; + sent++; + + if (skb_queue_is_last(&chan->tx_q, skb)) + chan->tx_send_head = NULL; + else + chan->tx_send_head = skb_queue_next(&chan->tx_q, skb); + + l2cap_do_send(chan, tx_skb); + BT_DBG("Sent txseq %u", control->txseq); + } + + BT_DBG("Sent %d, %u unacked, %u in ERTM queue", sent, + chan->unacked_frames, skb_queue_len(&chan->tx_q)); + + return sent; +} + +static void l2cap_ertm_resend(struct l2cap_chan *chan) +{ + struct l2cap_ctrl control; + struct sk_buff *skb; + struct sk_buff *tx_skb; + u16 seq; + + BT_DBG("chan %p", chan); + + if (test_bit(CONN_REMOTE_BUSY, &chan->conn_state)) + return; + + if (__chan_is_moving(chan)) + return; + + while (chan->retrans_list.head != L2CAP_SEQ_LIST_CLEAR) { + seq = l2cap_seq_list_pop(&chan->retrans_list); + + skb = l2cap_ertm_seq_in_queue(&chan->tx_q, seq); + if (!skb) { + BT_DBG("Error: Can't retransmit seq %d, frame missing", + seq); + continue; + } + + bt_cb(skb)->l2cap.retries++; + control = bt_cb(skb)->l2cap; + + if (chan->max_tx != 0 && + bt_cb(skb)->l2cap.retries > chan->max_tx) { + BT_DBG("Retry limit exceeded (%d)", chan->max_tx); + l2cap_send_disconn_req(chan, ECONNRESET); + l2cap_seq_list_clear(&chan->retrans_list); + break; + } + + control.reqseq = chan->buffer_seq; + if (test_and_clear_bit(CONN_SEND_FBIT, &chan->conn_state)) + control.final = 1; + else + control.final = 0; + + if (skb_cloned(skb)) { + /* Cloned sk_buffs are read-only, so we need a + * writeable copy + */ + tx_skb = skb_copy(skb, GFP_KERNEL); + } else { + tx_skb = skb_clone(skb, GFP_KERNEL); + } + + if (!tx_skb) { + l2cap_seq_list_clear(&chan->retrans_list); + break; + } + + /* Update skb contents */ + if (test_bit(FLAG_EXT_CTRL, &chan->flags)) { + put_unaligned_le32(__pack_extended_control(&control), + tx_skb->data + L2CAP_HDR_SIZE); + } else { + put_unaligned_le16(__pack_enhanced_control(&control), + tx_skb->data + L2CAP_HDR_SIZE); + } + + /* Update FCS */ + if (chan->fcs == L2CAP_FCS_CRC16) { + u16 fcs = crc16(0, (u8 *) tx_skb->data, + tx_skb->len - L2CAP_FCS_SIZE); + put_unaligned_le16(fcs, skb_tail_pointer(tx_skb) - + L2CAP_FCS_SIZE); + } + + l2cap_do_send(chan, tx_skb); + + BT_DBG("Resent txseq %d", control.txseq); + + chan->last_acked_seq = chan->buffer_seq; + } +} + +static void l2cap_retransmit(struct l2cap_chan *chan, + struct l2cap_ctrl *control) +{ + BT_DBG("chan %p, control %p", chan, control); + + l2cap_seq_list_append(&chan->retrans_list, control->reqseq); + l2cap_ertm_resend(chan); +} + +static void l2cap_retransmit_all(struct l2cap_chan *chan, + struct l2cap_ctrl *control) +{ + struct sk_buff *skb; + + BT_DBG("chan %p, control %p", chan, control); + + if (control->poll) + set_bit(CONN_SEND_FBIT, &chan->conn_state); + + l2cap_seq_list_clear(&chan->retrans_list); + + if (test_bit(CONN_REMOTE_BUSY, &chan->conn_state)) + return; + + if (chan->unacked_frames) { + skb_queue_walk(&chan->tx_q, skb) { + if (bt_cb(skb)->l2cap.txseq == control->reqseq || + skb == chan->tx_send_head) + break; + } + + skb_queue_walk_from(&chan->tx_q, skb) { + if (skb == chan->tx_send_head) + break; + + l2cap_seq_list_append(&chan->retrans_list, + bt_cb(skb)->l2cap.txseq); + } + + l2cap_ertm_resend(chan); + } +} + +static void l2cap_send_ack(struct l2cap_chan *chan) +{ + struct l2cap_ctrl control; + u16 frames_to_ack = __seq_offset(chan, chan->buffer_seq, + chan->last_acked_seq); + int threshold; + + BT_DBG("chan %p last_acked_seq %d buffer_seq %d", + chan, chan->last_acked_seq, chan->buffer_seq); + + memset(&control, 0, sizeof(control)); + control.sframe = 1; + + if (test_bit(CONN_LOCAL_BUSY, &chan->conn_state) && + chan->rx_state == L2CAP_RX_STATE_RECV) { + __clear_ack_timer(chan); + control.super = L2CAP_SUPER_RNR; + control.reqseq = chan->buffer_seq; + l2cap_send_sframe(chan, &control); + } else { + if (!test_bit(CONN_REMOTE_BUSY, &chan->conn_state)) { + l2cap_ertm_send(chan); + /* If any i-frames were sent, they included an ack */ + if (chan->buffer_seq == chan->last_acked_seq) + frames_to_ack = 0; + } + + /* Ack now if the window is 3/4ths full. + * Calculate without mul or div + */ + threshold = chan->ack_win; + threshold += threshold << 1; + threshold >>= 2; + + BT_DBG("frames_to_ack %u, threshold %d", frames_to_ack, + threshold); + + if (frames_to_ack >= threshold) { + __clear_ack_timer(chan); + control.super = L2CAP_SUPER_RR; + control.reqseq = chan->buffer_seq; + l2cap_send_sframe(chan, &control); + frames_to_ack = 0; + } + + if (frames_to_ack) + __set_ack_timer(chan); + } +} + +static inline int l2cap_skbuff_fromiovec(struct l2cap_chan *chan, + struct msghdr *msg, int len, + int count, struct sk_buff *skb) +{ + struct l2cap_conn *conn = chan->conn; + struct sk_buff **frag; + int sent = 0; + + if (copy_from_iter(skb_put(skb, count), count, &msg->msg_iter) != count) + return -EFAULT; + + sent += count; + len -= count; + + /* Continuation fragments (no L2CAP header) */ + frag = &skb_shinfo(skb)->frag_list; + while (len) { + struct sk_buff *tmp; + + count = min_t(unsigned int, conn->mtu, len); + + tmp = chan->ops->alloc_skb(chan, 0, count, + msg->msg_flags & MSG_DONTWAIT); + if (IS_ERR(tmp)) + return PTR_ERR(tmp); + + *frag = tmp; + + if (copy_from_iter(skb_put(*frag, count), count, + &msg->msg_iter) != count) + return -EFAULT; + + sent += count; + len -= count; + + skb->len += (*frag)->len; + skb->data_len += (*frag)->len; + + frag = &(*frag)->next; + } + + return sent; +} + +static struct sk_buff *l2cap_create_connless_pdu(struct l2cap_chan *chan, + struct msghdr *msg, size_t len) +{ + struct l2cap_conn *conn = chan->conn; + struct sk_buff *skb; + int err, count, hlen = L2CAP_HDR_SIZE + L2CAP_PSMLEN_SIZE; + struct l2cap_hdr *lh; + + BT_DBG("chan %p psm 0x%2.2x len %zu", chan, + __le16_to_cpu(chan->psm), len); + + count = min_t(unsigned int, (conn->mtu - hlen), len); + + skb = chan->ops->alloc_skb(chan, hlen, count, + msg->msg_flags & MSG_DONTWAIT); + if (IS_ERR(skb)) + return skb; + + /* Create L2CAP header */ + lh = (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE); + lh->cid = cpu_to_le16(chan->dcid); + lh->len = cpu_to_le16(len + L2CAP_PSMLEN_SIZE); + put_unaligned(chan->psm, (__le16 *) skb_put(skb, L2CAP_PSMLEN_SIZE)); + + err = l2cap_skbuff_fromiovec(chan, msg, len, count, skb); + if (unlikely(err < 0)) { + kfree_skb(skb); + return ERR_PTR(err); + } + return skb; +} + +static struct sk_buff *l2cap_create_basic_pdu(struct l2cap_chan *chan, + struct msghdr *msg, size_t len) +{ + struct l2cap_conn *conn = chan->conn; + struct sk_buff *skb; + int err, count; + struct l2cap_hdr *lh; + + BT_DBG("chan %p len %zu", chan, len); + + count = min_t(unsigned int, (conn->mtu - L2CAP_HDR_SIZE), len); + + skb = chan->ops->alloc_skb(chan, L2CAP_HDR_SIZE, count, + msg->msg_flags & MSG_DONTWAIT); + if (IS_ERR(skb)) + return skb; + + /* Create L2CAP header */ + lh = (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE); + lh->cid = cpu_to_le16(chan->dcid); + lh->len = cpu_to_le16(len); + + err = l2cap_skbuff_fromiovec(chan, msg, len, count, skb); + if (unlikely(err < 0)) { + kfree_skb(skb); + return ERR_PTR(err); + } + return skb; +} + +static struct sk_buff *l2cap_create_iframe_pdu(struct l2cap_chan *chan, + struct msghdr *msg, size_t len, + u16 sdulen) +{ + struct l2cap_conn *conn = chan->conn; + struct sk_buff *skb; + int err, count, hlen; + struct l2cap_hdr *lh; + + BT_DBG("chan %p len %zu", chan, len); + + if (!conn) + return ERR_PTR(-ENOTCONN); + + hlen = __ertm_hdr_size(chan); + + if (sdulen) + hlen += L2CAP_SDULEN_SIZE; + + if (chan->fcs == L2CAP_FCS_CRC16) + hlen += L2CAP_FCS_SIZE; + + count = min_t(unsigned int, (conn->mtu - hlen), len); + + skb = chan->ops->alloc_skb(chan, hlen, count, + msg->msg_flags & MSG_DONTWAIT); + if (IS_ERR(skb)) + return skb; + + /* Create L2CAP header */ + lh = (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE); + lh->cid = cpu_to_le16(chan->dcid); + lh->len = cpu_to_le16(len + (hlen - L2CAP_HDR_SIZE)); + + /* Control header is populated later */ + if (test_bit(FLAG_EXT_CTRL, &chan->flags)) + put_unaligned_le32(0, skb_put(skb, L2CAP_EXT_CTRL_SIZE)); + else + put_unaligned_le16(0, skb_put(skb, L2CAP_ENH_CTRL_SIZE)); + + if (sdulen) + put_unaligned_le16(sdulen, skb_put(skb, L2CAP_SDULEN_SIZE)); + + err = l2cap_skbuff_fromiovec(chan, msg, len, count, skb); + if (unlikely(err < 0)) { + kfree_skb(skb); + return ERR_PTR(err); + } + + bt_cb(skb)->l2cap.fcs = chan->fcs; + bt_cb(skb)->l2cap.retries = 0; + return skb; +} + +static int l2cap_segment_sdu(struct l2cap_chan *chan, + struct sk_buff_head *seg_queue, + struct msghdr *msg, size_t len) +{ + struct sk_buff *skb; + u16 sdu_len; + size_t pdu_len; + u8 sar; + + BT_DBG("chan %p, msg %p, len %zu", chan, msg, len); + + /* It is critical that ERTM PDUs fit in a single HCI fragment, + * so fragmented skbs are not used. The HCI layer's handling + * of fragmented skbs is not compatible with ERTM's queueing. + */ + + /* PDU size is derived from the HCI MTU */ + pdu_len = chan->conn->mtu; + + /* Constrain PDU size for BR/EDR connections */ + if (!chan->hs_hcon) + pdu_len = min_t(size_t, pdu_len, L2CAP_BREDR_MAX_PAYLOAD); + + /* Adjust for largest possible L2CAP overhead. */ + if (chan->fcs) + pdu_len -= L2CAP_FCS_SIZE; + + pdu_len -= __ertm_hdr_size(chan); + + /* Remote device may have requested smaller PDUs */ + pdu_len = min_t(size_t, pdu_len, chan->remote_mps); + + if (len <= pdu_len) { + sar = L2CAP_SAR_UNSEGMENTED; + sdu_len = 0; + pdu_len = len; + } else { + sar = L2CAP_SAR_START; + sdu_len = len; + } + + while (len > 0) { + skb = l2cap_create_iframe_pdu(chan, msg, pdu_len, sdu_len); + + if (IS_ERR(skb)) { + __skb_queue_purge(seg_queue); + return PTR_ERR(skb); + } + + bt_cb(skb)->l2cap.sar = sar; + __skb_queue_tail(seg_queue, skb); + + len -= pdu_len; + if (sdu_len) + sdu_len = 0; + + if (len <= pdu_len) { + sar = L2CAP_SAR_END; + pdu_len = len; + } else { + sar = L2CAP_SAR_CONTINUE; + } + } + + return 0; +} + +static struct sk_buff *l2cap_create_le_flowctl_pdu(struct l2cap_chan *chan, + struct msghdr *msg, + size_t len, u16 sdulen) +{ + struct l2cap_conn *conn = chan->conn; + struct sk_buff *skb; + int err, count, hlen; + struct l2cap_hdr *lh; + + BT_DBG("chan %p len %zu", chan, len); + + if (!conn) + return ERR_PTR(-ENOTCONN); + + hlen = L2CAP_HDR_SIZE; + + if (sdulen) + hlen += L2CAP_SDULEN_SIZE; + + count = min_t(unsigned int, (conn->mtu - hlen), len); + + skb = chan->ops->alloc_skb(chan, hlen, count, + msg->msg_flags & MSG_DONTWAIT); + if (IS_ERR(skb)) + return skb; + + /* Create L2CAP header */ + lh = (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE); + lh->cid = cpu_to_le16(chan->dcid); + lh->len = cpu_to_le16(len + (hlen - L2CAP_HDR_SIZE)); + + if (sdulen) + put_unaligned_le16(sdulen, skb_put(skb, L2CAP_SDULEN_SIZE)); + + err = l2cap_skbuff_fromiovec(chan, msg, len, count, skb); + if (unlikely(err < 0)) { + kfree_skb(skb); + return ERR_PTR(err); + } + + return skb; +} + +static int l2cap_segment_le_sdu(struct l2cap_chan *chan, + struct sk_buff_head *seg_queue, + struct msghdr *msg, size_t len) +{ + struct sk_buff *skb; + size_t pdu_len; + u16 sdu_len; + + BT_DBG("chan %p, msg %p, len %zu", chan, msg, len); + + sdu_len = len; + pdu_len = chan->remote_mps - L2CAP_SDULEN_SIZE; + + while (len > 0) { + if (len <= pdu_len) + pdu_len = len; + + skb = l2cap_create_le_flowctl_pdu(chan, msg, pdu_len, sdu_len); + if (IS_ERR(skb)) { + __skb_queue_purge(seg_queue); + return PTR_ERR(skb); + } + + __skb_queue_tail(seg_queue, skb); + + len -= pdu_len; + + if (sdu_len) { + sdu_len = 0; + pdu_len += L2CAP_SDULEN_SIZE; + } + } + + return 0; +} + +int l2cap_chan_send(struct l2cap_chan *chan, struct msghdr *msg, size_t len) +{ + struct sk_buff *skb; + int err; + struct sk_buff_head seg_queue; + + if (!chan->conn) + return -ENOTCONN; + + /* Connectionless channel */ + if (chan->chan_type == L2CAP_CHAN_CONN_LESS) { + skb = l2cap_create_connless_pdu(chan, msg, len); + if (IS_ERR(skb)) + return PTR_ERR(skb); + + /* Channel lock is released before requesting new skb and then + * reacquired thus we need to recheck channel state. + */ + if (chan->state != BT_CONNECTED) { + kfree_skb(skb); + return -ENOTCONN; + } + + l2cap_do_send(chan, skb); + return len; + } + + switch (chan->mode) { + case L2CAP_MODE_LE_FLOWCTL: + /* Check outgoing MTU */ + if (len > chan->omtu) + return -EMSGSIZE; + + if (!chan->tx_credits) + return -EAGAIN; + + __skb_queue_head_init(&seg_queue); + + err = l2cap_segment_le_sdu(chan, &seg_queue, msg, len); + + if (chan->state != BT_CONNECTED) { + __skb_queue_purge(&seg_queue); + err = -ENOTCONN; + } + + if (err) + return err; + + skb_queue_splice_tail_init(&seg_queue, &chan->tx_q); + + while (chan->tx_credits && !skb_queue_empty(&chan->tx_q)) { + l2cap_do_send(chan, skb_dequeue(&chan->tx_q)); + chan->tx_credits--; + } + + if (!chan->tx_credits) + chan->ops->suspend(chan); + + err = len; + + break; + + case L2CAP_MODE_BASIC: + /* Check outgoing MTU */ + if (len > chan->omtu) + return -EMSGSIZE; + + /* Create a basic PDU */ + skb = l2cap_create_basic_pdu(chan, msg, len); + if (IS_ERR(skb)) + return PTR_ERR(skb); + + /* Channel lock is released before requesting new skb and then + * reacquired thus we need to recheck channel state. + */ + if (chan->state != BT_CONNECTED) { + kfree_skb(skb); + return -ENOTCONN; + } + + l2cap_do_send(chan, skb); + err = len; + break; + + case L2CAP_MODE_ERTM: + case L2CAP_MODE_STREAMING: + /* Check outgoing MTU */ + if (len > chan->omtu) { + err = -EMSGSIZE; + break; + } + + __skb_queue_head_init(&seg_queue); + + /* Do segmentation before calling in to the state machine, + * since it's possible to block while waiting for memory + * allocation. + */ + err = l2cap_segment_sdu(chan, &seg_queue, msg, len); + + /* The channel could have been closed while segmenting, + * check that it is still connected. + */ + if (chan->state != BT_CONNECTED) { + __skb_queue_purge(&seg_queue); + err = -ENOTCONN; + } + + if (err) + break; + + if (chan->mode == L2CAP_MODE_ERTM) + l2cap_tx(chan, NULL, &seg_queue, L2CAP_EV_DATA_REQUEST); + else + l2cap_streaming_send(chan, &seg_queue); + + err = len; + + /* If the skbs were not queued for sending, they'll still be in + * seg_queue and need to be purged. + */ + __skb_queue_purge(&seg_queue); + break; + + default: + BT_DBG("bad state %1.1x", chan->mode); + err = -EBADFD; + } + + return err; +} +EXPORT_SYMBOL_GPL(l2cap_chan_send); + +static void l2cap_send_srej(struct l2cap_chan *chan, u16 txseq) +{ + struct l2cap_ctrl control; + u16 seq; + + BT_DBG("chan %p, txseq %u", chan, txseq); + + memset(&control, 0, sizeof(control)); + control.sframe = 1; + control.super = L2CAP_SUPER_SREJ; + + for (seq = chan->expected_tx_seq; seq != txseq; + seq = __next_seq(chan, seq)) { + if (!l2cap_ertm_seq_in_queue(&chan->srej_q, seq)) { + control.reqseq = seq; + l2cap_send_sframe(chan, &control); + l2cap_seq_list_append(&chan->srej_list, seq); + } + } + + chan->expected_tx_seq = __next_seq(chan, txseq); +} + +static void l2cap_send_srej_tail(struct l2cap_chan *chan) +{ + struct l2cap_ctrl control; + + BT_DBG("chan %p", chan); + + if (chan->srej_list.tail == L2CAP_SEQ_LIST_CLEAR) + return; + + memset(&control, 0, sizeof(control)); + control.sframe = 1; + control.super = L2CAP_SUPER_SREJ; + control.reqseq = chan->srej_list.tail; + l2cap_send_sframe(chan, &control); +} + +static void l2cap_send_srej_list(struct l2cap_chan *chan, u16 txseq) +{ + struct l2cap_ctrl control; + u16 initial_head; + u16 seq; + + BT_DBG("chan %p, txseq %u", chan, txseq); + + memset(&control, 0, sizeof(control)); + control.sframe = 1; + control.super = L2CAP_SUPER_SREJ; + + /* Capture initial list head to allow only one pass through the list. */ + initial_head = chan->srej_list.head; + + do { + seq = l2cap_seq_list_pop(&chan->srej_list); + if (seq == txseq || seq == L2CAP_SEQ_LIST_CLEAR) + break; + + control.reqseq = seq; + l2cap_send_sframe(chan, &control); + l2cap_seq_list_append(&chan->srej_list, seq); + } while (chan->srej_list.head != initial_head); +} + +static void l2cap_process_reqseq(struct l2cap_chan *chan, u16 reqseq) +{ + struct sk_buff *acked_skb; + u16 ackseq; + + BT_DBG("chan %p, reqseq %u", chan, reqseq); + + if (chan->unacked_frames == 0 || reqseq == chan->expected_ack_seq) + return; + + BT_DBG("expected_ack_seq %u, unacked_frames %u", + chan->expected_ack_seq, chan->unacked_frames); + + for (ackseq = chan->expected_ack_seq; ackseq != reqseq; + ackseq = __next_seq(chan, ackseq)) { + + acked_skb = l2cap_ertm_seq_in_queue(&chan->tx_q, ackseq); + if (acked_skb) { + skb_unlink(acked_skb, &chan->tx_q); + kfree_skb(acked_skb); + chan->unacked_frames--; + } + } + + chan->expected_ack_seq = reqseq; + + if (chan->unacked_frames == 0) + __clear_retrans_timer(chan); + + BT_DBG("unacked_frames %u", chan->unacked_frames); +} + +static void l2cap_abort_rx_srej_sent(struct l2cap_chan *chan) +{ + BT_DBG("chan %p", chan); + + chan->expected_tx_seq = chan->buffer_seq; + l2cap_seq_list_clear(&chan->srej_list); + skb_queue_purge(&chan->srej_q); + chan->rx_state = L2CAP_RX_STATE_RECV; +} + +static void l2cap_tx_state_xmit(struct l2cap_chan *chan, + struct l2cap_ctrl *control, + struct sk_buff_head *skbs, u8 event) +{ + BT_DBG("chan %p, control %p, skbs %p, event %d", chan, control, skbs, + event); + + switch (event) { + case L2CAP_EV_DATA_REQUEST: + if (chan->tx_send_head == NULL) + chan->tx_send_head = skb_peek(skbs); + + skb_queue_splice_tail_init(skbs, &chan->tx_q); + l2cap_ertm_send(chan); + break; + case L2CAP_EV_LOCAL_BUSY_DETECTED: + BT_DBG("Enter LOCAL_BUSY"); + set_bit(CONN_LOCAL_BUSY, &chan->conn_state); + + if (chan->rx_state == L2CAP_RX_STATE_SREJ_SENT) { + /* The SREJ_SENT state must be aborted if we are to + * enter the LOCAL_BUSY state. + */ + l2cap_abort_rx_srej_sent(chan); + } + + l2cap_send_ack(chan); + + break; + case L2CAP_EV_LOCAL_BUSY_CLEAR: + BT_DBG("Exit LOCAL_BUSY"); + clear_bit(CONN_LOCAL_BUSY, &chan->conn_state); + + if (test_bit(CONN_RNR_SENT, &chan->conn_state)) { + struct l2cap_ctrl local_control; + + memset(&local_control, 0, sizeof(local_control)); + local_control.sframe = 1; + local_control.super = L2CAP_SUPER_RR; + local_control.poll = 1; + local_control.reqseq = chan->buffer_seq; + l2cap_send_sframe(chan, &local_control); + + chan->retry_count = 1; + __set_monitor_timer(chan); + chan->tx_state = L2CAP_TX_STATE_WAIT_F; + } + break; + case L2CAP_EV_RECV_REQSEQ_AND_FBIT: + l2cap_process_reqseq(chan, control->reqseq); + break; + case L2CAP_EV_EXPLICIT_POLL: + l2cap_send_rr_or_rnr(chan, 1); + chan->retry_count = 1; + __set_monitor_timer(chan); + __clear_ack_timer(chan); + chan->tx_state = L2CAP_TX_STATE_WAIT_F; + break; + case L2CAP_EV_RETRANS_TO: + l2cap_send_rr_or_rnr(chan, 1); + chan->retry_count = 1; + __set_monitor_timer(chan); + chan->tx_state = L2CAP_TX_STATE_WAIT_F; + break; + case L2CAP_EV_RECV_FBIT: + /* Nothing to process */ + break; + default: + break; + } +} + +static void l2cap_tx_state_wait_f(struct l2cap_chan *chan, + struct l2cap_ctrl *control, + struct sk_buff_head *skbs, u8 event) +{ + BT_DBG("chan %p, control %p, skbs %p, event %d", chan, control, skbs, + event); + + switch (event) { + case L2CAP_EV_DATA_REQUEST: + if (chan->tx_send_head == NULL) + chan->tx_send_head = skb_peek(skbs); + /* Queue data, but don't send. */ + skb_queue_splice_tail_init(skbs, &chan->tx_q); + break; + case L2CAP_EV_LOCAL_BUSY_DETECTED: + BT_DBG("Enter LOCAL_BUSY"); + set_bit(CONN_LOCAL_BUSY, &chan->conn_state); + + if (chan->rx_state == L2CAP_RX_STATE_SREJ_SENT) { + /* The SREJ_SENT state must be aborted if we are to + * enter the LOCAL_BUSY state. + */ + l2cap_abort_rx_srej_sent(chan); + } + + l2cap_send_ack(chan); + + break; + case L2CAP_EV_LOCAL_BUSY_CLEAR: + BT_DBG("Exit LOCAL_BUSY"); + clear_bit(CONN_LOCAL_BUSY, &chan->conn_state); + + if (test_bit(CONN_RNR_SENT, &chan->conn_state)) { + struct l2cap_ctrl local_control; + memset(&local_control, 0, sizeof(local_control)); + local_control.sframe = 1; + local_control.super = L2CAP_SUPER_RR; + local_control.poll = 1; + local_control.reqseq = chan->buffer_seq; + l2cap_send_sframe(chan, &local_control); + + chan->retry_count = 1; + __set_monitor_timer(chan); + chan->tx_state = L2CAP_TX_STATE_WAIT_F; + } + break; + case L2CAP_EV_RECV_REQSEQ_AND_FBIT: + l2cap_process_reqseq(chan, control->reqseq); + + /* Fall through */ + + case L2CAP_EV_RECV_FBIT: + if (control && control->final) { + __clear_monitor_timer(chan); + if (chan->unacked_frames > 0) + __set_retrans_timer(chan); + chan->retry_count = 0; + chan->tx_state = L2CAP_TX_STATE_XMIT; + BT_DBG("recv fbit tx_state 0x2.2%x", chan->tx_state); + } + break; + case L2CAP_EV_EXPLICIT_POLL: + /* Ignore */ + break; + case L2CAP_EV_MONITOR_TO: + if (chan->max_tx == 0 || chan->retry_count < chan->max_tx) { + l2cap_send_rr_or_rnr(chan, 1); + __set_monitor_timer(chan); + chan->retry_count++; + } else { + l2cap_send_disconn_req(chan, ECONNABORTED); + } + break; + default: + break; + } +} + +static void l2cap_tx(struct l2cap_chan *chan, struct l2cap_ctrl *control, + struct sk_buff_head *skbs, u8 event) +{ + BT_DBG("chan %p, control %p, skbs %p, event %d, state %d", + chan, control, skbs, event, chan->tx_state); + + switch (chan->tx_state) { + case L2CAP_TX_STATE_XMIT: + l2cap_tx_state_xmit(chan, control, skbs, event); + break; + case L2CAP_TX_STATE_WAIT_F: + l2cap_tx_state_wait_f(chan, control, skbs, event); + break; + default: + /* Ignore event */ + break; + } +} + +static void l2cap_pass_to_tx(struct l2cap_chan *chan, + struct l2cap_ctrl *control) +{ + BT_DBG("chan %p, control %p", chan, control); + l2cap_tx(chan, control, NULL, L2CAP_EV_RECV_REQSEQ_AND_FBIT); +} + +static void l2cap_pass_to_tx_fbit(struct l2cap_chan *chan, + struct l2cap_ctrl *control) +{ + BT_DBG("chan %p, control %p", chan, control); + l2cap_tx(chan, control, NULL, L2CAP_EV_RECV_FBIT); +} + +/* Copy frame to all raw sockets on that connection */ +static void l2cap_raw_recv(struct l2cap_conn *conn, struct sk_buff *skb) +{ + struct sk_buff *nskb; + struct l2cap_chan *chan; + + BT_DBG("conn %p", conn); + + mutex_lock(&conn->chan_lock); + + list_for_each_entry(chan, &conn->chan_l, list) { + if (chan->chan_type != L2CAP_CHAN_RAW) + continue; + + /* Don't send frame to the channel it came from */ + if (bt_cb(skb)->l2cap.chan == chan) + continue; + + nskb = skb_clone(skb, GFP_KERNEL); + if (!nskb) + continue; + if (chan->ops->recv(chan, nskb)) + kfree_skb(nskb); + } + + mutex_unlock(&conn->chan_lock); +} + +/* ---- L2CAP signalling commands ---- */ +static struct sk_buff *l2cap_build_cmd(struct l2cap_conn *conn, u8 code, + u8 ident, u16 dlen, void *data) +{ + struct sk_buff *skb, **frag; + struct l2cap_cmd_hdr *cmd; + struct l2cap_hdr *lh; + int len, count; + + BT_DBG("conn %p, code 0x%2.2x, ident 0x%2.2x, len %u", + conn, code, ident, dlen); + + if (conn->mtu < L2CAP_HDR_SIZE + L2CAP_CMD_HDR_SIZE) + return NULL; + + len = L2CAP_HDR_SIZE + L2CAP_CMD_HDR_SIZE + dlen; + count = min_t(unsigned int, conn->mtu, len); + + skb = bt_skb_alloc(count, GFP_KERNEL); + if (!skb) + return NULL; + + lh = (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE); + lh->len = cpu_to_le16(L2CAP_CMD_HDR_SIZE + dlen); + + if (conn->hcon->type == LE_LINK) + lh->cid = cpu_to_le16(L2CAP_CID_LE_SIGNALING); + else + lh->cid = cpu_to_le16(L2CAP_CID_SIGNALING); + + cmd = (struct l2cap_cmd_hdr *) skb_put(skb, L2CAP_CMD_HDR_SIZE); + cmd->code = code; + cmd->ident = ident; + cmd->len = cpu_to_le16(dlen); + + if (dlen) { + count -= L2CAP_HDR_SIZE + L2CAP_CMD_HDR_SIZE; + memcpy(skb_put(skb, count), data, count); + data += count; + } + + len -= skb->len; + + /* Continuation fragments (no L2CAP header) */ + frag = &skb_shinfo(skb)->frag_list; + while (len) { + count = min_t(unsigned int, conn->mtu, len); + + *frag = bt_skb_alloc(count, GFP_KERNEL); + if (!*frag) + goto fail; + + memcpy(skb_put(*frag, count), data, count); + + len -= count; + data += count; + + frag = &(*frag)->next; + } + + return skb; + +fail: + kfree_skb(skb); + return NULL; +} + +static inline int l2cap_get_conf_opt(void **ptr, int *type, int *olen, + unsigned long *val) +{ + struct l2cap_conf_opt *opt = *ptr; + int len; + + len = L2CAP_CONF_OPT_SIZE + opt->len; + *ptr += len; + + *type = opt->type; + *olen = opt->len; + + switch (opt->len) { + case 1: + *val = *((u8 *) opt->val); + break; + + case 2: + *val = get_unaligned_le16(opt->val); + break; + + case 4: + *val = get_unaligned_le32(opt->val); + break; + + default: + *val = (unsigned long) opt->val; + break; + } + + BT_DBG("type 0x%2.2x len %u val 0x%lx", *type, opt->len, *val); + return len; +} + +static void l2cap_add_conf_opt(void **ptr, u8 type, u8 len, unsigned long val) +{ + struct l2cap_conf_opt *opt = *ptr; + + BT_DBG("type 0x%2.2x len %u val 0x%lx", type, len, val); + + opt->type = type; + opt->len = len; + + switch (len) { + case 1: + *((u8 *) opt->val) = val; + break; + + case 2: + put_unaligned_le16(val, opt->val); + break; + + case 4: + put_unaligned_le32(val, opt->val); + break; + + default: + memcpy(opt->val, (void *) val, len); + break; + } + + *ptr += L2CAP_CONF_OPT_SIZE + len; +} + +static void l2cap_add_opt_efs(void **ptr, struct l2cap_chan *chan) +{ + struct l2cap_conf_efs efs; + + switch (chan->mode) { + case L2CAP_MODE_ERTM: + efs.id = chan->local_id; + efs.stype = chan->local_stype; + efs.msdu = cpu_to_le16(chan->local_msdu); + efs.sdu_itime = cpu_to_le32(chan->local_sdu_itime); + efs.acc_lat = cpu_to_le32(L2CAP_DEFAULT_ACC_LAT); + efs.flush_to = cpu_to_le32(L2CAP_EFS_DEFAULT_FLUSH_TO); + break; + + case L2CAP_MODE_STREAMING: + efs.id = 1; + efs.stype = L2CAP_SERV_BESTEFFORT; + efs.msdu = cpu_to_le16(chan->local_msdu); + efs.sdu_itime = cpu_to_le32(chan->local_sdu_itime); + efs.acc_lat = 0; + efs.flush_to = 0; + break; + + default: + return; + } + + l2cap_add_conf_opt(ptr, L2CAP_CONF_EFS, sizeof(efs), + (unsigned long) &efs); +} + +static void l2cap_ack_timeout(struct work_struct *work) +{ + struct l2cap_chan *chan = container_of(work, struct l2cap_chan, + ack_timer.work); + u16 frames_to_ack; + + BT_DBG("chan %p", chan); + + l2cap_chan_lock(chan); + + frames_to_ack = __seq_offset(chan, chan->buffer_seq, + chan->last_acked_seq); + + if (frames_to_ack) + l2cap_send_rr_or_rnr(chan, 0); + + l2cap_chan_unlock(chan); + l2cap_chan_put(chan); +} + +int l2cap_ertm_init(struct l2cap_chan *chan) +{ + int err; + + chan->next_tx_seq = 0; + chan->expected_tx_seq = 0; + chan->expected_ack_seq = 0; + chan->unacked_frames = 0; + chan->buffer_seq = 0; + chan->frames_sent = 0; + chan->last_acked_seq = 0; + chan->sdu = NULL; + chan->sdu_last_frag = NULL; + chan->sdu_len = 0; + + skb_queue_head_init(&chan->tx_q); + + chan->local_amp_id = AMP_ID_BREDR; + chan->move_id = AMP_ID_BREDR; + chan->move_state = L2CAP_MOVE_STABLE; + chan->move_role = L2CAP_MOVE_ROLE_NONE; + + if (chan->mode != L2CAP_MODE_ERTM) + return 0; + + chan->rx_state = L2CAP_RX_STATE_RECV; + chan->tx_state = L2CAP_TX_STATE_XMIT; + + INIT_DELAYED_WORK(&chan->retrans_timer, l2cap_retrans_timeout); + INIT_DELAYED_WORK(&chan->monitor_timer, l2cap_monitor_timeout); + INIT_DELAYED_WORK(&chan->ack_timer, l2cap_ack_timeout); + + skb_queue_head_init(&chan->srej_q); + + err = l2cap_seq_list_init(&chan->srej_list, chan->tx_win); + if (err < 0) + return err; + + err = l2cap_seq_list_init(&chan->retrans_list, chan->remote_tx_win); + if (err < 0) + l2cap_seq_list_free(&chan->srej_list); + + return err; +} + +static inline __u8 l2cap_select_mode(__u8 mode, __u16 remote_feat_mask) +{ + switch (mode) { + case L2CAP_MODE_STREAMING: + case L2CAP_MODE_ERTM: + if (l2cap_mode_supported(mode, remote_feat_mask)) + return mode; + /* fall through */ + default: + return L2CAP_MODE_BASIC; + } +} + +static inline bool __l2cap_ews_supported(struct l2cap_conn *conn) +{ + return ((conn->local_fixed_chan & L2CAP_FC_A2MP) && + (conn->feat_mask & L2CAP_FEAT_EXT_WINDOW)); +} + +static inline bool __l2cap_efs_supported(struct l2cap_conn *conn) +{ + return ((conn->local_fixed_chan & L2CAP_FC_A2MP) && + (conn->feat_mask & L2CAP_FEAT_EXT_FLOW)); +} + +static void __l2cap_set_ertm_timeouts(struct l2cap_chan *chan, + struct l2cap_conf_rfc *rfc) +{ + if (chan->local_amp_id != AMP_ID_BREDR && chan->hs_hcon) { + u64 ertm_to = chan->hs_hcon->hdev->amp_be_flush_to; + + /* Class 1 devices have must have ERTM timeouts + * exceeding the Link Supervision Timeout. The + * default Link Supervision Timeout for AMP + * controllers is 10 seconds. + * + * Class 1 devices use 0xffffffff for their + * best-effort flush timeout, so the clamping logic + * will result in a timeout that meets the above + * requirement. ERTM timeouts are 16-bit values, so + * the maximum timeout is 65.535 seconds. + */ + + /* Convert timeout to milliseconds and round */ + ertm_to = DIV_ROUND_UP_ULL(ertm_to, 1000); + + /* This is the recommended formula for class 2 devices + * that start ERTM timers when packets are sent to the + * controller. + */ + ertm_to = 3 * ertm_to + 500; + + if (ertm_to > 0xffff) + ertm_to = 0xffff; + + rfc->retrans_timeout = cpu_to_le16((u16) ertm_to); + rfc->monitor_timeout = rfc->retrans_timeout; + } else { + rfc->retrans_timeout = cpu_to_le16(L2CAP_DEFAULT_RETRANS_TO); + rfc->monitor_timeout = cpu_to_le16(L2CAP_DEFAULT_MONITOR_TO); + } +} + +static inline void l2cap_txwin_setup(struct l2cap_chan *chan) +{ + if (chan->tx_win > L2CAP_DEFAULT_TX_WINDOW && + __l2cap_ews_supported(chan->conn)) { + /* use extended control field */ + set_bit(FLAG_EXT_CTRL, &chan->flags); + chan->tx_win_max = L2CAP_DEFAULT_EXT_WINDOW; + } else { + chan->tx_win = min_t(u16, chan->tx_win, + L2CAP_DEFAULT_TX_WINDOW); + chan->tx_win_max = L2CAP_DEFAULT_TX_WINDOW; + } + chan->ack_win = chan->tx_win; +} + +static int l2cap_build_conf_req(struct l2cap_chan *chan, void *data) +{ + struct l2cap_conf_req *req = data; + struct l2cap_conf_rfc rfc = { .mode = chan->mode }; + void *ptr = req->data; + u16 size; + + BT_DBG("chan %p", chan); + + if (chan->num_conf_req || chan->num_conf_rsp) + goto done; + + switch (chan->mode) { + case L2CAP_MODE_STREAMING: + case L2CAP_MODE_ERTM: + if (test_bit(CONF_STATE2_DEVICE, &chan->conf_state)) + break; + + if (__l2cap_efs_supported(chan->conn)) + set_bit(FLAG_EFS_ENABLE, &chan->flags); + + /* fall through */ + default: + chan->mode = l2cap_select_mode(rfc.mode, chan->conn->feat_mask); + break; + } + +done: + if (chan->imtu != L2CAP_DEFAULT_MTU) + l2cap_add_conf_opt(&ptr, L2CAP_CONF_MTU, 2, chan->imtu); + + switch (chan->mode) { + case L2CAP_MODE_BASIC: + if (disable_ertm) + break; + + if (!(chan->conn->feat_mask & L2CAP_FEAT_ERTM) && + !(chan->conn->feat_mask & L2CAP_FEAT_STREAMING)) + break; + + rfc.mode = L2CAP_MODE_BASIC; + rfc.txwin_size = 0; + rfc.max_transmit = 0; + rfc.retrans_timeout = 0; + rfc.monitor_timeout = 0; + rfc.max_pdu_size = 0; + + l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC, sizeof(rfc), + (unsigned long) &rfc); + break; + + case L2CAP_MODE_ERTM: + rfc.mode = L2CAP_MODE_ERTM; + rfc.max_transmit = chan->max_tx; + + __l2cap_set_ertm_timeouts(chan, &rfc); + + size = min_t(u16, L2CAP_DEFAULT_MAX_PDU_SIZE, chan->conn->mtu - + L2CAP_EXT_HDR_SIZE - L2CAP_SDULEN_SIZE - + L2CAP_FCS_SIZE); + rfc.max_pdu_size = cpu_to_le16(size); + + l2cap_txwin_setup(chan); + + rfc.txwin_size = min_t(u16, chan->tx_win, + L2CAP_DEFAULT_TX_WINDOW); + + l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC, sizeof(rfc), + (unsigned long) &rfc); + + if (test_bit(FLAG_EFS_ENABLE, &chan->flags)) + l2cap_add_opt_efs(&ptr, chan); + + if (test_bit(FLAG_EXT_CTRL, &chan->flags)) + l2cap_add_conf_opt(&ptr, L2CAP_CONF_EWS, 2, + chan->tx_win); + + if (chan->conn->feat_mask & L2CAP_FEAT_FCS) + if (chan->fcs == L2CAP_FCS_NONE || + test_bit(CONF_RECV_NO_FCS, &chan->conf_state)) { + chan->fcs = L2CAP_FCS_NONE; + l2cap_add_conf_opt(&ptr, L2CAP_CONF_FCS, 1, + chan->fcs); + } + break; + + case L2CAP_MODE_STREAMING: + l2cap_txwin_setup(chan); + rfc.mode = L2CAP_MODE_STREAMING; + rfc.txwin_size = 0; + rfc.max_transmit = 0; + rfc.retrans_timeout = 0; + rfc.monitor_timeout = 0; + + size = min_t(u16, L2CAP_DEFAULT_MAX_PDU_SIZE, chan->conn->mtu - + L2CAP_EXT_HDR_SIZE - L2CAP_SDULEN_SIZE - + L2CAP_FCS_SIZE); + rfc.max_pdu_size = cpu_to_le16(size); + + l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC, sizeof(rfc), + (unsigned long) &rfc); + + if (test_bit(FLAG_EFS_ENABLE, &chan->flags)) + l2cap_add_opt_efs(&ptr, chan); + + if (chan->conn->feat_mask & L2CAP_FEAT_FCS) + if (chan->fcs == L2CAP_FCS_NONE || + test_bit(CONF_RECV_NO_FCS, &chan->conf_state)) { + chan->fcs = L2CAP_FCS_NONE; + l2cap_add_conf_opt(&ptr, L2CAP_CONF_FCS, 1, + chan->fcs); + } + break; + } + + req->dcid = cpu_to_le16(chan->dcid); + req->flags = cpu_to_le16(0); + + return ptr - data; +} + +static int l2cap_parse_conf_req(struct l2cap_chan *chan, void *data) +{ + struct l2cap_conf_rsp *rsp = data; + void *ptr = rsp->data; + void *req = chan->conf_req; + int len = chan->conf_len; + int type, hint, olen; + unsigned long val; + struct l2cap_conf_rfc rfc = { .mode = L2CAP_MODE_BASIC }; + struct l2cap_conf_efs efs; + u8 remote_efs = 0; + u16 mtu = L2CAP_DEFAULT_MTU; + u16 result = L2CAP_CONF_SUCCESS; + u16 size; + + BT_DBG("chan %p", chan); + + while (len >= L2CAP_CONF_OPT_SIZE) { + len -= l2cap_get_conf_opt(&req, &type, &olen, &val); + + hint = type & L2CAP_CONF_HINT; + type &= L2CAP_CONF_MASK; + + switch (type) { + case L2CAP_CONF_MTU: + mtu = val; + break; + + case L2CAP_CONF_FLUSH_TO: + chan->flush_to = val; + break; + + case L2CAP_CONF_QOS: + break; + + case L2CAP_CONF_RFC: + if (olen == sizeof(rfc)) + memcpy(&rfc, (void *) val, olen); + break; + + case L2CAP_CONF_FCS: + if (val == L2CAP_FCS_NONE) + set_bit(CONF_RECV_NO_FCS, &chan->conf_state); + break; + + case L2CAP_CONF_EFS: + remote_efs = 1; + if (olen == sizeof(efs)) + memcpy(&efs, (void *) val, olen); + break; + + case L2CAP_CONF_EWS: + if (!(chan->conn->local_fixed_chan & L2CAP_FC_A2MP)) + return -ECONNREFUSED; + + set_bit(FLAG_EXT_CTRL, &chan->flags); + set_bit(CONF_EWS_RECV, &chan->conf_state); + chan->tx_win_max = L2CAP_DEFAULT_EXT_WINDOW; + chan->remote_tx_win = val; + break; + + default: + if (hint) + break; + + result = L2CAP_CONF_UNKNOWN; + *((u8 *) ptr++) = type; + break; + } + } + + if (chan->num_conf_rsp || chan->num_conf_req > 1) + goto done; + + switch (chan->mode) { + case L2CAP_MODE_STREAMING: + case L2CAP_MODE_ERTM: + if (!test_bit(CONF_STATE2_DEVICE, &chan->conf_state)) { + chan->mode = l2cap_select_mode(rfc.mode, + chan->conn->feat_mask); + break; + } + + if (remote_efs) { + if (__l2cap_efs_supported(chan->conn)) + set_bit(FLAG_EFS_ENABLE, &chan->flags); + else + return -ECONNREFUSED; + } + + if (chan->mode != rfc.mode) + return -ECONNREFUSED; + + break; + } + +done: + if (chan->mode != rfc.mode) { + result = L2CAP_CONF_UNACCEPT; + rfc.mode = chan->mode; + + if (chan->num_conf_rsp == 1) + return -ECONNREFUSED; + + l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC, sizeof(rfc), + (unsigned long) &rfc); + } + + if (result == L2CAP_CONF_SUCCESS) { + /* Configure output options and let the other side know + * which ones we don't like. */ + + if (mtu < L2CAP_DEFAULT_MIN_MTU) + result = L2CAP_CONF_UNACCEPT; + else { + chan->omtu = mtu; + set_bit(CONF_MTU_DONE, &chan->conf_state); + } + l2cap_add_conf_opt(&ptr, L2CAP_CONF_MTU, 2, chan->omtu); + + if (remote_efs) { + if (chan->local_stype != L2CAP_SERV_NOTRAFIC && + efs.stype != L2CAP_SERV_NOTRAFIC && + efs.stype != chan->local_stype) { + + result = L2CAP_CONF_UNACCEPT; + + if (chan->num_conf_req >= 1) + return -ECONNREFUSED; + + l2cap_add_conf_opt(&ptr, L2CAP_CONF_EFS, + sizeof(efs), + (unsigned long) &efs); + } else { + /* Send PENDING Conf Rsp */ + result = L2CAP_CONF_PENDING; + set_bit(CONF_LOC_CONF_PEND, &chan->conf_state); + } + } + + switch (rfc.mode) { + case L2CAP_MODE_BASIC: + chan->fcs = L2CAP_FCS_NONE; + set_bit(CONF_MODE_DONE, &chan->conf_state); + break; + + case L2CAP_MODE_ERTM: + if (!test_bit(CONF_EWS_RECV, &chan->conf_state)) + chan->remote_tx_win = rfc.txwin_size; + else + rfc.txwin_size = L2CAP_DEFAULT_TX_WINDOW; + + chan->remote_max_tx = rfc.max_transmit; + + size = min_t(u16, le16_to_cpu(rfc.max_pdu_size), + chan->conn->mtu - L2CAP_EXT_HDR_SIZE - + L2CAP_SDULEN_SIZE - L2CAP_FCS_SIZE); + rfc.max_pdu_size = cpu_to_le16(size); + chan->remote_mps = size; + + __l2cap_set_ertm_timeouts(chan, &rfc); + + set_bit(CONF_MODE_DONE, &chan->conf_state); + + l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC, + sizeof(rfc), (unsigned long) &rfc); + + if (test_bit(FLAG_EFS_ENABLE, &chan->flags)) { + chan->remote_id = efs.id; + chan->remote_stype = efs.stype; + chan->remote_msdu = le16_to_cpu(efs.msdu); + chan->remote_flush_to = + le32_to_cpu(efs.flush_to); + chan->remote_acc_lat = + le32_to_cpu(efs.acc_lat); + chan->remote_sdu_itime = + le32_to_cpu(efs.sdu_itime); + l2cap_add_conf_opt(&ptr, L2CAP_CONF_EFS, + sizeof(efs), + (unsigned long) &efs); + } + break; + + case L2CAP_MODE_STREAMING: + size = min_t(u16, le16_to_cpu(rfc.max_pdu_size), + chan->conn->mtu - L2CAP_EXT_HDR_SIZE - + L2CAP_SDULEN_SIZE - L2CAP_FCS_SIZE); + rfc.max_pdu_size = cpu_to_le16(size); + chan->remote_mps = size; + + set_bit(CONF_MODE_DONE, &chan->conf_state); + + l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC, sizeof(rfc), + (unsigned long) &rfc); + + break; + + default: + result = L2CAP_CONF_UNACCEPT; + + memset(&rfc, 0, sizeof(rfc)); + rfc.mode = chan->mode; + } + + if (result == L2CAP_CONF_SUCCESS) + set_bit(CONF_OUTPUT_DONE, &chan->conf_state); + } + rsp->scid = cpu_to_le16(chan->dcid); + rsp->result = cpu_to_le16(result); + rsp->flags = cpu_to_le16(0); + + return ptr - data; +} + +static int l2cap_parse_conf_rsp(struct l2cap_chan *chan, void *rsp, int len, + void *data, u16 *result) +{ + struct l2cap_conf_req *req = data; + void *ptr = req->data; + int type, olen; + unsigned long val; + struct l2cap_conf_rfc rfc = { .mode = L2CAP_MODE_BASIC }; + struct l2cap_conf_efs efs; + + BT_DBG("chan %p, rsp %p, len %d, req %p", chan, rsp, len, data); + + while (len >= L2CAP_CONF_OPT_SIZE) { + len -= l2cap_get_conf_opt(&rsp, &type, &olen, &val); + + switch (type) { + case L2CAP_CONF_MTU: + if (val < L2CAP_DEFAULT_MIN_MTU) { + *result = L2CAP_CONF_UNACCEPT; + chan->imtu = L2CAP_DEFAULT_MIN_MTU; + } else + chan->imtu = val; + l2cap_add_conf_opt(&ptr, L2CAP_CONF_MTU, 2, chan->imtu); + break; + + case L2CAP_CONF_FLUSH_TO: + chan->flush_to = val; + l2cap_add_conf_opt(&ptr, L2CAP_CONF_FLUSH_TO, + 2, chan->flush_to); + break; + + case L2CAP_CONF_RFC: + if (olen == sizeof(rfc)) + memcpy(&rfc, (void *)val, olen); + + if (test_bit(CONF_STATE2_DEVICE, &chan->conf_state) && + rfc.mode != chan->mode) + return -ECONNREFUSED; + + chan->fcs = 0; + + l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC, + sizeof(rfc), (unsigned long) &rfc); + break; + + case L2CAP_CONF_EWS: + chan->ack_win = min_t(u16, val, chan->ack_win); + l2cap_add_conf_opt(&ptr, L2CAP_CONF_EWS, 2, + chan->tx_win); + break; + + case L2CAP_CONF_EFS: + if (olen == sizeof(efs)) + memcpy(&efs, (void *)val, olen); + + if (chan->local_stype != L2CAP_SERV_NOTRAFIC && + efs.stype != L2CAP_SERV_NOTRAFIC && + efs.stype != chan->local_stype) + return -ECONNREFUSED; + + l2cap_add_conf_opt(&ptr, L2CAP_CONF_EFS, sizeof(efs), + (unsigned long) &efs); + break; + + case L2CAP_CONF_FCS: + if (*result == L2CAP_CONF_PENDING) + if (val == L2CAP_FCS_NONE) + set_bit(CONF_RECV_NO_FCS, + &chan->conf_state); + break; + } + } + + if (chan->mode == L2CAP_MODE_BASIC && chan->mode != rfc.mode) + return -ECONNREFUSED; + + chan->mode = rfc.mode; + + if (*result == L2CAP_CONF_SUCCESS || *result == L2CAP_CONF_PENDING) { + switch (rfc.mode) { + case L2CAP_MODE_ERTM: + chan->retrans_timeout = le16_to_cpu(rfc.retrans_timeout); + chan->monitor_timeout = le16_to_cpu(rfc.monitor_timeout); + chan->mps = le16_to_cpu(rfc.max_pdu_size); + if (!test_bit(FLAG_EXT_CTRL, &chan->flags)) + chan->ack_win = min_t(u16, chan->ack_win, + rfc.txwin_size); + + if (test_bit(FLAG_EFS_ENABLE, &chan->flags)) { + chan->local_msdu = le16_to_cpu(efs.msdu); + chan->local_sdu_itime = + le32_to_cpu(efs.sdu_itime); + chan->local_acc_lat = le32_to_cpu(efs.acc_lat); + chan->local_flush_to = + le32_to_cpu(efs.flush_to); + } + break; + + case L2CAP_MODE_STREAMING: + chan->mps = le16_to_cpu(rfc.max_pdu_size); + } + } + + req->dcid = cpu_to_le16(chan->dcid); + req->flags = cpu_to_le16(0); + + return ptr - data; +} + +static int l2cap_build_conf_rsp(struct l2cap_chan *chan, void *data, + u16 result, u16 flags) +{ + struct l2cap_conf_rsp *rsp = data; + void *ptr = rsp->data; + + BT_DBG("chan %p", chan); + + rsp->scid = cpu_to_le16(chan->dcid); + rsp->result = cpu_to_le16(result); + rsp->flags = cpu_to_le16(flags); + + return ptr - data; +} + +void __l2cap_le_connect_rsp_defer(struct l2cap_chan *chan) +{ + struct l2cap_le_conn_rsp rsp; + struct l2cap_conn *conn = chan->conn; + + BT_DBG("chan %p", chan); + + rsp.dcid = cpu_to_le16(chan->scid); + rsp.mtu = cpu_to_le16(chan->imtu); + rsp.mps = cpu_to_le16(chan->mps); + rsp.credits = cpu_to_le16(chan->rx_credits); + rsp.result = cpu_to_le16(L2CAP_CR_SUCCESS); + + l2cap_send_cmd(conn, chan->ident, L2CAP_LE_CONN_RSP, sizeof(rsp), + &rsp); +} + +void __l2cap_connect_rsp_defer(struct l2cap_chan *chan) +{ + struct l2cap_conn_rsp rsp; + struct l2cap_conn *conn = chan->conn; + u8 buf[128]; + u8 rsp_code; + + rsp.scid = cpu_to_le16(chan->dcid); + rsp.dcid = cpu_to_le16(chan->scid); + rsp.result = cpu_to_le16(L2CAP_CR_SUCCESS); + rsp.status = cpu_to_le16(L2CAP_CS_NO_INFO); + + if (chan->hs_hcon) + rsp_code = L2CAP_CREATE_CHAN_RSP; + else + rsp_code = L2CAP_CONN_RSP; + + BT_DBG("chan %p rsp_code %u", chan, rsp_code); + + l2cap_send_cmd(conn, chan->ident, rsp_code, sizeof(rsp), &rsp); + + if (test_and_set_bit(CONF_REQ_SENT, &chan->conf_state)) + return; + + l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ, + l2cap_build_conf_req(chan, buf), buf); + chan->num_conf_req++; +} + +static void l2cap_conf_rfc_get(struct l2cap_chan *chan, void *rsp, int len) +{ + int type, olen; + unsigned long val; + /* Use sane default values in case a misbehaving remote device + * did not send an RFC or extended window size option. + */ + u16 txwin_ext = chan->ack_win; + struct l2cap_conf_rfc rfc = { + .mode = chan->mode, + .retrans_timeout = cpu_to_le16(L2CAP_DEFAULT_RETRANS_TO), + .monitor_timeout = cpu_to_le16(L2CAP_DEFAULT_MONITOR_TO), + .max_pdu_size = cpu_to_le16(chan->imtu), + .txwin_size = min_t(u16, chan->ack_win, L2CAP_DEFAULT_TX_WINDOW), + }; + + BT_DBG("chan %p, rsp %p, len %d", chan, rsp, len); + + if ((chan->mode != L2CAP_MODE_ERTM) && (chan->mode != L2CAP_MODE_STREAMING)) + return; + + while (len >= L2CAP_CONF_OPT_SIZE) { + len -= l2cap_get_conf_opt(&rsp, &type, &olen, &val); + + switch (type) { + case L2CAP_CONF_RFC: + if (olen == sizeof(rfc)) + memcpy(&rfc, (void *)val, olen); + break; + case L2CAP_CONF_EWS: + txwin_ext = val; + break; + } + } + + switch (rfc.mode) { + case L2CAP_MODE_ERTM: + chan->retrans_timeout = le16_to_cpu(rfc.retrans_timeout); + chan->monitor_timeout = le16_to_cpu(rfc.monitor_timeout); + chan->mps = le16_to_cpu(rfc.max_pdu_size); + if (test_bit(FLAG_EXT_CTRL, &chan->flags)) + chan->ack_win = min_t(u16, chan->ack_win, txwin_ext); + else + chan->ack_win = min_t(u16, chan->ack_win, + rfc.txwin_size); + break; + case L2CAP_MODE_STREAMING: + chan->mps = le16_to_cpu(rfc.max_pdu_size); + } +} + +static inline int l2cap_command_rej(struct l2cap_conn *conn, + struct l2cap_cmd_hdr *cmd, u16 cmd_len, + u8 *data) +{ + struct l2cap_cmd_rej_unk *rej = (struct l2cap_cmd_rej_unk *) data; + + if (cmd_len < sizeof(*rej)) + return -EPROTO; + + if (rej->reason != L2CAP_REJ_NOT_UNDERSTOOD) + return 0; + + if ((conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_SENT) && + cmd->ident == conn->info_ident) { + cancel_delayed_work(&conn->info_timer); + + conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_DONE; + conn->info_ident = 0; + + l2cap_conn_start(conn); + } + + return 0; +} + +static struct l2cap_chan *l2cap_connect(struct l2cap_conn *conn, + struct l2cap_cmd_hdr *cmd, + u8 *data, u8 rsp_code, u8 amp_id) +{ + struct l2cap_conn_req *req = (struct l2cap_conn_req *) data; + struct l2cap_conn_rsp rsp; + struct l2cap_chan *chan = NULL, *pchan; + int result, status = L2CAP_CS_NO_INFO; + + u16 dcid = 0, scid = __le16_to_cpu(req->scid); + __le16 psm = req->psm; + + BT_DBG("psm 0x%2.2x scid 0x%4.4x", __le16_to_cpu(psm), scid); + + /* Check if we have socket listening on psm */ + pchan = l2cap_global_chan_by_psm(BT_LISTEN, psm, &conn->hcon->src, + &conn->hcon->dst, ACL_LINK); + if (!pchan) { + result = L2CAP_CR_BAD_PSM; + goto sendresp; + } + + mutex_lock(&conn->chan_lock); + l2cap_chan_lock(pchan); + + /* Check if the ACL is secure enough (if not SDP) */ + if (psm != cpu_to_le16(L2CAP_PSM_SDP) && + !hci_conn_check_link_mode(conn->hcon)) { + conn->disc_reason = HCI_ERROR_AUTH_FAILURE; + result = L2CAP_CR_SEC_BLOCK; + goto response; + } + + result = L2CAP_CR_NO_MEM; + + /* Check if we already have channel with that dcid */ + if (__l2cap_get_chan_by_dcid(conn, scid)) + goto response; + + chan = pchan->ops->new_connection(pchan); + if (!chan) + goto response; + + /* For certain devices (ex: HID mouse), support for authentication, + * pairing and bonding is optional. For such devices, inorder to avoid + * the ACL alive for too long after L2CAP disconnection, reset the ACL + * disc_timeout back to HCI_DISCONN_TIMEOUT during L2CAP connect. + */ + conn->hcon->disc_timeout = HCI_DISCONN_TIMEOUT; + + bacpy(&chan->src, &conn->hcon->src); + bacpy(&chan->dst, &conn->hcon->dst); + chan->src_type = bdaddr_src_type(conn->hcon); + chan->dst_type = bdaddr_dst_type(conn->hcon); + chan->psm = psm; + chan->dcid = scid; + chan->local_amp_id = amp_id; + + __l2cap_chan_add(conn, chan); + + dcid = chan->scid; + + __set_chan_timer(chan, chan->ops->get_sndtimeo(chan)); + + chan->ident = cmd->ident; + + if (conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_DONE) { + if (l2cap_chan_check_security(chan, false)) { + if (test_bit(FLAG_DEFER_SETUP, &chan->flags)) { + l2cap_state_change(chan, BT_CONNECT2); + result = L2CAP_CR_PEND; + status = L2CAP_CS_AUTHOR_PEND; + chan->ops->defer(chan); + } else { + /* Force pending result for AMP controllers. + * The connection will succeed after the + * physical link is up. + */ + if (amp_id == AMP_ID_BREDR) { + l2cap_state_change(chan, BT_CONFIG); + result = L2CAP_CR_SUCCESS; + } else { + l2cap_state_change(chan, BT_CONNECT2); + result = L2CAP_CR_PEND; + } + status = L2CAP_CS_NO_INFO; + } + } else { + l2cap_state_change(chan, BT_CONNECT2); + result = L2CAP_CR_PEND; + status = L2CAP_CS_AUTHEN_PEND; + } + } else { + l2cap_state_change(chan, BT_CONNECT2); + result = L2CAP_CR_PEND; + status = L2CAP_CS_NO_INFO; + } + +response: + l2cap_chan_unlock(pchan); + mutex_unlock(&conn->chan_lock); + l2cap_chan_put(pchan); + +sendresp: + rsp.scid = cpu_to_le16(scid); + rsp.dcid = cpu_to_le16(dcid); + rsp.result = cpu_to_le16(result); + rsp.status = cpu_to_le16(status); + l2cap_send_cmd(conn, cmd->ident, rsp_code, sizeof(rsp), &rsp); + + if (result == L2CAP_CR_PEND && status == L2CAP_CS_NO_INFO) { + struct l2cap_info_req info; + info.type = cpu_to_le16(L2CAP_IT_FEAT_MASK); + + conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_SENT; + conn->info_ident = l2cap_get_ident(conn); + + schedule_delayed_work(&conn->info_timer, L2CAP_INFO_TIMEOUT); + + l2cap_send_cmd(conn, conn->info_ident, L2CAP_INFO_REQ, + sizeof(info), &info); + } + + if (chan && !test_bit(CONF_REQ_SENT, &chan->conf_state) && + result == L2CAP_CR_SUCCESS) { + u8 buf[128]; + set_bit(CONF_REQ_SENT, &chan->conf_state); + l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ, + l2cap_build_conf_req(chan, buf), buf); + chan->num_conf_req++; + } + + return chan; +} + +static int l2cap_connect_req(struct l2cap_conn *conn, + struct l2cap_cmd_hdr *cmd, u16 cmd_len, u8 *data) +{ + struct hci_dev *hdev = conn->hcon->hdev; + struct hci_conn *hcon = conn->hcon; + + if (cmd_len < sizeof(struct l2cap_conn_req)) + return -EPROTO; + + hci_dev_lock(hdev); + if (hci_dev_test_flag(hdev, HCI_MGMT) && + !test_and_set_bit(HCI_CONN_MGMT_CONNECTED, &hcon->flags)) + mgmt_device_connected(hdev, hcon, 0, NULL, 0); + hci_dev_unlock(hdev); + + l2cap_connect(conn, cmd, data, L2CAP_CONN_RSP, 0); + return 0; +} + +static int l2cap_connect_create_rsp(struct l2cap_conn *conn, + struct l2cap_cmd_hdr *cmd, u16 cmd_len, + u8 *data) +{ + struct l2cap_conn_rsp *rsp = (struct l2cap_conn_rsp *) data; + u16 scid, dcid, result, status; + struct l2cap_chan *chan; + u8 req[128]; + int err; + + if (cmd_len < sizeof(*rsp)) + return -EPROTO; + + scid = __le16_to_cpu(rsp->scid); + dcid = __le16_to_cpu(rsp->dcid); + result = __le16_to_cpu(rsp->result); + status = __le16_to_cpu(rsp->status); + + BT_DBG("dcid 0x%4.4x scid 0x%4.4x result 0x%2.2x status 0x%2.2x", + dcid, scid, result, status); + + mutex_lock(&conn->chan_lock); + + if (scid) { + chan = __l2cap_get_chan_by_scid(conn, scid); + if (!chan) { + err = -EBADSLT; + goto unlock; + } + } else { + chan = __l2cap_get_chan_by_ident(conn, cmd->ident); + if (!chan) { + err = -EBADSLT; + goto unlock; + } + } + + err = 0; + + l2cap_chan_lock(chan); + + switch (result) { + case L2CAP_CR_SUCCESS: + l2cap_state_change(chan, BT_CONFIG); + chan->ident = 0; + chan->dcid = dcid; + clear_bit(CONF_CONNECT_PEND, &chan->conf_state); + + if (test_and_set_bit(CONF_REQ_SENT, &chan->conf_state)) + break; + + l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ, + l2cap_build_conf_req(chan, req), req); + chan->num_conf_req++; + break; + + case L2CAP_CR_PEND: + set_bit(CONF_CONNECT_PEND, &chan->conf_state); + break; + + default: + l2cap_chan_del(chan, ECONNREFUSED); + break; + } + + l2cap_chan_unlock(chan); + +unlock: + mutex_unlock(&conn->chan_lock); + + return err; +} + +static inline void set_default_fcs(struct l2cap_chan *chan) +{ + /* FCS is enabled only in ERTM or streaming mode, if one or both + * sides request it. + */ + if (chan->mode != L2CAP_MODE_ERTM && chan->mode != L2CAP_MODE_STREAMING) + chan->fcs = L2CAP_FCS_NONE; + else if (!test_bit(CONF_RECV_NO_FCS, &chan->conf_state)) + chan->fcs = L2CAP_FCS_CRC16; +} + +static void l2cap_send_efs_conf_rsp(struct l2cap_chan *chan, void *data, + u8 ident, u16 flags) +{ + struct l2cap_conn *conn = chan->conn; + + BT_DBG("conn %p chan %p ident %d flags 0x%4.4x", conn, chan, ident, + flags); + + clear_bit(CONF_LOC_CONF_PEND, &chan->conf_state); + set_bit(CONF_OUTPUT_DONE, &chan->conf_state); + + l2cap_send_cmd(conn, ident, L2CAP_CONF_RSP, + l2cap_build_conf_rsp(chan, data, + L2CAP_CONF_SUCCESS, flags), data); +} + +static void cmd_reject_invalid_cid(struct l2cap_conn *conn, u8 ident, + u16 scid, u16 dcid) +{ + struct l2cap_cmd_rej_cid rej; + + rej.reason = cpu_to_le16(L2CAP_REJ_INVALID_CID); + rej.scid = __cpu_to_le16(scid); + rej.dcid = __cpu_to_le16(dcid); + + l2cap_send_cmd(conn, ident, L2CAP_COMMAND_REJ, sizeof(rej), &rej); +} + +static inline int l2cap_config_req(struct l2cap_conn *conn, + struct l2cap_cmd_hdr *cmd, u16 cmd_len, + u8 *data) +{ + struct l2cap_conf_req *req = (struct l2cap_conf_req *) data; + u16 dcid, flags; + u8 rsp[64]; + struct l2cap_chan *chan; + int len, err = 0; + + if (cmd_len < sizeof(*req)) + return -EPROTO; + + dcid = __le16_to_cpu(req->dcid); + flags = __le16_to_cpu(req->flags); + + BT_DBG("dcid 0x%4.4x flags 0x%2.2x", dcid, flags); + + chan = l2cap_get_chan_by_scid(conn, dcid); + if (!chan) { + cmd_reject_invalid_cid(conn, cmd->ident, dcid, 0); + return 0; + } + + if (chan->state != BT_CONFIG && chan->state != BT_CONNECT2) { + cmd_reject_invalid_cid(conn, cmd->ident, chan->scid, + chan->dcid); + goto unlock; + } + + /* Reject if config buffer is too small. */ + len = cmd_len - sizeof(*req); + if (chan->conf_len + len > sizeof(chan->conf_req)) { + l2cap_send_cmd(conn, cmd->ident, L2CAP_CONF_RSP, + l2cap_build_conf_rsp(chan, rsp, + L2CAP_CONF_REJECT, flags), rsp); + goto unlock; + } + + /* Store config. */ + memcpy(chan->conf_req + chan->conf_len, req->data, len); + chan->conf_len += len; + + if (flags & L2CAP_CONF_FLAG_CONTINUATION) { + /* Incomplete config. Send empty response. */ + l2cap_send_cmd(conn, cmd->ident, L2CAP_CONF_RSP, + l2cap_build_conf_rsp(chan, rsp, + L2CAP_CONF_SUCCESS, flags), rsp); + goto unlock; + } + + /* Complete config. */ + len = l2cap_parse_conf_req(chan, rsp); + if (len < 0) { + l2cap_send_disconn_req(chan, ECONNRESET); + goto unlock; + } + + chan->ident = cmd->ident; + l2cap_send_cmd(conn, cmd->ident, L2CAP_CONF_RSP, len, rsp); + chan->num_conf_rsp++; + + /* Reset config buffer. */ + chan->conf_len = 0; + + if (!test_bit(CONF_OUTPUT_DONE, &chan->conf_state)) + goto unlock; + + if (test_bit(CONF_INPUT_DONE, &chan->conf_state)) { + set_default_fcs(chan); + + if (chan->mode == L2CAP_MODE_ERTM || + chan->mode == L2CAP_MODE_STREAMING) + err = l2cap_ertm_init(chan); + + if (err < 0) + l2cap_send_disconn_req(chan, -err); + else + l2cap_chan_ready(chan); + + goto unlock; + } + + if (!test_and_set_bit(CONF_REQ_SENT, &chan->conf_state)) { + u8 buf[64]; + l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ, + l2cap_build_conf_req(chan, buf), buf); + chan->num_conf_req++; + } + + /* Got Conf Rsp PENDING from remote side and assume we sent + Conf Rsp PENDING in the code above */ + if (test_bit(CONF_REM_CONF_PEND, &chan->conf_state) && + test_bit(CONF_LOC_CONF_PEND, &chan->conf_state)) { + + /* check compatibility */ + + /* Send rsp for BR/EDR channel */ + if (!chan->hs_hcon) + l2cap_send_efs_conf_rsp(chan, rsp, cmd->ident, flags); + else + chan->ident = cmd->ident; + } + +unlock: + l2cap_chan_unlock(chan); + return err; +} + +static inline int l2cap_config_rsp(struct l2cap_conn *conn, + struct l2cap_cmd_hdr *cmd, u16 cmd_len, + u8 *data) +{ + struct l2cap_conf_rsp *rsp = (struct l2cap_conf_rsp *)data; + u16 scid, flags, result; + struct l2cap_chan *chan; + int len = cmd_len - sizeof(*rsp); + int err = 0; + + if (cmd_len < sizeof(*rsp)) + return -EPROTO; + + scid = __le16_to_cpu(rsp->scid); + flags = __le16_to_cpu(rsp->flags); + result = __le16_to_cpu(rsp->result); + + BT_DBG("scid 0x%4.4x flags 0x%2.2x result 0x%2.2x len %d", scid, flags, + result, len); + + chan = l2cap_get_chan_by_scid(conn, scid); + if (!chan) + return 0; + + switch (result) { + case L2CAP_CONF_SUCCESS: + l2cap_conf_rfc_get(chan, rsp->data, len); + clear_bit(CONF_REM_CONF_PEND, &chan->conf_state); + break; + + case L2CAP_CONF_PENDING: + set_bit(CONF_REM_CONF_PEND, &chan->conf_state); + + if (test_bit(CONF_LOC_CONF_PEND, &chan->conf_state)) { + char buf[64]; + + len = l2cap_parse_conf_rsp(chan, rsp->data, len, + buf, &result); + if (len < 0) { + l2cap_send_disconn_req(chan, ECONNRESET); + goto done; + } + + if (!chan->hs_hcon) { + l2cap_send_efs_conf_rsp(chan, buf, cmd->ident, + 0); + } else { + if (l2cap_check_efs(chan)) { + amp_create_logical_link(chan); + chan->ident = cmd->ident; + } + } + } + goto done; + + case L2CAP_CONF_UNACCEPT: + if (chan->num_conf_rsp <= L2CAP_CONF_MAX_CONF_RSP) { + char req[64]; + + if (len > sizeof(req) - sizeof(struct l2cap_conf_req)) { + l2cap_send_disconn_req(chan, ECONNRESET); + goto done; + } + + /* throw out any old stored conf requests */ + result = L2CAP_CONF_SUCCESS; + len = l2cap_parse_conf_rsp(chan, rsp->data, len, + req, &result); + if (len < 0) { + l2cap_send_disconn_req(chan, ECONNRESET); + goto done; + } + + l2cap_send_cmd(conn, l2cap_get_ident(conn), + L2CAP_CONF_REQ, len, req); + chan->num_conf_req++; + if (result != L2CAP_CONF_SUCCESS) + goto done; + break; + } + + default: + l2cap_chan_set_err(chan, ECONNRESET); + + __set_chan_timer(chan, L2CAP_DISC_REJ_TIMEOUT); + l2cap_send_disconn_req(chan, ECONNRESET); + goto done; + } + + if (flags & L2CAP_CONF_FLAG_CONTINUATION) + goto done; + + set_bit(CONF_INPUT_DONE, &chan->conf_state); + + if (test_bit(CONF_OUTPUT_DONE, &chan->conf_state)) { + set_default_fcs(chan); + + if (chan->mode == L2CAP_MODE_ERTM || + chan->mode == L2CAP_MODE_STREAMING) + err = l2cap_ertm_init(chan); + + if (err < 0) + l2cap_send_disconn_req(chan, -err); + else + l2cap_chan_ready(chan); + } + +done: + l2cap_chan_unlock(chan); + return err; +} + +static inline int l2cap_disconnect_req(struct l2cap_conn *conn, + struct l2cap_cmd_hdr *cmd, u16 cmd_len, + u8 *data) +{ + struct l2cap_disconn_req *req = (struct l2cap_disconn_req *) data; + struct l2cap_disconn_rsp rsp; + u16 dcid, scid; + struct l2cap_chan *chan; + + if (cmd_len != sizeof(*req)) + return -EPROTO; + + scid = __le16_to_cpu(req->scid); + dcid = __le16_to_cpu(req->dcid); + + BT_DBG("scid 0x%4.4x dcid 0x%4.4x", scid, dcid); + + mutex_lock(&conn->chan_lock); + + chan = __l2cap_get_chan_by_scid(conn, dcid); + if (!chan) { + mutex_unlock(&conn->chan_lock); + cmd_reject_invalid_cid(conn, cmd->ident, dcid, scid); + return 0; + } + + l2cap_chan_lock(chan); + + rsp.dcid = cpu_to_le16(chan->scid); + rsp.scid = cpu_to_le16(chan->dcid); + l2cap_send_cmd(conn, cmd->ident, L2CAP_DISCONN_RSP, sizeof(rsp), &rsp); + + chan->ops->set_shutdown(chan); + + l2cap_chan_hold(chan); + l2cap_chan_del(chan, ECONNRESET); + + l2cap_chan_unlock(chan); + + chan->ops->close(chan); + l2cap_chan_put(chan); + + mutex_unlock(&conn->chan_lock); + + return 0; +} + +static inline int l2cap_disconnect_rsp(struct l2cap_conn *conn, + struct l2cap_cmd_hdr *cmd, u16 cmd_len, + u8 *data) +{ + struct l2cap_disconn_rsp *rsp = (struct l2cap_disconn_rsp *) data; + u16 dcid, scid; + struct l2cap_chan *chan; + + if (cmd_len != sizeof(*rsp)) + return -EPROTO; + + scid = __le16_to_cpu(rsp->scid); + dcid = __le16_to_cpu(rsp->dcid); + + BT_DBG("dcid 0x%4.4x scid 0x%4.4x", dcid, scid); + + mutex_lock(&conn->chan_lock); + + chan = __l2cap_get_chan_by_scid(conn, scid); + if (!chan) { + mutex_unlock(&conn->chan_lock); + return 0; + } + + l2cap_chan_lock(chan); + + l2cap_chan_hold(chan); + l2cap_chan_del(chan, 0); + + l2cap_chan_unlock(chan); + + chan->ops->close(chan); + l2cap_chan_put(chan); + + mutex_unlock(&conn->chan_lock); + + return 0; +} + +static inline int l2cap_information_req(struct l2cap_conn *conn, + struct l2cap_cmd_hdr *cmd, u16 cmd_len, + u8 *data) +{ + struct l2cap_info_req *req = (struct l2cap_info_req *) data; + u16 type; + + if (cmd_len != sizeof(*req)) + return -EPROTO; + + type = __le16_to_cpu(req->type); + + BT_DBG("type 0x%4.4x", type); + + if (type == L2CAP_IT_FEAT_MASK) { + u8 buf[8]; + u32 feat_mask = l2cap_feat_mask; + struct l2cap_info_rsp *rsp = (struct l2cap_info_rsp *) buf; + rsp->type = cpu_to_le16(L2CAP_IT_FEAT_MASK); + rsp->result = cpu_to_le16(L2CAP_IR_SUCCESS); + if (!disable_ertm) + feat_mask |= L2CAP_FEAT_ERTM | L2CAP_FEAT_STREAMING + | L2CAP_FEAT_FCS; + if (conn->local_fixed_chan & L2CAP_FC_A2MP) + feat_mask |= L2CAP_FEAT_EXT_FLOW + | L2CAP_FEAT_EXT_WINDOW; + + put_unaligned_le32(feat_mask, rsp->data); + l2cap_send_cmd(conn, cmd->ident, L2CAP_INFO_RSP, sizeof(buf), + buf); + } else if (type == L2CAP_IT_FIXED_CHAN) { + u8 buf[12]; + struct l2cap_info_rsp *rsp = (struct l2cap_info_rsp *) buf; + + rsp->type = cpu_to_le16(L2CAP_IT_FIXED_CHAN); + rsp->result = cpu_to_le16(L2CAP_IR_SUCCESS); + rsp->data[0] = conn->local_fixed_chan; + memset(rsp->data + 1, 0, 7); + l2cap_send_cmd(conn, cmd->ident, L2CAP_INFO_RSP, sizeof(buf), + buf); + } else { + struct l2cap_info_rsp rsp; + rsp.type = cpu_to_le16(type); + rsp.result = cpu_to_le16(L2CAP_IR_NOTSUPP); + l2cap_send_cmd(conn, cmd->ident, L2CAP_INFO_RSP, sizeof(rsp), + &rsp); + } + + return 0; +} + +static inline int l2cap_information_rsp(struct l2cap_conn *conn, + struct l2cap_cmd_hdr *cmd, u16 cmd_len, + u8 *data) +{ + struct l2cap_info_rsp *rsp = (struct l2cap_info_rsp *) data; + u16 type, result; + + if (cmd_len < sizeof(*rsp)) + return -EPROTO; + + type = __le16_to_cpu(rsp->type); + result = __le16_to_cpu(rsp->result); + + BT_DBG("type 0x%4.4x result 0x%2.2x", type, result); + + /* L2CAP Info req/rsp are unbound to channels, add extra checks */ + if (cmd->ident != conn->info_ident || + conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_DONE) + return 0; + + cancel_delayed_work(&conn->info_timer); + + if (result != L2CAP_IR_SUCCESS) { + conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_DONE; + conn->info_ident = 0; + + l2cap_conn_start(conn); + + return 0; + } + + switch (type) { + case L2CAP_IT_FEAT_MASK: + conn->feat_mask = get_unaligned_le32(rsp->data); + + if (conn->feat_mask & L2CAP_FEAT_FIXED_CHAN) { + struct l2cap_info_req req; + req.type = cpu_to_le16(L2CAP_IT_FIXED_CHAN); + + conn->info_ident = l2cap_get_ident(conn); + + l2cap_send_cmd(conn, conn->info_ident, + L2CAP_INFO_REQ, sizeof(req), &req); + } else { + conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_DONE; + conn->info_ident = 0; + + l2cap_conn_start(conn); + } + break; + + case L2CAP_IT_FIXED_CHAN: + conn->remote_fixed_chan = rsp->data[0]; + conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_DONE; + conn->info_ident = 0; + + l2cap_conn_start(conn); + break; + } + + return 0; +} + +static int l2cap_create_channel_req(struct l2cap_conn *conn, + struct l2cap_cmd_hdr *cmd, + u16 cmd_len, void *data) +{ + struct l2cap_create_chan_req *req = data; + struct l2cap_create_chan_rsp rsp; + struct l2cap_chan *chan; + struct hci_dev *hdev; + u16 psm, scid; + + if (cmd_len != sizeof(*req)) + return -EPROTO; + + if (!(conn->local_fixed_chan & L2CAP_FC_A2MP)) + return -EINVAL; + + psm = le16_to_cpu(req->psm); + scid = le16_to_cpu(req->scid); + + BT_DBG("psm 0x%2.2x, scid 0x%4.4x, amp_id %d", psm, scid, req->amp_id); + + /* For controller id 0 make BR/EDR connection */ + if (req->amp_id == AMP_ID_BREDR) { + l2cap_connect(conn, cmd, data, L2CAP_CREATE_CHAN_RSP, + req->amp_id); + return 0; + } + + /* Validate AMP controller id */ + hdev = hci_dev_get(req->amp_id); + if (!hdev) + goto error; + + if (hdev->dev_type != HCI_AMP || !test_bit(HCI_UP, &hdev->flags)) { + hci_dev_put(hdev); + goto error; + } + + chan = l2cap_connect(conn, cmd, data, L2CAP_CREATE_CHAN_RSP, + req->amp_id); + if (chan) { + struct amp_mgr *mgr = conn->hcon->amp_mgr; + struct hci_conn *hs_hcon; + + hs_hcon = hci_conn_hash_lookup_ba(hdev, AMP_LINK, + &conn->hcon->dst); + if (!hs_hcon) { + hci_dev_put(hdev); + cmd_reject_invalid_cid(conn, cmd->ident, chan->scid, + chan->dcid); + return 0; + } + + BT_DBG("mgr %p bredr_chan %p hs_hcon %p", mgr, chan, hs_hcon); + + mgr->bredr_chan = chan; + chan->hs_hcon = hs_hcon; + chan->fcs = L2CAP_FCS_NONE; + conn->mtu = hdev->block_mtu; + } + + hci_dev_put(hdev); + + return 0; + +error: + rsp.dcid = 0; + rsp.scid = cpu_to_le16(scid); + rsp.result = cpu_to_le16(L2CAP_CR_BAD_AMP); + rsp.status = cpu_to_le16(L2CAP_CS_NO_INFO); + + l2cap_send_cmd(conn, cmd->ident, L2CAP_CREATE_CHAN_RSP, + sizeof(rsp), &rsp); + + return 0; +} + +static void l2cap_send_move_chan_req(struct l2cap_chan *chan, u8 dest_amp_id) +{ + struct l2cap_move_chan_req req; + u8 ident; + + BT_DBG("chan %p, dest_amp_id %d", chan, dest_amp_id); + + ident = l2cap_get_ident(chan->conn); + chan->ident = ident; + + req.icid = cpu_to_le16(chan->scid); + req.dest_amp_id = dest_amp_id; + + l2cap_send_cmd(chan->conn, ident, L2CAP_MOVE_CHAN_REQ, sizeof(req), + &req); + + __set_chan_timer(chan, L2CAP_MOVE_TIMEOUT); +} + +static void l2cap_send_move_chan_rsp(struct l2cap_chan *chan, u16 result) +{ + struct l2cap_move_chan_rsp rsp; + + BT_DBG("chan %p, result 0x%4.4x", chan, result); + + rsp.icid = cpu_to_le16(chan->dcid); + rsp.result = cpu_to_le16(result); + + l2cap_send_cmd(chan->conn, chan->ident, L2CAP_MOVE_CHAN_RSP, + sizeof(rsp), &rsp); +} + +static void l2cap_send_move_chan_cfm(struct l2cap_chan *chan, u16 result) +{ + struct l2cap_move_chan_cfm cfm; + + BT_DBG("chan %p, result 0x%4.4x", chan, result); + + chan->ident = l2cap_get_ident(chan->conn); + + cfm.icid = cpu_to_le16(chan->scid); + cfm.result = cpu_to_le16(result); + + l2cap_send_cmd(chan->conn, chan->ident, L2CAP_MOVE_CHAN_CFM, + sizeof(cfm), &cfm); + + __set_chan_timer(chan, L2CAP_MOVE_TIMEOUT); +} + +static void l2cap_send_move_chan_cfm_icid(struct l2cap_conn *conn, u16 icid) +{ + struct l2cap_move_chan_cfm cfm; + + BT_DBG("conn %p, icid 0x%4.4x", conn, icid); + + cfm.icid = cpu_to_le16(icid); + cfm.result = cpu_to_le16(L2CAP_MC_UNCONFIRMED); + + l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_MOVE_CHAN_CFM, + sizeof(cfm), &cfm); +} + +static void l2cap_send_move_chan_cfm_rsp(struct l2cap_conn *conn, u8 ident, + u16 icid) +{ + struct l2cap_move_chan_cfm_rsp rsp; + + BT_DBG("icid 0x%4.4x", icid); + + rsp.icid = cpu_to_le16(icid); + l2cap_send_cmd(conn, ident, L2CAP_MOVE_CHAN_CFM_RSP, sizeof(rsp), &rsp); +} + +static void __release_logical_link(struct l2cap_chan *chan) +{ + chan->hs_hchan = NULL; + chan->hs_hcon = NULL; + + /* Placeholder - release the logical link */ +} + +static void l2cap_logical_fail(struct l2cap_chan *chan) +{ + /* Logical link setup failed */ + if (chan->state != BT_CONNECTED) { + /* Create channel failure, disconnect */ + l2cap_send_disconn_req(chan, ECONNRESET); + return; + } + + switch (chan->move_role) { + case L2CAP_MOVE_ROLE_RESPONDER: + l2cap_move_done(chan); + l2cap_send_move_chan_rsp(chan, L2CAP_MR_NOT_SUPP); + break; + case L2CAP_MOVE_ROLE_INITIATOR: + if (chan->move_state == L2CAP_MOVE_WAIT_LOGICAL_COMP || + chan->move_state == L2CAP_MOVE_WAIT_LOGICAL_CFM) { + /* Remote has only sent pending or + * success responses, clean up + */ + l2cap_move_done(chan); + } + + /* Other amp move states imply that the move + * has already aborted + */ + l2cap_send_move_chan_cfm(chan, L2CAP_MC_UNCONFIRMED); + break; + } +} + +static void l2cap_logical_finish_create(struct l2cap_chan *chan, + struct hci_chan *hchan) +{ + struct l2cap_conf_rsp rsp; + + chan->hs_hchan = hchan; + chan->hs_hcon->l2cap_data = chan->conn; + + l2cap_send_efs_conf_rsp(chan, &rsp, chan->ident, 0); + + if (test_bit(CONF_INPUT_DONE, &chan->conf_state)) { + int err; + + set_default_fcs(chan); + + err = l2cap_ertm_init(chan); + if (err < 0) + l2cap_send_disconn_req(chan, -err); + else + l2cap_chan_ready(chan); + } +} + +static void l2cap_logical_finish_move(struct l2cap_chan *chan, + struct hci_chan *hchan) +{ + chan->hs_hcon = hchan->conn; + chan->hs_hcon->l2cap_data = chan->conn; + + BT_DBG("move_state %d", chan->move_state); + + switch (chan->move_state) { + case L2CAP_MOVE_WAIT_LOGICAL_COMP: + /* Move confirm will be sent after a success + * response is received + */ + chan->move_state = L2CAP_MOVE_WAIT_RSP_SUCCESS; + break; + case L2CAP_MOVE_WAIT_LOGICAL_CFM: + if (test_bit(CONN_LOCAL_BUSY, &chan->conn_state)) { + chan->move_state = L2CAP_MOVE_WAIT_LOCAL_BUSY; + } else if (chan->move_role == L2CAP_MOVE_ROLE_INITIATOR) { + chan->move_state = L2CAP_MOVE_WAIT_CONFIRM_RSP; + l2cap_send_move_chan_cfm(chan, L2CAP_MC_CONFIRMED); + } else if (chan->move_role == L2CAP_MOVE_ROLE_RESPONDER) { + chan->move_state = L2CAP_MOVE_WAIT_CONFIRM; + l2cap_send_move_chan_rsp(chan, L2CAP_MR_SUCCESS); + } + break; + default: + /* Move was not in expected state, free the channel */ + __release_logical_link(chan); + + chan->move_state = L2CAP_MOVE_STABLE; + } +} + +/* Call with chan locked */ +void l2cap_logical_cfm(struct l2cap_chan *chan, struct hci_chan *hchan, + u8 status) +{ + BT_DBG("chan %p, hchan %p, status %d", chan, hchan, status); + + if (status) { + l2cap_logical_fail(chan); + __release_logical_link(chan); + return; + } + + if (chan->state != BT_CONNECTED) { + /* Ignore logical link if channel is on BR/EDR */ + if (chan->local_amp_id != AMP_ID_BREDR) + l2cap_logical_finish_create(chan, hchan); + } else { + l2cap_logical_finish_move(chan, hchan); + } +} + +void l2cap_move_start(struct l2cap_chan *chan) +{ + BT_DBG("chan %p", chan); + + if (chan->local_amp_id == AMP_ID_BREDR) { + if (chan->chan_policy != BT_CHANNEL_POLICY_AMP_PREFERRED) + return; + chan->move_role = L2CAP_MOVE_ROLE_INITIATOR; + chan->move_state = L2CAP_MOVE_WAIT_PREPARE; + /* Placeholder - start physical link setup */ + } else { + chan->move_role = L2CAP_MOVE_ROLE_INITIATOR; + chan->move_state = L2CAP_MOVE_WAIT_RSP_SUCCESS; + chan->move_id = 0; + l2cap_move_setup(chan); + l2cap_send_move_chan_req(chan, 0); + } +} + +static void l2cap_do_create(struct l2cap_chan *chan, int result, + u8 local_amp_id, u8 remote_amp_id) +{ + BT_DBG("chan %p state %s %u -> %u", chan, state_to_string(chan->state), + local_amp_id, remote_amp_id); + + chan->fcs = L2CAP_FCS_NONE; + + /* Outgoing channel on AMP */ + if (chan->state == BT_CONNECT) { + if (result == L2CAP_CR_SUCCESS) { + chan->local_amp_id = local_amp_id; + l2cap_send_create_chan_req(chan, remote_amp_id); + } else { + /* Revert to BR/EDR connect */ + l2cap_send_conn_req(chan); + } + + return; + } + + /* Incoming channel on AMP */ + if (__l2cap_no_conn_pending(chan)) { + struct l2cap_conn_rsp rsp; + char buf[128]; + rsp.scid = cpu_to_le16(chan->dcid); + rsp.dcid = cpu_to_le16(chan->scid); + + if (result == L2CAP_CR_SUCCESS) { + /* Send successful response */ + rsp.result = cpu_to_le16(L2CAP_CR_SUCCESS); + rsp.status = cpu_to_le16(L2CAP_CS_NO_INFO); + } else { + /* Send negative response */ + rsp.result = cpu_to_le16(L2CAP_CR_NO_MEM); + rsp.status = cpu_to_le16(L2CAP_CS_NO_INFO); + } + + l2cap_send_cmd(chan->conn, chan->ident, L2CAP_CREATE_CHAN_RSP, + sizeof(rsp), &rsp); + + if (result == L2CAP_CR_SUCCESS) { + l2cap_state_change(chan, BT_CONFIG); + set_bit(CONF_REQ_SENT, &chan->conf_state); + l2cap_send_cmd(chan->conn, l2cap_get_ident(chan->conn), + L2CAP_CONF_REQ, + l2cap_build_conf_req(chan, buf), buf); + chan->num_conf_req++; + } + } +} + +static void l2cap_do_move_initiate(struct l2cap_chan *chan, u8 local_amp_id, + u8 remote_amp_id) +{ + l2cap_move_setup(chan); + chan->move_id = local_amp_id; + chan->move_state = L2CAP_MOVE_WAIT_RSP; + + l2cap_send_move_chan_req(chan, remote_amp_id); +} + +static void l2cap_do_move_respond(struct l2cap_chan *chan, int result) +{ + struct hci_chan *hchan = NULL; + + /* Placeholder - get hci_chan for logical link */ + + if (hchan) { + if (hchan->state == BT_CONNECTED) { + /* Logical link is ready to go */ + chan->hs_hcon = hchan->conn; + chan->hs_hcon->l2cap_data = chan->conn; + chan->move_state = L2CAP_MOVE_WAIT_CONFIRM; + l2cap_send_move_chan_rsp(chan, L2CAP_MR_SUCCESS); + + l2cap_logical_cfm(chan, hchan, L2CAP_MR_SUCCESS); + } else { + /* Wait for logical link to be ready */ + chan->move_state = L2CAP_MOVE_WAIT_LOGICAL_CFM; + } + } else { + /* Logical link not available */ + l2cap_send_move_chan_rsp(chan, L2CAP_MR_NOT_ALLOWED); + } +} + +static void l2cap_do_move_cancel(struct l2cap_chan *chan, int result) +{ + if (chan->move_role == L2CAP_MOVE_ROLE_RESPONDER) { + u8 rsp_result; + if (result == -EINVAL) + rsp_result = L2CAP_MR_BAD_ID; + else + rsp_result = L2CAP_MR_NOT_ALLOWED; + + l2cap_send_move_chan_rsp(chan, rsp_result); + } + + chan->move_role = L2CAP_MOVE_ROLE_NONE; + chan->move_state = L2CAP_MOVE_STABLE; + + /* Restart data transmission */ + l2cap_ertm_send(chan); +} + +/* Invoke with locked chan */ +void __l2cap_physical_cfm(struct l2cap_chan *chan, int result) +{ + u8 local_amp_id = chan->local_amp_id; + u8 remote_amp_id = chan->remote_amp_id; + + BT_DBG("chan %p, result %d, local_amp_id %d, remote_amp_id %d", + chan, result, local_amp_id, remote_amp_id); + + if (chan->state == BT_DISCONN || chan->state == BT_CLOSED) { + l2cap_chan_unlock(chan); + return; + } + + if (chan->state != BT_CONNECTED) { + l2cap_do_create(chan, result, local_amp_id, remote_amp_id); + } else if (result != L2CAP_MR_SUCCESS) { + l2cap_do_move_cancel(chan, result); + } else { + switch (chan->move_role) { + case L2CAP_MOVE_ROLE_INITIATOR: + l2cap_do_move_initiate(chan, local_amp_id, + remote_amp_id); + break; + case L2CAP_MOVE_ROLE_RESPONDER: + l2cap_do_move_respond(chan, result); + break; + default: + l2cap_do_move_cancel(chan, result); + break; + } + } +} + +static inline int l2cap_move_channel_req(struct l2cap_conn *conn, + struct l2cap_cmd_hdr *cmd, + u16 cmd_len, void *data) +{ + struct l2cap_move_chan_req *req = data; + struct l2cap_move_chan_rsp rsp; + struct l2cap_chan *chan; + u16 icid = 0; + u16 result = L2CAP_MR_NOT_ALLOWED; + + if (cmd_len != sizeof(*req)) + return -EPROTO; + + icid = le16_to_cpu(req->icid); + + BT_DBG("icid 0x%4.4x, dest_amp_id %d", icid, req->dest_amp_id); + + if (!(conn->local_fixed_chan & L2CAP_FC_A2MP)) + return -EINVAL; + + chan = l2cap_get_chan_by_dcid(conn, icid); + if (!chan) { + rsp.icid = cpu_to_le16(icid); + rsp.result = cpu_to_le16(L2CAP_MR_NOT_ALLOWED); + l2cap_send_cmd(conn, cmd->ident, L2CAP_MOVE_CHAN_RSP, + sizeof(rsp), &rsp); + return 0; + } + + chan->ident = cmd->ident; + + if (chan->scid < L2CAP_CID_DYN_START || + chan->chan_policy == BT_CHANNEL_POLICY_BREDR_ONLY || + (chan->mode != L2CAP_MODE_ERTM && + chan->mode != L2CAP_MODE_STREAMING)) { + result = L2CAP_MR_NOT_ALLOWED; + goto send_move_response; + } + + if (chan->local_amp_id == req->dest_amp_id) { + result = L2CAP_MR_SAME_ID; + goto send_move_response; + } + + if (req->dest_amp_id != AMP_ID_BREDR) { + struct hci_dev *hdev; + hdev = hci_dev_get(req->dest_amp_id); + if (!hdev || hdev->dev_type != HCI_AMP || + !test_bit(HCI_UP, &hdev->flags)) { + if (hdev) + hci_dev_put(hdev); + + result = L2CAP_MR_BAD_ID; + goto send_move_response; + } + hci_dev_put(hdev); + } + + /* Detect a move collision. Only send a collision response + * if this side has "lost", otherwise proceed with the move. + * The winner has the larger bd_addr. + */ + if ((__chan_is_moving(chan) || + chan->move_role != L2CAP_MOVE_ROLE_NONE) && + bacmp(&conn->hcon->src, &conn->hcon->dst) > 0) { + result = L2CAP_MR_COLLISION; + goto send_move_response; + } + + chan->move_role = L2CAP_MOVE_ROLE_RESPONDER; + l2cap_move_setup(chan); + chan->move_id = req->dest_amp_id; + icid = chan->dcid; + + if (req->dest_amp_id == AMP_ID_BREDR) { + /* Moving to BR/EDR */ + if (test_bit(CONN_LOCAL_BUSY, &chan->conn_state)) { + chan->move_state = L2CAP_MOVE_WAIT_LOCAL_BUSY; + result = L2CAP_MR_PEND; + } else { + chan->move_state = L2CAP_MOVE_WAIT_CONFIRM; + result = L2CAP_MR_SUCCESS; + } + } else { + chan->move_state = L2CAP_MOVE_WAIT_PREPARE; + /* Placeholder - uncomment when amp functions are available */ + /*amp_accept_physical(chan, req->dest_amp_id);*/ + result = L2CAP_MR_PEND; + } + +send_move_response: + l2cap_send_move_chan_rsp(chan, result); + + l2cap_chan_unlock(chan); + + return 0; +} + +static void l2cap_move_continue(struct l2cap_conn *conn, u16 icid, u16 result) +{ + struct l2cap_chan *chan; + struct hci_chan *hchan = NULL; + + chan = l2cap_get_chan_by_scid(conn, icid); + if (!chan) { + l2cap_send_move_chan_cfm_icid(conn, icid); + return; + } + + __clear_chan_timer(chan); + if (result == L2CAP_MR_PEND) + __set_chan_timer(chan, L2CAP_MOVE_ERTX_TIMEOUT); + + switch (chan->move_state) { + case L2CAP_MOVE_WAIT_LOGICAL_COMP: + /* Move confirm will be sent when logical link + * is complete. + */ + chan->move_state = L2CAP_MOVE_WAIT_LOGICAL_CFM; + break; + case L2CAP_MOVE_WAIT_RSP_SUCCESS: + if (result == L2CAP_MR_PEND) { + break; + } else if (test_bit(CONN_LOCAL_BUSY, + &chan->conn_state)) { + chan->move_state = L2CAP_MOVE_WAIT_LOCAL_BUSY; + } else { + /* Logical link is up or moving to BR/EDR, + * proceed with move + */ + chan->move_state = L2CAP_MOVE_WAIT_CONFIRM_RSP; + l2cap_send_move_chan_cfm(chan, L2CAP_MC_CONFIRMED); + } + break; + case L2CAP_MOVE_WAIT_RSP: + /* Moving to AMP */ + if (result == L2CAP_MR_SUCCESS) { + /* Remote is ready, send confirm immediately + * after logical link is ready + */ + chan->move_state = L2CAP_MOVE_WAIT_LOGICAL_CFM; + } else { + /* Both logical link and move success + * are required to confirm + */ + chan->move_state = L2CAP_MOVE_WAIT_LOGICAL_COMP; + } + + /* Placeholder - get hci_chan for logical link */ + if (!hchan) { + /* Logical link not available */ + l2cap_send_move_chan_cfm(chan, L2CAP_MC_UNCONFIRMED); + break; + } + + /* If the logical link is not yet connected, do not + * send confirmation. + */ + if (hchan->state != BT_CONNECTED) + break; + + /* Logical link is already ready to go */ + + chan->hs_hcon = hchan->conn; + chan->hs_hcon->l2cap_data = chan->conn; + + if (result == L2CAP_MR_SUCCESS) { + /* Can confirm now */ + l2cap_send_move_chan_cfm(chan, L2CAP_MC_CONFIRMED); + } else { + /* Now only need move success + * to confirm + */ + chan->move_state = L2CAP_MOVE_WAIT_RSP_SUCCESS; + } + + l2cap_logical_cfm(chan, hchan, L2CAP_MR_SUCCESS); + break; + default: + /* Any other amp move state means the move failed. */ + chan->move_id = chan->local_amp_id; + l2cap_move_done(chan); + l2cap_send_move_chan_cfm(chan, L2CAP_MC_UNCONFIRMED); + } + + l2cap_chan_unlock(chan); +} + +static void l2cap_move_fail(struct l2cap_conn *conn, u8 ident, u16 icid, + u16 result) +{ + struct l2cap_chan *chan; + + chan = l2cap_get_chan_by_ident(conn, ident); + if (!chan) { + /* Could not locate channel, icid is best guess */ + l2cap_send_move_chan_cfm_icid(conn, icid); + return; + } + + __clear_chan_timer(chan); + + if (chan->move_role == L2CAP_MOVE_ROLE_INITIATOR) { + if (result == L2CAP_MR_COLLISION) { + chan->move_role = L2CAP_MOVE_ROLE_RESPONDER; + } else { + /* Cleanup - cancel move */ + chan->move_id = chan->local_amp_id; + l2cap_move_done(chan); + } + } + + l2cap_send_move_chan_cfm(chan, L2CAP_MC_UNCONFIRMED); + + l2cap_chan_unlock(chan); +} + +static int l2cap_move_channel_rsp(struct l2cap_conn *conn, + struct l2cap_cmd_hdr *cmd, + u16 cmd_len, void *data) +{ + struct l2cap_move_chan_rsp *rsp = data; + u16 icid, result; + + if (cmd_len != sizeof(*rsp)) + return -EPROTO; + + icid = le16_to_cpu(rsp->icid); + result = le16_to_cpu(rsp->result); + + BT_DBG("icid 0x%4.4x, result 0x%4.4x", icid, result); + + if (result == L2CAP_MR_SUCCESS || result == L2CAP_MR_PEND) + l2cap_move_continue(conn, icid, result); + else + l2cap_move_fail(conn, cmd->ident, icid, result); + + return 0; +} + +static int l2cap_move_channel_confirm(struct l2cap_conn *conn, + struct l2cap_cmd_hdr *cmd, + u16 cmd_len, void *data) +{ + struct l2cap_move_chan_cfm *cfm = data; + struct l2cap_chan *chan; + u16 icid, result; + + if (cmd_len != sizeof(*cfm)) + return -EPROTO; + + icid = le16_to_cpu(cfm->icid); + result = le16_to_cpu(cfm->result); + + BT_DBG("icid 0x%4.4x, result 0x%4.4x", icid, result); + + chan = l2cap_get_chan_by_dcid(conn, icid); + if (!chan) { + /* Spec requires a response even if the icid was not found */ + l2cap_send_move_chan_cfm_rsp(conn, cmd->ident, icid); + return 0; + } + + if (chan->move_state == L2CAP_MOVE_WAIT_CONFIRM) { + if (result == L2CAP_MC_CONFIRMED) { + chan->local_amp_id = chan->move_id; + if (chan->local_amp_id == AMP_ID_BREDR) + __release_logical_link(chan); + } else { + chan->move_id = chan->local_amp_id; + } + + l2cap_move_done(chan); + } + + l2cap_send_move_chan_cfm_rsp(conn, cmd->ident, icid); + + l2cap_chan_unlock(chan); + + return 0; +} + +static inline int l2cap_move_channel_confirm_rsp(struct l2cap_conn *conn, + struct l2cap_cmd_hdr *cmd, + u16 cmd_len, void *data) +{ + struct l2cap_move_chan_cfm_rsp *rsp = data; + struct l2cap_chan *chan; + u16 icid; + + if (cmd_len != sizeof(*rsp)) + return -EPROTO; + + icid = le16_to_cpu(rsp->icid); + + BT_DBG("icid 0x%4.4x", icid); + + chan = l2cap_get_chan_by_scid(conn, icid); + if (!chan) + return 0; + + __clear_chan_timer(chan); + + if (chan->move_state == L2CAP_MOVE_WAIT_CONFIRM_RSP) { + chan->local_amp_id = chan->move_id; + + if (chan->local_amp_id == AMP_ID_BREDR && chan->hs_hchan) + __release_logical_link(chan); + + l2cap_move_done(chan); + } + + l2cap_chan_unlock(chan); + + return 0; +} + +static inline int l2cap_conn_param_update_req(struct l2cap_conn *conn, + struct l2cap_cmd_hdr *cmd, + u16 cmd_len, u8 *data) +{ + struct hci_conn *hcon = conn->hcon; + struct l2cap_conn_param_update_req *req; + struct l2cap_conn_param_update_rsp rsp; + u16 min, max, latency, to_multiplier; + int err; + + if (hcon->role != HCI_ROLE_MASTER) + return -EINVAL; + + if (cmd_len != sizeof(struct l2cap_conn_param_update_req)) + return -EPROTO; + + req = (struct l2cap_conn_param_update_req *) data; + min = __le16_to_cpu(req->min); + max = __le16_to_cpu(req->max); + latency = __le16_to_cpu(req->latency); + to_multiplier = __le16_to_cpu(req->to_multiplier); + + BT_DBG("min 0x%4.4x max 0x%4.4x latency: 0x%4.4x Timeout: 0x%4.4x", + min, max, latency, to_multiplier); + + memset(&rsp, 0, sizeof(rsp)); + + err = hci_check_conn_params(min, max, latency, to_multiplier); + if (err) + rsp.result = cpu_to_le16(L2CAP_CONN_PARAM_REJECTED); + else + rsp.result = cpu_to_le16(L2CAP_CONN_PARAM_ACCEPTED); + + l2cap_send_cmd(conn, cmd->ident, L2CAP_CONN_PARAM_UPDATE_RSP, + sizeof(rsp), &rsp); + + if (!err) { + u8 store_hint; + + store_hint = hci_le_conn_update(hcon, min, max, latency, + to_multiplier); + mgmt_new_conn_param(hcon->hdev, &hcon->dst, hcon->dst_type, + store_hint, min, max, latency, + to_multiplier); + + } + + return 0; +} + +static int l2cap_le_connect_rsp(struct l2cap_conn *conn, + struct l2cap_cmd_hdr *cmd, u16 cmd_len, + u8 *data) +{ + struct l2cap_le_conn_rsp *rsp = (struct l2cap_le_conn_rsp *) data; + struct hci_conn *hcon = conn->hcon; + u16 dcid, mtu, mps, credits, result; + struct l2cap_chan *chan; + int err, sec_level; + + if (cmd_len < sizeof(*rsp)) + return -EPROTO; + + dcid = __le16_to_cpu(rsp->dcid); + mtu = __le16_to_cpu(rsp->mtu); + mps = __le16_to_cpu(rsp->mps); + credits = __le16_to_cpu(rsp->credits); + result = __le16_to_cpu(rsp->result); + + if (result == L2CAP_CR_SUCCESS && (mtu < 23 || mps < 23)) + return -EPROTO; + + BT_DBG("dcid 0x%4.4x mtu %u mps %u credits %u result 0x%2.2x", + dcid, mtu, mps, credits, result); + + mutex_lock(&conn->chan_lock); + + chan = __l2cap_get_chan_by_ident(conn, cmd->ident); + if (!chan) { + err = -EBADSLT; + goto unlock; + } + + err = 0; + + l2cap_chan_lock(chan); + + switch (result) { + case L2CAP_CR_SUCCESS: + chan->ident = 0; + chan->dcid = dcid; + chan->omtu = mtu; + chan->remote_mps = mps; + chan->tx_credits = credits; + l2cap_chan_ready(chan); + break; + + case L2CAP_CR_AUTHENTICATION: + case L2CAP_CR_ENCRYPTION: + /* If we already have MITM protection we can't do + * anything. + */ + if (hcon->sec_level > BT_SECURITY_MEDIUM) { + l2cap_chan_del(chan, ECONNREFUSED); + break; + } + + sec_level = hcon->sec_level + 1; + if (chan->sec_level < sec_level) + chan->sec_level = sec_level; + + /* We'll need to send a new Connect Request */ + clear_bit(FLAG_LE_CONN_REQ_SENT, &chan->flags); + + smp_conn_security(hcon, chan->sec_level); + break; + + default: + l2cap_chan_del(chan, ECONNREFUSED); + break; + } + + l2cap_chan_unlock(chan); + +unlock: + mutex_unlock(&conn->chan_lock); + + return err; +} + +static inline int l2cap_bredr_sig_cmd(struct l2cap_conn *conn, + struct l2cap_cmd_hdr *cmd, u16 cmd_len, + u8 *data) +{ + int err = 0; + + switch (cmd->code) { + case L2CAP_COMMAND_REJ: + l2cap_command_rej(conn, cmd, cmd_len, data); + break; + + case L2CAP_CONN_REQ: + err = l2cap_connect_req(conn, cmd, cmd_len, data); + break; + + case L2CAP_CONN_RSP: + case L2CAP_CREATE_CHAN_RSP: + l2cap_connect_create_rsp(conn, cmd, cmd_len, data); + break; + + case L2CAP_CONF_REQ: + err = l2cap_config_req(conn, cmd, cmd_len, data); + break; + + case L2CAP_CONF_RSP: + l2cap_config_rsp(conn, cmd, cmd_len, data); + break; + + case L2CAP_DISCONN_REQ: + err = l2cap_disconnect_req(conn, cmd, cmd_len, data); + break; + + case L2CAP_DISCONN_RSP: + l2cap_disconnect_rsp(conn, cmd, cmd_len, data); + break; + + case L2CAP_ECHO_REQ: + l2cap_send_cmd(conn, cmd->ident, L2CAP_ECHO_RSP, cmd_len, data); + break; + + case L2CAP_ECHO_RSP: + break; + + case L2CAP_INFO_REQ: + err = l2cap_information_req(conn, cmd, cmd_len, data); + break; + + case L2CAP_INFO_RSP: + l2cap_information_rsp(conn, cmd, cmd_len, data); + break; + + case L2CAP_CREATE_CHAN_REQ: + err = l2cap_create_channel_req(conn, cmd, cmd_len, data); + break; + + case L2CAP_MOVE_CHAN_REQ: + err = l2cap_move_channel_req(conn, cmd, cmd_len, data); + break; + + case L2CAP_MOVE_CHAN_RSP: + l2cap_move_channel_rsp(conn, cmd, cmd_len, data); + break; + + case L2CAP_MOVE_CHAN_CFM: + err = l2cap_move_channel_confirm(conn, cmd, cmd_len, data); + break; + + case L2CAP_MOVE_CHAN_CFM_RSP: + l2cap_move_channel_confirm_rsp(conn, cmd, cmd_len, data); + break; + + default: + BT_ERR("Unknown BR/EDR signaling command 0x%2.2x", cmd->code); + err = -EINVAL; + break; + } + + return err; +} + +static int l2cap_le_connect_req(struct l2cap_conn *conn, + struct l2cap_cmd_hdr *cmd, u16 cmd_len, + u8 *data) +{ + struct l2cap_le_conn_req *req = (struct l2cap_le_conn_req *) data; + struct l2cap_le_conn_rsp rsp; + struct l2cap_chan *chan, *pchan; + u16 dcid, scid, credits, mtu, mps; + __le16 psm; + u8 result; + + if (cmd_len != sizeof(*req)) + return -EPROTO; + + scid = __le16_to_cpu(req->scid); + mtu = __le16_to_cpu(req->mtu); + mps = __le16_to_cpu(req->mps); + psm = req->psm; + dcid = 0; + credits = 0; + + if (mtu < 23 || mps < 23) + return -EPROTO; + + BT_DBG("psm 0x%2.2x scid 0x%4.4x mtu %u mps %u", __le16_to_cpu(psm), + scid, mtu, mps); + + /* Check if we have socket listening on psm */ + pchan = l2cap_global_chan_by_psm(BT_LISTEN, psm, &conn->hcon->src, + &conn->hcon->dst, LE_LINK); + if (!pchan) { + result = L2CAP_CR_BAD_PSM; + chan = NULL; + goto response; + } + + mutex_lock(&conn->chan_lock); + l2cap_chan_lock(pchan); + + if (!smp_sufficient_security(conn->hcon, pchan->sec_level, + SMP_ALLOW_STK)) { + result = L2CAP_CR_AUTHENTICATION; + chan = NULL; + goto response_unlock; + } + + /* Check if we already have channel with that dcid */ + if (__l2cap_get_chan_by_dcid(conn, scid)) { + result = L2CAP_CR_NO_MEM; + chan = NULL; + goto response_unlock; + } + + chan = pchan->ops->new_connection(pchan); + if (!chan) { + result = L2CAP_CR_NO_MEM; + goto response_unlock; + } + + l2cap_le_flowctl_init(chan); + + bacpy(&chan->src, &conn->hcon->src); + bacpy(&chan->dst, &conn->hcon->dst); + chan->src_type = bdaddr_src_type(conn->hcon); + chan->dst_type = bdaddr_dst_type(conn->hcon); + chan->psm = psm; + chan->dcid = scid; + chan->omtu = mtu; + chan->remote_mps = mps; + chan->tx_credits = __le16_to_cpu(req->credits); + + __l2cap_chan_add(conn, chan); + dcid = chan->scid; + credits = chan->rx_credits; + + __set_chan_timer(chan, chan->ops->get_sndtimeo(chan)); + + chan->ident = cmd->ident; + + if (test_bit(FLAG_DEFER_SETUP, &chan->flags)) { + l2cap_state_change(chan, BT_CONNECT2); + /* The following result value is actually not defined + * for LE CoC but we use it to let the function know + * that it should bail out after doing its cleanup + * instead of sending a response. + */ + result = L2CAP_CR_PEND; + chan->ops->defer(chan); + } else { + l2cap_chan_ready(chan); + result = L2CAP_CR_SUCCESS; + } + +response_unlock: + l2cap_chan_unlock(pchan); + mutex_unlock(&conn->chan_lock); + l2cap_chan_put(pchan); + + if (result == L2CAP_CR_PEND) + return 0; + +response: + if (chan) { + rsp.mtu = cpu_to_le16(chan->imtu); + rsp.mps = cpu_to_le16(chan->mps); + } else { + rsp.mtu = 0; + rsp.mps = 0; + } + + rsp.dcid = cpu_to_le16(dcid); + rsp.credits = cpu_to_le16(credits); + rsp.result = cpu_to_le16(result); + + l2cap_send_cmd(conn, cmd->ident, L2CAP_LE_CONN_RSP, sizeof(rsp), &rsp); + + return 0; +} + +static inline int l2cap_le_credits(struct l2cap_conn *conn, + struct l2cap_cmd_hdr *cmd, u16 cmd_len, + u8 *data) +{ + struct l2cap_le_credits *pkt; + struct l2cap_chan *chan; + u16 cid, credits, max_credits; + + if (cmd_len != sizeof(*pkt)) + return -EPROTO; + + pkt = (struct l2cap_le_credits *) data; + cid = __le16_to_cpu(pkt->cid); + credits = __le16_to_cpu(pkt->credits); + + BT_DBG("cid 0x%4.4x credits 0x%4.4x", cid, credits); + + chan = l2cap_get_chan_by_dcid(conn, cid); + if (!chan) + return -EBADSLT; + + max_credits = LE_FLOWCTL_MAX_CREDITS - chan->tx_credits; + if (credits > max_credits) { + BT_ERR("LE credits overflow"); + l2cap_send_disconn_req(chan, ECONNRESET); + l2cap_chan_unlock(chan); + + /* Return 0 so that we don't trigger an unnecessary + * command reject packet. + */ + return 0; + } + + chan->tx_credits += credits; + + while (chan->tx_credits && !skb_queue_empty(&chan->tx_q)) { + l2cap_do_send(chan, skb_dequeue(&chan->tx_q)); + chan->tx_credits--; + } + + if (chan->tx_credits) + chan->ops->resume(chan); + + l2cap_chan_unlock(chan); + + return 0; +} + +static inline int l2cap_le_command_rej(struct l2cap_conn *conn, + struct l2cap_cmd_hdr *cmd, u16 cmd_len, + u8 *data) +{ + struct l2cap_cmd_rej_unk *rej = (struct l2cap_cmd_rej_unk *) data; + struct l2cap_chan *chan; + + if (cmd_len < sizeof(*rej)) + return -EPROTO; + + mutex_lock(&conn->chan_lock); + + chan = __l2cap_get_chan_by_ident(conn, cmd->ident); + if (!chan) + goto done; + + l2cap_chan_lock(chan); + l2cap_chan_del(chan, ECONNREFUSED); + l2cap_chan_unlock(chan); + +done: + mutex_unlock(&conn->chan_lock); + return 0; +} + +static inline int l2cap_le_sig_cmd(struct l2cap_conn *conn, + struct l2cap_cmd_hdr *cmd, u16 cmd_len, + u8 *data) +{ + int err = 0; + + switch (cmd->code) { + case L2CAP_COMMAND_REJ: + l2cap_le_command_rej(conn, cmd, cmd_len, data); + break; + + case L2CAP_CONN_PARAM_UPDATE_REQ: + err = l2cap_conn_param_update_req(conn, cmd, cmd_len, data); + break; + + case L2CAP_CONN_PARAM_UPDATE_RSP: + break; + + case L2CAP_LE_CONN_RSP: + l2cap_le_connect_rsp(conn, cmd, cmd_len, data); + break; + + case L2CAP_LE_CONN_REQ: + err = l2cap_le_connect_req(conn, cmd, cmd_len, data); + break; + + case L2CAP_LE_CREDITS: + err = l2cap_le_credits(conn, cmd, cmd_len, data); + break; + + case L2CAP_DISCONN_REQ: + err = l2cap_disconnect_req(conn, cmd, cmd_len, data); + break; + + case L2CAP_DISCONN_RSP: + l2cap_disconnect_rsp(conn, cmd, cmd_len, data); + break; + + default: + BT_ERR("Unknown LE signaling command 0x%2.2x", cmd->code); + err = -EINVAL; + break; + } + + return err; +} + +static inline void l2cap_le_sig_channel(struct l2cap_conn *conn, + struct sk_buff *skb) +{ + struct hci_conn *hcon = conn->hcon; + struct l2cap_cmd_hdr *cmd; + u16 len; + int err; + + if (hcon->type != LE_LINK) + goto drop; + + if (skb->len < L2CAP_CMD_HDR_SIZE) + goto drop; + + cmd = (void *) skb->data; + skb_pull(skb, L2CAP_CMD_HDR_SIZE); + + len = le16_to_cpu(cmd->len); + + BT_DBG("code 0x%2.2x len %d id 0x%2.2x", cmd->code, len, cmd->ident); + + if (len != skb->len || !cmd->ident) { + BT_DBG("corrupted command"); + goto drop; + } + + err = l2cap_le_sig_cmd(conn, cmd, len, skb->data); + if (err) { + struct l2cap_cmd_rej_unk rej; + + BT_ERR("Wrong link type (%d)", err); + + rej.reason = cpu_to_le16(L2CAP_REJ_NOT_UNDERSTOOD); + l2cap_send_cmd(conn, cmd->ident, L2CAP_COMMAND_REJ, + sizeof(rej), &rej); + } + +drop: + kfree_skb(skb); +} + +static inline void l2cap_sig_channel(struct l2cap_conn *conn, + struct sk_buff *skb) +{ + struct hci_conn *hcon = conn->hcon; + u8 *data = skb->data; + int len = skb->len; + struct l2cap_cmd_hdr cmd; + int err; + + l2cap_raw_recv(conn, skb); + + if (hcon->type != ACL_LINK) + goto drop; + + while (len >= L2CAP_CMD_HDR_SIZE) { + u16 cmd_len; + memcpy(&cmd, data, L2CAP_CMD_HDR_SIZE); + data += L2CAP_CMD_HDR_SIZE; + len -= L2CAP_CMD_HDR_SIZE; + + cmd_len = le16_to_cpu(cmd.len); + + BT_DBG("code 0x%2.2x len %d id 0x%2.2x", cmd.code, cmd_len, + cmd.ident); + + if (cmd_len > len || !cmd.ident) { + BT_DBG("corrupted command"); + break; + } + + err = l2cap_bredr_sig_cmd(conn, &cmd, cmd_len, data); + if (err) { + struct l2cap_cmd_rej_unk rej; + + BT_ERR("Wrong link type (%d)", err); + + rej.reason = cpu_to_le16(L2CAP_REJ_NOT_UNDERSTOOD); + l2cap_send_cmd(conn, cmd.ident, L2CAP_COMMAND_REJ, + sizeof(rej), &rej); + } + + data += cmd_len; + len -= cmd_len; + } + +drop: + kfree_skb(skb); +} + +static int l2cap_check_fcs(struct l2cap_chan *chan, struct sk_buff *skb) +{ + u16 our_fcs, rcv_fcs; + int hdr_size; + + if (test_bit(FLAG_EXT_CTRL, &chan->flags)) + hdr_size = L2CAP_EXT_HDR_SIZE; + else + hdr_size = L2CAP_ENH_HDR_SIZE; + + if (chan->fcs == L2CAP_FCS_CRC16) { + skb_trim(skb, skb->len - L2CAP_FCS_SIZE); + rcv_fcs = get_unaligned_le16(skb->data + skb->len); + our_fcs = crc16(0, skb->data - hdr_size, skb->len + hdr_size); + + if (our_fcs != rcv_fcs) + return -EBADMSG; + } + return 0; +} + +static void l2cap_send_i_or_rr_or_rnr(struct l2cap_chan *chan) +{ + struct l2cap_ctrl control; + + BT_DBG("chan %p", chan); + + memset(&control, 0, sizeof(control)); + control.sframe = 1; + control.final = 1; + control.reqseq = chan->buffer_seq; + set_bit(CONN_SEND_FBIT, &chan->conn_state); + + if (test_bit(CONN_LOCAL_BUSY, &chan->conn_state)) { + control.super = L2CAP_SUPER_RNR; + l2cap_send_sframe(chan, &control); + } + + if (test_and_clear_bit(CONN_REMOTE_BUSY, &chan->conn_state) && + chan->unacked_frames > 0) + __set_retrans_timer(chan); + + /* Send pending iframes */ + l2cap_ertm_send(chan); + + if (!test_bit(CONN_LOCAL_BUSY, &chan->conn_state) && + test_bit(CONN_SEND_FBIT, &chan->conn_state)) { + /* F-bit wasn't sent in an s-frame or i-frame yet, so + * send it now. + */ + control.super = L2CAP_SUPER_RR; + l2cap_send_sframe(chan, &control); + } +} + +static void append_skb_frag(struct sk_buff *skb, struct sk_buff *new_frag, + struct sk_buff **last_frag) +{ + /* skb->len reflects data in skb as well as all fragments + * skb->data_len reflects only data in fragments + */ + if (!skb_has_frag_list(skb)) + skb_shinfo(skb)->frag_list = new_frag; + + new_frag->next = NULL; + + (*last_frag)->next = new_frag; + *last_frag = new_frag; + + skb->len += new_frag->len; + skb->data_len += new_frag->len; + skb->truesize += new_frag->truesize; +} + +static int l2cap_reassemble_sdu(struct l2cap_chan *chan, struct sk_buff *skb, + struct l2cap_ctrl *control) +{ + int err = -EINVAL; + + switch (control->sar) { + case L2CAP_SAR_UNSEGMENTED: + if (chan->sdu) + break; + + err = chan->ops->recv(chan, skb); + break; + + case L2CAP_SAR_START: + if (chan->sdu) + break; + + chan->sdu_len = get_unaligned_le16(skb->data); + skb_pull(skb, L2CAP_SDULEN_SIZE); + + if (chan->sdu_len > chan->imtu) { + err = -EMSGSIZE; + break; + } + + if (skb->len >= chan->sdu_len) + break; + + chan->sdu = skb; + chan->sdu_last_frag = skb; + + skb = NULL; + err = 0; + break; + + case L2CAP_SAR_CONTINUE: + if (!chan->sdu) + break; + + append_skb_frag(chan->sdu, skb, + &chan->sdu_last_frag); + skb = NULL; + + if (chan->sdu->len >= chan->sdu_len) + break; + + err = 0; + break; + + case L2CAP_SAR_END: + if (!chan->sdu) + break; + + append_skb_frag(chan->sdu, skb, + &chan->sdu_last_frag); + skb = NULL; + + if (chan->sdu->len != chan->sdu_len) + break; + + err = chan->ops->recv(chan, chan->sdu); + + if (!err) { + /* Reassembly complete */ + chan->sdu = NULL; + chan->sdu_last_frag = NULL; + chan->sdu_len = 0; + } + break; + } + + if (err) { + kfree_skb(skb); + kfree_skb(chan->sdu); + chan->sdu = NULL; + chan->sdu_last_frag = NULL; + chan->sdu_len = 0; + } + + return err; +} + +static int l2cap_resegment(struct l2cap_chan *chan) +{ + /* Placeholder */ + return 0; +} + +void l2cap_chan_busy(struct l2cap_chan *chan, int busy) +{ + u8 event; + + if (chan->mode != L2CAP_MODE_ERTM) + return; + + event = busy ? L2CAP_EV_LOCAL_BUSY_DETECTED : L2CAP_EV_LOCAL_BUSY_CLEAR; + l2cap_tx(chan, NULL, NULL, event); +} + +static int l2cap_rx_queued_iframes(struct l2cap_chan *chan) +{ + int err = 0; + /* Pass sequential frames to l2cap_reassemble_sdu() + * until a gap is encountered. + */ + + BT_DBG("chan %p", chan); + + while (!test_bit(CONN_LOCAL_BUSY, &chan->conn_state)) { + struct sk_buff *skb; + BT_DBG("Searching for skb with txseq %d (queue len %d)", + chan->buffer_seq, skb_queue_len(&chan->srej_q)); + + skb = l2cap_ertm_seq_in_queue(&chan->srej_q, chan->buffer_seq); + + if (!skb) + break; + + skb_unlink(skb, &chan->srej_q); + chan->buffer_seq = __next_seq(chan, chan->buffer_seq); + err = l2cap_reassemble_sdu(chan, skb, &bt_cb(skb)->l2cap); + if (err) + break; + } + + if (skb_queue_empty(&chan->srej_q)) { + chan->rx_state = L2CAP_RX_STATE_RECV; + l2cap_send_ack(chan); + } + + return err; +} + +static void l2cap_handle_srej(struct l2cap_chan *chan, + struct l2cap_ctrl *control) +{ + struct sk_buff *skb; + + BT_DBG("chan %p, control %p", chan, control); + + if (control->reqseq == chan->next_tx_seq) { + BT_DBG("Invalid reqseq %d, disconnecting", control->reqseq); + l2cap_send_disconn_req(chan, ECONNRESET); + return; + } + + skb = l2cap_ertm_seq_in_queue(&chan->tx_q, control->reqseq); + + if (skb == NULL) { + BT_DBG("Seq %d not available for retransmission", + control->reqseq); + return; + } + + if (chan->max_tx != 0 && bt_cb(skb)->l2cap.retries >= chan->max_tx) { + BT_DBG("Retry limit exceeded (%d)", chan->max_tx); + l2cap_send_disconn_req(chan, ECONNRESET); + return; + } + + clear_bit(CONN_REMOTE_BUSY, &chan->conn_state); + + if (control->poll) { + l2cap_pass_to_tx(chan, control); + + set_bit(CONN_SEND_FBIT, &chan->conn_state); + l2cap_retransmit(chan, control); + l2cap_ertm_send(chan); + + if (chan->tx_state == L2CAP_TX_STATE_WAIT_F) { + set_bit(CONN_SREJ_ACT, &chan->conn_state); + chan->srej_save_reqseq = control->reqseq; + } + } else { + l2cap_pass_to_tx_fbit(chan, control); + + if (control->final) { + if (chan->srej_save_reqseq != control->reqseq || + !test_and_clear_bit(CONN_SREJ_ACT, + &chan->conn_state)) + l2cap_retransmit(chan, control); + } else { + l2cap_retransmit(chan, control); + if (chan->tx_state == L2CAP_TX_STATE_WAIT_F) { + set_bit(CONN_SREJ_ACT, &chan->conn_state); + chan->srej_save_reqseq = control->reqseq; + } + } + } +} + +static void l2cap_handle_rej(struct l2cap_chan *chan, + struct l2cap_ctrl *control) +{ + struct sk_buff *skb; + + BT_DBG("chan %p, control %p", chan, control); + + if (control->reqseq == chan->next_tx_seq) { + BT_DBG("Invalid reqseq %d, disconnecting", control->reqseq); + l2cap_send_disconn_req(chan, ECONNRESET); + return; + } + + skb = l2cap_ertm_seq_in_queue(&chan->tx_q, control->reqseq); + + if (chan->max_tx && skb && + bt_cb(skb)->l2cap.retries >= chan->max_tx) { + BT_DBG("Retry limit exceeded (%d)", chan->max_tx); + l2cap_send_disconn_req(chan, ECONNRESET); + return; + } + + clear_bit(CONN_REMOTE_BUSY, &chan->conn_state); + + l2cap_pass_to_tx(chan, control); + + if (control->final) { + if (!test_and_clear_bit(CONN_REJ_ACT, &chan->conn_state)) + l2cap_retransmit_all(chan, control); + } else { + l2cap_retransmit_all(chan, control); + l2cap_ertm_send(chan); + if (chan->tx_state == L2CAP_TX_STATE_WAIT_F) + set_bit(CONN_REJ_ACT, &chan->conn_state); + } +} + +static u8 l2cap_classify_txseq(struct l2cap_chan *chan, u16 txseq) +{ + BT_DBG("chan %p, txseq %d", chan, txseq); + + BT_DBG("last_acked_seq %d, expected_tx_seq %d", chan->last_acked_seq, + chan->expected_tx_seq); + + if (chan->rx_state == L2CAP_RX_STATE_SREJ_SENT) { + if (__seq_offset(chan, txseq, chan->last_acked_seq) >= + chan->tx_win) { + /* See notes below regarding "double poll" and + * invalid packets. + */ + if (chan->tx_win <= ((chan->tx_win_max + 1) >> 1)) { + BT_DBG("Invalid/Ignore - after SREJ"); + return L2CAP_TXSEQ_INVALID_IGNORE; + } else { + BT_DBG("Invalid - in window after SREJ sent"); + return L2CAP_TXSEQ_INVALID; + } + } + + if (chan->srej_list.head == txseq) { + BT_DBG("Expected SREJ"); + return L2CAP_TXSEQ_EXPECTED_SREJ; + } + + if (l2cap_ertm_seq_in_queue(&chan->srej_q, txseq)) { + BT_DBG("Duplicate SREJ - txseq already stored"); + return L2CAP_TXSEQ_DUPLICATE_SREJ; + } + + if (l2cap_seq_list_contains(&chan->srej_list, txseq)) { + BT_DBG("Unexpected SREJ - not requested"); + return L2CAP_TXSEQ_UNEXPECTED_SREJ; + } + } + + if (chan->expected_tx_seq == txseq) { + if (__seq_offset(chan, txseq, chan->last_acked_seq) >= + chan->tx_win) { + BT_DBG("Invalid - txseq outside tx window"); + return L2CAP_TXSEQ_INVALID; + } else { + BT_DBG("Expected"); + return L2CAP_TXSEQ_EXPECTED; + } + } + + if (__seq_offset(chan, txseq, chan->last_acked_seq) < + __seq_offset(chan, chan->expected_tx_seq, chan->last_acked_seq)) { + BT_DBG("Duplicate - expected_tx_seq later than txseq"); + return L2CAP_TXSEQ_DUPLICATE; + } + + if (__seq_offset(chan, txseq, chan->last_acked_seq) >= chan->tx_win) { + /* A source of invalid packets is a "double poll" condition, + * where delays cause us to send multiple poll packets. If + * the remote stack receives and processes both polls, + * sequence numbers can wrap around in such a way that a + * resent frame has a sequence number that looks like new data + * with a sequence gap. This would trigger an erroneous SREJ + * request. + * + * Fortunately, this is impossible with a tx window that's + * less than half of the maximum sequence number, which allows + * invalid frames to be safely ignored. + * + * With tx window sizes greater than half of the tx window + * maximum, the frame is invalid and cannot be ignored. This + * causes a disconnect. + */ + + if (chan->tx_win <= ((chan->tx_win_max + 1) >> 1)) { + BT_DBG("Invalid/Ignore - txseq outside tx window"); + return L2CAP_TXSEQ_INVALID_IGNORE; + } else { + BT_DBG("Invalid - txseq outside tx window"); + return L2CAP_TXSEQ_INVALID; + } + } else { + BT_DBG("Unexpected - txseq indicates missing frames"); + return L2CAP_TXSEQ_UNEXPECTED; + } +} + +static int l2cap_rx_state_recv(struct l2cap_chan *chan, + struct l2cap_ctrl *control, + struct sk_buff *skb, u8 event) +{ + int err = 0; + bool skb_in_use = false; + + BT_DBG("chan %p, control %p, skb %p, event %d", chan, control, skb, + event); + + switch (event) { + case L2CAP_EV_RECV_IFRAME: + switch (l2cap_classify_txseq(chan, control->txseq)) { + case L2CAP_TXSEQ_EXPECTED: + l2cap_pass_to_tx(chan, control); + + if (test_bit(CONN_LOCAL_BUSY, &chan->conn_state)) { + BT_DBG("Busy, discarding expected seq %d", + control->txseq); + break; + } + + chan->expected_tx_seq = __next_seq(chan, + control->txseq); + + chan->buffer_seq = chan->expected_tx_seq; + skb_in_use = true; + + err = l2cap_reassemble_sdu(chan, skb, control); + if (err) + break; + + if (control->final) { + if (!test_and_clear_bit(CONN_REJ_ACT, + &chan->conn_state)) { + control->final = 0; + l2cap_retransmit_all(chan, control); + l2cap_ertm_send(chan); + } + } + + if (!test_bit(CONN_LOCAL_BUSY, &chan->conn_state)) + l2cap_send_ack(chan); + break; + case L2CAP_TXSEQ_UNEXPECTED: + l2cap_pass_to_tx(chan, control); + + /* Can't issue SREJ frames in the local busy state. + * Drop this frame, it will be seen as missing + * when local busy is exited. + */ + if (test_bit(CONN_LOCAL_BUSY, &chan->conn_state)) { + BT_DBG("Busy, discarding unexpected seq %d", + control->txseq); + break; + } + + /* There was a gap in the sequence, so an SREJ + * must be sent for each missing frame. The + * current frame is stored for later use. + */ + skb_queue_tail(&chan->srej_q, skb); + skb_in_use = true; + BT_DBG("Queued %p (queue len %d)", skb, + skb_queue_len(&chan->srej_q)); + + clear_bit(CONN_SREJ_ACT, &chan->conn_state); + l2cap_seq_list_clear(&chan->srej_list); + l2cap_send_srej(chan, control->txseq); + + chan->rx_state = L2CAP_RX_STATE_SREJ_SENT; + break; + case L2CAP_TXSEQ_DUPLICATE: + l2cap_pass_to_tx(chan, control); + break; + case L2CAP_TXSEQ_INVALID_IGNORE: + break; + case L2CAP_TXSEQ_INVALID: + default: + l2cap_send_disconn_req(chan, ECONNRESET); + break; + } + break; + case L2CAP_EV_RECV_RR: + l2cap_pass_to_tx(chan, control); + if (control->final) { + clear_bit(CONN_REMOTE_BUSY, &chan->conn_state); + + if (!test_and_clear_bit(CONN_REJ_ACT, &chan->conn_state) && + !__chan_is_moving(chan)) { + control->final = 0; + l2cap_retransmit_all(chan, control); + } + + l2cap_ertm_send(chan); + } else if (control->poll) { + l2cap_send_i_or_rr_or_rnr(chan); + } else { + if (test_and_clear_bit(CONN_REMOTE_BUSY, + &chan->conn_state) && + chan->unacked_frames) + __set_retrans_timer(chan); + + l2cap_ertm_send(chan); + } + break; + case L2CAP_EV_RECV_RNR: + set_bit(CONN_REMOTE_BUSY, &chan->conn_state); + l2cap_pass_to_tx(chan, control); + if (control && control->poll) { + set_bit(CONN_SEND_FBIT, &chan->conn_state); + l2cap_send_rr_or_rnr(chan, 0); + } + __clear_retrans_timer(chan); + l2cap_seq_list_clear(&chan->retrans_list); + break; + case L2CAP_EV_RECV_REJ: + l2cap_handle_rej(chan, control); + break; + case L2CAP_EV_RECV_SREJ: + l2cap_handle_srej(chan, control); + break; + default: + break; + } + + if (skb && !skb_in_use) { + BT_DBG("Freeing %p", skb); + kfree_skb(skb); + } + + return err; +} + +static int l2cap_rx_state_srej_sent(struct l2cap_chan *chan, + struct l2cap_ctrl *control, + struct sk_buff *skb, u8 event) +{ + int err = 0; + u16 txseq = control->txseq; + bool skb_in_use = false; + + BT_DBG("chan %p, control %p, skb %p, event %d", chan, control, skb, + event); + + switch (event) { + case L2CAP_EV_RECV_IFRAME: + switch (l2cap_classify_txseq(chan, txseq)) { + case L2CAP_TXSEQ_EXPECTED: + /* Keep frame for reassembly later */ + l2cap_pass_to_tx(chan, control); + skb_queue_tail(&chan->srej_q, skb); + skb_in_use = true; + BT_DBG("Queued %p (queue len %d)", skb, + skb_queue_len(&chan->srej_q)); + + chan->expected_tx_seq = __next_seq(chan, txseq); + break; + case L2CAP_TXSEQ_EXPECTED_SREJ: + l2cap_seq_list_pop(&chan->srej_list); + + l2cap_pass_to_tx(chan, control); + skb_queue_tail(&chan->srej_q, skb); + skb_in_use = true; + BT_DBG("Queued %p (queue len %d)", skb, + skb_queue_len(&chan->srej_q)); + + err = l2cap_rx_queued_iframes(chan); + if (err) + break; + + break; + case L2CAP_TXSEQ_UNEXPECTED: + /* Got a frame that can't be reassembled yet. + * Save it for later, and send SREJs to cover + * the missing frames. + */ + skb_queue_tail(&chan->srej_q, skb); + skb_in_use = true; + BT_DBG("Queued %p (queue len %d)", skb, + skb_queue_len(&chan->srej_q)); + + l2cap_pass_to_tx(chan, control); + l2cap_send_srej(chan, control->txseq); + break; + case L2CAP_TXSEQ_UNEXPECTED_SREJ: + /* This frame was requested with an SREJ, but + * some expected retransmitted frames are + * missing. Request retransmission of missing + * SREJ'd frames. + */ + skb_queue_tail(&chan->srej_q, skb); + skb_in_use = true; + BT_DBG("Queued %p (queue len %d)", skb, + skb_queue_len(&chan->srej_q)); + + l2cap_pass_to_tx(chan, control); + l2cap_send_srej_list(chan, control->txseq); + break; + case L2CAP_TXSEQ_DUPLICATE_SREJ: + /* We've already queued this frame. Drop this copy. */ + l2cap_pass_to_tx(chan, control); + break; + case L2CAP_TXSEQ_DUPLICATE: + /* Expecting a later sequence number, so this frame + * was already received. Ignore it completely. + */ + break; + case L2CAP_TXSEQ_INVALID_IGNORE: + break; + case L2CAP_TXSEQ_INVALID: + default: + l2cap_send_disconn_req(chan, ECONNRESET); + break; + } + break; + case L2CAP_EV_RECV_RR: + l2cap_pass_to_tx(chan, control); + if (control->final) { + clear_bit(CONN_REMOTE_BUSY, &chan->conn_state); + + if (!test_and_clear_bit(CONN_REJ_ACT, + &chan->conn_state)) { + control->final = 0; + l2cap_retransmit_all(chan, control); + } + + l2cap_ertm_send(chan); + } else if (control->poll) { + if (test_and_clear_bit(CONN_REMOTE_BUSY, + &chan->conn_state) && + chan->unacked_frames) { + __set_retrans_timer(chan); + } + + set_bit(CONN_SEND_FBIT, &chan->conn_state); + l2cap_send_srej_tail(chan); + } else { + if (test_and_clear_bit(CONN_REMOTE_BUSY, + &chan->conn_state) && + chan->unacked_frames) + __set_retrans_timer(chan); + + l2cap_send_ack(chan); + } + break; + case L2CAP_EV_RECV_RNR: + set_bit(CONN_REMOTE_BUSY, &chan->conn_state); + l2cap_pass_to_tx(chan, control); + if (control->poll) { + l2cap_send_srej_tail(chan); + } else { + struct l2cap_ctrl rr_control; + memset(&rr_control, 0, sizeof(rr_control)); + rr_control.sframe = 1; + rr_control.super = L2CAP_SUPER_RR; + rr_control.reqseq = chan->buffer_seq; + l2cap_send_sframe(chan, &rr_control); + } + + break; + case L2CAP_EV_RECV_REJ: + l2cap_handle_rej(chan, control); + break; + case L2CAP_EV_RECV_SREJ: + l2cap_handle_srej(chan, control); + break; + } + + if (skb && !skb_in_use) { + BT_DBG("Freeing %p", skb); + kfree_skb(skb); + } + + return err; +} + +static int l2cap_finish_move(struct l2cap_chan *chan) +{ + BT_DBG("chan %p", chan); + + chan->rx_state = L2CAP_RX_STATE_RECV; + + if (chan->hs_hcon) + chan->conn->mtu = chan->hs_hcon->hdev->block_mtu; + else + chan->conn->mtu = chan->conn->hcon->hdev->acl_mtu; + + return l2cap_resegment(chan); +} + +static int l2cap_rx_state_wait_p(struct l2cap_chan *chan, + struct l2cap_ctrl *control, + struct sk_buff *skb, u8 event) +{ + int err; + + BT_DBG("chan %p, control %p, skb %p, event %d", chan, control, skb, + event); + + if (!control->poll) + return -EPROTO; + + l2cap_process_reqseq(chan, control->reqseq); + + if (!skb_queue_empty(&chan->tx_q)) + chan->tx_send_head = skb_peek(&chan->tx_q); + else + chan->tx_send_head = NULL; + + /* Rewind next_tx_seq to the point expected + * by the receiver. + */ + chan->next_tx_seq = control->reqseq; + chan->unacked_frames = 0; + + err = l2cap_finish_move(chan); + if (err) + return err; + + set_bit(CONN_SEND_FBIT, &chan->conn_state); + l2cap_send_i_or_rr_or_rnr(chan); + + if (event == L2CAP_EV_RECV_IFRAME) + return -EPROTO; + + return l2cap_rx_state_recv(chan, control, NULL, event); +} + +static int l2cap_rx_state_wait_f(struct l2cap_chan *chan, + struct l2cap_ctrl *control, + struct sk_buff *skb, u8 event) +{ + int err; + + if (!control->final) + return -EPROTO; + + clear_bit(CONN_REMOTE_BUSY, &chan->conn_state); + + chan->rx_state = L2CAP_RX_STATE_RECV; + l2cap_process_reqseq(chan, control->reqseq); + + if (!skb_queue_empty(&chan->tx_q)) + chan->tx_send_head = skb_peek(&chan->tx_q); + else + chan->tx_send_head = NULL; + + /* Rewind next_tx_seq to the point expected + * by the receiver. + */ + chan->next_tx_seq = control->reqseq; + chan->unacked_frames = 0; + + if (chan->hs_hcon) + chan->conn->mtu = chan->hs_hcon->hdev->block_mtu; + else + chan->conn->mtu = chan->conn->hcon->hdev->acl_mtu; + + err = l2cap_resegment(chan); + + if (!err) + err = l2cap_rx_state_recv(chan, control, skb, event); + + return err; +} + +static bool __valid_reqseq(struct l2cap_chan *chan, u16 reqseq) +{ + /* Make sure reqseq is for a packet that has been sent but not acked */ + u16 unacked; + + unacked = __seq_offset(chan, chan->next_tx_seq, chan->expected_ack_seq); + return __seq_offset(chan, chan->next_tx_seq, reqseq) <= unacked; +} + +static int l2cap_rx(struct l2cap_chan *chan, struct l2cap_ctrl *control, + struct sk_buff *skb, u8 event) +{ + int err = 0; + + BT_DBG("chan %p, control %p, skb %p, event %d, state %d", chan, + control, skb, event, chan->rx_state); + + if (__valid_reqseq(chan, control->reqseq)) { + switch (chan->rx_state) { + case L2CAP_RX_STATE_RECV: + err = l2cap_rx_state_recv(chan, control, skb, event); + break; + case L2CAP_RX_STATE_SREJ_SENT: + err = l2cap_rx_state_srej_sent(chan, control, skb, + event); + break; + case L2CAP_RX_STATE_WAIT_P: + err = l2cap_rx_state_wait_p(chan, control, skb, event); + break; + case L2CAP_RX_STATE_WAIT_F: + err = l2cap_rx_state_wait_f(chan, control, skb, event); + break; + default: + /* shut it down */ + break; + } + } else { + BT_DBG("Invalid reqseq %d (next_tx_seq %d, expected_ack_seq %d", + control->reqseq, chan->next_tx_seq, + chan->expected_ack_seq); + l2cap_send_disconn_req(chan, ECONNRESET); + } + + return err; +} + +static int l2cap_stream_rx(struct l2cap_chan *chan, struct l2cap_ctrl *control, + struct sk_buff *skb) +{ + int err = 0; + + BT_DBG("chan %p, control %p, skb %p, state %d", chan, control, skb, + chan->rx_state); + + if (l2cap_classify_txseq(chan, control->txseq) == + L2CAP_TXSEQ_EXPECTED) { + l2cap_pass_to_tx(chan, control); + + BT_DBG("buffer_seq %d->%d", chan->buffer_seq, + __next_seq(chan, chan->buffer_seq)); + + chan->buffer_seq = __next_seq(chan, chan->buffer_seq); + + l2cap_reassemble_sdu(chan, skb, control); + } else { + if (chan->sdu) { + kfree_skb(chan->sdu); + chan->sdu = NULL; + } + chan->sdu_last_frag = NULL; + chan->sdu_len = 0; + + if (skb) { + BT_DBG("Freeing %p", skb); + kfree_skb(skb); + } + } + + chan->last_acked_seq = control->txseq; + chan->expected_tx_seq = __next_seq(chan, control->txseq); + + return err; +} + +static int l2cap_data_rcv(struct l2cap_chan *chan, struct sk_buff *skb) +{ + struct l2cap_ctrl *control = &bt_cb(skb)->l2cap; + u16 len; + u8 event; + + __unpack_control(chan, skb); + + len = skb->len; + + /* + * We can just drop the corrupted I-frame here. + * Receiver will miss it and start proper recovery + * procedures and ask for retransmission. + */ + if (l2cap_check_fcs(chan, skb)) + goto drop; + + if (!control->sframe && control->sar == L2CAP_SAR_START) + len -= L2CAP_SDULEN_SIZE; + + if (chan->fcs == L2CAP_FCS_CRC16) + len -= L2CAP_FCS_SIZE; + + if (len > chan->mps) { + l2cap_send_disconn_req(chan, ECONNRESET); + goto drop; + } + + if (!control->sframe) { + int err; + + BT_DBG("iframe sar %d, reqseq %d, final %d, txseq %d", + control->sar, control->reqseq, control->final, + control->txseq); + + /* Validate F-bit - F=0 always valid, F=1 only + * valid in TX WAIT_F + */ + if (control->final && chan->tx_state != L2CAP_TX_STATE_WAIT_F) + goto drop; + + if (chan->mode != L2CAP_MODE_STREAMING) { + event = L2CAP_EV_RECV_IFRAME; + err = l2cap_rx(chan, control, skb, event); + } else { + err = l2cap_stream_rx(chan, control, skb); + } + + if (err) + l2cap_send_disconn_req(chan, ECONNRESET); + } else { + const u8 rx_func_to_event[4] = { + L2CAP_EV_RECV_RR, L2CAP_EV_RECV_REJ, + L2CAP_EV_RECV_RNR, L2CAP_EV_RECV_SREJ + }; + + /* Only I-frames are expected in streaming mode */ + if (chan->mode == L2CAP_MODE_STREAMING) + goto drop; + + BT_DBG("sframe reqseq %d, final %d, poll %d, super %d", + control->reqseq, control->final, control->poll, + control->super); + + if (len != 0) { + BT_ERR("Trailing bytes: %d in sframe", len); + l2cap_send_disconn_req(chan, ECONNRESET); + goto drop; + } + + /* Validate F and P bits */ + if (control->final && (control->poll || + chan->tx_state != L2CAP_TX_STATE_WAIT_F)) + goto drop; + + event = rx_func_to_event[control->super]; + if (l2cap_rx(chan, control, skb, event)) + l2cap_send_disconn_req(chan, ECONNRESET); + } + + return 0; + +drop: + kfree_skb(skb); + return 0; +} + +static void l2cap_chan_le_send_credits(struct l2cap_chan *chan) +{ + struct l2cap_conn *conn = chan->conn; + struct l2cap_le_credits pkt; + u16 return_credits; + + /* We return more credits to the sender only after the amount of + * credits falls below half of the initial amount. + */ + if (chan->rx_credits >= (le_max_credits + 1) / 2) + return; + + return_credits = le_max_credits - chan->rx_credits; + + BT_DBG("chan %p returning %u credits to sender", chan, return_credits); + + chan->rx_credits += return_credits; + + pkt.cid = cpu_to_le16(chan->scid); + pkt.credits = cpu_to_le16(return_credits); + + chan->ident = l2cap_get_ident(conn); + + l2cap_send_cmd(conn, chan->ident, L2CAP_LE_CREDITS, sizeof(pkt), &pkt); +} + +static int l2cap_le_data_rcv(struct l2cap_chan *chan, struct sk_buff *skb) +{ + int err; + + if (!chan->rx_credits) { + BT_ERR("No credits to receive LE L2CAP data"); + l2cap_send_disconn_req(chan, ECONNRESET); + return -ENOBUFS; + } + + if (chan->imtu < skb->len) { + BT_ERR("Too big LE L2CAP PDU"); + return -ENOBUFS; + } + + chan->rx_credits--; + BT_DBG("rx_credits %u -> %u", chan->rx_credits + 1, chan->rx_credits); + + l2cap_chan_le_send_credits(chan); + + err = 0; + + if (!chan->sdu) { + u16 sdu_len; + + sdu_len = get_unaligned_le16(skb->data); + skb_pull(skb, L2CAP_SDULEN_SIZE); + + BT_DBG("Start of new SDU. sdu_len %u skb->len %u imtu %u", + sdu_len, skb->len, chan->imtu); + + if (sdu_len > chan->imtu) { + BT_ERR("Too big LE L2CAP SDU length received"); + err = -EMSGSIZE; + goto failed; + } + + if (skb->len > sdu_len) { + BT_ERR("Too much LE L2CAP data received"); + err = -EINVAL; + goto failed; + } + + if (skb->len == sdu_len) + return chan->ops->recv(chan, skb); + + chan->sdu = skb; + chan->sdu_len = sdu_len; + chan->sdu_last_frag = skb; + + return 0; + } + + BT_DBG("SDU fragment. chan->sdu->len %u skb->len %u chan->sdu_len %u", + chan->sdu->len, skb->len, chan->sdu_len); + + if (chan->sdu->len + skb->len > chan->sdu_len) { + BT_ERR("Too much LE L2CAP data received"); + err = -EINVAL; + goto failed; + } + + append_skb_frag(chan->sdu, skb, &chan->sdu_last_frag); + skb = NULL; + + if (chan->sdu->len == chan->sdu_len) { + err = chan->ops->recv(chan, chan->sdu); + if (!err) { + chan->sdu = NULL; + chan->sdu_last_frag = NULL; + chan->sdu_len = 0; + } + } + +failed: + if (err) { + kfree_skb(skb); + kfree_skb(chan->sdu); + chan->sdu = NULL; + chan->sdu_last_frag = NULL; + chan->sdu_len = 0; + } + + /* We can't return an error here since we took care of the skb + * freeing internally. An error return would cause the caller to + * do a double-free of the skb. + */ + return 0; +} + +static void l2cap_data_channel(struct l2cap_conn *conn, u16 cid, + struct sk_buff *skb) +{ + struct l2cap_chan *chan; + + chan = l2cap_get_chan_by_scid(conn, cid); + if (!chan) { + if (cid == L2CAP_CID_A2MP) { + chan = a2mp_channel_create(conn, skb); + if (!chan) { + kfree_skb(skb); + return; + } + + l2cap_chan_lock(chan); + } else { + BT_DBG("unknown cid 0x%4.4x", cid); + /* Drop packet and return */ + kfree_skb(skb); + return; + } + } + + BT_DBG("chan %p, len %d", chan, skb->len); + + /* If we receive data on a fixed channel before the info req/rsp + * procdure is done simply assume that the channel is supported + * and mark it as ready. + */ + if (chan->chan_type == L2CAP_CHAN_FIXED) + l2cap_chan_ready(chan); + + if (chan->state != BT_CONNECTED) + goto drop; + + switch (chan->mode) { + case L2CAP_MODE_LE_FLOWCTL: + if (l2cap_le_data_rcv(chan, skb) < 0) + goto drop; + + goto done; + + case L2CAP_MODE_BASIC: + /* If socket recv buffers overflows we drop data here + * which is *bad* because L2CAP has to be reliable. + * But we don't have any other choice. L2CAP doesn't + * provide flow control mechanism. */ + + if (chan->imtu < skb->len) { + BT_ERR("Dropping L2CAP data: receive buffer overflow"); + goto drop; + } + + if (!chan->ops->recv(chan, skb)) + goto done; + break; + + case L2CAP_MODE_ERTM: + case L2CAP_MODE_STREAMING: + l2cap_data_rcv(chan, skb); + goto done; + + default: + BT_DBG("chan %p: bad mode 0x%2.2x", chan, chan->mode); + break; + } + +drop: + kfree_skb(skb); + +done: + l2cap_chan_unlock(chan); +} + +static void l2cap_conless_channel(struct l2cap_conn *conn, __le16 psm, + struct sk_buff *skb) +{ + struct hci_conn *hcon = conn->hcon; + struct l2cap_chan *chan; + + if (hcon->type != ACL_LINK) + goto free_skb; + + chan = l2cap_global_chan_by_psm(0, psm, &hcon->src, &hcon->dst, + ACL_LINK); + if (!chan) + goto free_skb; + + BT_DBG("chan %p, len %d", chan, skb->len); + + if (chan->state != BT_BOUND && chan->state != BT_CONNECTED) + goto drop; + + if (chan->imtu < skb->len) + goto drop; + + /* Store remote BD_ADDR and PSM for msg_name */ + bacpy(&bt_cb(skb)->l2cap.bdaddr, &hcon->dst); + bt_cb(skb)->l2cap.psm = psm; + + if (!chan->ops->recv(chan, skb)) { + l2cap_chan_put(chan); + return; + } + +drop: + l2cap_chan_put(chan); +free_skb: + kfree_skb(skb); +} + +static void l2cap_recv_frame(struct l2cap_conn *conn, struct sk_buff *skb) +{ + struct l2cap_hdr *lh = (void *) skb->data; + struct hci_conn *hcon = conn->hcon; + u16 cid, len; + __le16 psm; + + if (hcon->state != BT_CONNECTED) { + BT_DBG("queueing pending rx skb"); + skb_queue_tail(&conn->pending_rx, skb); + return; + } + + skb_pull(skb, L2CAP_HDR_SIZE); + cid = __le16_to_cpu(lh->cid); + len = __le16_to_cpu(lh->len); + + if (len != skb->len) { + kfree_skb(skb); + return; + } + + /* Since we can't actively block incoming LE connections we must + * at least ensure that we ignore incoming data from them. + */ + if (hcon->type == LE_LINK && + hci_bdaddr_list_lookup(&hcon->hdev->blacklist, &hcon->dst, + bdaddr_dst_type(hcon))) { + kfree_skb(skb); + return; + } + + BT_DBG("len %d, cid 0x%4.4x", len, cid); + + switch (cid) { + case L2CAP_CID_SIGNALING: + l2cap_sig_channel(conn, skb); + break; + + case L2CAP_CID_CONN_LESS: + psm = get_unaligned((__le16 *) skb->data); + skb_pull(skb, L2CAP_PSMLEN_SIZE); + l2cap_conless_channel(conn, psm, skb); + break; + + case L2CAP_CID_LE_SIGNALING: + l2cap_le_sig_channel(conn, skb); + break; + + default: + l2cap_data_channel(conn, cid, skb); + break; + } +} + +static void process_pending_rx(struct work_struct *work) +{ + struct l2cap_conn *conn = container_of(work, struct l2cap_conn, + pending_rx_work); + struct sk_buff *skb; + + BT_DBG(""); + + while ((skb = skb_dequeue(&conn->pending_rx))) + l2cap_recv_frame(conn, skb); +} + +static struct l2cap_conn *l2cap_conn_add(struct hci_conn *hcon) +{ + struct l2cap_conn *conn = hcon->l2cap_data; + struct hci_chan *hchan; + + if (conn) + return conn; + + hchan = hci_chan_create(hcon); + if (!hchan) + return NULL; + + conn = kzalloc(sizeof(*conn), GFP_KERNEL); + if (!conn) { + hci_chan_del(hchan); + return NULL; + } + + kref_init(&conn->ref); + hcon->l2cap_data = conn; + conn->hcon = hci_conn_get(hcon); + conn->hchan = hchan; + + BT_DBG("hcon %p conn %p hchan %p", hcon, conn, hchan); + + switch (hcon->type) { + case LE_LINK: + if (hcon->hdev->le_mtu) { + conn->mtu = hcon->hdev->le_mtu; + break; + } + /* fall through */ + default: + conn->mtu = hcon->hdev->acl_mtu; + break; + } + + conn->feat_mask = 0; + + conn->local_fixed_chan = L2CAP_FC_SIG_BREDR | L2CAP_FC_CONNLESS; + + if (hcon->type == ACL_LINK && + hci_dev_test_flag(hcon->hdev, HCI_HS_ENABLED)) + conn->local_fixed_chan |= L2CAP_FC_A2MP; + + if (hci_dev_test_flag(hcon->hdev, HCI_LE_ENABLED) && + (bredr_sc_enabled(hcon->hdev) || + hci_dev_test_flag(hcon->hdev, HCI_FORCE_BREDR_SMP))) + conn->local_fixed_chan |= L2CAP_FC_SMP_BREDR; + + mutex_init(&conn->ident_lock); + mutex_init(&conn->chan_lock); + + INIT_LIST_HEAD(&conn->chan_l); + INIT_LIST_HEAD(&conn->users); + + INIT_DELAYED_WORK(&conn->info_timer, l2cap_info_timeout); + + skb_queue_head_init(&conn->pending_rx); + INIT_WORK(&conn->pending_rx_work, process_pending_rx); + INIT_WORK(&conn->id_addr_update_work, l2cap_conn_update_id_addr); + + conn->disc_reason = HCI_ERROR_REMOTE_USER_TERM; + + return conn; +} + +static bool is_valid_psm(u16 psm, u8 dst_type) { + if (!psm) + return false; + + if (bdaddr_type_is_le(dst_type)) + return (psm <= 0x00ff); + + /* PSM must be odd and lsb of upper byte must be 0 */ + return ((psm & 0x0101) == 0x0001); +} + +int l2cap_chan_connect(struct l2cap_chan *chan, __le16 psm, u16 cid, + bdaddr_t *dst, u8 dst_type) +{ + struct l2cap_conn *conn; + struct hci_conn *hcon; + struct hci_dev *hdev; + int err; + + BT_DBG("%pMR -> %pMR (type %u) psm 0x%2.2x", &chan->src, dst, + dst_type, __le16_to_cpu(psm)); + + hdev = hci_get_route(dst, &chan->src); + if (!hdev) + return -EHOSTUNREACH; + + hci_dev_lock(hdev); + + if (!is_valid_psm(__le16_to_cpu(psm), dst_type) && !cid && + chan->chan_type != L2CAP_CHAN_RAW) { + err = -EINVAL; + goto done; + } + + if (chan->chan_type == L2CAP_CHAN_CONN_ORIENTED && !psm) { + err = -EINVAL; + goto done; + } + + if (chan->chan_type == L2CAP_CHAN_FIXED && !cid) { + err = -EINVAL; + goto done; + } + + switch (chan->mode) { + case L2CAP_MODE_BASIC: + break; + case L2CAP_MODE_LE_FLOWCTL: + l2cap_le_flowctl_init(chan); + break; + case L2CAP_MODE_ERTM: + case L2CAP_MODE_STREAMING: + if (!disable_ertm) + break; + /* fall through */ + default: + err = -EOPNOTSUPP; + goto done; + } + + switch (chan->state) { + case BT_CONNECT: + case BT_CONNECT2: + case BT_CONFIG: + /* Already connecting */ + err = 0; + goto done; + + case BT_CONNECTED: + /* Already connected */ + err = -EISCONN; + goto done; + + case BT_OPEN: + case BT_BOUND: + /* Can connect */ + break; + + default: + err = -EBADFD; + goto done; + } + + /* Set destination address and psm */ + bacpy(&chan->dst, dst); + chan->dst_type = dst_type; + + chan->psm = psm; + chan->dcid = cid; + + if (bdaddr_type_is_le(dst_type)) { + u8 role; + + /* Convert from L2CAP channel address type to HCI address type + */ + if (dst_type == BDADDR_LE_PUBLIC) + dst_type = ADDR_LE_DEV_PUBLIC; + else + dst_type = ADDR_LE_DEV_RANDOM; + + if (hci_dev_test_flag(hdev, HCI_ADVERTISING)) + role = HCI_ROLE_SLAVE; + else + role = HCI_ROLE_MASTER; + + hcon = hci_connect_le(hdev, dst, dst_type, chan->sec_level, + HCI_LE_CONN_TIMEOUT, role); + } else { + u8 auth_type = l2cap_get_auth_type(chan); + hcon = hci_connect_acl(hdev, dst, chan->sec_level, auth_type); + } + + if (IS_ERR(hcon)) { + err = PTR_ERR(hcon); + goto done; + } + + conn = l2cap_conn_add(hcon); + if (!conn) { + hci_conn_drop(hcon); + err = -ENOMEM; + goto done; + } + + mutex_lock(&conn->chan_lock); + l2cap_chan_lock(chan); + + if (cid && __l2cap_get_chan_by_dcid(conn, cid)) { + hci_conn_drop(hcon); + err = -EBUSY; + goto chan_unlock; + } + + /* Update source addr of the socket */ + bacpy(&chan->src, &hcon->src); + chan->src_type = bdaddr_src_type(hcon); + + __l2cap_chan_add(conn, chan); + + /* l2cap_chan_add takes its own ref so we can drop this one */ + hci_conn_drop(hcon); + + l2cap_state_change(chan, BT_CONNECT); + __set_chan_timer(chan, chan->ops->get_sndtimeo(chan)); + + /* Release chan->sport so that it can be reused by other + * sockets (as it's only used for listening sockets). + */ + write_lock(&chan_list_lock); + chan->sport = 0; + write_unlock(&chan_list_lock); + + if (hcon->state == BT_CONNECTED) { + if (chan->chan_type != L2CAP_CHAN_CONN_ORIENTED) { + __clear_chan_timer(chan); + if (l2cap_chan_check_security(chan, true)) + l2cap_state_change(chan, BT_CONNECTED); + } else + l2cap_do_start(chan); + } + + err = 0; + +chan_unlock: + l2cap_chan_unlock(chan); + mutex_unlock(&conn->chan_lock); +done: + hci_dev_unlock(hdev); + hci_dev_put(hdev); + return err; +} +EXPORT_SYMBOL_GPL(l2cap_chan_connect); + +/* ---- L2CAP interface with lower layer (HCI) ---- */ + +int l2cap_connect_ind(struct hci_dev *hdev, bdaddr_t *bdaddr) +{ + int exact = 0, lm1 = 0, lm2 = 0; + struct l2cap_chan *c; + + BT_DBG("hdev %s, bdaddr %pMR", hdev->name, bdaddr); + + /* Find listening sockets and check their link_mode */ + read_lock(&chan_list_lock); + list_for_each_entry(c, &chan_list, global_l) { + if (c->state != BT_LISTEN) + continue; + + if (!bacmp(&c->src, &hdev->bdaddr)) { + lm1 |= HCI_LM_ACCEPT; + if (test_bit(FLAG_ROLE_SWITCH, &c->flags)) + lm1 |= HCI_LM_MASTER; + exact++; + } else if (!bacmp(&c->src, BDADDR_ANY)) { + lm2 |= HCI_LM_ACCEPT; + if (test_bit(FLAG_ROLE_SWITCH, &c->flags)) + lm2 |= HCI_LM_MASTER; + } + } + read_unlock(&chan_list_lock); + + return exact ? lm1 : lm2; +} + +/* Find the next fixed channel in BT_LISTEN state, continue iteration + * from an existing channel in the list or from the beginning of the + * global list (by passing NULL as first parameter). + */ +static struct l2cap_chan *l2cap_global_fixed_chan(struct l2cap_chan *c, + struct hci_conn *hcon) +{ + u8 src_type = bdaddr_src_type(hcon); + + read_lock(&chan_list_lock); + + if (c) + c = list_next_entry(c, global_l); + else + c = list_entry(chan_list.next, typeof(*c), global_l); + + list_for_each_entry_from(c, &chan_list, global_l) { + if (c->chan_type != L2CAP_CHAN_FIXED) + continue; + if (c->state != BT_LISTEN) + continue; + if (bacmp(&c->src, &hcon->src) && bacmp(&c->src, BDADDR_ANY)) + continue; + if (src_type != c->src_type) + continue; + + l2cap_chan_hold(c); + read_unlock(&chan_list_lock); + return c; + } + + read_unlock(&chan_list_lock); + + return NULL; +} + +static void l2cap_connect_cfm(struct hci_conn *hcon, u8 status) +{ + struct hci_dev *hdev = hcon->hdev; + struct l2cap_conn *conn; + struct l2cap_chan *pchan; + u8 dst_type; + + if (hcon->type != ACL_LINK && hcon->type != LE_LINK) + return; + + BT_DBG("hcon %p bdaddr %pMR status %d", hcon, &hcon->dst, status); + + if (status) { + l2cap_conn_del(hcon, bt_to_errno(status)); + return; + } + + conn = l2cap_conn_add(hcon); + if (!conn) + return; + + dst_type = bdaddr_dst_type(hcon); + + /* If device is blocked, do not create channels for it */ + if (hci_bdaddr_list_lookup(&hdev->blacklist, &hcon->dst, dst_type)) + return; + + /* Find fixed channels and notify them of the new connection. We + * use multiple individual lookups, continuing each time where + * we left off, because the list lock would prevent calling the + * potentially sleeping l2cap_chan_lock() function. + */ + pchan = l2cap_global_fixed_chan(NULL, hcon); + while (pchan) { + struct l2cap_chan *chan, *next; + + /* Client fixed channels should override server ones */ + if (__l2cap_get_chan_by_dcid(conn, pchan->scid)) + goto next; + + l2cap_chan_lock(pchan); + chan = pchan->ops->new_connection(pchan); + if (chan) { + bacpy(&chan->src, &hcon->src); + bacpy(&chan->dst, &hcon->dst); + chan->src_type = bdaddr_src_type(hcon); + chan->dst_type = dst_type; + + __l2cap_chan_add(conn, chan); + } + + l2cap_chan_unlock(pchan); +next: + next = l2cap_global_fixed_chan(pchan, hcon); + l2cap_chan_put(pchan); + pchan = next; + } + + l2cap_conn_ready(conn); +} + +int l2cap_disconn_ind(struct hci_conn *hcon) +{ + struct l2cap_conn *conn = hcon->l2cap_data; + + BT_DBG("hcon %p", hcon); + + if (!conn) + return HCI_ERROR_REMOTE_USER_TERM; + return conn->disc_reason; +} + +static void l2cap_disconn_cfm(struct hci_conn *hcon, u8 reason) +{ + if (hcon->type != ACL_LINK && hcon->type != LE_LINK) + return; + + BT_DBG("hcon %p reason %d", hcon, reason); + + l2cap_conn_del(hcon, bt_to_errno(reason)); +} + +static inline void l2cap_check_encryption(struct l2cap_chan *chan, u8 encrypt) +{ + if (chan->chan_type != L2CAP_CHAN_CONN_ORIENTED) + return; + + if (encrypt == 0x00) { + if (chan->sec_level == BT_SECURITY_MEDIUM) { + __set_chan_timer(chan, L2CAP_ENC_TIMEOUT); + } else if (chan->sec_level == BT_SECURITY_HIGH || + chan->sec_level == BT_SECURITY_FIPS) + l2cap_chan_close(chan, ECONNREFUSED); + } else { + if (chan->sec_level == BT_SECURITY_MEDIUM) + __clear_chan_timer(chan); + } +} + +static void l2cap_security_cfm(struct hci_conn *hcon, u8 status, u8 encrypt) +{ + struct l2cap_conn *conn = hcon->l2cap_data; + struct l2cap_chan *chan; + + if (!conn) + return; + + BT_DBG("conn %p status 0x%2.2x encrypt %u", conn, status, encrypt); + + mutex_lock(&conn->chan_lock); + + list_for_each_entry(chan, &conn->chan_l, list) { + l2cap_chan_lock(chan); + + BT_DBG("chan %p scid 0x%4.4x state %s", chan, chan->scid, + state_to_string(chan->state)); + + if (chan->scid == L2CAP_CID_A2MP) { + l2cap_chan_unlock(chan); + continue; + } + + if (!status && encrypt) + chan->sec_level = hcon->sec_level; + + if (!__l2cap_no_conn_pending(chan)) { + l2cap_chan_unlock(chan); + continue; + } + + if (!status && (chan->state == BT_CONNECTED || + chan->state == BT_CONFIG)) { + chan->ops->resume(chan); + l2cap_check_encryption(chan, encrypt); + l2cap_chan_unlock(chan); + continue; + } + + if (chan->state == BT_CONNECT) { + if (!status) + l2cap_start_connection(chan); + else + __set_chan_timer(chan, L2CAP_DISC_TIMEOUT); + } else if (chan->state == BT_CONNECT2 && + chan->mode != L2CAP_MODE_LE_FLOWCTL) { + struct l2cap_conn_rsp rsp; + __u16 res, stat; + + if (!status) { + if (test_bit(FLAG_DEFER_SETUP, &chan->flags)) { + res = L2CAP_CR_PEND; + stat = L2CAP_CS_AUTHOR_PEND; + chan->ops->defer(chan); + } else { + l2cap_state_change(chan, BT_CONFIG); + res = L2CAP_CR_SUCCESS; + stat = L2CAP_CS_NO_INFO; + } + } else { + l2cap_state_change(chan, BT_DISCONN); + __set_chan_timer(chan, L2CAP_DISC_TIMEOUT); + res = L2CAP_CR_SEC_BLOCK; + stat = L2CAP_CS_NO_INFO; + } + + rsp.scid = cpu_to_le16(chan->dcid); + rsp.dcid = cpu_to_le16(chan->scid); + rsp.result = cpu_to_le16(res); + rsp.status = cpu_to_le16(stat); + l2cap_send_cmd(conn, chan->ident, L2CAP_CONN_RSP, + sizeof(rsp), &rsp); + + if (!test_bit(CONF_REQ_SENT, &chan->conf_state) && + res == L2CAP_CR_SUCCESS) { + char buf[128]; + set_bit(CONF_REQ_SENT, &chan->conf_state); + l2cap_send_cmd(conn, l2cap_get_ident(conn), + L2CAP_CONF_REQ, + l2cap_build_conf_req(chan, buf), + buf); + chan->num_conf_req++; + } + } + + l2cap_chan_unlock(chan); + } + + mutex_unlock(&conn->chan_lock); +} + +int l2cap_recv_acldata(struct hci_conn *hcon, struct sk_buff *skb, u16 flags) +{ + struct l2cap_conn *conn = hcon->l2cap_data; + struct l2cap_hdr *hdr; + int len; + + /* For AMP controller do not create l2cap conn */ + if (!conn && hcon->hdev->dev_type != HCI_BREDR) + goto drop; + + if (!conn) + conn = l2cap_conn_add(hcon); + + if (!conn) + goto drop; + + BT_DBG("conn %p len %d flags 0x%x", conn, skb->len, flags); + + switch (flags) { + case ACL_START: + case ACL_START_NO_FLUSH: + case ACL_COMPLETE: + if (conn->rx_len) { + BT_ERR("Unexpected start frame (len %d)", skb->len); + kfree_skb(conn->rx_skb); + conn->rx_skb = NULL; + conn->rx_len = 0; + l2cap_conn_unreliable(conn, ECOMM); + } + + /* Start fragment always begin with Basic L2CAP header */ + if (skb->len < L2CAP_HDR_SIZE) { + BT_ERR("Frame is too short (len %d)", skb->len); + l2cap_conn_unreliable(conn, ECOMM); + goto drop; + } + + hdr = (struct l2cap_hdr *) skb->data; + len = __le16_to_cpu(hdr->len) + L2CAP_HDR_SIZE; + + if (len == skb->len) { + /* Complete frame received */ + l2cap_recv_frame(conn, skb); + return 0; + } + + BT_DBG("Start: total len %d, frag len %d", len, skb->len); + + if (skb->len > len) { + BT_ERR("Frame is too long (len %d, expected len %d)", + skb->len, len); + l2cap_conn_unreliable(conn, ECOMM); + goto drop; + } + + /* Allocate skb for the complete frame (with header) */ + conn->rx_skb = bt_skb_alloc(len, GFP_KERNEL); + if (!conn->rx_skb) + goto drop; + + skb_copy_from_linear_data(skb, skb_put(conn->rx_skb, skb->len), + skb->len); + conn->rx_len = len - skb->len; + break; + + case ACL_CONT: + BT_DBG("Cont: frag len %d (expecting %d)", skb->len, conn->rx_len); + + if (!conn->rx_len) { + BT_ERR("Unexpected continuation frame (len %d)", skb->len); + l2cap_conn_unreliable(conn, ECOMM); + goto drop; + } + + if (skb->len > conn->rx_len) { + BT_ERR("Fragment is too long (len %d, expected %d)", + skb->len, conn->rx_len); + kfree_skb(conn->rx_skb); + conn->rx_skb = NULL; + conn->rx_len = 0; + l2cap_conn_unreliable(conn, ECOMM); + goto drop; + } + + skb_copy_from_linear_data(skb, skb_put(conn->rx_skb, skb->len), + skb->len); + conn->rx_len -= skb->len; + + if (!conn->rx_len) { + /* Complete frame received. l2cap_recv_frame + * takes ownership of the skb so set the global + * rx_skb pointer to NULL first. + */ + struct sk_buff *rx_skb = conn->rx_skb; + conn->rx_skb = NULL; + l2cap_recv_frame(conn, rx_skb); + } + break; + } + +drop: + kfree_skb(skb); + return 0; +} + +static struct hci_cb l2cap_cb = { + .name = "L2CAP", + .connect_cfm = l2cap_connect_cfm, + .disconn_cfm = l2cap_disconn_cfm, + .security_cfm = l2cap_security_cfm, +}; + +static int l2cap_debugfs_show(struct seq_file *f, void *p) +{ + struct l2cap_chan *c; + + read_lock(&chan_list_lock); + + list_for_each_entry(c, &chan_list, global_l) { + seq_printf(f, "%pMR (%u) %pMR (%u) %d %d 0x%4.4x 0x%4.4x %d %d %d %d\n", + &c->src, c->src_type, &c->dst, c->dst_type, + c->state, __le16_to_cpu(c->psm), + c->scid, c->dcid, c->imtu, c->omtu, + c->sec_level, c->mode); + } + + read_unlock(&chan_list_lock); + + return 0; +} + +static int l2cap_debugfs_open(struct inode *inode, struct file *file) +{ + return single_open(file, l2cap_debugfs_show, inode->i_private); +} + +static const struct file_operations l2cap_debugfs_fops = { + .open = l2cap_debugfs_open, + .read = seq_read, + .llseek = seq_lseek, + .release = single_release, +}; + +static struct dentry *l2cap_debugfs; + +int __init l2cap_init(void) +{ + int err; + + err = l2cap_init_sockets(); + if (err < 0) + return err; + + hci_register_cb(&l2cap_cb); + + if (IS_ERR_OR_NULL(bt_debugfs)) + return 0; + + l2cap_debugfs = debugfs_create_file("l2cap", 0444, bt_debugfs, + NULL, &l2cap_debugfs_fops); + + debugfs_create_u16("l2cap_le_max_credits", 0644, bt_debugfs, + &le_max_credits); + debugfs_create_u16("l2cap_le_default_mps", 0644, bt_debugfs, + &le_default_mps); + + return 0; +} + +void l2cap_exit(void) +{ + debugfs_remove(l2cap_debugfs); + hci_unregister_cb(&l2cap_cb); + l2cap_cleanup_sockets(); +} + +module_param(disable_ertm, bool, 0644); +MODULE_PARM_DESC(disable_ertm, "Disable enhanced retransmission mode"); diff --git a/kernel/net/bluetooth/l2cap_sock.c b/kernel/net/bluetooth/l2cap_sock.c new file mode 100644 index 000000000..a7278f05e --- /dev/null +++ b/kernel/net/bluetooth/l2cap_sock.c @@ -0,0 +1,1650 @@ +/* + BlueZ - Bluetooth protocol stack for Linux + Copyright (C) 2000-2001 Qualcomm Incorporated + Copyright (C) 2009-2010 Gustavo F. Padovan <gustavo@padovan.org> + Copyright (C) 2010 Google Inc. + Copyright (C) 2011 ProFUSION Embedded Systems + + Written 2000,2001 by Maxim Krasnyansky <maxk@qualcomm.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; + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS + OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT OF THIRD PARTY RIGHTS. + IN NO EVENT SHALL THE COPYRIGHT HOLDER(S) AND AUTHOR(S) BE LIABLE FOR ANY + CLAIM, OR ANY SPECIAL INDIRECT OR CONSEQUENTIAL DAMAGES, OR ANY DAMAGES + WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN + ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF + OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + + ALL LIABILITY, INCLUDING LIABILITY FOR INFRINGEMENT OF ANY PATENTS, + COPYRIGHTS, TRADEMARKS OR OTHER RIGHTS, RELATING TO USE OF THIS + SOFTWARE IS DISCLAIMED. +*/ + +/* Bluetooth L2CAP sockets. */ + +#include <linux/module.h> +#include <linux/export.h> + +#include <net/bluetooth/bluetooth.h> +#include <net/bluetooth/hci_core.h> +#include <net/bluetooth/l2cap.h> + +#include "smp.h" + +static struct bt_sock_list l2cap_sk_list = { + .lock = __RW_LOCK_UNLOCKED(l2cap_sk_list.lock) +}; + +static const struct proto_ops l2cap_sock_ops; +static void l2cap_sock_init(struct sock *sk, struct sock *parent); +static struct sock *l2cap_sock_alloc(struct net *net, struct socket *sock, + int proto, gfp_t prio); + +bool l2cap_is_socket(struct socket *sock) +{ + return sock && sock->ops == &l2cap_sock_ops; +} +EXPORT_SYMBOL(l2cap_is_socket); + +static int l2cap_validate_bredr_psm(u16 psm) +{ + /* PSM must be odd and lsb of upper byte must be 0 */ + if ((psm & 0x0101) != 0x0001) + return -EINVAL; + + /* Restrict usage of well-known PSMs */ + if (psm < 0x1001 && !capable(CAP_NET_BIND_SERVICE)) + return -EACCES; + + return 0; +} + +static int l2cap_validate_le_psm(u16 psm) +{ + /* Valid LE_PSM ranges are defined only until 0x00ff */ + if (psm > 0x00ff) + return -EINVAL; + + /* Restrict fixed, SIG assigned PSM values to CAP_NET_BIND_SERVICE */ + if (psm <= 0x007f && !capable(CAP_NET_BIND_SERVICE)) + return -EACCES; + + return 0; +} + +static int l2cap_sock_bind(struct socket *sock, struct sockaddr *addr, int alen) +{ + struct sock *sk = sock->sk; + struct l2cap_chan *chan = l2cap_pi(sk)->chan; + struct sockaddr_l2 la; + int len, err = 0; + + BT_DBG("sk %p", sk); + + if (!addr || addr->sa_family != AF_BLUETOOTH) + return -EINVAL; + + memset(&la, 0, sizeof(la)); + len = min_t(unsigned int, sizeof(la), alen); + memcpy(&la, addr, len); + + if (la.l2_cid && la.l2_psm) + return -EINVAL; + + if (!bdaddr_type_is_valid(la.l2_bdaddr_type)) + return -EINVAL; + + if (bdaddr_type_is_le(la.l2_bdaddr_type)) { + /* We only allow ATT user space socket */ + if (la.l2_cid && + la.l2_cid != cpu_to_le16(L2CAP_CID_ATT)) + return -EINVAL; + } + + lock_sock(sk); + + if (sk->sk_state != BT_OPEN) { + err = -EBADFD; + goto done; + } + + if (la.l2_psm) { + __u16 psm = __le16_to_cpu(la.l2_psm); + + if (la.l2_bdaddr_type == BDADDR_BREDR) + err = l2cap_validate_bredr_psm(psm); + else + err = l2cap_validate_le_psm(psm); + + if (err) + goto done; + } + + if (la.l2_cid) + err = l2cap_add_scid(chan, __le16_to_cpu(la.l2_cid)); + else + err = l2cap_add_psm(chan, &la.l2_bdaddr, la.l2_psm); + + if (err < 0) + goto done; + + switch (chan->chan_type) { + case L2CAP_CHAN_CONN_LESS: + if (__le16_to_cpu(la.l2_psm) == L2CAP_PSM_3DSP) + chan->sec_level = BT_SECURITY_SDP; + break; + case L2CAP_CHAN_CONN_ORIENTED: + if (__le16_to_cpu(la.l2_psm) == L2CAP_PSM_SDP || + __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; + case L2CAP_CHAN_FIXED: + /* Fixed channels default to the L2CAP core not holding a + * hci_conn reference for them. For fixed channels mapping to + * L2CAP sockets we do want to hold a reference so set the + * appropriate flag to request it. + */ + set_bit(FLAG_HOLD_HCI_CONN, &chan->flags); + break; + } + + bacpy(&chan->src, &la.l2_bdaddr); + chan->src_type = la.l2_bdaddr_type; + + if (chan->psm && bdaddr_type_is_le(chan->src_type)) + chan->mode = L2CAP_MODE_LE_FLOWCTL; + + chan->state = BT_BOUND; + sk->sk_state = BT_BOUND; + +done: + release_sock(sk); + return err; +} + +static int l2cap_sock_connect(struct socket *sock, struct sockaddr *addr, + int alen, int flags) +{ + struct sock *sk = sock->sk; + struct l2cap_chan *chan = l2cap_pi(sk)->chan; + struct sockaddr_l2 la; + int len, err = 0; + + BT_DBG("sk %p", sk); + + if (!addr || alen < sizeof(addr->sa_family) || + addr->sa_family != AF_BLUETOOTH) + return -EINVAL; + + memset(&la, 0, sizeof(la)); + len = min_t(unsigned int, sizeof(la), alen); + memcpy(&la, addr, len); + + if (la.l2_cid && la.l2_psm) + return -EINVAL; + + if (!bdaddr_type_is_valid(la.l2_bdaddr_type)) + return -EINVAL; + + /* Check that the socket wasn't bound to something that + * conflicts with the address given to connect(). If chan->src + * is BDADDR_ANY it means bind() was never used, in which case + * chan->src_type and la.l2_bdaddr_type do not need to match. + */ + if (chan->src_type == BDADDR_BREDR && bacmp(&chan->src, BDADDR_ANY) && + bdaddr_type_is_le(la.l2_bdaddr_type)) { + /* Old user space versions will try to incorrectly bind + * the ATT socket using BDADDR_BREDR. We need to accept + * this and fix up the source address type only when + * both the source CID and destination CID indicate + * ATT. Anything else is an invalid combination. + */ + if (chan->scid != L2CAP_CID_ATT || + la.l2_cid != cpu_to_le16(L2CAP_CID_ATT)) + return -EINVAL; + + /* We don't have the hdev available here to make a + * better decision on random vs public, but since all + * user space versions that exhibit this issue anyway do + * not support random local addresses assuming public + * here is good enough. + */ + chan->src_type = BDADDR_LE_PUBLIC; + } + + if (chan->src_type != BDADDR_BREDR && la.l2_bdaddr_type == BDADDR_BREDR) + return -EINVAL; + + if (bdaddr_type_is_le(la.l2_bdaddr_type)) { + /* We only allow ATT user space socket */ + if (la.l2_cid && + la.l2_cid != cpu_to_le16(L2CAP_CID_ATT)) + return -EINVAL; + } + + if (chan->psm && bdaddr_type_is_le(chan->src_type)) + chan->mode = L2CAP_MODE_LE_FLOWCTL; + + err = l2cap_chan_connect(chan, la.l2_psm, __le16_to_cpu(la.l2_cid), + &la.l2_bdaddr, la.l2_bdaddr_type); + if (err) + return err; + + lock_sock(sk); + + err = bt_sock_wait_state(sk, BT_CONNECTED, + sock_sndtimeo(sk, flags & O_NONBLOCK)); + + release_sock(sk); + + return err; +} + +static int l2cap_sock_listen(struct socket *sock, int backlog) +{ + struct sock *sk = sock->sk; + struct l2cap_chan *chan = l2cap_pi(sk)->chan; + int err = 0; + + BT_DBG("sk %p backlog %d", sk, backlog); + + lock_sock(sk); + + if (sk->sk_state != BT_BOUND) { + err = -EBADFD; + goto done; + } + + if (sk->sk_type != SOCK_SEQPACKET && sk->sk_type != SOCK_STREAM) { + err = -EINVAL; + goto done; + } + + switch (chan->mode) { + case L2CAP_MODE_BASIC: + case L2CAP_MODE_LE_FLOWCTL: + break; + case L2CAP_MODE_ERTM: + case L2CAP_MODE_STREAMING: + if (!disable_ertm) + break; + /* fall through */ + default: + err = -EOPNOTSUPP; + goto done; + } + + sk->sk_max_ack_backlog = backlog; + sk->sk_ack_backlog = 0; + + /* Listening channels need to use nested locking in order not to + * cause lockdep warnings when the created child channels end up + * being locked in the same thread as the parent channel. + */ + atomic_set(&chan->nesting, L2CAP_NESTING_PARENT); + + chan->state = BT_LISTEN; + sk->sk_state = BT_LISTEN; + +done: + release_sock(sk); + return err; +} + +static int l2cap_sock_accept(struct socket *sock, struct socket *newsock, + int flags) +{ + DEFINE_WAIT_FUNC(wait, woken_wake_function); + struct sock *sk = sock->sk, *nsk; + long timeo; + int err = 0; + + lock_sock_nested(sk, L2CAP_NESTING_PARENT); + + timeo = sock_rcvtimeo(sk, flags & O_NONBLOCK); + + BT_DBG("sk %p timeo %ld", sk, timeo); + + /* Wait for an incoming connection. (wake-one). */ + add_wait_queue_exclusive(sk_sleep(sk), &wait); + while (1) { + if (sk->sk_state != BT_LISTEN) { + err = -EBADFD; + break; + } + + nsk = bt_accept_dequeue(sk, newsock); + if (nsk) + break; + + if (!timeo) { + err = -EAGAIN; + break; + } + + if (signal_pending(current)) { + err = sock_intr_errno(timeo); + break; + } + + release_sock(sk); + + timeo = wait_woken(&wait, TASK_INTERRUPTIBLE, timeo); + + lock_sock_nested(sk, L2CAP_NESTING_PARENT); + } + remove_wait_queue(sk_sleep(sk), &wait); + + if (err) + goto done; + + newsock->state = SS_CONNECTED; + + BT_DBG("new socket %p", nsk); + +done: + release_sock(sk); + return err; +} + +static int l2cap_sock_getname(struct socket *sock, struct sockaddr *addr, + int *len, int peer) +{ + struct sockaddr_l2 *la = (struct sockaddr_l2 *) addr; + struct sock *sk = sock->sk; + struct l2cap_chan *chan = l2cap_pi(sk)->chan; + + BT_DBG("sock %p, sk %p", sock, sk); + + if (peer && sk->sk_state != BT_CONNECTED && + sk->sk_state != BT_CONNECT && sk->sk_state != BT_CONNECT2 && + sk->sk_state != BT_CONFIG) + return -ENOTCONN; + + memset(la, 0, sizeof(struct sockaddr_l2)); + addr->sa_family = AF_BLUETOOTH; + *len = sizeof(struct sockaddr_l2); + + la->l2_psm = chan->psm; + + if (peer) { + bacpy(&la->l2_bdaddr, &chan->dst); + la->l2_cid = cpu_to_le16(chan->dcid); + la->l2_bdaddr_type = chan->dst_type; + } else { + bacpy(&la->l2_bdaddr, &chan->src); + la->l2_cid = cpu_to_le16(chan->scid); + la->l2_bdaddr_type = chan->src_type; + } + + return 0; +} + +static int l2cap_sock_getsockopt_old(struct socket *sock, int optname, + char __user *optval, int __user *optlen) +{ + struct sock *sk = sock->sk; + struct l2cap_chan *chan = l2cap_pi(sk)->chan; + struct l2cap_options opts; + struct l2cap_conninfo cinfo; + int len, err = 0; + u32 opt; + + BT_DBG("sk %p", sk); + + if (get_user(len, optlen)) + return -EFAULT; + + lock_sock(sk); + + switch (optname) { + case L2CAP_OPTIONS: + /* LE sockets should use BT_SNDMTU/BT_RCVMTU, but since + * legacy ATT code depends on getsockopt for + * L2CAP_OPTIONS we need to let this pass. + */ + if (bdaddr_type_is_le(chan->src_type) && + chan->scid != L2CAP_CID_ATT) { + err = -EINVAL; + break; + } + + memset(&opts, 0, sizeof(opts)); + opts.imtu = chan->imtu; + opts.omtu = chan->omtu; + opts.flush_to = chan->flush_to; + opts.mode = chan->mode; + opts.fcs = chan->fcs; + opts.max_tx = chan->max_tx; + opts.txwin_size = chan->tx_win; + + len = min_t(unsigned int, len, sizeof(opts)); + if (copy_to_user(optval, (char *) &opts, len)) + err = -EFAULT; + + break; + + case L2CAP_LM: + switch (chan->sec_level) { + case BT_SECURITY_LOW: + opt = L2CAP_LM_AUTH; + break; + case BT_SECURITY_MEDIUM: + opt = L2CAP_LM_AUTH | L2CAP_LM_ENCRYPT; + break; + case BT_SECURITY_HIGH: + opt = L2CAP_LM_AUTH | L2CAP_LM_ENCRYPT | + L2CAP_LM_SECURE; + break; + case BT_SECURITY_FIPS: + opt = L2CAP_LM_AUTH | L2CAP_LM_ENCRYPT | + L2CAP_LM_SECURE | L2CAP_LM_FIPS; + break; + default: + opt = 0; + break; + } + + if (test_bit(FLAG_ROLE_SWITCH, &chan->flags)) + opt |= L2CAP_LM_MASTER; + + if (test_bit(FLAG_FORCE_RELIABLE, &chan->flags)) + opt |= L2CAP_LM_RELIABLE; + + if (put_user(opt, (u32 __user *) optval)) + err = -EFAULT; + + break; + + case L2CAP_CONNINFO: + if (sk->sk_state != BT_CONNECTED && + !(sk->sk_state == BT_CONNECT2 && + test_bit(BT_SK_DEFER_SETUP, &bt_sk(sk)->flags))) { + err = -ENOTCONN; + break; + } + + memset(&cinfo, 0, sizeof(cinfo)); + cinfo.hci_handle = chan->conn->hcon->handle; + memcpy(cinfo.dev_class, chan->conn->hcon->dev_class, 3); + + len = min_t(unsigned int, len, sizeof(cinfo)); + if (copy_to_user(optval, (char *) &cinfo, len)) + err = -EFAULT; + + break; + + default: + err = -ENOPROTOOPT; + break; + } + + release_sock(sk); + return err; +} + +static int l2cap_sock_getsockopt(struct socket *sock, int level, int optname, + char __user *optval, int __user *optlen) +{ + struct sock *sk = sock->sk; + struct l2cap_chan *chan = l2cap_pi(sk)->chan; + struct bt_security sec; + struct bt_power pwr; + int len, err = 0; + + BT_DBG("sk %p", sk); + + if (level == SOL_L2CAP) + return l2cap_sock_getsockopt_old(sock, optname, optval, optlen); + + if (level != SOL_BLUETOOTH) + return -ENOPROTOOPT; + + if (get_user(len, optlen)) + return -EFAULT; + + lock_sock(sk); + + switch (optname) { + case BT_SECURITY: + if (chan->chan_type != L2CAP_CHAN_CONN_ORIENTED && + chan->chan_type != L2CAP_CHAN_FIXED && + chan->chan_type != L2CAP_CHAN_RAW) { + err = -EINVAL; + break; + } + + memset(&sec, 0, sizeof(sec)); + if (chan->conn) { + sec.level = chan->conn->hcon->sec_level; + + if (sk->sk_state == BT_CONNECTED) + sec.key_size = chan->conn->hcon->enc_key_size; + } else { + sec.level = chan->sec_level; + } + + len = min_t(unsigned int, len, sizeof(sec)); + if (copy_to_user(optval, (char *) &sec, len)) + err = -EFAULT; + + break; + + case BT_DEFER_SETUP: + if (sk->sk_state != BT_BOUND && sk->sk_state != BT_LISTEN) { + err = -EINVAL; + break; + } + + if (put_user(test_bit(BT_SK_DEFER_SETUP, &bt_sk(sk)->flags), + (u32 __user *) optval)) + err = -EFAULT; + + break; + + case BT_FLUSHABLE: + if (put_user(test_bit(FLAG_FLUSHABLE, &chan->flags), + (u32 __user *) optval)) + err = -EFAULT; + + break; + + case BT_POWER: + if (sk->sk_type != SOCK_SEQPACKET && sk->sk_type != SOCK_STREAM + && sk->sk_type != SOCK_RAW) { + err = -EINVAL; + break; + } + + pwr.force_active = test_bit(FLAG_FORCE_ACTIVE, &chan->flags); + + len = min_t(unsigned int, len, sizeof(pwr)); + if (copy_to_user(optval, (char *) &pwr, len)) + err = -EFAULT; + + break; + + case BT_CHANNEL_POLICY: + if (put_user(chan->chan_policy, (u32 __user *) optval)) + err = -EFAULT; + break; + + case BT_SNDMTU: + if (!bdaddr_type_is_le(chan->src_type)) { + err = -EINVAL; + break; + } + + if (sk->sk_state != BT_CONNECTED) { + err = -ENOTCONN; + break; + } + + if (put_user(chan->omtu, (u16 __user *) optval)) + err = -EFAULT; + break; + + case BT_RCVMTU: + if (!bdaddr_type_is_le(chan->src_type)) { + err = -EINVAL; + break; + } + + if (put_user(chan->imtu, (u16 __user *) optval)) + err = -EFAULT; + break; + + default: + err = -ENOPROTOOPT; + break; + } + + release_sock(sk); + return err; +} + +static bool l2cap_valid_mtu(struct l2cap_chan *chan, u16 mtu) +{ + switch (chan->scid) { + case L2CAP_CID_ATT: + if (mtu < L2CAP_LE_MIN_MTU) + return false; + break; + + default: + if (mtu < L2CAP_DEFAULT_MIN_MTU) + return false; + } + + return true; +} + +static int l2cap_sock_setsockopt_old(struct socket *sock, int optname, + char __user *optval, unsigned int optlen) +{ + struct sock *sk = sock->sk; + struct l2cap_chan *chan = l2cap_pi(sk)->chan; + struct l2cap_options opts; + int len, err = 0; + u32 opt; + + BT_DBG("sk %p", sk); + + lock_sock(sk); + + switch (optname) { + case L2CAP_OPTIONS: + if (bdaddr_type_is_le(chan->src_type)) { + err = -EINVAL; + break; + } + + if (sk->sk_state == BT_CONNECTED) { + err = -EINVAL; + break; + } + + opts.imtu = chan->imtu; + opts.omtu = chan->omtu; + opts.flush_to = chan->flush_to; + opts.mode = chan->mode; + opts.fcs = chan->fcs; + opts.max_tx = chan->max_tx; + opts.txwin_size = chan->tx_win; + + len = min_t(unsigned int, sizeof(opts), optlen); + if (copy_from_user((char *) &opts, optval, len)) { + err = -EFAULT; + break; + } + + if (opts.txwin_size > L2CAP_DEFAULT_EXT_WINDOW) { + err = -EINVAL; + break; + } + + if (!l2cap_valid_mtu(chan, opts.imtu)) { + err = -EINVAL; + break; + } + + chan->mode = opts.mode; + switch (chan->mode) { + case L2CAP_MODE_LE_FLOWCTL: + break; + case L2CAP_MODE_BASIC: + clear_bit(CONF_STATE2_DEVICE, &chan->conf_state); + break; + case L2CAP_MODE_ERTM: + case L2CAP_MODE_STREAMING: + if (!disable_ertm) + break; + /* fall through */ + default: + err = -EINVAL; + break; + } + + chan->imtu = opts.imtu; + chan->omtu = opts.omtu; + chan->fcs = opts.fcs; + chan->max_tx = opts.max_tx; + chan->tx_win = opts.txwin_size; + chan->flush_to = opts.flush_to; + break; + + case L2CAP_LM: + if (get_user(opt, (u32 __user *) optval)) { + err = -EFAULT; + break; + } + + if (opt & L2CAP_LM_FIPS) { + err = -EINVAL; + break; + } + + if (opt & L2CAP_LM_AUTH) + chan->sec_level = BT_SECURITY_LOW; + if (opt & L2CAP_LM_ENCRYPT) + chan->sec_level = BT_SECURITY_MEDIUM; + if (opt & L2CAP_LM_SECURE) + chan->sec_level = BT_SECURITY_HIGH; + + if (opt & L2CAP_LM_MASTER) + set_bit(FLAG_ROLE_SWITCH, &chan->flags); + else + clear_bit(FLAG_ROLE_SWITCH, &chan->flags); + + if (opt & L2CAP_LM_RELIABLE) + set_bit(FLAG_FORCE_RELIABLE, &chan->flags); + else + clear_bit(FLAG_FORCE_RELIABLE, &chan->flags); + break; + + default: + err = -ENOPROTOOPT; + break; + } + + release_sock(sk); + return err; +} + +static int l2cap_sock_setsockopt(struct socket *sock, int level, int optname, + char __user *optval, unsigned int optlen) +{ + struct sock *sk = sock->sk; + struct l2cap_chan *chan = l2cap_pi(sk)->chan; + struct bt_security sec; + struct bt_power pwr; + struct l2cap_conn *conn; + int len, err = 0; + u32 opt; + + BT_DBG("sk %p", sk); + + if (level == SOL_L2CAP) + return l2cap_sock_setsockopt_old(sock, optname, optval, optlen); + + if (level != SOL_BLUETOOTH) + return -ENOPROTOOPT; + + lock_sock(sk); + + switch (optname) { + case BT_SECURITY: + if (chan->chan_type != L2CAP_CHAN_CONN_ORIENTED && + chan->chan_type != L2CAP_CHAN_FIXED && + chan->chan_type != L2CAP_CHAN_RAW) { + err = -EINVAL; + break; + } + + sec.level = BT_SECURITY_LOW; + + len = min_t(unsigned int, sizeof(sec), optlen); + if (copy_from_user((char *) &sec, optval, len)) { + err = -EFAULT; + break; + } + + if (sec.level < BT_SECURITY_LOW || + sec.level > BT_SECURITY_HIGH) { + err = -EINVAL; + break; + } + + chan->sec_level = sec.level; + + if (!chan->conn) + break; + + conn = chan->conn; + + /*change security for LE channels */ + if (chan->scid == L2CAP_CID_ATT) { + if (smp_conn_security(conn->hcon, sec.level)) + break; + set_bit(FLAG_PENDING_SECURITY, &chan->flags); + sk->sk_state = BT_CONFIG; + chan->state = BT_CONFIG; + + /* or for ACL link */ + } else if ((sk->sk_state == BT_CONNECT2 && + test_bit(BT_SK_DEFER_SETUP, &bt_sk(sk)->flags)) || + sk->sk_state == BT_CONNECTED) { + if (!l2cap_chan_check_security(chan, true)) + set_bit(BT_SK_SUSPEND, &bt_sk(sk)->flags); + else + sk->sk_state_change(sk); + } else { + err = -EINVAL; + } + break; + + case BT_DEFER_SETUP: + if (sk->sk_state != BT_BOUND && sk->sk_state != BT_LISTEN) { + err = -EINVAL; + break; + } + + if (get_user(opt, (u32 __user *) optval)) { + err = -EFAULT; + break; + } + + if (opt) { + set_bit(BT_SK_DEFER_SETUP, &bt_sk(sk)->flags); + set_bit(FLAG_DEFER_SETUP, &chan->flags); + } else { + clear_bit(BT_SK_DEFER_SETUP, &bt_sk(sk)->flags); + clear_bit(FLAG_DEFER_SETUP, &chan->flags); + } + break; + + case BT_FLUSHABLE: + if (get_user(opt, (u32 __user *) optval)) { + err = -EFAULT; + break; + } + + if (opt > BT_FLUSHABLE_ON) { + err = -EINVAL; + break; + } + + if (opt == BT_FLUSHABLE_OFF) { + conn = chan->conn; + /* proceed further only when we have l2cap_conn and + No Flush support in the LM */ + if (!conn || !lmp_no_flush_capable(conn->hcon->hdev)) { + err = -EINVAL; + break; + } + } + + if (opt) + set_bit(FLAG_FLUSHABLE, &chan->flags); + else + clear_bit(FLAG_FLUSHABLE, &chan->flags); + break; + + case BT_POWER: + if (chan->chan_type != L2CAP_CHAN_CONN_ORIENTED && + chan->chan_type != L2CAP_CHAN_RAW) { + err = -EINVAL; + break; + } + + pwr.force_active = BT_POWER_FORCE_ACTIVE_ON; + + len = min_t(unsigned int, sizeof(pwr), optlen); + if (copy_from_user((char *) &pwr, optval, len)) { + err = -EFAULT; + break; + } + + if (pwr.force_active) + set_bit(FLAG_FORCE_ACTIVE, &chan->flags); + else + clear_bit(FLAG_FORCE_ACTIVE, &chan->flags); + break; + + case BT_CHANNEL_POLICY: + if (get_user(opt, (u32 __user *) optval)) { + err = -EFAULT; + break; + } + + if (opt > BT_CHANNEL_POLICY_AMP_PREFERRED) { + err = -EINVAL; + break; + } + + if (chan->mode != L2CAP_MODE_ERTM && + chan->mode != L2CAP_MODE_STREAMING) { + err = -EOPNOTSUPP; + break; + } + + chan->chan_policy = (u8) opt; + + if (sk->sk_state == BT_CONNECTED && + chan->move_role == L2CAP_MOVE_ROLE_NONE) + l2cap_move_start(chan); + + break; + + case BT_SNDMTU: + if (!bdaddr_type_is_le(chan->src_type)) { + err = -EINVAL; + break; + } + + /* Setting is not supported as it's the remote side that + * decides this. + */ + err = -EPERM; + break; + + case BT_RCVMTU: + if (!bdaddr_type_is_le(chan->src_type)) { + err = -EINVAL; + break; + } + + if (sk->sk_state == BT_CONNECTED) { + err = -EISCONN; + break; + } + + if (get_user(opt, (u32 __user *) optval)) { + err = -EFAULT; + break; + } + + chan->imtu = opt; + break; + + default: + err = -ENOPROTOOPT; + break; + } + + release_sock(sk); + return err; +} + +static int l2cap_sock_sendmsg(struct socket *sock, struct msghdr *msg, + size_t len) +{ + struct sock *sk = sock->sk; + struct l2cap_chan *chan = l2cap_pi(sk)->chan; + int err; + + BT_DBG("sock %p, sk %p", sock, sk); + + err = sock_error(sk); + if (err) + return err; + + if (msg->msg_flags & MSG_OOB) + return -EOPNOTSUPP; + + if (sk->sk_state != BT_CONNECTED) + return -ENOTCONN; + + lock_sock(sk); + err = bt_sock_wait_ready(sk, msg->msg_flags); + release_sock(sk); + if (err) + return err; + + l2cap_chan_lock(chan); + err = l2cap_chan_send(chan, msg, len); + l2cap_chan_unlock(chan); + + return err; +} + +static int l2cap_sock_recvmsg(struct socket *sock, struct msghdr *msg, + size_t len, int flags) +{ + struct sock *sk = sock->sk; + struct l2cap_pinfo *pi = l2cap_pi(sk); + int err; + + lock_sock(sk); + + if (sk->sk_state == BT_CONNECT2 && test_bit(BT_SK_DEFER_SETUP, + &bt_sk(sk)->flags)) { + if (bdaddr_type_is_le(pi->chan->src_type)) { + sk->sk_state = BT_CONNECTED; + pi->chan->state = BT_CONNECTED; + __l2cap_le_connect_rsp_defer(pi->chan); + } else { + sk->sk_state = BT_CONFIG; + pi->chan->state = BT_CONFIG; + __l2cap_connect_rsp_defer(pi->chan); + } + + err = 0; + goto done; + } + + release_sock(sk); + + if (sock->type == SOCK_STREAM) + err = bt_sock_stream_recvmsg(sock, msg, len, flags); + else + err = bt_sock_recvmsg(sock, msg, len, flags); + + if (pi->chan->mode != L2CAP_MODE_ERTM) + return err; + + /* Attempt to put pending rx data in the socket buffer */ + + lock_sock(sk); + + if (!test_bit(CONN_LOCAL_BUSY, &pi->chan->conn_state)) + goto done; + + if (pi->rx_busy_skb) { + if (!sock_queue_rcv_skb(sk, pi->rx_busy_skb)) + pi->rx_busy_skb = NULL; + else + goto done; + } + + /* Restore data flow when half of the receive buffer is + * available. This avoids resending large numbers of + * frames. + */ + if (atomic_read(&sk->sk_rmem_alloc) <= sk->sk_rcvbuf >> 1) + l2cap_chan_busy(pi->chan, 0); + +done: + release_sock(sk); + return err; +} + +/* Kill socket (only if zapped and orphan) + * Must be called on unlocked socket. + */ +static void l2cap_sock_kill(struct sock *sk) +{ + if (!sock_flag(sk, SOCK_ZAPPED) || sk->sk_socket) + return; + + BT_DBG("sk %p state %s", sk, state_to_string(sk->sk_state)); + + /* Kill poor orphan */ + + l2cap_chan_put(l2cap_pi(sk)->chan); + sock_set_flag(sk, SOCK_DEAD); + sock_put(sk); +} + +static int __l2cap_wait_ack(struct sock *sk) +{ + struct l2cap_chan *chan = l2cap_pi(sk)->chan; + DECLARE_WAITQUEUE(wait, current); + int err = 0; + int timeo = HZ/5; + + add_wait_queue(sk_sleep(sk), &wait); + set_current_state(TASK_INTERRUPTIBLE); + while (chan->unacked_frames > 0 && chan->conn) { + if (!timeo) + timeo = HZ/5; + + if (signal_pending(current)) { + err = sock_intr_errno(timeo); + break; + } + + release_sock(sk); + timeo = schedule_timeout(timeo); + lock_sock(sk); + set_current_state(TASK_INTERRUPTIBLE); + + err = sock_error(sk); + if (err) + break; + } + set_current_state(TASK_RUNNING); + remove_wait_queue(sk_sleep(sk), &wait); + return err; +} + +static int l2cap_sock_shutdown(struct socket *sock, int how) +{ + struct sock *sk = sock->sk; + struct l2cap_chan *chan; + struct l2cap_conn *conn; + int err = 0; + + BT_DBG("sock %p, sk %p", sock, sk); + + if (!sk) + return 0; + + chan = l2cap_pi(sk)->chan; + conn = chan->conn; + + BT_DBG("chan %p state %s", chan, state_to_string(chan->state)); + + if (conn) + mutex_lock(&conn->chan_lock); + + l2cap_chan_lock(chan); + lock_sock(sk); + + if (!sk->sk_shutdown) { + if (chan->mode == L2CAP_MODE_ERTM) + err = __l2cap_wait_ack(sk); + + sk->sk_shutdown = SHUTDOWN_MASK; + + release_sock(sk); + l2cap_chan_close(chan, 0); + lock_sock(sk); + + if (sock_flag(sk, SOCK_LINGER) && sk->sk_lingertime && + !(current->flags & PF_EXITING)) + err = bt_sock_wait_state(sk, BT_CLOSED, + sk->sk_lingertime); + } + + if (!err && sk->sk_err) + err = -sk->sk_err; + + release_sock(sk); + l2cap_chan_unlock(chan); + + if (conn) + mutex_unlock(&conn->chan_lock); + + return err; +} + +static int l2cap_sock_release(struct socket *sock) +{ + struct sock *sk = sock->sk; + int err; + + BT_DBG("sock %p, sk %p", sock, sk); + + if (!sk) + return 0; + + bt_sock_unlink(&l2cap_sk_list, sk); + + err = l2cap_sock_shutdown(sock, 2); + + sock_orphan(sk); + l2cap_sock_kill(sk); + return err; +} + +static void l2cap_sock_cleanup_listen(struct sock *parent) +{ + struct sock *sk; + + BT_DBG("parent %p state %s", parent, + state_to_string(parent->sk_state)); + + /* Close not yet accepted channels */ + while ((sk = bt_accept_dequeue(parent, NULL))) { + struct l2cap_chan *chan = l2cap_pi(sk)->chan; + + BT_DBG("child chan %p state %s", chan, + state_to_string(chan->state)); + + l2cap_chan_lock(chan); + __clear_chan_timer(chan); + l2cap_chan_close(chan, ECONNRESET); + l2cap_chan_unlock(chan); + + l2cap_sock_kill(sk); + } +} + +static struct l2cap_chan *l2cap_sock_new_connection_cb(struct l2cap_chan *chan) +{ + struct sock *sk, *parent = chan->data; + + lock_sock(parent); + + /* Check for backlog size */ + if (sk_acceptq_is_full(parent)) { + BT_DBG("backlog full %d", parent->sk_ack_backlog); + release_sock(parent); + return NULL; + } + + sk = l2cap_sock_alloc(sock_net(parent), NULL, BTPROTO_L2CAP, + GFP_ATOMIC); + if (!sk) { + release_sock(parent); + return NULL; + } + + bt_sock_reclassify_lock(sk, BTPROTO_L2CAP); + + l2cap_sock_init(sk, parent); + + bt_accept_enqueue(parent, sk); + + release_sock(parent); + + return l2cap_pi(sk)->chan; +} + +static int l2cap_sock_recv_cb(struct l2cap_chan *chan, struct sk_buff *skb) +{ + struct sock *sk = chan->data; + int err; + + lock_sock(sk); + + if (l2cap_pi(sk)->rx_busy_skb) { + err = -ENOMEM; + goto done; + } + + err = sock_queue_rcv_skb(sk, skb); + + /* For ERTM, handle one skb that doesn't fit into the recv + * buffer. This is important to do because the data frames + * have already been acked, so the skb cannot be discarded. + * + * Notify the l2cap core that the buffer is full, so the + * LOCAL_BUSY state is entered and no more frames are + * acked and reassembled until there is buffer space + * available. + */ + if (err < 0 && chan->mode == L2CAP_MODE_ERTM) { + l2cap_pi(sk)->rx_busy_skb = skb; + l2cap_chan_busy(chan, 1); + err = 0; + } + +done: + release_sock(sk); + + return err; +} + +static void l2cap_sock_close_cb(struct l2cap_chan *chan) +{ + struct sock *sk = chan->data; + + l2cap_sock_kill(sk); +} + +static void l2cap_sock_teardown_cb(struct l2cap_chan *chan, int err) +{ + struct sock *sk = chan->data; + struct sock *parent; + + BT_DBG("chan %p state %s", chan, state_to_string(chan->state)); + + /* This callback can be called both for server (BT_LISTEN) + * sockets as well as "normal" ones. To avoid lockdep warnings + * with child socket locking (through l2cap_sock_cleanup_listen) + * we need separation into separate nesting levels. The simplest + * way to accomplish this is to inherit the nesting level used + * for the channel. + */ + lock_sock_nested(sk, atomic_read(&chan->nesting)); + + parent = bt_sk(sk)->parent; + + sock_set_flag(sk, SOCK_ZAPPED); + + switch (chan->state) { + case BT_OPEN: + case BT_BOUND: + case BT_CLOSED: + break; + case BT_LISTEN: + l2cap_sock_cleanup_listen(sk); + sk->sk_state = BT_CLOSED; + chan->state = BT_CLOSED; + + break; + default: + sk->sk_state = BT_CLOSED; + chan->state = BT_CLOSED; + + sk->sk_err = err; + + if (parent) { + bt_accept_unlink(sk); + parent->sk_data_ready(parent); + } else { + sk->sk_state_change(sk); + } + + break; + } + + release_sock(sk); +} + +static void l2cap_sock_state_change_cb(struct l2cap_chan *chan, int state, + int err) +{ + struct sock *sk = chan->data; + + sk->sk_state = state; + + if (err) + sk->sk_err = err; +} + +static struct sk_buff *l2cap_sock_alloc_skb_cb(struct l2cap_chan *chan, + unsigned long hdr_len, + unsigned long len, int nb) +{ + struct sock *sk = chan->data; + struct sk_buff *skb; + int err; + + l2cap_chan_unlock(chan); + skb = bt_skb_send_alloc(sk, hdr_len + len, nb, &err); + l2cap_chan_lock(chan); + + if (!skb) + return ERR_PTR(err); + + skb->priority = sk->sk_priority; + + bt_cb(skb)->l2cap.chan = chan; + + return skb; +} + +static void l2cap_sock_ready_cb(struct l2cap_chan *chan) +{ + struct sock *sk = chan->data; + struct sock *parent; + + lock_sock(sk); + + parent = bt_sk(sk)->parent; + + BT_DBG("sk %p, parent %p", sk, parent); + + sk->sk_state = BT_CONNECTED; + sk->sk_state_change(sk); + + if (parent) + parent->sk_data_ready(parent); + + release_sock(sk); +} + +static void l2cap_sock_defer_cb(struct l2cap_chan *chan) +{ + struct sock *parent, *sk = chan->data; + + lock_sock(sk); + + parent = bt_sk(sk)->parent; + if (parent) + parent->sk_data_ready(parent); + + release_sock(sk); +} + +static void l2cap_sock_resume_cb(struct l2cap_chan *chan) +{ + struct sock *sk = chan->data; + + if (test_and_clear_bit(FLAG_PENDING_SECURITY, &chan->flags)) { + sk->sk_state = BT_CONNECTED; + chan->state = BT_CONNECTED; + } + + clear_bit(BT_SK_SUSPEND, &bt_sk(sk)->flags); + sk->sk_state_change(sk); +} + +static void l2cap_sock_set_shutdown_cb(struct l2cap_chan *chan) +{ + struct sock *sk = chan->data; + + lock_sock(sk); + sk->sk_shutdown = SHUTDOWN_MASK; + release_sock(sk); +} + +static long l2cap_sock_get_sndtimeo_cb(struct l2cap_chan *chan) +{ + struct sock *sk = chan->data; + + return sk->sk_sndtimeo; +} + +static void l2cap_sock_suspend_cb(struct l2cap_chan *chan) +{ + struct sock *sk = chan->data; + + set_bit(BT_SK_SUSPEND, &bt_sk(sk)->flags); + sk->sk_state_change(sk); +} + +static const struct l2cap_ops l2cap_chan_ops = { + .name = "L2CAP Socket Interface", + .new_connection = l2cap_sock_new_connection_cb, + .recv = l2cap_sock_recv_cb, + .close = l2cap_sock_close_cb, + .teardown = l2cap_sock_teardown_cb, + .state_change = l2cap_sock_state_change_cb, + .ready = l2cap_sock_ready_cb, + .defer = l2cap_sock_defer_cb, + .resume = l2cap_sock_resume_cb, + .suspend = l2cap_sock_suspend_cb, + .set_shutdown = l2cap_sock_set_shutdown_cb, + .get_sndtimeo = l2cap_sock_get_sndtimeo_cb, + .alloc_skb = l2cap_sock_alloc_skb_cb, +}; + +static void l2cap_sock_destruct(struct sock *sk) +{ + BT_DBG("sk %p", sk); + + if (l2cap_pi(sk)->chan) + l2cap_chan_put(l2cap_pi(sk)->chan); + + if (l2cap_pi(sk)->rx_busy_skb) { + kfree_skb(l2cap_pi(sk)->rx_busy_skb); + l2cap_pi(sk)->rx_busy_skb = NULL; + } + + skb_queue_purge(&sk->sk_receive_queue); + skb_queue_purge(&sk->sk_write_queue); +} + +static void l2cap_skb_msg_name(struct sk_buff *skb, void *msg_name, + int *msg_namelen) +{ + DECLARE_SOCKADDR(struct sockaddr_l2 *, la, msg_name); + + memset(la, 0, sizeof(struct sockaddr_l2)); + la->l2_family = AF_BLUETOOTH; + la->l2_psm = bt_cb(skb)->l2cap.psm; + bacpy(&la->l2_bdaddr, &bt_cb(skb)->l2cap.bdaddr); + + *msg_namelen = sizeof(struct sockaddr_l2); +} + +static void l2cap_sock_init(struct sock *sk, struct sock *parent) +{ + struct l2cap_chan *chan = l2cap_pi(sk)->chan; + + BT_DBG("sk %p", sk); + + if (parent) { + struct l2cap_chan *pchan = l2cap_pi(parent)->chan; + + sk->sk_type = parent->sk_type; + bt_sk(sk)->flags = bt_sk(parent)->flags; + + chan->chan_type = pchan->chan_type; + chan->imtu = pchan->imtu; + chan->omtu = pchan->omtu; + chan->conf_state = pchan->conf_state; + chan->mode = pchan->mode; + chan->fcs = pchan->fcs; + chan->max_tx = pchan->max_tx; + chan->tx_win = pchan->tx_win; + chan->tx_win_max = pchan->tx_win_max; + chan->sec_level = pchan->sec_level; + chan->flags = pchan->flags; + chan->tx_credits = pchan->tx_credits; + chan->rx_credits = pchan->rx_credits; + + if (chan->chan_type == L2CAP_CHAN_FIXED) { + chan->scid = pchan->scid; + chan->dcid = pchan->scid; + } + + security_sk_clone(parent, sk); + } else { + switch (sk->sk_type) { + case SOCK_RAW: + chan->chan_type = L2CAP_CHAN_RAW; + break; + case SOCK_DGRAM: + chan->chan_type = L2CAP_CHAN_CONN_LESS; + bt_sk(sk)->skb_msg_name = l2cap_skb_msg_name; + break; + case SOCK_SEQPACKET: + case SOCK_STREAM: + chan->chan_type = L2CAP_CHAN_CONN_ORIENTED; + break; + } + + chan->imtu = L2CAP_DEFAULT_MTU; + chan->omtu = 0; + if (!disable_ertm && sk->sk_type == SOCK_STREAM) { + chan->mode = L2CAP_MODE_ERTM; + set_bit(CONF_STATE2_DEVICE, &chan->conf_state); + } else { + chan->mode = L2CAP_MODE_BASIC; + } + + l2cap_chan_set_defaults(chan); + } + + /* Default config options */ + chan->flush_to = L2CAP_DEFAULT_FLUSH_TO; + + chan->data = sk; + chan->ops = &l2cap_chan_ops; +} + +static struct proto l2cap_proto = { + .name = "L2CAP", + .owner = THIS_MODULE, + .obj_size = sizeof(struct l2cap_pinfo) +}; + +static struct sock *l2cap_sock_alloc(struct net *net, struct socket *sock, + int proto, gfp_t prio) +{ + struct sock *sk; + struct l2cap_chan *chan; + + sk = sk_alloc(net, PF_BLUETOOTH, prio, &l2cap_proto); + if (!sk) + return NULL; + + sock_init_data(sock, sk); + INIT_LIST_HEAD(&bt_sk(sk)->accept_q); + + sk->sk_destruct = l2cap_sock_destruct; + sk->sk_sndtimeo = L2CAP_CONN_TIMEOUT; + + sock_reset_flag(sk, SOCK_ZAPPED); + + sk->sk_protocol = proto; + sk->sk_state = BT_OPEN; + + chan = l2cap_chan_create(); + if (!chan) { + sk_free(sk); + return NULL; + } + + l2cap_chan_hold(chan); + + l2cap_pi(sk)->chan = chan; + + return sk; +} + +static int l2cap_sock_create(struct net *net, struct socket *sock, int protocol, + int kern) +{ + struct sock *sk; + + BT_DBG("sock %p", sock); + + sock->state = SS_UNCONNECTED; + + if (sock->type != SOCK_SEQPACKET && sock->type != SOCK_STREAM && + sock->type != SOCK_DGRAM && sock->type != SOCK_RAW) + return -ESOCKTNOSUPPORT; + + if (sock->type == SOCK_RAW && !kern && !capable(CAP_NET_RAW)) + return -EPERM; + + sock->ops = &l2cap_sock_ops; + + sk = l2cap_sock_alloc(net, sock, protocol, GFP_ATOMIC); + if (!sk) + return -ENOMEM; + + l2cap_sock_init(sk, NULL); + bt_sock_link(&l2cap_sk_list, sk); + return 0; +} + +static const struct proto_ops l2cap_sock_ops = { + .family = PF_BLUETOOTH, + .owner = THIS_MODULE, + .release = l2cap_sock_release, + .bind = l2cap_sock_bind, + .connect = l2cap_sock_connect, + .listen = l2cap_sock_listen, + .accept = l2cap_sock_accept, + .getname = l2cap_sock_getname, + .sendmsg = l2cap_sock_sendmsg, + .recvmsg = l2cap_sock_recvmsg, + .poll = bt_sock_poll, + .ioctl = bt_sock_ioctl, + .mmap = sock_no_mmap, + .socketpair = sock_no_socketpair, + .shutdown = l2cap_sock_shutdown, + .setsockopt = l2cap_sock_setsockopt, + .getsockopt = l2cap_sock_getsockopt +}; + +static const struct net_proto_family l2cap_sock_family_ops = { + .family = PF_BLUETOOTH, + .owner = THIS_MODULE, + .create = l2cap_sock_create, +}; + +int __init l2cap_init_sockets(void) +{ + int err; + + BUILD_BUG_ON(sizeof(struct sockaddr_l2) > sizeof(struct sockaddr)); + + err = proto_register(&l2cap_proto, 0); + if (err < 0) + return err; + + err = bt_sock_register(BTPROTO_L2CAP, &l2cap_sock_family_ops); + if (err < 0) { + BT_ERR("L2CAP socket registration failed"); + goto error; + } + + err = bt_procfs_init(&init_net, "l2cap", &l2cap_sk_list, + NULL); + if (err < 0) { + BT_ERR("Failed to create L2CAP proc file"); + bt_sock_unregister(BTPROTO_L2CAP); + goto error; + } + + BT_INFO("L2CAP socket layer initialized"); + + return 0; + +error: + proto_unregister(&l2cap_proto); + return err; +} + +void l2cap_cleanup_sockets(void) +{ + bt_procfs_cleanup(&init_net, "l2cap"); + bt_sock_unregister(BTPROTO_L2CAP); + proto_unregister(&l2cap_proto); +} diff --git a/kernel/net/bluetooth/lib.c b/kernel/net/bluetooth/lib.c new file mode 100644 index 000000000..b36bc0415 --- /dev/null +++ b/kernel/net/bluetooth/lib.c @@ -0,0 +1,168 @@ +/* + BlueZ - Bluetooth protocol stack for Linux + Copyright (C) 2000-2001 Qualcomm Incorporated + + Written 2000,2001 by Maxim Krasnyansky <maxk@qualcomm.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; + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS + OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT OF THIRD PARTY RIGHTS. + IN NO EVENT SHALL THE COPYRIGHT HOLDER(S) AND AUTHOR(S) BE LIABLE FOR ANY + CLAIM, OR ANY SPECIAL INDIRECT OR CONSEQUENTIAL DAMAGES, OR ANY DAMAGES + WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN + ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF + OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + + ALL LIABILITY, INCLUDING LIABILITY FOR INFRINGEMENT OF ANY PATENTS, + COPYRIGHTS, TRADEMARKS OR OTHER RIGHTS, RELATING TO USE OF THIS + SOFTWARE IS DISCLAIMED. +*/ + +/* Bluetooth kernel library. */ + +#define pr_fmt(fmt) "Bluetooth: " fmt + +#include <linux/export.h> + +#include <net/bluetooth/bluetooth.h> + +void baswap(bdaddr_t *dst, bdaddr_t *src) +{ + unsigned char *d = (unsigned char *) dst; + unsigned char *s = (unsigned char *) src; + unsigned int i; + + for (i = 0; i < 6; i++) + d[i] = s[5 - i]; +} +EXPORT_SYMBOL(baswap); + +/* Bluetooth error codes to Unix errno mapping */ +int bt_to_errno(__u16 code) +{ + switch (code) { + case 0: + return 0; + + case 0x01: + return EBADRQC; + + case 0x02: + return ENOTCONN; + + case 0x03: + return EIO; + + case 0x04: + case 0x3c: + return EHOSTDOWN; + + case 0x05: + return EACCES; + + case 0x06: + return EBADE; + + case 0x07: + return ENOMEM; + + case 0x08: + return ETIMEDOUT; + + case 0x09: + return EMLINK; + + case 0x0a: + return EMLINK; + + case 0x0b: + return EALREADY; + + case 0x0c: + return EBUSY; + + case 0x0d: + case 0x0e: + case 0x0f: + return ECONNREFUSED; + + case 0x10: + return ETIMEDOUT; + + case 0x11: + case 0x27: + case 0x29: + case 0x20: + return EOPNOTSUPP; + + case 0x12: + return EINVAL; + + case 0x13: + case 0x14: + case 0x15: + return ECONNRESET; + + case 0x16: + return ECONNABORTED; + + case 0x17: + return ELOOP; + + case 0x18: + return EACCES; + + case 0x1a: + return EPROTONOSUPPORT; + + case 0x1b: + return ECONNREFUSED; + + case 0x19: + case 0x1e: + case 0x23: + case 0x24: + case 0x25: + return EPROTO; + + default: + return ENOSYS; + } +} +EXPORT_SYMBOL(bt_to_errno); + +void bt_info(const char *format, ...) +{ + struct va_format vaf; + va_list args; + + va_start(args, format); + + vaf.fmt = format; + vaf.va = &args; + + pr_info("%pV", &vaf); + + va_end(args); +} +EXPORT_SYMBOL(bt_info); + +void bt_err(const char *format, ...) +{ + struct va_format vaf; + va_list args; + + va_start(args, format); + + vaf.fmt = format; + vaf.va = &args; + + pr_err("%pV", &vaf); + + va_end(args); +} +EXPORT_SYMBOL(bt_err); diff --git a/kernel/net/bluetooth/mgmt.c b/kernel/net/bluetooth/mgmt.c new file mode 100644 index 000000000..7fd87e713 --- /dev/null +++ b/kernel/net/bluetooth/mgmt.c @@ -0,0 +1,8415 @@ +/* + BlueZ - Bluetooth protocol stack for Linux + + Copyright (C) 2010 Nokia Corporation + Copyright (C) 2011-2012 Intel Corporation + + 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; + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS + OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT OF THIRD PARTY RIGHTS. + IN NO EVENT SHALL THE COPYRIGHT HOLDER(S) AND AUTHOR(S) BE LIABLE FOR ANY + CLAIM, OR ANY SPECIAL INDIRECT OR CONSEQUENTIAL DAMAGES, OR ANY DAMAGES + WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN + ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF + OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + + ALL LIABILITY, INCLUDING LIABILITY FOR INFRINGEMENT OF ANY PATENTS, + COPYRIGHTS, TRADEMARKS OR OTHER RIGHTS, RELATING TO USE OF THIS + SOFTWARE IS DISCLAIMED. +*/ + +/* Bluetooth HCI Management interface */ + +#include <linux/module.h> +#include <asm/unaligned.h> + +#include <net/bluetooth/bluetooth.h> +#include <net/bluetooth/hci_core.h> +#include <net/bluetooth/hci_sock.h> +#include <net/bluetooth/l2cap.h> +#include <net/bluetooth/mgmt.h> + +#include "hci_request.h" +#include "smp.h" +#include "mgmt_util.h" + +#define MGMT_VERSION 1 +#define MGMT_REVISION 9 + +static const u16 mgmt_commands[] = { + MGMT_OP_READ_INDEX_LIST, + MGMT_OP_READ_INFO, + MGMT_OP_SET_POWERED, + MGMT_OP_SET_DISCOVERABLE, + MGMT_OP_SET_CONNECTABLE, + MGMT_OP_SET_FAST_CONNECTABLE, + MGMT_OP_SET_BONDABLE, + MGMT_OP_SET_LINK_SECURITY, + MGMT_OP_SET_SSP, + MGMT_OP_SET_HS, + MGMT_OP_SET_LE, + MGMT_OP_SET_DEV_CLASS, + MGMT_OP_SET_LOCAL_NAME, + MGMT_OP_ADD_UUID, + MGMT_OP_REMOVE_UUID, + MGMT_OP_LOAD_LINK_KEYS, + MGMT_OP_LOAD_LONG_TERM_KEYS, + MGMT_OP_DISCONNECT, + MGMT_OP_GET_CONNECTIONS, + MGMT_OP_PIN_CODE_REPLY, + MGMT_OP_PIN_CODE_NEG_REPLY, + MGMT_OP_SET_IO_CAPABILITY, + MGMT_OP_PAIR_DEVICE, + MGMT_OP_CANCEL_PAIR_DEVICE, + MGMT_OP_UNPAIR_DEVICE, + MGMT_OP_USER_CONFIRM_REPLY, + MGMT_OP_USER_CONFIRM_NEG_REPLY, + MGMT_OP_USER_PASSKEY_REPLY, + MGMT_OP_USER_PASSKEY_NEG_REPLY, + MGMT_OP_READ_LOCAL_OOB_DATA, + MGMT_OP_ADD_REMOTE_OOB_DATA, + MGMT_OP_REMOVE_REMOTE_OOB_DATA, + MGMT_OP_START_DISCOVERY, + MGMT_OP_STOP_DISCOVERY, + MGMT_OP_CONFIRM_NAME, + MGMT_OP_BLOCK_DEVICE, + MGMT_OP_UNBLOCK_DEVICE, + MGMT_OP_SET_DEVICE_ID, + MGMT_OP_SET_ADVERTISING, + MGMT_OP_SET_BREDR, + MGMT_OP_SET_STATIC_ADDRESS, + MGMT_OP_SET_SCAN_PARAMS, + MGMT_OP_SET_SECURE_CONN, + MGMT_OP_SET_DEBUG_KEYS, + MGMT_OP_SET_PRIVACY, + MGMT_OP_LOAD_IRKS, + MGMT_OP_GET_CONN_INFO, + MGMT_OP_GET_CLOCK_INFO, + MGMT_OP_ADD_DEVICE, + MGMT_OP_REMOVE_DEVICE, + MGMT_OP_LOAD_CONN_PARAM, + MGMT_OP_READ_UNCONF_INDEX_LIST, + MGMT_OP_READ_CONFIG_INFO, + MGMT_OP_SET_EXTERNAL_CONFIG, + MGMT_OP_SET_PUBLIC_ADDRESS, + MGMT_OP_START_SERVICE_DISCOVERY, + MGMT_OP_READ_LOCAL_OOB_EXT_DATA, + MGMT_OP_READ_EXT_INDEX_LIST, + MGMT_OP_READ_ADV_FEATURES, + MGMT_OP_ADD_ADVERTISING, + MGMT_OP_REMOVE_ADVERTISING, +}; + +static const u16 mgmt_events[] = { + MGMT_EV_CONTROLLER_ERROR, + MGMT_EV_INDEX_ADDED, + MGMT_EV_INDEX_REMOVED, + MGMT_EV_NEW_SETTINGS, + MGMT_EV_CLASS_OF_DEV_CHANGED, + MGMT_EV_LOCAL_NAME_CHANGED, + MGMT_EV_NEW_LINK_KEY, + MGMT_EV_NEW_LONG_TERM_KEY, + MGMT_EV_DEVICE_CONNECTED, + MGMT_EV_DEVICE_DISCONNECTED, + MGMT_EV_CONNECT_FAILED, + MGMT_EV_PIN_CODE_REQUEST, + MGMT_EV_USER_CONFIRM_REQUEST, + MGMT_EV_USER_PASSKEY_REQUEST, + MGMT_EV_AUTH_FAILED, + MGMT_EV_DEVICE_FOUND, + MGMT_EV_DISCOVERING, + MGMT_EV_DEVICE_BLOCKED, + MGMT_EV_DEVICE_UNBLOCKED, + MGMT_EV_DEVICE_UNPAIRED, + MGMT_EV_PASSKEY_NOTIFY, + MGMT_EV_NEW_IRK, + MGMT_EV_NEW_CSRK, + MGMT_EV_DEVICE_ADDED, + MGMT_EV_DEVICE_REMOVED, + MGMT_EV_NEW_CONN_PARAM, + MGMT_EV_UNCONF_INDEX_ADDED, + MGMT_EV_UNCONF_INDEX_REMOVED, + MGMT_EV_NEW_CONFIG_OPTIONS, + MGMT_EV_EXT_INDEX_ADDED, + MGMT_EV_EXT_INDEX_REMOVED, + MGMT_EV_LOCAL_OOB_DATA_UPDATED, + MGMT_EV_ADVERTISING_ADDED, + MGMT_EV_ADVERTISING_REMOVED, +}; + +static const u16 mgmt_untrusted_commands[] = { + MGMT_OP_READ_INDEX_LIST, + MGMT_OP_READ_INFO, + MGMT_OP_READ_UNCONF_INDEX_LIST, + MGMT_OP_READ_CONFIG_INFO, + MGMT_OP_READ_EXT_INDEX_LIST, +}; + +static const u16 mgmt_untrusted_events[] = { + MGMT_EV_INDEX_ADDED, + MGMT_EV_INDEX_REMOVED, + MGMT_EV_NEW_SETTINGS, + MGMT_EV_CLASS_OF_DEV_CHANGED, + MGMT_EV_LOCAL_NAME_CHANGED, + MGMT_EV_UNCONF_INDEX_ADDED, + MGMT_EV_UNCONF_INDEX_REMOVED, + MGMT_EV_NEW_CONFIG_OPTIONS, + MGMT_EV_EXT_INDEX_ADDED, + MGMT_EV_EXT_INDEX_REMOVED, +}; + +#define CACHE_TIMEOUT msecs_to_jiffies(2 * 1000) + +#define ZERO_KEY "\x00\x00\x00\x00\x00\x00\x00\x00" \ + "\x00\x00\x00\x00\x00\x00\x00\x00" + +/* HCI to MGMT error code conversion table */ +static u8 mgmt_status_table[] = { + MGMT_STATUS_SUCCESS, + MGMT_STATUS_UNKNOWN_COMMAND, /* Unknown Command */ + MGMT_STATUS_NOT_CONNECTED, /* No Connection */ + MGMT_STATUS_FAILED, /* Hardware Failure */ + MGMT_STATUS_CONNECT_FAILED, /* Page Timeout */ + MGMT_STATUS_AUTH_FAILED, /* Authentication Failed */ + MGMT_STATUS_AUTH_FAILED, /* PIN or Key Missing */ + MGMT_STATUS_NO_RESOURCES, /* Memory Full */ + MGMT_STATUS_TIMEOUT, /* Connection Timeout */ + MGMT_STATUS_NO_RESOURCES, /* Max Number of Connections */ + MGMT_STATUS_NO_RESOURCES, /* Max Number of SCO Connections */ + MGMT_STATUS_ALREADY_CONNECTED, /* ACL Connection Exists */ + MGMT_STATUS_BUSY, /* Command Disallowed */ + MGMT_STATUS_NO_RESOURCES, /* Rejected Limited Resources */ + MGMT_STATUS_REJECTED, /* Rejected Security */ + MGMT_STATUS_REJECTED, /* Rejected Personal */ + MGMT_STATUS_TIMEOUT, /* Host Timeout */ + MGMT_STATUS_NOT_SUPPORTED, /* Unsupported Feature */ + MGMT_STATUS_INVALID_PARAMS, /* Invalid Parameters */ + MGMT_STATUS_DISCONNECTED, /* OE User Ended Connection */ + MGMT_STATUS_NO_RESOURCES, /* OE Low Resources */ + MGMT_STATUS_DISCONNECTED, /* OE Power Off */ + MGMT_STATUS_DISCONNECTED, /* Connection Terminated */ + MGMT_STATUS_BUSY, /* Repeated Attempts */ + MGMT_STATUS_REJECTED, /* Pairing Not Allowed */ + MGMT_STATUS_FAILED, /* Unknown LMP PDU */ + MGMT_STATUS_NOT_SUPPORTED, /* Unsupported Remote Feature */ + MGMT_STATUS_REJECTED, /* SCO Offset Rejected */ + MGMT_STATUS_REJECTED, /* SCO Interval Rejected */ + MGMT_STATUS_REJECTED, /* Air Mode Rejected */ + MGMT_STATUS_INVALID_PARAMS, /* Invalid LMP Parameters */ + MGMT_STATUS_FAILED, /* Unspecified Error */ + MGMT_STATUS_NOT_SUPPORTED, /* Unsupported LMP Parameter Value */ + MGMT_STATUS_FAILED, /* Role Change Not Allowed */ + MGMT_STATUS_TIMEOUT, /* LMP Response Timeout */ + MGMT_STATUS_FAILED, /* LMP Error Transaction Collision */ + MGMT_STATUS_FAILED, /* LMP PDU Not Allowed */ + MGMT_STATUS_REJECTED, /* Encryption Mode Not Accepted */ + MGMT_STATUS_FAILED, /* Unit Link Key Used */ + MGMT_STATUS_NOT_SUPPORTED, /* QoS Not Supported */ + MGMT_STATUS_TIMEOUT, /* Instant Passed */ + MGMT_STATUS_NOT_SUPPORTED, /* Pairing Not Supported */ + MGMT_STATUS_FAILED, /* Transaction Collision */ + MGMT_STATUS_INVALID_PARAMS, /* Unacceptable Parameter */ + MGMT_STATUS_REJECTED, /* QoS Rejected */ + MGMT_STATUS_NOT_SUPPORTED, /* Classification Not Supported */ + MGMT_STATUS_REJECTED, /* Insufficient Security */ + MGMT_STATUS_INVALID_PARAMS, /* Parameter Out Of Range */ + MGMT_STATUS_BUSY, /* Role Switch Pending */ + MGMT_STATUS_FAILED, /* Slot Violation */ + MGMT_STATUS_FAILED, /* Role Switch Failed */ + MGMT_STATUS_INVALID_PARAMS, /* EIR Too Large */ + MGMT_STATUS_NOT_SUPPORTED, /* Simple Pairing Not Supported */ + MGMT_STATUS_BUSY, /* Host Busy Pairing */ + MGMT_STATUS_REJECTED, /* Rejected, No Suitable Channel */ + MGMT_STATUS_BUSY, /* Controller Busy */ + MGMT_STATUS_INVALID_PARAMS, /* Unsuitable Connection Interval */ + MGMT_STATUS_TIMEOUT, /* Directed Advertising Timeout */ + MGMT_STATUS_AUTH_FAILED, /* Terminated Due to MIC Failure */ + MGMT_STATUS_CONNECT_FAILED, /* Connection Establishment Failed */ + MGMT_STATUS_CONNECT_FAILED, /* MAC Connection Failed */ +}; + +static u8 mgmt_status(u8 hci_status) +{ + if (hci_status < ARRAY_SIZE(mgmt_status_table)) + return mgmt_status_table[hci_status]; + + return MGMT_STATUS_FAILED; +} + +static int mgmt_index_event(u16 event, struct hci_dev *hdev, void *data, + u16 len, int flag) +{ + return mgmt_send_event(event, hdev, HCI_CHANNEL_CONTROL, data, len, + flag, NULL); +} + +static int mgmt_limited_event(u16 event, struct hci_dev *hdev, void *data, + u16 len, int flag, struct sock *skip_sk) +{ + return mgmt_send_event(event, hdev, HCI_CHANNEL_CONTROL, data, len, + flag, skip_sk); +} + +static int mgmt_generic_event(u16 event, struct hci_dev *hdev, void *data, + u16 len, struct sock *skip_sk) +{ + return mgmt_send_event(event, hdev, HCI_CHANNEL_CONTROL, data, len, + HCI_MGMT_GENERIC_EVENTS, skip_sk); +} + +static int mgmt_event(u16 event, struct hci_dev *hdev, void *data, u16 len, + struct sock *skip_sk) +{ + return mgmt_send_event(event, hdev, HCI_CHANNEL_CONTROL, data, len, + HCI_SOCK_TRUSTED, skip_sk); +} + +static int read_version(struct sock *sk, struct hci_dev *hdev, void *data, + u16 data_len) +{ + struct mgmt_rp_read_version rp; + + BT_DBG("sock %p", sk); + + rp.version = MGMT_VERSION; + rp.revision = cpu_to_le16(MGMT_REVISION); + + return mgmt_cmd_complete(sk, MGMT_INDEX_NONE, MGMT_OP_READ_VERSION, 0, + &rp, sizeof(rp)); +} + +static int read_commands(struct sock *sk, struct hci_dev *hdev, void *data, + u16 data_len) +{ + struct mgmt_rp_read_commands *rp; + u16 num_commands, num_events; + size_t rp_size; + int i, err; + + BT_DBG("sock %p", sk); + + if (hci_sock_test_flag(sk, HCI_SOCK_TRUSTED)) { + num_commands = ARRAY_SIZE(mgmt_commands); + num_events = ARRAY_SIZE(mgmt_events); + } else { + num_commands = ARRAY_SIZE(mgmt_untrusted_commands); + num_events = ARRAY_SIZE(mgmt_untrusted_events); + } + + rp_size = sizeof(*rp) + ((num_commands + num_events) * sizeof(u16)); + + rp = kmalloc(rp_size, GFP_KERNEL); + if (!rp) + return -ENOMEM; + + rp->num_commands = cpu_to_le16(num_commands); + rp->num_events = cpu_to_le16(num_events); + + if (hci_sock_test_flag(sk, HCI_SOCK_TRUSTED)) { + __le16 *opcode = rp->opcodes; + + for (i = 0; i < num_commands; i++, opcode++) + put_unaligned_le16(mgmt_commands[i], opcode); + + for (i = 0; i < num_events; i++, opcode++) + put_unaligned_le16(mgmt_events[i], opcode); + } else { + __le16 *opcode = rp->opcodes; + + for (i = 0; i < num_commands; i++, opcode++) + put_unaligned_le16(mgmt_untrusted_commands[i], opcode); + + for (i = 0; i < num_events; i++, opcode++) + put_unaligned_le16(mgmt_untrusted_events[i], opcode); + } + + err = mgmt_cmd_complete(sk, MGMT_INDEX_NONE, MGMT_OP_READ_COMMANDS, 0, + rp, rp_size); + kfree(rp); + + return err; +} + +static int read_index_list(struct sock *sk, struct hci_dev *hdev, void *data, + u16 data_len) +{ + struct mgmt_rp_read_index_list *rp; + struct hci_dev *d; + size_t rp_len; + u16 count; + int err; + + BT_DBG("sock %p", sk); + + read_lock(&hci_dev_list_lock); + + count = 0; + list_for_each_entry(d, &hci_dev_list, list) { + if (d->dev_type == HCI_BREDR && + !hci_dev_test_flag(d, HCI_UNCONFIGURED)) + count++; + } + + rp_len = sizeof(*rp) + (2 * count); + rp = kmalloc(rp_len, GFP_ATOMIC); + if (!rp) { + read_unlock(&hci_dev_list_lock); + return -ENOMEM; + } + + count = 0; + list_for_each_entry(d, &hci_dev_list, list) { + if (hci_dev_test_flag(d, HCI_SETUP) || + hci_dev_test_flag(d, HCI_CONFIG) || + hci_dev_test_flag(d, HCI_USER_CHANNEL)) + continue; + + /* Devices marked as raw-only are neither configured + * nor unconfigured controllers. + */ + if (test_bit(HCI_QUIRK_RAW_DEVICE, &d->quirks)) + continue; + + if (d->dev_type == HCI_BREDR && + !hci_dev_test_flag(d, HCI_UNCONFIGURED)) { + rp->index[count++] = cpu_to_le16(d->id); + BT_DBG("Added hci%u", d->id); + } + } + + rp->num_controllers = cpu_to_le16(count); + rp_len = sizeof(*rp) + (2 * count); + + read_unlock(&hci_dev_list_lock); + + err = mgmt_cmd_complete(sk, MGMT_INDEX_NONE, MGMT_OP_READ_INDEX_LIST, + 0, rp, rp_len); + + kfree(rp); + + return err; +} + +static int read_unconf_index_list(struct sock *sk, struct hci_dev *hdev, + void *data, u16 data_len) +{ + struct mgmt_rp_read_unconf_index_list *rp; + struct hci_dev *d; + size_t rp_len; + u16 count; + int err; + + BT_DBG("sock %p", sk); + + read_lock(&hci_dev_list_lock); + + count = 0; + list_for_each_entry(d, &hci_dev_list, list) { + if (d->dev_type == HCI_BREDR && + hci_dev_test_flag(d, HCI_UNCONFIGURED)) + count++; + } + + rp_len = sizeof(*rp) + (2 * count); + rp = kmalloc(rp_len, GFP_ATOMIC); + if (!rp) { + read_unlock(&hci_dev_list_lock); + return -ENOMEM; + } + + count = 0; + list_for_each_entry(d, &hci_dev_list, list) { + if (hci_dev_test_flag(d, HCI_SETUP) || + hci_dev_test_flag(d, HCI_CONFIG) || + hci_dev_test_flag(d, HCI_USER_CHANNEL)) + continue; + + /* Devices marked as raw-only are neither configured + * nor unconfigured controllers. + */ + if (test_bit(HCI_QUIRK_RAW_DEVICE, &d->quirks)) + continue; + + if (d->dev_type == HCI_BREDR && + hci_dev_test_flag(d, HCI_UNCONFIGURED)) { + rp->index[count++] = cpu_to_le16(d->id); + BT_DBG("Added hci%u", d->id); + } + } + + rp->num_controllers = cpu_to_le16(count); + rp_len = sizeof(*rp) + (2 * count); + + read_unlock(&hci_dev_list_lock); + + err = mgmt_cmd_complete(sk, MGMT_INDEX_NONE, + MGMT_OP_READ_UNCONF_INDEX_LIST, 0, rp, rp_len); + + kfree(rp); + + return err; +} + +static int read_ext_index_list(struct sock *sk, struct hci_dev *hdev, + void *data, u16 data_len) +{ + struct mgmt_rp_read_ext_index_list *rp; + struct hci_dev *d; + size_t rp_len; + u16 count; + int err; + + BT_DBG("sock %p", sk); + + read_lock(&hci_dev_list_lock); + + count = 0; + list_for_each_entry(d, &hci_dev_list, list) { + if (d->dev_type == HCI_BREDR || d->dev_type == HCI_AMP) + count++; + } + + rp_len = sizeof(*rp) + (sizeof(rp->entry[0]) * count); + rp = kmalloc(rp_len, GFP_ATOMIC); + if (!rp) { + read_unlock(&hci_dev_list_lock); + return -ENOMEM; + } + + count = 0; + list_for_each_entry(d, &hci_dev_list, list) { + if (hci_dev_test_flag(d, HCI_SETUP) || + hci_dev_test_flag(d, HCI_CONFIG) || + hci_dev_test_flag(d, HCI_USER_CHANNEL)) + continue; + + /* Devices marked as raw-only are neither configured + * nor unconfigured controllers. + */ + if (test_bit(HCI_QUIRK_RAW_DEVICE, &d->quirks)) + continue; + + if (d->dev_type == HCI_BREDR) { + if (hci_dev_test_flag(d, HCI_UNCONFIGURED)) + rp->entry[count].type = 0x01; + else + rp->entry[count].type = 0x00; + } else if (d->dev_type == HCI_AMP) { + rp->entry[count].type = 0x02; + } else { + continue; + } + + rp->entry[count].bus = d->bus; + rp->entry[count++].index = cpu_to_le16(d->id); + BT_DBG("Added hci%u", d->id); + } + + rp->num_controllers = cpu_to_le16(count); + rp_len = sizeof(*rp) + (sizeof(rp->entry[0]) * count); + + read_unlock(&hci_dev_list_lock); + + /* If this command is called at least once, then all the + * default index and unconfigured index events are disabled + * and from now on only extended index events are used. + */ + hci_sock_set_flag(sk, HCI_MGMT_EXT_INDEX_EVENTS); + hci_sock_clear_flag(sk, HCI_MGMT_INDEX_EVENTS); + hci_sock_clear_flag(sk, HCI_MGMT_UNCONF_INDEX_EVENTS); + + err = mgmt_cmd_complete(sk, MGMT_INDEX_NONE, + MGMT_OP_READ_EXT_INDEX_LIST, 0, rp, rp_len); + + kfree(rp); + + return err; +} + +static bool is_configured(struct hci_dev *hdev) +{ + if (test_bit(HCI_QUIRK_EXTERNAL_CONFIG, &hdev->quirks) && + !hci_dev_test_flag(hdev, HCI_EXT_CONFIGURED)) + return false; + + if (test_bit(HCI_QUIRK_INVALID_BDADDR, &hdev->quirks) && + !bacmp(&hdev->public_addr, BDADDR_ANY)) + return false; + + return true; +} + +static __le32 get_missing_options(struct hci_dev *hdev) +{ + u32 options = 0; + + if (test_bit(HCI_QUIRK_EXTERNAL_CONFIG, &hdev->quirks) && + !hci_dev_test_flag(hdev, HCI_EXT_CONFIGURED)) + options |= MGMT_OPTION_EXTERNAL_CONFIG; + + if (test_bit(HCI_QUIRK_INVALID_BDADDR, &hdev->quirks) && + !bacmp(&hdev->public_addr, BDADDR_ANY)) + options |= MGMT_OPTION_PUBLIC_ADDRESS; + + return cpu_to_le32(options); +} + +static int new_options(struct hci_dev *hdev, struct sock *skip) +{ + __le32 options = get_missing_options(hdev); + + return mgmt_generic_event(MGMT_EV_NEW_CONFIG_OPTIONS, hdev, &options, + sizeof(options), skip); +} + +static int send_options_rsp(struct sock *sk, u16 opcode, struct hci_dev *hdev) +{ + __le32 options = get_missing_options(hdev); + + return mgmt_cmd_complete(sk, hdev->id, opcode, 0, &options, + sizeof(options)); +} + +static int read_config_info(struct sock *sk, struct hci_dev *hdev, + void *data, u16 data_len) +{ + struct mgmt_rp_read_config_info rp; + u32 options = 0; + + BT_DBG("sock %p %s", sk, hdev->name); + + hci_dev_lock(hdev); + + memset(&rp, 0, sizeof(rp)); + rp.manufacturer = cpu_to_le16(hdev->manufacturer); + + if (test_bit(HCI_QUIRK_EXTERNAL_CONFIG, &hdev->quirks)) + options |= MGMT_OPTION_EXTERNAL_CONFIG; + + if (hdev->set_bdaddr) + options |= MGMT_OPTION_PUBLIC_ADDRESS; + + rp.supported_options = cpu_to_le32(options); + rp.missing_options = get_missing_options(hdev); + + hci_dev_unlock(hdev); + + return mgmt_cmd_complete(sk, hdev->id, MGMT_OP_READ_CONFIG_INFO, 0, + &rp, sizeof(rp)); +} + +static u32 get_supported_settings(struct hci_dev *hdev) +{ + u32 settings = 0; + + settings |= MGMT_SETTING_POWERED; + settings |= MGMT_SETTING_BONDABLE; + settings |= MGMT_SETTING_DEBUG_KEYS; + settings |= MGMT_SETTING_CONNECTABLE; + settings |= MGMT_SETTING_DISCOVERABLE; + + if (lmp_bredr_capable(hdev)) { + if (hdev->hci_ver >= BLUETOOTH_VER_1_2) + settings |= MGMT_SETTING_FAST_CONNECTABLE; + settings |= MGMT_SETTING_BREDR; + settings |= MGMT_SETTING_LINK_SECURITY; + + if (lmp_ssp_capable(hdev)) { + settings |= MGMT_SETTING_SSP; + settings |= MGMT_SETTING_HS; + } + + if (lmp_sc_capable(hdev)) + settings |= MGMT_SETTING_SECURE_CONN; + } + + if (lmp_le_capable(hdev)) { + settings |= MGMT_SETTING_LE; + settings |= MGMT_SETTING_ADVERTISING; + settings |= MGMT_SETTING_SECURE_CONN; + settings |= MGMT_SETTING_PRIVACY; + settings |= MGMT_SETTING_STATIC_ADDRESS; + } + + if (test_bit(HCI_QUIRK_EXTERNAL_CONFIG, &hdev->quirks) || + hdev->set_bdaddr) + settings |= MGMT_SETTING_CONFIGURATION; + + return settings; +} + +static u32 get_current_settings(struct hci_dev *hdev) +{ + u32 settings = 0; + + if (hdev_is_powered(hdev)) + settings |= MGMT_SETTING_POWERED; + + if (hci_dev_test_flag(hdev, HCI_CONNECTABLE)) + settings |= MGMT_SETTING_CONNECTABLE; + + if (hci_dev_test_flag(hdev, HCI_FAST_CONNECTABLE)) + settings |= MGMT_SETTING_FAST_CONNECTABLE; + + if (hci_dev_test_flag(hdev, HCI_DISCOVERABLE)) + settings |= MGMT_SETTING_DISCOVERABLE; + + if (hci_dev_test_flag(hdev, HCI_BONDABLE)) + settings |= MGMT_SETTING_BONDABLE; + + if (hci_dev_test_flag(hdev, HCI_BREDR_ENABLED)) + settings |= MGMT_SETTING_BREDR; + + if (hci_dev_test_flag(hdev, HCI_LE_ENABLED)) + settings |= MGMT_SETTING_LE; + + if (hci_dev_test_flag(hdev, HCI_LINK_SECURITY)) + settings |= MGMT_SETTING_LINK_SECURITY; + + if (hci_dev_test_flag(hdev, HCI_SSP_ENABLED)) + settings |= MGMT_SETTING_SSP; + + if (hci_dev_test_flag(hdev, HCI_HS_ENABLED)) + settings |= MGMT_SETTING_HS; + + if (hci_dev_test_flag(hdev, HCI_ADVERTISING)) + settings |= MGMT_SETTING_ADVERTISING; + + if (hci_dev_test_flag(hdev, HCI_SC_ENABLED)) + settings |= MGMT_SETTING_SECURE_CONN; + + if (hci_dev_test_flag(hdev, HCI_KEEP_DEBUG_KEYS)) + settings |= MGMT_SETTING_DEBUG_KEYS; + + if (hci_dev_test_flag(hdev, HCI_PRIVACY)) + settings |= MGMT_SETTING_PRIVACY; + + /* The current setting for static address has two purposes. The + * first is to indicate if the static address will be used and + * the second is to indicate if it is actually set. + * + * This means if the static address is not configured, this flag + * will never be set. If the address is configured, then if the + * address is actually used decides if the flag is set or not. + * + * For single mode LE only controllers and dual-mode controllers + * with BR/EDR disabled, the existence of the static address will + * be evaluated. + */ + if (hci_dev_test_flag(hdev, HCI_FORCE_STATIC_ADDR) || + !hci_dev_test_flag(hdev, HCI_BREDR_ENABLED) || + !bacmp(&hdev->bdaddr, BDADDR_ANY)) { + if (bacmp(&hdev->static_addr, BDADDR_ANY)) + settings |= MGMT_SETTING_STATIC_ADDRESS; + } + + return settings; +} + +#define PNP_INFO_SVCLASS_ID 0x1200 + +static u8 *create_uuid16_list(struct hci_dev *hdev, u8 *data, ptrdiff_t len) +{ + u8 *ptr = data, *uuids_start = NULL; + struct bt_uuid *uuid; + + if (len < 4) + return ptr; + + list_for_each_entry(uuid, &hdev->uuids, list) { + u16 uuid16; + + if (uuid->size != 16) + continue; + + uuid16 = get_unaligned_le16(&uuid->uuid[12]); + if (uuid16 < 0x1100) + continue; + + if (uuid16 == PNP_INFO_SVCLASS_ID) + continue; + + if (!uuids_start) { + uuids_start = ptr; + uuids_start[0] = 1; + uuids_start[1] = EIR_UUID16_ALL; + ptr += 2; + } + + /* Stop if not enough space to put next UUID */ + if ((ptr - data) + sizeof(u16) > len) { + uuids_start[1] = EIR_UUID16_SOME; + break; + } + + *ptr++ = (uuid16 & 0x00ff); + *ptr++ = (uuid16 & 0xff00) >> 8; + uuids_start[0] += sizeof(uuid16); + } + + return ptr; +} + +static u8 *create_uuid32_list(struct hci_dev *hdev, u8 *data, ptrdiff_t len) +{ + u8 *ptr = data, *uuids_start = NULL; + struct bt_uuid *uuid; + + if (len < 6) + return ptr; + + list_for_each_entry(uuid, &hdev->uuids, list) { + if (uuid->size != 32) + continue; + + if (!uuids_start) { + uuids_start = ptr; + uuids_start[0] = 1; + uuids_start[1] = EIR_UUID32_ALL; + ptr += 2; + } + + /* Stop if not enough space to put next UUID */ + if ((ptr - data) + sizeof(u32) > len) { + uuids_start[1] = EIR_UUID32_SOME; + break; + } + + memcpy(ptr, &uuid->uuid[12], sizeof(u32)); + ptr += sizeof(u32); + uuids_start[0] += sizeof(u32); + } + + return ptr; +} + +static u8 *create_uuid128_list(struct hci_dev *hdev, u8 *data, ptrdiff_t len) +{ + u8 *ptr = data, *uuids_start = NULL; + struct bt_uuid *uuid; + + if (len < 18) + return ptr; + + list_for_each_entry(uuid, &hdev->uuids, list) { + if (uuid->size != 128) + continue; + + if (!uuids_start) { + uuids_start = ptr; + uuids_start[0] = 1; + uuids_start[1] = EIR_UUID128_ALL; + ptr += 2; + } + + /* Stop if not enough space to put next UUID */ + if ((ptr - data) + 16 > len) { + uuids_start[1] = EIR_UUID128_SOME; + break; + } + + memcpy(ptr, uuid->uuid, 16); + ptr += 16; + uuids_start[0] += 16; + } + + return ptr; +} + +static struct mgmt_pending_cmd *pending_find(u16 opcode, struct hci_dev *hdev) +{ + return mgmt_pending_find(HCI_CHANNEL_CONTROL, opcode, hdev); +} + +static struct mgmt_pending_cmd *pending_find_data(u16 opcode, + struct hci_dev *hdev, + const void *data) +{ + return mgmt_pending_find_data(HCI_CHANNEL_CONTROL, opcode, hdev, data); +} + +static u8 create_default_scan_rsp_data(struct hci_dev *hdev, u8 *ptr) +{ + u8 ad_len = 0; + size_t name_len; + + name_len = strlen(hdev->dev_name); + if (name_len > 0) { + size_t max_len = HCI_MAX_AD_LENGTH - ad_len - 2; + + if (name_len > max_len) { + name_len = max_len; + ptr[1] = EIR_NAME_SHORT; + } else + ptr[1] = EIR_NAME_COMPLETE; + + ptr[0] = name_len + 1; + + memcpy(ptr + 2, hdev->dev_name, name_len); + + ad_len += (name_len + 2); + ptr += (name_len + 2); + } + + return ad_len; +} + +static u8 create_instance_scan_rsp_data(struct hci_dev *hdev, u8 *ptr) +{ + /* TODO: Set the appropriate entries based on advertising instance flags + * here once flags other than 0 are supported. + */ + memcpy(ptr, hdev->adv_instance.scan_rsp_data, + hdev->adv_instance.scan_rsp_len); + + return hdev->adv_instance.scan_rsp_len; +} + +static void update_scan_rsp_data_for_instance(struct hci_request *req, + u8 instance) +{ + struct hci_dev *hdev = req->hdev; + struct hci_cp_le_set_scan_rsp_data cp; + u8 len; + + if (!hci_dev_test_flag(hdev, HCI_LE_ENABLED)) + return; + + memset(&cp, 0, sizeof(cp)); + + if (instance) + len = create_instance_scan_rsp_data(hdev, cp.data); + else + len = create_default_scan_rsp_data(hdev, cp.data); + + if (hdev->scan_rsp_data_len == len && + !memcmp(cp.data, hdev->scan_rsp_data, len)) + return; + + memcpy(hdev->scan_rsp_data, cp.data, sizeof(cp.data)); + hdev->scan_rsp_data_len = len; + + cp.length = len; + + hci_req_add(req, HCI_OP_LE_SET_SCAN_RSP_DATA, sizeof(cp), &cp); +} + +static void update_scan_rsp_data(struct hci_request *req) +{ + struct hci_dev *hdev = req->hdev; + u8 instance; + + /* The "Set Advertising" setting supersedes the "Add Advertising" + * setting. Here we set the scan response data based on which + * setting was set. When neither apply, default to the global settings, + * represented by instance "0". + */ + if (hci_dev_test_flag(hdev, HCI_ADVERTISING_INSTANCE) && + !hci_dev_test_flag(hdev, HCI_ADVERTISING)) + instance = 0x01; + else + instance = 0x00; + + update_scan_rsp_data_for_instance(req, instance); +} + +static u8 get_adv_discov_flags(struct hci_dev *hdev) +{ + struct mgmt_pending_cmd *cmd; + + /* If there's a pending mgmt command the flags will not yet have + * their final values, so check for this first. + */ + cmd = pending_find(MGMT_OP_SET_DISCOVERABLE, hdev); + if (cmd) { + struct mgmt_mode *cp = cmd->param; + if (cp->val == 0x01) + return LE_AD_GENERAL; + else if (cp->val == 0x02) + return LE_AD_LIMITED; + } else { + if (hci_dev_test_flag(hdev, HCI_LIMITED_DISCOVERABLE)) + return LE_AD_LIMITED; + else if (hci_dev_test_flag(hdev, HCI_DISCOVERABLE)) + return LE_AD_GENERAL; + } + + return 0; +} + +static u8 get_current_adv_instance(struct hci_dev *hdev) +{ + /* The "Set Advertising" setting supersedes the "Add Advertising" + * setting. Here we set the advertising data based on which + * setting was set. When neither apply, default to the global settings, + * represented by instance "0". + */ + if (hci_dev_test_flag(hdev, HCI_ADVERTISING_INSTANCE) && + !hci_dev_test_flag(hdev, HCI_ADVERTISING)) + return 0x01; + + return 0x00; +} + +static bool get_connectable(struct hci_dev *hdev) +{ + struct mgmt_pending_cmd *cmd; + + /* If there's a pending mgmt command the flag will not yet have + * it's final value, so check for this first. + */ + cmd = pending_find(MGMT_OP_SET_CONNECTABLE, hdev); + if (cmd) { + struct mgmt_mode *cp = cmd->param; + + return cp->val; + } + + return hci_dev_test_flag(hdev, HCI_CONNECTABLE); +} + +static u32 get_adv_instance_flags(struct hci_dev *hdev, u8 instance) +{ + u32 flags; + + if (instance > 0x01) + return 0; + + if (instance == 0x01) + return hdev->adv_instance.flags; + + /* Instance 0 always manages the "Tx Power" and "Flags" fields */ + flags = MGMT_ADV_FLAG_TX_POWER | MGMT_ADV_FLAG_MANAGED_FLAGS; + + /* For instance 0, the HCI_ADVERTISING_CONNECTABLE setting corresponds + * to the "connectable" instance flag. + */ + if (hci_dev_test_flag(hdev, HCI_ADVERTISING_CONNECTABLE)) + flags |= MGMT_ADV_FLAG_CONNECTABLE; + + return flags; +} + +static u8 get_adv_instance_scan_rsp_len(struct hci_dev *hdev, u8 instance) +{ + /* Ignore instance 0 and other unsupported instances */ + if (instance != 0x01) + return 0; + + /* TODO: Take into account the "appearance" and "local-name" flags here. + * These are currently being ignored as they are not supported. + */ + return hdev->adv_instance.scan_rsp_len; +} + +static u8 create_instance_adv_data(struct hci_dev *hdev, u8 instance, u8 *ptr) +{ + u8 ad_len = 0, flags = 0; + u32 instance_flags = get_adv_instance_flags(hdev, instance); + + /* The Add Advertising command allows userspace to set both the general + * and limited discoverable flags. + */ + if (instance_flags & MGMT_ADV_FLAG_DISCOV) + flags |= LE_AD_GENERAL; + + if (instance_flags & MGMT_ADV_FLAG_LIMITED_DISCOV) + flags |= LE_AD_LIMITED; + + if (flags || (instance_flags & MGMT_ADV_FLAG_MANAGED_FLAGS)) { + /* If a discovery flag wasn't provided, simply use the global + * settings. + */ + if (!flags) + flags |= get_adv_discov_flags(hdev); + + if (!hci_dev_test_flag(hdev, HCI_BREDR_ENABLED)) + flags |= LE_AD_NO_BREDR; + + /* If flags would still be empty, then there is no need to + * include the "Flags" AD field". + */ + if (flags) { + ptr[0] = 0x02; + ptr[1] = EIR_FLAGS; + ptr[2] = flags; + + ad_len += 3; + ptr += 3; + } + } + + if (instance) { + memcpy(ptr, hdev->adv_instance.adv_data, + hdev->adv_instance.adv_data_len); + + ad_len += hdev->adv_instance.adv_data_len; + ptr += hdev->adv_instance.adv_data_len; + } + + /* Provide Tx Power only if we can provide a valid value for it */ + if (hdev->adv_tx_power != HCI_TX_POWER_INVALID && + (instance_flags & MGMT_ADV_FLAG_TX_POWER)) { + ptr[0] = 0x02; + ptr[1] = EIR_TX_POWER; + ptr[2] = (u8)hdev->adv_tx_power; + + ad_len += 3; + ptr += 3; + } + + return ad_len; +} + +static void update_adv_data_for_instance(struct hci_request *req, u8 instance) +{ + struct hci_dev *hdev = req->hdev; + struct hci_cp_le_set_adv_data cp; + u8 len; + + if (!hci_dev_test_flag(hdev, HCI_LE_ENABLED)) + return; + + memset(&cp, 0, sizeof(cp)); + + len = create_instance_adv_data(hdev, instance, cp.data); + + /* There's nothing to do if the data hasn't changed */ + if (hdev->adv_data_len == len && + memcmp(cp.data, hdev->adv_data, len) == 0) + return; + + memcpy(hdev->adv_data, cp.data, sizeof(cp.data)); + hdev->adv_data_len = len; + + cp.length = len; + + hci_req_add(req, HCI_OP_LE_SET_ADV_DATA, sizeof(cp), &cp); +} + +static void update_adv_data(struct hci_request *req) +{ + struct hci_dev *hdev = req->hdev; + u8 instance = get_current_adv_instance(hdev); + + update_adv_data_for_instance(req, instance); +} + +int mgmt_update_adv_data(struct hci_dev *hdev) +{ + struct hci_request req; + + hci_req_init(&req, hdev); + update_adv_data(&req); + + return hci_req_run(&req, NULL); +} + +static void create_eir(struct hci_dev *hdev, u8 *data) +{ + u8 *ptr = data; + size_t name_len; + + name_len = strlen(hdev->dev_name); + + if (name_len > 0) { + /* EIR Data type */ + if (name_len > 48) { + name_len = 48; + ptr[1] = EIR_NAME_SHORT; + } else + ptr[1] = EIR_NAME_COMPLETE; + + /* EIR Data length */ + ptr[0] = name_len + 1; + + memcpy(ptr + 2, hdev->dev_name, name_len); + + ptr += (name_len + 2); + } + + if (hdev->inq_tx_power != HCI_TX_POWER_INVALID) { + ptr[0] = 2; + ptr[1] = EIR_TX_POWER; + ptr[2] = (u8) hdev->inq_tx_power; + + ptr += 3; + } + + if (hdev->devid_source > 0) { + ptr[0] = 9; + ptr[1] = EIR_DEVICE_ID; + + put_unaligned_le16(hdev->devid_source, ptr + 2); + put_unaligned_le16(hdev->devid_vendor, ptr + 4); + put_unaligned_le16(hdev->devid_product, ptr + 6); + put_unaligned_le16(hdev->devid_version, ptr + 8); + + ptr += 10; + } + + ptr = create_uuid16_list(hdev, ptr, HCI_MAX_EIR_LENGTH - (ptr - data)); + ptr = create_uuid32_list(hdev, ptr, HCI_MAX_EIR_LENGTH - (ptr - data)); + ptr = create_uuid128_list(hdev, ptr, HCI_MAX_EIR_LENGTH - (ptr - data)); +} + +static void update_eir(struct hci_request *req) +{ + struct hci_dev *hdev = req->hdev; + struct hci_cp_write_eir cp; + + if (!hdev_is_powered(hdev)) + return; + + if (!lmp_ext_inq_capable(hdev)) + return; + + if (!hci_dev_test_flag(hdev, HCI_SSP_ENABLED)) + return; + + if (hci_dev_test_flag(hdev, HCI_SERVICE_CACHE)) + return; + + memset(&cp, 0, sizeof(cp)); + + create_eir(hdev, cp.data); + + if (memcmp(cp.data, hdev->eir, sizeof(cp.data)) == 0) + return; + + memcpy(hdev->eir, cp.data, sizeof(cp.data)); + + hci_req_add(req, HCI_OP_WRITE_EIR, sizeof(cp), &cp); +} + +static u8 get_service_classes(struct hci_dev *hdev) +{ + struct bt_uuid *uuid; + u8 val = 0; + + list_for_each_entry(uuid, &hdev->uuids, list) + val |= uuid->svc_hint; + + return val; +} + +static void update_class(struct hci_request *req) +{ + struct hci_dev *hdev = req->hdev; + u8 cod[3]; + + BT_DBG("%s", hdev->name); + + if (!hdev_is_powered(hdev)) + return; + + if (!hci_dev_test_flag(hdev, HCI_BREDR_ENABLED)) + return; + + if (hci_dev_test_flag(hdev, HCI_SERVICE_CACHE)) + return; + + cod[0] = hdev->minor_class; + cod[1] = hdev->major_class; + cod[2] = get_service_classes(hdev); + + if (hci_dev_test_flag(hdev, HCI_LIMITED_DISCOVERABLE)) + cod[1] |= 0x20; + + if (memcmp(cod, hdev->dev_class, 3) == 0) + return; + + hci_req_add(req, HCI_OP_WRITE_CLASS_OF_DEV, sizeof(cod), cod); +} + +static void disable_advertising(struct hci_request *req) +{ + u8 enable = 0x00; + + hci_req_add(req, HCI_OP_LE_SET_ADV_ENABLE, sizeof(enable), &enable); +} + +static void enable_advertising(struct hci_request *req) +{ + struct hci_dev *hdev = req->hdev; + struct hci_cp_le_set_adv_param cp; + u8 own_addr_type, enable = 0x01; + bool connectable; + u8 instance; + u32 flags; + + if (hci_conn_num(hdev, LE_LINK) > 0) + return; + + if (hci_dev_test_flag(hdev, HCI_LE_ADV)) + disable_advertising(req); + + /* Clear the HCI_LE_ADV bit temporarily so that the + * hci_update_random_address knows that it's safe to go ahead + * and write a new random address. The flag will be set back on + * as soon as the SET_ADV_ENABLE HCI command completes. + */ + hci_dev_clear_flag(hdev, HCI_LE_ADV); + + instance = get_current_adv_instance(hdev); + flags = get_adv_instance_flags(hdev, instance); + + /* If the "connectable" instance flag was not set, then choose between + * ADV_IND and ADV_NONCONN_IND based on the global connectable setting. + */ + connectable = (flags & MGMT_ADV_FLAG_CONNECTABLE) || + get_connectable(hdev); + + /* Set require_privacy to true only when non-connectable + * advertising is used. In that case it is fine to use a + * non-resolvable private address. + */ + if (hci_update_random_address(req, !connectable, &own_addr_type) < 0) + return; + + memset(&cp, 0, sizeof(cp)); + cp.min_interval = cpu_to_le16(hdev->le_adv_min_interval); + cp.max_interval = cpu_to_le16(hdev->le_adv_max_interval); + + if (connectable) + cp.type = LE_ADV_IND; + else if (get_adv_instance_scan_rsp_len(hdev, instance)) + cp.type = LE_ADV_SCAN_IND; + else + cp.type = LE_ADV_NONCONN_IND; + + cp.own_address_type = own_addr_type; + cp.channel_map = hdev->le_adv_channel_map; + + hci_req_add(req, HCI_OP_LE_SET_ADV_PARAM, sizeof(cp), &cp); + + hci_req_add(req, HCI_OP_LE_SET_ADV_ENABLE, sizeof(enable), &enable); +} + +static void service_cache_off(struct work_struct *work) +{ + struct hci_dev *hdev = container_of(work, struct hci_dev, + service_cache.work); + struct hci_request req; + + if (!hci_dev_test_and_clear_flag(hdev, HCI_SERVICE_CACHE)) + return; + + hci_req_init(&req, hdev); + + hci_dev_lock(hdev); + + update_eir(&req); + update_class(&req); + + hci_dev_unlock(hdev); + + hci_req_run(&req, NULL); +} + +static void rpa_expired(struct work_struct *work) +{ + struct hci_dev *hdev = container_of(work, struct hci_dev, + rpa_expired.work); + struct hci_request req; + + BT_DBG(""); + + hci_dev_set_flag(hdev, HCI_RPA_EXPIRED); + + if (!hci_dev_test_flag(hdev, HCI_ADVERTISING)) + return; + + /* The generation of a new RPA and programming it into the + * controller happens in the enable_advertising() function. + */ + hci_req_init(&req, hdev); + enable_advertising(&req); + hci_req_run(&req, NULL); +} + +static void mgmt_init_hdev(struct sock *sk, struct hci_dev *hdev) +{ + if (hci_dev_test_and_set_flag(hdev, HCI_MGMT)) + return; + + INIT_DELAYED_WORK(&hdev->service_cache, service_cache_off); + INIT_DELAYED_WORK(&hdev->rpa_expired, rpa_expired); + + /* Non-mgmt controlled devices get this bit set + * implicitly so that pairing works for them, however + * for mgmt we require user-space to explicitly enable + * it + */ + hci_dev_clear_flag(hdev, HCI_BONDABLE); +} + +static int read_controller_info(struct sock *sk, struct hci_dev *hdev, + void *data, u16 data_len) +{ + struct mgmt_rp_read_info rp; + + BT_DBG("sock %p %s", sk, hdev->name); + + hci_dev_lock(hdev); + + memset(&rp, 0, sizeof(rp)); + + bacpy(&rp.bdaddr, &hdev->bdaddr); + + rp.version = hdev->hci_ver; + rp.manufacturer = cpu_to_le16(hdev->manufacturer); + + rp.supported_settings = cpu_to_le32(get_supported_settings(hdev)); + rp.current_settings = cpu_to_le32(get_current_settings(hdev)); + + memcpy(rp.dev_class, hdev->dev_class, 3); + + memcpy(rp.name, hdev->dev_name, sizeof(hdev->dev_name)); + memcpy(rp.short_name, hdev->short_name, sizeof(hdev->short_name)); + + hci_dev_unlock(hdev); + + return mgmt_cmd_complete(sk, hdev->id, MGMT_OP_READ_INFO, 0, &rp, + sizeof(rp)); +} + +static int send_settings_rsp(struct sock *sk, u16 opcode, struct hci_dev *hdev) +{ + __le32 settings = cpu_to_le32(get_current_settings(hdev)); + + return mgmt_cmd_complete(sk, hdev->id, opcode, 0, &settings, + sizeof(settings)); +} + +static void clean_up_hci_complete(struct hci_dev *hdev, u8 status, u16 opcode) +{ + BT_DBG("%s status 0x%02x", hdev->name, status); + + if (hci_conn_count(hdev) == 0) { + cancel_delayed_work(&hdev->power_off); + queue_work(hdev->req_workqueue, &hdev->power_off.work); + } +} + +static bool hci_stop_discovery(struct hci_request *req) +{ + struct hci_dev *hdev = req->hdev; + struct hci_cp_remote_name_req_cancel cp; + struct inquiry_entry *e; + + switch (hdev->discovery.state) { + case DISCOVERY_FINDING: + if (test_bit(HCI_INQUIRY, &hdev->flags)) + hci_req_add(req, HCI_OP_INQUIRY_CANCEL, 0, NULL); + + if (hci_dev_test_flag(hdev, HCI_LE_SCAN)) { + cancel_delayed_work(&hdev->le_scan_disable); + hci_req_add_le_scan_disable(req); + } + + return true; + + case DISCOVERY_RESOLVING: + e = hci_inquiry_cache_lookup_resolve(hdev, BDADDR_ANY, + NAME_PENDING); + if (!e) + break; + + bacpy(&cp.bdaddr, &e->data.bdaddr); + hci_req_add(req, HCI_OP_REMOTE_NAME_REQ_CANCEL, sizeof(cp), + &cp); + + return true; + + default: + /* Passive scanning */ + if (hci_dev_test_flag(hdev, HCI_LE_SCAN)) { + hci_req_add_le_scan_disable(req); + return true; + } + + break; + } + + return false; +} + +static void advertising_added(struct sock *sk, struct hci_dev *hdev, + u8 instance) +{ + struct mgmt_ev_advertising_added ev; + + ev.instance = instance; + + mgmt_event(MGMT_EV_ADVERTISING_ADDED, hdev, &ev, sizeof(ev), sk); +} + +static void advertising_removed(struct sock *sk, struct hci_dev *hdev, + u8 instance) +{ + struct mgmt_ev_advertising_removed ev; + + ev.instance = instance; + + mgmt_event(MGMT_EV_ADVERTISING_REMOVED, hdev, &ev, sizeof(ev), sk); +} + +static void clear_adv_instance(struct hci_dev *hdev) +{ + struct hci_request req; + + if (!hci_dev_test_flag(hdev, HCI_ADVERTISING_INSTANCE)) + return; + + if (hdev->adv_instance.timeout) + cancel_delayed_work(&hdev->adv_instance.timeout_exp); + + memset(&hdev->adv_instance, 0, sizeof(hdev->adv_instance)); + advertising_removed(NULL, hdev, 1); + hci_dev_clear_flag(hdev, HCI_ADVERTISING_INSTANCE); + + if (!hdev_is_powered(hdev) || + hci_dev_test_flag(hdev, HCI_ADVERTISING)) + return; + + hci_req_init(&req, hdev); + disable_advertising(&req); + hci_req_run(&req, NULL); +} + +static int clean_up_hci_state(struct hci_dev *hdev) +{ + struct hci_request req; + struct hci_conn *conn; + bool discov_stopped; + int err; + + hci_req_init(&req, hdev); + + if (test_bit(HCI_ISCAN, &hdev->flags) || + test_bit(HCI_PSCAN, &hdev->flags)) { + u8 scan = 0x00; + hci_req_add(&req, HCI_OP_WRITE_SCAN_ENABLE, 1, &scan); + } + + if (hdev->adv_instance.timeout) + clear_adv_instance(hdev); + + if (hci_dev_test_flag(hdev, HCI_LE_ADV)) + disable_advertising(&req); + + discov_stopped = hci_stop_discovery(&req); + + list_for_each_entry(conn, &hdev->conn_hash.list, list) { + struct hci_cp_disconnect dc; + struct hci_cp_reject_conn_req rej; + + switch (conn->state) { + case BT_CONNECTED: + case BT_CONFIG: + dc.handle = cpu_to_le16(conn->handle); + dc.reason = 0x15; /* Terminated due to Power Off */ + hci_req_add(&req, HCI_OP_DISCONNECT, sizeof(dc), &dc); + break; + case BT_CONNECT: + if (conn->type == LE_LINK) + hci_req_add(&req, HCI_OP_LE_CREATE_CONN_CANCEL, + 0, NULL); + else if (conn->type == ACL_LINK) + hci_req_add(&req, HCI_OP_CREATE_CONN_CANCEL, + 6, &conn->dst); + break; + case BT_CONNECT2: + bacpy(&rej.bdaddr, &conn->dst); + rej.reason = 0x15; /* Terminated due to Power Off */ + if (conn->type == ACL_LINK) + hci_req_add(&req, HCI_OP_REJECT_CONN_REQ, + sizeof(rej), &rej); + else if (conn->type == SCO_LINK) + hci_req_add(&req, HCI_OP_REJECT_SYNC_CONN_REQ, + sizeof(rej), &rej); + break; + } + } + + err = hci_req_run(&req, clean_up_hci_complete); + if (!err && discov_stopped) + hci_discovery_set_state(hdev, DISCOVERY_STOPPING); + + return err; +} + +static int set_powered(struct sock *sk, struct hci_dev *hdev, void *data, + u16 len) +{ + struct mgmt_mode *cp = data; + struct mgmt_pending_cmd *cmd; + int err; + + BT_DBG("request for %s", hdev->name); + + if (cp->val != 0x00 && cp->val != 0x01) + return mgmt_cmd_status(sk, hdev->id, MGMT_OP_SET_POWERED, + MGMT_STATUS_INVALID_PARAMS); + + hci_dev_lock(hdev); + + if (pending_find(MGMT_OP_SET_POWERED, hdev)) { + err = mgmt_cmd_status(sk, hdev->id, MGMT_OP_SET_POWERED, + MGMT_STATUS_BUSY); + goto failed; + } + + if (hci_dev_test_and_clear_flag(hdev, HCI_AUTO_OFF)) { + cancel_delayed_work(&hdev->power_off); + + if (cp->val) { + mgmt_pending_add(sk, MGMT_OP_SET_POWERED, hdev, + data, len); + err = mgmt_powered(hdev, 1); + goto failed; + } + } + + if (!!cp->val == hdev_is_powered(hdev)) { + err = send_settings_rsp(sk, MGMT_OP_SET_POWERED, hdev); + goto failed; + } + + cmd = mgmt_pending_add(sk, MGMT_OP_SET_POWERED, hdev, data, len); + if (!cmd) { + err = -ENOMEM; + goto failed; + } + + if (cp->val) { + queue_work(hdev->req_workqueue, &hdev->power_on); + err = 0; + } else { + /* Disconnect connections, stop scans, etc */ + err = clean_up_hci_state(hdev); + if (!err) + queue_delayed_work(hdev->req_workqueue, &hdev->power_off, + HCI_POWER_OFF_TIMEOUT); + + /* ENODATA means there were no HCI commands queued */ + if (err == -ENODATA) { + cancel_delayed_work(&hdev->power_off); + queue_work(hdev->req_workqueue, &hdev->power_off.work); + err = 0; + } + } + +failed: + hci_dev_unlock(hdev); + return err; +} + +static int new_settings(struct hci_dev *hdev, struct sock *skip) +{ + __le32 ev = cpu_to_le32(get_current_settings(hdev)); + + return mgmt_generic_event(MGMT_EV_NEW_SETTINGS, hdev, &ev, + sizeof(ev), skip); +} + +int mgmt_new_settings(struct hci_dev *hdev) +{ + return new_settings(hdev, NULL); +} + +struct cmd_lookup { + struct sock *sk; + struct hci_dev *hdev; + u8 mgmt_status; +}; + +static void settings_rsp(struct mgmt_pending_cmd *cmd, void *data) +{ + struct cmd_lookup *match = data; + + send_settings_rsp(cmd->sk, cmd->opcode, match->hdev); + + list_del(&cmd->list); + + if (match->sk == NULL) { + match->sk = cmd->sk; + sock_hold(match->sk); + } + + mgmt_pending_free(cmd); +} + +static void cmd_status_rsp(struct mgmt_pending_cmd *cmd, void *data) +{ + u8 *status = data; + + mgmt_cmd_status(cmd->sk, cmd->index, cmd->opcode, *status); + mgmt_pending_remove(cmd); +} + +static void cmd_complete_rsp(struct mgmt_pending_cmd *cmd, void *data) +{ + if (cmd->cmd_complete) { + u8 *status = data; + + cmd->cmd_complete(cmd, *status); + mgmt_pending_remove(cmd); + + return; + } + + cmd_status_rsp(cmd, data); +} + +static int generic_cmd_complete(struct mgmt_pending_cmd *cmd, u8 status) +{ + return mgmt_cmd_complete(cmd->sk, cmd->index, cmd->opcode, status, + cmd->param, cmd->param_len); +} + +static int addr_cmd_complete(struct mgmt_pending_cmd *cmd, u8 status) +{ + return mgmt_cmd_complete(cmd->sk, cmd->index, cmd->opcode, status, + cmd->param, sizeof(struct mgmt_addr_info)); +} + +static u8 mgmt_bredr_support(struct hci_dev *hdev) +{ + if (!lmp_bredr_capable(hdev)) + return MGMT_STATUS_NOT_SUPPORTED; + else if (!hci_dev_test_flag(hdev, HCI_BREDR_ENABLED)) + return MGMT_STATUS_REJECTED; + else + return MGMT_STATUS_SUCCESS; +} + +static u8 mgmt_le_support(struct hci_dev *hdev) +{ + if (!lmp_le_capable(hdev)) + return MGMT_STATUS_NOT_SUPPORTED; + else if (!hci_dev_test_flag(hdev, HCI_LE_ENABLED)) + return MGMT_STATUS_REJECTED; + else + return MGMT_STATUS_SUCCESS; +} + +static void set_discoverable_complete(struct hci_dev *hdev, u8 status, + u16 opcode) +{ + struct mgmt_pending_cmd *cmd; + struct mgmt_mode *cp; + struct hci_request req; + bool changed; + + BT_DBG("status 0x%02x", status); + + hci_dev_lock(hdev); + + cmd = pending_find(MGMT_OP_SET_DISCOVERABLE, hdev); + if (!cmd) + goto unlock; + + if (status) { + u8 mgmt_err = mgmt_status(status); + mgmt_cmd_status(cmd->sk, cmd->index, cmd->opcode, mgmt_err); + hci_dev_clear_flag(hdev, HCI_LIMITED_DISCOVERABLE); + goto remove_cmd; + } + + cp = cmd->param; + if (cp->val) { + changed = !hci_dev_test_and_set_flag(hdev, HCI_DISCOVERABLE); + + if (hdev->discov_timeout > 0) { + int to = msecs_to_jiffies(hdev->discov_timeout * 1000); + queue_delayed_work(hdev->workqueue, &hdev->discov_off, + to); + } + } else { + changed = hci_dev_test_and_clear_flag(hdev, HCI_DISCOVERABLE); + } + + send_settings_rsp(cmd->sk, MGMT_OP_SET_DISCOVERABLE, hdev); + + if (changed) + new_settings(hdev, cmd->sk); + + /* When the discoverable mode gets changed, make sure + * that class of device has the limited discoverable + * bit correctly set. Also update page scan based on whitelist + * entries. + */ + hci_req_init(&req, hdev); + __hci_update_page_scan(&req); + update_class(&req); + hci_req_run(&req, NULL); + +remove_cmd: + mgmt_pending_remove(cmd); + +unlock: + hci_dev_unlock(hdev); +} + +static int set_discoverable(struct sock *sk, struct hci_dev *hdev, void *data, + u16 len) +{ + struct mgmt_cp_set_discoverable *cp = data; + struct mgmt_pending_cmd *cmd; + struct hci_request req; + u16 timeout; + u8 scan; + int err; + + BT_DBG("request for %s", hdev->name); + + if (!hci_dev_test_flag(hdev, HCI_LE_ENABLED) && + !hci_dev_test_flag(hdev, HCI_BREDR_ENABLED)) + return mgmt_cmd_status(sk, hdev->id, MGMT_OP_SET_DISCOVERABLE, + MGMT_STATUS_REJECTED); + + if (cp->val != 0x00 && cp->val != 0x01 && cp->val != 0x02) + return mgmt_cmd_status(sk, hdev->id, MGMT_OP_SET_DISCOVERABLE, + MGMT_STATUS_INVALID_PARAMS); + + timeout = __le16_to_cpu(cp->timeout); + + /* Disabling discoverable requires that no timeout is set, + * and enabling limited discoverable requires a timeout. + */ + if ((cp->val == 0x00 && timeout > 0) || + (cp->val == 0x02 && timeout == 0)) + return mgmt_cmd_status(sk, hdev->id, MGMT_OP_SET_DISCOVERABLE, + MGMT_STATUS_INVALID_PARAMS); + + hci_dev_lock(hdev); + + if (!hdev_is_powered(hdev) && timeout > 0) { + err = mgmt_cmd_status(sk, hdev->id, MGMT_OP_SET_DISCOVERABLE, + MGMT_STATUS_NOT_POWERED); + goto failed; + } + + if (pending_find(MGMT_OP_SET_DISCOVERABLE, hdev) || + pending_find(MGMT_OP_SET_CONNECTABLE, hdev)) { + err = mgmt_cmd_status(sk, hdev->id, MGMT_OP_SET_DISCOVERABLE, + MGMT_STATUS_BUSY); + goto failed; + } + + if (!hci_dev_test_flag(hdev, HCI_CONNECTABLE)) { + err = mgmt_cmd_status(sk, hdev->id, MGMT_OP_SET_DISCOVERABLE, + MGMT_STATUS_REJECTED); + goto failed; + } + + if (!hdev_is_powered(hdev)) { + bool changed = false; + + /* Setting limited discoverable when powered off is + * not a valid operation since it requires a timeout + * and so no need to check HCI_LIMITED_DISCOVERABLE. + */ + if (!!cp->val != hci_dev_test_flag(hdev, HCI_DISCOVERABLE)) { + hci_dev_change_flag(hdev, HCI_DISCOVERABLE); + changed = true; + } + + err = send_settings_rsp(sk, MGMT_OP_SET_DISCOVERABLE, hdev); + if (err < 0) + goto failed; + + if (changed) + err = new_settings(hdev, sk); + + goto failed; + } + + /* If the current mode is the same, then just update the timeout + * value with the new value. And if only the timeout gets updated, + * then no need for any HCI transactions. + */ + if (!!cp->val == hci_dev_test_flag(hdev, HCI_DISCOVERABLE) && + (cp->val == 0x02) == hci_dev_test_flag(hdev, + HCI_LIMITED_DISCOVERABLE)) { + cancel_delayed_work(&hdev->discov_off); + hdev->discov_timeout = timeout; + + if (cp->val && hdev->discov_timeout > 0) { + int to = msecs_to_jiffies(hdev->discov_timeout * 1000); + queue_delayed_work(hdev->workqueue, &hdev->discov_off, + to); + } + + err = send_settings_rsp(sk, MGMT_OP_SET_DISCOVERABLE, hdev); + goto failed; + } + + cmd = mgmt_pending_add(sk, MGMT_OP_SET_DISCOVERABLE, hdev, data, len); + if (!cmd) { + err = -ENOMEM; + goto failed; + } + + /* Cancel any potential discoverable timeout that might be + * still active and store new timeout value. The arming of + * the timeout happens in the complete handler. + */ + cancel_delayed_work(&hdev->discov_off); + hdev->discov_timeout = timeout; + + /* Limited discoverable mode */ + if (cp->val == 0x02) + hci_dev_set_flag(hdev, HCI_LIMITED_DISCOVERABLE); + else + hci_dev_clear_flag(hdev, HCI_LIMITED_DISCOVERABLE); + + hci_req_init(&req, hdev); + + /* The procedure for LE-only controllers is much simpler - just + * update the advertising data. + */ + if (!hci_dev_test_flag(hdev, HCI_BREDR_ENABLED)) + goto update_ad; + + scan = SCAN_PAGE; + + if (cp->val) { + struct hci_cp_write_current_iac_lap hci_cp; + + if (cp->val == 0x02) { + /* Limited discoverable mode */ + hci_cp.num_iac = min_t(u8, hdev->num_iac, 2); + hci_cp.iac_lap[0] = 0x00; /* LIAC */ + hci_cp.iac_lap[1] = 0x8b; + hci_cp.iac_lap[2] = 0x9e; + hci_cp.iac_lap[3] = 0x33; /* GIAC */ + hci_cp.iac_lap[4] = 0x8b; + hci_cp.iac_lap[5] = 0x9e; + } else { + /* General discoverable mode */ + hci_cp.num_iac = 1; + hci_cp.iac_lap[0] = 0x33; /* GIAC */ + hci_cp.iac_lap[1] = 0x8b; + hci_cp.iac_lap[2] = 0x9e; + } + + hci_req_add(&req, HCI_OP_WRITE_CURRENT_IAC_LAP, + (hci_cp.num_iac * 3) + 1, &hci_cp); + + scan |= SCAN_INQUIRY; + } else { + hci_dev_clear_flag(hdev, HCI_LIMITED_DISCOVERABLE); + } + + hci_req_add(&req, HCI_OP_WRITE_SCAN_ENABLE, sizeof(scan), &scan); + +update_ad: + update_adv_data(&req); + + err = hci_req_run(&req, set_discoverable_complete); + if (err < 0) + mgmt_pending_remove(cmd); + +failed: + hci_dev_unlock(hdev); + return err; +} + +static void write_fast_connectable(struct hci_request *req, bool enable) +{ + struct hci_dev *hdev = req->hdev; + struct hci_cp_write_page_scan_activity acp; + u8 type; + + if (!hci_dev_test_flag(hdev, HCI_BREDR_ENABLED)) + return; + + if (hdev->hci_ver < BLUETOOTH_VER_1_2) + return; + + if (enable) { + type = PAGE_SCAN_TYPE_INTERLACED; + + /* 160 msec page scan interval */ + acp.interval = cpu_to_le16(0x0100); + } else { + type = PAGE_SCAN_TYPE_STANDARD; /* default */ + + /* default 1.28 sec page scan */ + acp.interval = cpu_to_le16(0x0800); + } + + acp.window = cpu_to_le16(0x0012); + + if (__cpu_to_le16(hdev->page_scan_interval) != acp.interval || + __cpu_to_le16(hdev->page_scan_window) != acp.window) + hci_req_add(req, HCI_OP_WRITE_PAGE_SCAN_ACTIVITY, + sizeof(acp), &acp); + + if (hdev->page_scan_type != type) + hci_req_add(req, HCI_OP_WRITE_PAGE_SCAN_TYPE, 1, &type); +} + +static void set_connectable_complete(struct hci_dev *hdev, u8 status, + u16 opcode) +{ + struct mgmt_pending_cmd *cmd; + struct mgmt_mode *cp; + bool conn_changed, discov_changed; + + BT_DBG("status 0x%02x", status); + + hci_dev_lock(hdev); + + cmd = pending_find(MGMT_OP_SET_CONNECTABLE, hdev); + if (!cmd) + goto unlock; + + if (status) { + u8 mgmt_err = mgmt_status(status); + mgmt_cmd_status(cmd->sk, cmd->index, cmd->opcode, mgmt_err); + goto remove_cmd; + } + + cp = cmd->param; + if (cp->val) { + conn_changed = !hci_dev_test_and_set_flag(hdev, + HCI_CONNECTABLE); + discov_changed = false; + } else { + conn_changed = hci_dev_test_and_clear_flag(hdev, + HCI_CONNECTABLE); + discov_changed = hci_dev_test_and_clear_flag(hdev, + HCI_DISCOVERABLE); + } + + send_settings_rsp(cmd->sk, MGMT_OP_SET_CONNECTABLE, hdev); + + if (conn_changed || discov_changed) { + new_settings(hdev, cmd->sk); + hci_update_page_scan(hdev); + if (discov_changed) + mgmt_update_adv_data(hdev); + hci_update_background_scan(hdev); + } + +remove_cmd: + mgmt_pending_remove(cmd); + +unlock: + hci_dev_unlock(hdev); +} + +static int set_connectable_update_settings(struct hci_dev *hdev, + struct sock *sk, u8 val) +{ + bool changed = false; + int err; + + if (!!val != hci_dev_test_flag(hdev, HCI_CONNECTABLE)) + changed = true; + + if (val) { + hci_dev_set_flag(hdev, HCI_CONNECTABLE); + } else { + hci_dev_clear_flag(hdev, HCI_CONNECTABLE); + hci_dev_clear_flag(hdev, HCI_DISCOVERABLE); + } + + err = send_settings_rsp(sk, MGMT_OP_SET_CONNECTABLE, hdev); + if (err < 0) + return err; + + if (changed) { + hci_update_page_scan(hdev); + hci_update_background_scan(hdev); + return new_settings(hdev, sk); + } + + return 0; +} + +static int set_connectable(struct sock *sk, struct hci_dev *hdev, void *data, + u16 len) +{ + struct mgmt_mode *cp = data; + struct mgmt_pending_cmd *cmd; + struct hci_request req; + u8 scan; + int err; + + BT_DBG("request for %s", hdev->name); + + if (!hci_dev_test_flag(hdev, HCI_LE_ENABLED) && + !hci_dev_test_flag(hdev, HCI_BREDR_ENABLED)) + return mgmt_cmd_status(sk, hdev->id, MGMT_OP_SET_CONNECTABLE, + MGMT_STATUS_REJECTED); + + if (cp->val != 0x00 && cp->val != 0x01) + return mgmt_cmd_status(sk, hdev->id, MGMT_OP_SET_CONNECTABLE, + MGMT_STATUS_INVALID_PARAMS); + + hci_dev_lock(hdev); + + if (!hdev_is_powered(hdev)) { + err = set_connectable_update_settings(hdev, sk, cp->val); + goto failed; + } + + if (pending_find(MGMT_OP_SET_DISCOVERABLE, hdev) || + pending_find(MGMT_OP_SET_CONNECTABLE, hdev)) { + err = mgmt_cmd_status(sk, hdev->id, MGMT_OP_SET_CONNECTABLE, + MGMT_STATUS_BUSY); + goto failed; + } + + cmd = mgmt_pending_add(sk, MGMT_OP_SET_CONNECTABLE, hdev, data, len); + if (!cmd) { + err = -ENOMEM; + goto failed; + } + + hci_req_init(&req, hdev); + + /* If BR/EDR is not enabled and we disable advertising as a + * by-product of disabling connectable, we need to update the + * advertising flags. + */ + if (!hci_dev_test_flag(hdev, HCI_BREDR_ENABLED)) { + if (!cp->val) { + hci_dev_clear_flag(hdev, HCI_LIMITED_DISCOVERABLE); + hci_dev_clear_flag(hdev, HCI_DISCOVERABLE); + } + update_adv_data(&req); + } else if (cp->val != test_bit(HCI_PSCAN, &hdev->flags)) { + if (cp->val) { + scan = SCAN_PAGE; + } else { + /* If we don't have any whitelist entries just + * disable all scanning. If there are entries + * and we had both page and inquiry scanning + * enabled then fall back to only page scanning. + * Otherwise no changes are needed. + */ + if (list_empty(&hdev->whitelist)) + scan = SCAN_DISABLED; + else if (test_bit(HCI_ISCAN, &hdev->flags)) + scan = SCAN_PAGE; + else + goto no_scan_update; + + if (test_bit(HCI_ISCAN, &hdev->flags) && + hdev->discov_timeout > 0) + cancel_delayed_work(&hdev->discov_off); + } + + hci_req_add(&req, HCI_OP_WRITE_SCAN_ENABLE, 1, &scan); + } + +no_scan_update: + /* Update the advertising parameters if necessary */ + if (hci_dev_test_flag(hdev, HCI_ADVERTISING) || + hci_dev_test_flag(hdev, HCI_ADVERTISING_INSTANCE)) + enable_advertising(&req); + + err = hci_req_run(&req, set_connectable_complete); + if (err < 0) { + mgmt_pending_remove(cmd); + if (err == -ENODATA) + err = set_connectable_update_settings(hdev, sk, + cp->val); + goto failed; + } + +failed: + hci_dev_unlock(hdev); + return err; +} + +static int set_bondable(struct sock *sk, struct hci_dev *hdev, void *data, + u16 len) +{ + struct mgmt_mode *cp = data; + bool changed; + int err; + + BT_DBG("request for %s", hdev->name); + + if (cp->val != 0x00 && cp->val != 0x01) + return mgmt_cmd_status(sk, hdev->id, MGMT_OP_SET_BONDABLE, + MGMT_STATUS_INVALID_PARAMS); + + hci_dev_lock(hdev); + + if (cp->val) + changed = !hci_dev_test_and_set_flag(hdev, HCI_BONDABLE); + else + changed = hci_dev_test_and_clear_flag(hdev, HCI_BONDABLE); + + err = send_settings_rsp(sk, MGMT_OP_SET_BONDABLE, hdev); + if (err < 0) + goto unlock; + + if (changed) + err = new_settings(hdev, sk); + +unlock: + hci_dev_unlock(hdev); + return err; +} + +static int set_link_security(struct sock *sk, struct hci_dev *hdev, void *data, + u16 len) +{ + struct mgmt_mode *cp = data; + struct mgmt_pending_cmd *cmd; + u8 val, status; + int err; + + BT_DBG("request for %s", hdev->name); + + status = mgmt_bredr_support(hdev); + if (status) + return mgmt_cmd_status(sk, hdev->id, MGMT_OP_SET_LINK_SECURITY, + status); + + if (cp->val != 0x00 && cp->val != 0x01) + return mgmt_cmd_status(sk, hdev->id, MGMT_OP_SET_LINK_SECURITY, + MGMT_STATUS_INVALID_PARAMS); + + hci_dev_lock(hdev); + + if (!hdev_is_powered(hdev)) { + bool changed = false; + + if (!!cp->val != hci_dev_test_flag(hdev, HCI_LINK_SECURITY)) { + hci_dev_change_flag(hdev, HCI_LINK_SECURITY); + changed = true; + } + + err = send_settings_rsp(sk, MGMT_OP_SET_LINK_SECURITY, hdev); + if (err < 0) + goto failed; + + if (changed) + err = new_settings(hdev, sk); + + goto failed; + } + + if (pending_find(MGMT_OP_SET_LINK_SECURITY, hdev)) { + err = mgmt_cmd_status(sk, hdev->id, MGMT_OP_SET_LINK_SECURITY, + MGMT_STATUS_BUSY); + goto failed; + } + + val = !!cp->val; + + if (test_bit(HCI_AUTH, &hdev->flags) == val) { + err = send_settings_rsp(sk, MGMT_OP_SET_LINK_SECURITY, hdev); + goto failed; + } + + cmd = mgmt_pending_add(sk, MGMT_OP_SET_LINK_SECURITY, hdev, data, len); + if (!cmd) { + err = -ENOMEM; + goto failed; + } + + err = hci_send_cmd(hdev, HCI_OP_WRITE_AUTH_ENABLE, sizeof(val), &val); + if (err < 0) { + mgmt_pending_remove(cmd); + goto failed; + } + +failed: + hci_dev_unlock(hdev); + return err; +} + +static int set_ssp(struct sock *sk, struct hci_dev *hdev, void *data, u16 len) +{ + struct mgmt_mode *cp = data; + struct mgmt_pending_cmd *cmd; + u8 status; + int err; + + BT_DBG("request for %s", hdev->name); + + status = mgmt_bredr_support(hdev); + if (status) + return mgmt_cmd_status(sk, hdev->id, MGMT_OP_SET_SSP, status); + + if (!lmp_ssp_capable(hdev)) + return mgmt_cmd_status(sk, hdev->id, MGMT_OP_SET_SSP, + MGMT_STATUS_NOT_SUPPORTED); + + if (cp->val != 0x00 && cp->val != 0x01) + return mgmt_cmd_status(sk, hdev->id, MGMT_OP_SET_SSP, + MGMT_STATUS_INVALID_PARAMS); + + hci_dev_lock(hdev); + + if (!hdev_is_powered(hdev)) { + bool changed; + + if (cp->val) { + changed = !hci_dev_test_and_set_flag(hdev, + HCI_SSP_ENABLED); + } else { + changed = hci_dev_test_and_clear_flag(hdev, + HCI_SSP_ENABLED); + if (!changed) + changed = hci_dev_test_and_clear_flag(hdev, + HCI_HS_ENABLED); + else + hci_dev_clear_flag(hdev, HCI_HS_ENABLED); + } + + err = send_settings_rsp(sk, MGMT_OP_SET_SSP, hdev); + if (err < 0) + goto failed; + + if (changed) + err = new_settings(hdev, sk); + + goto failed; + } + + if (pending_find(MGMT_OP_SET_SSP, hdev)) { + err = mgmt_cmd_status(sk, hdev->id, MGMT_OP_SET_SSP, + MGMT_STATUS_BUSY); + goto failed; + } + + if (!!cp->val == hci_dev_test_flag(hdev, HCI_SSP_ENABLED)) { + err = send_settings_rsp(sk, MGMT_OP_SET_SSP, hdev); + goto failed; + } + + cmd = mgmt_pending_add(sk, MGMT_OP_SET_SSP, hdev, data, len); + if (!cmd) { + err = -ENOMEM; + goto failed; + } + + if (!cp->val && hci_dev_test_flag(hdev, HCI_USE_DEBUG_KEYS)) + hci_send_cmd(hdev, HCI_OP_WRITE_SSP_DEBUG_MODE, + sizeof(cp->val), &cp->val); + + err = hci_send_cmd(hdev, HCI_OP_WRITE_SSP_MODE, 1, &cp->val); + if (err < 0) { + mgmt_pending_remove(cmd); + goto failed; + } + +failed: + hci_dev_unlock(hdev); + return err; +} + +static int set_hs(struct sock *sk, struct hci_dev *hdev, void *data, u16 len) +{ + struct mgmt_mode *cp = data; + bool changed; + u8 status; + int err; + + BT_DBG("request for %s", hdev->name); + + status = mgmt_bredr_support(hdev); + if (status) + return mgmt_cmd_status(sk, hdev->id, MGMT_OP_SET_HS, status); + + if (!lmp_ssp_capable(hdev)) + return mgmt_cmd_status(sk, hdev->id, MGMT_OP_SET_HS, + MGMT_STATUS_NOT_SUPPORTED); + + if (!hci_dev_test_flag(hdev, HCI_SSP_ENABLED)) + return mgmt_cmd_status(sk, hdev->id, MGMT_OP_SET_HS, + MGMT_STATUS_REJECTED); + + if (cp->val != 0x00 && cp->val != 0x01) + return mgmt_cmd_status(sk, hdev->id, MGMT_OP_SET_HS, + MGMT_STATUS_INVALID_PARAMS); + + hci_dev_lock(hdev); + + if (pending_find(MGMT_OP_SET_SSP, hdev)) { + err = mgmt_cmd_status(sk, hdev->id, MGMT_OP_SET_HS, + MGMT_STATUS_BUSY); + goto unlock; + } + + if (cp->val) { + changed = !hci_dev_test_and_set_flag(hdev, HCI_HS_ENABLED); + } else { + if (hdev_is_powered(hdev)) { + err = mgmt_cmd_status(sk, hdev->id, MGMT_OP_SET_HS, + MGMT_STATUS_REJECTED); + goto unlock; + } + + changed = hci_dev_test_and_clear_flag(hdev, HCI_HS_ENABLED); + } + + err = send_settings_rsp(sk, MGMT_OP_SET_HS, hdev); + if (err < 0) + goto unlock; + + if (changed) + err = new_settings(hdev, sk); + +unlock: + hci_dev_unlock(hdev); + return err; +} + +static void le_enable_complete(struct hci_dev *hdev, u8 status, u16 opcode) +{ + struct cmd_lookup match = { NULL, hdev }; + + hci_dev_lock(hdev); + + if (status) { + u8 mgmt_err = mgmt_status(status); + + mgmt_pending_foreach(MGMT_OP_SET_LE, hdev, cmd_status_rsp, + &mgmt_err); + goto unlock; + } + + mgmt_pending_foreach(MGMT_OP_SET_LE, hdev, settings_rsp, &match); + + new_settings(hdev, match.sk); + + if (match.sk) + sock_put(match.sk); + + /* Make sure the controller has a good default for + * advertising data. Restrict the update to when LE + * has actually been enabled. During power on, the + * update in powered_update_hci will take care of it. + */ + if (hci_dev_test_flag(hdev, HCI_LE_ENABLED)) { + struct hci_request req; + + hci_req_init(&req, hdev); + update_adv_data(&req); + update_scan_rsp_data(&req); + __hci_update_background_scan(&req); + hci_req_run(&req, NULL); + } + +unlock: + hci_dev_unlock(hdev); +} + +static int set_le(struct sock *sk, struct hci_dev *hdev, void *data, u16 len) +{ + struct mgmt_mode *cp = data; + struct hci_cp_write_le_host_supported hci_cp; + struct mgmt_pending_cmd *cmd; + struct hci_request req; + int err; + u8 val, enabled; + + BT_DBG("request for %s", hdev->name); + + if (!lmp_le_capable(hdev)) + return mgmt_cmd_status(sk, hdev->id, MGMT_OP_SET_LE, + MGMT_STATUS_NOT_SUPPORTED); + + if (cp->val != 0x00 && cp->val != 0x01) + return mgmt_cmd_status(sk, hdev->id, MGMT_OP_SET_LE, + MGMT_STATUS_INVALID_PARAMS); + + /* Bluetooth single mode LE only controllers or dual-mode + * controllers configured as LE only devices, do not allow + * switching LE off. These have either LE enabled explicitly + * or BR/EDR has been previously switched off. + * + * When trying to enable an already enabled LE, then gracefully + * send a positive response. Trying to disable it however will + * result into rejection. + */ + if (!hci_dev_test_flag(hdev, HCI_BREDR_ENABLED)) { + if (cp->val == 0x01) + return send_settings_rsp(sk, MGMT_OP_SET_LE, hdev); + + return mgmt_cmd_status(sk, hdev->id, MGMT_OP_SET_LE, + MGMT_STATUS_REJECTED); + } + + hci_dev_lock(hdev); + + val = !!cp->val; + enabled = lmp_host_le_capable(hdev); + + if (!hdev_is_powered(hdev) || val == enabled) { + bool changed = false; + + if (val != hci_dev_test_flag(hdev, HCI_LE_ENABLED)) { + hci_dev_change_flag(hdev, HCI_LE_ENABLED); + changed = true; + } + + if (!val && hci_dev_test_flag(hdev, HCI_ADVERTISING)) { + hci_dev_clear_flag(hdev, HCI_ADVERTISING); + changed = true; + } + + err = send_settings_rsp(sk, MGMT_OP_SET_LE, hdev); + if (err < 0) + goto unlock; + + if (changed) + err = new_settings(hdev, sk); + + goto unlock; + } + + if (pending_find(MGMT_OP_SET_LE, hdev) || + pending_find(MGMT_OP_SET_ADVERTISING, hdev)) { + err = mgmt_cmd_status(sk, hdev->id, MGMT_OP_SET_LE, + MGMT_STATUS_BUSY); + goto unlock; + } + + cmd = mgmt_pending_add(sk, MGMT_OP_SET_LE, hdev, data, len); + if (!cmd) { + err = -ENOMEM; + goto unlock; + } + + hci_req_init(&req, hdev); + + memset(&hci_cp, 0, sizeof(hci_cp)); + + if (val) { + hci_cp.le = val; + hci_cp.simul = 0x00; + } else { + if (hci_dev_test_flag(hdev, HCI_LE_ADV)) + disable_advertising(&req); + } + + hci_req_add(&req, HCI_OP_WRITE_LE_HOST_SUPPORTED, sizeof(hci_cp), + &hci_cp); + + err = hci_req_run(&req, le_enable_complete); + if (err < 0) + mgmt_pending_remove(cmd); + +unlock: + hci_dev_unlock(hdev); + return err; +} + +/* This is a helper function to test for pending mgmt commands that can + * cause CoD or EIR HCI commands. We can only allow one such pending + * mgmt command at a time since otherwise we cannot easily track what + * the current values are, will be, and based on that calculate if a new + * HCI command needs to be sent and if yes with what value. + */ +static bool pending_eir_or_class(struct hci_dev *hdev) +{ + struct mgmt_pending_cmd *cmd; + + list_for_each_entry(cmd, &hdev->mgmt_pending, list) { + switch (cmd->opcode) { + case MGMT_OP_ADD_UUID: + case MGMT_OP_REMOVE_UUID: + case MGMT_OP_SET_DEV_CLASS: + case MGMT_OP_SET_POWERED: + return true; + } + } + + return false; +} + +static const u8 bluetooth_base_uuid[] = { + 0xfb, 0x34, 0x9b, 0x5f, 0x80, 0x00, 0x00, 0x80, + 0x00, 0x10, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, +}; + +static u8 get_uuid_size(const u8 *uuid) +{ + u32 val; + + if (memcmp(uuid, bluetooth_base_uuid, 12)) + return 128; + + val = get_unaligned_le32(&uuid[12]); + if (val > 0xffff) + return 32; + + return 16; +} + +static void mgmt_class_complete(struct hci_dev *hdev, u16 mgmt_op, u8 status) +{ + struct mgmt_pending_cmd *cmd; + + hci_dev_lock(hdev); + + cmd = pending_find(mgmt_op, hdev); + if (!cmd) + goto unlock; + + mgmt_cmd_complete(cmd->sk, cmd->index, cmd->opcode, + mgmt_status(status), hdev->dev_class, 3); + + mgmt_pending_remove(cmd); + +unlock: + hci_dev_unlock(hdev); +} + +static void add_uuid_complete(struct hci_dev *hdev, u8 status, u16 opcode) +{ + BT_DBG("status 0x%02x", status); + + mgmt_class_complete(hdev, MGMT_OP_ADD_UUID, status); +} + +static int add_uuid(struct sock *sk, struct hci_dev *hdev, void *data, u16 len) +{ + struct mgmt_cp_add_uuid *cp = data; + struct mgmt_pending_cmd *cmd; + struct hci_request req; + struct bt_uuid *uuid; + int err; + + BT_DBG("request for %s", hdev->name); + + hci_dev_lock(hdev); + + if (pending_eir_or_class(hdev)) { + err = mgmt_cmd_status(sk, hdev->id, MGMT_OP_ADD_UUID, + MGMT_STATUS_BUSY); + goto failed; + } + + uuid = kmalloc(sizeof(*uuid), GFP_KERNEL); + if (!uuid) { + err = -ENOMEM; + goto failed; + } + + memcpy(uuid->uuid, cp->uuid, 16); + uuid->svc_hint = cp->svc_hint; + uuid->size = get_uuid_size(cp->uuid); + + list_add_tail(&uuid->list, &hdev->uuids); + + hci_req_init(&req, hdev); + + update_class(&req); + update_eir(&req); + + err = hci_req_run(&req, add_uuid_complete); + if (err < 0) { + if (err != -ENODATA) + goto failed; + + err = mgmt_cmd_complete(sk, hdev->id, MGMT_OP_ADD_UUID, 0, + hdev->dev_class, 3); + goto failed; + } + + cmd = mgmt_pending_add(sk, MGMT_OP_ADD_UUID, hdev, data, len); + if (!cmd) { + err = -ENOMEM; + goto failed; + } + + err = 0; + +failed: + hci_dev_unlock(hdev); + return err; +} + +static bool enable_service_cache(struct hci_dev *hdev) +{ + if (!hdev_is_powered(hdev)) + return false; + + if (!hci_dev_test_and_set_flag(hdev, HCI_SERVICE_CACHE)) { + queue_delayed_work(hdev->workqueue, &hdev->service_cache, + CACHE_TIMEOUT); + return true; + } + + return false; +} + +static void remove_uuid_complete(struct hci_dev *hdev, u8 status, u16 opcode) +{ + BT_DBG("status 0x%02x", status); + + mgmt_class_complete(hdev, MGMT_OP_REMOVE_UUID, status); +} + +static int remove_uuid(struct sock *sk, struct hci_dev *hdev, void *data, + u16 len) +{ + struct mgmt_cp_remove_uuid *cp = data; + struct mgmt_pending_cmd *cmd; + struct bt_uuid *match, *tmp; + u8 bt_uuid_any[] = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 }; + struct hci_request req; + int err, found; + + BT_DBG("request for %s", hdev->name); + + hci_dev_lock(hdev); + + if (pending_eir_or_class(hdev)) { + err = mgmt_cmd_status(sk, hdev->id, MGMT_OP_REMOVE_UUID, + MGMT_STATUS_BUSY); + goto unlock; + } + + if (memcmp(cp->uuid, bt_uuid_any, 16) == 0) { + hci_uuids_clear(hdev); + + if (enable_service_cache(hdev)) { + err = mgmt_cmd_complete(sk, hdev->id, + MGMT_OP_REMOVE_UUID, + 0, hdev->dev_class, 3); + goto unlock; + } + + goto update_class; + } + + found = 0; + + list_for_each_entry_safe(match, tmp, &hdev->uuids, list) { + if (memcmp(match->uuid, cp->uuid, 16) != 0) + continue; + + list_del(&match->list); + kfree(match); + found++; + } + + if (found == 0) { + err = mgmt_cmd_status(sk, hdev->id, MGMT_OP_REMOVE_UUID, + MGMT_STATUS_INVALID_PARAMS); + goto unlock; + } + +update_class: + hci_req_init(&req, hdev); + + update_class(&req); + update_eir(&req); + + err = hci_req_run(&req, remove_uuid_complete); + if (err < 0) { + if (err != -ENODATA) + goto unlock; + + err = mgmt_cmd_complete(sk, hdev->id, MGMT_OP_REMOVE_UUID, 0, + hdev->dev_class, 3); + goto unlock; + } + + cmd = mgmt_pending_add(sk, MGMT_OP_REMOVE_UUID, hdev, data, len); + if (!cmd) { + err = -ENOMEM; + goto unlock; + } + + err = 0; + +unlock: + hci_dev_unlock(hdev); + return err; +} + +static void set_class_complete(struct hci_dev *hdev, u8 status, u16 opcode) +{ + BT_DBG("status 0x%02x", status); + + mgmt_class_complete(hdev, MGMT_OP_SET_DEV_CLASS, status); +} + +static int set_dev_class(struct sock *sk, struct hci_dev *hdev, void *data, + u16 len) +{ + struct mgmt_cp_set_dev_class *cp = data; + struct mgmt_pending_cmd *cmd; + struct hci_request req; + int err; + + BT_DBG("request for %s", hdev->name); + + if (!lmp_bredr_capable(hdev)) + return mgmt_cmd_status(sk, hdev->id, MGMT_OP_SET_DEV_CLASS, + MGMT_STATUS_NOT_SUPPORTED); + + hci_dev_lock(hdev); + + if (pending_eir_or_class(hdev)) { + err = mgmt_cmd_status(sk, hdev->id, MGMT_OP_SET_DEV_CLASS, + MGMT_STATUS_BUSY); + goto unlock; + } + + if ((cp->minor & 0x03) != 0 || (cp->major & 0xe0) != 0) { + err = mgmt_cmd_status(sk, hdev->id, MGMT_OP_SET_DEV_CLASS, + MGMT_STATUS_INVALID_PARAMS); + goto unlock; + } + + hdev->major_class = cp->major; + hdev->minor_class = cp->minor; + + if (!hdev_is_powered(hdev)) { + err = mgmt_cmd_complete(sk, hdev->id, MGMT_OP_SET_DEV_CLASS, 0, + hdev->dev_class, 3); + goto unlock; + } + + hci_req_init(&req, hdev); + + if (hci_dev_test_and_clear_flag(hdev, HCI_SERVICE_CACHE)) { + hci_dev_unlock(hdev); + cancel_delayed_work_sync(&hdev->service_cache); + hci_dev_lock(hdev); + update_eir(&req); + } + + update_class(&req); + + err = hci_req_run(&req, set_class_complete); + if (err < 0) { + if (err != -ENODATA) + goto unlock; + + err = mgmt_cmd_complete(sk, hdev->id, MGMT_OP_SET_DEV_CLASS, 0, + hdev->dev_class, 3); + goto unlock; + } + + cmd = mgmt_pending_add(sk, MGMT_OP_SET_DEV_CLASS, hdev, data, len); + if (!cmd) { + err = -ENOMEM; + goto unlock; + } + + err = 0; + +unlock: + hci_dev_unlock(hdev); + return err; +} + +static int load_link_keys(struct sock *sk, struct hci_dev *hdev, void *data, + u16 len) +{ + struct mgmt_cp_load_link_keys *cp = data; + const u16 max_key_count = ((U16_MAX - sizeof(*cp)) / + sizeof(struct mgmt_link_key_info)); + u16 key_count, expected_len; + bool changed; + int i; + + BT_DBG("request for %s", hdev->name); + + if (!lmp_bredr_capable(hdev)) + return mgmt_cmd_status(sk, hdev->id, MGMT_OP_LOAD_LINK_KEYS, + MGMT_STATUS_NOT_SUPPORTED); + + key_count = __le16_to_cpu(cp->key_count); + if (key_count > max_key_count) { + BT_ERR("load_link_keys: too big key_count value %u", + key_count); + return mgmt_cmd_status(sk, hdev->id, MGMT_OP_LOAD_LINK_KEYS, + MGMT_STATUS_INVALID_PARAMS); + } + + expected_len = sizeof(*cp) + key_count * + sizeof(struct mgmt_link_key_info); + if (expected_len != len) { + BT_ERR("load_link_keys: expected %u bytes, got %u bytes", + expected_len, len); + return mgmt_cmd_status(sk, hdev->id, MGMT_OP_LOAD_LINK_KEYS, + MGMT_STATUS_INVALID_PARAMS); + } + + if (cp->debug_keys != 0x00 && cp->debug_keys != 0x01) + return mgmt_cmd_status(sk, hdev->id, MGMT_OP_LOAD_LINK_KEYS, + MGMT_STATUS_INVALID_PARAMS); + + BT_DBG("%s debug_keys %u key_count %u", hdev->name, cp->debug_keys, + key_count); + + for (i = 0; i < key_count; i++) { + struct mgmt_link_key_info *key = &cp->keys[i]; + + if (key->addr.type != BDADDR_BREDR || key->type > 0x08) + return mgmt_cmd_status(sk, hdev->id, + MGMT_OP_LOAD_LINK_KEYS, + MGMT_STATUS_INVALID_PARAMS); + } + + hci_dev_lock(hdev); + + hci_link_keys_clear(hdev); + + if (cp->debug_keys) + changed = !hci_dev_test_and_set_flag(hdev, HCI_KEEP_DEBUG_KEYS); + else + changed = hci_dev_test_and_clear_flag(hdev, + HCI_KEEP_DEBUG_KEYS); + + if (changed) + new_settings(hdev, NULL); + + for (i = 0; i < key_count; i++) { + struct mgmt_link_key_info *key = &cp->keys[i]; + + /* Always ignore debug keys and require a new pairing if + * the user wants to use them. + */ + if (key->type == HCI_LK_DEBUG_COMBINATION) + continue; + + hci_add_link_key(hdev, NULL, &key->addr.bdaddr, key->val, + key->type, key->pin_len, NULL); + } + + mgmt_cmd_complete(sk, hdev->id, MGMT_OP_LOAD_LINK_KEYS, 0, NULL, 0); + + hci_dev_unlock(hdev); + + return 0; +} + +static int device_unpaired(struct hci_dev *hdev, bdaddr_t *bdaddr, + u8 addr_type, struct sock *skip_sk) +{ + struct mgmt_ev_device_unpaired ev; + + bacpy(&ev.addr.bdaddr, bdaddr); + ev.addr.type = addr_type; + + return mgmt_event(MGMT_EV_DEVICE_UNPAIRED, hdev, &ev, sizeof(ev), + skip_sk); +} + +static int unpair_device(struct sock *sk, struct hci_dev *hdev, void *data, + u16 len) +{ + struct mgmt_cp_unpair_device *cp = data; + struct mgmt_rp_unpair_device rp; + struct hci_cp_disconnect dc; + struct mgmt_pending_cmd *cmd; + struct hci_conn *conn; + int err; + + memset(&rp, 0, sizeof(rp)); + bacpy(&rp.addr.bdaddr, &cp->addr.bdaddr); + rp.addr.type = cp->addr.type; + + if (!bdaddr_type_is_valid(cp->addr.type)) + return mgmt_cmd_complete(sk, hdev->id, MGMT_OP_UNPAIR_DEVICE, + MGMT_STATUS_INVALID_PARAMS, + &rp, sizeof(rp)); + + if (cp->disconnect != 0x00 && cp->disconnect != 0x01) + return mgmt_cmd_complete(sk, hdev->id, MGMT_OP_UNPAIR_DEVICE, + MGMT_STATUS_INVALID_PARAMS, + &rp, sizeof(rp)); + + hci_dev_lock(hdev); + + if (!hdev_is_powered(hdev)) { + err = mgmt_cmd_complete(sk, hdev->id, MGMT_OP_UNPAIR_DEVICE, + MGMT_STATUS_NOT_POWERED, &rp, + sizeof(rp)); + goto unlock; + } + + if (cp->addr.type == BDADDR_BREDR) { + /* If disconnection is requested, then look up the + * connection. If the remote device is connected, it + * will be later used to terminate the link. + * + * Setting it to NULL explicitly will cause no + * termination of the link. + */ + if (cp->disconnect) + conn = hci_conn_hash_lookup_ba(hdev, ACL_LINK, + &cp->addr.bdaddr); + else + conn = NULL; + + err = hci_remove_link_key(hdev, &cp->addr.bdaddr); + } else { + u8 addr_type; + + conn = hci_conn_hash_lookup_ba(hdev, LE_LINK, + &cp->addr.bdaddr); + if (conn) { + /* Defer clearing up the connection parameters + * until closing to give a chance of keeping + * them if a repairing happens. + */ + set_bit(HCI_CONN_PARAM_REMOVAL_PEND, &conn->flags); + + /* If disconnection is not requested, then + * clear the connection variable so that the + * link is not terminated. + */ + if (!cp->disconnect) + conn = NULL; + } + + if (cp->addr.type == BDADDR_LE_PUBLIC) + addr_type = ADDR_LE_DEV_PUBLIC; + else + addr_type = ADDR_LE_DEV_RANDOM; + + hci_remove_irk(hdev, &cp->addr.bdaddr, addr_type); + + err = hci_remove_ltk(hdev, &cp->addr.bdaddr, addr_type); + } + + if (err < 0) { + err = mgmt_cmd_complete(sk, hdev->id, MGMT_OP_UNPAIR_DEVICE, + MGMT_STATUS_NOT_PAIRED, &rp, + sizeof(rp)); + goto unlock; + } + + /* If the connection variable is set, then termination of the + * link is requested. + */ + if (!conn) { + err = mgmt_cmd_complete(sk, hdev->id, MGMT_OP_UNPAIR_DEVICE, 0, + &rp, sizeof(rp)); + device_unpaired(hdev, &cp->addr.bdaddr, cp->addr.type, sk); + goto unlock; + } + + cmd = mgmt_pending_add(sk, MGMT_OP_UNPAIR_DEVICE, hdev, cp, + sizeof(*cp)); + if (!cmd) { + err = -ENOMEM; + goto unlock; + } + + cmd->cmd_complete = addr_cmd_complete; + + dc.handle = cpu_to_le16(conn->handle); + dc.reason = 0x13; /* Remote User Terminated Connection */ + err = hci_send_cmd(hdev, HCI_OP_DISCONNECT, sizeof(dc), &dc); + if (err < 0) + mgmt_pending_remove(cmd); + +unlock: + hci_dev_unlock(hdev); + return err; +} + +static int disconnect(struct sock *sk, struct hci_dev *hdev, void *data, + u16 len) +{ + struct mgmt_cp_disconnect *cp = data; + struct mgmt_rp_disconnect rp; + struct mgmt_pending_cmd *cmd; + struct hci_conn *conn; + int err; + + BT_DBG(""); + + memset(&rp, 0, sizeof(rp)); + bacpy(&rp.addr.bdaddr, &cp->addr.bdaddr); + rp.addr.type = cp->addr.type; + + if (!bdaddr_type_is_valid(cp->addr.type)) + return mgmt_cmd_complete(sk, hdev->id, MGMT_OP_DISCONNECT, + MGMT_STATUS_INVALID_PARAMS, + &rp, sizeof(rp)); + + hci_dev_lock(hdev); + + if (!test_bit(HCI_UP, &hdev->flags)) { + err = mgmt_cmd_complete(sk, hdev->id, MGMT_OP_DISCONNECT, + MGMT_STATUS_NOT_POWERED, &rp, + sizeof(rp)); + goto failed; + } + + if (pending_find(MGMT_OP_DISCONNECT, hdev)) { + err = mgmt_cmd_complete(sk, hdev->id, MGMT_OP_DISCONNECT, + MGMT_STATUS_BUSY, &rp, sizeof(rp)); + goto failed; + } + + if (cp->addr.type == BDADDR_BREDR) + conn = hci_conn_hash_lookup_ba(hdev, ACL_LINK, + &cp->addr.bdaddr); + else + conn = hci_conn_hash_lookup_ba(hdev, LE_LINK, &cp->addr.bdaddr); + + if (!conn || conn->state == BT_OPEN || conn->state == BT_CLOSED) { + err = mgmt_cmd_complete(sk, hdev->id, MGMT_OP_DISCONNECT, + MGMT_STATUS_NOT_CONNECTED, &rp, + sizeof(rp)); + goto failed; + } + + cmd = mgmt_pending_add(sk, MGMT_OP_DISCONNECT, hdev, data, len); + if (!cmd) { + err = -ENOMEM; + goto failed; + } + + cmd->cmd_complete = generic_cmd_complete; + + err = hci_disconnect(conn, HCI_ERROR_REMOTE_USER_TERM); + if (err < 0) + mgmt_pending_remove(cmd); + +failed: + hci_dev_unlock(hdev); + return err; +} + +static u8 link_to_bdaddr(u8 link_type, u8 addr_type) +{ + switch (link_type) { + case LE_LINK: + switch (addr_type) { + case ADDR_LE_DEV_PUBLIC: + return BDADDR_LE_PUBLIC; + + default: + /* Fallback to LE Random address type */ + return BDADDR_LE_RANDOM; + } + + default: + /* Fallback to BR/EDR type */ + return BDADDR_BREDR; + } +} + +static int get_connections(struct sock *sk, struct hci_dev *hdev, void *data, + u16 data_len) +{ + struct mgmt_rp_get_connections *rp; + struct hci_conn *c; + size_t rp_len; + int err; + u16 i; + + BT_DBG(""); + + hci_dev_lock(hdev); + + if (!hdev_is_powered(hdev)) { + err = mgmt_cmd_status(sk, hdev->id, MGMT_OP_GET_CONNECTIONS, + MGMT_STATUS_NOT_POWERED); + goto unlock; + } + + i = 0; + list_for_each_entry(c, &hdev->conn_hash.list, list) { + if (test_bit(HCI_CONN_MGMT_CONNECTED, &c->flags)) + i++; + } + + rp_len = sizeof(*rp) + (i * sizeof(struct mgmt_addr_info)); + rp = kmalloc(rp_len, GFP_KERNEL); + if (!rp) { + err = -ENOMEM; + goto unlock; + } + + i = 0; + list_for_each_entry(c, &hdev->conn_hash.list, list) { + if (!test_bit(HCI_CONN_MGMT_CONNECTED, &c->flags)) + continue; + bacpy(&rp->addr[i].bdaddr, &c->dst); + rp->addr[i].type = link_to_bdaddr(c->type, c->dst_type); + if (c->type == SCO_LINK || c->type == ESCO_LINK) + continue; + i++; + } + + rp->conn_count = cpu_to_le16(i); + + /* Recalculate length in case of filtered SCO connections, etc */ + rp_len = sizeof(*rp) + (i * sizeof(struct mgmt_addr_info)); + + err = mgmt_cmd_complete(sk, hdev->id, MGMT_OP_GET_CONNECTIONS, 0, rp, + rp_len); + + kfree(rp); + +unlock: + hci_dev_unlock(hdev); + return err; +} + +static int send_pin_code_neg_reply(struct sock *sk, struct hci_dev *hdev, + struct mgmt_cp_pin_code_neg_reply *cp) +{ + struct mgmt_pending_cmd *cmd; + int err; + + cmd = mgmt_pending_add(sk, MGMT_OP_PIN_CODE_NEG_REPLY, hdev, cp, + sizeof(*cp)); + if (!cmd) + return -ENOMEM; + + err = hci_send_cmd(hdev, HCI_OP_PIN_CODE_NEG_REPLY, + sizeof(cp->addr.bdaddr), &cp->addr.bdaddr); + if (err < 0) + mgmt_pending_remove(cmd); + + return err; +} + +static int pin_code_reply(struct sock *sk, struct hci_dev *hdev, void *data, + u16 len) +{ + struct hci_conn *conn; + struct mgmt_cp_pin_code_reply *cp = data; + struct hci_cp_pin_code_reply reply; + struct mgmt_pending_cmd *cmd; + int err; + + BT_DBG(""); + + hci_dev_lock(hdev); + + if (!hdev_is_powered(hdev)) { + err = mgmt_cmd_status(sk, hdev->id, MGMT_OP_PIN_CODE_REPLY, + MGMT_STATUS_NOT_POWERED); + goto failed; + } + + conn = hci_conn_hash_lookup_ba(hdev, ACL_LINK, &cp->addr.bdaddr); + if (!conn) { + err = mgmt_cmd_status(sk, hdev->id, MGMT_OP_PIN_CODE_REPLY, + MGMT_STATUS_NOT_CONNECTED); + goto failed; + } + + if (conn->pending_sec_level == BT_SECURITY_HIGH && cp->pin_len != 16) { + struct mgmt_cp_pin_code_neg_reply ncp; + + memcpy(&ncp.addr, &cp->addr, sizeof(ncp.addr)); + + BT_ERR("PIN code is not 16 bytes long"); + + err = send_pin_code_neg_reply(sk, hdev, &ncp); + if (err >= 0) + err = mgmt_cmd_status(sk, hdev->id, MGMT_OP_PIN_CODE_REPLY, + MGMT_STATUS_INVALID_PARAMS); + + goto failed; + } + + cmd = mgmt_pending_add(sk, MGMT_OP_PIN_CODE_REPLY, hdev, data, len); + if (!cmd) { + err = -ENOMEM; + goto failed; + } + + cmd->cmd_complete = addr_cmd_complete; + + bacpy(&reply.bdaddr, &cp->addr.bdaddr); + reply.pin_len = cp->pin_len; + memcpy(reply.pin_code, cp->pin_code, sizeof(reply.pin_code)); + + err = hci_send_cmd(hdev, HCI_OP_PIN_CODE_REPLY, sizeof(reply), &reply); + if (err < 0) + mgmt_pending_remove(cmd); + +failed: + hci_dev_unlock(hdev); + return err; +} + +static int set_io_capability(struct sock *sk, struct hci_dev *hdev, void *data, + u16 len) +{ + struct mgmt_cp_set_io_capability *cp = data; + + BT_DBG(""); + + if (cp->io_capability > SMP_IO_KEYBOARD_DISPLAY) + return mgmt_cmd_complete(sk, hdev->id, MGMT_OP_SET_IO_CAPABILITY, + MGMT_STATUS_INVALID_PARAMS, NULL, 0); + + hci_dev_lock(hdev); + + hdev->io_capability = cp->io_capability; + + BT_DBG("%s IO capability set to 0x%02x", hdev->name, + hdev->io_capability); + + hci_dev_unlock(hdev); + + return mgmt_cmd_complete(sk, hdev->id, MGMT_OP_SET_IO_CAPABILITY, 0, + NULL, 0); +} + +static struct mgmt_pending_cmd *find_pairing(struct hci_conn *conn) +{ + struct hci_dev *hdev = conn->hdev; + struct mgmt_pending_cmd *cmd; + + list_for_each_entry(cmd, &hdev->mgmt_pending, list) { + if (cmd->opcode != MGMT_OP_PAIR_DEVICE) + continue; + + if (cmd->user_data != conn) + continue; + + return cmd; + } + + return NULL; +} + +static int pairing_complete(struct mgmt_pending_cmd *cmd, u8 status) +{ + struct mgmt_rp_pair_device rp; + struct hci_conn *conn = cmd->user_data; + int err; + + bacpy(&rp.addr.bdaddr, &conn->dst); + rp.addr.type = link_to_bdaddr(conn->type, conn->dst_type); + + err = mgmt_cmd_complete(cmd->sk, cmd->index, MGMT_OP_PAIR_DEVICE, + status, &rp, sizeof(rp)); + + /* So we don't get further callbacks for this connection */ + conn->connect_cfm_cb = NULL; + conn->security_cfm_cb = NULL; + conn->disconn_cfm_cb = NULL; + + hci_conn_drop(conn); + + /* The device is paired so there is no need to remove + * its connection parameters anymore. + */ + clear_bit(HCI_CONN_PARAM_REMOVAL_PEND, &conn->flags); + + hci_conn_put(conn); + + return err; +} + +void mgmt_smp_complete(struct hci_conn *conn, bool complete) +{ + u8 status = complete ? MGMT_STATUS_SUCCESS : MGMT_STATUS_FAILED; + struct mgmt_pending_cmd *cmd; + + cmd = find_pairing(conn); + if (cmd) { + cmd->cmd_complete(cmd, status); + mgmt_pending_remove(cmd); + } +} + +static void pairing_complete_cb(struct hci_conn *conn, u8 status) +{ + struct mgmt_pending_cmd *cmd; + + BT_DBG("status %u", status); + + cmd = find_pairing(conn); + if (!cmd) { + BT_DBG("Unable to find a pending command"); + return; + } + + cmd->cmd_complete(cmd, mgmt_status(status)); + mgmt_pending_remove(cmd); +} + +static void le_pairing_complete_cb(struct hci_conn *conn, u8 status) +{ + struct mgmt_pending_cmd *cmd; + + BT_DBG("status %u", status); + + if (!status) + return; + + cmd = find_pairing(conn); + if (!cmd) { + BT_DBG("Unable to find a pending command"); + return; + } + + cmd->cmd_complete(cmd, mgmt_status(status)); + mgmt_pending_remove(cmd); +} + +static int pair_device(struct sock *sk, struct hci_dev *hdev, void *data, + u16 len) +{ + struct mgmt_cp_pair_device *cp = data; + struct mgmt_rp_pair_device rp; + struct mgmt_pending_cmd *cmd; + u8 sec_level, auth_type; + struct hci_conn *conn; + int err; + + BT_DBG(""); + + memset(&rp, 0, sizeof(rp)); + bacpy(&rp.addr.bdaddr, &cp->addr.bdaddr); + rp.addr.type = cp->addr.type; + + if (!bdaddr_type_is_valid(cp->addr.type)) + return mgmt_cmd_complete(sk, hdev->id, MGMT_OP_PAIR_DEVICE, + MGMT_STATUS_INVALID_PARAMS, + &rp, sizeof(rp)); + + if (cp->io_cap > SMP_IO_KEYBOARD_DISPLAY) + return mgmt_cmd_complete(sk, hdev->id, MGMT_OP_PAIR_DEVICE, + MGMT_STATUS_INVALID_PARAMS, + &rp, sizeof(rp)); + + hci_dev_lock(hdev); + + if (!hdev_is_powered(hdev)) { + err = mgmt_cmd_complete(sk, hdev->id, MGMT_OP_PAIR_DEVICE, + MGMT_STATUS_NOT_POWERED, &rp, + sizeof(rp)); + goto unlock; + } + + if (hci_bdaddr_is_paired(hdev, &cp->addr.bdaddr, cp->addr.type)) { + err = mgmt_cmd_complete(sk, hdev->id, MGMT_OP_PAIR_DEVICE, + MGMT_STATUS_ALREADY_PAIRED, &rp, + sizeof(rp)); + goto unlock; + } + + sec_level = BT_SECURITY_MEDIUM; + auth_type = HCI_AT_DEDICATED_BONDING; + + if (cp->addr.type == BDADDR_BREDR) { + conn = hci_connect_acl(hdev, &cp->addr.bdaddr, sec_level, + auth_type); + } else { + u8 addr_type; + + /* Convert from L2CAP channel address type to HCI address type + */ + if (cp->addr.type == BDADDR_LE_PUBLIC) + addr_type = ADDR_LE_DEV_PUBLIC; + else + addr_type = ADDR_LE_DEV_RANDOM; + + /* When pairing a new device, it is expected to remember + * this device for future connections. Adding the connection + * parameter information ahead of time allows tracking + * of the slave preferred values and will speed up any + * further connection establishment. + * + * If connection parameters already exist, then they + * will be kept and this function does nothing. + */ + hci_conn_params_add(hdev, &cp->addr.bdaddr, addr_type); + + conn = hci_connect_le(hdev, &cp->addr.bdaddr, addr_type, + sec_level, HCI_LE_CONN_TIMEOUT, + HCI_ROLE_MASTER); + } + + if (IS_ERR(conn)) { + int status; + + if (PTR_ERR(conn) == -EBUSY) + status = MGMT_STATUS_BUSY; + else if (PTR_ERR(conn) == -EOPNOTSUPP) + status = MGMT_STATUS_NOT_SUPPORTED; + else if (PTR_ERR(conn) == -ECONNREFUSED) + status = MGMT_STATUS_REJECTED; + else + status = MGMT_STATUS_CONNECT_FAILED; + + err = mgmt_cmd_complete(sk, hdev->id, MGMT_OP_PAIR_DEVICE, + status, &rp, sizeof(rp)); + goto unlock; + } + + if (conn->connect_cfm_cb) { + hci_conn_drop(conn); + err = mgmt_cmd_complete(sk, hdev->id, MGMT_OP_PAIR_DEVICE, + MGMT_STATUS_BUSY, &rp, sizeof(rp)); + goto unlock; + } + + cmd = mgmt_pending_add(sk, MGMT_OP_PAIR_DEVICE, hdev, data, len); + if (!cmd) { + err = -ENOMEM; + hci_conn_drop(conn); + goto unlock; + } + + cmd->cmd_complete = pairing_complete; + + /* For LE, just connecting isn't a proof that the pairing finished */ + if (cp->addr.type == BDADDR_BREDR) { + conn->connect_cfm_cb = pairing_complete_cb; + conn->security_cfm_cb = pairing_complete_cb; + conn->disconn_cfm_cb = pairing_complete_cb; + } else { + conn->connect_cfm_cb = le_pairing_complete_cb; + conn->security_cfm_cb = le_pairing_complete_cb; + conn->disconn_cfm_cb = le_pairing_complete_cb; + } + + conn->io_capability = cp->io_cap; + cmd->user_data = hci_conn_get(conn); + + if ((conn->state == BT_CONNECTED || conn->state == BT_CONFIG) && + hci_conn_security(conn, sec_level, auth_type, true)) { + cmd->cmd_complete(cmd, 0); + mgmt_pending_remove(cmd); + } + + err = 0; + +unlock: + hci_dev_unlock(hdev); + return err; +} + +static int cancel_pair_device(struct sock *sk, struct hci_dev *hdev, void *data, + u16 len) +{ + struct mgmt_addr_info *addr = data; + struct mgmt_pending_cmd *cmd; + struct hci_conn *conn; + int err; + + BT_DBG(""); + + hci_dev_lock(hdev); + + if (!hdev_is_powered(hdev)) { + err = mgmt_cmd_status(sk, hdev->id, MGMT_OP_CANCEL_PAIR_DEVICE, + MGMT_STATUS_NOT_POWERED); + goto unlock; + } + + cmd = pending_find(MGMT_OP_PAIR_DEVICE, hdev); + if (!cmd) { + err = mgmt_cmd_status(sk, hdev->id, MGMT_OP_CANCEL_PAIR_DEVICE, + MGMT_STATUS_INVALID_PARAMS); + goto unlock; + } + + conn = cmd->user_data; + + if (bacmp(&addr->bdaddr, &conn->dst) != 0) { + err = mgmt_cmd_status(sk, hdev->id, MGMT_OP_CANCEL_PAIR_DEVICE, + MGMT_STATUS_INVALID_PARAMS); + goto unlock; + } + + cmd->cmd_complete(cmd, MGMT_STATUS_CANCELLED); + mgmt_pending_remove(cmd); + + err = mgmt_cmd_complete(sk, hdev->id, MGMT_OP_CANCEL_PAIR_DEVICE, 0, + addr, sizeof(*addr)); +unlock: + hci_dev_unlock(hdev); + return err; +} + +static int user_pairing_resp(struct sock *sk, struct hci_dev *hdev, + struct mgmt_addr_info *addr, u16 mgmt_op, + u16 hci_op, __le32 passkey) +{ + struct mgmt_pending_cmd *cmd; + struct hci_conn *conn; + int err; + + hci_dev_lock(hdev); + + if (!hdev_is_powered(hdev)) { + err = mgmt_cmd_complete(sk, hdev->id, mgmt_op, + MGMT_STATUS_NOT_POWERED, addr, + sizeof(*addr)); + goto done; + } + + if (addr->type == BDADDR_BREDR) + conn = hci_conn_hash_lookup_ba(hdev, ACL_LINK, &addr->bdaddr); + else + conn = hci_conn_hash_lookup_ba(hdev, LE_LINK, &addr->bdaddr); + + if (!conn) { + err = mgmt_cmd_complete(sk, hdev->id, mgmt_op, + MGMT_STATUS_NOT_CONNECTED, addr, + sizeof(*addr)); + goto done; + } + + if (addr->type == BDADDR_LE_PUBLIC || addr->type == BDADDR_LE_RANDOM) { + err = smp_user_confirm_reply(conn, mgmt_op, passkey); + if (!err) + err = mgmt_cmd_complete(sk, hdev->id, mgmt_op, + MGMT_STATUS_SUCCESS, addr, + sizeof(*addr)); + else + err = mgmt_cmd_complete(sk, hdev->id, mgmt_op, + MGMT_STATUS_FAILED, addr, + sizeof(*addr)); + + goto done; + } + + cmd = mgmt_pending_add(sk, mgmt_op, hdev, addr, sizeof(*addr)); + if (!cmd) { + err = -ENOMEM; + goto done; + } + + cmd->cmd_complete = addr_cmd_complete; + + /* Continue with pairing via HCI */ + if (hci_op == HCI_OP_USER_PASSKEY_REPLY) { + struct hci_cp_user_passkey_reply cp; + + bacpy(&cp.bdaddr, &addr->bdaddr); + cp.passkey = passkey; + err = hci_send_cmd(hdev, hci_op, sizeof(cp), &cp); + } else + err = hci_send_cmd(hdev, hci_op, sizeof(addr->bdaddr), + &addr->bdaddr); + + if (err < 0) + mgmt_pending_remove(cmd); + +done: + hci_dev_unlock(hdev); + return err; +} + +static int pin_code_neg_reply(struct sock *sk, struct hci_dev *hdev, + void *data, u16 len) +{ + struct mgmt_cp_pin_code_neg_reply *cp = data; + + BT_DBG(""); + + return user_pairing_resp(sk, hdev, &cp->addr, + MGMT_OP_PIN_CODE_NEG_REPLY, + HCI_OP_PIN_CODE_NEG_REPLY, 0); +} + +static int user_confirm_reply(struct sock *sk, struct hci_dev *hdev, void *data, + u16 len) +{ + struct mgmt_cp_user_confirm_reply *cp = data; + + BT_DBG(""); + + if (len != sizeof(*cp)) + return mgmt_cmd_status(sk, hdev->id, MGMT_OP_USER_CONFIRM_REPLY, + MGMT_STATUS_INVALID_PARAMS); + + return user_pairing_resp(sk, hdev, &cp->addr, + MGMT_OP_USER_CONFIRM_REPLY, + HCI_OP_USER_CONFIRM_REPLY, 0); +} + +static int user_confirm_neg_reply(struct sock *sk, struct hci_dev *hdev, + void *data, u16 len) +{ + struct mgmt_cp_user_confirm_neg_reply *cp = data; + + BT_DBG(""); + + return user_pairing_resp(sk, hdev, &cp->addr, + MGMT_OP_USER_CONFIRM_NEG_REPLY, + HCI_OP_USER_CONFIRM_NEG_REPLY, 0); +} + +static int user_passkey_reply(struct sock *sk, struct hci_dev *hdev, void *data, + u16 len) +{ + struct mgmt_cp_user_passkey_reply *cp = data; + + BT_DBG(""); + + return user_pairing_resp(sk, hdev, &cp->addr, + MGMT_OP_USER_PASSKEY_REPLY, + HCI_OP_USER_PASSKEY_REPLY, cp->passkey); +} + +static int user_passkey_neg_reply(struct sock *sk, struct hci_dev *hdev, + void *data, u16 len) +{ + struct mgmt_cp_user_passkey_neg_reply *cp = data; + + BT_DBG(""); + + return user_pairing_resp(sk, hdev, &cp->addr, + MGMT_OP_USER_PASSKEY_NEG_REPLY, + HCI_OP_USER_PASSKEY_NEG_REPLY, 0); +} + +static void update_name(struct hci_request *req) +{ + struct hci_dev *hdev = req->hdev; + struct hci_cp_write_local_name cp; + + memcpy(cp.name, hdev->dev_name, sizeof(cp.name)); + + hci_req_add(req, HCI_OP_WRITE_LOCAL_NAME, sizeof(cp), &cp); +} + +static void set_name_complete(struct hci_dev *hdev, u8 status, u16 opcode) +{ + struct mgmt_cp_set_local_name *cp; + struct mgmt_pending_cmd *cmd; + + BT_DBG("status 0x%02x", status); + + hci_dev_lock(hdev); + + cmd = pending_find(MGMT_OP_SET_LOCAL_NAME, hdev); + if (!cmd) + goto unlock; + + cp = cmd->param; + + if (status) + mgmt_cmd_status(cmd->sk, hdev->id, MGMT_OP_SET_LOCAL_NAME, + mgmt_status(status)); + else + mgmt_cmd_complete(cmd->sk, hdev->id, MGMT_OP_SET_LOCAL_NAME, 0, + cp, sizeof(*cp)); + + mgmt_pending_remove(cmd); + +unlock: + hci_dev_unlock(hdev); +} + +static int set_local_name(struct sock *sk, struct hci_dev *hdev, void *data, + u16 len) +{ + struct mgmt_cp_set_local_name *cp = data; + struct mgmt_pending_cmd *cmd; + struct hci_request req; + int err; + + BT_DBG(""); + + hci_dev_lock(hdev); + + /* If the old values are the same as the new ones just return a + * direct command complete event. + */ + if (!memcmp(hdev->dev_name, cp->name, sizeof(hdev->dev_name)) && + !memcmp(hdev->short_name, cp->short_name, + sizeof(hdev->short_name))) { + err = mgmt_cmd_complete(sk, hdev->id, MGMT_OP_SET_LOCAL_NAME, 0, + data, len); + goto failed; + } + + memcpy(hdev->short_name, cp->short_name, sizeof(hdev->short_name)); + + if (!hdev_is_powered(hdev)) { + memcpy(hdev->dev_name, cp->name, sizeof(hdev->dev_name)); + + err = mgmt_cmd_complete(sk, hdev->id, MGMT_OP_SET_LOCAL_NAME, 0, + data, len); + if (err < 0) + goto failed; + + err = mgmt_generic_event(MGMT_EV_LOCAL_NAME_CHANGED, hdev, + data, len, sk); + + goto failed; + } + + cmd = mgmt_pending_add(sk, MGMT_OP_SET_LOCAL_NAME, hdev, data, len); + if (!cmd) { + err = -ENOMEM; + goto failed; + } + + memcpy(hdev->dev_name, cp->name, sizeof(hdev->dev_name)); + + hci_req_init(&req, hdev); + + if (lmp_bredr_capable(hdev)) { + update_name(&req); + update_eir(&req); + } + + /* The name is stored in the scan response data and so + * no need to udpate the advertising data here. + */ + if (lmp_le_capable(hdev)) + update_scan_rsp_data(&req); + + err = hci_req_run(&req, set_name_complete); + if (err < 0) + mgmt_pending_remove(cmd); + +failed: + hci_dev_unlock(hdev); + return err; +} + +static void read_local_oob_data_complete(struct hci_dev *hdev, u8 status, + u16 opcode, struct sk_buff *skb) +{ + struct mgmt_rp_read_local_oob_data mgmt_rp; + size_t rp_size = sizeof(mgmt_rp); + struct mgmt_pending_cmd *cmd; + + BT_DBG("%s status %u", hdev->name, status); + + cmd = pending_find(MGMT_OP_READ_LOCAL_OOB_DATA, hdev); + if (!cmd) + return; + + if (status || !skb) { + mgmt_cmd_status(cmd->sk, hdev->id, MGMT_OP_READ_LOCAL_OOB_DATA, + status ? mgmt_status(status) : MGMT_STATUS_FAILED); + goto remove; + } + + memset(&mgmt_rp, 0, sizeof(mgmt_rp)); + + if (opcode == HCI_OP_READ_LOCAL_OOB_DATA) { + struct hci_rp_read_local_oob_data *rp = (void *) skb->data; + + if (skb->len < sizeof(*rp)) { + mgmt_cmd_status(cmd->sk, hdev->id, + MGMT_OP_READ_LOCAL_OOB_DATA, + MGMT_STATUS_FAILED); + goto remove; + } + + memcpy(mgmt_rp.hash192, rp->hash, sizeof(rp->hash)); + memcpy(mgmt_rp.rand192, rp->rand, sizeof(rp->rand)); + + rp_size -= sizeof(mgmt_rp.hash256) + sizeof(mgmt_rp.rand256); + } else { + struct hci_rp_read_local_oob_ext_data *rp = (void *) skb->data; + + if (skb->len < sizeof(*rp)) { + mgmt_cmd_status(cmd->sk, hdev->id, + MGMT_OP_READ_LOCAL_OOB_DATA, + MGMT_STATUS_FAILED); + goto remove; + } + + memcpy(mgmt_rp.hash192, rp->hash192, sizeof(rp->hash192)); + memcpy(mgmt_rp.rand192, rp->rand192, sizeof(rp->rand192)); + + memcpy(mgmt_rp.hash256, rp->hash256, sizeof(rp->hash256)); + memcpy(mgmt_rp.rand256, rp->rand256, sizeof(rp->rand256)); + } + + mgmt_cmd_complete(cmd->sk, hdev->id, MGMT_OP_READ_LOCAL_OOB_DATA, + MGMT_STATUS_SUCCESS, &mgmt_rp, rp_size); + +remove: + mgmt_pending_remove(cmd); +} + +static int read_local_oob_data(struct sock *sk, struct hci_dev *hdev, + void *data, u16 data_len) +{ + struct mgmt_pending_cmd *cmd; + struct hci_request req; + int err; + + BT_DBG("%s", hdev->name); + + hci_dev_lock(hdev); + + if (!hdev_is_powered(hdev)) { + err = mgmt_cmd_status(sk, hdev->id, MGMT_OP_READ_LOCAL_OOB_DATA, + MGMT_STATUS_NOT_POWERED); + goto unlock; + } + + if (!lmp_ssp_capable(hdev)) { + err = mgmt_cmd_status(sk, hdev->id, MGMT_OP_READ_LOCAL_OOB_DATA, + MGMT_STATUS_NOT_SUPPORTED); + goto unlock; + } + + if (pending_find(MGMT_OP_READ_LOCAL_OOB_DATA, hdev)) { + err = mgmt_cmd_status(sk, hdev->id, MGMT_OP_READ_LOCAL_OOB_DATA, + MGMT_STATUS_BUSY); + goto unlock; + } + + cmd = mgmt_pending_add(sk, MGMT_OP_READ_LOCAL_OOB_DATA, hdev, NULL, 0); + if (!cmd) { + err = -ENOMEM; + goto unlock; + } + + hci_req_init(&req, hdev); + + if (bredr_sc_enabled(hdev)) + hci_req_add(&req, HCI_OP_READ_LOCAL_OOB_EXT_DATA, 0, NULL); + else + hci_req_add(&req, HCI_OP_READ_LOCAL_OOB_DATA, 0, NULL); + + err = hci_req_run_skb(&req, read_local_oob_data_complete); + if (err < 0) + mgmt_pending_remove(cmd); + +unlock: + hci_dev_unlock(hdev); + return err; +} + +static int add_remote_oob_data(struct sock *sk, struct hci_dev *hdev, + void *data, u16 len) +{ + struct mgmt_addr_info *addr = data; + int err; + + BT_DBG("%s ", hdev->name); + + if (!bdaddr_type_is_valid(addr->type)) + return mgmt_cmd_complete(sk, hdev->id, + MGMT_OP_ADD_REMOTE_OOB_DATA, + MGMT_STATUS_INVALID_PARAMS, + addr, sizeof(*addr)); + + hci_dev_lock(hdev); + + if (len == MGMT_ADD_REMOTE_OOB_DATA_SIZE) { + struct mgmt_cp_add_remote_oob_data *cp = data; + u8 status; + + if (cp->addr.type != BDADDR_BREDR) { + err = mgmt_cmd_complete(sk, hdev->id, + MGMT_OP_ADD_REMOTE_OOB_DATA, + MGMT_STATUS_INVALID_PARAMS, + &cp->addr, sizeof(cp->addr)); + goto unlock; + } + + err = hci_add_remote_oob_data(hdev, &cp->addr.bdaddr, + cp->addr.type, cp->hash, + cp->rand, NULL, NULL); + if (err < 0) + status = MGMT_STATUS_FAILED; + else + status = MGMT_STATUS_SUCCESS; + + err = mgmt_cmd_complete(sk, hdev->id, + MGMT_OP_ADD_REMOTE_OOB_DATA, status, + &cp->addr, sizeof(cp->addr)); + } else if (len == MGMT_ADD_REMOTE_OOB_EXT_DATA_SIZE) { + struct mgmt_cp_add_remote_oob_ext_data *cp = data; + u8 *rand192, *hash192, *rand256, *hash256; + u8 status; + + if (bdaddr_type_is_le(cp->addr.type)) { + /* Enforce zero-valued 192-bit parameters as + * long as legacy SMP OOB isn't implemented. + */ + if (memcmp(cp->rand192, ZERO_KEY, 16) || + memcmp(cp->hash192, ZERO_KEY, 16)) { + err = mgmt_cmd_complete(sk, hdev->id, + MGMT_OP_ADD_REMOTE_OOB_DATA, + MGMT_STATUS_INVALID_PARAMS, + addr, sizeof(*addr)); + goto unlock; + } + + rand192 = NULL; + hash192 = NULL; + } else { + /* In case one of the P-192 values is set to zero, + * then just disable OOB data for P-192. + */ + if (!memcmp(cp->rand192, ZERO_KEY, 16) || + !memcmp(cp->hash192, ZERO_KEY, 16)) { + rand192 = NULL; + hash192 = NULL; + } else { + rand192 = cp->rand192; + hash192 = cp->hash192; + } + } + + /* In case one of the P-256 values is set to zero, then just + * disable OOB data for P-256. + */ + if (!memcmp(cp->rand256, ZERO_KEY, 16) || + !memcmp(cp->hash256, ZERO_KEY, 16)) { + rand256 = NULL; + hash256 = NULL; + } else { + rand256 = cp->rand256; + hash256 = cp->hash256; + } + + err = hci_add_remote_oob_data(hdev, &cp->addr.bdaddr, + cp->addr.type, hash192, rand192, + hash256, rand256); + if (err < 0) + status = MGMT_STATUS_FAILED; + else + status = MGMT_STATUS_SUCCESS; + + err = mgmt_cmd_complete(sk, hdev->id, + MGMT_OP_ADD_REMOTE_OOB_DATA, + status, &cp->addr, sizeof(cp->addr)); + } else { + BT_ERR("add_remote_oob_data: invalid length of %u bytes", len); + err = mgmt_cmd_status(sk, hdev->id, MGMT_OP_ADD_REMOTE_OOB_DATA, + MGMT_STATUS_INVALID_PARAMS); + } + +unlock: + hci_dev_unlock(hdev); + return err; +} + +static int remove_remote_oob_data(struct sock *sk, struct hci_dev *hdev, + void *data, u16 len) +{ + struct mgmt_cp_remove_remote_oob_data *cp = data; + u8 status; + int err; + + BT_DBG("%s", hdev->name); + + if (cp->addr.type != BDADDR_BREDR) + return mgmt_cmd_complete(sk, hdev->id, + MGMT_OP_REMOVE_REMOTE_OOB_DATA, + MGMT_STATUS_INVALID_PARAMS, + &cp->addr, sizeof(cp->addr)); + + hci_dev_lock(hdev); + + if (!bacmp(&cp->addr.bdaddr, BDADDR_ANY)) { + hci_remote_oob_data_clear(hdev); + status = MGMT_STATUS_SUCCESS; + goto done; + } + + err = hci_remove_remote_oob_data(hdev, &cp->addr.bdaddr, cp->addr.type); + if (err < 0) + status = MGMT_STATUS_INVALID_PARAMS; + else + status = MGMT_STATUS_SUCCESS; + +done: + err = mgmt_cmd_complete(sk, hdev->id, MGMT_OP_REMOVE_REMOTE_OOB_DATA, + status, &cp->addr, sizeof(cp->addr)); + + hci_dev_unlock(hdev); + return err; +} + +static bool trigger_bredr_inquiry(struct hci_request *req, u8 *status) +{ + struct hci_dev *hdev = req->hdev; + struct hci_cp_inquiry cp; + /* General inquiry access code (GIAC) */ + u8 lap[3] = { 0x33, 0x8b, 0x9e }; + + *status = mgmt_bredr_support(hdev); + if (*status) + return false; + + if (hci_dev_test_flag(hdev, HCI_INQUIRY)) { + *status = MGMT_STATUS_BUSY; + return false; + } + + hci_inquiry_cache_flush(hdev); + + memset(&cp, 0, sizeof(cp)); + memcpy(&cp.lap, lap, sizeof(cp.lap)); + cp.length = DISCOV_BREDR_INQUIRY_LEN; + + hci_req_add(req, HCI_OP_INQUIRY, sizeof(cp), &cp); + + return true; +} + +static bool trigger_le_scan(struct hci_request *req, u16 interval, u8 *status) +{ + struct hci_dev *hdev = req->hdev; + struct hci_cp_le_set_scan_param param_cp; + struct hci_cp_le_set_scan_enable enable_cp; + u8 own_addr_type; + int err; + + *status = mgmt_le_support(hdev); + if (*status) + return false; + + if (hci_dev_test_flag(hdev, HCI_LE_ADV)) { + /* Don't let discovery abort an outgoing connection attempt + * that's using directed advertising. + */ + if (hci_conn_hash_lookup_state(hdev, LE_LINK, BT_CONNECT)) { + *status = MGMT_STATUS_REJECTED; + return false; + } + + disable_advertising(req); + } + + /* If controller is scanning, it means the background scanning is + * running. Thus, we should temporarily stop it in order to set the + * discovery scanning parameters. + */ + if (hci_dev_test_flag(hdev, HCI_LE_SCAN)) + hci_req_add_le_scan_disable(req); + + /* All active scans will be done with either a resolvable private + * address (when privacy feature has been enabled) or non-resolvable + * private address. + */ + err = hci_update_random_address(req, true, &own_addr_type); + if (err < 0) { + *status = MGMT_STATUS_FAILED; + return false; + } + + memset(¶m_cp, 0, sizeof(param_cp)); + param_cp.type = LE_SCAN_ACTIVE; + param_cp.interval = cpu_to_le16(interval); + param_cp.window = cpu_to_le16(DISCOV_LE_SCAN_WIN); + param_cp.own_address_type = own_addr_type; + + hci_req_add(req, HCI_OP_LE_SET_SCAN_PARAM, sizeof(param_cp), + ¶m_cp); + + memset(&enable_cp, 0, sizeof(enable_cp)); + enable_cp.enable = LE_SCAN_ENABLE; + enable_cp.filter_dup = LE_SCAN_FILTER_DUP_ENABLE; + + hci_req_add(req, HCI_OP_LE_SET_SCAN_ENABLE, sizeof(enable_cp), + &enable_cp); + + return true; +} + +static bool trigger_discovery(struct hci_request *req, u8 *status) +{ + struct hci_dev *hdev = req->hdev; + + switch (hdev->discovery.type) { + case DISCOV_TYPE_BREDR: + if (!trigger_bredr_inquiry(req, status)) + return false; + break; + + case DISCOV_TYPE_INTERLEAVED: + if (test_bit(HCI_QUIRK_SIMULTANEOUS_DISCOVERY, + &hdev->quirks)) { + /* During simultaneous discovery, we double LE scan + * interval. We must leave some time for the controller + * to do BR/EDR inquiry. + */ + if (!trigger_le_scan(req, DISCOV_LE_SCAN_INT * 2, + status)) + return false; + + if (!trigger_bredr_inquiry(req, status)) + return false; + + return true; + } + + if (!hci_dev_test_flag(hdev, HCI_BREDR_ENABLED)) { + *status = MGMT_STATUS_NOT_SUPPORTED; + return false; + } + /* fall through */ + + case DISCOV_TYPE_LE: + if (!trigger_le_scan(req, DISCOV_LE_SCAN_INT, status)) + return false; + break; + + default: + *status = MGMT_STATUS_INVALID_PARAMS; + return false; + } + + return true; +} + +static void start_discovery_complete(struct hci_dev *hdev, u8 status, + u16 opcode) +{ + struct mgmt_pending_cmd *cmd; + unsigned long timeout; + + BT_DBG("status %d", status); + + hci_dev_lock(hdev); + + cmd = pending_find(MGMT_OP_START_DISCOVERY, hdev); + if (!cmd) + cmd = pending_find(MGMT_OP_START_SERVICE_DISCOVERY, hdev); + + if (cmd) { + cmd->cmd_complete(cmd, mgmt_status(status)); + mgmt_pending_remove(cmd); + } + + if (status) { + hci_discovery_set_state(hdev, DISCOVERY_STOPPED); + goto unlock; + } + + hci_discovery_set_state(hdev, DISCOVERY_FINDING); + + /* If the scan involves LE scan, pick proper timeout to schedule + * hdev->le_scan_disable that will stop it. + */ + switch (hdev->discovery.type) { + case DISCOV_TYPE_LE: + timeout = msecs_to_jiffies(DISCOV_LE_TIMEOUT); + break; + case DISCOV_TYPE_INTERLEAVED: + /* When running simultaneous discovery, the LE scanning time + * should occupy the whole discovery time sine BR/EDR inquiry + * and LE scanning are scheduled by the controller. + * + * For interleaving discovery in comparison, BR/EDR inquiry + * and LE scanning are done sequentially with separate + * timeouts. + */ + if (test_bit(HCI_QUIRK_SIMULTANEOUS_DISCOVERY, &hdev->quirks)) + timeout = msecs_to_jiffies(DISCOV_LE_TIMEOUT); + else + timeout = msecs_to_jiffies(hdev->discov_interleaved_timeout); + break; + case DISCOV_TYPE_BREDR: + timeout = 0; + break; + default: + BT_ERR("Invalid discovery type %d", hdev->discovery.type); + timeout = 0; + break; + } + + if (timeout) { + /* When service discovery is used and the controller has + * a strict duplicate filter, it is important to remember + * the start and duration of the scan. This is required + * for restarting scanning during the discovery phase. + */ + if (test_bit(HCI_QUIRK_STRICT_DUPLICATE_FILTER, + &hdev->quirks) && + hdev->discovery.result_filtering) { + hdev->discovery.scan_start = jiffies; + hdev->discovery.scan_duration = timeout; + } + + queue_delayed_work(hdev->workqueue, + &hdev->le_scan_disable, timeout); + } + +unlock: + hci_dev_unlock(hdev); +} + +static int start_discovery(struct sock *sk, struct hci_dev *hdev, + void *data, u16 len) +{ + struct mgmt_cp_start_discovery *cp = data; + struct mgmt_pending_cmd *cmd; + struct hci_request req; + u8 status; + int err; + + BT_DBG("%s", hdev->name); + + hci_dev_lock(hdev); + + if (!hdev_is_powered(hdev)) { + err = mgmt_cmd_complete(sk, hdev->id, MGMT_OP_START_DISCOVERY, + MGMT_STATUS_NOT_POWERED, + &cp->type, sizeof(cp->type)); + goto failed; + } + + if (hdev->discovery.state != DISCOVERY_STOPPED || + hci_dev_test_flag(hdev, HCI_PERIODIC_INQ)) { + err = mgmt_cmd_complete(sk, hdev->id, MGMT_OP_START_DISCOVERY, + MGMT_STATUS_BUSY, &cp->type, + sizeof(cp->type)); + goto failed; + } + + cmd = mgmt_pending_add(sk, MGMT_OP_START_DISCOVERY, hdev, data, len); + if (!cmd) { + err = -ENOMEM; + goto failed; + } + + cmd->cmd_complete = generic_cmd_complete; + + /* Clear the discovery filter first to free any previously + * allocated memory for the UUID list. + */ + hci_discovery_filter_clear(hdev); + + hdev->discovery.type = cp->type; + hdev->discovery.report_invalid_rssi = false; + + hci_req_init(&req, hdev); + + if (!trigger_discovery(&req, &status)) { + err = mgmt_cmd_complete(sk, hdev->id, MGMT_OP_START_DISCOVERY, + status, &cp->type, sizeof(cp->type)); + mgmt_pending_remove(cmd); + goto failed; + } + + err = hci_req_run(&req, start_discovery_complete); + if (err < 0) { + mgmt_pending_remove(cmd); + goto failed; + } + + hci_discovery_set_state(hdev, DISCOVERY_STARTING); + +failed: + hci_dev_unlock(hdev); + return err; +} + +static int service_discovery_cmd_complete(struct mgmt_pending_cmd *cmd, + u8 status) +{ + return mgmt_cmd_complete(cmd->sk, cmd->index, cmd->opcode, status, + cmd->param, 1); +} + +static int start_service_discovery(struct sock *sk, struct hci_dev *hdev, + void *data, u16 len) +{ + struct mgmt_cp_start_service_discovery *cp = data; + struct mgmt_pending_cmd *cmd; + struct hci_request req; + const u16 max_uuid_count = ((U16_MAX - sizeof(*cp)) / 16); + u16 uuid_count, expected_len; + u8 status; + int err; + + BT_DBG("%s", hdev->name); + + hci_dev_lock(hdev); + + if (!hdev_is_powered(hdev)) { + err = mgmt_cmd_complete(sk, hdev->id, + MGMT_OP_START_SERVICE_DISCOVERY, + MGMT_STATUS_NOT_POWERED, + &cp->type, sizeof(cp->type)); + goto failed; + } + + if (hdev->discovery.state != DISCOVERY_STOPPED || + hci_dev_test_flag(hdev, HCI_PERIODIC_INQ)) { + err = mgmt_cmd_complete(sk, hdev->id, + MGMT_OP_START_SERVICE_DISCOVERY, + MGMT_STATUS_BUSY, &cp->type, + sizeof(cp->type)); + goto failed; + } + + uuid_count = __le16_to_cpu(cp->uuid_count); + if (uuid_count > max_uuid_count) { + BT_ERR("service_discovery: too big uuid_count value %u", + uuid_count); + err = mgmt_cmd_complete(sk, hdev->id, + MGMT_OP_START_SERVICE_DISCOVERY, + MGMT_STATUS_INVALID_PARAMS, &cp->type, + sizeof(cp->type)); + goto failed; + } + + expected_len = sizeof(*cp) + uuid_count * 16; + if (expected_len != len) { + BT_ERR("service_discovery: expected %u bytes, got %u bytes", + expected_len, len); + err = mgmt_cmd_complete(sk, hdev->id, + MGMT_OP_START_SERVICE_DISCOVERY, + MGMT_STATUS_INVALID_PARAMS, &cp->type, + sizeof(cp->type)); + goto failed; + } + + cmd = mgmt_pending_add(sk, MGMT_OP_START_SERVICE_DISCOVERY, + hdev, data, len); + if (!cmd) { + err = -ENOMEM; + goto failed; + } + + cmd->cmd_complete = service_discovery_cmd_complete; + + /* Clear the discovery filter first to free any previously + * allocated memory for the UUID list. + */ + hci_discovery_filter_clear(hdev); + + hdev->discovery.result_filtering = true; + hdev->discovery.type = cp->type; + hdev->discovery.rssi = cp->rssi; + hdev->discovery.uuid_count = uuid_count; + + if (uuid_count > 0) { + hdev->discovery.uuids = kmemdup(cp->uuids, uuid_count * 16, + GFP_KERNEL); + if (!hdev->discovery.uuids) { + err = mgmt_cmd_complete(sk, hdev->id, + MGMT_OP_START_SERVICE_DISCOVERY, + MGMT_STATUS_FAILED, + &cp->type, sizeof(cp->type)); + mgmt_pending_remove(cmd); + goto failed; + } + } + + hci_req_init(&req, hdev); + + if (!trigger_discovery(&req, &status)) { + err = mgmt_cmd_complete(sk, hdev->id, + MGMT_OP_START_SERVICE_DISCOVERY, + status, &cp->type, sizeof(cp->type)); + mgmt_pending_remove(cmd); + goto failed; + } + + err = hci_req_run(&req, start_discovery_complete); + if (err < 0) { + mgmt_pending_remove(cmd); + goto failed; + } + + hci_discovery_set_state(hdev, DISCOVERY_STARTING); + +failed: + hci_dev_unlock(hdev); + return err; +} + +static void stop_discovery_complete(struct hci_dev *hdev, u8 status, u16 opcode) +{ + struct mgmt_pending_cmd *cmd; + + BT_DBG("status %d", status); + + hci_dev_lock(hdev); + + cmd = pending_find(MGMT_OP_STOP_DISCOVERY, hdev); + if (cmd) { + cmd->cmd_complete(cmd, mgmt_status(status)); + mgmt_pending_remove(cmd); + } + + if (!status) + hci_discovery_set_state(hdev, DISCOVERY_STOPPED); + + hci_dev_unlock(hdev); +} + +static int stop_discovery(struct sock *sk, struct hci_dev *hdev, void *data, + u16 len) +{ + struct mgmt_cp_stop_discovery *mgmt_cp = data; + struct mgmt_pending_cmd *cmd; + struct hci_request req; + int err; + + BT_DBG("%s", hdev->name); + + hci_dev_lock(hdev); + + if (!hci_discovery_active(hdev)) { + err = mgmt_cmd_complete(sk, hdev->id, MGMT_OP_STOP_DISCOVERY, + MGMT_STATUS_REJECTED, &mgmt_cp->type, + sizeof(mgmt_cp->type)); + goto unlock; + } + + if (hdev->discovery.type != mgmt_cp->type) { + err = mgmt_cmd_complete(sk, hdev->id, MGMT_OP_STOP_DISCOVERY, + MGMT_STATUS_INVALID_PARAMS, + &mgmt_cp->type, sizeof(mgmt_cp->type)); + goto unlock; + } + + cmd = mgmt_pending_add(sk, MGMT_OP_STOP_DISCOVERY, hdev, data, len); + if (!cmd) { + err = -ENOMEM; + goto unlock; + } + + cmd->cmd_complete = generic_cmd_complete; + + hci_req_init(&req, hdev); + + hci_stop_discovery(&req); + + err = hci_req_run(&req, stop_discovery_complete); + if (!err) { + hci_discovery_set_state(hdev, DISCOVERY_STOPPING); + goto unlock; + } + + mgmt_pending_remove(cmd); + + /* If no HCI commands were sent we're done */ + if (err == -ENODATA) { + err = mgmt_cmd_complete(sk, hdev->id, MGMT_OP_STOP_DISCOVERY, 0, + &mgmt_cp->type, sizeof(mgmt_cp->type)); + hci_discovery_set_state(hdev, DISCOVERY_STOPPED); + } + +unlock: + hci_dev_unlock(hdev); + return err; +} + +static int confirm_name(struct sock *sk, struct hci_dev *hdev, void *data, + u16 len) +{ + struct mgmt_cp_confirm_name *cp = data; + struct inquiry_entry *e; + int err; + + BT_DBG("%s", hdev->name); + + hci_dev_lock(hdev); + + if (!hci_discovery_active(hdev)) { + err = mgmt_cmd_complete(sk, hdev->id, MGMT_OP_CONFIRM_NAME, + MGMT_STATUS_FAILED, &cp->addr, + sizeof(cp->addr)); + goto failed; + } + + e = hci_inquiry_cache_lookup_unknown(hdev, &cp->addr.bdaddr); + if (!e) { + err = mgmt_cmd_complete(sk, hdev->id, MGMT_OP_CONFIRM_NAME, + MGMT_STATUS_INVALID_PARAMS, &cp->addr, + sizeof(cp->addr)); + goto failed; + } + + if (cp->name_known) { + e->name_state = NAME_KNOWN; + list_del(&e->list); + } else { + e->name_state = NAME_NEEDED; + hci_inquiry_cache_update_resolve(hdev, e); + } + + err = mgmt_cmd_complete(sk, hdev->id, MGMT_OP_CONFIRM_NAME, 0, + &cp->addr, sizeof(cp->addr)); + +failed: + hci_dev_unlock(hdev); + return err; +} + +static int block_device(struct sock *sk, struct hci_dev *hdev, void *data, + u16 len) +{ + struct mgmt_cp_block_device *cp = data; + u8 status; + int err; + + BT_DBG("%s", hdev->name); + + if (!bdaddr_type_is_valid(cp->addr.type)) + return mgmt_cmd_complete(sk, hdev->id, MGMT_OP_BLOCK_DEVICE, + MGMT_STATUS_INVALID_PARAMS, + &cp->addr, sizeof(cp->addr)); + + hci_dev_lock(hdev); + + err = hci_bdaddr_list_add(&hdev->blacklist, &cp->addr.bdaddr, + cp->addr.type); + if (err < 0) { + status = MGMT_STATUS_FAILED; + goto done; + } + + mgmt_event(MGMT_EV_DEVICE_BLOCKED, hdev, &cp->addr, sizeof(cp->addr), + sk); + status = MGMT_STATUS_SUCCESS; + +done: + err = mgmt_cmd_complete(sk, hdev->id, MGMT_OP_BLOCK_DEVICE, status, + &cp->addr, sizeof(cp->addr)); + + hci_dev_unlock(hdev); + + return err; +} + +static int unblock_device(struct sock *sk, struct hci_dev *hdev, void *data, + u16 len) +{ + struct mgmt_cp_unblock_device *cp = data; + u8 status; + int err; + + BT_DBG("%s", hdev->name); + + if (!bdaddr_type_is_valid(cp->addr.type)) + return mgmt_cmd_complete(sk, hdev->id, MGMT_OP_UNBLOCK_DEVICE, + MGMT_STATUS_INVALID_PARAMS, + &cp->addr, sizeof(cp->addr)); + + hci_dev_lock(hdev); + + err = hci_bdaddr_list_del(&hdev->blacklist, &cp->addr.bdaddr, + cp->addr.type); + if (err < 0) { + status = MGMT_STATUS_INVALID_PARAMS; + goto done; + } + + mgmt_event(MGMT_EV_DEVICE_UNBLOCKED, hdev, &cp->addr, sizeof(cp->addr), + sk); + status = MGMT_STATUS_SUCCESS; + +done: + err = mgmt_cmd_complete(sk, hdev->id, MGMT_OP_UNBLOCK_DEVICE, status, + &cp->addr, sizeof(cp->addr)); + + hci_dev_unlock(hdev); + + return err; +} + +static int set_device_id(struct sock *sk, struct hci_dev *hdev, void *data, + u16 len) +{ + struct mgmt_cp_set_device_id *cp = data; + struct hci_request req; + int err; + __u16 source; + + BT_DBG("%s", hdev->name); + + source = __le16_to_cpu(cp->source); + + if (source > 0x0002) + return mgmt_cmd_status(sk, hdev->id, MGMT_OP_SET_DEVICE_ID, + MGMT_STATUS_INVALID_PARAMS); + + hci_dev_lock(hdev); + + hdev->devid_source = source; + hdev->devid_vendor = __le16_to_cpu(cp->vendor); + hdev->devid_product = __le16_to_cpu(cp->product); + hdev->devid_version = __le16_to_cpu(cp->version); + + err = mgmt_cmd_complete(sk, hdev->id, MGMT_OP_SET_DEVICE_ID, 0, + NULL, 0); + + hci_req_init(&req, hdev); + update_eir(&req); + hci_req_run(&req, NULL); + + hci_dev_unlock(hdev); + + return err; +} + +static void enable_advertising_instance(struct hci_dev *hdev, u8 status, + u16 opcode) +{ + BT_DBG("status %d", status); +} + +static void set_advertising_complete(struct hci_dev *hdev, u8 status, + u16 opcode) +{ + struct cmd_lookup match = { NULL, hdev }; + struct hci_request req; + + hci_dev_lock(hdev); + + if (status) { + u8 mgmt_err = mgmt_status(status); + + mgmt_pending_foreach(MGMT_OP_SET_ADVERTISING, hdev, + cmd_status_rsp, &mgmt_err); + goto unlock; + } + + if (hci_dev_test_flag(hdev, HCI_LE_ADV)) + hci_dev_set_flag(hdev, HCI_ADVERTISING); + else + hci_dev_clear_flag(hdev, HCI_ADVERTISING); + + mgmt_pending_foreach(MGMT_OP_SET_ADVERTISING, hdev, settings_rsp, + &match); + + new_settings(hdev, match.sk); + + if (match.sk) + sock_put(match.sk); + + /* If "Set Advertising" was just disabled and instance advertising was + * set up earlier, then enable the advertising instance. + */ + if (hci_dev_test_flag(hdev, HCI_ADVERTISING) || + !hci_dev_test_flag(hdev, HCI_ADVERTISING_INSTANCE)) + goto unlock; + + hci_req_init(&req, hdev); + + update_adv_data(&req); + enable_advertising(&req); + + if (hci_req_run(&req, enable_advertising_instance) < 0) + BT_ERR("Failed to re-configure advertising"); + +unlock: + hci_dev_unlock(hdev); +} + +static int set_advertising(struct sock *sk, struct hci_dev *hdev, void *data, + u16 len) +{ + struct mgmt_mode *cp = data; + struct mgmt_pending_cmd *cmd; + struct hci_request req; + u8 val, status; + int err; + + BT_DBG("request for %s", hdev->name); + + status = mgmt_le_support(hdev); + if (status) + return mgmt_cmd_status(sk, hdev->id, MGMT_OP_SET_ADVERTISING, + status); + + if (cp->val != 0x00 && cp->val != 0x01 && cp->val != 0x02) + return mgmt_cmd_status(sk, hdev->id, MGMT_OP_SET_ADVERTISING, + MGMT_STATUS_INVALID_PARAMS); + + hci_dev_lock(hdev); + + val = !!cp->val; + + /* The following conditions are ones which mean that we should + * not do any HCI communication but directly send a mgmt + * response to user space (after toggling the flag if + * necessary). + */ + if (!hdev_is_powered(hdev) || + (val == hci_dev_test_flag(hdev, HCI_ADVERTISING) && + (cp->val == 0x02) == hci_dev_test_flag(hdev, HCI_ADVERTISING_CONNECTABLE)) || + hci_conn_num(hdev, LE_LINK) > 0 || + (hci_dev_test_flag(hdev, HCI_LE_SCAN) && + hdev->le_scan_type == LE_SCAN_ACTIVE)) { + bool changed; + + if (cp->val) { + changed = !hci_dev_test_and_set_flag(hdev, HCI_ADVERTISING); + if (cp->val == 0x02) + hci_dev_set_flag(hdev, HCI_ADVERTISING_CONNECTABLE); + else + hci_dev_clear_flag(hdev, HCI_ADVERTISING_CONNECTABLE); + } else { + changed = hci_dev_test_and_clear_flag(hdev, HCI_ADVERTISING); + hci_dev_clear_flag(hdev, HCI_ADVERTISING_CONNECTABLE); + } + + err = send_settings_rsp(sk, MGMT_OP_SET_ADVERTISING, hdev); + if (err < 0) + goto unlock; + + if (changed) + err = new_settings(hdev, sk); + + goto unlock; + } + + if (pending_find(MGMT_OP_SET_ADVERTISING, hdev) || + pending_find(MGMT_OP_SET_LE, hdev)) { + err = mgmt_cmd_status(sk, hdev->id, MGMT_OP_SET_ADVERTISING, + MGMT_STATUS_BUSY); + goto unlock; + } + + cmd = mgmt_pending_add(sk, MGMT_OP_SET_ADVERTISING, hdev, data, len); + if (!cmd) { + err = -ENOMEM; + goto unlock; + } + + hci_req_init(&req, hdev); + + if (cp->val == 0x02) + hci_dev_set_flag(hdev, HCI_ADVERTISING_CONNECTABLE); + else + hci_dev_clear_flag(hdev, HCI_ADVERTISING_CONNECTABLE); + + if (val) { + /* Switch to instance "0" for the Set Advertising setting. */ + update_adv_data_for_instance(&req, 0); + update_scan_rsp_data_for_instance(&req, 0); + enable_advertising(&req); + } else { + disable_advertising(&req); + } + + err = hci_req_run(&req, set_advertising_complete); + if (err < 0) + mgmt_pending_remove(cmd); + +unlock: + hci_dev_unlock(hdev); + return err; +} + +static int set_static_address(struct sock *sk, struct hci_dev *hdev, + void *data, u16 len) +{ + struct mgmt_cp_set_static_address *cp = data; + int err; + + BT_DBG("%s", hdev->name); + + if (!lmp_le_capable(hdev)) + return mgmt_cmd_status(sk, hdev->id, MGMT_OP_SET_STATIC_ADDRESS, + MGMT_STATUS_NOT_SUPPORTED); + + if (hdev_is_powered(hdev)) + return mgmt_cmd_status(sk, hdev->id, MGMT_OP_SET_STATIC_ADDRESS, + MGMT_STATUS_REJECTED); + + if (bacmp(&cp->bdaddr, BDADDR_ANY)) { + if (!bacmp(&cp->bdaddr, BDADDR_NONE)) + return mgmt_cmd_status(sk, hdev->id, + MGMT_OP_SET_STATIC_ADDRESS, + MGMT_STATUS_INVALID_PARAMS); + + /* Two most significant bits shall be set */ + if ((cp->bdaddr.b[5] & 0xc0) != 0xc0) + return mgmt_cmd_status(sk, hdev->id, + MGMT_OP_SET_STATIC_ADDRESS, + MGMT_STATUS_INVALID_PARAMS); + } + + hci_dev_lock(hdev); + + bacpy(&hdev->static_addr, &cp->bdaddr); + + err = send_settings_rsp(sk, MGMT_OP_SET_STATIC_ADDRESS, hdev); + if (err < 0) + goto unlock; + + err = new_settings(hdev, sk); + +unlock: + hci_dev_unlock(hdev); + return err; +} + +static int set_scan_params(struct sock *sk, struct hci_dev *hdev, + void *data, u16 len) +{ + struct mgmt_cp_set_scan_params *cp = data; + __u16 interval, window; + int err; + + BT_DBG("%s", hdev->name); + + if (!lmp_le_capable(hdev)) + return mgmt_cmd_status(sk, hdev->id, MGMT_OP_SET_SCAN_PARAMS, + MGMT_STATUS_NOT_SUPPORTED); + + interval = __le16_to_cpu(cp->interval); + + if (interval < 0x0004 || interval > 0x4000) + return mgmt_cmd_status(sk, hdev->id, MGMT_OP_SET_SCAN_PARAMS, + MGMT_STATUS_INVALID_PARAMS); + + window = __le16_to_cpu(cp->window); + + if (window < 0x0004 || window > 0x4000) + return mgmt_cmd_status(sk, hdev->id, MGMT_OP_SET_SCAN_PARAMS, + MGMT_STATUS_INVALID_PARAMS); + + if (window > interval) + return mgmt_cmd_status(sk, hdev->id, MGMT_OP_SET_SCAN_PARAMS, + MGMT_STATUS_INVALID_PARAMS); + + hci_dev_lock(hdev); + + hdev->le_scan_interval = interval; + hdev->le_scan_window = window; + + err = mgmt_cmd_complete(sk, hdev->id, MGMT_OP_SET_SCAN_PARAMS, 0, + NULL, 0); + + /* If background scan is running, restart it so new parameters are + * loaded. + */ + if (hci_dev_test_flag(hdev, HCI_LE_SCAN) && + hdev->discovery.state == DISCOVERY_STOPPED) { + struct hci_request req; + + hci_req_init(&req, hdev); + + hci_req_add_le_scan_disable(&req); + hci_req_add_le_passive_scan(&req); + + hci_req_run(&req, NULL); + } + + hci_dev_unlock(hdev); + + return err; +} + +static void fast_connectable_complete(struct hci_dev *hdev, u8 status, + u16 opcode) +{ + struct mgmt_pending_cmd *cmd; + + BT_DBG("status 0x%02x", status); + + hci_dev_lock(hdev); + + cmd = pending_find(MGMT_OP_SET_FAST_CONNECTABLE, hdev); + if (!cmd) + goto unlock; + + if (status) { + mgmt_cmd_status(cmd->sk, hdev->id, MGMT_OP_SET_FAST_CONNECTABLE, + mgmt_status(status)); + } else { + struct mgmt_mode *cp = cmd->param; + + if (cp->val) + hci_dev_set_flag(hdev, HCI_FAST_CONNECTABLE); + else + hci_dev_clear_flag(hdev, HCI_FAST_CONNECTABLE); + + send_settings_rsp(cmd->sk, MGMT_OP_SET_FAST_CONNECTABLE, hdev); + new_settings(hdev, cmd->sk); + } + + mgmt_pending_remove(cmd); + +unlock: + hci_dev_unlock(hdev); +} + +static int set_fast_connectable(struct sock *sk, struct hci_dev *hdev, + void *data, u16 len) +{ + struct mgmt_mode *cp = data; + struct mgmt_pending_cmd *cmd; + struct hci_request req; + int err; + + BT_DBG("%s", hdev->name); + + if (!hci_dev_test_flag(hdev, HCI_BREDR_ENABLED) || + hdev->hci_ver < BLUETOOTH_VER_1_2) + return mgmt_cmd_status(sk, hdev->id, MGMT_OP_SET_FAST_CONNECTABLE, + MGMT_STATUS_NOT_SUPPORTED); + + if (cp->val != 0x00 && cp->val != 0x01) + return mgmt_cmd_status(sk, hdev->id, MGMT_OP_SET_FAST_CONNECTABLE, + MGMT_STATUS_INVALID_PARAMS); + + hci_dev_lock(hdev); + + if (pending_find(MGMT_OP_SET_FAST_CONNECTABLE, hdev)) { + err = mgmt_cmd_status(sk, hdev->id, MGMT_OP_SET_FAST_CONNECTABLE, + MGMT_STATUS_BUSY); + goto unlock; + } + + if (!!cp->val == hci_dev_test_flag(hdev, HCI_FAST_CONNECTABLE)) { + err = send_settings_rsp(sk, MGMT_OP_SET_FAST_CONNECTABLE, + hdev); + goto unlock; + } + + if (!hdev_is_powered(hdev)) { + hci_dev_change_flag(hdev, HCI_FAST_CONNECTABLE); + err = send_settings_rsp(sk, MGMT_OP_SET_FAST_CONNECTABLE, + hdev); + new_settings(hdev, sk); + goto unlock; + } + + cmd = mgmt_pending_add(sk, MGMT_OP_SET_FAST_CONNECTABLE, hdev, + data, len); + if (!cmd) { + err = -ENOMEM; + goto unlock; + } + + hci_req_init(&req, hdev); + + write_fast_connectable(&req, cp->val); + + err = hci_req_run(&req, fast_connectable_complete); + if (err < 0) { + err = mgmt_cmd_status(sk, hdev->id, MGMT_OP_SET_FAST_CONNECTABLE, + MGMT_STATUS_FAILED); + mgmt_pending_remove(cmd); + } + +unlock: + hci_dev_unlock(hdev); + + return err; +} + +static void set_bredr_complete(struct hci_dev *hdev, u8 status, u16 opcode) +{ + struct mgmt_pending_cmd *cmd; + + BT_DBG("status 0x%02x", status); + + hci_dev_lock(hdev); + + cmd = pending_find(MGMT_OP_SET_BREDR, hdev); + if (!cmd) + goto unlock; + + if (status) { + u8 mgmt_err = mgmt_status(status); + + /* We need to restore the flag if related HCI commands + * failed. + */ + hci_dev_clear_flag(hdev, HCI_BREDR_ENABLED); + + mgmt_cmd_status(cmd->sk, cmd->index, cmd->opcode, mgmt_err); + } else { + send_settings_rsp(cmd->sk, MGMT_OP_SET_BREDR, hdev); + new_settings(hdev, cmd->sk); + } + + mgmt_pending_remove(cmd); + +unlock: + hci_dev_unlock(hdev); +} + +static int set_bredr(struct sock *sk, struct hci_dev *hdev, void *data, u16 len) +{ + struct mgmt_mode *cp = data; + struct mgmt_pending_cmd *cmd; + struct hci_request req; + int err; + + BT_DBG("request for %s", hdev->name); + + if (!lmp_bredr_capable(hdev) || !lmp_le_capable(hdev)) + return mgmt_cmd_status(sk, hdev->id, MGMT_OP_SET_BREDR, + MGMT_STATUS_NOT_SUPPORTED); + + if (!hci_dev_test_flag(hdev, HCI_LE_ENABLED)) + return mgmt_cmd_status(sk, hdev->id, MGMT_OP_SET_BREDR, + MGMT_STATUS_REJECTED); + + if (cp->val != 0x00 && cp->val != 0x01) + return mgmt_cmd_status(sk, hdev->id, MGMT_OP_SET_BREDR, + MGMT_STATUS_INVALID_PARAMS); + + hci_dev_lock(hdev); + + if (cp->val == hci_dev_test_flag(hdev, HCI_BREDR_ENABLED)) { + err = send_settings_rsp(sk, MGMT_OP_SET_BREDR, hdev); + goto unlock; + } + + if (!hdev_is_powered(hdev)) { + if (!cp->val) { + hci_dev_clear_flag(hdev, HCI_DISCOVERABLE); + hci_dev_clear_flag(hdev, HCI_SSP_ENABLED); + hci_dev_clear_flag(hdev, HCI_LINK_SECURITY); + hci_dev_clear_flag(hdev, HCI_FAST_CONNECTABLE); + hci_dev_clear_flag(hdev, HCI_HS_ENABLED); + } + + hci_dev_change_flag(hdev, HCI_BREDR_ENABLED); + + err = send_settings_rsp(sk, MGMT_OP_SET_BREDR, hdev); + if (err < 0) + goto unlock; + + err = new_settings(hdev, sk); + goto unlock; + } + + /* Reject disabling when powered on */ + if (!cp->val) { + err = mgmt_cmd_status(sk, hdev->id, MGMT_OP_SET_BREDR, + MGMT_STATUS_REJECTED); + goto unlock; + } else { + /* When configuring a dual-mode controller to operate + * with LE only and using a static address, then switching + * BR/EDR back on is not allowed. + * + * Dual-mode controllers shall operate with the public + * address as its identity address for BR/EDR and LE. So + * reject the attempt to create an invalid configuration. + * + * The same restrictions applies when secure connections + * has been enabled. For BR/EDR this is a controller feature + * while for LE it is a host stack feature. This means that + * switching BR/EDR back on when secure connections has been + * enabled is not a supported transaction. + */ + if (!hci_dev_test_flag(hdev, HCI_BREDR_ENABLED) && + (bacmp(&hdev->static_addr, BDADDR_ANY) || + hci_dev_test_flag(hdev, HCI_SC_ENABLED))) { + err = mgmt_cmd_status(sk, hdev->id, MGMT_OP_SET_BREDR, + MGMT_STATUS_REJECTED); + goto unlock; + } + } + + if (pending_find(MGMT_OP_SET_BREDR, hdev)) { + err = mgmt_cmd_status(sk, hdev->id, MGMT_OP_SET_BREDR, + MGMT_STATUS_BUSY); + goto unlock; + } + + cmd = mgmt_pending_add(sk, MGMT_OP_SET_BREDR, hdev, data, len); + if (!cmd) { + err = -ENOMEM; + goto unlock; + } + + /* We need to flip the bit already here so that update_adv_data + * generates the correct flags. + */ + hci_dev_set_flag(hdev, HCI_BREDR_ENABLED); + + hci_req_init(&req, hdev); + + write_fast_connectable(&req, false); + __hci_update_page_scan(&req); + + /* Since only the advertising data flags will change, there + * is no need to update the scan response data. + */ + update_adv_data(&req); + + err = hci_req_run(&req, set_bredr_complete); + if (err < 0) + mgmt_pending_remove(cmd); + +unlock: + hci_dev_unlock(hdev); + return err; +} + +static void sc_enable_complete(struct hci_dev *hdev, u8 status, u16 opcode) +{ + struct mgmt_pending_cmd *cmd; + struct mgmt_mode *cp; + + BT_DBG("%s status %u", hdev->name, status); + + hci_dev_lock(hdev); + + cmd = pending_find(MGMT_OP_SET_SECURE_CONN, hdev); + if (!cmd) + goto unlock; + + if (status) { + mgmt_cmd_status(cmd->sk, cmd->index, cmd->opcode, + mgmt_status(status)); + goto remove; + } + + cp = cmd->param; + + switch (cp->val) { + case 0x00: + hci_dev_clear_flag(hdev, HCI_SC_ENABLED); + hci_dev_clear_flag(hdev, HCI_SC_ONLY); + break; + case 0x01: + hci_dev_set_flag(hdev, HCI_SC_ENABLED); + hci_dev_clear_flag(hdev, HCI_SC_ONLY); + break; + case 0x02: + hci_dev_set_flag(hdev, HCI_SC_ENABLED); + hci_dev_set_flag(hdev, HCI_SC_ONLY); + break; + } + + send_settings_rsp(cmd->sk, MGMT_OP_SET_SECURE_CONN, hdev); + new_settings(hdev, cmd->sk); + +remove: + mgmt_pending_remove(cmd); +unlock: + hci_dev_unlock(hdev); +} + +static int set_secure_conn(struct sock *sk, struct hci_dev *hdev, + void *data, u16 len) +{ + struct mgmt_mode *cp = data; + struct mgmt_pending_cmd *cmd; + struct hci_request req; + u8 val; + int err; + + BT_DBG("request for %s", hdev->name); + + if (!lmp_sc_capable(hdev) && + !hci_dev_test_flag(hdev, HCI_LE_ENABLED)) + return mgmt_cmd_status(sk, hdev->id, MGMT_OP_SET_SECURE_CONN, + MGMT_STATUS_NOT_SUPPORTED); + + if (hci_dev_test_flag(hdev, HCI_BREDR_ENABLED) && + lmp_sc_capable(hdev) && + !hci_dev_test_flag(hdev, HCI_SSP_ENABLED)) + return mgmt_cmd_status(sk, hdev->id, MGMT_OP_SET_SECURE_CONN, + MGMT_STATUS_REJECTED); + + if (cp->val != 0x00 && cp->val != 0x01 && cp->val != 0x02) + return mgmt_cmd_status(sk, hdev->id, MGMT_OP_SET_SECURE_CONN, + MGMT_STATUS_INVALID_PARAMS); + + hci_dev_lock(hdev); + + if (!hdev_is_powered(hdev) || !lmp_sc_capable(hdev) || + !hci_dev_test_flag(hdev, HCI_BREDR_ENABLED)) { + bool changed; + + if (cp->val) { + changed = !hci_dev_test_and_set_flag(hdev, + HCI_SC_ENABLED); + if (cp->val == 0x02) + hci_dev_set_flag(hdev, HCI_SC_ONLY); + else + hci_dev_clear_flag(hdev, HCI_SC_ONLY); + } else { + changed = hci_dev_test_and_clear_flag(hdev, + HCI_SC_ENABLED); + hci_dev_clear_flag(hdev, HCI_SC_ONLY); + } + + err = send_settings_rsp(sk, MGMT_OP_SET_SECURE_CONN, hdev); + if (err < 0) + goto failed; + + if (changed) + err = new_settings(hdev, sk); + + goto failed; + } + + if (pending_find(MGMT_OP_SET_SECURE_CONN, hdev)) { + err = mgmt_cmd_status(sk, hdev->id, MGMT_OP_SET_SECURE_CONN, + MGMT_STATUS_BUSY); + goto failed; + } + + val = !!cp->val; + + if (val == hci_dev_test_flag(hdev, HCI_SC_ENABLED) && + (cp->val == 0x02) == hci_dev_test_flag(hdev, HCI_SC_ONLY)) { + err = send_settings_rsp(sk, MGMT_OP_SET_SECURE_CONN, hdev); + goto failed; + } + + cmd = mgmt_pending_add(sk, MGMT_OP_SET_SECURE_CONN, hdev, data, len); + if (!cmd) { + err = -ENOMEM; + goto failed; + } + + hci_req_init(&req, hdev); + hci_req_add(&req, HCI_OP_WRITE_SC_SUPPORT, 1, &val); + err = hci_req_run(&req, sc_enable_complete); + if (err < 0) { + mgmt_pending_remove(cmd); + goto failed; + } + +failed: + hci_dev_unlock(hdev); + return err; +} + +static int set_debug_keys(struct sock *sk, struct hci_dev *hdev, + void *data, u16 len) +{ + struct mgmt_mode *cp = data; + bool changed, use_changed; + int err; + + BT_DBG("request for %s", hdev->name); + + if (cp->val != 0x00 && cp->val != 0x01 && cp->val != 0x02) + return mgmt_cmd_status(sk, hdev->id, MGMT_OP_SET_DEBUG_KEYS, + MGMT_STATUS_INVALID_PARAMS); + + hci_dev_lock(hdev); + + if (cp->val) + changed = !hci_dev_test_and_set_flag(hdev, HCI_KEEP_DEBUG_KEYS); + else + changed = hci_dev_test_and_clear_flag(hdev, + HCI_KEEP_DEBUG_KEYS); + + if (cp->val == 0x02) + use_changed = !hci_dev_test_and_set_flag(hdev, + HCI_USE_DEBUG_KEYS); + else + use_changed = hci_dev_test_and_clear_flag(hdev, + HCI_USE_DEBUG_KEYS); + + if (hdev_is_powered(hdev) && use_changed && + hci_dev_test_flag(hdev, HCI_SSP_ENABLED)) { + u8 mode = (cp->val == 0x02) ? 0x01 : 0x00; + hci_send_cmd(hdev, HCI_OP_WRITE_SSP_DEBUG_MODE, + sizeof(mode), &mode); + } + + err = send_settings_rsp(sk, MGMT_OP_SET_DEBUG_KEYS, hdev); + if (err < 0) + goto unlock; + + if (changed) + err = new_settings(hdev, sk); + +unlock: + hci_dev_unlock(hdev); + return err; +} + +static int set_privacy(struct sock *sk, struct hci_dev *hdev, void *cp_data, + u16 len) +{ + struct mgmt_cp_set_privacy *cp = cp_data; + bool changed; + int err; + + BT_DBG("request for %s", hdev->name); + + if (!lmp_le_capable(hdev)) + return mgmt_cmd_status(sk, hdev->id, MGMT_OP_SET_PRIVACY, + MGMT_STATUS_NOT_SUPPORTED); + + if (cp->privacy != 0x00 && cp->privacy != 0x01) + return mgmt_cmd_status(sk, hdev->id, MGMT_OP_SET_PRIVACY, + MGMT_STATUS_INVALID_PARAMS); + + if (hdev_is_powered(hdev)) + return mgmt_cmd_status(sk, hdev->id, MGMT_OP_SET_PRIVACY, + MGMT_STATUS_REJECTED); + + hci_dev_lock(hdev); + + /* If user space supports this command it is also expected to + * handle IRKs. Therefore, set the HCI_RPA_RESOLVING flag. + */ + hci_dev_set_flag(hdev, HCI_RPA_RESOLVING); + + if (cp->privacy) { + changed = !hci_dev_test_and_set_flag(hdev, HCI_PRIVACY); + memcpy(hdev->irk, cp->irk, sizeof(hdev->irk)); + hci_dev_set_flag(hdev, HCI_RPA_EXPIRED); + } else { + changed = hci_dev_test_and_clear_flag(hdev, HCI_PRIVACY); + memset(hdev->irk, 0, sizeof(hdev->irk)); + hci_dev_clear_flag(hdev, HCI_RPA_EXPIRED); + } + + err = send_settings_rsp(sk, MGMT_OP_SET_PRIVACY, hdev); + if (err < 0) + goto unlock; + + if (changed) + err = new_settings(hdev, sk); + +unlock: + hci_dev_unlock(hdev); + return err; +} + +static bool irk_is_valid(struct mgmt_irk_info *irk) +{ + switch (irk->addr.type) { + case BDADDR_LE_PUBLIC: + return true; + + case BDADDR_LE_RANDOM: + /* Two most significant bits shall be set */ + if ((irk->addr.bdaddr.b[5] & 0xc0) != 0xc0) + return false; + return true; + } + + return false; +} + +static int load_irks(struct sock *sk, struct hci_dev *hdev, void *cp_data, + u16 len) +{ + struct mgmt_cp_load_irks *cp = cp_data; + const u16 max_irk_count = ((U16_MAX - sizeof(*cp)) / + sizeof(struct mgmt_irk_info)); + u16 irk_count, expected_len; + int i, err; + + BT_DBG("request for %s", hdev->name); + + if (!lmp_le_capable(hdev)) + return mgmt_cmd_status(sk, hdev->id, MGMT_OP_LOAD_IRKS, + MGMT_STATUS_NOT_SUPPORTED); + + irk_count = __le16_to_cpu(cp->irk_count); + if (irk_count > max_irk_count) { + BT_ERR("load_irks: too big irk_count value %u", irk_count); + return mgmt_cmd_status(sk, hdev->id, MGMT_OP_LOAD_IRKS, + MGMT_STATUS_INVALID_PARAMS); + } + + expected_len = sizeof(*cp) + irk_count * sizeof(struct mgmt_irk_info); + if (expected_len != len) { + BT_ERR("load_irks: expected %u bytes, got %u bytes", + expected_len, len); + return mgmt_cmd_status(sk, hdev->id, MGMT_OP_LOAD_IRKS, + MGMT_STATUS_INVALID_PARAMS); + } + + BT_DBG("%s irk_count %u", hdev->name, irk_count); + + for (i = 0; i < irk_count; i++) { + struct mgmt_irk_info *key = &cp->irks[i]; + + if (!irk_is_valid(key)) + return mgmt_cmd_status(sk, hdev->id, + MGMT_OP_LOAD_IRKS, + MGMT_STATUS_INVALID_PARAMS); + } + + hci_dev_lock(hdev); + + hci_smp_irks_clear(hdev); + + for (i = 0; i < irk_count; i++) { + struct mgmt_irk_info *irk = &cp->irks[i]; + u8 addr_type; + + if (irk->addr.type == BDADDR_LE_PUBLIC) + addr_type = ADDR_LE_DEV_PUBLIC; + else + addr_type = ADDR_LE_DEV_RANDOM; + + hci_add_irk(hdev, &irk->addr.bdaddr, addr_type, irk->val, + BDADDR_ANY); + } + + hci_dev_set_flag(hdev, HCI_RPA_RESOLVING); + + err = mgmt_cmd_complete(sk, hdev->id, MGMT_OP_LOAD_IRKS, 0, NULL, 0); + + hci_dev_unlock(hdev); + + return err; +} + +static bool ltk_is_valid(struct mgmt_ltk_info *key) +{ + if (key->master != 0x00 && key->master != 0x01) + return false; + + switch (key->addr.type) { + case BDADDR_LE_PUBLIC: + return true; + + case BDADDR_LE_RANDOM: + /* Two most significant bits shall be set */ + if ((key->addr.bdaddr.b[5] & 0xc0) != 0xc0) + return false; + return true; + } + + return false; +} + +static int load_long_term_keys(struct sock *sk, struct hci_dev *hdev, + void *cp_data, u16 len) +{ + struct mgmt_cp_load_long_term_keys *cp = cp_data; + const u16 max_key_count = ((U16_MAX - sizeof(*cp)) / + sizeof(struct mgmt_ltk_info)); + u16 key_count, expected_len; + int i, err; + + BT_DBG("request for %s", hdev->name); + + if (!lmp_le_capable(hdev)) + return mgmt_cmd_status(sk, hdev->id, MGMT_OP_LOAD_LONG_TERM_KEYS, + MGMT_STATUS_NOT_SUPPORTED); + + key_count = __le16_to_cpu(cp->key_count); + if (key_count > max_key_count) { + BT_ERR("load_ltks: too big key_count value %u", key_count); + return mgmt_cmd_status(sk, hdev->id, MGMT_OP_LOAD_LONG_TERM_KEYS, + MGMT_STATUS_INVALID_PARAMS); + } + + expected_len = sizeof(*cp) + key_count * + sizeof(struct mgmt_ltk_info); + if (expected_len != len) { + BT_ERR("load_keys: expected %u bytes, got %u bytes", + expected_len, len); + return mgmt_cmd_status(sk, hdev->id, MGMT_OP_LOAD_LONG_TERM_KEYS, + MGMT_STATUS_INVALID_PARAMS); + } + + BT_DBG("%s key_count %u", hdev->name, key_count); + + for (i = 0; i < key_count; i++) { + struct mgmt_ltk_info *key = &cp->keys[i]; + + if (!ltk_is_valid(key)) + return mgmt_cmd_status(sk, hdev->id, + MGMT_OP_LOAD_LONG_TERM_KEYS, + MGMT_STATUS_INVALID_PARAMS); + } + + hci_dev_lock(hdev); + + hci_smp_ltks_clear(hdev); + + for (i = 0; i < key_count; i++) { + struct mgmt_ltk_info *key = &cp->keys[i]; + u8 type, addr_type, authenticated; + + if (key->addr.type == BDADDR_LE_PUBLIC) + addr_type = ADDR_LE_DEV_PUBLIC; + else + addr_type = ADDR_LE_DEV_RANDOM; + + switch (key->type) { + case MGMT_LTK_UNAUTHENTICATED: + authenticated = 0x00; + type = key->master ? SMP_LTK : SMP_LTK_SLAVE; + break; + case MGMT_LTK_AUTHENTICATED: + authenticated = 0x01; + type = key->master ? SMP_LTK : SMP_LTK_SLAVE; + break; + case MGMT_LTK_P256_UNAUTH: + authenticated = 0x00; + type = SMP_LTK_P256; + break; + case MGMT_LTK_P256_AUTH: + authenticated = 0x01; + type = SMP_LTK_P256; + break; + case MGMT_LTK_P256_DEBUG: + authenticated = 0x00; + type = SMP_LTK_P256_DEBUG; + default: + continue; + } + + hci_add_ltk(hdev, &key->addr.bdaddr, addr_type, type, + authenticated, key->val, key->enc_size, key->ediv, + key->rand); + } + + err = mgmt_cmd_complete(sk, hdev->id, MGMT_OP_LOAD_LONG_TERM_KEYS, 0, + NULL, 0); + + hci_dev_unlock(hdev); + + return err; +} + +static int conn_info_cmd_complete(struct mgmt_pending_cmd *cmd, u8 status) +{ + struct hci_conn *conn = cmd->user_data; + struct mgmt_rp_get_conn_info rp; + int err; + + memcpy(&rp.addr, cmd->param, sizeof(rp.addr)); + + if (status == MGMT_STATUS_SUCCESS) { + rp.rssi = conn->rssi; + rp.tx_power = conn->tx_power; + rp.max_tx_power = conn->max_tx_power; + } else { + rp.rssi = HCI_RSSI_INVALID; + rp.tx_power = HCI_TX_POWER_INVALID; + rp.max_tx_power = HCI_TX_POWER_INVALID; + } + + err = mgmt_cmd_complete(cmd->sk, cmd->index, MGMT_OP_GET_CONN_INFO, + status, &rp, sizeof(rp)); + + hci_conn_drop(conn); + hci_conn_put(conn); + + return err; +} + +static void conn_info_refresh_complete(struct hci_dev *hdev, u8 hci_status, + u16 opcode) +{ + struct hci_cp_read_rssi *cp; + struct mgmt_pending_cmd *cmd; + struct hci_conn *conn; + u16 handle; + u8 status; + + BT_DBG("status 0x%02x", hci_status); + + hci_dev_lock(hdev); + + /* Commands sent in request are either Read RSSI or Read Transmit Power + * Level so we check which one was last sent to retrieve connection + * handle. Both commands have handle as first parameter so it's safe to + * cast data on the same command struct. + * + * First command sent is always Read RSSI and we fail only if it fails. + * In other case we simply override error to indicate success as we + * already remembered if TX power value is actually valid. + */ + cp = hci_sent_cmd_data(hdev, HCI_OP_READ_RSSI); + if (!cp) { + cp = hci_sent_cmd_data(hdev, HCI_OP_READ_TX_POWER); + status = MGMT_STATUS_SUCCESS; + } else { + status = mgmt_status(hci_status); + } + + if (!cp) { + BT_ERR("invalid sent_cmd in conn_info response"); + goto unlock; + } + + handle = __le16_to_cpu(cp->handle); + conn = hci_conn_hash_lookup_handle(hdev, handle); + if (!conn) { + BT_ERR("unknown handle (%d) in conn_info response", handle); + goto unlock; + } + + cmd = pending_find_data(MGMT_OP_GET_CONN_INFO, hdev, conn); + if (!cmd) + goto unlock; + + cmd->cmd_complete(cmd, status); + mgmt_pending_remove(cmd); + +unlock: + hci_dev_unlock(hdev); +} + +static int get_conn_info(struct sock *sk, struct hci_dev *hdev, void *data, + u16 len) +{ + struct mgmt_cp_get_conn_info *cp = data; + struct mgmt_rp_get_conn_info rp; + struct hci_conn *conn; + unsigned long conn_info_age; + int err = 0; + + BT_DBG("%s", hdev->name); + + memset(&rp, 0, sizeof(rp)); + bacpy(&rp.addr.bdaddr, &cp->addr.bdaddr); + rp.addr.type = cp->addr.type; + + if (!bdaddr_type_is_valid(cp->addr.type)) + return mgmt_cmd_complete(sk, hdev->id, MGMT_OP_GET_CONN_INFO, + MGMT_STATUS_INVALID_PARAMS, + &rp, sizeof(rp)); + + hci_dev_lock(hdev); + + if (!hdev_is_powered(hdev)) { + err = mgmt_cmd_complete(sk, hdev->id, MGMT_OP_GET_CONN_INFO, + MGMT_STATUS_NOT_POWERED, &rp, + sizeof(rp)); + goto unlock; + } + + if (cp->addr.type == BDADDR_BREDR) + conn = hci_conn_hash_lookup_ba(hdev, ACL_LINK, + &cp->addr.bdaddr); + else + conn = hci_conn_hash_lookup_ba(hdev, LE_LINK, &cp->addr.bdaddr); + + if (!conn || conn->state != BT_CONNECTED) { + err = mgmt_cmd_complete(sk, hdev->id, MGMT_OP_GET_CONN_INFO, + MGMT_STATUS_NOT_CONNECTED, &rp, + sizeof(rp)); + goto unlock; + } + + if (pending_find_data(MGMT_OP_GET_CONN_INFO, hdev, conn)) { + err = mgmt_cmd_complete(sk, hdev->id, MGMT_OP_GET_CONN_INFO, + MGMT_STATUS_BUSY, &rp, sizeof(rp)); + goto unlock; + } + + /* To avoid client trying to guess when to poll again for information we + * calculate conn info age as random value between min/max set in hdev. + */ + conn_info_age = hdev->conn_info_min_age + + prandom_u32_max(hdev->conn_info_max_age - + hdev->conn_info_min_age); + + /* Query controller to refresh cached values if they are too old or were + * never read. + */ + if (time_after(jiffies, conn->conn_info_timestamp + + msecs_to_jiffies(conn_info_age)) || + !conn->conn_info_timestamp) { + struct hci_request req; + struct hci_cp_read_tx_power req_txp_cp; + struct hci_cp_read_rssi req_rssi_cp; + struct mgmt_pending_cmd *cmd; + + hci_req_init(&req, hdev); + req_rssi_cp.handle = cpu_to_le16(conn->handle); + hci_req_add(&req, HCI_OP_READ_RSSI, sizeof(req_rssi_cp), + &req_rssi_cp); + + /* For LE links TX power does not change thus we don't need to + * query for it once value is known. + */ + if (!bdaddr_type_is_le(cp->addr.type) || + conn->tx_power == HCI_TX_POWER_INVALID) { + req_txp_cp.handle = cpu_to_le16(conn->handle); + req_txp_cp.type = 0x00; + hci_req_add(&req, HCI_OP_READ_TX_POWER, + sizeof(req_txp_cp), &req_txp_cp); + } + + /* Max TX power needs to be read only once per connection */ + if (conn->max_tx_power == HCI_TX_POWER_INVALID) { + req_txp_cp.handle = cpu_to_le16(conn->handle); + req_txp_cp.type = 0x01; + hci_req_add(&req, HCI_OP_READ_TX_POWER, + sizeof(req_txp_cp), &req_txp_cp); + } + + err = hci_req_run(&req, conn_info_refresh_complete); + if (err < 0) + goto unlock; + + cmd = mgmt_pending_add(sk, MGMT_OP_GET_CONN_INFO, hdev, + data, len); + if (!cmd) { + err = -ENOMEM; + goto unlock; + } + + hci_conn_hold(conn); + cmd->user_data = hci_conn_get(conn); + cmd->cmd_complete = conn_info_cmd_complete; + + conn->conn_info_timestamp = jiffies; + } else { + /* Cache is valid, just reply with values cached in hci_conn */ + rp.rssi = conn->rssi; + rp.tx_power = conn->tx_power; + rp.max_tx_power = conn->max_tx_power; + + err = mgmt_cmd_complete(sk, hdev->id, MGMT_OP_GET_CONN_INFO, + MGMT_STATUS_SUCCESS, &rp, sizeof(rp)); + } + +unlock: + hci_dev_unlock(hdev); + return err; +} + +static int clock_info_cmd_complete(struct mgmt_pending_cmd *cmd, u8 status) +{ + struct hci_conn *conn = cmd->user_data; + struct mgmt_rp_get_clock_info rp; + struct hci_dev *hdev; + int err; + + memset(&rp, 0, sizeof(rp)); + memcpy(&rp.addr, &cmd->param, sizeof(rp.addr)); + + if (status) + goto complete; + + hdev = hci_dev_get(cmd->index); + if (hdev) { + rp.local_clock = cpu_to_le32(hdev->clock); + hci_dev_put(hdev); + } + + if (conn) { + rp.piconet_clock = cpu_to_le32(conn->clock); + rp.accuracy = cpu_to_le16(conn->clock_accuracy); + } + +complete: + err = mgmt_cmd_complete(cmd->sk, cmd->index, cmd->opcode, status, &rp, + sizeof(rp)); + + if (conn) { + hci_conn_drop(conn); + hci_conn_put(conn); + } + + return err; +} + +static void get_clock_info_complete(struct hci_dev *hdev, u8 status, u16 opcode) +{ + struct hci_cp_read_clock *hci_cp; + struct mgmt_pending_cmd *cmd; + struct hci_conn *conn; + + BT_DBG("%s status %u", hdev->name, status); + + hci_dev_lock(hdev); + + hci_cp = hci_sent_cmd_data(hdev, HCI_OP_READ_CLOCK); + if (!hci_cp) + goto unlock; + + if (hci_cp->which) { + u16 handle = __le16_to_cpu(hci_cp->handle); + conn = hci_conn_hash_lookup_handle(hdev, handle); + } else { + conn = NULL; + } + + cmd = pending_find_data(MGMT_OP_GET_CLOCK_INFO, hdev, conn); + if (!cmd) + goto unlock; + + cmd->cmd_complete(cmd, mgmt_status(status)); + mgmt_pending_remove(cmd); + +unlock: + hci_dev_unlock(hdev); +} + +static int get_clock_info(struct sock *sk, struct hci_dev *hdev, void *data, + u16 len) +{ + struct mgmt_cp_get_clock_info *cp = data; + struct mgmt_rp_get_clock_info rp; + struct hci_cp_read_clock hci_cp; + struct mgmt_pending_cmd *cmd; + struct hci_request req; + struct hci_conn *conn; + int err; + + BT_DBG("%s", hdev->name); + + memset(&rp, 0, sizeof(rp)); + bacpy(&rp.addr.bdaddr, &cp->addr.bdaddr); + rp.addr.type = cp->addr.type; + + if (cp->addr.type != BDADDR_BREDR) + return mgmt_cmd_complete(sk, hdev->id, MGMT_OP_GET_CLOCK_INFO, + MGMT_STATUS_INVALID_PARAMS, + &rp, sizeof(rp)); + + hci_dev_lock(hdev); + + if (!hdev_is_powered(hdev)) { + err = mgmt_cmd_complete(sk, hdev->id, MGMT_OP_GET_CLOCK_INFO, + MGMT_STATUS_NOT_POWERED, &rp, + sizeof(rp)); + goto unlock; + } + + if (bacmp(&cp->addr.bdaddr, BDADDR_ANY)) { + conn = hci_conn_hash_lookup_ba(hdev, ACL_LINK, + &cp->addr.bdaddr); + if (!conn || conn->state != BT_CONNECTED) { + err = mgmt_cmd_complete(sk, hdev->id, + MGMT_OP_GET_CLOCK_INFO, + MGMT_STATUS_NOT_CONNECTED, + &rp, sizeof(rp)); + goto unlock; + } + } else { + conn = NULL; + } + + cmd = mgmt_pending_add(sk, MGMT_OP_GET_CLOCK_INFO, hdev, data, len); + if (!cmd) { + err = -ENOMEM; + goto unlock; + } + + cmd->cmd_complete = clock_info_cmd_complete; + + hci_req_init(&req, hdev); + + memset(&hci_cp, 0, sizeof(hci_cp)); + hci_req_add(&req, HCI_OP_READ_CLOCK, sizeof(hci_cp), &hci_cp); + + if (conn) { + hci_conn_hold(conn); + cmd->user_data = hci_conn_get(conn); + + hci_cp.handle = cpu_to_le16(conn->handle); + hci_cp.which = 0x01; /* Piconet clock */ + hci_req_add(&req, HCI_OP_READ_CLOCK, sizeof(hci_cp), &hci_cp); + } + + err = hci_req_run(&req, get_clock_info_complete); + if (err < 0) + mgmt_pending_remove(cmd); + +unlock: + hci_dev_unlock(hdev); + return err; +} + +static bool is_connected(struct hci_dev *hdev, bdaddr_t *addr, u8 type) +{ + struct hci_conn *conn; + + conn = hci_conn_hash_lookup_ba(hdev, LE_LINK, addr); + if (!conn) + return false; + + if (conn->dst_type != type) + return false; + + if (conn->state != BT_CONNECTED) + return false; + + return true; +} + +/* This function requires the caller holds hdev->lock */ +static int hci_conn_params_set(struct hci_request *req, bdaddr_t *addr, + u8 addr_type, u8 auto_connect) +{ + struct hci_dev *hdev = req->hdev; + struct hci_conn_params *params; + + params = hci_conn_params_add(hdev, addr, addr_type); + if (!params) + return -EIO; + + if (params->auto_connect == auto_connect) + return 0; + + list_del_init(¶ms->action); + + switch (auto_connect) { + case HCI_AUTO_CONN_DISABLED: + case HCI_AUTO_CONN_LINK_LOSS: + __hci_update_background_scan(req); + break; + case HCI_AUTO_CONN_REPORT: + list_add(¶ms->action, &hdev->pend_le_reports); + __hci_update_background_scan(req); + break; + case HCI_AUTO_CONN_DIRECT: + case HCI_AUTO_CONN_ALWAYS: + if (!is_connected(hdev, addr, addr_type)) { + list_add(¶ms->action, &hdev->pend_le_conns); + __hci_update_background_scan(req); + } + break; + } + + params->auto_connect = auto_connect; + + BT_DBG("addr %pMR (type %u) auto_connect %u", addr, addr_type, + auto_connect); + + return 0; +} + +static void device_added(struct sock *sk, struct hci_dev *hdev, + bdaddr_t *bdaddr, u8 type, u8 action) +{ + struct mgmt_ev_device_added ev; + + bacpy(&ev.addr.bdaddr, bdaddr); + ev.addr.type = type; + ev.action = action; + + mgmt_event(MGMT_EV_DEVICE_ADDED, hdev, &ev, sizeof(ev), sk); +} + +static void add_device_complete(struct hci_dev *hdev, u8 status, u16 opcode) +{ + struct mgmt_pending_cmd *cmd; + + BT_DBG("status 0x%02x", status); + + hci_dev_lock(hdev); + + cmd = pending_find(MGMT_OP_ADD_DEVICE, hdev); + if (!cmd) + goto unlock; + + cmd->cmd_complete(cmd, mgmt_status(status)); + mgmt_pending_remove(cmd); + +unlock: + hci_dev_unlock(hdev); +} + +static int add_device(struct sock *sk, struct hci_dev *hdev, + void *data, u16 len) +{ + struct mgmt_cp_add_device *cp = data; + struct mgmt_pending_cmd *cmd; + struct hci_request req; + u8 auto_conn, addr_type; + int err; + + BT_DBG("%s", hdev->name); + + if (!bdaddr_type_is_valid(cp->addr.type) || + !bacmp(&cp->addr.bdaddr, BDADDR_ANY)) + return mgmt_cmd_complete(sk, hdev->id, MGMT_OP_ADD_DEVICE, + MGMT_STATUS_INVALID_PARAMS, + &cp->addr, sizeof(cp->addr)); + + if (cp->action != 0x00 && cp->action != 0x01 && cp->action != 0x02) + return mgmt_cmd_complete(sk, hdev->id, MGMT_OP_ADD_DEVICE, + MGMT_STATUS_INVALID_PARAMS, + &cp->addr, sizeof(cp->addr)); + + hci_req_init(&req, hdev); + + hci_dev_lock(hdev); + + cmd = mgmt_pending_add(sk, MGMT_OP_ADD_DEVICE, hdev, data, len); + if (!cmd) { + err = -ENOMEM; + goto unlock; + } + + cmd->cmd_complete = addr_cmd_complete; + + if (cp->addr.type == BDADDR_BREDR) { + /* Only incoming connections action is supported for now */ + if (cp->action != 0x01) { + err = cmd->cmd_complete(cmd, + MGMT_STATUS_INVALID_PARAMS); + mgmt_pending_remove(cmd); + goto unlock; + } + + err = hci_bdaddr_list_add(&hdev->whitelist, &cp->addr.bdaddr, + cp->addr.type); + if (err) + goto unlock; + + __hci_update_page_scan(&req); + + goto added; + } + + if (cp->addr.type == BDADDR_LE_PUBLIC) + addr_type = ADDR_LE_DEV_PUBLIC; + else + addr_type = ADDR_LE_DEV_RANDOM; + + if (cp->action == 0x02) + auto_conn = HCI_AUTO_CONN_ALWAYS; + else if (cp->action == 0x01) + auto_conn = HCI_AUTO_CONN_DIRECT; + else + auto_conn = HCI_AUTO_CONN_REPORT; + + /* If the connection parameters don't exist for this device, + * they will be created and configured with defaults. + */ + if (hci_conn_params_set(&req, &cp->addr.bdaddr, addr_type, + auto_conn) < 0) { + err = cmd->cmd_complete(cmd, MGMT_STATUS_FAILED); + mgmt_pending_remove(cmd); + goto unlock; + } + +added: + device_added(sk, hdev, &cp->addr.bdaddr, cp->addr.type, cp->action); + + err = hci_req_run(&req, add_device_complete); + if (err < 0) { + /* ENODATA means no HCI commands were needed (e.g. if + * the adapter is powered off). + */ + if (err == -ENODATA) + err = cmd->cmd_complete(cmd, MGMT_STATUS_SUCCESS); + mgmt_pending_remove(cmd); + } + +unlock: + hci_dev_unlock(hdev); + return err; +} + +static void device_removed(struct sock *sk, struct hci_dev *hdev, + bdaddr_t *bdaddr, u8 type) +{ + struct mgmt_ev_device_removed ev; + + bacpy(&ev.addr.bdaddr, bdaddr); + ev.addr.type = type; + + mgmt_event(MGMT_EV_DEVICE_REMOVED, hdev, &ev, sizeof(ev), sk); +} + +static void remove_device_complete(struct hci_dev *hdev, u8 status, u16 opcode) +{ + struct mgmt_pending_cmd *cmd; + + BT_DBG("status 0x%02x", status); + + hci_dev_lock(hdev); + + cmd = pending_find(MGMT_OP_REMOVE_DEVICE, hdev); + if (!cmd) + goto unlock; + + cmd->cmd_complete(cmd, mgmt_status(status)); + mgmt_pending_remove(cmd); + +unlock: + hci_dev_unlock(hdev); +} + +static int remove_device(struct sock *sk, struct hci_dev *hdev, + void *data, u16 len) +{ + struct mgmt_cp_remove_device *cp = data; + struct mgmt_pending_cmd *cmd; + struct hci_request req; + int err; + + BT_DBG("%s", hdev->name); + + hci_req_init(&req, hdev); + + hci_dev_lock(hdev); + + cmd = mgmt_pending_add(sk, MGMT_OP_REMOVE_DEVICE, hdev, data, len); + if (!cmd) { + err = -ENOMEM; + goto unlock; + } + + cmd->cmd_complete = addr_cmd_complete; + + if (bacmp(&cp->addr.bdaddr, BDADDR_ANY)) { + struct hci_conn_params *params; + u8 addr_type; + + if (!bdaddr_type_is_valid(cp->addr.type)) { + err = cmd->cmd_complete(cmd, + MGMT_STATUS_INVALID_PARAMS); + mgmt_pending_remove(cmd); + goto unlock; + } + + if (cp->addr.type == BDADDR_BREDR) { + err = hci_bdaddr_list_del(&hdev->whitelist, + &cp->addr.bdaddr, + cp->addr.type); + if (err) { + err = cmd->cmd_complete(cmd, + MGMT_STATUS_INVALID_PARAMS); + mgmt_pending_remove(cmd); + goto unlock; + } + + __hci_update_page_scan(&req); + + device_removed(sk, hdev, &cp->addr.bdaddr, + cp->addr.type); + goto complete; + } + + if (cp->addr.type == BDADDR_LE_PUBLIC) + addr_type = ADDR_LE_DEV_PUBLIC; + else + addr_type = ADDR_LE_DEV_RANDOM; + + params = hci_conn_params_lookup(hdev, &cp->addr.bdaddr, + addr_type); + if (!params) { + err = cmd->cmd_complete(cmd, + MGMT_STATUS_INVALID_PARAMS); + mgmt_pending_remove(cmd); + goto unlock; + } + + if (params->auto_connect == HCI_AUTO_CONN_DISABLED) { + err = cmd->cmd_complete(cmd, + MGMT_STATUS_INVALID_PARAMS); + mgmt_pending_remove(cmd); + goto unlock; + } + + list_del(¶ms->action); + list_del(¶ms->list); + kfree(params); + __hci_update_background_scan(&req); + + device_removed(sk, hdev, &cp->addr.bdaddr, cp->addr.type); + } else { + struct hci_conn_params *p, *tmp; + struct bdaddr_list *b, *btmp; + + if (cp->addr.type) { + err = cmd->cmd_complete(cmd, + MGMT_STATUS_INVALID_PARAMS); + mgmt_pending_remove(cmd); + goto unlock; + } + + list_for_each_entry_safe(b, btmp, &hdev->whitelist, list) { + device_removed(sk, hdev, &b->bdaddr, b->bdaddr_type); + list_del(&b->list); + kfree(b); + } + + __hci_update_page_scan(&req); + + list_for_each_entry_safe(p, tmp, &hdev->le_conn_params, list) { + if (p->auto_connect == HCI_AUTO_CONN_DISABLED) + continue; + device_removed(sk, hdev, &p->addr, p->addr_type); + list_del(&p->action); + list_del(&p->list); + kfree(p); + } + + BT_DBG("All LE connection parameters were removed"); + + __hci_update_background_scan(&req); + } + +complete: + err = hci_req_run(&req, remove_device_complete); + if (err < 0) { + /* ENODATA means no HCI commands were needed (e.g. if + * the adapter is powered off). + */ + if (err == -ENODATA) + err = cmd->cmd_complete(cmd, MGMT_STATUS_SUCCESS); + mgmt_pending_remove(cmd); + } + +unlock: + hci_dev_unlock(hdev); + return err; +} + +static int load_conn_param(struct sock *sk, struct hci_dev *hdev, void *data, + u16 len) +{ + struct mgmt_cp_load_conn_param *cp = data; + const u16 max_param_count = ((U16_MAX - sizeof(*cp)) / + sizeof(struct mgmt_conn_param)); + u16 param_count, expected_len; + int i; + + if (!lmp_le_capable(hdev)) + return mgmt_cmd_status(sk, hdev->id, MGMT_OP_LOAD_CONN_PARAM, + MGMT_STATUS_NOT_SUPPORTED); + + param_count = __le16_to_cpu(cp->param_count); + if (param_count > max_param_count) { + BT_ERR("load_conn_param: too big param_count value %u", + param_count); + return mgmt_cmd_status(sk, hdev->id, MGMT_OP_LOAD_CONN_PARAM, + MGMT_STATUS_INVALID_PARAMS); + } + + expected_len = sizeof(*cp) + param_count * + sizeof(struct mgmt_conn_param); + if (expected_len != len) { + BT_ERR("load_conn_param: expected %u bytes, got %u bytes", + expected_len, len); + return mgmt_cmd_status(sk, hdev->id, MGMT_OP_LOAD_CONN_PARAM, + MGMT_STATUS_INVALID_PARAMS); + } + + BT_DBG("%s param_count %u", hdev->name, param_count); + + hci_dev_lock(hdev); + + hci_conn_params_clear_disabled(hdev); + + for (i = 0; i < param_count; i++) { + struct mgmt_conn_param *param = &cp->params[i]; + struct hci_conn_params *hci_param; + u16 min, max, latency, timeout; + u8 addr_type; + + BT_DBG("Adding %pMR (type %u)", ¶m->addr.bdaddr, + param->addr.type); + + if (param->addr.type == BDADDR_LE_PUBLIC) { + addr_type = ADDR_LE_DEV_PUBLIC; + } else if (param->addr.type == BDADDR_LE_RANDOM) { + addr_type = ADDR_LE_DEV_RANDOM; + } else { + BT_ERR("Ignoring invalid connection parameters"); + continue; + } + + min = le16_to_cpu(param->min_interval); + max = le16_to_cpu(param->max_interval); + latency = le16_to_cpu(param->latency); + timeout = le16_to_cpu(param->timeout); + + BT_DBG("min 0x%04x max 0x%04x latency 0x%04x timeout 0x%04x", + min, max, latency, timeout); + + if (hci_check_conn_params(min, max, latency, timeout) < 0) { + BT_ERR("Ignoring invalid connection parameters"); + continue; + } + + hci_param = hci_conn_params_add(hdev, ¶m->addr.bdaddr, + addr_type); + if (!hci_param) { + BT_ERR("Failed to add connection parameters"); + continue; + } + + hci_param->conn_min_interval = min; + hci_param->conn_max_interval = max; + hci_param->conn_latency = latency; + hci_param->supervision_timeout = timeout; + } + + hci_dev_unlock(hdev); + + return mgmt_cmd_complete(sk, hdev->id, MGMT_OP_LOAD_CONN_PARAM, 0, + NULL, 0); +} + +static int set_external_config(struct sock *sk, struct hci_dev *hdev, + void *data, u16 len) +{ + struct mgmt_cp_set_external_config *cp = data; + bool changed; + int err; + + BT_DBG("%s", hdev->name); + + if (hdev_is_powered(hdev)) + return mgmt_cmd_status(sk, hdev->id, MGMT_OP_SET_EXTERNAL_CONFIG, + MGMT_STATUS_REJECTED); + + if (cp->config != 0x00 && cp->config != 0x01) + return mgmt_cmd_status(sk, hdev->id, MGMT_OP_SET_EXTERNAL_CONFIG, + MGMT_STATUS_INVALID_PARAMS); + + if (!test_bit(HCI_QUIRK_EXTERNAL_CONFIG, &hdev->quirks)) + return mgmt_cmd_status(sk, hdev->id, MGMT_OP_SET_EXTERNAL_CONFIG, + MGMT_STATUS_NOT_SUPPORTED); + + hci_dev_lock(hdev); + + if (cp->config) + changed = !hci_dev_test_and_set_flag(hdev, HCI_EXT_CONFIGURED); + else + changed = hci_dev_test_and_clear_flag(hdev, HCI_EXT_CONFIGURED); + + err = send_options_rsp(sk, MGMT_OP_SET_EXTERNAL_CONFIG, hdev); + if (err < 0) + goto unlock; + + if (!changed) + goto unlock; + + err = new_options(hdev, sk); + + if (hci_dev_test_flag(hdev, HCI_UNCONFIGURED) == is_configured(hdev)) { + mgmt_index_removed(hdev); + + if (hci_dev_test_and_change_flag(hdev, HCI_UNCONFIGURED)) { + hci_dev_set_flag(hdev, HCI_CONFIG); + hci_dev_set_flag(hdev, HCI_AUTO_OFF); + + queue_work(hdev->req_workqueue, &hdev->power_on); + } else { + set_bit(HCI_RAW, &hdev->flags); + mgmt_index_added(hdev); + } + } + +unlock: + hci_dev_unlock(hdev); + return err; +} + +static int set_public_address(struct sock *sk, struct hci_dev *hdev, + void *data, u16 len) +{ + struct mgmt_cp_set_public_address *cp = data; + bool changed; + int err; + + BT_DBG("%s", hdev->name); + + if (hdev_is_powered(hdev)) + return mgmt_cmd_status(sk, hdev->id, MGMT_OP_SET_PUBLIC_ADDRESS, + MGMT_STATUS_REJECTED); + + if (!bacmp(&cp->bdaddr, BDADDR_ANY)) + return mgmt_cmd_status(sk, hdev->id, MGMT_OP_SET_PUBLIC_ADDRESS, + MGMT_STATUS_INVALID_PARAMS); + + if (!hdev->set_bdaddr) + return mgmt_cmd_status(sk, hdev->id, MGMT_OP_SET_PUBLIC_ADDRESS, + MGMT_STATUS_NOT_SUPPORTED); + + hci_dev_lock(hdev); + + changed = !!bacmp(&hdev->public_addr, &cp->bdaddr); + bacpy(&hdev->public_addr, &cp->bdaddr); + + err = send_options_rsp(sk, MGMT_OP_SET_PUBLIC_ADDRESS, hdev); + if (err < 0) + goto unlock; + + if (!changed) + goto unlock; + + if (hci_dev_test_flag(hdev, HCI_UNCONFIGURED)) + err = new_options(hdev, sk); + + if (is_configured(hdev)) { + mgmt_index_removed(hdev); + + hci_dev_clear_flag(hdev, HCI_UNCONFIGURED); + + hci_dev_set_flag(hdev, HCI_CONFIG); + hci_dev_set_flag(hdev, HCI_AUTO_OFF); + + queue_work(hdev->req_workqueue, &hdev->power_on); + } + +unlock: + hci_dev_unlock(hdev); + return err; +} + +static inline u16 eir_append_data(u8 *eir, u16 eir_len, u8 type, u8 *data, + u8 data_len) +{ + eir[eir_len++] = sizeof(type) + data_len; + eir[eir_len++] = type; + memcpy(&eir[eir_len], data, data_len); + eir_len += data_len; + + return eir_len; +} + +static void read_local_oob_ext_data_complete(struct hci_dev *hdev, u8 status, + u16 opcode, struct sk_buff *skb) +{ + const struct mgmt_cp_read_local_oob_ext_data *mgmt_cp; + struct mgmt_rp_read_local_oob_ext_data *mgmt_rp; + u8 *h192, *r192, *h256, *r256; + struct mgmt_pending_cmd *cmd; + u16 eir_len; + int err; + + BT_DBG("%s status %u", hdev->name, status); + + cmd = pending_find(MGMT_OP_READ_LOCAL_OOB_EXT_DATA, hdev); + if (!cmd) + return; + + mgmt_cp = cmd->param; + + if (status) { + status = mgmt_status(status); + eir_len = 0; + + h192 = NULL; + r192 = NULL; + h256 = NULL; + r256 = NULL; + } else if (opcode == HCI_OP_READ_LOCAL_OOB_DATA) { + struct hci_rp_read_local_oob_data *rp; + + if (skb->len != sizeof(*rp)) { + status = MGMT_STATUS_FAILED; + eir_len = 0; + } else { + status = MGMT_STATUS_SUCCESS; + rp = (void *)skb->data; + + eir_len = 5 + 18 + 18; + h192 = rp->hash; + r192 = rp->rand; + h256 = NULL; + r256 = NULL; + } + } else { + struct hci_rp_read_local_oob_ext_data *rp; + + if (skb->len != sizeof(*rp)) { + status = MGMT_STATUS_FAILED; + eir_len = 0; + } else { + status = MGMT_STATUS_SUCCESS; + rp = (void *)skb->data; + + if (hci_dev_test_flag(hdev, HCI_SC_ONLY)) { + eir_len = 5 + 18 + 18; + h192 = NULL; + r192 = NULL; + } else { + eir_len = 5 + 18 + 18 + 18 + 18; + h192 = rp->hash192; + r192 = rp->rand192; + } + + h256 = rp->hash256; + r256 = rp->rand256; + } + } + + mgmt_rp = kmalloc(sizeof(*mgmt_rp) + eir_len, GFP_KERNEL); + if (!mgmt_rp) + goto done; + + if (status) + goto send_rsp; + + eir_len = eir_append_data(mgmt_rp->eir, 0, EIR_CLASS_OF_DEV, + hdev->dev_class, 3); + + if (h192 && r192) { + eir_len = eir_append_data(mgmt_rp->eir, eir_len, + EIR_SSP_HASH_C192, h192, 16); + eir_len = eir_append_data(mgmt_rp->eir, eir_len, + EIR_SSP_RAND_R192, r192, 16); + } + + if (h256 && r256) { + eir_len = eir_append_data(mgmt_rp->eir, eir_len, + EIR_SSP_HASH_C256, h256, 16); + eir_len = eir_append_data(mgmt_rp->eir, eir_len, + EIR_SSP_RAND_R256, r256, 16); + } + +send_rsp: + mgmt_rp->type = mgmt_cp->type; + mgmt_rp->eir_len = cpu_to_le16(eir_len); + + err = mgmt_cmd_complete(cmd->sk, hdev->id, + MGMT_OP_READ_LOCAL_OOB_EXT_DATA, status, + mgmt_rp, sizeof(*mgmt_rp) + eir_len); + if (err < 0 || status) + goto done; + + hci_sock_set_flag(cmd->sk, HCI_MGMT_OOB_DATA_EVENTS); + + err = mgmt_limited_event(MGMT_EV_LOCAL_OOB_DATA_UPDATED, hdev, + mgmt_rp, sizeof(*mgmt_rp) + eir_len, + HCI_MGMT_OOB_DATA_EVENTS, cmd->sk); +done: + kfree(mgmt_rp); + mgmt_pending_remove(cmd); +} + +static int read_local_ssp_oob_req(struct hci_dev *hdev, struct sock *sk, + struct mgmt_cp_read_local_oob_ext_data *cp) +{ + struct mgmt_pending_cmd *cmd; + struct hci_request req; + int err; + + cmd = mgmt_pending_add(sk, MGMT_OP_READ_LOCAL_OOB_EXT_DATA, hdev, + cp, sizeof(*cp)); + if (!cmd) + return -ENOMEM; + + hci_req_init(&req, hdev); + + if (bredr_sc_enabled(hdev)) + hci_req_add(&req, HCI_OP_READ_LOCAL_OOB_EXT_DATA, 0, NULL); + else + hci_req_add(&req, HCI_OP_READ_LOCAL_OOB_DATA, 0, NULL); + + err = hci_req_run_skb(&req, read_local_oob_ext_data_complete); + if (err < 0) { + mgmt_pending_remove(cmd); + return err; + } + + return 0; +} + +static int read_local_oob_ext_data(struct sock *sk, struct hci_dev *hdev, + void *data, u16 data_len) +{ + struct mgmt_cp_read_local_oob_ext_data *cp = data; + struct mgmt_rp_read_local_oob_ext_data *rp; + size_t rp_len; + u16 eir_len; + u8 status, flags, role, addr[7], hash[16], rand[16]; + int err; + + BT_DBG("%s", hdev->name); + + if (hdev_is_powered(hdev)) { + switch (cp->type) { + case BIT(BDADDR_BREDR): + status = mgmt_bredr_support(hdev); + if (status) + eir_len = 0; + else + eir_len = 5; + break; + case (BIT(BDADDR_LE_PUBLIC) | BIT(BDADDR_LE_RANDOM)): + status = mgmt_le_support(hdev); + if (status) + eir_len = 0; + else + eir_len = 9 + 3 + 18 + 18 + 3; + break; + default: + status = MGMT_STATUS_INVALID_PARAMS; + eir_len = 0; + break; + } + } else { + status = MGMT_STATUS_NOT_POWERED; + eir_len = 0; + } + + rp_len = sizeof(*rp) + eir_len; + rp = kmalloc(rp_len, GFP_ATOMIC); + if (!rp) + return -ENOMEM; + + if (status) + goto complete; + + hci_dev_lock(hdev); + + eir_len = 0; + switch (cp->type) { + case BIT(BDADDR_BREDR): + if (hci_dev_test_flag(hdev, HCI_SSP_ENABLED)) { + err = read_local_ssp_oob_req(hdev, sk, cp); + hci_dev_unlock(hdev); + if (!err) + goto done; + + status = MGMT_STATUS_FAILED; + goto complete; + } else { + eir_len = eir_append_data(rp->eir, eir_len, + EIR_CLASS_OF_DEV, + hdev->dev_class, 3); + } + break; + case (BIT(BDADDR_LE_PUBLIC) | BIT(BDADDR_LE_RANDOM)): + if (hci_dev_test_flag(hdev, HCI_SC_ENABLED) && + smp_generate_oob(hdev, hash, rand) < 0) { + hci_dev_unlock(hdev); + status = MGMT_STATUS_FAILED; + goto complete; + } + + /* This should return the active RPA, but since the RPA + * is only programmed on demand, it is really hard to fill + * this in at the moment. For now disallow retrieving + * local out-of-band data when privacy is in use. + * + * Returning the identity address will not help here since + * pairing happens before the identity resolving key is + * known and thus the connection establishment happens + * based on the RPA and not the identity address. + */ + if (hci_dev_test_flag(hdev, HCI_PRIVACY)) { + hci_dev_unlock(hdev); + status = MGMT_STATUS_REJECTED; + goto complete; + } + + if (hci_dev_test_flag(hdev, HCI_FORCE_STATIC_ADDR) || + !bacmp(&hdev->bdaddr, BDADDR_ANY) || + (!hci_dev_test_flag(hdev, HCI_BREDR_ENABLED) && + bacmp(&hdev->static_addr, BDADDR_ANY))) { + memcpy(addr, &hdev->static_addr, 6); + addr[6] = 0x01; + } else { + memcpy(addr, &hdev->bdaddr, 6); + addr[6] = 0x00; + } + + eir_len = eir_append_data(rp->eir, eir_len, EIR_LE_BDADDR, + addr, sizeof(addr)); + + if (hci_dev_test_flag(hdev, HCI_ADVERTISING)) + role = 0x02; + else + role = 0x01; + + eir_len = eir_append_data(rp->eir, eir_len, EIR_LE_ROLE, + &role, sizeof(role)); + + if (hci_dev_test_flag(hdev, HCI_SC_ENABLED)) { + eir_len = eir_append_data(rp->eir, eir_len, + EIR_LE_SC_CONFIRM, + hash, sizeof(hash)); + + eir_len = eir_append_data(rp->eir, eir_len, + EIR_LE_SC_RANDOM, + rand, sizeof(rand)); + } + + flags = get_adv_discov_flags(hdev); + + if (!hci_dev_test_flag(hdev, HCI_BREDR_ENABLED)) + flags |= LE_AD_NO_BREDR; + + eir_len = eir_append_data(rp->eir, eir_len, EIR_FLAGS, + &flags, sizeof(flags)); + break; + } + + hci_dev_unlock(hdev); + + hci_sock_set_flag(sk, HCI_MGMT_OOB_DATA_EVENTS); + + status = MGMT_STATUS_SUCCESS; + +complete: + rp->type = cp->type; + rp->eir_len = cpu_to_le16(eir_len); + + err = mgmt_cmd_complete(sk, hdev->id, MGMT_OP_READ_LOCAL_OOB_EXT_DATA, + status, rp, sizeof(*rp) + eir_len); + if (err < 0 || status) + goto done; + + err = mgmt_limited_event(MGMT_EV_LOCAL_OOB_DATA_UPDATED, hdev, + rp, sizeof(*rp) + eir_len, + HCI_MGMT_OOB_DATA_EVENTS, sk); + +done: + kfree(rp); + + return err; +} + +static u32 get_supported_adv_flags(struct hci_dev *hdev) +{ + u32 flags = 0; + + flags |= MGMT_ADV_FLAG_CONNECTABLE; + flags |= MGMT_ADV_FLAG_DISCOV; + flags |= MGMT_ADV_FLAG_LIMITED_DISCOV; + flags |= MGMT_ADV_FLAG_MANAGED_FLAGS; + + if (hdev->adv_tx_power != HCI_TX_POWER_INVALID) + flags |= MGMT_ADV_FLAG_TX_POWER; + + return flags; +} + +static int read_adv_features(struct sock *sk, struct hci_dev *hdev, + void *data, u16 data_len) +{ + struct mgmt_rp_read_adv_features *rp; + size_t rp_len; + int err; + bool instance; + u32 supported_flags; + + BT_DBG("%s", hdev->name); + + if (!lmp_le_capable(hdev)) + return mgmt_cmd_status(sk, hdev->id, MGMT_OP_READ_ADV_FEATURES, + MGMT_STATUS_REJECTED); + + hci_dev_lock(hdev); + + rp_len = sizeof(*rp); + + /* Currently only one instance is supported, so just add 1 to the + * response length. + */ + instance = hci_dev_test_flag(hdev, HCI_ADVERTISING_INSTANCE); + if (instance) + rp_len++; + + rp = kmalloc(rp_len, GFP_ATOMIC); + if (!rp) { + hci_dev_unlock(hdev); + return -ENOMEM; + } + + supported_flags = get_supported_adv_flags(hdev); + + rp->supported_flags = cpu_to_le32(supported_flags); + rp->max_adv_data_len = HCI_MAX_AD_LENGTH; + rp->max_scan_rsp_len = HCI_MAX_AD_LENGTH; + rp->max_instances = 1; + + /* Currently only one instance is supported, so simply return the + * current instance number. + */ + if (instance) { + rp->num_instances = 1; + rp->instance[0] = 1; + } else { + rp->num_instances = 0; + } + + hci_dev_unlock(hdev); + + err = mgmt_cmd_complete(sk, hdev->id, MGMT_OP_READ_ADV_FEATURES, + MGMT_STATUS_SUCCESS, rp, rp_len); + + kfree(rp); + + return err; +} + +static bool tlv_data_is_valid(struct hci_dev *hdev, u32 adv_flags, u8 *data, + u8 len, bool is_adv_data) +{ + u8 max_len = HCI_MAX_AD_LENGTH; + int i, cur_len; + bool flags_managed = false; + bool tx_power_managed = false; + u32 flags_params = MGMT_ADV_FLAG_DISCOV | MGMT_ADV_FLAG_LIMITED_DISCOV | + MGMT_ADV_FLAG_MANAGED_FLAGS; + + if (is_adv_data && (adv_flags & flags_params)) { + flags_managed = true; + max_len -= 3; + } + + if (is_adv_data && (adv_flags & MGMT_ADV_FLAG_TX_POWER)) { + tx_power_managed = true; + max_len -= 3; + } + + if (len > max_len) + return false; + + /* Make sure that the data is correctly formatted. */ + for (i = 0, cur_len = 0; i < len; i += (cur_len + 1)) { + cur_len = data[i]; + + if (flags_managed && data[i + 1] == EIR_FLAGS) + return false; + + if (tx_power_managed && data[i + 1] == EIR_TX_POWER) + return false; + + /* If the current field length would exceed the total data + * length, then it's invalid. + */ + if (i + cur_len >= len) + return false; + } + + return true; +} + +static void add_advertising_complete(struct hci_dev *hdev, u8 status, + u16 opcode) +{ + struct mgmt_pending_cmd *cmd; + struct mgmt_rp_add_advertising rp; + + BT_DBG("status %d", status); + + hci_dev_lock(hdev); + + cmd = pending_find(MGMT_OP_ADD_ADVERTISING, hdev); + + if (status) { + hci_dev_clear_flag(hdev, HCI_ADVERTISING_INSTANCE); + memset(&hdev->adv_instance, 0, sizeof(hdev->adv_instance)); + advertising_removed(cmd ? cmd->sk : NULL, hdev, 1); + } + + if (!cmd) + goto unlock; + + rp.instance = 0x01; + + if (status) + mgmt_cmd_status(cmd->sk, cmd->index, cmd->opcode, + mgmt_status(status)); + else + mgmt_cmd_complete(cmd->sk, cmd->index, cmd->opcode, + mgmt_status(status), &rp, sizeof(rp)); + + mgmt_pending_remove(cmd); + +unlock: + hci_dev_unlock(hdev); +} + +static void adv_timeout_expired(struct work_struct *work) +{ + struct hci_dev *hdev = container_of(work, struct hci_dev, + adv_instance.timeout_exp.work); + + hdev->adv_instance.timeout = 0; + + hci_dev_lock(hdev); + clear_adv_instance(hdev); + hci_dev_unlock(hdev); +} + +static int add_advertising(struct sock *sk, struct hci_dev *hdev, + void *data, u16 data_len) +{ + struct mgmt_cp_add_advertising *cp = data; + struct mgmt_rp_add_advertising rp; + u32 flags; + u32 supported_flags; + u8 status; + u16 timeout; + int err; + struct mgmt_pending_cmd *cmd; + struct hci_request req; + + BT_DBG("%s", hdev->name); + + status = mgmt_le_support(hdev); + if (status) + return mgmt_cmd_status(sk, hdev->id, MGMT_OP_ADD_ADVERTISING, + status); + + flags = __le32_to_cpu(cp->flags); + timeout = __le16_to_cpu(cp->timeout); + + /* The current implementation only supports adding one instance and only + * a subset of the specified flags. + */ + supported_flags = get_supported_adv_flags(hdev); + if (cp->instance != 0x01 || (flags & ~supported_flags)) + return mgmt_cmd_status(sk, hdev->id, MGMT_OP_ADD_ADVERTISING, + MGMT_STATUS_INVALID_PARAMS); + + hci_dev_lock(hdev); + + if (timeout && !hdev_is_powered(hdev)) { + err = mgmt_cmd_status(sk, hdev->id, MGMT_OP_ADD_ADVERTISING, + MGMT_STATUS_REJECTED); + goto unlock; + } + + if (pending_find(MGMT_OP_ADD_ADVERTISING, hdev) || + pending_find(MGMT_OP_REMOVE_ADVERTISING, hdev) || + pending_find(MGMT_OP_SET_LE, hdev)) { + err = mgmt_cmd_status(sk, hdev->id, MGMT_OP_ADD_ADVERTISING, + MGMT_STATUS_BUSY); + goto unlock; + } + + if (!tlv_data_is_valid(hdev, flags, cp->data, cp->adv_data_len, true) || + !tlv_data_is_valid(hdev, flags, cp->data + cp->adv_data_len, + cp->scan_rsp_len, false)) { + err = mgmt_cmd_status(sk, hdev->id, MGMT_OP_ADD_ADVERTISING, + MGMT_STATUS_INVALID_PARAMS); + goto unlock; + } + + INIT_DELAYED_WORK(&hdev->adv_instance.timeout_exp, adv_timeout_expired); + + hdev->adv_instance.flags = flags; + hdev->adv_instance.adv_data_len = cp->adv_data_len; + hdev->adv_instance.scan_rsp_len = cp->scan_rsp_len; + + if (cp->adv_data_len) + memcpy(hdev->adv_instance.adv_data, cp->data, cp->adv_data_len); + + if (cp->scan_rsp_len) + memcpy(hdev->adv_instance.scan_rsp_data, + cp->data + cp->adv_data_len, cp->scan_rsp_len); + + if (hdev->adv_instance.timeout) + cancel_delayed_work(&hdev->adv_instance.timeout_exp); + + hdev->adv_instance.timeout = timeout; + + if (timeout) + queue_delayed_work(hdev->workqueue, + &hdev->adv_instance.timeout_exp, + msecs_to_jiffies(timeout * 1000)); + + if (!hci_dev_test_and_set_flag(hdev, HCI_ADVERTISING_INSTANCE)) + advertising_added(sk, hdev, 1); + + /* If the HCI_ADVERTISING flag is set or the device isn't powered then + * we have no HCI communication to make. Simply return. + */ + if (!hdev_is_powered(hdev) || + hci_dev_test_flag(hdev, HCI_ADVERTISING)) { + rp.instance = 0x01; + err = mgmt_cmd_complete(sk, hdev->id, MGMT_OP_ADD_ADVERTISING, + MGMT_STATUS_SUCCESS, &rp, sizeof(rp)); + goto unlock; + } + + /* We're good to go, update advertising data, parameters, and start + * advertising. + */ + cmd = mgmt_pending_add(sk, MGMT_OP_ADD_ADVERTISING, hdev, data, + data_len); + if (!cmd) { + err = -ENOMEM; + goto unlock; + } + + hci_req_init(&req, hdev); + + update_adv_data(&req); + update_scan_rsp_data(&req); + enable_advertising(&req); + + err = hci_req_run(&req, add_advertising_complete); + if (err < 0) + mgmt_pending_remove(cmd); + +unlock: + hci_dev_unlock(hdev); + + return err; +} + +static void remove_advertising_complete(struct hci_dev *hdev, u8 status, + u16 opcode) +{ + struct mgmt_pending_cmd *cmd; + struct mgmt_rp_remove_advertising rp; + + BT_DBG("status %d", status); + + hci_dev_lock(hdev); + + /* A failure status here only means that we failed to disable + * advertising. Otherwise, the advertising instance has been removed, + * so report success. + */ + cmd = pending_find(MGMT_OP_REMOVE_ADVERTISING, hdev); + if (!cmd) + goto unlock; + + rp.instance = 1; + + mgmt_cmd_complete(cmd->sk, cmd->index, cmd->opcode, MGMT_STATUS_SUCCESS, + &rp, sizeof(rp)); + mgmt_pending_remove(cmd); + +unlock: + hci_dev_unlock(hdev); +} + +static int remove_advertising(struct sock *sk, struct hci_dev *hdev, + void *data, u16 data_len) +{ + struct mgmt_cp_remove_advertising *cp = data; + struct mgmt_rp_remove_advertising rp; + int err; + struct mgmt_pending_cmd *cmd; + struct hci_request req; + + BT_DBG("%s", hdev->name); + + /* The current implementation only allows modifying instance no 1. A + * value of 0 indicates that all instances should be cleared. + */ + if (cp->instance > 1) + return mgmt_cmd_status(sk, hdev->id, MGMT_OP_REMOVE_ADVERTISING, + MGMT_STATUS_INVALID_PARAMS); + + hci_dev_lock(hdev); + + if (pending_find(MGMT_OP_ADD_ADVERTISING, hdev) || + pending_find(MGMT_OP_REMOVE_ADVERTISING, hdev) || + pending_find(MGMT_OP_SET_LE, hdev)) { + err = mgmt_cmd_status(sk, hdev->id, MGMT_OP_REMOVE_ADVERTISING, + MGMT_STATUS_BUSY); + goto unlock; + } + + if (!hci_dev_test_flag(hdev, HCI_ADVERTISING_INSTANCE)) { + err = mgmt_cmd_status(sk, hdev->id, MGMT_OP_REMOVE_ADVERTISING, + MGMT_STATUS_INVALID_PARAMS); + goto unlock; + } + + if (hdev->adv_instance.timeout) + cancel_delayed_work(&hdev->adv_instance.timeout_exp); + + memset(&hdev->adv_instance, 0, sizeof(hdev->adv_instance)); + + advertising_removed(sk, hdev, 1); + + hci_dev_clear_flag(hdev, HCI_ADVERTISING_INSTANCE); + + /* If the HCI_ADVERTISING flag is set or the device isn't powered then + * we have no HCI communication to make. Simply return. + */ + if (!hdev_is_powered(hdev) || + hci_dev_test_flag(hdev, HCI_ADVERTISING)) { + rp.instance = 1; + err = mgmt_cmd_complete(sk, hdev->id, + MGMT_OP_REMOVE_ADVERTISING, + MGMT_STATUS_SUCCESS, &rp, sizeof(rp)); + goto unlock; + } + + cmd = mgmt_pending_add(sk, MGMT_OP_REMOVE_ADVERTISING, hdev, data, + data_len); + if (!cmd) { + err = -ENOMEM; + goto unlock; + } + + hci_req_init(&req, hdev); + disable_advertising(&req); + + err = hci_req_run(&req, remove_advertising_complete); + if (err < 0) + mgmt_pending_remove(cmd); + +unlock: + hci_dev_unlock(hdev); + + return err; +} + +static const struct hci_mgmt_handler mgmt_handlers[] = { + { NULL }, /* 0x0000 (no command) */ + { read_version, MGMT_READ_VERSION_SIZE, + HCI_MGMT_NO_HDEV | + HCI_MGMT_UNTRUSTED }, + { read_commands, MGMT_READ_COMMANDS_SIZE, + HCI_MGMT_NO_HDEV | + HCI_MGMT_UNTRUSTED }, + { read_index_list, MGMT_READ_INDEX_LIST_SIZE, + HCI_MGMT_NO_HDEV | + HCI_MGMT_UNTRUSTED }, + { read_controller_info, MGMT_READ_INFO_SIZE, + HCI_MGMT_UNTRUSTED }, + { set_powered, MGMT_SETTING_SIZE }, + { set_discoverable, MGMT_SET_DISCOVERABLE_SIZE }, + { set_connectable, MGMT_SETTING_SIZE }, + { set_fast_connectable, MGMT_SETTING_SIZE }, + { set_bondable, MGMT_SETTING_SIZE }, + { set_link_security, MGMT_SETTING_SIZE }, + { set_ssp, MGMT_SETTING_SIZE }, + { set_hs, MGMT_SETTING_SIZE }, + { set_le, MGMT_SETTING_SIZE }, + { set_dev_class, MGMT_SET_DEV_CLASS_SIZE }, + { set_local_name, MGMT_SET_LOCAL_NAME_SIZE }, + { add_uuid, MGMT_ADD_UUID_SIZE }, + { remove_uuid, MGMT_REMOVE_UUID_SIZE }, + { load_link_keys, MGMT_LOAD_LINK_KEYS_SIZE, + HCI_MGMT_VAR_LEN }, + { load_long_term_keys, MGMT_LOAD_LONG_TERM_KEYS_SIZE, + HCI_MGMT_VAR_LEN }, + { disconnect, MGMT_DISCONNECT_SIZE }, + { get_connections, MGMT_GET_CONNECTIONS_SIZE }, + { pin_code_reply, MGMT_PIN_CODE_REPLY_SIZE }, + { pin_code_neg_reply, MGMT_PIN_CODE_NEG_REPLY_SIZE }, + { set_io_capability, MGMT_SET_IO_CAPABILITY_SIZE }, + { pair_device, MGMT_PAIR_DEVICE_SIZE }, + { cancel_pair_device, MGMT_CANCEL_PAIR_DEVICE_SIZE }, + { unpair_device, MGMT_UNPAIR_DEVICE_SIZE }, + { user_confirm_reply, MGMT_USER_CONFIRM_REPLY_SIZE }, + { user_confirm_neg_reply, MGMT_USER_CONFIRM_NEG_REPLY_SIZE }, + { user_passkey_reply, MGMT_USER_PASSKEY_REPLY_SIZE }, + { user_passkey_neg_reply, MGMT_USER_PASSKEY_NEG_REPLY_SIZE }, + { read_local_oob_data, MGMT_READ_LOCAL_OOB_DATA_SIZE }, + { add_remote_oob_data, MGMT_ADD_REMOTE_OOB_DATA_SIZE, + HCI_MGMT_VAR_LEN }, + { remove_remote_oob_data, MGMT_REMOVE_REMOTE_OOB_DATA_SIZE }, + { start_discovery, MGMT_START_DISCOVERY_SIZE }, + { stop_discovery, MGMT_STOP_DISCOVERY_SIZE }, + { confirm_name, MGMT_CONFIRM_NAME_SIZE }, + { block_device, MGMT_BLOCK_DEVICE_SIZE }, + { unblock_device, MGMT_UNBLOCK_DEVICE_SIZE }, + { set_device_id, MGMT_SET_DEVICE_ID_SIZE }, + { set_advertising, MGMT_SETTING_SIZE }, + { set_bredr, MGMT_SETTING_SIZE }, + { set_static_address, MGMT_SET_STATIC_ADDRESS_SIZE }, + { set_scan_params, MGMT_SET_SCAN_PARAMS_SIZE }, + { set_secure_conn, MGMT_SETTING_SIZE }, + { set_debug_keys, MGMT_SETTING_SIZE }, + { set_privacy, MGMT_SET_PRIVACY_SIZE }, + { load_irks, MGMT_LOAD_IRKS_SIZE, + HCI_MGMT_VAR_LEN }, + { get_conn_info, MGMT_GET_CONN_INFO_SIZE }, + { get_clock_info, MGMT_GET_CLOCK_INFO_SIZE }, + { add_device, MGMT_ADD_DEVICE_SIZE }, + { remove_device, MGMT_REMOVE_DEVICE_SIZE }, + { load_conn_param, MGMT_LOAD_CONN_PARAM_SIZE, + HCI_MGMT_VAR_LEN }, + { read_unconf_index_list, MGMT_READ_UNCONF_INDEX_LIST_SIZE, + HCI_MGMT_NO_HDEV | + HCI_MGMT_UNTRUSTED }, + { read_config_info, MGMT_READ_CONFIG_INFO_SIZE, + HCI_MGMT_UNCONFIGURED | + HCI_MGMT_UNTRUSTED }, + { set_external_config, MGMT_SET_EXTERNAL_CONFIG_SIZE, + HCI_MGMT_UNCONFIGURED }, + { set_public_address, MGMT_SET_PUBLIC_ADDRESS_SIZE, + HCI_MGMT_UNCONFIGURED }, + { start_service_discovery, MGMT_START_SERVICE_DISCOVERY_SIZE, + HCI_MGMT_VAR_LEN }, + { read_local_oob_ext_data, MGMT_READ_LOCAL_OOB_EXT_DATA_SIZE }, + { read_ext_index_list, MGMT_READ_EXT_INDEX_LIST_SIZE, + HCI_MGMT_NO_HDEV | + HCI_MGMT_UNTRUSTED }, + { read_adv_features, MGMT_READ_ADV_FEATURES_SIZE }, + { add_advertising, MGMT_ADD_ADVERTISING_SIZE, + HCI_MGMT_VAR_LEN }, + { remove_advertising, MGMT_REMOVE_ADVERTISING_SIZE }, +}; + +void mgmt_index_added(struct hci_dev *hdev) +{ + struct mgmt_ev_ext_index ev; + + if (test_bit(HCI_QUIRK_RAW_DEVICE, &hdev->quirks)) + return; + + switch (hdev->dev_type) { + case HCI_BREDR: + if (hci_dev_test_flag(hdev, HCI_UNCONFIGURED)) { + mgmt_index_event(MGMT_EV_UNCONF_INDEX_ADDED, hdev, + NULL, 0, HCI_MGMT_UNCONF_INDEX_EVENTS); + ev.type = 0x01; + } else { + mgmt_index_event(MGMT_EV_INDEX_ADDED, hdev, NULL, 0, + HCI_MGMT_INDEX_EVENTS); + ev.type = 0x00; + } + break; + case HCI_AMP: + ev.type = 0x02; + break; + default: + return; + } + + ev.bus = hdev->bus; + + mgmt_index_event(MGMT_EV_EXT_INDEX_ADDED, hdev, &ev, sizeof(ev), + HCI_MGMT_EXT_INDEX_EVENTS); +} + +void mgmt_index_removed(struct hci_dev *hdev) +{ + struct mgmt_ev_ext_index ev; + u8 status = MGMT_STATUS_INVALID_INDEX; + + if (test_bit(HCI_QUIRK_RAW_DEVICE, &hdev->quirks)) + return; + + switch (hdev->dev_type) { + case HCI_BREDR: + mgmt_pending_foreach(0, hdev, cmd_complete_rsp, &status); + + if (hci_dev_test_flag(hdev, HCI_UNCONFIGURED)) { + mgmt_index_event(MGMT_EV_UNCONF_INDEX_REMOVED, hdev, + NULL, 0, HCI_MGMT_UNCONF_INDEX_EVENTS); + ev.type = 0x01; + } else { + mgmt_index_event(MGMT_EV_INDEX_REMOVED, hdev, NULL, 0, + HCI_MGMT_INDEX_EVENTS); + ev.type = 0x00; + } + break; + case HCI_AMP: + ev.type = 0x02; + break; + default: + return; + } + + ev.bus = hdev->bus; + + mgmt_index_event(MGMT_EV_EXT_INDEX_REMOVED, hdev, &ev, sizeof(ev), + HCI_MGMT_EXT_INDEX_EVENTS); +} + +/* This function requires the caller holds hdev->lock */ +static void restart_le_actions(struct hci_request *req) +{ + struct hci_dev *hdev = req->hdev; + struct hci_conn_params *p; + + list_for_each_entry(p, &hdev->le_conn_params, list) { + /* Needed for AUTO_OFF case where might not "really" + * have been powered off. + */ + list_del_init(&p->action); + + switch (p->auto_connect) { + case HCI_AUTO_CONN_DIRECT: + case HCI_AUTO_CONN_ALWAYS: + list_add(&p->action, &hdev->pend_le_conns); + break; + case HCI_AUTO_CONN_REPORT: + list_add(&p->action, &hdev->pend_le_reports); + break; + default: + break; + } + } + + __hci_update_background_scan(req); +} + +static void powered_complete(struct hci_dev *hdev, u8 status, u16 opcode) +{ + struct cmd_lookup match = { NULL, hdev }; + + BT_DBG("status 0x%02x", status); + + if (!status) { + /* Register the available SMP channels (BR/EDR and LE) only + * when successfully powering on the controller. This late + * registration is required so that LE SMP can clearly + * decide if the public address or static address is used. + */ + smp_register(hdev); + } + + hci_dev_lock(hdev); + + mgmt_pending_foreach(MGMT_OP_SET_POWERED, hdev, settings_rsp, &match); + + new_settings(hdev, match.sk); + + hci_dev_unlock(hdev); + + if (match.sk) + sock_put(match.sk); +} + +static int powered_update_hci(struct hci_dev *hdev) +{ + struct hci_request req; + u8 link_sec; + + hci_req_init(&req, hdev); + + if (hci_dev_test_flag(hdev, HCI_SSP_ENABLED) && + !lmp_host_ssp_capable(hdev)) { + u8 mode = 0x01; + + hci_req_add(&req, HCI_OP_WRITE_SSP_MODE, sizeof(mode), &mode); + + if (bredr_sc_enabled(hdev) && !lmp_host_sc_capable(hdev)) { + u8 support = 0x01; + + hci_req_add(&req, HCI_OP_WRITE_SC_SUPPORT, + sizeof(support), &support); + } + } + + if (hci_dev_test_flag(hdev, HCI_LE_ENABLED) && + lmp_bredr_capable(hdev)) { + struct hci_cp_write_le_host_supported cp; + + cp.le = 0x01; + cp.simul = 0x00; + + /* Check first if we already have the right + * host state (host features set) + */ + if (cp.le != lmp_host_le_capable(hdev) || + cp.simul != lmp_host_le_br_capable(hdev)) + hci_req_add(&req, HCI_OP_WRITE_LE_HOST_SUPPORTED, + sizeof(cp), &cp); + } + + if (lmp_le_capable(hdev)) { + /* Make sure the controller has a good default for + * advertising data. This also applies to the case + * where BR/EDR was toggled during the AUTO_OFF phase. + */ + if (hci_dev_test_flag(hdev, HCI_LE_ENABLED)) { + update_adv_data(&req); + update_scan_rsp_data(&req); + } + + if (hci_dev_test_flag(hdev, HCI_ADVERTISING) || + hci_dev_test_flag(hdev, HCI_ADVERTISING_INSTANCE)) + enable_advertising(&req); + + restart_le_actions(&req); + } + + link_sec = hci_dev_test_flag(hdev, HCI_LINK_SECURITY); + if (link_sec != test_bit(HCI_AUTH, &hdev->flags)) + hci_req_add(&req, HCI_OP_WRITE_AUTH_ENABLE, + sizeof(link_sec), &link_sec); + + if (lmp_bredr_capable(hdev)) { + if (hci_dev_test_flag(hdev, HCI_FAST_CONNECTABLE)) + write_fast_connectable(&req, true); + else + write_fast_connectable(&req, false); + __hci_update_page_scan(&req); + update_class(&req); + update_name(&req); + update_eir(&req); + } + + return hci_req_run(&req, powered_complete); +} + +int mgmt_powered(struct hci_dev *hdev, u8 powered) +{ + struct cmd_lookup match = { NULL, hdev }; + u8 status, zero_cod[] = { 0, 0, 0 }; + int err; + + if (!hci_dev_test_flag(hdev, HCI_MGMT)) + return 0; + + if (powered) { + if (powered_update_hci(hdev) == 0) + return 0; + + mgmt_pending_foreach(MGMT_OP_SET_POWERED, hdev, settings_rsp, + &match); + goto new_settings; + } + + mgmt_pending_foreach(MGMT_OP_SET_POWERED, hdev, settings_rsp, &match); + + /* If the power off is because of hdev unregistration let + * use the appropriate INVALID_INDEX status. Otherwise use + * NOT_POWERED. We cover both scenarios here since later in + * mgmt_index_removed() any hci_conn callbacks will have already + * been triggered, potentially causing misleading DISCONNECTED + * status responses. + */ + if (hci_dev_test_flag(hdev, HCI_UNREGISTER)) + status = MGMT_STATUS_INVALID_INDEX; + else + status = MGMT_STATUS_NOT_POWERED; + + mgmt_pending_foreach(0, hdev, cmd_complete_rsp, &status); + + if (memcmp(hdev->dev_class, zero_cod, sizeof(zero_cod)) != 0) + mgmt_generic_event(MGMT_EV_CLASS_OF_DEV_CHANGED, hdev, + zero_cod, sizeof(zero_cod), NULL); + +new_settings: + err = new_settings(hdev, match.sk); + + if (match.sk) + sock_put(match.sk); + + return err; +} + +void mgmt_set_powered_failed(struct hci_dev *hdev, int err) +{ + struct mgmt_pending_cmd *cmd; + u8 status; + + cmd = pending_find(MGMT_OP_SET_POWERED, hdev); + if (!cmd) + return; + + if (err == -ERFKILL) + status = MGMT_STATUS_RFKILLED; + else + status = MGMT_STATUS_FAILED; + + mgmt_cmd_status(cmd->sk, hdev->id, MGMT_OP_SET_POWERED, status); + + mgmt_pending_remove(cmd); +} + +void mgmt_discoverable_timeout(struct hci_dev *hdev) +{ + struct hci_request req; + + hci_dev_lock(hdev); + + /* When discoverable timeout triggers, then just make sure + * the limited discoverable flag is cleared. Even in the case + * of a timeout triggered from general discoverable, it is + * safe to unconditionally clear the flag. + */ + hci_dev_clear_flag(hdev, HCI_LIMITED_DISCOVERABLE); + hci_dev_clear_flag(hdev, HCI_DISCOVERABLE); + + hci_req_init(&req, hdev); + if (hci_dev_test_flag(hdev, HCI_BREDR_ENABLED)) { + u8 scan = SCAN_PAGE; + hci_req_add(&req, HCI_OP_WRITE_SCAN_ENABLE, + sizeof(scan), &scan); + } + update_class(&req); + + /* Advertising instances don't use the global discoverable setting, so + * only update AD if advertising was enabled using Set Advertising. + */ + if (hci_dev_test_flag(hdev, HCI_ADVERTISING)) + update_adv_data(&req); + + hci_req_run(&req, NULL); + + hdev->discov_timeout = 0; + + new_settings(hdev, NULL); + + hci_dev_unlock(hdev); +} + +void mgmt_new_link_key(struct hci_dev *hdev, struct link_key *key, + bool persistent) +{ + struct mgmt_ev_new_link_key ev; + + memset(&ev, 0, sizeof(ev)); + + ev.store_hint = persistent; + bacpy(&ev.key.addr.bdaddr, &key->bdaddr); + ev.key.addr.type = BDADDR_BREDR; + ev.key.type = key->type; + memcpy(ev.key.val, key->val, HCI_LINK_KEY_SIZE); + ev.key.pin_len = key->pin_len; + + mgmt_event(MGMT_EV_NEW_LINK_KEY, hdev, &ev, sizeof(ev), NULL); +} + +static u8 mgmt_ltk_type(struct smp_ltk *ltk) +{ + switch (ltk->type) { + case SMP_LTK: + case SMP_LTK_SLAVE: + if (ltk->authenticated) + return MGMT_LTK_AUTHENTICATED; + return MGMT_LTK_UNAUTHENTICATED; + case SMP_LTK_P256: + if (ltk->authenticated) + return MGMT_LTK_P256_AUTH; + return MGMT_LTK_P256_UNAUTH; + case SMP_LTK_P256_DEBUG: + return MGMT_LTK_P256_DEBUG; + } + + return MGMT_LTK_UNAUTHENTICATED; +} + +void mgmt_new_ltk(struct hci_dev *hdev, struct smp_ltk *key, bool persistent) +{ + struct mgmt_ev_new_long_term_key ev; + + memset(&ev, 0, sizeof(ev)); + + /* Devices using resolvable or non-resolvable random addresses + * without providing an indentity resolving key don't require + * to store long term keys. Their addresses will change the + * next time around. + * + * Only when a remote device provides an identity address + * make sure the long term key is stored. If the remote + * identity is known, the long term keys are internally + * mapped to the identity address. So allow static random + * and public addresses here. + */ + if (key->bdaddr_type == ADDR_LE_DEV_RANDOM && + (key->bdaddr.b[5] & 0xc0) != 0xc0) + ev.store_hint = 0x00; + else + ev.store_hint = persistent; + + bacpy(&ev.key.addr.bdaddr, &key->bdaddr); + ev.key.addr.type = link_to_bdaddr(LE_LINK, key->bdaddr_type); + ev.key.type = mgmt_ltk_type(key); + ev.key.enc_size = key->enc_size; + ev.key.ediv = key->ediv; + ev.key.rand = key->rand; + + if (key->type == SMP_LTK) + ev.key.master = 1; + + memcpy(ev.key.val, key->val, sizeof(key->val)); + + mgmt_event(MGMT_EV_NEW_LONG_TERM_KEY, hdev, &ev, sizeof(ev), NULL); +} + +void mgmt_new_irk(struct hci_dev *hdev, struct smp_irk *irk) +{ + struct mgmt_ev_new_irk ev; + + memset(&ev, 0, sizeof(ev)); + + /* For identity resolving keys from devices that are already + * using a public address or static random address, do not + * ask for storing this key. The identity resolving key really + * is only mandatory for devices using resovlable random + * addresses. + * + * Storing all identity resolving keys has the downside that + * they will be also loaded on next boot of they system. More + * identity resolving keys, means more time during scanning is + * needed to actually resolve these addresses. + */ + if (bacmp(&irk->rpa, BDADDR_ANY)) + ev.store_hint = 0x01; + else + ev.store_hint = 0x00; + + bacpy(&ev.rpa, &irk->rpa); + bacpy(&ev.irk.addr.bdaddr, &irk->bdaddr); + ev.irk.addr.type = link_to_bdaddr(LE_LINK, irk->addr_type); + memcpy(ev.irk.val, irk->val, sizeof(irk->val)); + + mgmt_event(MGMT_EV_NEW_IRK, hdev, &ev, sizeof(ev), NULL); +} + +void mgmt_new_csrk(struct hci_dev *hdev, struct smp_csrk *csrk, + bool persistent) +{ + struct mgmt_ev_new_csrk ev; + + memset(&ev, 0, sizeof(ev)); + + /* Devices using resolvable or non-resolvable random addresses + * without providing an indentity resolving key don't require + * to store signature resolving keys. Their addresses will change + * the next time around. + * + * Only when a remote device provides an identity address + * make sure the signature resolving key is stored. So allow + * static random and public addresses here. + */ + if (csrk->bdaddr_type == ADDR_LE_DEV_RANDOM && + (csrk->bdaddr.b[5] & 0xc0) != 0xc0) + ev.store_hint = 0x00; + else + ev.store_hint = persistent; + + bacpy(&ev.key.addr.bdaddr, &csrk->bdaddr); + ev.key.addr.type = link_to_bdaddr(LE_LINK, csrk->bdaddr_type); + ev.key.type = csrk->type; + memcpy(ev.key.val, csrk->val, sizeof(csrk->val)); + + mgmt_event(MGMT_EV_NEW_CSRK, hdev, &ev, sizeof(ev), NULL); +} + +void mgmt_new_conn_param(struct hci_dev *hdev, bdaddr_t *bdaddr, + u8 bdaddr_type, u8 store_hint, u16 min_interval, + u16 max_interval, u16 latency, u16 timeout) +{ + struct mgmt_ev_new_conn_param ev; + + if (!hci_is_identity_address(bdaddr, bdaddr_type)) + return; + + memset(&ev, 0, sizeof(ev)); + bacpy(&ev.addr.bdaddr, bdaddr); + ev.addr.type = link_to_bdaddr(LE_LINK, bdaddr_type); + ev.store_hint = store_hint; + ev.min_interval = cpu_to_le16(min_interval); + ev.max_interval = cpu_to_le16(max_interval); + ev.latency = cpu_to_le16(latency); + ev.timeout = cpu_to_le16(timeout); + + mgmt_event(MGMT_EV_NEW_CONN_PARAM, hdev, &ev, sizeof(ev), NULL); +} + +void mgmt_device_connected(struct hci_dev *hdev, struct hci_conn *conn, + u32 flags, u8 *name, u8 name_len) +{ + char buf[512]; + struct mgmt_ev_device_connected *ev = (void *) buf; + u16 eir_len = 0; + + bacpy(&ev->addr.bdaddr, &conn->dst); + ev->addr.type = link_to_bdaddr(conn->type, conn->dst_type); + + ev->flags = __cpu_to_le32(flags); + + /* We must ensure that the EIR Data fields are ordered and + * unique. Keep it simple for now and avoid the problem by not + * adding any BR/EDR data to the LE adv. + */ + if (conn->le_adv_data_len > 0) { + memcpy(&ev->eir[eir_len], + conn->le_adv_data, conn->le_adv_data_len); + eir_len = conn->le_adv_data_len; + } else { + if (name_len > 0) + eir_len = eir_append_data(ev->eir, 0, EIR_NAME_COMPLETE, + name, name_len); + + if (memcmp(conn->dev_class, "\0\0\0", 3) != 0) + eir_len = eir_append_data(ev->eir, eir_len, + EIR_CLASS_OF_DEV, + conn->dev_class, 3); + } + + ev->eir_len = cpu_to_le16(eir_len); + + mgmt_event(MGMT_EV_DEVICE_CONNECTED, hdev, buf, + sizeof(*ev) + eir_len, NULL); +} + +static void disconnect_rsp(struct mgmt_pending_cmd *cmd, void *data) +{ + struct sock **sk = data; + + cmd->cmd_complete(cmd, 0); + + *sk = cmd->sk; + sock_hold(*sk); + + mgmt_pending_remove(cmd); +} + +static void unpair_device_rsp(struct mgmt_pending_cmd *cmd, void *data) +{ + struct hci_dev *hdev = data; + struct mgmt_cp_unpair_device *cp = cmd->param; + + device_unpaired(hdev, &cp->addr.bdaddr, cp->addr.type, cmd->sk); + + cmd->cmd_complete(cmd, 0); + mgmt_pending_remove(cmd); +} + +bool mgmt_powering_down(struct hci_dev *hdev) +{ + struct mgmt_pending_cmd *cmd; + struct mgmt_mode *cp; + + cmd = pending_find(MGMT_OP_SET_POWERED, hdev); + if (!cmd) + return false; + + cp = cmd->param; + if (!cp->val) + return true; + + return false; +} + +void mgmt_device_disconnected(struct hci_dev *hdev, bdaddr_t *bdaddr, + u8 link_type, u8 addr_type, u8 reason, + bool mgmt_connected) +{ + struct mgmt_ev_device_disconnected ev; + struct sock *sk = NULL; + + /* The connection is still in hci_conn_hash so test for 1 + * instead of 0 to know if this is the last one. + */ + if (mgmt_powering_down(hdev) && hci_conn_count(hdev) == 1) { + cancel_delayed_work(&hdev->power_off); + queue_work(hdev->req_workqueue, &hdev->power_off.work); + } + + if (!mgmt_connected) + return; + + if (link_type != ACL_LINK && link_type != LE_LINK) + return; + + mgmt_pending_foreach(MGMT_OP_DISCONNECT, hdev, disconnect_rsp, &sk); + + bacpy(&ev.addr.bdaddr, bdaddr); + ev.addr.type = link_to_bdaddr(link_type, addr_type); + ev.reason = reason; + + mgmt_event(MGMT_EV_DEVICE_DISCONNECTED, hdev, &ev, sizeof(ev), sk); + + if (sk) + sock_put(sk); + + mgmt_pending_foreach(MGMT_OP_UNPAIR_DEVICE, hdev, unpair_device_rsp, + hdev); +} + +void mgmt_disconnect_failed(struct hci_dev *hdev, bdaddr_t *bdaddr, + u8 link_type, u8 addr_type, u8 status) +{ + u8 bdaddr_type = link_to_bdaddr(link_type, addr_type); + struct mgmt_cp_disconnect *cp; + struct mgmt_pending_cmd *cmd; + + mgmt_pending_foreach(MGMT_OP_UNPAIR_DEVICE, hdev, unpair_device_rsp, + hdev); + + cmd = pending_find(MGMT_OP_DISCONNECT, hdev); + if (!cmd) + return; + + cp = cmd->param; + + if (bacmp(bdaddr, &cp->addr.bdaddr)) + return; + + if (cp->addr.type != bdaddr_type) + return; + + cmd->cmd_complete(cmd, mgmt_status(status)); + mgmt_pending_remove(cmd); +} + +void mgmt_connect_failed(struct hci_dev *hdev, bdaddr_t *bdaddr, u8 link_type, + u8 addr_type, u8 status) +{ + struct mgmt_ev_connect_failed ev; + + /* The connection is still in hci_conn_hash so test for 1 + * instead of 0 to know if this is the last one. + */ + if (mgmt_powering_down(hdev) && hci_conn_count(hdev) == 1) { + cancel_delayed_work(&hdev->power_off); + queue_work(hdev->req_workqueue, &hdev->power_off.work); + } + + bacpy(&ev.addr.bdaddr, bdaddr); + ev.addr.type = link_to_bdaddr(link_type, addr_type); + ev.status = mgmt_status(status); + + mgmt_event(MGMT_EV_CONNECT_FAILED, hdev, &ev, sizeof(ev), NULL); +} + +void mgmt_pin_code_request(struct hci_dev *hdev, bdaddr_t *bdaddr, u8 secure) +{ + struct mgmt_ev_pin_code_request ev; + + bacpy(&ev.addr.bdaddr, bdaddr); + ev.addr.type = BDADDR_BREDR; + ev.secure = secure; + + mgmt_event(MGMT_EV_PIN_CODE_REQUEST, hdev, &ev, sizeof(ev), NULL); +} + +void mgmt_pin_code_reply_complete(struct hci_dev *hdev, bdaddr_t *bdaddr, + u8 status) +{ + struct mgmt_pending_cmd *cmd; + + cmd = pending_find(MGMT_OP_PIN_CODE_REPLY, hdev); + if (!cmd) + return; + + cmd->cmd_complete(cmd, mgmt_status(status)); + mgmt_pending_remove(cmd); +} + +void mgmt_pin_code_neg_reply_complete(struct hci_dev *hdev, bdaddr_t *bdaddr, + u8 status) +{ + struct mgmt_pending_cmd *cmd; + + cmd = pending_find(MGMT_OP_PIN_CODE_NEG_REPLY, hdev); + if (!cmd) + return; + + cmd->cmd_complete(cmd, mgmt_status(status)); + mgmt_pending_remove(cmd); +} + +int mgmt_user_confirm_request(struct hci_dev *hdev, bdaddr_t *bdaddr, + u8 link_type, u8 addr_type, u32 value, + u8 confirm_hint) +{ + struct mgmt_ev_user_confirm_request ev; + + BT_DBG("%s", hdev->name); + + bacpy(&ev.addr.bdaddr, bdaddr); + ev.addr.type = link_to_bdaddr(link_type, addr_type); + ev.confirm_hint = confirm_hint; + ev.value = cpu_to_le32(value); + + return mgmt_event(MGMT_EV_USER_CONFIRM_REQUEST, hdev, &ev, sizeof(ev), + NULL); +} + +int mgmt_user_passkey_request(struct hci_dev *hdev, bdaddr_t *bdaddr, + u8 link_type, u8 addr_type) +{ + struct mgmt_ev_user_passkey_request ev; + + BT_DBG("%s", hdev->name); + + bacpy(&ev.addr.bdaddr, bdaddr); + ev.addr.type = link_to_bdaddr(link_type, addr_type); + + return mgmt_event(MGMT_EV_USER_PASSKEY_REQUEST, hdev, &ev, sizeof(ev), + NULL); +} + +static int user_pairing_resp_complete(struct hci_dev *hdev, bdaddr_t *bdaddr, + u8 link_type, u8 addr_type, u8 status, + u8 opcode) +{ + struct mgmt_pending_cmd *cmd; + + cmd = pending_find(opcode, hdev); + if (!cmd) + return -ENOENT; + + cmd->cmd_complete(cmd, mgmt_status(status)); + mgmt_pending_remove(cmd); + + return 0; +} + +int mgmt_user_confirm_reply_complete(struct hci_dev *hdev, bdaddr_t *bdaddr, + u8 link_type, u8 addr_type, u8 status) +{ + return user_pairing_resp_complete(hdev, bdaddr, link_type, addr_type, + status, MGMT_OP_USER_CONFIRM_REPLY); +} + +int mgmt_user_confirm_neg_reply_complete(struct hci_dev *hdev, bdaddr_t *bdaddr, + u8 link_type, u8 addr_type, u8 status) +{ + return user_pairing_resp_complete(hdev, bdaddr, link_type, addr_type, + status, + MGMT_OP_USER_CONFIRM_NEG_REPLY); +} + +int mgmt_user_passkey_reply_complete(struct hci_dev *hdev, bdaddr_t *bdaddr, + u8 link_type, u8 addr_type, u8 status) +{ + return user_pairing_resp_complete(hdev, bdaddr, link_type, addr_type, + status, MGMT_OP_USER_PASSKEY_REPLY); +} + +int mgmt_user_passkey_neg_reply_complete(struct hci_dev *hdev, bdaddr_t *bdaddr, + u8 link_type, u8 addr_type, u8 status) +{ + return user_pairing_resp_complete(hdev, bdaddr, link_type, addr_type, + status, + MGMT_OP_USER_PASSKEY_NEG_REPLY); +} + +int mgmt_user_passkey_notify(struct hci_dev *hdev, bdaddr_t *bdaddr, + u8 link_type, u8 addr_type, u32 passkey, + u8 entered) +{ + struct mgmt_ev_passkey_notify ev; + + BT_DBG("%s", hdev->name); + + bacpy(&ev.addr.bdaddr, bdaddr); + ev.addr.type = link_to_bdaddr(link_type, addr_type); + ev.passkey = __cpu_to_le32(passkey); + ev.entered = entered; + + return mgmt_event(MGMT_EV_PASSKEY_NOTIFY, hdev, &ev, sizeof(ev), NULL); +} + +void mgmt_auth_failed(struct hci_conn *conn, u8 hci_status) +{ + struct mgmt_ev_auth_failed ev; + struct mgmt_pending_cmd *cmd; + u8 status = mgmt_status(hci_status); + + bacpy(&ev.addr.bdaddr, &conn->dst); + ev.addr.type = link_to_bdaddr(conn->type, conn->dst_type); + ev.status = status; + + cmd = find_pairing(conn); + + mgmt_event(MGMT_EV_AUTH_FAILED, conn->hdev, &ev, sizeof(ev), + cmd ? cmd->sk : NULL); + + if (cmd) { + cmd->cmd_complete(cmd, status); + mgmt_pending_remove(cmd); + } +} + +void mgmt_auth_enable_complete(struct hci_dev *hdev, u8 status) +{ + struct cmd_lookup match = { NULL, hdev }; + bool changed; + + if (status) { + u8 mgmt_err = mgmt_status(status); + mgmt_pending_foreach(MGMT_OP_SET_LINK_SECURITY, hdev, + cmd_status_rsp, &mgmt_err); + return; + } + + if (test_bit(HCI_AUTH, &hdev->flags)) + changed = !hci_dev_test_and_set_flag(hdev, HCI_LINK_SECURITY); + else + changed = hci_dev_test_and_clear_flag(hdev, HCI_LINK_SECURITY); + + mgmt_pending_foreach(MGMT_OP_SET_LINK_SECURITY, hdev, settings_rsp, + &match); + + if (changed) + new_settings(hdev, match.sk); + + if (match.sk) + sock_put(match.sk); +} + +static void clear_eir(struct hci_request *req) +{ + struct hci_dev *hdev = req->hdev; + struct hci_cp_write_eir cp; + + if (!lmp_ext_inq_capable(hdev)) + return; + + memset(hdev->eir, 0, sizeof(hdev->eir)); + + memset(&cp, 0, sizeof(cp)); + + hci_req_add(req, HCI_OP_WRITE_EIR, sizeof(cp), &cp); +} + +void mgmt_ssp_enable_complete(struct hci_dev *hdev, u8 enable, u8 status) +{ + struct cmd_lookup match = { NULL, hdev }; + struct hci_request req; + bool changed = false; + + if (status) { + u8 mgmt_err = mgmt_status(status); + + if (enable && hci_dev_test_and_clear_flag(hdev, + HCI_SSP_ENABLED)) { + hci_dev_clear_flag(hdev, HCI_HS_ENABLED); + new_settings(hdev, NULL); + } + + mgmt_pending_foreach(MGMT_OP_SET_SSP, hdev, cmd_status_rsp, + &mgmt_err); + return; + } + + if (enable) { + changed = !hci_dev_test_and_set_flag(hdev, HCI_SSP_ENABLED); + } else { + changed = hci_dev_test_and_clear_flag(hdev, HCI_SSP_ENABLED); + if (!changed) + changed = hci_dev_test_and_clear_flag(hdev, + HCI_HS_ENABLED); + else + hci_dev_clear_flag(hdev, HCI_HS_ENABLED); + } + + mgmt_pending_foreach(MGMT_OP_SET_SSP, hdev, settings_rsp, &match); + + if (changed) + new_settings(hdev, match.sk); + + if (match.sk) + sock_put(match.sk); + + hci_req_init(&req, hdev); + + if (hci_dev_test_flag(hdev, HCI_SSP_ENABLED)) { + if (hci_dev_test_flag(hdev, HCI_USE_DEBUG_KEYS)) + hci_req_add(&req, HCI_OP_WRITE_SSP_DEBUG_MODE, + sizeof(enable), &enable); + update_eir(&req); + } else { + clear_eir(&req); + } + + hci_req_run(&req, NULL); +} + +static void sk_lookup(struct mgmt_pending_cmd *cmd, void *data) +{ + struct cmd_lookup *match = data; + + if (match->sk == NULL) { + match->sk = cmd->sk; + sock_hold(match->sk); + } +} + +void mgmt_set_class_of_dev_complete(struct hci_dev *hdev, u8 *dev_class, + u8 status) +{ + struct cmd_lookup match = { NULL, hdev, mgmt_status(status) }; + + mgmt_pending_foreach(MGMT_OP_SET_DEV_CLASS, hdev, sk_lookup, &match); + mgmt_pending_foreach(MGMT_OP_ADD_UUID, hdev, sk_lookup, &match); + mgmt_pending_foreach(MGMT_OP_REMOVE_UUID, hdev, sk_lookup, &match); + + if (!status) + mgmt_generic_event(MGMT_EV_CLASS_OF_DEV_CHANGED, hdev, + dev_class, 3, NULL); + + if (match.sk) + sock_put(match.sk); +} + +void mgmt_set_local_name_complete(struct hci_dev *hdev, u8 *name, u8 status) +{ + struct mgmt_cp_set_local_name ev; + struct mgmt_pending_cmd *cmd; + + if (status) + return; + + memset(&ev, 0, sizeof(ev)); + memcpy(ev.name, name, HCI_MAX_NAME_LENGTH); + memcpy(ev.short_name, hdev->short_name, HCI_MAX_SHORT_NAME_LENGTH); + + cmd = pending_find(MGMT_OP_SET_LOCAL_NAME, hdev); + if (!cmd) { + memcpy(hdev->dev_name, name, sizeof(hdev->dev_name)); + + /* If this is a HCI command related to powering on the + * HCI dev don't send any mgmt signals. + */ + if (pending_find(MGMT_OP_SET_POWERED, hdev)) + return; + } + + mgmt_generic_event(MGMT_EV_LOCAL_NAME_CHANGED, hdev, &ev, sizeof(ev), + cmd ? cmd->sk : NULL); +} + +static inline bool has_uuid(u8 *uuid, u16 uuid_count, u8 (*uuids)[16]) +{ + int i; + + for (i = 0; i < uuid_count; i++) { + if (!memcmp(uuid, uuids[i], 16)) + return true; + } + + return false; +} + +static bool eir_has_uuids(u8 *eir, u16 eir_len, u16 uuid_count, u8 (*uuids)[16]) +{ + u16 parsed = 0; + + while (parsed < eir_len) { + u8 field_len = eir[0]; + u8 uuid[16]; + int i; + + if (field_len == 0) + break; + + if (eir_len - parsed < field_len + 1) + break; + + switch (eir[1]) { + case EIR_UUID16_ALL: + case EIR_UUID16_SOME: + for (i = 0; i + 3 <= field_len; i += 2) { + memcpy(uuid, bluetooth_base_uuid, 16); + uuid[13] = eir[i + 3]; + uuid[12] = eir[i + 2]; + if (has_uuid(uuid, uuid_count, uuids)) + return true; + } + break; + case EIR_UUID32_ALL: + case EIR_UUID32_SOME: + for (i = 0; i + 5 <= field_len; i += 4) { + memcpy(uuid, bluetooth_base_uuid, 16); + uuid[15] = eir[i + 5]; + uuid[14] = eir[i + 4]; + uuid[13] = eir[i + 3]; + uuid[12] = eir[i + 2]; + if (has_uuid(uuid, uuid_count, uuids)) + return true; + } + break; + case EIR_UUID128_ALL: + case EIR_UUID128_SOME: + for (i = 0; i + 17 <= field_len; i += 16) { + memcpy(uuid, eir + i + 2, 16); + if (has_uuid(uuid, uuid_count, uuids)) + return true; + } + break; + } + + parsed += field_len + 1; + eir += field_len + 1; + } + + return false; +} + +static void restart_le_scan(struct hci_dev *hdev) +{ + /* If controller is not scanning we are done. */ + if (!hci_dev_test_flag(hdev, HCI_LE_SCAN)) + return; + + if (time_after(jiffies + DISCOV_LE_RESTART_DELAY, + hdev->discovery.scan_start + + hdev->discovery.scan_duration)) + return; + + queue_delayed_work(hdev->workqueue, &hdev->le_scan_restart, + DISCOV_LE_RESTART_DELAY); +} + +static bool is_filter_match(struct hci_dev *hdev, s8 rssi, u8 *eir, + u16 eir_len, u8 *scan_rsp, u8 scan_rsp_len) +{ + /* If a RSSI threshold has been specified, and + * HCI_QUIRK_STRICT_DUPLICATE_FILTER is not set, then all results with + * a RSSI smaller than the RSSI threshold will be dropped. If the quirk + * is set, let it through for further processing, as we might need to + * restart the scan. + * + * For BR/EDR devices (pre 1.2) providing no RSSI during inquiry, + * the results are also dropped. + */ + if (hdev->discovery.rssi != HCI_RSSI_INVALID && + (rssi == HCI_RSSI_INVALID || + (rssi < hdev->discovery.rssi && + !test_bit(HCI_QUIRK_STRICT_DUPLICATE_FILTER, &hdev->quirks)))) + return false; + + if (hdev->discovery.uuid_count != 0) { + /* If a list of UUIDs is provided in filter, results with no + * matching UUID should be dropped. + */ + if (!eir_has_uuids(eir, eir_len, hdev->discovery.uuid_count, + hdev->discovery.uuids) && + !eir_has_uuids(scan_rsp, scan_rsp_len, + hdev->discovery.uuid_count, + hdev->discovery.uuids)) + return false; + } + + /* If duplicate filtering does not report RSSI changes, then restart + * scanning to ensure updated result with updated RSSI values. + */ + if (test_bit(HCI_QUIRK_STRICT_DUPLICATE_FILTER, &hdev->quirks)) { + restart_le_scan(hdev); + + /* Validate RSSI value against the RSSI threshold once more. */ + if (hdev->discovery.rssi != HCI_RSSI_INVALID && + rssi < hdev->discovery.rssi) + return false; + } + + return true; +} + +void mgmt_device_found(struct hci_dev *hdev, bdaddr_t *bdaddr, u8 link_type, + u8 addr_type, u8 *dev_class, s8 rssi, u32 flags, + u8 *eir, u16 eir_len, u8 *scan_rsp, u8 scan_rsp_len) +{ + char buf[512]; + struct mgmt_ev_device_found *ev = (void *)buf; + size_t ev_size; + + /* Don't send events for a non-kernel initiated discovery. With + * LE one exception is if we have pend_le_reports > 0 in which + * case we're doing passive scanning and want these events. + */ + if (!hci_discovery_active(hdev)) { + if (link_type == ACL_LINK) + return; + if (link_type == LE_LINK && list_empty(&hdev->pend_le_reports)) + return; + } + + if (hdev->discovery.result_filtering) { + /* We are using service discovery */ + if (!is_filter_match(hdev, rssi, eir, eir_len, scan_rsp, + scan_rsp_len)) + return; + } + + /* Make sure that the buffer is big enough. The 5 extra bytes + * are for the potential CoD field. + */ + if (sizeof(*ev) + eir_len + scan_rsp_len + 5 > sizeof(buf)) + return; + + memset(buf, 0, sizeof(buf)); + + /* In case of device discovery with BR/EDR devices (pre 1.2), the + * RSSI value was reported as 0 when not available. This behavior + * is kept when using device discovery. This is required for full + * backwards compatibility with the API. + * + * However when using service discovery, the value 127 will be + * returned when the RSSI is not available. + */ + if (rssi == HCI_RSSI_INVALID && !hdev->discovery.report_invalid_rssi && + link_type == ACL_LINK) + rssi = 0; + + bacpy(&ev->addr.bdaddr, bdaddr); + ev->addr.type = link_to_bdaddr(link_type, addr_type); + ev->rssi = rssi; + ev->flags = cpu_to_le32(flags); + + if (eir_len > 0) + /* Copy EIR or advertising data into event */ + memcpy(ev->eir, eir, eir_len); + + if (dev_class && !eir_has_data_type(ev->eir, eir_len, EIR_CLASS_OF_DEV)) + eir_len = eir_append_data(ev->eir, eir_len, EIR_CLASS_OF_DEV, + dev_class, 3); + + if (scan_rsp_len > 0) + /* Append scan response data to event */ + memcpy(ev->eir + eir_len, scan_rsp, scan_rsp_len); + + ev->eir_len = cpu_to_le16(eir_len + scan_rsp_len); + ev_size = sizeof(*ev) + eir_len + scan_rsp_len; + + mgmt_event(MGMT_EV_DEVICE_FOUND, hdev, ev, ev_size, NULL); +} + +void mgmt_remote_name(struct hci_dev *hdev, bdaddr_t *bdaddr, u8 link_type, + u8 addr_type, s8 rssi, u8 *name, u8 name_len) +{ + struct mgmt_ev_device_found *ev; + char buf[sizeof(*ev) + HCI_MAX_NAME_LENGTH + 2]; + u16 eir_len; + + ev = (struct mgmt_ev_device_found *) buf; + + memset(buf, 0, sizeof(buf)); + + bacpy(&ev->addr.bdaddr, bdaddr); + ev->addr.type = link_to_bdaddr(link_type, addr_type); + ev->rssi = rssi; + + eir_len = eir_append_data(ev->eir, 0, EIR_NAME_COMPLETE, name, + name_len); + + ev->eir_len = cpu_to_le16(eir_len); + + mgmt_event(MGMT_EV_DEVICE_FOUND, hdev, ev, sizeof(*ev) + eir_len, NULL); +} + +void mgmt_discovering(struct hci_dev *hdev, u8 discovering) +{ + struct mgmt_ev_discovering ev; + + BT_DBG("%s discovering %u", hdev->name, discovering); + + memset(&ev, 0, sizeof(ev)); + ev.type = hdev->discovery.type; + ev.discovering = discovering; + + mgmt_event(MGMT_EV_DISCOVERING, hdev, &ev, sizeof(ev), NULL); +} + +static void adv_enable_complete(struct hci_dev *hdev, u8 status, u16 opcode) +{ + BT_DBG("%s status %u", hdev->name, status); +} + +void mgmt_reenable_advertising(struct hci_dev *hdev) +{ + struct hci_request req; + + if (!hci_dev_test_flag(hdev, HCI_ADVERTISING) && + !hci_dev_test_flag(hdev, HCI_ADVERTISING_INSTANCE)) + return; + + hci_req_init(&req, hdev); + enable_advertising(&req); + hci_req_run(&req, adv_enable_complete); +} + +static struct hci_mgmt_chan chan = { + .channel = HCI_CHANNEL_CONTROL, + .handler_count = ARRAY_SIZE(mgmt_handlers), + .handlers = mgmt_handlers, + .hdev_init = mgmt_init_hdev, +}; + +int mgmt_init(void) +{ + return hci_mgmt_chan_register(&chan); +} + +void mgmt_exit(void) +{ + hci_mgmt_chan_unregister(&chan); +} diff --git a/kernel/net/bluetooth/mgmt_util.c b/kernel/net/bluetooth/mgmt_util.c new file mode 100644 index 000000000..8c30c7eb8 --- /dev/null +++ b/kernel/net/bluetooth/mgmt_util.c @@ -0,0 +1,210 @@ +/* + BlueZ - Bluetooth protocol stack for Linux + + Copyright (C) 2015 Intel Corporation + + 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; + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS + OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT OF THIRD PARTY RIGHTS. + IN NO EVENT SHALL THE COPYRIGHT HOLDER(S) AND AUTHOR(S) BE LIABLE FOR ANY + CLAIM, OR ANY SPECIAL INDIRECT OR CONSEQUENTIAL DAMAGES, OR ANY DAMAGES + WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN + ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF + OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + + ALL LIABILITY, INCLUDING LIABILITY FOR INFRINGEMENT OF ANY PATENTS, + COPYRIGHTS, TRADEMARKS OR OTHER RIGHTS, RELATING TO USE OF THIS + SOFTWARE IS DISCLAIMED. +*/ + +#include <net/bluetooth/bluetooth.h> +#include <net/bluetooth/hci_core.h> +#include <net/bluetooth/mgmt.h> + +#include "mgmt_util.h" + +int mgmt_send_event(u16 event, struct hci_dev *hdev, unsigned short channel, + void *data, u16 data_len, int flag, struct sock *skip_sk) +{ + struct sk_buff *skb; + struct mgmt_hdr *hdr; + + skb = alloc_skb(sizeof(*hdr) + data_len, GFP_KERNEL); + if (!skb) + return -ENOMEM; + + hdr = (void *) skb_put(skb, sizeof(*hdr)); + hdr->opcode = cpu_to_le16(event); + if (hdev) + hdr->index = cpu_to_le16(hdev->id); + else + hdr->index = cpu_to_le16(MGMT_INDEX_NONE); + hdr->len = cpu_to_le16(data_len); + + if (data) + memcpy(skb_put(skb, data_len), data, data_len); + + /* Time stamp */ + __net_timestamp(skb); + + hci_send_to_channel(channel, skb, flag, skip_sk); + kfree_skb(skb); + + return 0; +} + +int mgmt_cmd_status(struct sock *sk, u16 index, u16 cmd, u8 status) +{ + struct sk_buff *skb; + struct mgmt_hdr *hdr; + struct mgmt_ev_cmd_status *ev; + int err; + + BT_DBG("sock %p, index %u, cmd %u, status %u", sk, index, cmd, status); + + skb = alloc_skb(sizeof(*hdr) + sizeof(*ev), GFP_KERNEL); + if (!skb) + return -ENOMEM; + + hdr = (void *) skb_put(skb, sizeof(*hdr)); + + hdr->opcode = cpu_to_le16(MGMT_EV_CMD_STATUS); + hdr->index = cpu_to_le16(index); + hdr->len = cpu_to_le16(sizeof(*ev)); + + ev = (void *) skb_put(skb, sizeof(*ev)); + ev->status = status; + ev->opcode = cpu_to_le16(cmd); + + err = sock_queue_rcv_skb(sk, skb); + if (err < 0) + kfree_skb(skb); + + return err; +} + +int mgmt_cmd_complete(struct sock *sk, u16 index, u16 cmd, u8 status, + void *rp, size_t rp_len) +{ + struct sk_buff *skb; + struct mgmt_hdr *hdr; + struct mgmt_ev_cmd_complete *ev; + int err; + + BT_DBG("sock %p", sk); + + skb = alloc_skb(sizeof(*hdr) + sizeof(*ev) + rp_len, GFP_KERNEL); + if (!skb) + return -ENOMEM; + + hdr = (void *) skb_put(skb, sizeof(*hdr)); + + hdr->opcode = cpu_to_le16(MGMT_EV_CMD_COMPLETE); + hdr->index = cpu_to_le16(index); + hdr->len = cpu_to_le16(sizeof(*ev) + rp_len); + + ev = (void *) skb_put(skb, sizeof(*ev) + rp_len); + ev->opcode = cpu_to_le16(cmd); + ev->status = status; + + if (rp) + memcpy(ev->data, rp, rp_len); + + err = sock_queue_rcv_skb(sk, skb); + if (err < 0) + kfree_skb(skb); + + return err; +} + +struct mgmt_pending_cmd *mgmt_pending_find(unsigned short channel, u16 opcode, + struct hci_dev *hdev) +{ + struct mgmt_pending_cmd *cmd; + + list_for_each_entry(cmd, &hdev->mgmt_pending, list) { + if (hci_sock_get_channel(cmd->sk) != channel) + continue; + if (cmd->opcode == opcode) + return cmd; + } + + return NULL; +} + +struct mgmt_pending_cmd *mgmt_pending_find_data(unsigned short channel, + u16 opcode, + struct hci_dev *hdev, + const void *data) +{ + struct mgmt_pending_cmd *cmd; + + list_for_each_entry(cmd, &hdev->mgmt_pending, list) { + if (cmd->user_data != data) + continue; + if (cmd->opcode == opcode) + return cmd; + } + + return NULL; +} + +void mgmt_pending_foreach(u16 opcode, struct hci_dev *hdev, + void (*cb)(struct mgmt_pending_cmd *cmd, void *data), + void *data) +{ + struct mgmt_pending_cmd *cmd, *tmp; + + list_for_each_entry_safe(cmd, tmp, &hdev->mgmt_pending, list) { + if (opcode > 0 && cmd->opcode != opcode) + continue; + + cb(cmd, data); + } +} + +struct mgmt_pending_cmd *mgmt_pending_add(struct sock *sk, u16 opcode, + struct hci_dev *hdev, + void *data, u16 len) +{ + struct mgmt_pending_cmd *cmd; + + cmd = kzalloc(sizeof(*cmd), GFP_KERNEL); + if (!cmd) + return NULL; + + cmd->opcode = opcode; + cmd->index = hdev->id; + + cmd->param = kmemdup(data, len, GFP_KERNEL); + if (!cmd->param) { + kfree(cmd); + return NULL; + } + + cmd->param_len = len; + + cmd->sk = sk; + sock_hold(sk); + + list_add(&cmd->list, &hdev->mgmt_pending); + + return cmd; +} + +void mgmt_pending_free(struct mgmt_pending_cmd *cmd) +{ + sock_put(cmd->sk); + kfree(cmd->param); + kfree(cmd); +} + +void mgmt_pending_remove(struct mgmt_pending_cmd *cmd) +{ + list_del(&cmd->list); + mgmt_pending_free(cmd); +} diff --git a/kernel/net/bluetooth/mgmt_util.h b/kernel/net/bluetooth/mgmt_util.h new file mode 100644 index 000000000..6559f1892 --- /dev/null +++ b/kernel/net/bluetooth/mgmt_util.h @@ -0,0 +1,53 @@ +/* + BlueZ - Bluetooth protocol stack for Linux + Copyright (C) 2015 Intel Coropration + + 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; + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS + OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT OF THIRD PARTY RIGHTS. + IN NO EVENT SHALL THE COPYRIGHT HOLDER(S) AND AUTHOR(S) BE LIABLE FOR ANY + CLAIM, OR ANY SPECIAL INDIRECT OR CONSEQUENTIAL DAMAGES, OR ANY DAMAGES + WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN + ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF + OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + + ALL LIABILITY, INCLUDING LIABILITY FOR INFRINGEMENT OF ANY PATENTS, + COPYRIGHTS, TRADEMARKS OR OTHER RIGHTS, RELATING TO USE OF THIS + SOFTWARE IS DISCLAIMED. +*/ + +struct mgmt_pending_cmd { + struct list_head list; + u16 opcode; + int index; + void *param; + size_t param_len; + struct sock *sk; + void *user_data; + int (*cmd_complete)(struct mgmt_pending_cmd *cmd, u8 status); +}; + +int mgmt_send_event(u16 event, struct hci_dev *hdev, unsigned short channel, + void *data, u16 data_len, int flag, struct sock *skip_sk); +int mgmt_cmd_status(struct sock *sk, u16 index, u16 cmd, u8 status); +int mgmt_cmd_complete(struct sock *sk, u16 index, u16 cmd, u8 status, + void *rp, size_t rp_len); + +struct mgmt_pending_cmd *mgmt_pending_find(unsigned short channel, u16 opcode, + struct hci_dev *hdev); +struct mgmt_pending_cmd *mgmt_pending_find_data(unsigned short channel, + u16 opcode, + struct hci_dev *hdev, + const void *data); +void mgmt_pending_foreach(u16 opcode, struct hci_dev *hdev, + void (*cb)(struct mgmt_pending_cmd *cmd, void *data), + void *data); +struct mgmt_pending_cmd *mgmt_pending_add(struct sock *sk, u16 opcode, + struct hci_dev *hdev, + void *data, u16 len); +void mgmt_pending_free(struct mgmt_pending_cmd *cmd); +void mgmt_pending_remove(struct mgmt_pending_cmd *cmd); diff --git a/kernel/net/bluetooth/rfcomm/Kconfig b/kernel/net/bluetooth/rfcomm/Kconfig new file mode 100644 index 000000000..335df7515 --- /dev/null +++ b/kernel/net/bluetooth/rfcomm/Kconfig @@ -0,0 +1,18 @@ +config BT_RFCOMM + tristate "RFCOMM protocol support" + depends on BT_BREDR + help + RFCOMM provides connection oriented stream transport. RFCOMM + support is required for Dialup Networking, OBEX and other Bluetooth + applications. + + Say Y here to compile RFCOMM support into the kernel or say M to + compile it as module (rfcomm). + +config BT_RFCOMM_TTY + bool "RFCOMM TTY support" + depends on BT_RFCOMM + depends on TTY + help + This option enables TTY emulation support for RFCOMM channels. + diff --git a/kernel/net/bluetooth/rfcomm/Makefile b/kernel/net/bluetooth/rfcomm/Makefile new file mode 100644 index 000000000..fe07988a3 --- /dev/null +++ b/kernel/net/bluetooth/rfcomm/Makefile @@ -0,0 +1,8 @@ +# +# Makefile for the Linux Bluetooth RFCOMM layer. +# + +obj-$(CONFIG_BT_RFCOMM) += rfcomm.o + +rfcomm-y := core.o sock.o +rfcomm-$(CONFIG_BT_RFCOMM_TTY) += tty.o diff --git a/kernel/net/bluetooth/rfcomm/core.c b/kernel/net/bluetooth/rfcomm/core.c new file mode 100644 index 000000000..4fea24275 --- /dev/null +++ b/kernel/net/bluetooth/rfcomm/core.c @@ -0,0 +1,2277 @@ +/* + RFCOMM implementation for Linux Bluetooth stack (BlueZ). + Copyright (C) 2002 Maxim Krasnyansky <maxk@qualcomm.com> + Copyright (C) 2002 Marcel Holtmann <marcel@holtmann.org> + + 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; + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS + OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT OF THIRD PARTY RIGHTS. + IN NO EVENT SHALL THE COPYRIGHT HOLDER(S) AND AUTHOR(S) BE LIABLE FOR ANY + CLAIM, OR ANY SPECIAL INDIRECT OR CONSEQUENTIAL DAMAGES, OR ANY DAMAGES + WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN + ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF + OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + + ALL LIABILITY, INCLUDING LIABILITY FOR INFRINGEMENT OF ANY PATENTS, + COPYRIGHTS, TRADEMARKS OR OTHER RIGHTS, RELATING TO USE OF THIS + SOFTWARE IS DISCLAIMED. +*/ + +/* + * Bluetooth RFCOMM core. + */ + +#include <linux/module.h> +#include <linux/debugfs.h> +#include <linux/kthread.h> +#include <asm/unaligned.h> + +#include <net/bluetooth/bluetooth.h> +#include <net/bluetooth/hci_core.h> +#include <net/bluetooth/l2cap.h> +#include <net/bluetooth/rfcomm.h> + +#define VERSION "1.11" + +static bool disable_cfc; +static bool l2cap_ertm; +static int channel_mtu = -1; +static unsigned int l2cap_mtu = RFCOMM_MAX_L2CAP_MTU; + +static struct task_struct *rfcomm_thread; + +static DEFINE_MUTEX(rfcomm_mutex); +#define rfcomm_lock() mutex_lock(&rfcomm_mutex) +#define rfcomm_unlock() mutex_unlock(&rfcomm_mutex) + + +static LIST_HEAD(session_list); + +static int rfcomm_send_frame(struct rfcomm_session *s, u8 *data, int len); +static int rfcomm_send_sabm(struct rfcomm_session *s, u8 dlci); +static int rfcomm_send_disc(struct rfcomm_session *s, u8 dlci); +static int rfcomm_queue_disc(struct rfcomm_dlc *d); +static int rfcomm_send_nsc(struct rfcomm_session *s, int cr, u8 type); +static int rfcomm_send_pn(struct rfcomm_session *s, int cr, struct rfcomm_dlc *d); +static int rfcomm_send_msc(struct rfcomm_session *s, int cr, u8 dlci, u8 v24_sig); +static int rfcomm_send_test(struct rfcomm_session *s, int cr, u8 *pattern, int len); +static int rfcomm_send_credits(struct rfcomm_session *s, u8 addr, u8 credits); +static void rfcomm_make_uih(struct sk_buff *skb, u8 addr); + +static void rfcomm_process_connect(struct rfcomm_session *s); + +static struct rfcomm_session *rfcomm_session_create(bdaddr_t *src, + bdaddr_t *dst, + u8 sec_level, + int *err); +static struct rfcomm_session *rfcomm_session_get(bdaddr_t *src, bdaddr_t *dst); +static struct rfcomm_session *rfcomm_session_del(struct rfcomm_session *s); + +/* ---- RFCOMM frame parsing macros ---- */ +#define __get_dlci(b) ((b & 0xfc) >> 2) +#define __get_channel(b) ((b & 0xf8) >> 3) +#define __get_dir(b) ((b & 0x04) >> 2) +#define __get_type(b) ((b & 0xef)) + +#define __test_ea(b) ((b & 0x01)) +#define __test_cr(b) (!!(b & 0x02)) +#define __test_pf(b) (!!(b & 0x10)) + +#define __session_dir(s) ((s)->initiator ? 0x00 : 0x01) + +#define __addr(cr, dlci) (((dlci & 0x3f) << 2) | (cr << 1) | 0x01) +#define __ctrl(type, pf) (((type & 0xef) | (pf << 4))) +#define __dlci(dir, chn) (((chn & 0x1f) << 1) | dir) +#define __srv_channel(dlci) (dlci >> 1) +#define __dir(dlci) (dlci & 0x01) + +#define __len8(len) (((len) << 1) | 1) +#define __len16(len) ((len) << 1) + +/* MCC macros */ +#define __mcc_type(cr, type) (((type << 2) | (cr << 1) | 0x01)) +#define __get_mcc_type(b) ((b & 0xfc) >> 2) +#define __get_mcc_len(b) ((b & 0xfe) >> 1) + +/* RPN macros */ +#define __rpn_line_settings(data, stop, parity) ((data & 0x3) | ((stop & 0x1) << 2) | ((parity & 0x7) << 3)) +#define __get_rpn_data_bits(line) ((line) & 0x3) +#define __get_rpn_stop_bits(line) (((line) >> 2) & 0x1) +#define __get_rpn_parity(line) (((line) >> 3) & 0x7) + +static DECLARE_WAIT_QUEUE_HEAD(rfcomm_wq); + +static void rfcomm_schedule(void) +{ + wake_up_all(&rfcomm_wq); +} + +/* ---- RFCOMM FCS computation ---- */ + +/* reversed, 8-bit, poly=0x07 */ +static unsigned char rfcomm_crc_table[256] = { + 0x00, 0x91, 0xe3, 0x72, 0x07, 0x96, 0xe4, 0x75, + 0x0e, 0x9f, 0xed, 0x7c, 0x09, 0x98, 0xea, 0x7b, + 0x1c, 0x8d, 0xff, 0x6e, 0x1b, 0x8a, 0xf8, 0x69, + 0x12, 0x83, 0xf1, 0x60, 0x15, 0x84, 0xf6, 0x67, + + 0x38, 0xa9, 0xdb, 0x4a, 0x3f, 0xae, 0xdc, 0x4d, + 0x36, 0xa7, 0xd5, 0x44, 0x31, 0xa0, 0xd2, 0x43, + 0x24, 0xb5, 0xc7, 0x56, 0x23, 0xb2, 0xc0, 0x51, + 0x2a, 0xbb, 0xc9, 0x58, 0x2d, 0xbc, 0xce, 0x5f, + + 0x70, 0xe1, 0x93, 0x02, 0x77, 0xe6, 0x94, 0x05, + 0x7e, 0xef, 0x9d, 0x0c, 0x79, 0xe8, 0x9a, 0x0b, + 0x6c, 0xfd, 0x8f, 0x1e, 0x6b, 0xfa, 0x88, 0x19, + 0x62, 0xf3, 0x81, 0x10, 0x65, 0xf4, 0x86, 0x17, + + 0x48, 0xd9, 0xab, 0x3a, 0x4f, 0xde, 0xac, 0x3d, + 0x46, 0xd7, 0xa5, 0x34, 0x41, 0xd0, 0xa2, 0x33, + 0x54, 0xc5, 0xb7, 0x26, 0x53, 0xc2, 0xb0, 0x21, + 0x5a, 0xcb, 0xb9, 0x28, 0x5d, 0xcc, 0xbe, 0x2f, + + 0xe0, 0x71, 0x03, 0x92, 0xe7, 0x76, 0x04, 0x95, + 0xee, 0x7f, 0x0d, 0x9c, 0xe9, 0x78, 0x0a, 0x9b, + 0xfc, 0x6d, 0x1f, 0x8e, 0xfb, 0x6a, 0x18, 0x89, + 0xf2, 0x63, 0x11, 0x80, 0xf5, 0x64, 0x16, 0x87, + + 0xd8, 0x49, 0x3b, 0xaa, 0xdf, 0x4e, 0x3c, 0xad, + 0xd6, 0x47, 0x35, 0xa4, 0xd1, 0x40, 0x32, 0xa3, + 0xc4, 0x55, 0x27, 0xb6, 0xc3, 0x52, 0x20, 0xb1, + 0xca, 0x5b, 0x29, 0xb8, 0xcd, 0x5c, 0x2e, 0xbf, + + 0x90, 0x01, 0x73, 0xe2, 0x97, 0x06, 0x74, 0xe5, + 0x9e, 0x0f, 0x7d, 0xec, 0x99, 0x08, 0x7a, 0xeb, + 0x8c, 0x1d, 0x6f, 0xfe, 0x8b, 0x1a, 0x68, 0xf9, + 0x82, 0x13, 0x61, 0xf0, 0x85, 0x14, 0x66, 0xf7, + + 0xa8, 0x39, 0x4b, 0xda, 0xaf, 0x3e, 0x4c, 0xdd, + 0xa6, 0x37, 0x45, 0xd4, 0xa1, 0x30, 0x42, 0xd3, + 0xb4, 0x25, 0x57, 0xc6, 0xb3, 0x22, 0x50, 0xc1, + 0xba, 0x2b, 0x59, 0xc8, 0xbd, 0x2c, 0x5e, 0xcf +}; + +/* CRC on 2 bytes */ +#define __crc(data) (rfcomm_crc_table[rfcomm_crc_table[0xff ^ data[0]] ^ data[1]]) + +/* FCS on 2 bytes */ +static inline u8 __fcs(u8 *data) +{ + return 0xff - __crc(data); +} + +/* FCS on 3 bytes */ +static inline u8 __fcs2(u8 *data) +{ + return 0xff - rfcomm_crc_table[__crc(data) ^ data[2]]; +} + +/* Check FCS */ +static inline int __check_fcs(u8 *data, int type, u8 fcs) +{ + u8 f = __crc(data); + + if (type != RFCOMM_UIH) + f = rfcomm_crc_table[f ^ data[2]]; + + return rfcomm_crc_table[f ^ fcs] != 0xcf; +} + +/* ---- L2CAP callbacks ---- */ +static void rfcomm_l2state_change(struct sock *sk) +{ + BT_DBG("%p state %d", sk, sk->sk_state); + rfcomm_schedule(); +} + +static void rfcomm_l2data_ready(struct sock *sk) +{ + BT_DBG("%p", sk); + rfcomm_schedule(); +} + +static int rfcomm_l2sock_create(struct socket **sock) +{ + int err; + + BT_DBG(""); + + err = sock_create_kern(PF_BLUETOOTH, SOCK_SEQPACKET, BTPROTO_L2CAP, sock); + if (!err) { + struct sock *sk = (*sock)->sk; + sk->sk_data_ready = rfcomm_l2data_ready; + sk->sk_state_change = rfcomm_l2state_change; + } + return err; +} + +static int rfcomm_check_security(struct rfcomm_dlc *d) +{ + struct sock *sk = d->session->sock->sk; + struct l2cap_conn *conn = l2cap_pi(sk)->chan->conn; + + __u8 auth_type; + + switch (d->sec_level) { + case BT_SECURITY_HIGH: + case BT_SECURITY_FIPS: + auth_type = HCI_AT_GENERAL_BONDING_MITM; + break; + case BT_SECURITY_MEDIUM: + auth_type = HCI_AT_GENERAL_BONDING; + break; + default: + auth_type = HCI_AT_NO_BONDING; + break; + } + + return hci_conn_security(conn->hcon, d->sec_level, auth_type, + d->out); +} + +static void rfcomm_session_timeout(unsigned long arg) +{ + struct rfcomm_session *s = (void *) arg; + + BT_DBG("session %p state %ld", s, s->state); + + set_bit(RFCOMM_TIMED_OUT, &s->flags); + rfcomm_schedule(); +} + +static void rfcomm_session_set_timer(struct rfcomm_session *s, long timeout) +{ + BT_DBG("session %p state %ld timeout %ld", s, s->state, timeout); + + mod_timer(&s->timer, jiffies + timeout); +} + +static void rfcomm_session_clear_timer(struct rfcomm_session *s) +{ + BT_DBG("session %p state %ld", s, s->state); + + del_timer_sync(&s->timer); +} + +/* ---- RFCOMM DLCs ---- */ +static void rfcomm_dlc_timeout(unsigned long arg) +{ + struct rfcomm_dlc *d = (void *) arg; + + BT_DBG("dlc %p state %ld", d, d->state); + + set_bit(RFCOMM_TIMED_OUT, &d->flags); + rfcomm_dlc_put(d); + rfcomm_schedule(); +} + +static void rfcomm_dlc_set_timer(struct rfcomm_dlc *d, long timeout) +{ + BT_DBG("dlc %p state %ld timeout %ld", d, d->state, timeout); + + if (!mod_timer(&d->timer, jiffies + timeout)) + rfcomm_dlc_hold(d); +} + +static void rfcomm_dlc_clear_timer(struct rfcomm_dlc *d) +{ + BT_DBG("dlc %p state %ld", d, d->state); + + if (del_timer(&d->timer)) + rfcomm_dlc_put(d); +} + +static void rfcomm_dlc_clear_state(struct rfcomm_dlc *d) +{ + BT_DBG("%p", d); + + d->state = BT_OPEN; + d->flags = 0; + d->mscex = 0; + d->sec_level = BT_SECURITY_LOW; + d->mtu = RFCOMM_DEFAULT_MTU; + d->v24_sig = RFCOMM_V24_RTC | RFCOMM_V24_RTR | RFCOMM_V24_DV; + + d->cfc = RFCOMM_CFC_DISABLED; + d->rx_credits = RFCOMM_DEFAULT_CREDITS; +} + +struct rfcomm_dlc *rfcomm_dlc_alloc(gfp_t prio) +{ + struct rfcomm_dlc *d = kzalloc(sizeof(*d), prio); + + if (!d) + return NULL; + + setup_timer(&d->timer, rfcomm_dlc_timeout, (unsigned long)d); + + skb_queue_head_init(&d->tx_queue); + mutex_init(&d->lock); + atomic_set(&d->refcnt, 1); + + rfcomm_dlc_clear_state(d); + + BT_DBG("%p", d); + + return d; +} + +void rfcomm_dlc_free(struct rfcomm_dlc *d) +{ + BT_DBG("%p", d); + + skb_queue_purge(&d->tx_queue); + kfree(d); +} + +static void rfcomm_dlc_link(struct rfcomm_session *s, struct rfcomm_dlc *d) +{ + BT_DBG("dlc %p session %p", d, s); + + rfcomm_session_clear_timer(s); + rfcomm_dlc_hold(d); + list_add(&d->list, &s->dlcs); + d->session = s; +} + +static void rfcomm_dlc_unlink(struct rfcomm_dlc *d) +{ + struct rfcomm_session *s = d->session; + + BT_DBG("dlc %p refcnt %d session %p", d, atomic_read(&d->refcnt), s); + + list_del(&d->list); + d->session = NULL; + rfcomm_dlc_put(d); + + if (list_empty(&s->dlcs)) + rfcomm_session_set_timer(s, RFCOMM_IDLE_TIMEOUT); +} + +static struct rfcomm_dlc *rfcomm_dlc_get(struct rfcomm_session *s, u8 dlci) +{ + struct rfcomm_dlc *d; + + list_for_each_entry(d, &s->dlcs, list) + if (d->dlci == dlci) + return d; + + return NULL; +} + +static int rfcomm_check_channel(u8 channel) +{ + return channel < 1 || channel > 30; +} + +static int __rfcomm_dlc_open(struct rfcomm_dlc *d, bdaddr_t *src, bdaddr_t *dst, u8 channel) +{ + struct rfcomm_session *s; + int err = 0; + u8 dlci; + + BT_DBG("dlc %p state %ld %pMR -> %pMR channel %d", + d, d->state, src, dst, channel); + + if (rfcomm_check_channel(channel)) + return -EINVAL; + + if (d->state != BT_OPEN && d->state != BT_CLOSED) + return 0; + + s = rfcomm_session_get(src, dst); + if (!s) { + s = rfcomm_session_create(src, dst, d->sec_level, &err); + if (!s) + return err; + } + + dlci = __dlci(__session_dir(s), channel); + + /* Check if DLCI already exists */ + if (rfcomm_dlc_get(s, dlci)) + return -EBUSY; + + rfcomm_dlc_clear_state(d); + + d->dlci = dlci; + d->addr = __addr(s->initiator, dlci); + d->priority = 7; + + d->state = BT_CONFIG; + rfcomm_dlc_link(s, d); + + d->out = 1; + + d->mtu = s->mtu; + d->cfc = (s->cfc == RFCOMM_CFC_UNKNOWN) ? 0 : s->cfc; + + if (s->state == BT_CONNECTED) { + if (rfcomm_check_security(d)) + rfcomm_send_pn(s, 1, d); + else + set_bit(RFCOMM_AUTH_PENDING, &d->flags); + } + + rfcomm_dlc_set_timer(d, RFCOMM_CONN_TIMEOUT); + + return 0; +} + +int rfcomm_dlc_open(struct rfcomm_dlc *d, bdaddr_t *src, bdaddr_t *dst, u8 channel) +{ + int r; + + rfcomm_lock(); + + r = __rfcomm_dlc_open(d, src, dst, channel); + + rfcomm_unlock(); + return r; +} + +static void __rfcomm_dlc_disconn(struct rfcomm_dlc *d) +{ + struct rfcomm_session *s = d->session; + + d->state = BT_DISCONN; + if (skb_queue_empty(&d->tx_queue)) { + rfcomm_send_disc(s, d->dlci); + rfcomm_dlc_set_timer(d, RFCOMM_DISC_TIMEOUT); + } else { + rfcomm_queue_disc(d); + rfcomm_dlc_set_timer(d, RFCOMM_DISC_TIMEOUT * 2); + } +} + +static int __rfcomm_dlc_close(struct rfcomm_dlc *d, int err) +{ + struct rfcomm_session *s = d->session; + if (!s) + return 0; + + BT_DBG("dlc %p state %ld dlci %d err %d session %p", + d, d->state, d->dlci, err, s); + + switch (d->state) { + case BT_CONNECT: + case BT_CONFIG: + case BT_OPEN: + case BT_CONNECT2: + if (test_and_clear_bit(RFCOMM_DEFER_SETUP, &d->flags)) { + set_bit(RFCOMM_AUTH_REJECT, &d->flags); + rfcomm_schedule(); + return 0; + } + } + + switch (d->state) { + case BT_CONNECT: + case BT_CONNECTED: + __rfcomm_dlc_disconn(d); + break; + + case BT_CONFIG: + if (s->state != BT_BOUND) { + __rfcomm_dlc_disconn(d); + break; + } + /* if closing a dlc in a session that hasn't been started, + * just close and unlink the dlc + */ + + default: + rfcomm_dlc_clear_timer(d); + + rfcomm_dlc_lock(d); + d->state = BT_CLOSED; + d->state_change(d, err); + rfcomm_dlc_unlock(d); + + skb_queue_purge(&d->tx_queue); + rfcomm_dlc_unlink(d); + } + + return 0; +} + +int rfcomm_dlc_close(struct rfcomm_dlc *d, int err) +{ + int r = 0; + struct rfcomm_dlc *d_list; + struct rfcomm_session *s, *s_list; + + BT_DBG("dlc %p state %ld dlci %d err %d", d, d->state, d->dlci, err); + + rfcomm_lock(); + + s = d->session; + if (!s) + goto no_session; + + /* after waiting on the mutex check the session still exists + * then check the dlc still exists + */ + list_for_each_entry(s_list, &session_list, list) { + if (s_list == s) { + list_for_each_entry(d_list, &s->dlcs, list) { + if (d_list == d) { + r = __rfcomm_dlc_close(d, err); + break; + } + } + break; + } + } + +no_session: + rfcomm_unlock(); + return r; +} + +struct rfcomm_dlc *rfcomm_dlc_exists(bdaddr_t *src, bdaddr_t *dst, u8 channel) +{ + struct rfcomm_session *s; + struct rfcomm_dlc *dlc = NULL; + u8 dlci; + + if (rfcomm_check_channel(channel)) + return ERR_PTR(-EINVAL); + + rfcomm_lock(); + s = rfcomm_session_get(src, dst); + if (s) { + dlci = __dlci(__session_dir(s), channel); + dlc = rfcomm_dlc_get(s, dlci); + } + rfcomm_unlock(); + return dlc; +} + +int rfcomm_dlc_send(struct rfcomm_dlc *d, struct sk_buff *skb) +{ + int len = skb->len; + + if (d->state != BT_CONNECTED) + return -ENOTCONN; + + BT_DBG("dlc %p mtu %d len %d", d, d->mtu, len); + + if (len > d->mtu) + return -EINVAL; + + rfcomm_make_uih(skb, d->addr); + skb_queue_tail(&d->tx_queue, skb); + + if (!test_bit(RFCOMM_TX_THROTTLED, &d->flags)) + rfcomm_schedule(); + return len; +} + +void rfcomm_dlc_send_noerror(struct rfcomm_dlc *d, struct sk_buff *skb) +{ + int len = skb->len; + + BT_DBG("dlc %p mtu %d len %d", d, d->mtu, len); + + rfcomm_make_uih(skb, d->addr); + skb_queue_tail(&d->tx_queue, skb); + + if (d->state == BT_CONNECTED && + !test_bit(RFCOMM_TX_THROTTLED, &d->flags)) + rfcomm_schedule(); +} + +void __rfcomm_dlc_throttle(struct rfcomm_dlc *d) +{ + BT_DBG("dlc %p state %ld", d, d->state); + + if (!d->cfc) { + d->v24_sig |= RFCOMM_V24_FC; + set_bit(RFCOMM_MSC_PENDING, &d->flags); + } + rfcomm_schedule(); +} + +void __rfcomm_dlc_unthrottle(struct rfcomm_dlc *d) +{ + BT_DBG("dlc %p state %ld", d, d->state); + + if (!d->cfc) { + d->v24_sig &= ~RFCOMM_V24_FC; + set_bit(RFCOMM_MSC_PENDING, &d->flags); + } + rfcomm_schedule(); +} + +/* + Set/get modem status functions use _local_ status i.e. what we report + to the other side. + Remote status is provided by dlc->modem_status() callback. + */ +int rfcomm_dlc_set_modem_status(struct rfcomm_dlc *d, u8 v24_sig) +{ + BT_DBG("dlc %p state %ld v24_sig 0x%x", + d, d->state, v24_sig); + + if (test_bit(RFCOMM_RX_THROTTLED, &d->flags)) + v24_sig |= RFCOMM_V24_FC; + else + v24_sig &= ~RFCOMM_V24_FC; + + d->v24_sig = v24_sig; + + if (!test_and_set_bit(RFCOMM_MSC_PENDING, &d->flags)) + rfcomm_schedule(); + + return 0; +} + +int rfcomm_dlc_get_modem_status(struct rfcomm_dlc *d, u8 *v24_sig) +{ + BT_DBG("dlc %p state %ld v24_sig 0x%x", + d, d->state, d->v24_sig); + + *v24_sig = d->v24_sig; + return 0; +} + +/* ---- RFCOMM sessions ---- */ +static struct rfcomm_session *rfcomm_session_add(struct socket *sock, int state) +{ + struct rfcomm_session *s = kzalloc(sizeof(*s), GFP_KERNEL); + + if (!s) + return NULL; + + BT_DBG("session %p sock %p", s, sock); + + setup_timer(&s->timer, rfcomm_session_timeout, (unsigned long) s); + + INIT_LIST_HEAD(&s->dlcs); + s->state = state; + s->sock = sock; + + s->mtu = RFCOMM_DEFAULT_MTU; + s->cfc = disable_cfc ? RFCOMM_CFC_DISABLED : RFCOMM_CFC_UNKNOWN; + + /* Do not increment module usage count for listening sessions. + * Otherwise we won't be able to unload the module. */ + if (state != BT_LISTEN) + if (!try_module_get(THIS_MODULE)) { + kfree(s); + return NULL; + } + + list_add(&s->list, &session_list); + + return s; +} + +static struct rfcomm_session *rfcomm_session_del(struct rfcomm_session *s) +{ + int state = s->state; + + BT_DBG("session %p state %ld", s, s->state); + + list_del(&s->list); + + rfcomm_session_clear_timer(s); + sock_release(s->sock); + kfree(s); + + if (state != BT_LISTEN) + module_put(THIS_MODULE); + + return NULL; +} + +static struct rfcomm_session *rfcomm_session_get(bdaddr_t *src, bdaddr_t *dst) +{ + struct rfcomm_session *s; + struct list_head *p, *n; + struct l2cap_chan *chan; + list_for_each_safe(p, n, &session_list) { + s = list_entry(p, struct rfcomm_session, list); + chan = l2cap_pi(s->sock->sk)->chan; + + if ((!bacmp(src, BDADDR_ANY) || !bacmp(&chan->src, src)) && + !bacmp(&chan->dst, dst)) + return s; + } + return NULL; +} + +static struct rfcomm_session *rfcomm_session_close(struct rfcomm_session *s, + int err) +{ + struct rfcomm_dlc *d; + struct list_head *p, *n; + + s->state = BT_CLOSED; + + BT_DBG("session %p state %ld err %d", s, s->state, err); + + /* Close all dlcs */ + list_for_each_safe(p, n, &s->dlcs) { + d = list_entry(p, struct rfcomm_dlc, list); + d->state = BT_CLOSED; + __rfcomm_dlc_close(d, err); + } + + rfcomm_session_clear_timer(s); + return rfcomm_session_del(s); +} + +static struct rfcomm_session *rfcomm_session_create(bdaddr_t *src, + bdaddr_t *dst, + u8 sec_level, + int *err) +{ + struct rfcomm_session *s = NULL; + struct sockaddr_l2 addr; + struct socket *sock; + struct sock *sk; + + BT_DBG("%pMR -> %pMR", src, dst); + + *err = rfcomm_l2sock_create(&sock); + if (*err < 0) + return NULL; + + bacpy(&addr.l2_bdaddr, src); + addr.l2_family = AF_BLUETOOTH; + addr.l2_psm = 0; + addr.l2_cid = 0; + addr.l2_bdaddr_type = BDADDR_BREDR; + *err = kernel_bind(sock, (struct sockaddr *) &addr, sizeof(addr)); + if (*err < 0) + goto failed; + + /* Set L2CAP options */ + sk = sock->sk; + lock_sock(sk); + l2cap_pi(sk)->chan->imtu = l2cap_mtu; + l2cap_pi(sk)->chan->sec_level = sec_level; + if (l2cap_ertm) + l2cap_pi(sk)->chan->mode = L2CAP_MODE_ERTM; + release_sock(sk); + + s = rfcomm_session_add(sock, BT_BOUND); + if (!s) { + *err = -ENOMEM; + goto failed; + } + + s->initiator = 1; + + bacpy(&addr.l2_bdaddr, dst); + addr.l2_family = AF_BLUETOOTH; + addr.l2_psm = cpu_to_le16(L2CAP_PSM_RFCOMM); + addr.l2_cid = 0; + addr.l2_bdaddr_type = BDADDR_BREDR; + *err = kernel_connect(sock, (struct sockaddr *) &addr, sizeof(addr), O_NONBLOCK); + if (*err == 0 || *err == -EINPROGRESS) + return s; + + return rfcomm_session_del(s); + +failed: + sock_release(sock); + return NULL; +} + +void rfcomm_session_getaddr(struct rfcomm_session *s, bdaddr_t *src, bdaddr_t *dst) +{ + struct l2cap_chan *chan = l2cap_pi(s->sock->sk)->chan; + if (src) + bacpy(src, &chan->src); + if (dst) + bacpy(dst, &chan->dst); +} + +/* ---- RFCOMM frame sending ---- */ +static int rfcomm_send_frame(struct rfcomm_session *s, u8 *data, int len) +{ + struct kvec iv = { data, len }; + struct msghdr msg; + + BT_DBG("session %p len %d", s, len); + + memset(&msg, 0, sizeof(msg)); + + return kernel_sendmsg(s->sock, &msg, &iv, 1, len); +} + +static int rfcomm_send_cmd(struct rfcomm_session *s, struct rfcomm_cmd *cmd) +{ + BT_DBG("%p cmd %u", s, cmd->ctrl); + + return rfcomm_send_frame(s, (void *) cmd, sizeof(*cmd)); +} + +static int rfcomm_send_sabm(struct rfcomm_session *s, u8 dlci) +{ + struct rfcomm_cmd cmd; + + BT_DBG("%p dlci %d", s, dlci); + + cmd.addr = __addr(s->initiator, dlci); + cmd.ctrl = __ctrl(RFCOMM_SABM, 1); + cmd.len = __len8(0); + cmd.fcs = __fcs2((u8 *) &cmd); + + return rfcomm_send_cmd(s, &cmd); +} + +static int rfcomm_send_ua(struct rfcomm_session *s, u8 dlci) +{ + struct rfcomm_cmd cmd; + + BT_DBG("%p dlci %d", s, dlci); + + cmd.addr = __addr(!s->initiator, dlci); + cmd.ctrl = __ctrl(RFCOMM_UA, 1); + cmd.len = __len8(0); + cmd.fcs = __fcs2((u8 *) &cmd); + + return rfcomm_send_cmd(s, &cmd); +} + +static int rfcomm_send_disc(struct rfcomm_session *s, u8 dlci) +{ + struct rfcomm_cmd cmd; + + BT_DBG("%p dlci %d", s, dlci); + + cmd.addr = __addr(s->initiator, dlci); + cmd.ctrl = __ctrl(RFCOMM_DISC, 1); + cmd.len = __len8(0); + cmd.fcs = __fcs2((u8 *) &cmd); + + return rfcomm_send_cmd(s, &cmd); +} + +static int rfcomm_queue_disc(struct rfcomm_dlc *d) +{ + struct rfcomm_cmd *cmd; + struct sk_buff *skb; + + BT_DBG("dlc %p dlci %d", d, d->dlci); + + skb = alloc_skb(sizeof(*cmd), GFP_KERNEL); + if (!skb) + return -ENOMEM; + + cmd = (void *) __skb_put(skb, sizeof(*cmd)); + cmd->addr = d->addr; + cmd->ctrl = __ctrl(RFCOMM_DISC, 1); + cmd->len = __len8(0); + cmd->fcs = __fcs2((u8 *) cmd); + + skb_queue_tail(&d->tx_queue, skb); + rfcomm_schedule(); + return 0; +} + +static int rfcomm_send_dm(struct rfcomm_session *s, u8 dlci) +{ + struct rfcomm_cmd cmd; + + BT_DBG("%p dlci %d", s, dlci); + + cmd.addr = __addr(!s->initiator, dlci); + cmd.ctrl = __ctrl(RFCOMM_DM, 1); + cmd.len = __len8(0); + cmd.fcs = __fcs2((u8 *) &cmd); + + return rfcomm_send_cmd(s, &cmd); +} + +static int rfcomm_send_nsc(struct rfcomm_session *s, int cr, u8 type) +{ + struct rfcomm_hdr *hdr; + struct rfcomm_mcc *mcc; + u8 buf[16], *ptr = buf; + + BT_DBG("%p cr %d type %d", s, cr, type); + + hdr = (void *) ptr; ptr += sizeof(*hdr); + hdr->addr = __addr(s->initiator, 0); + hdr->ctrl = __ctrl(RFCOMM_UIH, 0); + hdr->len = __len8(sizeof(*mcc) + 1); + + mcc = (void *) ptr; ptr += sizeof(*mcc); + mcc->type = __mcc_type(0, RFCOMM_NSC); + mcc->len = __len8(1); + + /* Type that we didn't like */ + *ptr = __mcc_type(cr, type); ptr++; + + *ptr = __fcs(buf); ptr++; + + return rfcomm_send_frame(s, buf, ptr - buf); +} + +static int rfcomm_send_pn(struct rfcomm_session *s, int cr, struct rfcomm_dlc *d) +{ + struct rfcomm_hdr *hdr; + struct rfcomm_mcc *mcc; + struct rfcomm_pn *pn; + u8 buf[16], *ptr = buf; + + BT_DBG("%p cr %d dlci %d mtu %d", s, cr, d->dlci, d->mtu); + + hdr = (void *) ptr; ptr += sizeof(*hdr); + hdr->addr = __addr(s->initiator, 0); + hdr->ctrl = __ctrl(RFCOMM_UIH, 0); + hdr->len = __len8(sizeof(*mcc) + sizeof(*pn)); + + mcc = (void *) ptr; ptr += sizeof(*mcc); + mcc->type = __mcc_type(cr, RFCOMM_PN); + mcc->len = __len8(sizeof(*pn)); + + pn = (void *) ptr; ptr += sizeof(*pn); + pn->dlci = d->dlci; + pn->priority = d->priority; + pn->ack_timer = 0; + pn->max_retrans = 0; + + if (s->cfc) { + pn->flow_ctrl = cr ? 0xf0 : 0xe0; + pn->credits = RFCOMM_DEFAULT_CREDITS; + } else { + pn->flow_ctrl = 0; + pn->credits = 0; + } + + if (cr && channel_mtu >= 0) + pn->mtu = cpu_to_le16(channel_mtu); + else + pn->mtu = cpu_to_le16(d->mtu); + + *ptr = __fcs(buf); ptr++; + + return rfcomm_send_frame(s, buf, ptr - buf); +} + +int rfcomm_send_rpn(struct rfcomm_session *s, int cr, u8 dlci, + u8 bit_rate, u8 data_bits, u8 stop_bits, + u8 parity, u8 flow_ctrl_settings, + u8 xon_char, u8 xoff_char, u16 param_mask) +{ + struct rfcomm_hdr *hdr; + struct rfcomm_mcc *mcc; + struct rfcomm_rpn *rpn; + u8 buf[16], *ptr = buf; + + BT_DBG("%p cr %d dlci %d bit_r 0x%x data_b 0x%x stop_b 0x%x parity 0x%x" + " flwc_s 0x%x xon_c 0x%x xoff_c 0x%x p_mask 0x%x", + s, cr, dlci, bit_rate, data_bits, stop_bits, parity, + flow_ctrl_settings, xon_char, xoff_char, param_mask); + + hdr = (void *) ptr; ptr += sizeof(*hdr); + hdr->addr = __addr(s->initiator, 0); + hdr->ctrl = __ctrl(RFCOMM_UIH, 0); + hdr->len = __len8(sizeof(*mcc) + sizeof(*rpn)); + + mcc = (void *) ptr; ptr += sizeof(*mcc); + mcc->type = __mcc_type(cr, RFCOMM_RPN); + mcc->len = __len8(sizeof(*rpn)); + + rpn = (void *) ptr; ptr += sizeof(*rpn); + rpn->dlci = __addr(1, dlci); + rpn->bit_rate = bit_rate; + rpn->line_settings = __rpn_line_settings(data_bits, stop_bits, parity); + rpn->flow_ctrl = flow_ctrl_settings; + rpn->xon_char = xon_char; + rpn->xoff_char = xoff_char; + rpn->param_mask = cpu_to_le16(param_mask); + + *ptr = __fcs(buf); ptr++; + + return rfcomm_send_frame(s, buf, ptr - buf); +} + +static int rfcomm_send_rls(struct rfcomm_session *s, int cr, u8 dlci, u8 status) +{ + struct rfcomm_hdr *hdr; + struct rfcomm_mcc *mcc; + struct rfcomm_rls *rls; + u8 buf[16], *ptr = buf; + + BT_DBG("%p cr %d status 0x%x", s, cr, status); + + hdr = (void *) ptr; ptr += sizeof(*hdr); + hdr->addr = __addr(s->initiator, 0); + hdr->ctrl = __ctrl(RFCOMM_UIH, 0); + hdr->len = __len8(sizeof(*mcc) + sizeof(*rls)); + + mcc = (void *) ptr; ptr += sizeof(*mcc); + mcc->type = __mcc_type(cr, RFCOMM_RLS); + mcc->len = __len8(sizeof(*rls)); + + rls = (void *) ptr; ptr += sizeof(*rls); + rls->dlci = __addr(1, dlci); + rls->status = status; + + *ptr = __fcs(buf); ptr++; + + return rfcomm_send_frame(s, buf, ptr - buf); +} + +static int rfcomm_send_msc(struct rfcomm_session *s, int cr, u8 dlci, u8 v24_sig) +{ + struct rfcomm_hdr *hdr; + struct rfcomm_mcc *mcc; + struct rfcomm_msc *msc; + u8 buf[16], *ptr = buf; + + BT_DBG("%p cr %d v24 0x%x", s, cr, v24_sig); + + hdr = (void *) ptr; ptr += sizeof(*hdr); + hdr->addr = __addr(s->initiator, 0); + hdr->ctrl = __ctrl(RFCOMM_UIH, 0); + hdr->len = __len8(sizeof(*mcc) + sizeof(*msc)); + + mcc = (void *) ptr; ptr += sizeof(*mcc); + mcc->type = __mcc_type(cr, RFCOMM_MSC); + mcc->len = __len8(sizeof(*msc)); + + msc = (void *) ptr; ptr += sizeof(*msc); + msc->dlci = __addr(1, dlci); + msc->v24_sig = v24_sig | 0x01; + + *ptr = __fcs(buf); ptr++; + + return rfcomm_send_frame(s, buf, ptr - buf); +} + +static int rfcomm_send_fcoff(struct rfcomm_session *s, int cr) +{ + struct rfcomm_hdr *hdr; + struct rfcomm_mcc *mcc; + u8 buf[16], *ptr = buf; + + BT_DBG("%p cr %d", s, cr); + + hdr = (void *) ptr; ptr += sizeof(*hdr); + hdr->addr = __addr(s->initiator, 0); + hdr->ctrl = __ctrl(RFCOMM_UIH, 0); + hdr->len = __len8(sizeof(*mcc)); + + mcc = (void *) ptr; ptr += sizeof(*mcc); + mcc->type = __mcc_type(cr, RFCOMM_FCOFF); + mcc->len = __len8(0); + + *ptr = __fcs(buf); ptr++; + + return rfcomm_send_frame(s, buf, ptr - buf); +} + +static int rfcomm_send_fcon(struct rfcomm_session *s, int cr) +{ + struct rfcomm_hdr *hdr; + struct rfcomm_mcc *mcc; + u8 buf[16], *ptr = buf; + + BT_DBG("%p cr %d", s, cr); + + hdr = (void *) ptr; ptr += sizeof(*hdr); + hdr->addr = __addr(s->initiator, 0); + hdr->ctrl = __ctrl(RFCOMM_UIH, 0); + hdr->len = __len8(sizeof(*mcc)); + + mcc = (void *) ptr; ptr += sizeof(*mcc); + mcc->type = __mcc_type(cr, RFCOMM_FCON); + mcc->len = __len8(0); + + *ptr = __fcs(buf); ptr++; + + return rfcomm_send_frame(s, buf, ptr - buf); +} + +static int rfcomm_send_test(struct rfcomm_session *s, int cr, u8 *pattern, int len) +{ + struct socket *sock = s->sock; + struct kvec iv[3]; + struct msghdr msg; + unsigned char hdr[5], crc[1]; + + if (len > 125) + return -EINVAL; + + BT_DBG("%p cr %d", s, cr); + + hdr[0] = __addr(s->initiator, 0); + hdr[1] = __ctrl(RFCOMM_UIH, 0); + hdr[2] = 0x01 | ((len + 2) << 1); + hdr[3] = 0x01 | ((cr & 0x01) << 1) | (RFCOMM_TEST << 2); + hdr[4] = 0x01 | (len << 1); + + crc[0] = __fcs(hdr); + + iv[0].iov_base = hdr; + iv[0].iov_len = 5; + iv[1].iov_base = pattern; + iv[1].iov_len = len; + iv[2].iov_base = crc; + iv[2].iov_len = 1; + + memset(&msg, 0, sizeof(msg)); + + return kernel_sendmsg(sock, &msg, iv, 3, 6 + len); +} + +static int rfcomm_send_credits(struct rfcomm_session *s, u8 addr, u8 credits) +{ + struct rfcomm_hdr *hdr; + u8 buf[16], *ptr = buf; + + BT_DBG("%p addr %d credits %d", s, addr, credits); + + hdr = (void *) ptr; ptr += sizeof(*hdr); + hdr->addr = addr; + hdr->ctrl = __ctrl(RFCOMM_UIH, 1); + hdr->len = __len8(0); + + *ptr = credits; ptr++; + + *ptr = __fcs(buf); ptr++; + + return rfcomm_send_frame(s, buf, ptr - buf); +} + +static void rfcomm_make_uih(struct sk_buff *skb, u8 addr) +{ + struct rfcomm_hdr *hdr; + int len = skb->len; + u8 *crc; + + if (len > 127) { + hdr = (void *) skb_push(skb, 4); + put_unaligned(cpu_to_le16(__len16(len)), (__le16 *) &hdr->len); + } else { + hdr = (void *) skb_push(skb, 3); + hdr->len = __len8(len); + } + hdr->addr = addr; + hdr->ctrl = __ctrl(RFCOMM_UIH, 0); + + crc = skb_put(skb, 1); + *crc = __fcs((void *) hdr); +} + +/* ---- RFCOMM frame reception ---- */ +static struct rfcomm_session *rfcomm_recv_ua(struct rfcomm_session *s, u8 dlci) +{ + BT_DBG("session %p state %ld dlci %d", s, s->state, dlci); + + if (dlci) { + /* Data channel */ + struct rfcomm_dlc *d = rfcomm_dlc_get(s, dlci); + if (!d) { + rfcomm_send_dm(s, dlci); + return s; + } + + switch (d->state) { + case BT_CONNECT: + rfcomm_dlc_clear_timer(d); + + rfcomm_dlc_lock(d); + d->state = BT_CONNECTED; + d->state_change(d, 0); + rfcomm_dlc_unlock(d); + + rfcomm_send_msc(s, 1, dlci, d->v24_sig); + break; + + case BT_DISCONN: + d->state = BT_CLOSED; + __rfcomm_dlc_close(d, 0); + + if (list_empty(&s->dlcs)) { + s->state = BT_DISCONN; + rfcomm_send_disc(s, 0); + rfcomm_session_clear_timer(s); + } + + break; + } + } else { + /* Control channel */ + switch (s->state) { + case BT_CONNECT: + s->state = BT_CONNECTED; + rfcomm_process_connect(s); + break; + + case BT_DISCONN: + s = rfcomm_session_close(s, ECONNRESET); + break; + } + } + return s; +} + +static struct rfcomm_session *rfcomm_recv_dm(struct rfcomm_session *s, u8 dlci) +{ + int err = 0; + + BT_DBG("session %p state %ld dlci %d", s, s->state, dlci); + + if (dlci) { + /* Data DLC */ + struct rfcomm_dlc *d = rfcomm_dlc_get(s, dlci); + if (d) { + if (d->state == BT_CONNECT || d->state == BT_CONFIG) + err = ECONNREFUSED; + else + err = ECONNRESET; + + d->state = BT_CLOSED; + __rfcomm_dlc_close(d, err); + } + } else { + if (s->state == BT_CONNECT) + err = ECONNREFUSED; + else + err = ECONNRESET; + + s = rfcomm_session_close(s, err); + } + return s; +} + +static struct rfcomm_session *rfcomm_recv_disc(struct rfcomm_session *s, + u8 dlci) +{ + int err = 0; + + BT_DBG("session %p state %ld dlci %d", s, s->state, dlci); + + if (dlci) { + struct rfcomm_dlc *d = rfcomm_dlc_get(s, dlci); + if (d) { + rfcomm_send_ua(s, dlci); + + if (d->state == BT_CONNECT || d->state == BT_CONFIG) + err = ECONNREFUSED; + else + err = ECONNRESET; + + d->state = BT_CLOSED; + __rfcomm_dlc_close(d, err); + } else + rfcomm_send_dm(s, dlci); + + } else { + rfcomm_send_ua(s, 0); + + if (s->state == BT_CONNECT) + err = ECONNREFUSED; + else + err = ECONNRESET; + + s = rfcomm_session_close(s, err); + } + return s; +} + +void rfcomm_dlc_accept(struct rfcomm_dlc *d) +{ + struct sock *sk = d->session->sock->sk; + struct l2cap_conn *conn = l2cap_pi(sk)->chan->conn; + + BT_DBG("dlc %p", d); + + rfcomm_send_ua(d->session, d->dlci); + + rfcomm_dlc_clear_timer(d); + + rfcomm_dlc_lock(d); + d->state = BT_CONNECTED; + d->state_change(d, 0); + rfcomm_dlc_unlock(d); + + if (d->role_switch) + hci_conn_switch_role(conn->hcon, 0x00); + + rfcomm_send_msc(d->session, 1, d->dlci, d->v24_sig); +} + +static void rfcomm_check_accept(struct rfcomm_dlc *d) +{ + if (rfcomm_check_security(d)) { + if (d->defer_setup) { + set_bit(RFCOMM_DEFER_SETUP, &d->flags); + rfcomm_dlc_set_timer(d, RFCOMM_AUTH_TIMEOUT); + + rfcomm_dlc_lock(d); + d->state = BT_CONNECT2; + d->state_change(d, 0); + rfcomm_dlc_unlock(d); + } else + rfcomm_dlc_accept(d); + } else { + set_bit(RFCOMM_AUTH_PENDING, &d->flags); + rfcomm_dlc_set_timer(d, RFCOMM_AUTH_TIMEOUT); + } +} + +static int rfcomm_recv_sabm(struct rfcomm_session *s, u8 dlci) +{ + struct rfcomm_dlc *d; + u8 channel; + + BT_DBG("session %p state %ld dlci %d", s, s->state, dlci); + + if (!dlci) { + rfcomm_send_ua(s, 0); + + if (s->state == BT_OPEN) { + s->state = BT_CONNECTED; + rfcomm_process_connect(s); + } + return 0; + } + + /* Check if DLC exists */ + d = rfcomm_dlc_get(s, dlci); + if (d) { + if (d->state == BT_OPEN) { + /* DLC was previously opened by PN request */ + rfcomm_check_accept(d); + } + return 0; + } + + /* Notify socket layer about incoming connection */ + channel = __srv_channel(dlci); + if (rfcomm_connect_ind(s, channel, &d)) { + d->dlci = dlci; + d->addr = __addr(s->initiator, dlci); + rfcomm_dlc_link(s, d); + + rfcomm_check_accept(d); + } else { + rfcomm_send_dm(s, dlci); + } + + return 0; +} + +static int rfcomm_apply_pn(struct rfcomm_dlc *d, int cr, struct rfcomm_pn *pn) +{ + struct rfcomm_session *s = d->session; + + BT_DBG("dlc %p state %ld dlci %d mtu %d fc 0x%x credits %d", + d, d->state, d->dlci, pn->mtu, pn->flow_ctrl, pn->credits); + + if ((pn->flow_ctrl == 0xf0 && s->cfc != RFCOMM_CFC_DISABLED) || + pn->flow_ctrl == 0xe0) { + d->cfc = RFCOMM_CFC_ENABLED; + d->tx_credits = pn->credits; + } else { + d->cfc = RFCOMM_CFC_DISABLED; + set_bit(RFCOMM_TX_THROTTLED, &d->flags); + } + + if (s->cfc == RFCOMM_CFC_UNKNOWN) + s->cfc = d->cfc; + + d->priority = pn->priority; + + d->mtu = __le16_to_cpu(pn->mtu); + + if (cr && d->mtu > s->mtu) + d->mtu = s->mtu; + + return 0; +} + +static int rfcomm_recv_pn(struct rfcomm_session *s, int cr, struct sk_buff *skb) +{ + struct rfcomm_pn *pn = (void *) skb->data; + struct rfcomm_dlc *d; + u8 dlci = pn->dlci; + + BT_DBG("session %p state %ld dlci %d", s, s->state, dlci); + + if (!dlci) + return 0; + + d = rfcomm_dlc_get(s, dlci); + if (d) { + if (cr) { + /* PN request */ + rfcomm_apply_pn(d, cr, pn); + rfcomm_send_pn(s, 0, d); + } else { + /* PN response */ + switch (d->state) { + case BT_CONFIG: + rfcomm_apply_pn(d, cr, pn); + + d->state = BT_CONNECT; + rfcomm_send_sabm(s, d->dlci); + break; + } + } + } else { + u8 channel = __srv_channel(dlci); + + if (!cr) + return 0; + + /* PN request for non existing DLC. + * Assume incoming connection. */ + if (rfcomm_connect_ind(s, channel, &d)) { + d->dlci = dlci; + d->addr = __addr(s->initiator, dlci); + rfcomm_dlc_link(s, d); + + rfcomm_apply_pn(d, cr, pn); + + d->state = BT_OPEN; + rfcomm_send_pn(s, 0, d); + } else { + rfcomm_send_dm(s, dlci); + } + } + return 0; +} + +static int rfcomm_recv_rpn(struct rfcomm_session *s, int cr, int len, struct sk_buff *skb) +{ + struct rfcomm_rpn *rpn = (void *) skb->data; + u8 dlci = __get_dlci(rpn->dlci); + + u8 bit_rate = 0; + u8 data_bits = 0; + u8 stop_bits = 0; + u8 parity = 0; + u8 flow_ctrl = 0; + u8 xon_char = 0; + u8 xoff_char = 0; + u16 rpn_mask = RFCOMM_RPN_PM_ALL; + + BT_DBG("dlci %d cr %d len 0x%x bitr 0x%x line 0x%x flow 0x%x xonc 0x%x xoffc 0x%x pm 0x%x", + dlci, cr, len, rpn->bit_rate, rpn->line_settings, rpn->flow_ctrl, + rpn->xon_char, rpn->xoff_char, rpn->param_mask); + + if (!cr) + return 0; + + if (len == 1) { + /* This is a request, return default (according to ETSI TS 07.10) settings */ + bit_rate = RFCOMM_RPN_BR_9600; + data_bits = RFCOMM_RPN_DATA_8; + stop_bits = RFCOMM_RPN_STOP_1; + parity = RFCOMM_RPN_PARITY_NONE; + flow_ctrl = RFCOMM_RPN_FLOW_NONE; + xon_char = RFCOMM_RPN_XON_CHAR; + xoff_char = RFCOMM_RPN_XOFF_CHAR; + goto rpn_out; + } + + /* Check for sane values, ignore/accept bit_rate, 8 bits, 1 stop bit, + * no parity, no flow control lines, normal XON/XOFF chars */ + + if (rpn->param_mask & cpu_to_le16(RFCOMM_RPN_PM_BITRATE)) { + bit_rate = rpn->bit_rate; + if (bit_rate > RFCOMM_RPN_BR_230400) { + BT_DBG("RPN bit rate mismatch 0x%x", bit_rate); + bit_rate = RFCOMM_RPN_BR_9600; + rpn_mask ^= RFCOMM_RPN_PM_BITRATE; + } + } + + if (rpn->param_mask & cpu_to_le16(RFCOMM_RPN_PM_DATA)) { + data_bits = __get_rpn_data_bits(rpn->line_settings); + if (data_bits != RFCOMM_RPN_DATA_8) { + BT_DBG("RPN data bits mismatch 0x%x", data_bits); + data_bits = RFCOMM_RPN_DATA_8; + rpn_mask ^= RFCOMM_RPN_PM_DATA; + } + } + + if (rpn->param_mask & cpu_to_le16(RFCOMM_RPN_PM_STOP)) { + stop_bits = __get_rpn_stop_bits(rpn->line_settings); + if (stop_bits != RFCOMM_RPN_STOP_1) { + BT_DBG("RPN stop bits mismatch 0x%x", stop_bits); + stop_bits = RFCOMM_RPN_STOP_1; + rpn_mask ^= RFCOMM_RPN_PM_STOP; + } + } + + if (rpn->param_mask & cpu_to_le16(RFCOMM_RPN_PM_PARITY)) { + parity = __get_rpn_parity(rpn->line_settings); + if (parity != RFCOMM_RPN_PARITY_NONE) { + BT_DBG("RPN parity mismatch 0x%x", parity); + parity = RFCOMM_RPN_PARITY_NONE; + rpn_mask ^= RFCOMM_RPN_PM_PARITY; + } + } + + if (rpn->param_mask & cpu_to_le16(RFCOMM_RPN_PM_FLOW)) { + flow_ctrl = rpn->flow_ctrl; + if (flow_ctrl != RFCOMM_RPN_FLOW_NONE) { + BT_DBG("RPN flow ctrl mismatch 0x%x", flow_ctrl); + flow_ctrl = RFCOMM_RPN_FLOW_NONE; + rpn_mask ^= RFCOMM_RPN_PM_FLOW; + } + } + + if (rpn->param_mask & cpu_to_le16(RFCOMM_RPN_PM_XON)) { + xon_char = rpn->xon_char; + if (xon_char != RFCOMM_RPN_XON_CHAR) { + BT_DBG("RPN XON char mismatch 0x%x", xon_char); + xon_char = RFCOMM_RPN_XON_CHAR; + rpn_mask ^= RFCOMM_RPN_PM_XON; + } + } + + if (rpn->param_mask & cpu_to_le16(RFCOMM_RPN_PM_XOFF)) { + xoff_char = rpn->xoff_char; + if (xoff_char != RFCOMM_RPN_XOFF_CHAR) { + BT_DBG("RPN XOFF char mismatch 0x%x", xoff_char); + xoff_char = RFCOMM_RPN_XOFF_CHAR; + rpn_mask ^= RFCOMM_RPN_PM_XOFF; + } + } + +rpn_out: + rfcomm_send_rpn(s, 0, dlci, bit_rate, data_bits, stop_bits, + parity, flow_ctrl, xon_char, xoff_char, rpn_mask); + + return 0; +} + +static int rfcomm_recv_rls(struct rfcomm_session *s, int cr, struct sk_buff *skb) +{ + struct rfcomm_rls *rls = (void *) skb->data; + u8 dlci = __get_dlci(rls->dlci); + + BT_DBG("dlci %d cr %d status 0x%x", dlci, cr, rls->status); + + if (!cr) + return 0; + + /* We should probably do something with this information here. But + * for now it's sufficient just to reply -- Bluetooth 1.1 says it's + * mandatory to recognise and respond to RLS */ + + rfcomm_send_rls(s, 0, dlci, rls->status); + + return 0; +} + +static int rfcomm_recv_msc(struct rfcomm_session *s, int cr, struct sk_buff *skb) +{ + struct rfcomm_msc *msc = (void *) skb->data; + struct rfcomm_dlc *d; + u8 dlci = __get_dlci(msc->dlci); + + BT_DBG("dlci %d cr %d v24 0x%x", dlci, cr, msc->v24_sig); + + d = rfcomm_dlc_get(s, dlci); + if (!d) + return 0; + + if (cr) { + if (msc->v24_sig & RFCOMM_V24_FC && !d->cfc) + set_bit(RFCOMM_TX_THROTTLED, &d->flags); + else + clear_bit(RFCOMM_TX_THROTTLED, &d->flags); + + rfcomm_dlc_lock(d); + + d->remote_v24_sig = msc->v24_sig; + + if (d->modem_status) + d->modem_status(d, msc->v24_sig); + + rfcomm_dlc_unlock(d); + + rfcomm_send_msc(s, 0, dlci, msc->v24_sig); + + d->mscex |= RFCOMM_MSCEX_RX; + } else + d->mscex |= RFCOMM_MSCEX_TX; + + return 0; +} + +static int rfcomm_recv_mcc(struct rfcomm_session *s, struct sk_buff *skb) +{ + struct rfcomm_mcc *mcc = (void *) skb->data; + u8 type, cr, len; + + cr = __test_cr(mcc->type); + type = __get_mcc_type(mcc->type); + len = __get_mcc_len(mcc->len); + + BT_DBG("%p type 0x%x cr %d", s, type, cr); + + skb_pull(skb, 2); + + switch (type) { + case RFCOMM_PN: + rfcomm_recv_pn(s, cr, skb); + break; + + case RFCOMM_RPN: + rfcomm_recv_rpn(s, cr, len, skb); + break; + + case RFCOMM_RLS: + rfcomm_recv_rls(s, cr, skb); + break; + + case RFCOMM_MSC: + rfcomm_recv_msc(s, cr, skb); + break; + + case RFCOMM_FCOFF: + if (cr) { + set_bit(RFCOMM_TX_THROTTLED, &s->flags); + rfcomm_send_fcoff(s, 0); + } + break; + + case RFCOMM_FCON: + if (cr) { + clear_bit(RFCOMM_TX_THROTTLED, &s->flags); + rfcomm_send_fcon(s, 0); + } + break; + + case RFCOMM_TEST: + if (cr) + rfcomm_send_test(s, 0, skb->data, skb->len); + break; + + case RFCOMM_NSC: + break; + + default: + BT_ERR("Unknown control type 0x%02x", type); + rfcomm_send_nsc(s, cr, type); + break; + } + return 0; +} + +static int rfcomm_recv_data(struct rfcomm_session *s, u8 dlci, int pf, struct sk_buff *skb) +{ + struct rfcomm_dlc *d; + + BT_DBG("session %p state %ld dlci %d pf %d", s, s->state, dlci, pf); + + d = rfcomm_dlc_get(s, dlci); + if (!d) { + rfcomm_send_dm(s, dlci); + goto drop; + } + + if (pf && d->cfc) { + u8 credits = *(u8 *) skb->data; skb_pull(skb, 1); + + d->tx_credits += credits; + if (d->tx_credits) + clear_bit(RFCOMM_TX_THROTTLED, &d->flags); + } + + if (skb->len && d->state == BT_CONNECTED) { + rfcomm_dlc_lock(d); + d->rx_credits--; + d->data_ready(d, skb); + rfcomm_dlc_unlock(d); + return 0; + } + +drop: + kfree_skb(skb); + return 0; +} + +static struct rfcomm_session *rfcomm_recv_frame(struct rfcomm_session *s, + struct sk_buff *skb) +{ + struct rfcomm_hdr *hdr = (void *) skb->data; + u8 type, dlci, fcs; + + if (!s) { + /* no session, so free socket data */ + kfree_skb(skb); + return s; + } + + dlci = __get_dlci(hdr->addr); + type = __get_type(hdr->ctrl); + + /* Trim FCS */ + skb->len--; skb->tail--; + fcs = *(u8 *)skb_tail_pointer(skb); + + if (__check_fcs(skb->data, type, fcs)) { + BT_ERR("bad checksum in packet"); + kfree_skb(skb); + return s; + } + + if (__test_ea(hdr->len)) + skb_pull(skb, 3); + else + skb_pull(skb, 4); + + switch (type) { + case RFCOMM_SABM: + if (__test_pf(hdr->ctrl)) + rfcomm_recv_sabm(s, dlci); + break; + + case RFCOMM_DISC: + if (__test_pf(hdr->ctrl)) + s = rfcomm_recv_disc(s, dlci); + break; + + case RFCOMM_UA: + if (__test_pf(hdr->ctrl)) + s = rfcomm_recv_ua(s, dlci); + break; + + case RFCOMM_DM: + s = rfcomm_recv_dm(s, dlci); + break; + + case RFCOMM_UIH: + if (dlci) { + rfcomm_recv_data(s, dlci, __test_pf(hdr->ctrl), skb); + return s; + } + rfcomm_recv_mcc(s, skb); + break; + + default: + BT_ERR("Unknown packet type 0x%02x", type); + break; + } + kfree_skb(skb); + return s; +} + +/* ---- Connection and data processing ---- */ + +static void rfcomm_process_connect(struct rfcomm_session *s) +{ + struct rfcomm_dlc *d; + struct list_head *p, *n; + + BT_DBG("session %p state %ld", s, s->state); + + list_for_each_safe(p, n, &s->dlcs) { + d = list_entry(p, struct rfcomm_dlc, list); + if (d->state == BT_CONFIG) { + d->mtu = s->mtu; + if (rfcomm_check_security(d)) { + rfcomm_send_pn(s, 1, d); + } else { + set_bit(RFCOMM_AUTH_PENDING, &d->flags); + rfcomm_dlc_set_timer(d, RFCOMM_AUTH_TIMEOUT); + } + } + } +} + +/* Send data queued for the DLC. + * Return number of frames left in the queue. + */ +static int rfcomm_process_tx(struct rfcomm_dlc *d) +{ + struct sk_buff *skb; + int err; + + BT_DBG("dlc %p state %ld cfc %d rx_credits %d tx_credits %d", + d, d->state, d->cfc, d->rx_credits, d->tx_credits); + + /* Send pending MSC */ + if (test_and_clear_bit(RFCOMM_MSC_PENDING, &d->flags)) + rfcomm_send_msc(d->session, 1, d->dlci, d->v24_sig); + + if (d->cfc) { + /* CFC enabled. + * Give them some credits */ + if (!test_bit(RFCOMM_RX_THROTTLED, &d->flags) && + d->rx_credits <= (d->cfc >> 2)) { + rfcomm_send_credits(d->session, d->addr, d->cfc - d->rx_credits); + d->rx_credits = d->cfc; + } + } else { + /* CFC disabled. + * Give ourselves some credits */ + d->tx_credits = 5; + } + + if (test_bit(RFCOMM_TX_THROTTLED, &d->flags)) + return skb_queue_len(&d->tx_queue); + + while (d->tx_credits && (skb = skb_dequeue(&d->tx_queue))) { + err = rfcomm_send_frame(d->session, skb->data, skb->len); + if (err < 0) { + skb_queue_head(&d->tx_queue, skb); + break; + } + kfree_skb(skb); + d->tx_credits--; + } + + if (d->cfc && !d->tx_credits) { + /* We're out of TX credits. + * Set TX_THROTTLED flag to avoid unnesary wakeups by dlc_send. */ + set_bit(RFCOMM_TX_THROTTLED, &d->flags); + } + + return skb_queue_len(&d->tx_queue); +} + +static void rfcomm_process_dlcs(struct rfcomm_session *s) +{ + struct rfcomm_dlc *d; + struct list_head *p, *n; + + BT_DBG("session %p state %ld", s, s->state); + + list_for_each_safe(p, n, &s->dlcs) { + d = list_entry(p, struct rfcomm_dlc, list); + + if (test_bit(RFCOMM_TIMED_OUT, &d->flags)) { + __rfcomm_dlc_close(d, ETIMEDOUT); + continue; + } + + if (test_bit(RFCOMM_ENC_DROP, &d->flags)) { + __rfcomm_dlc_close(d, ECONNREFUSED); + continue; + } + + if (test_and_clear_bit(RFCOMM_AUTH_ACCEPT, &d->flags)) { + rfcomm_dlc_clear_timer(d); + if (d->out) { + rfcomm_send_pn(s, 1, d); + rfcomm_dlc_set_timer(d, RFCOMM_CONN_TIMEOUT); + } else { + if (d->defer_setup) { + set_bit(RFCOMM_DEFER_SETUP, &d->flags); + rfcomm_dlc_set_timer(d, RFCOMM_AUTH_TIMEOUT); + + rfcomm_dlc_lock(d); + d->state = BT_CONNECT2; + d->state_change(d, 0); + rfcomm_dlc_unlock(d); + } else + rfcomm_dlc_accept(d); + } + continue; + } else if (test_and_clear_bit(RFCOMM_AUTH_REJECT, &d->flags)) { + rfcomm_dlc_clear_timer(d); + if (!d->out) + rfcomm_send_dm(s, d->dlci); + else + d->state = BT_CLOSED; + __rfcomm_dlc_close(d, ECONNREFUSED); + continue; + } + + if (test_bit(RFCOMM_SEC_PENDING, &d->flags)) + continue; + + if (test_bit(RFCOMM_TX_THROTTLED, &s->flags)) + continue; + + if ((d->state == BT_CONNECTED || d->state == BT_DISCONN) && + d->mscex == RFCOMM_MSCEX_OK) + rfcomm_process_tx(d); + } +} + +static struct rfcomm_session *rfcomm_process_rx(struct rfcomm_session *s) +{ + struct socket *sock = s->sock; + struct sock *sk = sock->sk; + struct sk_buff *skb; + + BT_DBG("session %p state %ld qlen %d", s, s->state, skb_queue_len(&sk->sk_receive_queue)); + + /* Get data directly from socket receive queue without copying it. */ + while ((skb = skb_dequeue(&sk->sk_receive_queue))) { + skb_orphan(skb); + if (!skb_linearize(skb)) { + s = rfcomm_recv_frame(s, skb); + if (!s) + break; + } else { + kfree_skb(skb); + } + } + + if (s && (sk->sk_state == BT_CLOSED)) + s = rfcomm_session_close(s, sk->sk_err); + + return s; +} + +static void rfcomm_accept_connection(struct rfcomm_session *s) +{ + struct socket *sock = s->sock, *nsock; + int err; + + /* Fast check for a new connection. + * Avoids unnesesary socket allocations. */ + if (list_empty(&bt_sk(sock->sk)->accept_q)) + return; + + BT_DBG("session %p", s); + + err = kernel_accept(sock, &nsock, O_NONBLOCK); + if (err < 0) + return; + + /* Set our callbacks */ + nsock->sk->sk_data_ready = rfcomm_l2data_ready; + nsock->sk->sk_state_change = rfcomm_l2state_change; + + s = rfcomm_session_add(nsock, BT_OPEN); + if (s) { + /* We should adjust MTU on incoming sessions. + * L2CAP MTU minus UIH header and FCS. */ + s->mtu = min(l2cap_pi(nsock->sk)->chan->omtu, + l2cap_pi(nsock->sk)->chan->imtu) - 5; + + rfcomm_schedule(); + } else + sock_release(nsock); +} + +static struct rfcomm_session *rfcomm_check_connection(struct rfcomm_session *s) +{ + struct sock *sk = s->sock->sk; + + BT_DBG("%p state %ld", s, s->state); + + switch (sk->sk_state) { + case BT_CONNECTED: + s->state = BT_CONNECT; + + /* We can adjust MTU on outgoing sessions. + * L2CAP MTU minus UIH header and FCS. */ + s->mtu = min(l2cap_pi(sk)->chan->omtu, l2cap_pi(sk)->chan->imtu) - 5; + + rfcomm_send_sabm(s, 0); + break; + + case BT_CLOSED: + s = rfcomm_session_close(s, sk->sk_err); + break; + } + return s; +} + +static void rfcomm_process_sessions(void) +{ + struct list_head *p, *n; + + rfcomm_lock(); + + list_for_each_safe(p, n, &session_list) { + struct rfcomm_session *s; + s = list_entry(p, struct rfcomm_session, list); + + if (test_and_clear_bit(RFCOMM_TIMED_OUT, &s->flags)) { + s->state = BT_DISCONN; + rfcomm_send_disc(s, 0); + continue; + } + + switch (s->state) { + case BT_LISTEN: + rfcomm_accept_connection(s); + continue; + + case BT_BOUND: + s = rfcomm_check_connection(s); + break; + + default: + s = rfcomm_process_rx(s); + break; + } + + if (s) + rfcomm_process_dlcs(s); + } + + rfcomm_unlock(); +} + +static int rfcomm_add_listener(bdaddr_t *ba) +{ + struct sockaddr_l2 addr; + struct socket *sock; + struct sock *sk; + struct rfcomm_session *s; + int err = 0; + + /* Create socket */ + err = rfcomm_l2sock_create(&sock); + if (err < 0) { + BT_ERR("Create socket failed %d", err); + return err; + } + + /* Bind socket */ + bacpy(&addr.l2_bdaddr, ba); + addr.l2_family = AF_BLUETOOTH; + addr.l2_psm = cpu_to_le16(L2CAP_PSM_RFCOMM); + addr.l2_cid = 0; + addr.l2_bdaddr_type = BDADDR_BREDR; + err = kernel_bind(sock, (struct sockaddr *) &addr, sizeof(addr)); + if (err < 0) { + BT_ERR("Bind failed %d", err); + goto failed; + } + + /* Set L2CAP options */ + sk = sock->sk; + lock_sock(sk); + l2cap_pi(sk)->chan->imtu = l2cap_mtu; + release_sock(sk); + + /* Start listening on the socket */ + err = kernel_listen(sock, 10); + if (err) { + BT_ERR("Listen failed %d", err); + goto failed; + } + + /* Add listening session */ + s = rfcomm_session_add(sock, BT_LISTEN); + if (!s) { + err = -ENOMEM; + goto failed; + } + + return 0; +failed: + sock_release(sock); + return err; +} + +static void rfcomm_kill_listener(void) +{ + struct rfcomm_session *s; + struct list_head *p, *n; + + BT_DBG(""); + + list_for_each_safe(p, n, &session_list) { + s = list_entry(p, struct rfcomm_session, list); + rfcomm_session_del(s); + } +} + +static int rfcomm_run(void *unused) +{ + DEFINE_WAIT_FUNC(wait, woken_wake_function); + BT_DBG(""); + + set_user_nice(current, -10); + + rfcomm_add_listener(BDADDR_ANY); + + add_wait_queue(&rfcomm_wq, &wait); + while (!kthread_should_stop()) { + + /* Process stuff */ + rfcomm_process_sessions(); + + wait_woken(&wait, TASK_INTERRUPTIBLE, MAX_SCHEDULE_TIMEOUT); + } + remove_wait_queue(&rfcomm_wq, &wait); + + rfcomm_kill_listener(); + + return 0; +} + +static void rfcomm_security_cfm(struct hci_conn *conn, u8 status, u8 encrypt) +{ + struct rfcomm_session *s; + struct rfcomm_dlc *d; + struct list_head *p, *n; + + BT_DBG("conn %p status 0x%02x encrypt 0x%02x", conn, status, encrypt); + + s = rfcomm_session_get(&conn->hdev->bdaddr, &conn->dst); + if (!s) + return; + + list_for_each_safe(p, n, &s->dlcs) { + d = list_entry(p, struct rfcomm_dlc, list); + + if (test_and_clear_bit(RFCOMM_SEC_PENDING, &d->flags)) { + rfcomm_dlc_clear_timer(d); + if (status || encrypt == 0x00) { + set_bit(RFCOMM_ENC_DROP, &d->flags); + continue; + } + } + + if (d->state == BT_CONNECTED && !status && encrypt == 0x00) { + if (d->sec_level == BT_SECURITY_MEDIUM) { + set_bit(RFCOMM_SEC_PENDING, &d->flags); + rfcomm_dlc_set_timer(d, RFCOMM_AUTH_TIMEOUT); + continue; + } else if (d->sec_level == BT_SECURITY_HIGH || + d->sec_level == BT_SECURITY_FIPS) { + set_bit(RFCOMM_ENC_DROP, &d->flags); + continue; + } + } + + if (!test_and_clear_bit(RFCOMM_AUTH_PENDING, &d->flags)) + continue; + + if (!status && hci_conn_check_secure(conn, d->sec_level)) + set_bit(RFCOMM_AUTH_ACCEPT, &d->flags); + else + set_bit(RFCOMM_AUTH_REJECT, &d->flags); + } + + rfcomm_schedule(); +} + +static struct hci_cb rfcomm_cb = { + .name = "RFCOMM", + .security_cfm = rfcomm_security_cfm +}; + +static int rfcomm_dlc_debugfs_show(struct seq_file *f, void *x) +{ + struct rfcomm_session *s; + + rfcomm_lock(); + + list_for_each_entry(s, &session_list, list) { + struct l2cap_chan *chan = l2cap_pi(s->sock->sk)->chan; + struct rfcomm_dlc *d; + list_for_each_entry(d, &s->dlcs, list) { + seq_printf(f, "%pMR %pMR %ld %d %d %d %d\n", + &chan->src, &chan->dst, + d->state, d->dlci, d->mtu, + d->rx_credits, d->tx_credits); + } + } + + rfcomm_unlock(); + + return 0; +} + +static int rfcomm_dlc_debugfs_open(struct inode *inode, struct file *file) +{ + return single_open(file, rfcomm_dlc_debugfs_show, inode->i_private); +} + +static const struct file_operations rfcomm_dlc_debugfs_fops = { + .open = rfcomm_dlc_debugfs_open, + .read = seq_read, + .llseek = seq_lseek, + .release = single_release, +}; + +static struct dentry *rfcomm_dlc_debugfs; + +/* ---- Initialization ---- */ +static int __init rfcomm_init(void) +{ + int err; + + hci_register_cb(&rfcomm_cb); + + rfcomm_thread = kthread_run(rfcomm_run, NULL, "krfcommd"); + if (IS_ERR(rfcomm_thread)) { + err = PTR_ERR(rfcomm_thread); + goto unregister; + } + + err = rfcomm_init_ttys(); + if (err < 0) + goto stop; + + err = rfcomm_init_sockets(); + if (err < 0) + goto cleanup; + + BT_INFO("RFCOMM ver %s", VERSION); + + if (IS_ERR_OR_NULL(bt_debugfs)) + return 0; + + rfcomm_dlc_debugfs = debugfs_create_file("rfcomm_dlc", 0444, + bt_debugfs, NULL, + &rfcomm_dlc_debugfs_fops); + + return 0; + +cleanup: + rfcomm_cleanup_ttys(); + +stop: + kthread_stop(rfcomm_thread); + +unregister: + hci_unregister_cb(&rfcomm_cb); + + return err; +} + +static void __exit rfcomm_exit(void) +{ + debugfs_remove(rfcomm_dlc_debugfs); + + hci_unregister_cb(&rfcomm_cb); + + kthread_stop(rfcomm_thread); + + rfcomm_cleanup_ttys(); + + rfcomm_cleanup_sockets(); +} + +module_init(rfcomm_init); +module_exit(rfcomm_exit); + +module_param(disable_cfc, bool, 0644); +MODULE_PARM_DESC(disable_cfc, "Disable credit based flow control"); + +module_param(channel_mtu, int, 0644); +MODULE_PARM_DESC(channel_mtu, "Default MTU for the RFCOMM channel"); + +module_param(l2cap_mtu, uint, 0644); +MODULE_PARM_DESC(l2cap_mtu, "Default MTU for the L2CAP connection"); + +module_param(l2cap_ertm, bool, 0644); +MODULE_PARM_DESC(l2cap_ertm, "Use L2CAP ERTM mode for connection"); + +MODULE_AUTHOR("Marcel Holtmann <marcel@holtmann.org>"); +MODULE_DESCRIPTION("Bluetooth RFCOMM ver " VERSION); +MODULE_VERSION(VERSION); +MODULE_LICENSE("GPL"); +MODULE_ALIAS("bt-proto-3"); diff --git a/kernel/net/bluetooth/rfcomm/sock.c b/kernel/net/bluetooth/rfcomm/sock.c new file mode 100644 index 000000000..825e8fb51 --- /dev/null +++ b/kernel/net/bluetooth/rfcomm/sock.c @@ -0,0 +1,1104 @@ +/* + RFCOMM implementation for Linux Bluetooth stack (BlueZ). + Copyright (C) 2002 Maxim Krasnyansky <maxk@qualcomm.com> + Copyright (C) 2002 Marcel Holtmann <marcel@holtmann.org> + + 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; + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS + OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT OF THIRD PARTY RIGHTS. + IN NO EVENT SHALL THE COPYRIGHT HOLDER(S) AND AUTHOR(S) BE LIABLE FOR ANY + CLAIM, OR ANY SPECIAL INDIRECT OR CONSEQUENTIAL DAMAGES, OR ANY DAMAGES + WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN + ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF + OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + + ALL LIABILITY, INCLUDING LIABILITY FOR INFRINGEMENT OF ANY PATENTS, + COPYRIGHTS, TRADEMARKS OR OTHER RIGHTS, RELATING TO USE OF THIS + SOFTWARE IS DISCLAIMED. +*/ + +/* + * RFCOMM sockets. + */ + +#include <linux/export.h> +#include <linux/debugfs.h> + +#include <net/bluetooth/bluetooth.h> +#include <net/bluetooth/hci_core.h> +#include <net/bluetooth/l2cap.h> +#include <net/bluetooth/rfcomm.h> + +static const struct proto_ops rfcomm_sock_ops; + +static struct bt_sock_list rfcomm_sk_list = { + .lock = __RW_LOCK_UNLOCKED(rfcomm_sk_list.lock) +}; + +static void rfcomm_sock_close(struct sock *sk); +static void rfcomm_sock_kill(struct sock *sk); + +/* ---- DLC callbacks ---- + * + * called under rfcomm_dlc_lock() + */ +static void rfcomm_sk_data_ready(struct rfcomm_dlc *d, struct sk_buff *skb) +{ + struct sock *sk = d->owner; + if (!sk) + return; + + atomic_add(skb->len, &sk->sk_rmem_alloc); + skb_queue_tail(&sk->sk_receive_queue, skb); + sk->sk_data_ready(sk); + + if (atomic_read(&sk->sk_rmem_alloc) >= sk->sk_rcvbuf) + rfcomm_dlc_throttle(d); +} + +static void rfcomm_sk_state_change(struct rfcomm_dlc *d, int err) +{ + struct sock *sk = d->owner, *parent; + unsigned long flags; + + if (!sk) + return; + + BT_DBG("dlc %p state %ld err %d", d, d->state, err); + + local_irq_save(flags); + bh_lock_sock(sk); + + if (err) + sk->sk_err = err; + + sk->sk_state = d->state; + + parent = bt_sk(sk)->parent; + if (parent) { + if (d->state == BT_CLOSED) { + sock_set_flag(sk, SOCK_ZAPPED); + bt_accept_unlink(sk); + } + parent->sk_data_ready(parent); + } else { + if (d->state == BT_CONNECTED) + rfcomm_session_getaddr(d->session, + &rfcomm_pi(sk)->src, NULL); + sk->sk_state_change(sk); + } + + bh_unlock_sock(sk); + local_irq_restore(flags); + + if (parent && sock_flag(sk, SOCK_ZAPPED)) { + /* We have to drop DLC lock here, otherwise + * rfcomm_sock_destruct() will dead lock. */ + rfcomm_dlc_unlock(d); + rfcomm_sock_kill(sk); + rfcomm_dlc_lock(d); + } +} + +/* ---- Socket functions ---- */ +static struct sock *__rfcomm_get_listen_sock_by_addr(u8 channel, bdaddr_t *src) +{ + struct sock *sk = NULL; + + sk_for_each(sk, &rfcomm_sk_list.head) { + if (rfcomm_pi(sk)->channel != channel) + continue; + + if (bacmp(&rfcomm_pi(sk)->src, src)) + continue; + + if (sk->sk_state == BT_BOUND || sk->sk_state == BT_LISTEN) + break; + } + + return sk ? sk : NULL; +} + +/* Find socket with channel and source bdaddr. + * Returns closest match. + */ +static struct sock *rfcomm_get_sock_by_channel(int state, u8 channel, bdaddr_t *src) +{ + struct sock *sk = NULL, *sk1 = NULL; + + read_lock(&rfcomm_sk_list.lock); + + sk_for_each(sk, &rfcomm_sk_list.head) { + if (state && sk->sk_state != state) + continue; + + if (rfcomm_pi(sk)->channel == channel) { + /* Exact match. */ + if (!bacmp(&rfcomm_pi(sk)->src, src)) + break; + + /* Closest match */ + if (!bacmp(&rfcomm_pi(sk)->src, BDADDR_ANY)) + sk1 = sk; + } + } + + read_unlock(&rfcomm_sk_list.lock); + + return sk ? sk : sk1; +} + +static void rfcomm_sock_destruct(struct sock *sk) +{ + struct rfcomm_dlc *d = rfcomm_pi(sk)->dlc; + + BT_DBG("sk %p dlc %p", sk, d); + + skb_queue_purge(&sk->sk_receive_queue); + skb_queue_purge(&sk->sk_write_queue); + + rfcomm_dlc_lock(d); + rfcomm_pi(sk)->dlc = NULL; + + /* Detach DLC if it's owned by this socket */ + if (d->owner == sk) + d->owner = NULL; + rfcomm_dlc_unlock(d); + + rfcomm_dlc_put(d); +} + +static void rfcomm_sock_cleanup_listen(struct sock *parent) +{ + struct sock *sk; + + BT_DBG("parent %p", parent); + + /* Close not yet accepted dlcs */ + while ((sk = bt_accept_dequeue(parent, NULL))) { + rfcomm_sock_close(sk); + rfcomm_sock_kill(sk); + } + + parent->sk_state = BT_CLOSED; + sock_set_flag(parent, SOCK_ZAPPED); +} + +/* Kill socket (only if zapped and orphan) + * Must be called on unlocked socket. + */ +static void rfcomm_sock_kill(struct sock *sk) +{ + if (!sock_flag(sk, SOCK_ZAPPED) || sk->sk_socket) + return; + + BT_DBG("sk %p state %d refcnt %d", sk, sk->sk_state, atomic_read(&sk->sk_refcnt)); + + /* Kill poor orphan */ + bt_sock_unlink(&rfcomm_sk_list, sk); + sock_set_flag(sk, SOCK_DEAD); + sock_put(sk); +} + +static void __rfcomm_sock_close(struct sock *sk) +{ + struct rfcomm_dlc *d = rfcomm_pi(sk)->dlc; + + BT_DBG("sk %p state %d socket %p", sk, sk->sk_state, sk->sk_socket); + + switch (sk->sk_state) { + case BT_LISTEN: + rfcomm_sock_cleanup_listen(sk); + break; + + case BT_CONNECT: + case BT_CONNECT2: + case BT_CONFIG: + case BT_CONNECTED: + rfcomm_dlc_close(d, 0); + + default: + sock_set_flag(sk, SOCK_ZAPPED); + break; + } +} + +/* Close socket. + * Must be called on unlocked socket. + */ +static void rfcomm_sock_close(struct sock *sk) +{ + lock_sock(sk); + __rfcomm_sock_close(sk); + release_sock(sk); +} + +static void rfcomm_sock_init(struct sock *sk, struct sock *parent) +{ + struct rfcomm_pinfo *pi = rfcomm_pi(sk); + + BT_DBG("sk %p", sk); + + if (parent) { + sk->sk_type = parent->sk_type; + pi->dlc->defer_setup = test_bit(BT_SK_DEFER_SETUP, + &bt_sk(parent)->flags); + + pi->sec_level = rfcomm_pi(parent)->sec_level; + pi->role_switch = rfcomm_pi(parent)->role_switch; + + security_sk_clone(parent, sk); + } else { + pi->dlc->defer_setup = 0; + + pi->sec_level = BT_SECURITY_LOW; + pi->role_switch = 0; + } + + pi->dlc->sec_level = pi->sec_level; + pi->dlc->role_switch = pi->role_switch; +} + +static struct proto rfcomm_proto = { + .name = "RFCOMM", + .owner = THIS_MODULE, + .obj_size = sizeof(struct rfcomm_pinfo) +}; + +static struct sock *rfcomm_sock_alloc(struct net *net, struct socket *sock, int proto, gfp_t prio) +{ + struct rfcomm_dlc *d; + struct sock *sk; + + sk = sk_alloc(net, PF_BLUETOOTH, prio, &rfcomm_proto); + if (!sk) + return NULL; + + sock_init_data(sock, sk); + INIT_LIST_HEAD(&bt_sk(sk)->accept_q); + + d = rfcomm_dlc_alloc(prio); + if (!d) { + sk_free(sk); + return NULL; + } + + d->data_ready = rfcomm_sk_data_ready; + d->state_change = rfcomm_sk_state_change; + + rfcomm_pi(sk)->dlc = d; + d->owner = sk; + + sk->sk_destruct = rfcomm_sock_destruct; + sk->sk_sndtimeo = RFCOMM_CONN_TIMEOUT; + + sk->sk_sndbuf = RFCOMM_MAX_CREDITS * RFCOMM_DEFAULT_MTU * 10; + sk->sk_rcvbuf = RFCOMM_MAX_CREDITS * RFCOMM_DEFAULT_MTU * 10; + + sock_reset_flag(sk, SOCK_ZAPPED); + + sk->sk_protocol = proto; + sk->sk_state = BT_OPEN; + + bt_sock_link(&rfcomm_sk_list, sk); + + BT_DBG("sk %p", sk); + return sk; +} + +static int rfcomm_sock_create(struct net *net, struct socket *sock, + int protocol, int kern) +{ + struct sock *sk; + + BT_DBG("sock %p", sock); + + sock->state = SS_UNCONNECTED; + + if (sock->type != SOCK_STREAM && sock->type != SOCK_RAW) + return -ESOCKTNOSUPPORT; + + sock->ops = &rfcomm_sock_ops; + + sk = rfcomm_sock_alloc(net, sock, protocol, GFP_ATOMIC); + if (!sk) + return -ENOMEM; + + rfcomm_sock_init(sk, NULL); + return 0; +} + +static int rfcomm_sock_bind(struct socket *sock, struct sockaddr *addr, int addr_len) +{ + struct sockaddr_rc *sa = (struct sockaddr_rc *) addr; + struct sock *sk = sock->sk; + int chan = sa->rc_channel; + int err = 0; + + BT_DBG("sk %p %pMR", sk, &sa->rc_bdaddr); + + if (!addr || addr->sa_family != AF_BLUETOOTH) + return -EINVAL; + + lock_sock(sk); + + if (sk->sk_state != BT_OPEN) { + err = -EBADFD; + goto done; + } + + if (sk->sk_type != SOCK_STREAM) { + err = -EINVAL; + goto done; + } + + write_lock(&rfcomm_sk_list.lock); + + if (chan && __rfcomm_get_listen_sock_by_addr(chan, &sa->rc_bdaddr)) { + err = -EADDRINUSE; + } else { + /* Save source address */ + bacpy(&rfcomm_pi(sk)->src, &sa->rc_bdaddr); + rfcomm_pi(sk)->channel = chan; + sk->sk_state = BT_BOUND; + } + + write_unlock(&rfcomm_sk_list.lock); + +done: + release_sock(sk); + return err; +} + +static int rfcomm_sock_connect(struct socket *sock, struct sockaddr *addr, int alen, int flags) +{ + struct sockaddr_rc *sa = (struct sockaddr_rc *) addr; + struct sock *sk = sock->sk; + struct rfcomm_dlc *d = rfcomm_pi(sk)->dlc; + int err = 0; + + BT_DBG("sk %p", sk); + + if (alen < sizeof(struct sockaddr_rc) || + addr->sa_family != AF_BLUETOOTH) + return -EINVAL; + + lock_sock(sk); + + if (sk->sk_state != BT_OPEN && sk->sk_state != BT_BOUND) { + err = -EBADFD; + goto done; + } + + if (sk->sk_type != SOCK_STREAM) { + err = -EINVAL; + goto done; + } + + sk->sk_state = BT_CONNECT; + bacpy(&rfcomm_pi(sk)->dst, &sa->rc_bdaddr); + rfcomm_pi(sk)->channel = sa->rc_channel; + + d->sec_level = rfcomm_pi(sk)->sec_level; + d->role_switch = rfcomm_pi(sk)->role_switch; + + err = rfcomm_dlc_open(d, &rfcomm_pi(sk)->src, &sa->rc_bdaddr, + sa->rc_channel); + if (!err) + err = bt_sock_wait_state(sk, BT_CONNECTED, + sock_sndtimeo(sk, flags & O_NONBLOCK)); + +done: + release_sock(sk); + return err; +} + +static int rfcomm_sock_listen(struct socket *sock, int backlog) +{ + struct sock *sk = sock->sk; + int err = 0; + + BT_DBG("sk %p backlog %d", sk, backlog); + + lock_sock(sk); + + if (sk->sk_state != BT_BOUND) { + err = -EBADFD; + goto done; + } + + if (sk->sk_type != SOCK_STREAM) { + err = -EINVAL; + goto done; + } + + if (!rfcomm_pi(sk)->channel) { + bdaddr_t *src = &rfcomm_pi(sk)->src; + u8 channel; + + err = -EINVAL; + + write_lock(&rfcomm_sk_list.lock); + + for (channel = 1; channel < 31; channel++) + if (!__rfcomm_get_listen_sock_by_addr(channel, src)) { + rfcomm_pi(sk)->channel = channel; + err = 0; + break; + } + + write_unlock(&rfcomm_sk_list.lock); + + if (err < 0) + goto done; + } + + sk->sk_max_ack_backlog = backlog; + sk->sk_ack_backlog = 0; + sk->sk_state = BT_LISTEN; + +done: + release_sock(sk); + return err; +} + +static int rfcomm_sock_accept(struct socket *sock, struct socket *newsock, int flags) +{ + DEFINE_WAIT_FUNC(wait, woken_wake_function); + struct sock *sk = sock->sk, *nsk; + long timeo; + int err = 0; + + lock_sock_nested(sk, SINGLE_DEPTH_NESTING); + + if (sk->sk_type != SOCK_STREAM) { + err = -EINVAL; + goto done; + } + + timeo = sock_rcvtimeo(sk, flags & O_NONBLOCK); + + BT_DBG("sk %p timeo %ld", sk, timeo); + + /* Wait for an incoming connection. (wake-one). */ + add_wait_queue_exclusive(sk_sleep(sk), &wait); + while (1) { + if (sk->sk_state != BT_LISTEN) { + err = -EBADFD; + break; + } + + nsk = bt_accept_dequeue(sk, newsock); + if (nsk) + break; + + if (!timeo) { + err = -EAGAIN; + break; + } + + if (signal_pending(current)) { + err = sock_intr_errno(timeo); + break; + } + + release_sock(sk); + + timeo = wait_woken(&wait, TASK_INTERRUPTIBLE, timeo); + + lock_sock_nested(sk, SINGLE_DEPTH_NESTING); + } + remove_wait_queue(sk_sleep(sk), &wait); + + if (err) + goto done; + + newsock->state = SS_CONNECTED; + + BT_DBG("new socket %p", nsk); + +done: + release_sock(sk); + return err; +} + +static int rfcomm_sock_getname(struct socket *sock, struct sockaddr *addr, int *len, int peer) +{ + struct sockaddr_rc *sa = (struct sockaddr_rc *) addr; + struct sock *sk = sock->sk; + + BT_DBG("sock %p, sk %p", sock, sk); + + if (peer && sk->sk_state != BT_CONNECTED && + sk->sk_state != BT_CONNECT && sk->sk_state != BT_CONNECT2) + return -ENOTCONN; + + memset(sa, 0, sizeof(*sa)); + sa->rc_family = AF_BLUETOOTH; + sa->rc_channel = rfcomm_pi(sk)->channel; + if (peer) + bacpy(&sa->rc_bdaddr, &rfcomm_pi(sk)->dst); + else + bacpy(&sa->rc_bdaddr, &rfcomm_pi(sk)->src); + + *len = sizeof(struct sockaddr_rc); + return 0; +} + +static int rfcomm_sock_sendmsg(struct socket *sock, struct msghdr *msg, + size_t len) +{ + struct sock *sk = sock->sk; + struct rfcomm_dlc *d = rfcomm_pi(sk)->dlc; + struct sk_buff *skb; + int sent; + + if (test_bit(RFCOMM_DEFER_SETUP, &d->flags)) + return -ENOTCONN; + + if (msg->msg_flags & MSG_OOB) + return -EOPNOTSUPP; + + if (sk->sk_shutdown & SEND_SHUTDOWN) + return -EPIPE; + + BT_DBG("sock %p, sk %p", sock, sk); + + lock_sock(sk); + + sent = bt_sock_wait_ready(sk, msg->msg_flags); + if (sent) + goto done; + + while (len) { + size_t size = min_t(size_t, len, d->mtu); + int err; + + skb = sock_alloc_send_skb(sk, size + RFCOMM_SKB_RESERVE, + msg->msg_flags & MSG_DONTWAIT, &err); + if (!skb) { + if (sent == 0) + sent = err; + break; + } + skb_reserve(skb, RFCOMM_SKB_HEAD_RESERVE); + + err = memcpy_from_msg(skb_put(skb, size), msg, size); + if (err) { + kfree_skb(skb); + if (sent == 0) + sent = err; + break; + } + + skb->priority = sk->sk_priority; + + err = rfcomm_dlc_send(d, skb); + if (err < 0) { + kfree_skb(skb); + if (sent == 0) + sent = err; + break; + } + + sent += size; + len -= size; + } + +done: + release_sock(sk); + + return sent; +} + +static int rfcomm_sock_recvmsg(struct socket *sock, struct msghdr *msg, + size_t size, int flags) +{ + struct sock *sk = sock->sk; + struct rfcomm_dlc *d = rfcomm_pi(sk)->dlc; + int len; + + if (test_and_clear_bit(RFCOMM_DEFER_SETUP, &d->flags)) { + rfcomm_dlc_accept(d); + return 0; + } + + len = bt_sock_stream_recvmsg(sock, msg, size, flags); + + lock_sock(sk); + if (!(flags & MSG_PEEK) && len > 0) + atomic_sub(len, &sk->sk_rmem_alloc); + + if (atomic_read(&sk->sk_rmem_alloc) <= (sk->sk_rcvbuf >> 2)) + rfcomm_dlc_unthrottle(rfcomm_pi(sk)->dlc); + release_sock(sk); + + return len; +} + +static int rfcomm_sock_setsockopt_old(struct socket *sock, int optname, char __user *optval, unsigned int optlen) +{ + struct sock *sk = sock->sk; + int err = 0; + u32 opt; + + BT_DBG("sk %p", sk); + + lock_sock(sk); + + switch (optname) { + case RFCOMM_LM: + if (get_user(opt, (u32 __user *) optval)) { + err = -EFAULT; + break; + } + + if (opt & RFCOMM_LM_FIPS) { + err = -EINVAL; + break; + } + + if (opt & RFCOMM_LM_AUTH) + rfcomm_pi(sk)->sec_level = BT_SECURITY_LOW; + if (opt & RFCOMM_LM_ENCRYPT) + rfcomm_pi(sk)->sec_level = BT_SECURITY_MEDIUM; + if (opt & RFCOMM_LM_SECURE) + rfcomm_pi(sk)->sec_level = BT_SECURITY_HIGH; + + rfcomm_pi(sk)->role_switch = (opt & RFCOMM_LM_MASTER); + break; + + default: + err = -ENOPROTOOPT; + break; + } + + release_sock(sk); + return err; +} + +static int rfcomm_sock_setsockopt(struct socket *sock, int level, int optname, char __user *optval, unsigned int optlen) +{ + struct sock *sk = sock->sk; + struct bt_security sec; + int err = 0; + size_t len; + u32 opt; + + BT_DBG("sk %p", sk); + + if (level == SOL_RFCOMM) + return rfcomm_sock_setsockopt_old(sock, optname, optval, optlen); + + if (level != SOL_BLUETOOTH) + return -ENOPROTOOPT; + + lock_sock(sk); + + switch (optname) { + case BT_SECURITY: + if (sk->sk_type != SOCK_STREAM) { + err = -EINVAL; + break; + } + + sec.level = BT_SECURITY_LOW; + + len = min_t(unsigned int, sizeof(sec), optlen); + if (copy_from_user((char *) &sec, optval, len)) { + err = -EFAULT; + break; + } + + if (sec.level > BT_SECURITY_HIGH) { + err = -EINVAL; + break; + } + + rfcomm_pi(sk)->sec_level = sec.level; + break; + + case BT_DEFER_SETUP: + if (sk->sk_state != BT_BOUND && sk->sk_state != BT_LISTEN) { + err = -EINVAL; + break; + } + + if (get_user(opt, (u32 __user *) optval)) { + err = -EFAULT; + break; + } + + if (opt) + set_bit(BT_SK_DEFER_SETUP, &bt_sk(sk)->flags); + else + clear_bit(BT_SK_DEFER_SETUP, &bt_sk(sk)->flags); + + break; + + default: + err = -ENOPROTOOPT; + break; + } + + release_sock(sk); + return err; +} + +static int rfcomm_sock_getsockopt_old(struct socket *sock, int optname, char __user *optval, int __user *optlen) +{ + struct sock *sk = sock->sk; + struct sock *l2cap_sk; + struct l2cap_conn *conn; + struct rfcomm_conninfo cinfo; + int len, err = 0; + u32 opt; + + BT_DBG("sk %p", sk); + + if (get_user(len, optlen)) + return -EFAULT; + + lock_sock(sk); + + switch (optname) { + case RFCOMM_LM: + switch (rfcomm_pi(sk)->sec_level) { + case BT_SECURITY_LOW: + opt = RFCOMM_LM_AUTH; + break; + case BT_SECURITY_MEDIUM: + opt = RFCOMM_LM_AUTH | RFCOMM_LM_ENCRYPT; + break; + case BT_SECURITY_HIGH: + opt = RFCOMM_LM_AUTH | RFCOMM_LM_ENCRYPT | + RFCOMM_LM_SECURE; + break; + case BT_SECURITY_FIPS: + opt = RFCOMM_LM_AUTH | RFCOMM_LM_ENCRYPT | + RFCOMM_LM_SECURE | RFCOMM_LM_FIPS; + break; + default: + opt = 0; + break; + } + + if (rfcomm_pi(sk)->role_switch) + opt |= RFCOMM_LM_MASTER; + + if (put_user(opt, (u32 __user *) optval)) + err = -EFAULT; + + break; + + case RFCOMM_CONNINFO: + if (sk->sk_state != BT_CONNECTED && + !rfcomm_pi(sk)->dlc->defer_setup) { + err = -ENOTCONN; + break; + } + + l2cap_sk = rfcomm_pi(sk)->dlc->session->sock->sk; + conn = l2cap_pi(l2cap_sk)->chan->conn; + + memset(&cinfo, 0, sizeof(cinfo)); + cinfo.hci_handle = conn->hcon->handle; + memcpy(cinfo.dev_class, conn->hcon->dev_class, 3); + + len = min_t(unsigned int, len, sizeof(cinfo)); + if (copy_to_user(optval, (char *) &cinfo, len)) + err = -EFAULT; + + break; + + default: + err = -ENOPROTOOPT; + break; + } + + release_sock(sk); + return err; +} + +static int rfcomm_sock_getsockopt(struct socket *sock, int level, int optname, char __user *optval, int __user *optlen) +{ + struct sock *sk = sock->sk; + struct bt_security sec; + int len, err = 0; + + BT_DBG("sk %p", sk); + + if (level == SOL_RFCOMM) + return rfcomm_sock_getsockopt_old(sock, optname, optval, optlen); + + if (level != SOL_BLUETOOTH) + return -ENOPROTOOPT; + + if (get_user(len, optlen)) + return -EFAULT; + + lock_sock(sk); + + switch (optname) { + case BT_SECURITY: + if (sk->sk_type != SOCK_STREAM) { + err = -EINVAL; + break; + } + + sec.level = rfcomm_pi(sk)->sec_level; + sec.key_size = 0; + + len = min_t(unsigned int, len, sizeof(sec)); + if (copy_to_user(optval, (char *) &sec, len)) + err = -EFAULT; + + break; + + case BT_DEFER_SETUP: + if (sk->sk_state != BT_BOUND && sk->sk_state != BT_LISTEN) { + err = -EINVAL; + break; + } + + if (put_user(test_bit(BT_SK_DEFER_SETUP, &bt_sk(sk)->flags), + (u32 __user *) optval)) + err = -EFAULT; + + break; + + default: + err = -ENOPROTOOPT; + break; + } + + release_sock(sk); + return err; +} + +static int rfcomm_sock_ioctl(struct socket *sock, unsigned int cmd, unsigned long arg) +{ + struct sock *sk __maybe_unused = sock->sk; + int err; + + BT_DBG("sk %p cmd %x arg %lx", sk, cmd, arg); + + err = bt_sock_ioctl(sock, cmd, arg); + + if (err == -ENOIOCTLCMD) { +#ifdef CONFIG_BT_RFCOMM_TTY + lock_sock(sk); + err = rfcomm_dev_ioctl(sk, cmd, (void __user *) arg); + release_sock(sk); +#else + err = -EOPNOTSUPP; +#endif + } + + return err; +} + +static int rfcomm_sock_shutdown(struct socket *sock, int how) +{ + struct sock *sk = sock->sk; + int err = 0; + + BT_DBG("sock %p, sk %p", sock, sk); + + if (!sk) + return 0; + + lock_sock(sk); + if (!sk->sk_shutdown) { + sk->sk_shutdown = SHUTDOWN_MASK; + __rfcomm_sock_close(sk); + + if (sock_flag(sk, SOCK_LINGER) && sk->sk_lingertime && + !(current->flags & PF_EXITING)) + err = bt_sock_wait_state(sk, BT_CLOSED, sk->sk_lingertime); + } + release_sock(sk); + return err; +} + +static int rfcomm_sock_release(struct socket *sock) +{ + struct sock *sk = sock->sk; + int err; + + BT_DBG("sock %p, sk %p", sock, sk); + + if (!sk) + return 0; + + err = rfcomm_sock_shutdown(sock, 2); + + sock_orphan(sk); + rfcomm_sock_kill(sk); + return err; +} + +/* ---- RFCOMM core layer callbacks ---- + * + * called under rfcomm_lock() + */ +int rfcomm_connect_ind(struct rfcomm_session *s, u8 channel, struct rfcomm_dlc **d) +{ + struct sock *sk, *parent; + bdaddr_t src, dst; + int result = 0; + + BT_DBG("session %p channel %d", s, channel); + + rfcomm_session_getaddr(s, &src, &dst); + + /* Check if we have socket listening on channel */ + parent = rfcomm_get_sock_by_channel(BT_LISTEN, channel, &src); + if (!parent) + return 0; + + bh_lock_sock(parent); + + /* Check for backlog size */ + if (sk_acceptq_is_full(parent)) { + BT_DBG("backlog full %d", parent->sk_ack_backlog); + goto done; + } + + sk = rfcomm_sock_alloc(sock_net(parent), NULL, BTPROTO_RFCOMM, GFP_ATOMIC); + if (!sk) + goto done; + + bt_sock_reclassify_lock(sk, BTPROTO_RFCOMM); + + rfcomm_sock_init(sk, parent); + bacpy(&rfcomm_pi(sk)->src, &src); + bacpy(&rfcomm_pi(sk)->dst, &dst); + rfcomm_pi(sk)->channel = channel; + + sk->sk_state = BT_CONFIG; + bt_accept_enqueue(parent, sk); + + /* Accept connection and return socket DLC */ + *d = rfcomm_pi(sk)->dlc; + result = 1; + +done: + bh_unlock_sock(parent); + + if (test_bit(BT_SK_DEFER_SETUP, &bt_sk(parent)->flags)) + parent->sk_state_change(parent); + + return result; +} + +static int rfcomm_sock_debugfs_show(struct seq_file *f, void *p) +{ + struct sock *sk; + + read_lock(&rfcomm_sk_list.lock); + + sk_for_each(sk, &rfcomm_sk_list.head) { + seq_printf(f, "%pMR %pMR %d %d\n", + &rfcomm_pi(sk)->src, &rfcomm_pi(sk)->dst, + sk->sk_state, rfcomm_pi(sk)->channel); + } + + read_unlock(&rfcomm_sk_list.lock); + + return 0; +} + +static int rfcomm_sock_debugfs_open(struct inode *inode, struct file *file) +{ + return single_open(file, rfcomm_sock_debugfs_show, inode->i_private); +} + +static const struct file_operations rfcomm_sock_debugfs_fops = { + .open = rfcomm_sock_debugfs_open, + .read = seq_read, + .llseek = seq_lseek, + .release = single_release, +}; + +static struct dentry *rfcomm_sock_debugfs; + +static const struct proto_ops rfcomm_sock_ops = { + .family = PF_BLUETOOTH, + .owner = THIS_MODULE, + .release = rfcomm_sock_release, + .bind = rfcomm_sock_bind, + .connect = rfcomm_sock_connect, + .listen = rfcomm_sock_listen, + .accept = rfcomm_sock_accept, + .getname = rfcomm_sock_getname, + .sendmsg = rfcomm_sock_sendmsg, + .recvmsg = rfcomm_sock_recvmsg, + .shutdown = rfcomm_sock_shutdown, + .setsockopt = rfcomm_sock_setsockopt, + .getsockopt = rfcomm_sock_getsockopt, + .ioctl = rfcomm_sock_ioctl, + .poll = bt_sock_poll, + .socketpair = sock_no_socketpair, + .mmap = sock_no_mmap +}; + +static const struct net_proto_family rfcomm_sock_family_ops = { + .family = PF_BLUETOOTH, + .owner = THIS_MODULE, + .create = rfcomm_sock_create +}; + +int __init rfcomm_init_sockets(void) +{ + int err; + + BUILD_BUG_ON(sizeof(struct sockaddr_rc) > sizeof(struct sockaddr)); + + err = proto_register(&rfcomm_proto, 0); + if (err < 0) + return err; + + err = bt_sock_register(BTPROTO_RFCOMM, &rfcomm_sock_family_ops); + if (err < 0) { + BT_ERR("RFCOMM socket layer registration failed"); + goto error; + } + + err = bt_procfs_init(&init_net, "rfcomm", &rfcomm_sk_list, NULL); + if (err < 0) { + BT_ERR("Failed to create RFCOMM proc file"); + bt_sock_unregister(BTPROTO_RFCOMM); + goto error; + } + + BT_INFO("RFCOMM socket layer initialized"); + + if (IS_ERR_OR_NULL(bt_debugfs)) + return 0; + + rfcomm_sock_debugfs = debugfs_create_file("rfcomm", 0444, + bt_debugfs, NULL, + &rfcomm_sock_debugfs_fops); + + return 0; + +error: + proto_unregister(&rfcomm_proto); + return err; +} + +void __exit rfcomm_cleanup_sockets(void) +{ + bt_procfs_cleanup(&init_net, "rfcomm"); + + debugfs_remove(rfcomm_sock_debugfs); + + bt_sock_unregister(BTPROTO_RFCOMM); + + proto_unregister(&rfcomm_proto); +} diff --git a/kernel/net/bluetooth/rfcomm/tty.c b/kernel/net/bluetooth/rfcomm/tty.c new file mode 100644 index 000000000..8e385a0ae --- /dev/null +++ b/kernel/net/bluetooth/rfcomm/tty.c @@ -0,0 +1,1174 @@ +/* + RFCOMM implementation for Linux Bluetooth stack (BlueZ). + Copyright (C) 2002 Maxim Krasnyansky <maxk@qualcomm.com> + Copyright (C) 2002 Marcel Holtmann <marcel@holtmann.org> + + 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; + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS + OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT OF THIRD PARTY RIGHTS. + IN NO EVENT SHALL THE COPYRIGHT HOLDER(S) AND AUTHOR(S) BE LIABLE FOR ANY + CLAIM, OR ANY SPECIAL INDIRECT OR CONSEQUENTIAL DAMAGES, OR ANY DAMAGES + WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN + ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF + OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + + ALL LIABILITY, INCLUDING LIABILITY FOR INFRINGEMENT OF ANY PATENTS, + COPYRIGHTS, TRADEMARKS OR OTHER RIGHTS, RELATING TO USE OF THIS + SOFTWARE IS DISCLAIMED. +*/ + +/* + * RFCOMM TTY. + */ + +#include <linux/module.h> + +#include <linux/tty.h> +#include <linux/tty_driver.h> +#include <linux/tty_flip.h> + +#include <net/bluetooth/bluetooth.h> +#include <net/bluetooth/hci_core.h> +#include <net/bluetooth/rfcomm.h> + +#define RFCOMM_TTY_MAGIC 0x6d02 /* magic number for rfcomm struct */ +#define RFCOMM_TTY_PORTS RFCOMM_MAX_DEV /* whole lotta rfcomm devices */ +#define RFCOMM_TTY_MAJOR 216 /* device node major id of the usb/bluetooth.c driver */ +#define RFCOMM_TTY_MINOR 0 + +static DEFINE_MUTEX(rfcomm_ioctl_mutex); +static struct tty_driver *rfcomm_tty_driver; + +struct rfcomm_dev { + struct tty_port port; + struct list_head list; + + char name[12]; + int id; + unsigned long flags; + int err; + + unsigned long status; /* don't export to userspace */ + + bdaddr_t src; + bdaddr_t dst; + u8 channel; + + uint modem_status; + + struct rfcomm_dlc *dlc; + + struct device *tty_dev; + + atomic_t wmem_alloc; + + struct sk_buff_head pending; +}; + +static LIST_HEAD(rfcomm_dev_list); +static DEFINE_MUTEX(rfcomm_dev_lock); + +static void rfcomm_dev_data_ready(struct rfcomm_dlc *dlc, struct sk_buff *skb); +static void rfcomm_dev_state_change(struct rfcomm_dlc *dlc, int err); +static void rfcomm_dev_modem_status(struct rfcomm_dlc *dlc, u8 v24_sig); + +/* ---- Device functions ---- */ + +static void rfcomm_dev_destruct(struct tty_port *port) +{ + struct rfcomm_dev *dev = container_of(port, struct rfcomm_dev, port); + struct rfcomm_dlc *dlc = dev->dlc; + + BT_DBG("dev %p dlc %p", dev, dlc); + + rfcomm_dlc_lock(dlc); + /* Detach DLC if it's owned by this dev */ + if (dlc->owner == dev) + dlc->owner = NULL; + rfcomm_dlc_unlock(dlc); + + rfcomm_dlc_put(dlc); + + if (dev->tty_dev) + tty_unregister_device(rfcomm_tty_driver, dev->id); + + mutex_lock(&rfcomm_dev_lock); + list_del(&dev->list); + mutex_unlock(&rfcomm_dev_lock); + + kfree(dev); + + /* It's safe to call module_put() here because socket still + holds reference to this module. */ + module_put(THIS_MODULE); +} + +/* 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); + int err; + + err = rfcomm_dlc_open(dev->dlc, &dev->src, &dev->dst, dev->channel); + if (err) + set_bit(TTY_IO_ERROR, &tty->flags); + return err; +} + +/* we block the open until the dlc->state becomes BT_CONNECTED */ +static int rfcomm_dev_carrier_raised(struct tty_port *port) +{ + struct rfcomm_dev *dev = container_of(port, struct rfcomm_dev, port); + + return (dev->dlc->state == BT_CONNECTED); +} + +/* device-specific cleanup: close the dlc */ +static void rfcomm_dev_shutdown(struct tty_port *port) +{ + struct rfcomm_dev *dev = container_of(port, struct rfcomm_dev, port); + + if (dev->tty_dev->parent) + device_move(dev->tty_dev, NULL, DPM_ORDER_DEV_LAST); + + /* close the dlc */ + rfcomm_dlc_close(dev->dlc, 0); +} + +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_lookup(int id) +{ + struct rfcomm_dev *dev; + + list_for_each_entry(dev, &rfcomm_dev_list, list) + if (dev->id == id) + return dev; + + return NULL; +} + +static struct rfcomm_dev *rfcomm_dev_get(int id) +{ + struct rfcomm_dev *dev; + + mutex_lock(&rfcomm_dev_lock); + + dev = __rfcomm_dev_lookup(id); + + if (dev && !tty_port_get(&dev->port)) + dev = NULL; + + mutex_unlock(&rfcomm_dev_lock); + + return dev; +} + +static void rfcomm_reparent_device(struct rfcomm_dev *dev) +{ + struct hci_dev *hdev; + struct hci_conn *conn; + + hdev = hci_get_route(&dev->dst, &dev->src); + if (!hdev) + return; + + /* The lookup results are unsafe to access without the + * hci device lock (FIXME: why is this not documented?) + */ + hci_dev_lock(hdev); + conn = hci_conn_hash_lookup_ba(hdev, ACL_LINK, &dev->dst); + + /* Just because the acl link is in the hash table is no + * guarantee the sysfs device has been added ... + */ + if (conn && device_is_registered(&conn->dev)) + device_move(dev->tty_dev, &conn->dev, DPM_ORDER_DEV_AFTER_PARENT); + + hci_dev_unlock(hdev); + hci_dev_put(hdev); +} + +static ssize_t show_address(struct device *tty_dev, struct device_attribute *attr, char *buf) +{ + struct rfcomm_dev *dev = dev_get_drvdata(tty_dev); + return sprintf(buf, "%pMR\n", &dev->dst); +} + +static ssize_t show_channel(struct device *tty_dev, struct device_attribute *attr, char *buf) +{ + struct rfcomm_dev *dev = dev_get_drvdata(tty_dev); + return sprintf(buf, "%d\n", dev->channel); +} + +static DEVICE_ATTR(address, S_IRUGO, show_address, NULL); +static DEVICE_ATTR(channel, S_IRUGO, show_channel, NULL); + +static struct rfcomm_dev *__rfcomm_dev_add(struct rfcomm_dev_req *req, + struct rfcomm_dlc *dlc) +{ + struct rfcomm_dev *dev, *entry; + struct list_head *head = &rfcomm_dev_list; + int err = 0; + + dev = kzalloc(sizeof(struct rfcomm_dev), GFP_KERNEL); + if (!dev) + return ERR_PTR(-ENOMEM); + + mutex_lock(&rfcomm_dev_lock); + + if (req->dev_id < 0) { + dev->id = 0; + + list_for_each_entry(entry, &rfcomm_dev_list, list) { + if (entry->id != dev->id) + break; + + dev->id++; + head = &entry->list; + } + } else { + dev->id = req->dev_id; + + list_for_each_entry(entry, &rfcomm_dev_list, list) { + if (entry->id == dev->id) { + err = -EADDRINUSE; + goto out; + } + + if (entry->id > dev->id - 1) + break; + + head = &entry->list; + } + } + + if ((dev->id < 0) || (dev->id > RFCOMM_MAX_DEV - 1)) { + err = -ENFILE; + goto out; + } + + sprintf(dev->name, "rfcomm%d", dev->id); + + list_add(&dev->list, head); + + bacpy(&dev->src, &req->src); + bacpy(&dev->dst, &req->dst); + dev->channel = req->channel; + + dev->flags = req->flags & + ((1 << RFCOMM_RELEASE_ONHUP) | (1 << RFCOMM_REUSE_DLC)); + + tty_port_init(&dev->port); + dev->port.ops = &rfcomm_port_ops; + + skb_queue_head_init(&dev->pending); + + rfcomm_dlc_lock(dlc); + + if (req->flags & (1 << RFCOMM_REUSE_DLC)) { + struct sock *sk = dlc->owner; + struct sk_buff *skb; + + BUG_ON(!sk); + + rfcomm_dlc_throttle(dlc); + + while ((skb = skb_dequeue(&sk->sk_receive_queue))) { + skb_orphan(skb); + skb_queue_tail(&dev->pending, skb); + atomic_sub(skb->len, &sk->sk_rmem_alloc); + } + } + + dlc->data_ready = rfcomm_dev_data_ready; + dlc->state_change = rfcomm_dev_state_change; + dlc->modem_status = rfcomm_dev_modem_status; + + dlc->owner = dev; + dev->dlc = dlc; + + rfcomm_dev_modem_status(dlc, dlc->remote_v24_sig); + + rfcomm_dlc_unlock(dlc); + + /* It's safe to call __module_get() here because socket already + holds reference to this module. */ + __module_get(THIS_MODULE); + + mutex_unlock(&rfcomm_dev_lock); + return dev; + +out: + mutex_unlock(&rfcomm_dev_lock); + kfree(dev); + return ERR_PTR(err); +} + +static int rfcomm_dev_add(struct rfcomm_dev_req *req, struct rfcomm_dlc *dlc) +{ + struct rfcomm_dev *dev; + struct device *tty; + + BT_DBG("id %d channel %d", req->dev_id, req->channel); + + dev = __rfcomm_dev_add(req, dlc); + if (IS_ERR(dev)) { + rfcomm_dlc_put(dlc); + return PTR_ERR(dev); + } + + tty = tty_port_register_device(&dev->port, rfcomm_tty_driver, + dev->id, NULL); + if (IS_ERR(tty)) { + tty_port_put(&dev->port); + return PTR_ERR(tty); + } + + dev->tty_dev = tty; + rfcomm_reparent_device(dev); + dev_set_drvdata(dev->tty_dev, dev); + + if (device_create_file(dev->tty_dev, &dev_attr_address) < 0) + BT_ERR("Failed to create address attribute"); + + if (device_create_file(dev->tty_dev, &dev_attr_channel) < 0) + BT_ERR("Failed to create channel attribute"); + + return dev->id; +} + +/* ---- Send buffer ---- */ +static inline unsigned int rfcomm_room(struct rfcomm_dev *dev) +{ + struct rfcomm_dlc *dlc = dev->dlc; + + /* Limit the outstanding number of packets not yet sent to 40 */ + int pending = 40 - atomic_read(&dev->wmem_alloc); + + return max(0, pending) * dlc->mtu; +} + +static void rfcomm_wfree(struct sk_buff *skb) +{ + struct rfcomm_dev *dev = (void *) skb->sk; + atomic_dec(&dev->wmem_alloc); + if (test_bit(RFCOMM_TTY_ATTACHED, &dev->flags)) + tty_port_tty_wakeup(&dev->port); + tty_port_put(&dev->port); +} + +static void rfcomm_set_owner_w(struct sk_buff *skb, struct rfcomm_dev *dev) +{ + tty_port_get(&dev->port); + atomic_inc(&dev->wmem_alloc); + skb->sk = (void *) dev; + skb->destructor = rfcomm_wfree; +} + +static struct sk_buff *rfcomm_wmalloc(struct rfcomm_dev *dev, unsigned long size, gfp_t priority) +{ + struct sk_buff *skb = alloc_skb(size, priority); + if (skb) + rfcomm_set_owner_w(skb, dev); + return skb; +} + +/* ---- Device IOCTLs ---- */ + +#define NOCAP_FLAGS ((1 << RFCOMM_REUSE_DLC) | (1 << RFCOMM_RELEASE_ONHUP)) + +static int __rfcomm_create_dev(struct sock *sk, void __user *arg) +{ + struct rfcomm_dev_req req; + struct rfcomm_dlc *dlc; + int id; + + if (copy_from_user(&req, arg, sizeof(req))) + return -EFAULT; + + BT_DBG("sk %p dev_id %d flags 0x%x", sk, req.dev_id, req.flags); + + if (req.flags != NOCAP_FLAGS && !capable(CAP_NET_ADMIN)) + return -EPERM; + + if (req.flags & (1 << RFCOMM_REUSE_DLC)) { + /* Socket must be connected */ + if (sk->sk_state != BT_CONNECTED) + return -EBADFD; + + dlc = rfcomm_pi(sk)->dlc; + rfcomm_dlc_hold(dlc); + } else { + /* Validate the channel is unused */ + dlc = rfcomm_dlc_exists(&req.src, &req.dst, req.channel); + if (IS_ERR(dlc)) + return PTR_ERR(dlc); + else if (dlc) { + rfcomm_dlc_put(dlc); + return -EBUSY; + } + dlc = rfcomm_dlc_alloc(GFP_KERNEL); + if (!dlc) + return -ENOMEM; + } + + id = rfcomm_dev_add(&req, dlc); + if (id < 0) + return id; + + if (req.flags & (1 << RFCOMM_REUSE_DLC)) { + /* DLC is now used by device. + * Socket must be disconnected */ + sk->sk_state = BT_CLOSED; + } + + return id; +} + +static int __rfcomm_release_dev(void __user *arg) +{ + struct rfcomm_dev_req req; + struct rfcomm_dev *dev; + struct tty_struct *tty; + + if (copy_from_user(&req, arg, sizeof(req))) + return -EFAULT; + + BT_DBG("dev_id %d flags 0x%x", req.dev_id, req.flags); + + dev = rfcomm_dev_get(req.dev_id); + if (!dev) + return -ENODEV; + + if (dev->flags != NOCAP_FLAGS && !capable(CAP_NET_ADMIN)) { + tty_port_put(&dev->port); + return -EPERM; + } + + /* only release once */ + if (test_and_set_bit(RFCOMM_DEV_RELEASED, &dev->status)) { + tty_port_put(&dev->port); + return -EALREADY; + } + + if (req.flags & (1 << RFCOMM_HANGUP_NOW)) + rfcomm_dlc_close(dev->dlc, 0); + + /* Shut down TTY synchronously before freeing rfcomm_dev */ + tty = tty_port_tty_get(&dev->port); + if (tty) { + tty_vhangup(tty); + tty_kref_put(tty); + } + + if (!test_bit(RFCOMM_TTY_OWNED, &dev->status)) + tty_port_put(&dev->port); + + tty_port_put(&dev->port); + return 0; +} + +static int rfcomm_create_dev(struct sock *sk, void __user *arg) +{ + int ret; + + mutex_lock(&rfcomm_ioctl_mutex); + ret = __rfcomm_create_dev(sk, arg); + mutex_unlock(&rfcomm_ioctl_mutex); + + return ret; +} + +static int rfcomm_release_dev(void __user *arg) +{ + int ret; + + mutex_lock(&rfcomm_ioctl_mutex); + ret = __rfcomm_release_dev(arg); + mutex_unlock(&rfcomm_ioctl_mutex); + + return ret; +} + +static int rfcomm_get_dev_list(void __user *arg) +{ + struct rfcomm_dev *dev; + struct rfcomm_dev_list_req *dl; + struct rfcomm_dev_info *di; + int n = 0, size, err; + u16 dev_num; + + BT_DBG(""); + + if (get_user(dev_num, (u16 __user *) arg)) + return -EFAULT; + + if (!dev_num || dev_num > (PAGE_SIZE * 4) / sizeof(*di)) + return -EINVAL; + + size = sizeof(*dl) + dev_num * sizeof(*di); + + dl = kzalloc(size, GFP_KERNEL); + if (!dl) + return -ENOMEM; + + di = dl->dev_info; + + mutex_lock(&rfcomm_dev_lock); + + list_for_each_entry(dev, &rfcomm_dev_list, list) { + if (!tty_port_get(&dev->port)) + continue; + (di + n)->id = dev->id; + (di + n)->flags = dev->flags; + (di + n)->state = dev->dlc->state; + (di + n)->channel = dev->channel; + bacpy(&(di + n)->src, &dev->src); + bacpy(&(di + n)->dst, &dev->dst); + tty_port_put(&dev->port); + if (++n >= dev_num) + break; + } + + mutex_unlock(&rfcomm_dev_lock); + + dl->dev_num = n; + size = sizeof(*dl) + n * sizeof(*di); + + err = copy_to_user(arg, dl, size); + kfree(dl); + + return err ? -EFAULT : 0; +} + +static int rfcomm_get_dev_info(void __user *arg) +{ + struct rfcomm_dev *dev; + struct rfcomm_dev_info di; + int err = 0; + + BT_DBG(""); + + if (copy_from_user(&di, arg, sizeof(di))) + return -EFAULT; + + dev = rfcomm_dev_get(di.id); + if (!dev) + return -ENODEV; + + di.flags = dev->flags; + di.channel = dev->channel; + di.state = dev->dlc->state; + bacpy(&di.src, &dev->src); + bacpy(&di.dst, &dev->dst); + + if (copy_to_user(arg, &di, sizeof(di))) + err = -EFAULT; + + tty_port_put(&dev->port); + return err; +} + +int rfcomm_dev_ioctl(struct sock *sk, unsigned int cmd, void __user *arg) +{ + BT_DBG("cmd %d arg %p", cmd, arg); + + switch (cmd) { + case RFCOMMCREATEDEV: + return rfcomm_create_dev(sk, arg); + + case RFCOMMRELEASEDEV: + return rfcomm_release_dev(arg); + + case RFCOMMGETDEVLIST: + return rfcomm_get_dev_list(arg); + + case RFCOMMGETDEVINFO: + return rfcomm_get_dev_info(arg); + } + + return -EINVAL; +} + +/* ---- DLC callbacks ---- */ +static void rfcomm_dev_data_ready(struct rfcomm_dlc *dlc, struct sk_buff *skb) +{ + struct rfcomm_dev *dev = dlc->owner; + + if (!dev) { + kfree_skb(skb); + return; + } + + if (!skb_queue_empty(&dev->pending)) { + skb_queue_tail(&dev->pending, skb); + return; + } + + BT_DBG("dlc %p len %d", dlc, skb->len); + + tty_insert_flip_string(&dev->port, skb->data, skb->len); + tty_flip_buffer_push(&dev->port); + + kfree_skb(skb); +} + +static void rfcomm_dev_state_change(struct rfcomm_dlc *dlc, int err) +{ + struct rfcomm_dev *dev = dlc->owner; + if (!dev) + return; + + BT_DBG("dlc %p dev %p err %d", dlc, dev, err); + + dev->err = err; + if (dlc->state == BT_CONNECTED) { + rfcomm_reparent_device(dev); + + wake_up_interruptible(&dev->port.open_wait); + } else if (dlc->state == BT_CLOSED) + tty_port_tty_hangup(&dev->port, false); +} + +static void rfcomm_dev_modem_status(struct rfcomm_dlc *dlc, u8 v24_sig) +{ + struct rfcomm_dev *dev = dlc->owner; + if (!dev) + return; + + BT_DBG("dlc %p dev %p v24_sig 0x%02x", dlc, dev, v24_sig); + + if ((dev->modem_status & TIOCM_CD) && !(v24_sig & RFCOMM_V24_DV)) + tty_port_tty_hangup(&dev->port, true); + + dev->modem_status = + ((v24_sig & RFCOMM_V24_RTC) ? (TIOCM_DSR | TIOCM_DTR) : 0) | + ((v24_sig & RFCOMM_V24_RTR) ? (TIOCM_RTS | TIOCM_CTS) : 0) | + ((v24_sig & RFCOMM_V24_IC) ? TIOCM_RI : 0) | + ((v24_sig & RFCOMM_V24_DV) ? TIOCM_CD : 0); +} + +/* ---- TTY functions ---- */ +static void rfcomm_tty_copy_pending(struct rfcomm_dev *dev) +{ + struct sk_buff *skb; + int inserted = 0; + + BT_DBG("dev %p", dev); + + rfcomm_dlc_lock(dev->dlc); + + while ((skb = skb_dequeue(&dev->pending))) { + inserted += tty_insert_flip_string(&dev->port, skb->data, + skb->len); + kfree_skb(skb); + } + + rfcomm_dlc_unlock(dev->dlc); + + if (inserted > 0) + tty_flip_buffer_push(&dev->port); +} + +/* do the reverse of install, clearing the tty fields and releasing the + * reference to tty_port + */ +static void rfcomm_tty_cleanup(struct tty_struct *tty) +{ + struct rfcomm_dev *dev = tty->driver_data; + + clear_bit(RFCOMM_TTY_ATTACHED, &dev->flags); + + rfcomm_dlc_lock(dev->dlc); + tty->driver_data = NULL; + rfcomm_dlc_unlock(dev->dlc); + + /* + * purge the dlc->tx_queue to avoid circular dependencies + * between dev and dlc + */ + skb_queue_purge(&dev->dlc->tx_queue); + + tty_port_put(&dev->port); +} + +/* we acquire the tty_port reference since it's here the tty is first used + * by setting the termios. We also populate the driver_data field and install + * the tty port + */ +static int rfcomm_tty_install(struct tty_driver *driver, struct tty_struct *tty) +{ + struct rfcomm_dev *dev; + struct rfcomm_dlc *dlc; + int err; + + dev = rfcomm_dev_get(tty->index); + if (!dev) + return -ENODEV; + + dlc = dev->dlc; + + /* Attach TTY and open DLC */ + rfcomm_dlc_lock(dlc); + tty->driver_data = dev; + rfcomm_dlc_unlock(dlc); + set_bit(RFCOMM_TTY_ATTACHED, &dev->flags); + + /* install the tty_port */ + err = tty_port_install(&dev->port, driver, tty); + if (err) { + rfcomm_tty_cleanup(tty); + 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)) { + set_bit(RFCOMM_TTY_OWNED, &dev->status); + tty_port_put(&dev->port); + } + + return 0; +} + +static int rfcomm_tty_open(struct tty_struct *tty, struct file *filp) +{ + struct rfcomm_dev *dev = tty->driver_data; + int err; + + BT_DBG("tty %p id %d", tty, tty->index); + + BT_DBG("dev %p dst %pMR channel %d opened %d", dev, &dev->dst, + dev->channel, dev->port.count); + + err = tty_port_open(&dev->port, tty, filp); + if (err) + return err; + + /* + * FIXME: rfcomm should use proper flow control for + * received data. This hack will be unnecessary and can + * be removed when that's implemented + */ + rfcomm_tty_copy_pending(dev); + + rfcomm_dlc_unthrottle(dev->dlc); + + return 0; +} + +static void rfcomm_tty_close(struct tty_struct *tty, struct file *filp) +{ + struct rfcomm_dev *dev = (struct rfcomm_dev *) tty->driver_data; + + BT_DBG("tty %p dev %p dlc %p opened %d", tty, dev, dev->dlc, + dev->port.count); + + tty_port_close(&dev->port, tty, filp); +} + +static int rfcomm_tty_write(struct tty_struct *tty, const unsigned char *buf, int count) +{ + struct rfcomm_dev *dev = (struct rfcomm_dev *) tty->driver_data; + struct rfcomm_dlc *dlc = dev->dlc; + struct sk_buff *skb; + int sent = 0, size; + + BT_DBG("tty %p count %d", tty, count); + + while (count) { + size = min_t(uint, count, dlc->mtu); + + skb = rfcomm_wmalloc(dev, size + RFCOMM_SKB_RESERVE, GFP_ATOMIC); + if (!skb) + break; + + skb_reserve(skb, RFCOMM_SKB_HEAD_RESERVE); + + memcpy(skb_put(skb, size), buf + sent, size); + + rfcomm_dlc_send_noerror(dlc, skb); + + sent += size; + count -= size; + } + + return sent; +} + +static int rfcomm_tty_write_room(struct tty_struct *tty) +{ + struct rfcomm_dev *dev = (struct rfcomm_dev *) tty->driver_data; + int room = 0; + + if (dev && dev->dlc) + room = rfcomm_room(dev); + + BT_DBG("tty %p room %d", tty, room); + + return room; +} + +static int rfcomm_tty_ioctl(struct tty_struct *tty, unsigned int cmd, unsigned long arg) +{ + BT_DBG("tty %p cmd 0x%02x", tty, cmd); + + switch (cmd) { + case TCGETS: + BT_DBG("TCGETS is not supported"); + return -ENOIOCTLCMD; + + case TCSETS: + BT_DBG("TCSETS is not supported"); + return -ENOIOCTLCMD; + + case TIOCMIWAIT: + BT_DBG("TIOCMIWAIT"); + break; + + case TIOCGSERIAL: + BT_ERR("TIOCGSERIAL is not supported"); + return -ENOIOCTLCMD; + + case TIOCSSERIAL: + BT_ERR("TIOCSSERIAL is not supported"); + return -ENOIOCTLCMD; + + case TIOCSERGSTRUCT: + BT_ERR("TIOCSERGSTRUCT is not supported"); + return -ENOIOCTLCMD; + + case TIOCSERGETLSR: + BT_ERR("TIOCSERGETLSR is not supported"); + return -ENOIOCTLCMD; + + case TIOCSERCONFIG: + BT_ERR("TIOCSERCONFIG is not supported"); + return -ENOIOCTLCMD; + + default: + return -ENOIOCTLCMD; /* ioctls which we must ignore */ + + } + + return -ENOIOCTLCMD; +} + +static void rfcomm_tty_set_termios(struct tty_struct *tty, struct ktermios *old) +{ + struct ktermios *new = &tty->termios; + int old_baud_rate = tty_termios_baud_rate(old); + int new_baud_rate = tty_termios_baud_rate(new); + + u8 baud, data_bits, stop_bits, parity, x_on, x_off; + u16 changes = 0; + + struct rfcomm_dev *dev = (struct rfcomm_dev *) tty->driver_data; + + BT_DBG("tty %p termios %p", tty, old); + + if (!dev || !dev->dlc || !dev->dlc->session) + return; + + /* Handle turning off CRTSCTS */ + if ((old->c_cflag & CRTSCTS) && !(new->c_cflag & CRTSCTS)) + BT_DBG("Turning off CRTSCTS unsupported"); + + /* Parity on/off and when on, odd/even */ + if (((old->c_cflag & PARENB) != (new->c_cflag & PARENB)) || + ((old->c_cflag & PARODD) != (new->c_cflag & PARODD))) { + changes |= RFCOMM_RPN_PM_PARITY; + BT_DBG("Parity change detected."); + } + + /* Mark and space parity are not supported! */ + if (new->c_cflag & PARENB) { + if (new->c_cflag & PARODD) { + BT_DBG("Parity is ODD"); + parity = RFCOMM_RPN_PARITY_ODD; + } else { + BT_DBG("Parity is EVEN"); + parity = RFCOMM_RPN_PARITY_EVEN; + } + } else { + BT_DBG("Parity is OFF"); + parity = RFCOMM_RPN_PARITY_NONE; + } + + /* Setting the x_on / x_off characters */ + if (old->c_cc[VSTOP] != new->c_cc[VSTOP]) { + BT_DBG("XOFF custom"); + x_on = new->c_cc[VSTOP]; + changes |= RFCOMM_RPN_PM_XON; + } else { + BT_DBG("XOFF default"); + x_on = RFCOMM_RPN_XON_CHAR; + } + + if (old->c_cc[VSTART] != new->c_cc[VSTART]) { + BT_DBG("XON custom"); + x_off = new->c_cc[VSTART]; + changes |= RFCOMM_RPN_PM_XOFF; + } else { + BT_DBG("XON default"); + x_off = RFCOMM_RPN_XOFF_CHAR; + } + + /* Handle setting of stop bits */ + if ((old->c_cflag & CSTOPB) != (new->c_cflag & CSTOPB)) + changes |= RFCOMM_RPN_PM_STOP; + + /* POSIX does not support 1.5 stop bits and RFCOMM does not + * support 2 stop bits. So a request for 2 stop bits gets + * translated to 1.5 stop bits */ + if (new->c_cflag & CSTOPB) + stop_bits = RFCOMM_RPN_STOP_15; + else + stop_bits = RFCOMM_RPN_STOP_1; + + /* Handle number of data bits [5-8] */ + if ((old->c_cflag & CSIZE) != (new->c_cflag & CSIZE)) + changes |= RFCOMM_RPN_PM_DATA; + + switch (new->c_cflag & CSIZE) { + case CS5: + data_bits = RFCOMM_RPN_DATA_5; + break; + case CS6: + data_bits = RFCOMM_RPN_DATA_6; + break; + case CS7: + data_bits = RFCOMM_RPN_DATA_7; + break; + case CS8: + data_bits = RFCOMM_RPN_DATA_8; + break; + default: + data_bits = RFCOMM_RPN_DATA_8; + break; + } + + /* Handle baudrate settings */ + if (old_baud_rate != new_baud_rate) + changes |= RFCOMM_RPN_PM_BITRATE; + + switch (new_baud_rate) { + case 2400: + baud = RFCOMM_RPN_BR_2400; + break; + case 4800: + baud = RFCOMM_RPN_BR_4800; + break; + case 7200: + baud = RFCOMM_RPN_BR_7200; + break; + case 9600: + baud = RFCOMM_RPN_BR_9600; + break; + case 19200: + baud = RFCOMM_RPN_BR_19200; + break; + case 38400: + baud = RFCOMM_RPN_BR_38400; + break; + case 57600: + baud = RFCOMM_RPN_BR_57600; + break; + case 115200: + baud = RFCOMM_RPN_BR_115200; + break; + case 230400: + baud = RFCOMM_RPN_BR_230400; + break; + default: + /* 9600 is standard accordinag to the RFCOMM specification */ + baud = RFCOMM_RPN_BR_9600; + break; + + } + + if (changes) + rfcomm_send_rpn(dev->dlc->session, 1, dev->dlc->dlci, baud, + data_bits, stop_bits, parity, + RFCOMM_RPN_FLOW_NONE, x_on, x_off, changes); +} + +static void rfcomm_tty_throttle(struct tty_struct *tty) +{ + struct rfcomm_dev *dev = (struct rfcomm_dev *) tty->driver_data; + + BT_DBG("tty %p dev %p", tty, dev); + + rfcomm_dlc_throttle(dev->dlc); +} + +static void rfcomm_tty_unthrottle(struct tty_struct *tty) +{ + struct rfcomm_dev *dev = (struct rfcomm_dev *) tty->driver_data; + + BT_DBG("tty %p dev %p", tty, dev); + + rfcomm_dlc_unthrottle(dev->dlc); +} + +static int rfcomm_tty_chars_in_buffer(struct tty_struct *tty) +{ + struct rfcomm_dev *dev = (struct rfcomm_dev *) tty->driver_data; + + BT_DBG("tty %p dev %p", tty, dev); + + if (!dev || !dev->dlc) + return 0; + + if (!skb_queue_empty(&dev->dlc->tx_queue)) + return dev->dlc->mtu; + + return 0; +} + +static void rfcomm_tty_flush_buffer(struct tty_struct *tty) +{ + struct rfcomm_dev *dev = (struct rfcomm_dev *) tty->driver_data; + + BT_DBG("tty %p dev %p", tty, dev); + + if (!dev || !dev->dlc) + return; + + skb_queue_purge(&dev->dlc->tx_queue); + tty_wakeup(tty); +} + +static void rfcomm_tty_send_xchar(struct tty_struct *tty, char ch) +{ + BT_DBG("tty %p ch %c", tty, ch); +} + +static void rfcomm_tty_wait_until_sent(struct tty_struct *tty, int timeout) +{ + BT_DBG("tty %p timeout %d", tty, timeout); +} + +static void rfcomm_tty_hangup(struct tty_struct *tty) +{ + struct rfcomm_dev *dev = (struct rfcomm_dev *) tty->driver_data; + + BT_DBG("tty %p dev %p", tty, dev); + + tty_port_hangup(&dev->port); +} + +static int rfcomm_tty_tiocmget(struct tty_struct *tty) +{ + struct rfcomm_dev *dev = (struct rfcomm_dev *) tty->driver_data; + + BT_DBG("tty %p dev %p", tty, dev); + + return dev->modem_status; +} + +static int rfcomm_tty_tiocmset(struct tty_struct *tty, unsigned int set, unsigned int clear) +{ + struct rfcomm_dev *dev = (struct rfcomm_dev *) tty->driver_data; + struct rfcomm_dlc *dlc = dev->dlc; + u8 v24_sig; + + BT_DBG("tty %p dev %p set 0x%02x clear 0x%02x", tty, dev, set, clear); + + rfcomm_dlc_get_modem_status(dlc, &v24_sig); + + if (set & TIOCM_DSR || set & TIOCM_DTR) + v24_sig |= RFCOMM_V24_RTC; + if (set & TIOCM_RTS || set & TIOCM_CTS) + v24_sig |= RFCOMM_V24_RTR; + if (set & TIOCM_RI) + v24_sig |= RFCOMM_V24_IC; + if (set & TIOCM_CD) + v24_sig |= RFCOMM_V24_DV; + + if (clear & TIOCM_DSR || clear & TIOCM_DTR) + v24_sig &= ~RFCOMM_V24_RTC; + if (clear & TIOCM_RTS || clear & TIOCM_CTS) + v24_sig &= ~RFCOMM_V24_RTR; + if (clear & TIOCM_RI) + v24_sig &= ~RFCOMM_V24_IC; + if (clear & TIOCM_CD) + v24_sig &= ~RFCOMM_V24_DV; + + rfcomm_dlc_set_modem_status(dlc, v24_sig); + + return 0; +} + +/* ---- TTY structure ---- */ + +static const struct tty_operations rfcomm_ops = { + .open = rfcomm_tty_open, + .close = rfcomm_tty_close, + .write = rfcomm_tty_write, + .write_room = rfcomm_tty_write_room, + .chars_in_buffer = rfcomm_tty_chars_in_buffer, + .flush_buffer = rfcomm_tty_flush_buffer, + .ioctl = rfcomm_tty_ioctl, + .throttle = rfcomm_tty_throttle, + .unthrottle = rfcomm_tty_unthrottle, + .set_termios = rfcomm_tty_set_termios, + .send_xchar = rfcomm_tty_send_xchar, + .hangup = rfcomm_tty_hangup, + .wait_until_sent = rfcomm_tty_wait_until_sent, + .tiocmget = rfcomm_tty_tiocmget, + .tiocmset = rfcomm_tty_tiocmset, + .install = rfcomm_tty_install, + .cleanup = rfcomm_tty_cleanup, +}; + +int __init rfcomm_init_ttys(void) +{ + int error; + + rfcomm_tty_driver = alloc_tty_driver(RFCOMM_TTY_PORTS); + if (!rfcomm_tty_driver) + return -ENOMEM; + + rfcomm_tty_driver->driver_name = "rfcomm"; + rfcomm_tty_driver->name = "rfcomm"; + rfcomm_tty_driver->major = RFCOMM_TTY_MAJOR; + rfcomm_tty_driver->minor_start = RFCOMM_TTY_MINOR; + rfcomm_tty_driver->type = TTY_DRIVER_TYPE_SERIAL; + 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_lflag &= ~ICANON; + tty_set_operations(rfcomm_tty_driver, &rfcomm_ops); + + error = tty_register_driver(rfcomm_tty_driver); + if (error) { + BT_ERR("Can't register RFCOMM TTY driver"); + put_tty_driver(rfcomm_tty_driver); + return error; + } + + BT_INFO("RFCOMM TTY layer initialized"); + + return 0; +} + +void rfcomm_cleanup_ttys(void) +{ + tty_unregister_driver(rfcomm_tty_driver); + put_tty_driver(rfcomm_tty_driver); +} diff --git a/kernel/net/bluetooth/sco.c b/kernel/net/bluetooth/sco.c new file mode 100644 index 000000000..4322c833e --- /dev/null +++ b/kernel/net/bluetooth/sco.c @@ -0,0 +1,1248 @@ +/* + BlueZ - Bluetooth protocol stack for Linux + Copyright (C) 2000-2001 Qualcomm Incorporated + + Written 2000,2001 by Maxim Krasnyansky <maxk@qualcomm.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; + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS + OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT OF THIRD PARTY RIGHTS. + IN NO EVENT SHALL THE COPYRIGHT HOLDER(S) AND AUTHOR(S) BE LIABLE FOR ANY + CLAIM, OR ANY SPECIAL INDIRECT OR CONSEQUENTIAL DAMAGES, OR ANY DAMAGES + WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN + ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF + OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + + ALL LIABILITY, INCLUDING LIABILITY FOR INFRINGEMENT OF ANY PATENTS, + COPYRIGHTS, TRADEMARKS OR OTHER RIGHTS, RELATING TO USE OF THIS + SOFTWARE IS DISCLAIMED. +*/ + +/* Bluetooth SCO sockets. */ + +#include <linux/module.h> +#include <linux/debugfs.h> +#include <linux/seq_file.h> + +#include <net/bluetooth/bluetooth.h> +#include <net/bluetooth/hci_core.h> +#include <net/bluetooth/sco.h> + +static bool disable_esco; + +static const struct proto_ops sco_sock_ops; + +static struct bt_sock_list sco_sk_list = { + .lock = __RW_LOCK_UNLOCKED(sco_sk_list.lock) +}; + +/* ---- SCO connections ---- */ +struct sco_conn { + struct hci_conn *hcon; + + spinlock_t lock; + struct sock *sk; + + unsigned int mtu; +}; + +#define sco_conn_lock(c) spin_lock(&c->lock); +#define sco_conn_unlock(c) spin_unlock(&c->lock); + +static void sco_sock_close(struct sock *sk); +static void sco_sock_kill(struct sock *sk); + +/* ----- SCO socket info ----- */ +#define sco_pi(sk) ((struct sco_pinfo *) sk) + +struct sco_pinfo { + struct bt_sock bt; + bdaddr_t src; + bdaddr_t dst; + __u32 flags; + __u16 setting; + struct sco_conn *conn; +}; + +/* ---- SCO timers ---- */ +#define SCO_CONN_TIMEOUT (HZ * 40) +#define SCO_DISCONN_TIMEOUT (HZ * 2) + +static void sco_sock_timeout(unsigned long arg) +{ + struct sock *sk = (struct sock *) arg; + + BT_DBG("sock %p state %d", sk, sk->sk_state); + + bh_lock_sock(sk); + sk->sk_err = ETIMEDOUT; + sk->sk_state_change(sk); + bh_unlock_sock(sk); + + sco_sock_kill(sk); + sock_put(sk); +} + +static void sco_sock_set_timer(struct sock *sk, long timeout) +{ + BT_DBG("sock %p state %d timeout %ld", sk, sk->sk_state, timeout); + sk_reset_timer(sk, &sk->sk_timer, jiffies + timeout); +} + +static void sco_sock_clear_timer(struct sock *sk) +{ + BT_DBG("sock %p state %d", sk, sk->sk_state); + sk_stop_timer(sk, &sk->sk_timer); +} + +/* ---- SCO connections ---- */ +static struct sco_conn *sco_conn_add(struct hci_conn *hcon) +{ + struct hci_dev *hdev = hcon->hdev; + struct sco_conn *conn = hcon->sco_data; + + if (conn) + return conn; + + conn = kzalloc(sizeof(struct sco_conn), GFP_KERNEL); + if (!conn) + return NULL; + + spin_lock_init(&conn->lock); + + hcon->sco_data = conn; + conn->hcon = hcon; + + if (hdev->sco_mtu > 0) + conn->mtu = hdev->sco_mtu; + else + conn->mtu = 60; + + BT_DBG("hcon %p conn %p", hcon, conn); + + return conn; +} + +/* Delete channel. + * Must be called on the locked socket. */ +static void sco_chan_del(struct sock *sk, int err) +{ + struct sco_conn *conn; + + conn = sco_pi(sk)->conn; + + BT_DBG("sk %p, conn %p, err %d", sk, conn, err); + + if (conn) { + sco_conn_lock(conn); + conn->sk = NULL; + sco_pi(sk)->conn = NULL; + sco_conn_unlock(conn); + + if (conn->hcon) + hci_conn_drop(conn->hcon); + } + + sk->sk_state = BT_CLOSED; + sk->sk_err = err; + sk->sk_state_change(sk); + + sock_set_flag(sk, SOCK_ZAPPED); +} + +static int sco_conn_del(struct hci_conn *hcon, int err) +{ + struct sco_conn *conn = hcon->sco_data; + struct sock *sk; + + if (!conn) + return 0; + + BT_DBG("hcon %p conn %p, err %d", hcon, conn, err); + + /* Kill socket */ + sco_conn_lock(conn); + sk = conn->sk; + sco_conn_unlock(conn); + + if (sk) { + bh_lock_sock(sk); + sco_sock_clear_timer(sk); + sco_chan_del(sk, err); + bh_unlock_sock(sk); + sco_sock_kill(sk); + } + + hcon->sco_data = NULL; + kfree(conn); + return 0; +} + +static void __sco_chan_add(struct sco_conn *conn, struct sock *sk, struct sock *parent) +{ + BT_DBG("conn %p", conn); + + sco_pi(sk)->conn = conn; + conn->sk = sk; + + if (parent) + bt_accept_enqueue(parent, sk); +} + +static int sco_chan_add(struct sco_conn *conn, struct sock *sk, + struct sock *parent) +{ + int err = 0; + + sco_conn_lock(conn); + if (conn->sk) + err = -EBUSY; + else + __sco_chan_add(conn, sk, parent); + + sco_conn_unlock(conn); + return err; +} + +static int sco_connect(struct sock *sk) +{ + struct sco_conn *conn; + struct hci_conn *hcon; + struct hci_dev *hdev; + int err, type; + + BT_DBG("%pMR -> %pMR", &sco_pi(sk)->src, &sco_pi(sk)->dst); + + hdev = hci_get_route(&sco_pi(sk)->dst, &sco_pi(sk)->src); + if (!hdev) + return -EHOSTUNREACH; + + hci_dev_lock(hdev); + + if (lmp_esco_capable(hdev) && !disable_esco) + type = ESCO_LINK; + else + type = SCO_LINK; + + if (sco_pi(sk)->setting == BT_VOICE_TRANSPARENT && + (!lmp_transp_capable(hdev) || !lmp_esco_capable(hdev))) { + err = -EOPNOTSUPP; + goto done; + } + + hcon = hci_connect_sco(hdev, type, &sco_pi(sk)->dst, + sco_pi(sk)->setting); + if (IS_ERR(hcon)) { + err = PTR_ERR(hcon); + goto done; + } + + conn = sco_conn_add(hcon); + if (!conn) { + hci_conn_drop(hcon); + err = -ENOMEM; + goto done; + } + + /* Update source addr of the socket */ + bacpy(&sco_pi(sk)->src, &hcon->src); + + err = sco_chan_add(conn, sk, NULL); + if (err) + goto done; + + if (hcon->state == BT_CONNECTED) { + sco_sock_clear_timer(sk); + sk->sk_state = BT_CONNECTED; + } else { + sk->sk_state = BT_CONNECT; + sco_sock_set_timer(sk, sk->sk_sndtimeo); + } + +done: + hci_dev_unlock(hdev); + hci_dev_put(hdev); + return err; +} + +static int sco_send_frame(struct sock *sk, struct msghdr *msg, int len) +{ + struct sco_conn *conn = sco_pi(sk)->conn; + struct sk_buff *skb; + int err; + + /* Check outgoing MTU */ + if (len > conn->mtu) + return -EINVAL; + + BT_DBG("sk %p len %d", sk, len); + + skb = bt_skb_send_alloc(sk, len, msg->msg_flags & MSG_DONTWAIT, &err); + if (!skb) + return err; + + if (memcpy_from_msg(skb_put(skb, len), msg, len)) { + kfree_skb(skb); + return -EFAULT; + } + + hci_send_sco(conn->hcon, skb); + + return len; +} + +static void sco_recv_frame(struct sco_conn *conn, struct sk_buff *skb) +{ + struct sock *sk; + + sco_conn_lock(conn); + sk = conn->sk; + sco_conn_unlock(conn); + + if (!sk) + goto drop; + + BT_DBG("sk %p len %d", sk, skb->len); + + if (sk->sk_state != BT_CONNECTED) + goto drop; + + if (!sock_queue_rcv_skb(sk, skb)) + return; + +drop: + kfree_skb(skb); +} + +/* -------- Socket interface ---------- */ +static struct sock *__sco_get_sock_listen_by_addr(bdaddr_t *ba) +{ + struct sock *sk; + + sk_for_each(sk, &sco_sk_list.head) { + if (sk->sk_state != BT_LISTEN) + continue; + + if (!bacmp(&sco_pi(sk)->src, ba)) + return sk; + } + + return NULL; +} + +/* Find socket listening on source bdaddr. + * Returns closest match. + */ +static struct sock *sco_get_sock_listen(bdaddr_t *src) +{ + struct sock *sk = NULL, *sk1 = NULL; + + read_lock(&sco_sk_list.lock); + + sk_for_each(sk, &sco_sk_list.head) { + if (sk->sk_state != BT_LISTEN) + continue; + + /* Exact match. */ + if (!bacmp(&sco_pi(sk)->src, src)) + break; + + /* Closest match */ + if (!bacmp(&sco_pi(sk)->src, BDADDR_ANY)) + sk1 = sk; + } + + read_unlock(&sco_sk_list.lock); + + return sk ? sk : sk1; +} + +static void sco_sock_destruct(struct sock *sk) +{ + BT_DBG("sk %p", sk); + + skb_queue_purge(&sk->sk_receive_queue); + skb_queue_purge(&sk->sk_write_queue); +} + +static void sco_sock_cleanup_listen(struct sock *parent) +{ + struct sock *sk; + + BT_DBG("parent %p", parent); + + /* Close not yet accepted channels */ + while ((sk = bt_accept_dequeue(parent, NULL))) { + sco_sock_close(sk); + sco_sock_kill(sk); + } + + parent->sk_state = BT_CLOSED; + sock_set_flag(parent, SOCK_ZAPPED); +} + +/* Kill socket (only if zapped and orphan) + * Must be called on unlocked socket. + */ +static void sco_sock_kill(struct sock *sk) +{ + if (!sock_flag(sk, SOCK_ZAPPED) || sk->sk_socket) + return; + + BT_DBG("sk %p state %d", sk, sk->sk_state); + + /* Kill poor orphan */ + bt_sock_unlink(&sco_sk_list, sk); + sock_set_flag(sk, SOCK_DEAD); + sock_put(sk); +} + +static void __sco_sock_close(struct sock *sk) +{ + BT_DBG("sk %p state %d socket %p", sk, sk->sk_state, sk->sk_socket); + + switch (sk->sk_state) { + case BT_LISTEN: + sco_sock_cleanup_listen(sk); + break; + + case BT_CONNECTED: + case BT_CONFIG: + if (sco_pi(sk)->conn->hcon) { + sk->sk_state = BT_DISCONN; + sco_sock_set_timer(sk, SCO_DISCONN_TIMEOUT); + hci_conn_drop(sco_pi(sk)->conn->hcon); + sco_pi(sk)->conn->hcon = NULL; + } else + sco_chan_del(sk, ECONNRESET); + break; + + case BT_CONNECT2: + case BT_CONNECT: + case BT_DISCONN: + sco_chan_del(sk, ECONNRESET); + break; + + default: + sock_set_flag(sk, SOCK_ZAPPED); + break; + } +} + +/* Must be called on unlocked socket. */ +static void sco_sock_close(struct sock *sk) +{ + sco_sock_clear_timer(sk); + lock_sock(sk); + __sco_sock_close(sk); + release_sock(sk); + sco_sock_kill(sk); +} + +static void sco_sock_init(struct sock *sk, struct sock *parent) +{ + BT_DBG("sk %p", sk); + + if (parent) { + sk->sk_type = parent->sk_type; + bt_sk(sk)->flags = bt_sk(parent)->flags; + security_sk_clone(parent, sk); + } +} + +static struct proto sco_proto = { + .name = "SCO", + .owner = THIS_MODULE, + .obj_size = sizeof(struct sco_pinfo) +}; + +static struct sock *sco_sock_alloc(struct net *net, struct socket *sock, int proto, gfp_t prio) +{ + struct sock *sk; + + sk = sk_alloc(net, PF_BLUETOOTH, prio, &sco_proto); + if (!sk) + return NULL; + + sock_init_data(sock, sk); + INIT_LIST_HEAD(&bt_sk(sk)->accept_q); + + sk->sk_destruct = sco_sock_destruct; + sk->sk_sndtimeo = SCO_CONN_TIMEOUT; + + sock_reset_flag(sk, SOCK_ZAPPED); + + sk->sk_protocol = proto; + sk->sk_state = BT_OPEN; + + sco_pi(sk)->setting = BT_VOICE_CVSD_16BIT; + + setup_timer(&sk->sk_timer, sco_sock_timeout, (unsigned long)sk); + + bt_sock_link(&sco_sk_list, sk); + return sk; +} + +static int sco_sock_create(struct net *net, struct socket *sock, int protocol, + int kern) +{ + struct sock *sk; + + BT_DBG("sock %p", sock); + + sock->state = SS_UNCONNECTED; + + if (sock->type != SOCK_SEQPACKET) + return -ESOCKTNOSUPPORT; + + sock->ops = &sco_sock_ops; + + sk = sco_sock_alloc(net, sock, protocol, GFP_ATOMIC); + if (!sk) + return -ENOMEM; + + sco_sock_init(sk, NULL); + return 0; +} + +static int sco_sock_bind(struct socket *sock, struct sockaddr *addr, int addr_len) +{ + struct sockaddr_sco *sa = (struct sockaddr_sco *) addr; + struct sock *sk = sock->sk; + int err = 0; + + BT_DBG("sk %p %pMR", sk, &sa->sco_bdaddr); + + if (!addr || addr->sa_family != AF_BLUETOOTH) + return -EINVAL; + + lock_sock(sk); + + if (sk->sk_state != BT_OPEN) { + err = -EBADFD; + goto done; + } + + if (sk->sk_type != SOCK_SEQPACKET) { + err = -EINVAL; + goto done; + } + + bacpy(&sco_pi(sk)->src, &sa->sco_bdaddr); + + sk->sk_state = BT_BOUND; + +done: + release_sock(sk); + return err; +} + +static int sco_sock_connect(struct socket *sock, struct sockaddr *addr, int alen, int flags) +{ + struct sockaddr_sco *sa = (struct sockaddr_sco *) addr; + struct sock *sk = sock->sk; + int err; + + BT_DBG("sk %p", sk); + + if (alen < sizeof(struct sockaddr_sco) || + addr->sa_family != AF_BLUETOOTH) + return -EINVAL; + + if (sk->sk_state != BT_OPEN && sk->sk_state != BT_BOUND) + return -EBADFD; + + if (sk->sk_type != SOCK_SEQPACKET) + return -EINVAL; + + lock_sock(sk); + + /* Set destination address and psm */ + bacpy(&sco_pi(sk)->dst, &sa->sco_bdaddr); + + err = sco_connect(sk); + if (err) + goto done; + + err = bt_sock_wait_state(sk, BT_CONNECTED, + sock_sndtimeo(sk, flags & O_NONBLOCK)); + +done: + release_sock(sk); + return err; +} + +static int sco_sock_listen(struct socket *sock, int backlog) +{ + struct sock *sk = sock->sk; + bdaddr_t *src = &sco_pi(sk)->src; + int err = 0; + + BT_DBG("sk %p backlog %d", sk, backlog); + + lock_sock(sk); + + if (sk->sk_state != BT_BOUND) { + err = -EBADFD; + goto done; + } + + if (sk->sk_type != SOCK_SEQPACKET) { + err = -EINVAL; + goto done; + } + + write_lock(&sco_sk_list.lock); + + if (__sco_get_sock_listen_by_addr(src)) { + err = -EADDRINUSE; + goto unlock; + } + + sk->sk_max_ack_backlog = backlog; + sk->sk_ack_backlog = 0; + + sk->sk_state = BT_LISTEN; + +unlock: + write_unlock(&sco_sk_list.lock); + +done: + release_sock(sk); + return err; +} + +static int sco_sock_accept(struct socket *sock, struct socket *newsock, int flags) +{ + DEFINE_WAIT_FUNC(wait, woken_wake_function); + struct sock *sk = sock->sk, *ch; + long timeo; + int err = 0; + + lock_sock(sk); + + timeo = sock_rcvtimeo(sk, flags & O_NONBLOCK); + + BT_DBG("sk %p timeo %ld", sk, timeo); + + /* Wait for an incoming connection. (wake-one). */ + add_wait_queue_exclusive(sk_sleep(sk), &wait); + while (1) { + if (sk->sk_state != BT_LISTEN) { + err = -EBADFD; + break; + } + + ch = bt_accept_dequeue(sk, newsock); + if (ch) + break; + + if (!timeo) { + err = -EAGAIN; + break; + } + + if (signal_pending(current)) { + err = sock_intr_errno(timeo); + break; + } + + release_sock(sk); + + timeo = wait_woken(&wait, TASK_INTERRUPTIBLE, timeo); + lock_sock(sk); + } + remove_wait_queue(sk_sleep(sk), &wait); + + if (err) + goto done; + + newsock->state = SS_CONNECTED; + + BT_DBG("new socket %p", ch); + +done: + release_sock(sk); + return err; +} + +static int sco_sock_getname(struct socket *sock, struct sockaddr *addr, int *len, int peer) +{ + struct sockaddr_sco *sa = (struct sockaddr_sco *) addr; + struct sock *sk = sock->sk; + + BT_DBG("sock %p, sk %p", sock, sk); + + addr->sa_family = AF_BLUETOOTH; + *len = sizeof(struct sockaddr_sco); + + if (peer) + bacpy(&sa->sco_bdaddr, &sco_pi(sk)->dst); + else + bacpy(&sa->sco_bdaddr, &sco_pi(sk)->src); + + return 0; +} + +static int sco_sock_sendmsg(struct socket *sock, struct msghdr *msg, + size_t len) +{ + struct sock *sk = sock->sk; + int err; + + BT_DBG("sock %p, sk %p", sock, sk); + + err = sock_error(sk); + if (err) + return err; + + if (msg->msg_flags & MSG_OOB) + return -EOPNOTSUPP; + + lock_sock(sk); + + if (sk->sk_state == BT_CONNECTED) + err = sco_send_frame(sk, msg, len); + else + err = -ENOTCONN; + + release_sock(sk); + return err; +} + +static void sco_conn_defer_accept(struct hci_conn *conn, u16 setting) +{ + struct hci_dev *hdev = conn->hdev; + + BT_DBG("conn %p", conn); + + conn->state = BT_CONFIG; + + if (!lmp_esco_capable(hdev)) { + struct hci_cp_accept_conn_req cp; + + bacpy(&cp.bdaddr, &conn->dst); + cp.role = 0x00; /* Ignored */ + + hci_send_cmd(hdev, HCI_OP_ACCEPT_CONN_REQ, sizeof(cp), &cp); + } else { + struct hci_cp_accept_sync_conn_req cp; + + bacpy(&cp.bdaddr, &conn->dst); + cp.pkt_type = cpu_to_le16(conn->pkt_type); + + cp.tx_bandwidth = cpu_to_le32(0x00001f40); + cp.rx_bandwidth = cpu_to_le32(0x00001f40); + cp.content_format = cpu_to_le16(setting); + + switch (setting & SCO_AIRMODE_MASK) { + case SCO_AIRMODE_TRANSP: + if (conn->pkt_type & ESCO_2EV3) + cp.max_latency = cpu_to_le16(0x0008); + else + cp.max_latency = cpu_to_le16(0x000D); + cp.retrans_effort = 0x02; + break; + case SCO_AIRMODE_CVSD: + cp.max_latency = cpu_to_le16(0xffff); + cp.retrans_effort = 0xff; + break; + } + + hci_send_cmd(hdev, HCI_OP_ACCEPT_SYNC_CONN_REQ, + sizeof(cp), &cp); + } +} + +static int sco_sock_recvmsg(struct socket *sock, struct msghdr *msg, + size_t len, int flags) +{ + struct sock *sk = sock->sk; + struct sco_pinfo *pi = sco_pi(sk); + + lock_sock(sk); + + if (sk->sk_state == BT_CONNECT2 && + test_bit(BT_SK_DEFER_SETUP, &bt_sk(sk)->flags)) { + sco_conn_defer_accept(pi->conn->hcon, pi->setting); + sk->sk_state = BT_CONFIG; + + release_sock(sk); + return 0; + } + + release_sock(sk); + + return bt_sock_recvmsg(sock, msg, len, flags); +} + +static int sco_sock_setsockopt(struct socket *sock, int level, int optname, char __user *optval, unsigned int optlen) +{ + struct sock *sk = sock->sk; + int len, err = 0; + struct bt_voice voice; + u32 opt; + + BT_DBG("sk %p", sk); + + lock_sock(sk); + + switch (optname) { + + case BT_DEFER_SETUP: + if (sk->sk_state != BT_BOUND && sk->sk_state != BT_LISTEN) { + err = -EINVAL; + break; + } + + if (get_user(opt, (u32 __user *) optval)) { + err = -EFAULT; + break; + } + + if (opt) + set_bit(BT_SK_DEFER_SETUP, &bt_sk(sk)->flags); + else + clear_bit(BT_SK_DEFER_SETUP, &bt_sk(sk)->flags); + break; + + case BT_VOICE: + if (sk->sk_state != BT_OPEN && sk->sk_state != BT_BOUND && + sk->sk_state != BT_CONNECT2) { + err = -EINVAL; + break; + } + + voice.setting = sco_pi(sk)->setting; + + len = min_t(unsigned int, sizeof(voice), optlen); + if (copy_from_user((char *) &voice, optval, len)) { + err = -EFAULT; + break; + } + + /* Explicitly check for these values */ + if (voice.setting != BT_VOICE_TRANSPARENT && + voice.setting != BT_VOICE_CVSD_16BIT) { + err = -EINVAL; + break; + } + + sco_pi(sk)->setting = voice.setting; + break; + + default: + err = -ENOPROTOOPT; + break; + } + + release_sock(sk); + return err; +} + +static int sco_sock_getsockopt_old(struct socket *sock, int optname, char __user *optval, int __user *optlen) +{ + struct sock *sk = sock->sk; + struct sco_options opts; + struct sco_conninfo cinfo; + int len, err = 0; + + BT_DBG("sk %p", sk); + + if (get_user(len, optlen)) + return -EFAULT; + + lock_sock(sk); + + switch (optname) { + case SCO_OPTIONS: + if (sk->sk_state != BT_CONNECTED && + !(sk->sk_state == BT_CONNECT2 && + test_bit(BT_SK_DEFER_SETUP, &bt_sk(sk)->flags))) { + err = -ENOTCONN; + break; + } + + opts.mtu = sco_pi(sk)->conn->mtu; + + BT_DBG("mtu %d", opts.mtu); + + len = min_t(unsigned int, len, sizeof(opts)); + if (copy_to_user(optval, (char *)&opts, len)) + err = -EFAULT; + + break; + + case SCO_CONNINFO: + if (sk->sk_state != BT_CONNECTED && + !(sk->sk_state == BT_CONNECT2 && + test_bit(BT_SK_DEFER_SETUP, &bt_sk(sk)->flags))) { + err = -ENOTCONN; + break; + } + + memset(&cinfo, 0, sizeof(cinfo)); + cinfo.hci_handle = sco_pi(sk)->conn->hcon->handle; + memcpy(cinfo.dev_class, sco_pi(sk)->conn->hcon->dev_class, 3); + + len = min_t(unsigned int, len, sizeof(cinfo)); + if (copy_to_user(optval, (char *)&cinfo, len)) + err = -EFAULT; + + break; + + default: + err = -ENOPROTOOPT; + break; + } + + release_sock(sk); + return err; +} + +static int sco_sock_getsockopt(struct socket *sock, int level, int optname, char __user *optval, int __user *optlen) +{ + struct sock *sk = sock->sk; + int len, err = 0; + struct bt_voice voice; + + BT_DBG("sk %p", sk); + + if (level == SOL_SCO) + return sco_sock_getsockopt_old(sock, optname, optval, optlen); + + if (get_user(len, optlen)) + return -EFAULT; + + lock_sock(sk); + + switch (optname) { + + case BT_DEFER_SETUP: + if (sk->sk_state != BT_BOUND && sk->sk_state != BT_LISTEN) { + err = -EINVAL; + break; + } + + if (put_user(test_bit(BT_SK_DEFER_SETUP, &bt_sk(sk)->flags), + (u32 __user *) optval)) + err = -EFAULT; + + break; + + case BT_VOICE: + voice.setting = sco_pi(sk)->setting; + + len = min_t(unsigned int, len, sizeof(voice)); + if (copy_to_user(optval, (char *)&voice, len)) + err = -EFAULT; + + break; + + default: + err = -ENOPROTOOPT; + break; + } + + release_sock(sk); + return err; +} + +static int sco_sock_shutdown(struct socket *sock, int how) +{ + struct sock *sk = sock->sk; + int err = 0; + + BT_DBG("sock %p, sk %p", sock, sk); + + if (!sk) + return 0; + + lock_sock(sk); + if (!sk->sk_shutdown) { + sk->sk_shutdown = SHUTDOWN_MASK; + sco_sock_clear_timer(sk); + __sco_sock_close(sk); + + if (sock_flag(sk, SOCK_LINGER) && sk->sk_lingertime && + !(current->flags & PF_EXITING)) + err = bt_sock_wait_state(sk, BT_CLOSED, + sk->sk_lingertime); + } + release_sock(sk); + return err; +} + +static int sco_sock_release(struct socket *sock) +{ + struct sock *sk = sock->sk; + int err = 0; + + BT_DBG("sock %p, sk %p", sock, sk); + + if (!sk) + return 0; + + sco_sock_close(sk); + + if (sock_flag(sk, SOCK_LINGER) && sk->sk_lingertime && + !(current->flags & PF_EXITING)) { + lock_sock(sk); + err = bt_sock_wait_state(sk, BT_CLOSED, sk->sk_lingertime); + release_sock(sk); + } + + sock_orphan(sk); + sco_sock_kill(sk); + return err; +} + +static void sco_conn_ready(struct sco_conn *conn) +{ + struct sock *parent; + struct sock *sk = conn->sk; + + BT_DBG("conn %p", conn); + + if (sk) { + sco_sock_clear_timer(sk); + bh_lock_sock(sk); + sk->sk_state = BT_CONNECTED; + sk->sk_state_change(sk); + bh_unlock_sock(sk); + } else { + sco_conn_lock(conn); + + parent = sco_get_sock_listen(&conn->hcon->src); + if (!parent) { + sco_conn_unlock(conn); + return; + } + + bh_lock_sock(parent); + + sk = sco_sock_alloc(sock_net(parent), NULL, + BTPROTO_SCO, GFP_ATOMIC); + if (!sk) { + bh_unlock_sock(parent); + sco_conn_unlock(conn); + return; + } + + sco_sock_init(sk, parent); + + bacpy(&sco_pi(sk)->src, &conn->hcon->src); + bacpy(&sco_pi(sk)->dst, &conn->hcon->dst); + + hci_conn_hold(conn->hcon); + __sco_chan_add(conn, sk, parent); + + if (test_bit(BT_SK_DEFER_SETUP, &bt_sk(parent)->flags)) + sk->sk_state = BT_CONNECT2; + else + sk->sk_state = BT_CONNECTED; + + /* Wake up parent */ + parent->sk_data_ready(parent); + + bh_unlock_sock(parent); + + sco_conn_unlock(conn); + } +} + +/* ----- SCO interface with lower layer (HCI) ----- */ +int sco_connect_ind(struct hci_dev *hdev, bdaddr_t *bdaddr, __u8 *flags) +{ + struct sock *sk; + int lm = 0; + + BT_DBG("hdev %s, bdaddr %pMR", hdev->name, bdaddr); + + /* Find listening sockets */ + read_lock(&sco_sk_list.lock); + sk_for_each(sk, &sco_sk_list.head) { + if (sk->sk_state != BT_LISTEN) + continue; + + if (!bacmp(&sco_pi(sk)->src, &hdev->bdaddr) || + !bacmp(&sco_pi(sk)->src, BDADDR_ANY)) { + lm |= HCI_LM_ACCEPT; + + if (test_bit(BT_SK_DEFER_SETUP, &bt_sk(sk)->flags)) + *flags |= HCI_PROTO_DEFER; + break; + } + } + read_unlock(&sco_sk_list.lock); + + return lm; +} + +static void sco_connect_cfm(struct hci_conn *hcon, __u8 status) +{ + if (hcon->type != SCO_LINK && hcon->type != ESCO_LINK) + return; + + BT_DBG("hcon %p bdaddr %pMR status %d", hcon, &hcon->dst, status); + + if (!status) { + struct sco_conn *conn; + + conn = sco_conn_add(hcon); + if (conn) + sco_conn_ready(conn); + } else + sco_conn_del(hcon, bt_to_errno(status)); +} + +static void sco_disconn_cfm(struct hci_conn *hcon, __u8 reason) +{ + if (hcon->type != SCO_LINK && hcon->type != ESCO_LINK) + return; + + BT_DBG("hcon %p reason %d", hcon, reason); + + sco_conn_del(hcon, bt_to_errno(reason)); +} + +int sco_recv_scodata(struct hci_conn *hcon, struct sk_buff *skb) +{ + struct sco_conn *conn = hcon->sco_data; + + if (!conn) + goto drop; + + BT_DBG("conn %p len %d", conn, skb->len); + + if (skb->len) { + sco_recv_frame(conn, skb); + return 0; + } + +drop: + kfree_skb(skb); + return 0; +} + +static struct hci_cb sco_cb = { + .name = "SCO", + .connect_cfm = sco_connect_cfm, + .disconn_cfm = sco_disconn_cfm, +}; + +static int sco_debugfs_show(struct seq_file *f, void *p) +{ + struct sock *sk; + + read_lock(&sco_sk_list.lock); + + sk_for_each(sk, &sco_sk_list.head) { + seq_printf(f, "%pMR %pMR %d\n", &sco_pi(sk)->src, + &sco_pi(sk)->dst, sk->sk_state); + } + + read_unlock(&sco_sk_list.lock); + + return 0; +} + +static int sco_debugfs_open(struct inode *inode, struct file *file) +{ + return single_open(file, sco_debugfs_show, inode->i_private); +} + +static const struct file_operations sco_debugfs_fops = { + .open = sco_debugfs_open, + .read = seq_read, + .llseek = seq_lseek, + .release = single_release, +}; + +static struct dentry *sco_debugfs; + +static const struct proto_ops sco_sock_ops = { + .family = PF_BLUETOOTH, + .owner = THIS_MODULE, + .release = sco_sock_release, + .bind = sco_sock_bind, + .connect = sco_sock_connect, + .listen = sco_sock_listen, + .accept = sco_sock_accept, + .getname = sco_sock_getname, + .sendmsg = sco_sock_sendmsg, + .recvmsg = sco_sock_recvmsg, + .poll = bt_sock_poll, + .ioctl = bt_sock_ioctl, + .mmap = sock_no_mmap, + .socketpair = sock_no_socketpair, + .shutdown = sco_sock_shutdown, + .setsockopt = sco_sock_setsockopt, + .getsockopt = sco_sock_getsockopt +}; + +static const struct net_proto_family sco_sock_family_ops = { + .family = PF_BLUETOOTH, + .owner = THIS_MODULE, + .create = sco_sock_create, +}; + +int __init sco_init(void) +{ + int err; + + BUILD_BUG_ON(sizeof(struct sockaddr_sco) > sizeof(struct sockaddr)); + + err = proto_register(&sco_proto, 0); + if (err < 0) + return err; + + err = bt_sock_register(BTPROTO_SCO, &sco_sock_family_ops); + if (err < 0) { + BT_ERR("SCO socket registration failed"); + goto error; + } + + err = bt_procfs_init(&init_net, "sco", &sco_sk_list, NULL); + if (err < 0) { + BT_ERR("Failed to create SCO proc file"); + bt_sock_unregister(BTPROTO_SCO); + goto error; + } + + BT_INFO("SCO socket layer initialized"); + + hci_register_cb(&sco_cb); + + if (IS_ERR_OR_NULL(bt_debugfs)) + return 0; + + sco_debugfs = debugfs_create_file("sco", 0444, bt_debugfs, + NULL, &sco_debugfs_fops); + + return 0; + +error: + proto_unregister(&sco_proto); + return err; +} + +void sco_exit(void) +{ + bt_procfs_cleanup(&init_net, "sco"); + + debugfs_remove(sco_debugfs); + + hci_unregister_cb(&sco_cb); + + bt_sock_unregister(BTPROTO_SCO); + + proto_unregister(&sco_proto); +} + +module_param(disable_esco, bool, 0644); +MODULE_PARM_DESC(disable_esco, "Disable eSCO connection creation"); diff --git a/kernel/net/bluetooth/selftest.c b/kernel/net/bluetooth/selftest.c new file mode 100644 index 000000000..dc688f13e --- /dev/null +++ b/kernel/net/bluetooth/selftest.c @@ -0,0 +1,271 @@ +/* + BlueZ - Bluetooth protocol stack for Linux + + Copyright (C) 2014 Intel Corporation + + 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; + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS + OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT OF THIRD PARTY RIGHTS. + IN NO EVENT SHALL THE COPYRIGHT HOLDER(S) AND AUTHOR(S) BE LIABLE FOR ANY + CLAIM, OR ANY SPECIAL INDIRECT OR CONSEQUENTIAL DAMAGES, OR ANY DAMAGES + WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN + ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF + OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + + ALL LIABILITY, INCLUDING LIABILITY FOR INFRINGEMENT OF ANY PATENTS, + COPYRIGHTS, TRADEMARKS OR OTHER RIGHTS, RELATING TO USE OF THIS + SOFTWARE IS DISCLAIMED. +*/ + +#include <linux/debugfs.h> + +#include <net/bluetooth/bluetooth.h> +#include <net/bluetooth/hci_core.h> + +#include "ecc.h" +#include "smp.h" +#include "selftest.h" + +#if IS_ENABLED(CONFIG_BT_SELFTEST_ECDH) + +static const u8 priv_a_1[32] __initconst = { + 0xbd, 0x1a, 0x3c, 0xcd, 0xa6, 0xb8, 0x99, 0x58, + 0x99, 0xb7, 0x40, 0xeb, 0x7b, 0x60, 0xff, 0x4a, + 0x50, 0x3f, 0x10, 0xd2, 0xe3, 0xb3, 0xc9, 0x74, + 0x38, 0x5f, 0xc5, 0xa3, 0xd4, 0xf6, 0x49, 0x3f, +}; +static const u8 priv_b_1[32] __initconst = { + 0xfd, 0xc5, 0x7f, 0xf4, 0x49, 0xdd, 0x4f, 0x6b, + 0xfb, 0x7c, 0x9d, 0xf1, 0xc2, 0x9a, 0xcb, 0x59, + 0x2a, 0xe7, 0xd4, 0xee, 0xfb, 0xfc, 0x0a, 0x90, + 0x9a, 0xbb, 0xf6, 0x32, 0x3d, 0x8b, 0x18, 0x55, +}; +static const u8 pub_a_1[64] __initconst = { + 0xe6, 0x9d, 0x35, 0x0e, 0x48, 0x01, 0x03, 0xcc, + 0xdb, 0xfd, 0xf4, 0xac, 0x11, 0x91, 0xf4, 0xef, + 0xb9, 0xa5, 0xf9, 0xe9, 0xa7, 0x83, 0x2c, 0x5e, + 0x2c, 0xbe, 0x97, 0xf2, 0xd2, 0x03, 0xb0, 0x20, + + 0x8b, 0xd2, 0x89, 0x15, 0xd0, 0x8e, 0x1c, 0x74, + 0x24, 0x30, 0xed, 0x8f, 0xc2, 0x45, 0x63, 0x76, + 0x5c, 0x15, 0x52, 0x5a, 0xbf, 0x9a, 0x32, 0x63, + 0x6d, 0xeb, 0x2a, 0x65, 0x49, 0x9c, 0x80, 0xdc, +}; +static const u8 pub_b_1[64] __initconst = { + 0x90, 0xa1, 0xaa, 0x2f, 0xb2, 0x77, 0x90, 0x55, + 0x9f, 0xa6, 0x15, 0x86, 0xfd, 0x8a, 0xb5, 0x47, + 0x00, 0x4c, 0x9e, 0xf1, 0x84, 0x22, 0x59, 0x09, + 0x96, 0x1d, 0xaf, 0x1f, 0xf0, 0xf0, 0xa1, 0x1e, + + 0x4a, 0x21, 0xb1, 0x15, 0xf9, 0xaf, 0x89, 0x5f, + 0x76, 0x36, 0x8e, 0xe2, 0x30, 0x11, 0x2d, 0x47, + 0x60, 0x51, 0xb8, 0x9a, 0x3a, 0x70, 0x56, 0x73, + 0x37, 0xad, 0x9d, 0x42, 0x3e, 0xf3, 0x55, 0x4c, +}; +static const u8 dhkey_1[32] __initconst = { + 0x98, 0xa6, 0xbf, 0x73, 0xf3, 0x34, 0x8d, 0x86, + 0xf1, 0x66, 0xf8, 0xb4, 0x13, 0x6b, 0x79, 0x99, + 0x9b, 0x7d, 0x39, 0x0a, 0xa6, 0x10, 0x10, 0x34, + 0x05, 0xad, 0xc8, 0x57, 0xa3, 0x34, 0x02, 0xec, +}; + +static const u8 priv_a_2[32] __initconst = { + 0x63, 0x76, 0x45, 0xd0, 0xf7, 0x73, 0xac, 0xb7, + 0xff, 0xdd, 0x03, 0x72, 0xb9, 0x72, 0x85, 0xb4, + 0x41, 0xb6, 0x5d, 0x0c, 0x5d, 0x54, 0x84, 0x60, + 0x1a, 0xa3, 0x9a, 0x3c, 0x69, 0x16, 0xa5, 0x06, +}; +static const u8 priv_b_2[32] __initconst = { + 0xba, 0x30, 0x55, 0x50, 0x19, 0xa2, 0xca, 0xa3, + 0xa5, 0x29, 0x08, 0xc6, 0xb5, 0x03, 0x88, 0x7e, + 0x03, 0x2b, 0x50, 0x73, 0xd4, 0x2e, 0x50, 0x97, + 0x64, 0xcd, 0x72, 0x0d, 0x67, 0xa0, 0x9a, 0x52, +}; +static const u8 pub_a_2[64] __initconst = { + 0xdd, 0x78, 0x5c, 0x74, 0x03, 0x9b, 0x7e, 0x98, + 0xcb, 0x94, 0x87, 0x4a, 0xad, 0xfa, 0xf8, 0xd5, + 0x43, 0x3e, 0x5c, 0xaf, 0xea, 0xb5, 0x4c, 0xf4, + 0x9e, 0x80, 0x79, 0x57, 0x7b, 0xa4, 0x31, 0x2c, + + 0x4f, 0x5d, 0x71, 0x43, 0x77, 0x43, 0xf8, 0xea, + 0xd4, 0x3e, 0xbd, 0x17, 0x91, 0x10, 0x21, 0xd0, + 0x1f, 0x87, 0x43, 0x8e, 0x40, 0xe2, 0x52, 0xcd, + 0xbe, 0xdf, 0x98, 0x38, 0x18, 0x12, 0x95, 0x91, +}; +static const u8 pub_b_2[64] __initconst = { + 0xcc, 0x00, 0x65, 0xe1, 0xf5, 0x6c, 0x0d, 0xcf, + 0xec, 0x96, 0x47, 0x20, 0x66, 0xc9, 0xdb, 0x84, + 0x81, 0x75, 0xa8, 0x4d, 0xc0, 0xdf, 0xc7, 0x9d, + 0x1b, 0x3f, 0x3d, 0xf2, 0x3f, 0xe4, 0x65, 0xf4, + + 0x79, 0xb2, 0xec, 0xd8, 0xca, 0x55, 0xa1, 0xa8, + 0x43, 0x4d, 0x6b, 0xca, 0x10, 0xb0, 0xc2, 0x01, + 0xc2, 0x33, 0x4e, 0x16, 0x24, 0xc4, 0xef, 0xee, + 0x99, 0xd8, 0xbb, 0xbc, 0x48, 0xd0, 0x01, 0x02, +}; +static const u8 dhkey_2[32] __initconst = { + 0x69, 0xeb, 0x21, 0x32, 0xf2, 0xc6, 0x05, 0x41, + 0x60, 0x19, 0xcd, 0x5e, 0x94, 0xe1, 0xe6, 0x5f, + 0x33, 0x07, 0xe3, 0x38, 0x4b, 0x68, 0xe5, 0x62, + 0x3f, 0x88, 0x6d, 0x2f, 0x3a, 0x84, 0x85, 0xab, +}; + +static const u8 priv_a_3[32] __initconst = { + 0xbd, 0x1a, 0x3c, 0xcd, 0xa6, 0xb8, 0x99, 0x58, + 0x99, 0xb7, 0x40, 0xeb, 0x7b, 0x60, 0xff, 0x4a, + 0x50, 0x3f, 0x10, 0xd2, 0xe3, 0xb3, 0xc9, 0x74, + 0x38, 0x5f, 0xc5, 0xa3, 0xd4, 0xf6, 0x49, 0x3f, +}; +static const u8 pub_a_3[64] __initconst = { + 0xe6, 0x9d, 0x35, 0x0e, 0x48, 0x01, 0x03, 0xcc, + 0xdb, 0xfd, 0xf4, 0xac, 0x11, 0x91, 0xf4, 0xef, + 0xb9, 0xa5, 0xf9, 0xe9, 0xa7, 0x83, 0x2c, 0x5e, + 0x2c, 0xbe, 0x97, 0xf2, 0xd2, 0x03, 0xb0, 0x20, + + 0x8b, 0xd2, 0x89, 0x15, 0xd0, 0x8e, 0x1c, 0x74, + 0x24, 0x30, 0xed, 0x8f, 0xc2, 0x45, 0x63, 0x76, + 0x5c, 0x15, 0x52, 0x5a, 0xbf, 0x9a, 0x32, 0x63, + 0x6d, 0xeb, 0x2a, 0x65, 0x49, 0x9c, 0x80, 0xdc, +}; +static const u8 dhkey_3[32] __initconst = { + 0x2d, 0xab, 0x00, 0x48, 0xcb, 0xb3, 0x7b, 0xda, + 0x55, 0x7b, 0x8b, 0x72, 0xa8, 0x57, 0x87, 0xc3, + 0x87, 0x27, 0x99, 0x32, 0xfc, 0x79, 0x5f, 0xae, + 0x7c, 0x1c, 0xf9, 0x49, 0xe6, 0xd7, 0xaa, 0x70, +}; + +static int __init test_ecdh_sample(const u8 priv_a[32], const u8 priv_b[32], + const u8 pub_a[64], const u8 pub_b[64], + const u8 dhkey[32]) +{ + u8 dhkey_a[32], dhkey_b[32]; + + ecdh_shared_secret(pub_b, priv_a, dhkey_a); + ecdh_shared_secret(pub_a, priv_b, dhkey_b); + + if (memcmp(dhkey_a, dhkey, 32)) + return -EINVAL; + + if (memcmp(dhkey_b, dhkey, 32)) + return -EINVAL; + + return 0; +} + +static char test_ecdh_buffer[32]; + +static ssize_t test_ecdh_read(struct file *file, char __user *user_buf, + size_t count, loff_t *ppos) +{ + return simple_read_from_buffer(user_buf, count, ppos, test_ecdh_buffer, + strlen(test_ecdh_buffer)); +} + +static const struct file_operations test_ecdh_fops = { + .open = simple_open, + .read = test_ecdh_read, + .llseek = default_llseek, +}; + +static int __init test_ecdh(void) +{ + ktime_t calltime, delta, rettime; + unsigned long long duration; + int err; + + calltime = ktime_get(); + + err = test_ecdh_sample(priv_a_1, priv_b_1, pub_a_1, pub_b_1, dhkey_1); + if (err) { + BT_ERR("ECDH sample 1 failed"); + goto done; + } + + err = test_ecdh_sample(priv_a_2, priv_b_2, pub_a_2, pub_b_2, dhkey_2); + if (err) { + BT_ERR("ECDH sample 2 failed"); + goto done; + } + + err = test_ecdh_sample(priv_a_3, priv_a_3, pub_a_3, pub_a_3, dhkey_3); + if (err) { + BT_ERR("ECDH sample 3 failed"); + goto done; + } + + rettime = ktime_get(); + delta = ktime_sub(rettime, calltime); + duration = (unsigned long long) ktime_to_ns(delta) >> 10; + + BT_INFO("ECDH test passed in %llu usecs", duration); + +done: + if (!err) + snprintf(test_ecdh_buffer, sizeof(test_ecdh_buffer), + "PASS (%llu usecs)\n", duration); + else + snprintf(test_ecdh_buffer, sizeof(test_ecdh_buffer), "FAIL\n"); + + debugfs_create_file("selftest_ecdh", 0444, bt_debugfs, NULL, + &test_ecdh_fops); + + return err; +} + +#else + +static inline int test_ecdh(void) +{ + return 0; +} + +#endif + +static int __init run_selftest(void) +{ + int err; + + BT_INFO("Starting self testing"); + + err = test_ecdh(); + if (err) + goto done; + + err = bt_selftest_smp(); + +done: + BT_INFO("Finished self testing"); + + return err; +} + +#if IS_MODULE(CONFIG_BT) + +/* This is run when CONFIG_BT_SELFTEST=y and CONFIG_BT=m and is just a + * wrapper to allow running this at module init. + * + * If CONFIG_BT_SELFTEST=n, then this code is not compiled at all. + */ +int __init bt_selftest(void) +{ + return run_selftest(); +} + +#else + +/* This is run when CONFIG_BT_SELFTEST=y and CONFIG_BT=y and is run + * via late_initcall() as last item in the initialization sequence. + * + * If CONFIG_BT_SELFTEST=n, then this code is not compiled at all. + */ +static int __init bt_selftest_init(void) +{ + return run_selftest(); +} +late_initcall(bt_selftest_init); + +#endif diff --git a/kernel/net/bluetooth/selftest.h b/kernel/net/bluetooth/selftest.h new file mode 100644 index 000000000..2aa0a346a --- /dev/null +++ b/kernel/net/bluetooth/selftest.h @@ -0,0 +1,45 @@ +/* + BlueZ - Bluetooth protocol stack for Linux + Copyright (C) 2014 Intel Corporation + + 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; + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS + OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT OF THIRD PARTY RIGHTS. + IN NO EVENT SHALL THE COPYRIGHT HOLDER(S) AND AUTHOR(S) BE LIABLE FOR ANY + CLAIM, OR ANY SPECIAL INDIRECT OR CONSEQUENTIAL DAMAGES, OR ANY DAMAGES + WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN + ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF + OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + + ALL LIABILITY, INCLUDING LIABILITY FOR INFRINGEMENT OF ANY PATENTS, + COPYRIGHTS, TRADEMARKS OR OTHER RIGHTS, RELATING TO USE OF THIS + SOFTWARE IS DISCLAIMED. +*/ + +#if IS_ENABLED(CONFIG_BT_SELFTEST) && IS_MODULE(CONFIG_BT) + +/* When CONFIG_BT_SELFTEST=y and the CONFIG_BT=m, then the self testing + * is run at module loading time. + */ +int bt_selftest(void); + +#else + +/* When CONFIG_BT_SELFTEST=y and CONFIG_BT=y, then the self testing + * is run via late_initcall() to make sure that subsys_initcall() of + * the Bluetooth subsystem and device_initcall() of the Crypto subsystem + * do not clash. + * + * When CONFIG_BT_SELFTEST=n, then this turns into an empty call that + * has no impact. + */ +static inline int bt_selftest(void) +{ + return 0; +} + +#endif diff --git a/kernel/net/bluetooth/smp.c b/kernel/net/bluetooth/smp.c new file mode 100644 index 000000000..1ab3dc9c8 --- /dev/null +++ b/kernel/net/bluetooth/smp.c @@ -0,0 +1,3670 @@ +/* + BlueZ - Bluetooth protocol stack for Linux + Copyright (C) 2011 Nokia Corporation and/or its subsidiary(-ies). + + 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; + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS + OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT OF THIRD PARTY RIGHTS. + IN NO EVENT SHALL THE COPYRIGHT HOLDER(S) AND AUTHOR(S) BE LIABLE FOR ANY + CLAIM, OR ANY SPECIAL INDIRECT OR CONSEQUENTIAL DAMAGES, OR ANY DAMAGES + WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN + ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF + OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + + ALL LIABILITY, INCLUDING LIABILITY FOR INFRINGEMENT OF ANY PATENTS, + COPYRIGHTS, TRADEMARKS OR OTHER RIGHTS, RELATING TO USE OF THIS + SOFTWARE IS DISCLAIMED. +*/ + +#include <linux/debugfs.h> +#include <linux/crypto.h> +#include <linux/scatterlist.h> +#include <crypto/b128ops.h> + +#include <net/bluetooth/bluetooth.h> +#include <net/bluetooth/hci_core.h> +#include <net/bluetooth/l2cap.h> +#include <net/bluetooth/mgmt.h> + +#include "ecc.h" +#include "smp.h" + +/* Low-level debug macros to be used for stuff that we don't want + * accidentially in dmesg, i.e. the values of the various crypto keys + * and the inputs & outputs of crypto functions. + */ +#ifdef DEBUG +#define SMP_DBG(fmt, ...) printk(KERN_DEBUG "%s: " fmt, __func__, \ + ##__VA_ARGS__) +#else +#define SMP_DBG(fmt, ...) no_printk(KERN_DEBUG "%s: " fmt, __func__, \ + ##__VA_ARGS__) +#endif + +#define SMP_ALLOW_CMD(smp, code) set_bit(code, &smp->allow_cmd) + +/* Keys which are not distributed with Secure Connections */ +#define SMP_SC_NO_DIST (SMP_DIST_ENC_KEY | SMP_DIST_LINK_KEY); + +#define SMP_TIMEOUT msecs_to_jiffies(30000) + +#define AUTH_REQ_MASK(dev) (hci_dev_test_flag(dev, HCI_SC_ENABLED) ? \ + 0x1f : 0x07) +#define KEY_DIST_MASK 0x07 + +/* Maximum message length that can be passed to aes_cmac */ +#define CMAC_MSG_MAX 80 + +enum { + SMP_FLAG_TK_VALID, + SMP_FLAG_CFM_PENDING, + SMP_FLAG_MITM_AUTH, + SMP_FLAG_COMPLETE, + SMP_FLAG_INITIATOR, + SMP_FLAG_SC, + SMP_FLAG_REMOTE_PK, + SMP_FLAG_DEBUG_KEY, + SMP_FLAG_WAIT_USER, + SMP_FLAG_DHKEY_PENDING, + SMP_FLAG_REMOTE_OOB, + SMP_FLAG_LOCAL_OOB, +}; + +struct smp_dev { + /* Secure Connections OOB data */ + u8 local_pk[64]; + u8 local_sk[32]; + u8 local_rand[16]; + bool debug_key; + + struct crypto_blkcipher *tfm_aes; + struct crypto_hash *tfm_cmac; +}; + +struct smp_chan { + struct l2cap_conn *conn; + struct delayed_work security_timer; + unsigned long allow_cmd; /* Bitmask of allowed commands */ + + u8 preq[7]; /* SMP Pairing Request */ + u8 prsp[7]; /* SMP Pairing Response */ + u8 prnd[16]; /* SMP Pairing Random (local) */ + u8 rrnd[16]; /* SMP Pairing Random (remote) */ + u8 pcnf[16]; /* SMP Pairing Confirm */ + u8 tk[16]; /* SMP Temporary Key */ + u8 rr[16]; /* Remote OOB ra/rb value */ + u8 lr[16]; /* Local OOB ra/rb value */ + u8 enc_key_size; + u8 remote_key_dist; + bdaddr_t id_addr; + u8 id_addr_type; + u8 irk[16]; + struct smp_csrk *csrk; + struct smp_csrk *slave_csrk; + struct smp_ltk *ltk; + struct smp_ltk *slave_ltk; + struct smp_irk *remote_irk; + u8 *link_key; + unsigned long flags; + u8 method; + u8 passkey_round; + + /* Secure Connections variables */ + u8 local_pk[64]; + u8 local_sk[32]; + u8 remote_pk[64]; + u8 dhkey[32]; + u8 mackey[16]; + + struct crypto_blkcipher *tfm_aes; + struct crypto_hash *tfm_cmac; +}; + +/* These debug key values are defined in the SMP section of the core + * specification. debug_pk is the public debug key and debug_sk the + * private debug key. + */ +static const u8 debug_pk[64] = { + 0xe6, 0x9d, 0x35, 0x0e, 0x48, 0x01, 0x03, 0xcc, + 0xdb, 0xfd, 0xf4, 0xac, 0x11, 0x91, 0xf4, 0xef, + 0xb9, 0xa5, 0xf9, 0xe9, 0xa7, 0x83, 0x2c, 0x5e, + 0x2c, 0xbe, 0x97, 0xf2, 0xd2, 0x03, 0xb0, 0x20, + + 0x8b, 0xd2, 0x89, 0x15, 0xd0, 0x8e, 0x1c, 0x74, + 0x24, 0x30, 0xed, 0x8f, 0xc2, 0x45, 0x63, 0x76, + 0x5c, 0x15, 0x52, 0x5a, 0xbf, 0x9a, 0x32, 0x63, + 0x6d, 0xeb, 0x2a, 0x65, 0x49, 0x9c, 0x80, 0xdc, +}; + +static const u8 debug_sk[32] = { + 0xbd, 0x1a, 0x3c, 0xcd, 0xa6, 0xb8, 0x99, 0x58, + 0x99, 0xb7, 0x40, 0xeb, 0x7b, 0x60, 0xff, 0x4a, + 0x50, 0x3f, 0x10, 0xd2, 0xe3, 0xb3, 0xc9, 0x74, + 0x38, 0x5f, 0xc5, 0xa3, 0xd4, 0xf6, 0x49, 0x3f, +}; + +static inline void swap_buf(const u8 *src, u8 *dst, size_t len) +{ + size_t i; + + for (i = 0; i < len; i++) + dst[len - 1 - i] = src[i]; +} + +/* The following functions map to the LE SC SMP crypto functions + * AES-CMAC, f4, f5, f6, g2 and h6. + */ + +static int aes_cmac(struct crypto_hash *tfm, const u8 k[16], const u8 *m, + size_t len, u8 mac[16]) +{ + uint8_t tmp[16], mac_msb[16], msg_msb[CMAC_MSG_MAX]; + struct hash_desc desc; + struct scatterlist sg; + int err; + + if (len > CMAC_MSG_MAX) + return -EFBIG; + + if (!tfm) { + BT_ERR("tfm %p", tfm); + return -EINVAL; + } + + desc.tfm = tfm; + desc.flags = 0; + + crypto_hash_init(&desc); + + /* Swap key and message from LSB to MSB */ + swap_buf(k, tmp, 16); + swap_buf(m, msg_msb, len); + + SMP_DBG("msg (len %zu) %*phN", len, (int) len, m); + SMP_DBG("key %16phN", k); + + err = crypto_hash_setkey(tfm, tmp, 16); + if (err) { + BT_ERR("cipher setkey failed: %d", err); + return err; + } + + sg_init_one(&sg, msg_msb, len); + + err = crypto_hash_update(&desc, &sg, len); + if (err) { + BT_ERR("Hash update error %d", err); + return err; + } + + err = crypto_hash_final(&desc, mac_msb); + if (err) { + BT_ERR("Hash final error %d", err); + return err; + } + + swap_buf(mac_msb, mac, 16); + + SMP_DBG("mac %16phN", mac); + + return 0; +} + +static int smp_f4(struct crypto_hash *tfm_cmac, const u8 u[32], const u8 v[32], + const u8 x[16], u8 z, u8 res[16]) +{ + u8 m[65]; + int err; + + SMP_DBG("u %32phN", u); + SMP_DBG("v %32phN", v); + SMP_DBG("x %16phN z %02x", x, z); + + m[0] = z; + memcpy(m + 1, v, 32); + memcpy(m + 33, u, 32); + + err = aes_cmac(tfm_cmac, x, m, sizeof(m), res); + if (err) + return err; + + SMP_DBG("res %16phN", res); + + return err; +} + +static int smp_f5(struct crypto_hash *tfm_cmac, const u8 w[32], + const u8 n1[16], const u8 n2[16], const u8 a1[7], + const u8 a2[7], u8 mackey[16], u8 ltk[16]) +{ + /* The btle, salt and length "magic" values are as defined in + * the SMP section of the Bluetooth core specification. In ASCII + * the btle value ends up being 'btle'. The salt is just a + * random number whereas length is the value 256 in little + * endian format. + */ + const u8 btle[4] = { 0x65, 0x6c, 0x74, 0x62 }; + const u8 salt[16] = { 0xbe, 0x83, 0x60, 0x5a, 0xdb, 0x0b, 0x37, 0x60, + 0x38, 0xa5, 0xf5, 0xaa, 0x91, 0x83, 0x88, 0x6c }; + const u8 length[2] = { 0x00, 0x01 }; + u8 m[53], t[16]; + int err; + + SMP_DBG("w %32phN", w); + SMP_DBG("n1 %16phN n2 %16phN", n1, n2); + SMP_DBG("a1 %7phN a2 %7phN", a1, a2); + + err = aes_cmac(tfm_cmac, salt, w, 32, t); + if (err) + return err; + + SMP_DBG("t %16phN", t); + + memcpy(m, length, 2); + memcpy(m + 2, a2, 7); + memcpy(m + 9, a1, 7); + memcpy(m + 16, n2, 16); + memcpy(m + 32, n1, 16); + memcpy(m + 48, btle, 4); + + m[52] = 0; /* Counter */ + + err = aes_cmac(tfm_cmac, t, m, sizeof(m), mackey); + if (err) + return err; + + SMP_DBG("mackey %16phN", mackey); + + m[52] = 1; /* Counter */ + + err = aes_cmac(tfm_cmac, t, m, sizeof(m), ltk); + if (err) + return err; + + SMP_DBG("ltk %16phN", ltk); + + return 0; +} + +static int smp_f6(struct crypto_hash *tfm_cmac, const u8 w[16], + const u8 n1[16], const u8 n2[16], const u8 r[16], + const u8 io_cap[3], const u8 a1[7], const u8 a2[7], + u8 res[16]) +{ + u8 m[65]; + int err; + + SMP_DBG("w %16phN", w); + SMP_DBG("n1 %16phN n2 %16phN", n1, n2); + SMP_DBG("r %16phN io_cap %3phN a1 %7phN a2 %7phN", r, io_cap, a1, a2); + + memcpy(m, a2, 7); + memcpy(m + 7, a1, 7); + memcpy(m + 14, io_cap, 3); + memcpy(m + 17, r, 16); + memcpy(m + 33, n2, 16); + memcpy(m + 49, n1, 16); + + err = aes_cmac(tfm_cmac, w, m, sizeof(m), res); + if (err) + return err; + + SMP_DBG("res %16phN", res); + + return err; +} + +static int smp_g2(struct crypto_hash *tfm_cmac, const u8 u[32], const u8 v[32], + const u8 x[16], const u8 y[16], u32 *val) +{ + u8 m[80], tmp[16]; + int err; + + SMP_DBG("u %32phN", u); + SMP_DBG("v %32phN", v); + SMP_DBG("x %16phN y %16phN", x, y); + + memcpy(m, y, 16); + memcpy(m + 16, v, 32); + memcpy(m + 48, u, 32); + + err = aes_cmac(tfm_cmac, x, m, sizeof(m), tmp); + if (err) + return err; + + *val = get_unaligned_le32(tmp); + *val %= 1000000; + + SMP_DBG("val %06u", *val); + + return 0; +} + +static int smp_h6(struct crypto_hash *tfm_cmac, const u8 w[16], + const u8 key_id[4], u8 res[16]) +{ + int err; + + SMP_DBG("w %16phN key_id %4phN", w, key_id); + + err = aes_cmac(tfm_cmac, w, key_id, 4, res); + if (err) + return err; + + SMP_DBG("res %16phN", res); + + return err; +} + +/* The following functions map to the legacy SMP crypto functions e, c1, + * s1 and ah. + */ + +static int smp_e(struct crypto_blkcipher *tfm, const u8 *k, u8 *r) +{ + struct blkcipher_desc desc; + struct scatterlist sg; + uint8_t tmp[16], data[16]; + int err; + + if (!tfm) { + BT_ERR("tfm %p", tfm); + return -EINVAL; + } + + desc.tfm = tfm; + desc.flags = 0; + + /* The most significant octet of key corresponds to k[0] */ + swap_buf(k, tmp, 16); + + err = crypto_blkcipher_setkey(tfm, tmp, 16); + if (err) { + BT_ERR("cipher setkey failed: %d", err); + return err; + } + + /* Most significant octet of plaintextData corresponds to data[0] */ + swap_buf(r, data, 16); + + sg_init_one(&sg, data, 16); + + err = crypto_blkcipher_encrypt(&desc, &sg, &sg, 16); + if (err) + BT_ERR("Encrypt data error %d", err); + + /* Most significant octet of encryptedData corresponds to data[0] */ + swap_buf(data, r, 16); + + return err; +} + +static int smp_c1(struct crypto_blkcipher *tfm_aes, const u8 k[16], + const u8 r[16], const u8 preq[7], const u8 pres[7], u8 _iat, + const bdaddr_t *ia, u8 _rat, const bdaddr_t *ra, u8 res[16]) +{ + u8 p1[16], p2[16]; + int err; + + memset(p1, 0, 16); + + /* p1 = pres || preq || _rat || _iat */ + p1[0] = _iat; + p1[1] = _rat; + memcpy(p1 + 2, preq, 7); + memcpy(p1 + 9, pres, 7); + + /* p2 = padding || ia || ra */ + memcpy(p2, ra, 6); + memcpy(p2 + 6, ia, 6); + memset(p2 + 12, 0, 4); + + /* res = r XOR p1 */ + u128_xor((u128 *) res, (u128 *) r, (u128 *) p1); + + /* res = e(k, res) */ + err = smp_e(tfm_aes, k, res); + if (err) { + BT_ERR("Encrypt data error"); + return err; + } + + /* res = res XOR p2 */ + u128_xor((u128 *) res, (u128 *) res, (u128 *) p2); + + /* res = e(k, res) */ + err = smp_e(tfm_aes, k, res); + if (err) + BT_ERR("Encrypt data error"); + + return err; +} + +static int smp_s1(struct crypto_blkcipher *tfm_aes, const u8 k[16], + const u8 r1[16], const u8 r2[16], u8 _r[16]) +{ + int err; + + /* Just least significant octets from r1 and r2 are considered */ + memcpy(_r, r2, 8); + memcpy(_r + 8, r1, 8); + + err = smp_e(tfm_aes, k, _r); + if (err) + BT_ERR("Encrypt data error"); + + return err; +} + +static int smp_ah(struct crypto_blkcipher *tfm, const u8 irk[16], + const u8 r[3], u8 res[3]) +{ + u8 _res[16]; + int err; + + /* r' = padding || r */ + memcpy(_res, r, 3); + memset(_res + 3, 0, 13); + + err = smp_e(tfm, irk, _res); + if (err) { + BT_ERR("Encrypt error"); + return err; + } + + /* The output of the random address function ah is: + * ah(h, r) = e(k, r') mod 2^24 + * The output of the security function e is then truncated to 24 bits + * by taking the least significant 24 bits of the output of e as the + * result of ah. + */ + memcpy(res, _res, 3); + + return 0; +} + +bool smp_irk_matches(struct hci_dev *hdev, const u8 irk[16], + const bdaddr_t *bdaddr) +{ + struct l2cap_chan *chan = hdev->smp_data; + struct smp_dev *smp; + u8 hash[3]; + int err; + + if (!chan || !chan->data) + return false; + + smp = chan->data; + + BT_DBG("RPA %pMR IRK %*phN", bdaddr, 16, irk); + + err = smp_ah(smp->tfm_aes, irk, &bdaddr->b[3], hash); + if (err) + return false; + + return !memcmp(bdaddr->b, hash, 3); +} + +int smp_generate_rpa(struct hci_dev *hdev, const u8 irk[16], bdaddr_t *rpa) +{ + struct l2cap_chan *chan = hdev->smp_data; + struct smp_dev *smp; + int err; + + if (!chan || !chan->data) + return -EOPNOTSUPP; + + smp = chan->data; + + get_random_bytes(&rpa->b[3], 3); + + rpa->b[5] &= 0x3f; /* Clear two most significant bits */ + rpa->b[5] |= 0x40; /* Set second most significant bit */ + + err = smp_ah(smp->tfm_aes, irk, &rpa->b[3], rpa->b); + if (err < 0) + return err; + + BT_DBG("RPA %pMR", rpa); + + return 0; +} + +int smp_generate_oob(struct hci_dev *hdev, u8 hash[16], u8 rand[16]) +{ + struct l2cap_chan *chan = hdev->smp_data; + struct smp_dev *smp; + int err; + + if (!chan || !chan->data) + return -EOPNOTSUPP; + + smp = chan->data; + + if (hci_dev_test_flag(hdev, HCI_USE_DEBUG_KEYS)) { + BT_DBG("Using debug keys"); + memcpy(smp->local_pk, debug_pk, 64); + memcpy(smp->local_sk, debug_sk, 32); + smp->debug_key = true; + } else { + while (true) { + /* Generate local key pair for Secure Connections */ + if (!ecc_make_key(smp->local_pk, smp->local_sk)) + return -EIO; + + /* This is unlikely, but we need to check that + * we didn't accidentially generate a debug key. + */ + if (memcmp(smp->local_sk, debug_sk, 32)) + break; + } + smp->debug_key = false; + } + + SMP_DBG("OOB Public Key X: %32phN", smp->local_pk); + SMP_DBG("OOB Public Key Y: %32phN", smp->local_pk + 32); + SMP_DBG("OOB Private Key: %32phN", smp->local_sk); + + get_random_bytes(smp->local_rand, 16); + + err = smp_f4(smp->tfm_cmac, smp->local_pk, smp->local_pk, + smp->local_rand, 0, hash); + if (err < 0) + return err; + + memcpy(rand, smp->local_rand, 16); + + return 0; +} + +static void smp_send_cmd(struct l2cap_conn *conn, u8 code, u16 len, void *data) +{ + struct l2cap_chan *chan = conn->smp; + struct smp_chan *smp; + struct kvec iv[2]; + struct msghdr msg; + + if (!chan) + return; + + BT_DBG("code 0x%2.2x", code); + + iv[0].iov_base = &code; + iv[0].iov_len = 1; + + iv[1].iov_base = data; + iv[1].iov_len = len; + + memset(&msg, 0, sizeof(msg)); + + iov_iter_kvec(&msg.msg_iter, WRITE | ITER_KVEC, iv, 2, 1 + len); + + l2cap_chan_send(chan, &msg, 1 + len); + + if (!chan->data) + return; + + smp = chan->data; + + cancel_delayed_work_sync(&smp->security_timer); + schedule_delayed_work(&smp->security_timer, SMP_TIMEOUT); +} + +static u8 authreq_to_seclevel(u8 authreq) +{ + if (authreq & SMP_AUTH_MITM) { + if (authreq & SMP_AUTH_SC) + return BT_SECURITY_FIPS; + else + return BT_SECURITY_HIGH; + } else { + return BT_SECURITY_MEDIUM; + } +} + +static __u8 seclevel_to_authreq(__u8 sec_level) +{ + switch (sec_level) { + case BT_SECURITY_FIPS: + case BT_SECURITY_HIGH: + return SMP_AUTH_MITM | SMP_AUTH_BONDING; + case BT_SECURITY_MEDIUM: + return SMP_AUTH_BONDING; + default: + return SMP_AUTH_NONE; + } +} + +static void build_pairing_cmd(struct l2cap_conn *conn, + struct smp_cmd_pairing *req, + struct smp_cmd_pairing *rsp, __u8 authreq) +{ + struct l2cap_chan *chan = conn->smp; + struct smp_chan *smp = chan->data; + struct hci_conn *hcon = conn->hcon; + struct hci_dev *hdev = hcon->hdev; + u8 local_dist = 0, remote_dist = 0, oob_flag = SMP_OOB_NOT_PRESENT; + + if (hci_dev_test_flag(hdev, HCI_BONDABLE)) { + local_dist = SMP_DIST_ENC_KEY | SMP_DIST_SIGN; + remote_dist = SMP_DIST_ENC_KEY | SMP_DIST_SIGN; + authreq |= SMP_AUTH_BONDING; + } else { + authreq &= ~SMP_AUTH_BONDING; + } + + if (hci_dev_test_flag(hdev, HCI_RPA_RESOLVING)) + remote_dist |= SMP_DIST_ID_KEY; + + if (hci_dev_test_flag(hdev, HCI_PRIVACY)) + local_dist |= SMP_DIST_ID_KEY; + + if (hci_dev_test_flag(hdev, HCI_SC_ENABLED) && + (authreq & SMP_AUTH_SC)) { + struct oob_data *oob_data; + u8 bdaddr_type; + + if (hci_dev_test_flag(hdev, HCI_SSP_ENABLED)) { + local_dist |= SMP_DIST_LINK_KEY; + remote_dist |= SMP_DIST_LINK_KEY; + } + + if (hcon->dst_type == ADDR_LE_DEV_PUBLIC) + bdaddr_type = BDADDR_LE_PUBLIC; + else + bdaddr_type = BDADDR_LE_RANDOM; + + oob_data = hci_find_remote_oob_data(hdev, &hcon->dst, + bdaddr_type); + if (oob_data && oob_data->present) { + set_bit(SMP_FLAG_REMOTE_OOB, &smp->flags); + oob_flag = SMP_OOB_PRESENT; + memcpy(smp->rr, oob_data->rand256, 16); + memcpy(smp->pcnf, oob_data->hash256, 16); + SMP_DBG("OOB Remote Confirmation: %16phN", smp->pcnf); + SMP_DBG("OOB Remote Random: %16phN", smp->rr); + } + + } else { + authreq &= ~SMP_AUTH_SC; + } + + if (rsp == NULL) { + req->io_capability = conn->hcon->io_capability; + req->oob_flag = oob_flag; + req->max_key_size = SMP_MAX_ENC_KEY_SIZE; + req->init_key_dist = local_dist; + req->resp_key_dist = remote_dist; + req->auth_req = (authreq & AUTH_REQ_MASK(hdev)); + + smp->remote_key_dist = remote_dist; + return; + } + + rsp->io_capability = conn->hcon->io_capability; + rsp->oob_flag = oob_flag; + rsp->max_key_size = SMP_MAX_ENC_KEY_SIZE; + rsp->init_key_dist = req->init_key_dist & remote_dist; + rsp->resp_key_dist = req->resp_key_dist & local_dist; + rsp->auth_req = (authreq & AUTH_REQ_MASK(hdev)); + + smp->remote_key_dist = rsp->init_key_dist; +} + +static u8 check_enc_key_size(struct l2cap_conn *conn, __u8 max_key_size) +{ + struct l2cap_chan *chan = conn->smp; + struct smp_chan *smp = chan->data; + + if ((max_key_size > SMP_MAX_ENC_KEY_SIZE) || + (max_key_size < SMP_MIN_ENC_KEY_SIZE)) + return SMP_ENC_KEY_SIZE; + + smp->enc_key_size = max_key_size; + + return 0; +} + +static void smp_chan_destroy(struct l2cap_conn *conn) +{ + struct l2cap_chan *chan = conn->smp; + struct smp_chan *smp = chan->data; + struct hci_conn *hcon = conn->hcon; + bool complete; + + BUG_ON(!smp); + + cancel_delayed_work_sync(&smp->security_timer); + + complete = test_bit(SMP_FLAG_COMPLETE, &smp->flags); + mgmt_smp_complete(hcon, complete); + + kzfree(smp->csrk); + kzfree(smp->slave_csrk); + kzfree(smp->link_key); + + crypto_free_blkcipher(smp->tfm_aes); + crypto_free_hash(smp->tfm_cmac); + + /* Ensure that we don't leave any debug key around if debug key + * support hasn't been explicitly enabled. + */ + if (smp->ltk && smp->ltk->type == SMP_LTK_P256_DEBUG && + !hci_dev_test_flag(hcon->hdev, HCI_KEEP_DEBUG_KEYS)) { + list_del_rcu(&smp->ltk->list); + kfree_rcu(smp->ltk, rcu); + smp->ltk = NULL; + } + + /* If pairing failed clean up any keys we might have */ + if (!complete) { + if (smp->ltk) { + list_del_rcu(&smp->ltk->list); + kfree_rcu(smp->ltk, rcu); + } + + if (smp->slave_ltk) { + list_del_rcu(&smp->slave_ltk->list); + kfree_rcu(smp->slave_ltk, rcu); + } + + if (smp->remote_irk) { + list_del_rcu(&smp->remote_irk->list); + kfree_rcu(smp->remote_irk, rcu); + } + } + + chan->data = NULL; + kzfree(smp); + hci_conn_drop(hcon); +} + +static void smp_failure(struct l2cap_conn *conn, u8 reason) +{ + struct hci_conn *hcon = conn->hcon; + struct l2cap_chan *chan = conn->smp; + + if (reason) + smp_send_cmd(conn, SMP_CMD_PAIRING_FAIL, sizeof(reason), + &reason); + + clear_bit(HCI_CONN_ENCRYPT_PEND, &hcon->flags); + mgmt_auth_failed(hcon, HCI_ERROR_AUTH_FAILURE); + + if (chan->data) + smp_chan_destroy(conn); +} + +#define JUST_WORKS 0x00 +#define JUST_CFM 0x01 +#define REQ_PASSKEY 0x02 +#define CFM_PASSKEY 0x03 +#define REQ_OOB 0x04 +#define DSP_PASSKEY 0x05 +#define OVERLAP 0xFF + +static const u8 gen_method[5][5] = { + { JUST_WORKS, JUST_CFM, REQ_PASSKEY, JUST_WORKS, REQ_PASSKEY }, + { JUST_WORKS, JUST_CFM, REQ_PASSKEY, JUST_WORKS, REQ_PASSKEY }, + { CFM_PASSKEY, CFM_PASSKEY, REQ_PASSKEY, JUST_WORKS, CFM_PASSKEY }, + { JUST_WORKS, JUST_CFM, JUST_WORKS, JUST_WORKS, JUST_CFM }, + { CFM_PASSKEY, CFM_PASSKEY, REQ_PASSKEY, JUST_WORKS, OVERLAP }, +}; + +static const u8 sc_method[5][5] = { + { JUST_WORKS, JUST_CFM, REQ_PASSKEY, JUST_WORKS, REQ_PASSKEY }, + { JUST_WORKS, CFM_PASSKEY, REQ_PASSKEY, JUST_WORKS, CFM_PASSKEY }, + { DSP_PASSKEY, DSP_PASSKEY, REQ_PASSKEY, JUST_WORKS, DSP_PASSKEY }, + { JUST_WORKS, JUST_CFM, JUST_WORKS, JUST_WORKS, JUST_CFM }, + { DSP_PASSKEY, CFM_PASSKEY, REQ_PASSKEY, JUST_WORKS, CFM_PASSKEY }, +}; + +static u8 get_auth_method(struct smp_chan *smp, u8 local_io, u8 remote_io) +{ + /* If either side has unknown io_caps, use JUST_CFM (which gets + * converted later to JUST_WORKS if we're initiators. + */ + if (local_io > SMP_IO_KEYBOARD_DISPLAY || + remote_io > SMP_IO_KEYBOARD_DISPLAY) + return JUST_CFM; + + if (test_bit(SMP_FLAG_SC, &smp->flags)) + return sc_method[remote_io][local_io]; + + return gen_method[remote_io][local_io]; +} + +static int tk_request(struct l2cap_conn *conn, u8 remote_oob, u8 auth, + u8 local_io, u8 remote_io) +{ + struct hci_conn *hcon = conn->hcon; + struct l2cap_chan *chan = conn->smp; + struct smp_chan *smp = chan->data; + u32 passkey = 0; + int ret = 0; + + /* Initialize key for JUST WORKS */ + memset(smp->tk, 0, sizeof(smp->tk)); + clear_bit(SMP_FLAG_TK_VALID, &smp->flags); + + BT_DBG("tk_request: auth:%d lcl:%d rem:%d", auth, local_io, remote_io); + + /* If neither side wants MITM, either "just" confirm an incoming + * request or use just-works for outgoing ones. The JUST_CFM + * will be converted to JUST_WORKS if necessary later in this + * function. If either side has MITM look up the method from the + * table. + */ + if (!(auth & SMP_AUTH_MITM)) + smp->method = JUST_CFM; + else + smp->method = get_auth_method(smp, local_io, remote_io); + + /* Don't confirm locally initiated pairing attempts */ + if (smp->method == JUST_CFM && test_bit(SMP_FLAG_INITIATOR, + &smp->flags)) + smp->method = JUST_WORKS; + + /* Don't bother user space with no IO capabilities */ + if (smp->method == JUST_CFM && + hcon->io_capability == HCI_IO_NO_INPUT_OUTPUT) + smp->method = JUST_WORKS; + + /* If Just Works, Continue with Zero TK */ + if (smp->method == JUST_WORKS) { + set_bit(SMP_FLAG_TK_VALID, &smp->flags); + return 0; + } + + /* If this function is used for SC -> legacy fallback we + * can only recover the just-works case. + */ + if (test_bit(SMP_FLAG_SC, &smp->flags)) + return -EINVAL; + + /* Not Just Works/Confirm results in MITM Authentication */ + if (smp->method != JUST_CFM) { + set_bit(SMP_FLAG_MITM_AUTH, &smp->flags); + if (hcon->pending_sec_level < BT_SECURITY_HIGH) + hcon->pending_sec_level = BT_SECURITY_HIGH; + } + + /* If both devices have Keyoard-Display I/O, the master + * Confirms and the slave Enters the passkey. + */ + if (smp->method == OVERLAP) { + if (hcon->role == HCI_ROLE_MASTER) + smp->method = CFM_PASSKEY; + else + smp->method = REQ_PASSKEY; + } + + /* Generate random passkey. */ + if (smp->method == CFM_PASSKEY) { + memset(smp->tk, 0, sizeof(smp->tk)); + get_random_bytes(&passkey, sizeof(passkey)); + passkey %= 1000000; + put_unaligned_le32(passkey, smp->tk); + BT_DBG("PassKey: %d", passkey); + set_bit(SMP_FLAG_TK_VALID, &smp->flags); + } + + if (smp->method == REQ_PASSKEY) + ret = mgmt_user_passkey_request(hcon->hdev, &hcon->dst, + hcon->type, hcon->dst_type); + else if (smp->method == JUST_CFM) + ret = mgmt_user_confirm_request(hcon->hdev, &hcon->dst, + hcon->type, hcon->dst_type, + passkey, 1); + else + ret = mgmt_user_passkey_notify(hcon->hdev, &hcon->dst, + hcon->type, hcon->dst_type, + passkey, 0); + + return ret; +} + +static u8 smp_confirm(struct smp_chan *smp) +{ + struct l2cap_conn *conn = smp->conn; + struct smp_cmd_pairing_confirm cp; + int ret; + + BT_DBG("conn %p", conn); + + ret = smp_c1(smp->tfm_aes, smp->tk, smp->prnd, smp->preq, smp->prsp, + conn->hcon->init_addr_type, &conn->hcon->init_addr, + conn->hcon->resp_addr_type, &conn->hcon->resp_addr, + cp.confirm_val); + if (ret) + return SMP_UNSPECIFIED; + + clear_bit(SMP_FLAG_CFM_PENDING, &smp->flags); + + smp_send_cmd(smp->conn, SMP_CMD_PAIRING_CONFIRM, sizeof(cp), &cp); + + if (conn->hcon->out) + SMP_ALLOW_CMD(smp, SMP_CMD_PAIRING_CONFIRM); + else + SMP_ALLOW_CMD(smp, SMP_CMD_PAIRING_RANDOM); + + return 0; +} + +static u8 smp_random(struct smp_chan *smp) +{ + struct l2cap_conn *conn = smp->conn; + struct hci_conn *hcon = conn->hcon; + u8 confirm[16]; + int ret; + + if (IS_ERR_OR_NULL(smp->tfm_aes)) + return SMP_UNSPECIFIED; + + BT_DBG("conn %p %s", conn, conn->hcon->out ? "master" : "slave"); + + ret = smp_c1(smp->tfm_aes, smp->tk, smp->rrnd, smp->preq, smp->prsp, + hcon->init_addr_type, &hcon->init_addr, + hcon->resp_addr_type, &hcon->resp_addr, confirm); + if (ret) + return SMP_UNSPECIFIED; + + if (memcmp(smp->pcnf, confirm, sizeof(smp->pcnf)) != 0) { + BT_ERR("Pairing failed (confirmation values mismatch)"); + return SMP_CONFIRM_FAILED; + } + + if (hcon->out) { + u8 stk[16]; + __le64 rand = 0; + __le16 ediv = 0; + + smp_s1(smp->tfm_aes, smp->tk, smp->rrnd, smp->prnd, stk); + + memset(stk + smp->enc_key_size, 0, + SMP_MAX_ENC_KEY_SIZE - smp->enc_key_size); + + if (test_and_set_bit(HCI_CONN_ENCRYPT_PEND, &hcon->flags)) + return SMP_UNSPECIFIED; + + hci_le_start_enc(hcon, ediv, rand, stk); + hcon->enc_key_size = smp->enc_key_size; + set_bit(HCI_CONN_STK_ENCRYPT, &hcon->flags); + } else { + u8 stk[16], auth; + __le64 rand = 0; + __le16 ediv = 0; + + smp_send_cmd(conn, SMP_CMD_PAIRING_RANDOM, sizeof(smp->prnd), + smp->prnd); + + smp_s1(smp->tfm_aes, smp->tk, smp->prnd, smp->rrnd, stk); + + memset(stk + smp->enc_key_size, 0, + SMP_MAX_ENC_KEY_SIZE - smp->enc_key_size); + + if (hcon->pending_sec_level == BT_SECURITY_HIGH) + auth = 1; + else + auth = 0; + + /* Even though there's no _SLAVE suffix this is the + * slave STK we're adding for later lookup (the master + * STK never needs to be stored). + */ + hci_add_ltk(hcon->hdev, &hcon->dst, hcon->dst_type, + SMP_STK, auth, stk, smp->enc_key_size, ediv, rand); + } + + return 0; +} + +static void smp_notify_keys(struct l2cap_conn *conn) +{ + struct l2cap_chan *chan = conn->smp; + struct smp_chan *smp = chan->data; + struct hci_conn *hcon = conn->hcon; + struct hci_dev *hdev = hcon->hdev; + struct smp_cmd_pairing *req = (void *) &smp->preq[1]; + struct smp_cmd_pairing *rsp = (void *) &smp->prsp[1]; + bool persistent; + + if (smp->remote_irk) { + mgmt_new_irk(hdev, smp->remote_irk); + /* Now that user space can be considered to know the + * identity address track the connection based on it + * from now on (assuming this is an LE link). + */ + if (hcon->type == LE_LINK) { + bacpy(&hcon->dst, &smp->remote_irk->bdaddr); + hcon->dst_type = smp->remote_irk->addr_type; + queue_work(hdev->workqueue, &conn->id_addr_update_work); + } + + /* When receiving an indentity resolving key for + * a remote device that does not use a resolvable + * private address, just remove the key so that + * it is possible to use the controller white + * list for scanning. + * + * Userspace will have been told to not store + * this key at this point. So it is safe to + * just remove it. + */ + if (!bacmp(&smp->remote_irk->rpa, BDADDR_ANY)) { + list_del_rcu(&smp->remote_irk->list); + kfree_rcu(smp->remote_irk, rcu); + smp->remote_irk = NULL; + } + } + + if (hcon->type == ACL_LINK) { + if (hcon->key_type == HCI_LK_DEBUG_COMBINATION) + persistent = false; + else + persistent = !test_bit(HCI_CONN_FLUSH_KEY, + &hcon->flags); + } else { + /* The LTKs and CSRKs should be persistent only if both sides + * had the bonding bit set in their authentication requests. + */ + persistent = !!((req->auth_req & rsp->auth_req) & + SMP_AUTH_BONDING); + } + + + if (smp->csrk) { + smp->csrk->bdaddr_type = hcon->dst_type; + bacpy(&smp->csrk->bdaddr, &hcon->dst); + mgmt_new_csrk(hdev, smp->csrk, persistent); + } + + if (smp->slave_csrk) { + smp->slave_csrk->bdaddr_type = hcon->dst_type; + bacpy(&smp->slave_csrk->bdaddr, &hcon->dst); + mgmt_new_csrk(hdev, smp->slave_csrk, persistent); + } + + if (smp->ltk) { + smp->ltk->bdaddr_type = hcon->dst_type; + bacpy(&smp->ltk->bdaddr, &hcon->dst); + mgmt_new_ltk(hdev, smp->ltk, persistent); + } + + if (smp->slave_ltk) { + smp->slave_ltk->bdaddr_type = hcon->dst_type; + bacpy(&smp->slave_ltk->bdaddr, &hcon->dst); + mgmt_new_ltk(hdev, smp->slave_ltk, persistent); + } + + if (smp->link_key) { + struct link_key *key; + u8 type; + + if (test_bit(SMP_FLAG_DEBUG_KEY, &smp->flags)) + type = HCI_LK_DEBUG_COMBINATION; + else if (hcon->sec_level == BT_SECURITY_FIPS) + type = HCI_LK_AUTH_COMBINATION_P256; + else + type = HCI_LK_UNAUTH_COMBINATION_P256; + + key = hci_add_link_key(hdev, smp->conn->hcon, &hcon->dst, + smp->link_key, type, 0, &persistent); + if (key) { + mgmt_new_link_key(hdev, key, persistent); + + /* Don't keep debug keys around if the relevant + * flag is not set. + */ + if (!hci_dev_test_flag(hdev, HCI_KEEP_DEBUG_KEYS) && + key->type == HCI_LK_DEBUG_COMBINATION) { + list_del_rcu(&key->list); + kfree_rcu(key, rcu); + } + } + } +} + +static void sc_add_ltk(struct smp_chan *smp) +{ + struct hci_conn *hcon = smp->conn->hcon; + u8 key_type, auth; + + if (test_bit(SMP_FLAG_DEBUG_KEY, &smp->flags)) + key_type = SMP_LTK_P256_DEBUG; + else + key_type = SMP_LTK_P256; + + if (hcon->pending_sec_level == BT_SECURITY_FIPS) + auth = 1; + else + auth = 0; + + memset(smp->tk + smp->enc_key_size, 0, + SMP_MAX_ENC_KEY_SIZE - smp->enc_key_size); + + smp->ltk = hci_add_ltk(hcon->hdev, &hcon->dst, hcon->dst_type, + key_type, auth, smp->tk, smp->enc_key_size, + 0, 0); +} + +static void sc_generate_link_key(struct smp_chan *smp) +{ + /* These constants are as specified in the core specification. + * In ASCII they spell out to 'tmp1' and 'lebr'. + */ + const u8 tmp1[4] = { 0x31, 0x70, 0x6d, 0x74 }; + const u8 lebr[4] = { 0x72, 0x62, 0x65, 0x6c }; + + smp->link_key = kzalloc(16, GFP_KERNEL); + if (!smp->link_key) + return; + + if (smp_h6(smp->tfm_cmac, smp->tk, tmp1, smp->link_key)) { + kzfree(smp->link_key); + smp->link_key = NULL; + return; + } + + if (smp_h6(smp->tfm_cmac, smp->link_key, lebr, smp->link_key)) { + kzfree(smp->link_key); + smp->link_key = NULL; + return; + } +} + +static void smp_allow_key_dist(struct smp_chan *smp) +{ + /* Allow the first expected phase 3 PDU. The rest of the PDUs + * will be allowed in each PDU handler to ensure we receive + * them in the correct order. + */ + if (smp->remote_key_dist & SMP_DIST_ENC_KEY) + SMP_ALLOW_CMD(smp, SMP_CMD_ENCRYPT_INFO); + else if (smp->remote_key_dist & SMP_DIST_ID_KEY) + SMP_ALLOW_CMD(smp, SMP_CMD_IDENT_INFO); + else if (smp->remote_key_dist & SMP_DIST_SIGN) + SMP_ALLOW_CMD(smp, SMP_CMD_SIGN_INFO); +} + +static void sc_generate_ltk(struct smp_chan *smp) +{ + /* These constants are as specified in the core specification. + * In ASCII they spell out to 'tmp2' and 'brle'. + */ + const u8 tmp2[4] = { 0x32, 0x70, 0x6d, 0x74 }; + const u8 brle[4] = { 0x65, 0x6c, 0x72, 0x62 }; + struct hci_conn *hcon = smp->conn->hcon; + struct hci_dev *hdev = hcon->hdev; + struct link_key *key; + + key = hci_find_link_key(hdev, &hcon->dst); + if (!key) { + BT_ERR("%s No Link Key found to generate LTK", hdev->name); + return; + } + + if (key->type == HCI_LK_DEBUG_COMBINATION) + set_bit(SMP_FLAG_DEBUG_KEY, &smp->flags); + + if (smp_h6(smp->tfm_cmac, key->val, tmp2, smp->tk)) + return; + + if (smp_h6(smp->tfm_cmac, smp->tk, brle, smp->tk)) + return; + + sc_add_ltk(smp); +} + +static void smp_distribute_keys(struct smp_chan *smp) +{ + struct smp_cmd_pairing *req, *rsp; + struct l2cap_conn *conn = smp->conn; + struct hci_conn *hcon = conn->hcon; + struct hci_dev *hdev = hcon->hdev; + __u8 *keydist; + + BT_DBG("conn %p", conn); + + rsp = (void *) &smp->prsp[1]; + + /* The responder sends its keys first */ + if (hcon->out && (smp->remote_key_dist & KEY_DIST_MASK)) { + smp_allow_key_dist(smp); + return; + } + + req = (void *) &smp->preq[1]; + + if (hcon->out) { + keydist = &rsp->init_key_dist; + *keydist &= req->init_key_dist; + } else { + keydist = &rsp->resp_key_dist; + *keydist &= req->resp_key_dist; + } + + if (test_bit(SMP_FLAG_SC, &smp->flags)) { + if (hcon->type == LE_LINK && (*keydist & SMP_DIST_LINK_KEY)) + sc_generate_link_key(smp); + if (hcon->type == ACL_LINK && (*keydist & SMP_DIST_ENC_KEY)) + sc_generate_ltk(smp); + + /* Clear the keys which are generated but not distributed */ + *keydist &= ~SMP_SC_NO_DIST; + } + + BT_DBG("keydist 0x%x", *keydist); + + if (*keydist & SMP_DIST_ENC_KEY) { + struct smp_cmd_encrypt_info enc; + struct smp_cmd_master_ident ident; + struct smp_ltk *ltk; + u8 authenticated; + __le16 ediv; + __le64 rand; + + get_random_bytes(enc.ltk, sizeof(enc.ltk)); + get_random_bytes(&ediv, sizeof(ediv)); + get_random_bytes(&rand, sizeof(rand)); + + smp_send_cmd(conn, SMP_CMD_ENCRYPT_INFO, sizeof(enc), &enc); + + authenticated = hcon->sec_level == BT_SECURITY_HIGH; + ltk = hci_add_ltk(hdev, &hcon->dst, hcon->dst_type, + SMP_LTK_SLAVE, authenticated, enc.ltk, + smp->enc_key_size, ediv, rand); + smp->slave_ltk = ltk; + + ident.ediv = ediv; + ident.rand = rand; + + smp_send_cmd(conn, SMP_CMD_MASTER_IDENT, sizeof(ident), &ident); + + *keydist &= ~SMP_DIST_ENC_KEY; + } + + if (*keydist & SMP_DIST_ID_KEY) { + struct smp_cmd_ident_addr_info addrinfo; + struct smp_cmd_ident_info idinfo; + + memcpy(idinfo.irk, hdev->irk, sizeof(idinfo.irk)); + + smp_send_cmd(conn, SMP_CMD_IDENT_INFO, sizeof(idinfo), &idinfo); + + /* The hci_conn contains the local identity address + * after the connection has been established. + * + * This is true even when the connection has been + * established using a resolvable random address. + */ + bacpy(&addrinfo.bdaddr, &hcon->src); + addrinfo.addr_type = hcon->src_type; + + smp_send_cmd(conn, SMP_CMD_IDENT_ADDR_INFO, sizeof(addrinfo), + &addrinfo); + + *keydist &= ~SMP_DIST_ID_KEY; + } + + if (*keydist & SMP_DIST_SIGN) { + struct smp_cmd_sign_info sign; + struct smp_csrk *csrk; + + /* Generate a new random key */ + get_random_bytes(sign.csrk, sizeof(sign.csrk)); + + csrk = kzalloc(sizeof(*csrk), GFP_KERNEL); + if (csrk) { + if (hcon->sec_level > BT_SECURITY_MEDIUM) + csrk->type = MGMT_CSRK_LOCAL_AUTHENTICATED; + else + csrk->type = MGMT_CSRK_LOCAL_UNAUTHENTICATED; + memcpy(csrk->val, sign.csrk, sizeof(csrk->val)); + } + smp->slave_csrk = csrk; + + smp_send_cmd(conn, SMP_CMD_SIGN_INFO, sizeof(sign), &sign); + + *keydist &= ~SMP_DIST_SIGN; + } + + /* If there are still keys to be received wait for them */ + if (smp->remote_key_dist & KEY_DIST_MASK) { + smp_allow_key_dist(smp); + return; + } + + set_bit(SMP_FLAG_COMPLETE, &smp->flags); + smp_notify_keys(conn); + + smp_chan_destroy(conn); +} + +static void smp_timeout(struct work_struct *work) +{ + struct smp_chan *smp = container_of(work, struct smp_chan, + security_timer.work); + struct l2cap_conn *conn = smp->conn; + + BT_DBG("conn %p", conn); + + hci_disconnect(conn->hcon, HCI_ERROR_REMOTE_USER_TERM); +} + +static struct smp_chan *smp_chan_create(struct l2cap_conn *conn) +{ + struct l2cap_chan *chan = conn->smp; + struct smp_chan *smp; + + smp = kzalloc(sizeof(*smp), GFP_ATOMIC); + if (!smp) + return NULL; + + smp->tfm_aes = crypto_alloc_blkcipher("ecb(aes)", 0, CRYPTO_ALG_ASYNC); + if (IS_ERR(smp->tfm_aes)) { + BT_ERR("Unable to create ECB crypto context"); + kzfree(smp); + return NULL; + } + + smp->tfm_cmac = crypto_alloc_hash("cmac(aes)", 0, CRYPTO_ALG_ASYNC); + if (IS_ERR(smp->tfm_cmac)) { + BT_ERR("Unable to create CMAC crypto context"); + crypto_free_blkcipher(smp->tfm_aes); + kzfree(smp); + return NULL; + } + + smp->conn = conn; + chan->data = smp; + + SMP_ALLOW_CMD(smp, SMP_CMD_PAIRING_FAIL); + + INIT_DELAYED_WORK(&smp->security_timer, smp_timeout); + + hci_conn_hold(conn->hcon); + + return smp; +} + +static int sc_mackey_and_ltk(struct smp_chan *smp, u8 mackey[16], u8 ltk[16]) +{ + struct hci_conn *hcon = smp->conn->hcon; + u8 *na, *nb, a[7], b[7]; + + if (hcon->out) { + na = smp->prnd; + nb = smp->rrnd; + } else { + na = smp->rrnd; + nb = smp->prnd; + } + + memcpy(a, &hcon->init_addr, 6); + memcpy(b, &hcon->resp_addr, 6); + a[6] = hcon->init_addr_type; + b[6] = hcon->resp_addr_type; + + return smp_f5(smp->tfm_cmac, smp->dhkey, na, nb, a, b, mackey, ltk); +} + +static void sc_dhkey_check(struct smp_chan *smp) +{ + struct hci_conn *hcon = smp->conn->hcon; + struct smp_cmd_dhkey_check check; + u8 a[7], b[7], *local_addr, *remote_addr; + u8 io_cap[3], r[16]; + + memcpy(a, &hcon->init_addr, 6); + memcpy(b, &hcon->resp_addr, 6); + a[6] = hcon->init_addr_type; + b[6] = hcon->resp_addr_type; + + if (hcon->out) { + local_addr = a; + remote_addr = b; + memcpy(io_cap, &smp->preq[1], 3); + } else { + local_addr = b; + remote_addr = a; + memcpy(io_cap, &smp->prsp[1], 3); + } + + memset(r, 0, sizeof(r)); + + if (smp->method == REQ_PASSKEY || smp->method == DSP_PASSKEY) + put_unaligned_le32(hcon->passkey_notify, r); + + if (smp->method == REQ_OOB) + memcpy(r, smp->rr, 16); + + smp_f6(smp->tfm_cmac, smp->mackey, smp->prnd, smp->rrnd, r, io_cap, + local_addr, remote_addr, check.e); + + smp_send_cmd(smp->conn, SMP_CMD_DHKEY_CHECK, sizeof(check), &check); +} + +static u8 sc_passkey_send_confirm(struct smp_chan *smp) +{ + struct l2cap_conn *conn = smp->conn; + struct hci_conn *hcon = conn->hcon; + struct smp_cmd_pairing_confirm cfm; + u8 r; + + r = ((hcon->passkey_notify >> smp->passkey_round) & 0x01); + r |= 0x80; + + get_random_bytes(smp->prnd, sizeof(smp->prnd)); + + if (smp_f4(smp->tfm_cmac, smp->local_pk, smp->remote_pk, smp->prnd, r, + cfm.confirm_val)) + return SMP_UNSPECIFIED; + + smp_send_cmd(conn, SMP_CMD_PAIRING_CONFIRM, sizeof(cfm), &cfm); + + return 0; +} + +static u8 sc_passkey_round(struct smp_chan *smp, u8 smp_op) +{ + struct l2cap_conn *conn = smp->conn; + struct hci_conn *hcon = conn->hcon; + struct hci_dev *hdev = hcon->hdev; + u8 cfm[16], r; + + /* Ignore the PDU if we've already done 20 rounds (0 - 19) */ + if (smp->passkey_round >= 20) + return 0; + + switch (smp_op) { + case SMP_CMD_PAIRING_RANDOM: + r = ((hcon->passkey_notify >> smp->passkey_round) & 0x01); + r |= 0x80; + + if (smp_f4(smp->tfm_cmac, smp->remote_pk, smp->local_pk, + smp->rrnd, r, cfm)) + return SMP_UNSPECIFIED; + + if (memcmp(smp->pcnf, cfm, 16)) + return SMP_CONFIRM_FAILED; + + smp->passkey_round++; + + if (smp->passkey_round == 20) { + /* Generate MacKey and LTK */ + if (sc_mackey_and_ltk(smp, smp->mackey, smp->tk)) + return SMP_UNSPECIFIED; + } + + /* The round is only complete when the initiator + * receives pairing random. + */ + if (!hcon->out) { + smp_send_cmd(conn, SMP_CMD_PAIRING_RANDOM, + sizeof(smp->prnd), smp->prnd); + if (smp->passkey_round == 20) + SMP_ALLOW_CMD(smp, SMP_CMD_DHKEY_CHECK); + else + SMP_ALLOW_CMD(smp, SMP_CMD_PAIRING_CONFIRM); + return 0; + } + + /* Start the next round */ + if (smp->passkey_round != 20) + return sc_passkey_round(smp, 0); + + /* Passkey rounds are complete - start DHKey Check */ + sc_dhkey_check(smp); + SMP_ALLOW_CMD(smp, SMP_CMD_DHKEY_CHECK); + + break; + + case SMP_CMD_PAIRING_CONFIRM: + if (test_bit(SMP_FLAG_WAIT_USER, &smp->flags)) { + set_bit(SMP_FLAG_CFM_PENDING, &smp->flags); + return 0; + } + + SMP_ALLOW_CMD(smp, SMP_CMD_PAIRING_RANDOM); + + if (hcon->out) { + smp_send_cmd(conn, SMP_CMD_PAIRING_RANDOM, + sizeof(smp->prnd), smp->prnd); + return 0; + } + + return sc_passkey_send_confirm(smp); + + case SMP_CMD_PUBLIC_KEY: + default: + /* Initiating device starts the round */ + if (!hcon->out) + return 0; + + BT_DBG("%s Starting passkey round %u", hdev->name, + smp->passkey_round + 1); + + SMP_ALLOW_CMD(smp, SMP_CMD_PAIRING_CONFIRM); + + return sc_passkey_send_confirm(smp); + } + + return 0; +} + +static int sc_user_reply(struct smp_chan *smp, u16 mgmt_op, __le32 passkey) +{ + struct l2cap_conn *conn = smp->conn; + struct hci_conn *hcon = conn->hcon; + u8 smp_op; + + clear_bit(SMP_FLAG_WAIT_USER, &smp->flags); + + switch (mgmt_op) { + case MGMT_OP_USER_PASSKEY_NEG_REPLY: + smp_failure(smp->conn, SMP_PASSKEY_ENTRY_FAILED); + return 0; + case MGMT_OP_USER_CONFIRM_NEG_REPLY: + smp_failure(smp->conn, SMP_NUMERIC_COMP_FAILED); + return 0; + case MGMT_OP_USER_PASSKEY_REPLY: + hcon->passkey_notify = le32_to_cpu(passkey); + smp->passkey_round = 0; + + if (test_and_clear_bit(SMP_FLAG_CFM_PENDING, &smp->flags)) + smp_op = SMP_CMD_PAIRING_CONFIRM; + else + smp_op = 0; + + if (sc_passkey_round(smp, smp_op)) + return -EIO; + + return 0; + } + + /* Initiator sends DHKey check first */ + if (hcon->out) { + sc_dhkey_check(smp); + SMP_ALLOW_CMD(smp, SMP_CMD_DHKEY_CHECK); + } else if (test_and_clear_bit(SMP_FLAG_DHKEY_PENDING, &smp->flags)) { + sc_dhkey_check(smp); + sc_add_ltk(smp); + } + + return 0; +} + +int smp_user_confirm_reply(struct hci_conn *hcon, u16 mgmt_op, __le32 passkey) +{ + struct l2cap_conn *conn = hcon->l2cap_data; + struct l2cap_chan *chan; + struct smp_chan *smp; + u32 value; + int err; + + BT_DBG(""); + + if (!conn) + return -ENOTCONN; + + chan = conn->smp; + if (!chan) + return -ENOTCONN; + + l2cap_chan_lock(chan); + if (!chan->data) { + err = -ENOTCONN; + goto unlock; + } + + smp = chan->data; + + if (test_bit(SMP_FLAG_SC, &smp->flags)) { + err = sc_user_reply(smp, mgmt_op, passkey); + goto unlock; + } + + switch (mgmt_op) { + case MGMT_OP_USER_PASSKEY_REPLY: + value = le32_to_cpu(passkey); + memset(smp->tk, 0, sizeof(smp->tk)); + BT_DBG("PassKey: %d", value); + put_unaligned_le32(value, smp->tk); + /* Fall Through */ + case MGMT_OP_USER_CONFIRM_REPLY: + set_bit(SMP_FLAG_TK_VALID, &smp->flags); + break; + case MGMT_OP_USER_PASSKEY_NEG_REPLY: + case MGMT_OP_USER_CONFIRM_NEG_REPLY: + smp_failure(conn, SMP_PASSKEY_ENTRY_FAILED); + err = 0; + goto unlock; + default: + smp_failure(conn, SMP_PASSKEY_ENTRY_FAILED); + err = -EOPNOTSUPP; + goto unlock; + } + + err = 0; + + /* If it is our turn to send Pairing Confirm, do so now */ + if (test_bit(SMP_FLAG_CFM_PENDING, &smp->flags)) { + u8 rsp = smp_confirm(smp); + if (rsp) + smp_failure(conn, rsp); + } + +unlock: + l2cap_chan_unlock(chan); + return err; +} + +static void build_bredr_pairing_cmd(struct smp_chan *smp, + struct smp_cmd_pairing *req, + struct smp_cmd_pairing *rsp) +{ + struct l2cap_conn *conn = smp->conn; + struct hci_dev *hdev = conn->hcon->hdev; + u8 local_dist = 0, remote_dist = 0; + + if (hci_dev_test_flag(hdev, HCI_BONDABLE)) { + local_dist = SMP_DIST_ENC_KEY | SMP_DIST_SIGN; + remote_dist = SMP_DIST_ENC_KEY | SMP_DIST_SIGN; + } + + if (hci_dev_test_flag(hdev, HCI_RPA_RESOLVING)) + remote_dist |= SMP_DIST_ID_KEY; + + if (hci_dev_test_flag(hdev, HCI_PRIVACY)) + local_dist |= SMP_DIST_ID_KEY; + + if (!rsp) { + memset(req, 0, sizeof(*req)); + + req->init_key_dist = local_dist; + req->resp_key_dist = remote_dist; + req->max_key_size = SMP_MAX_ENC_KEY_SIZE; + + smp->remote_key_dist = remote_dist; + + return; + } + + memset(rsp, 0, sizeof(*rsp)); + + rsp->max_key_size = SMP_MAX_ENC_KEY_SIZE; + rsp->init_key_dist = req->init_key_dist & remote_dist; + rsp->resp_key_dist = req->resp_key_dist & local_dist; + + smp->remote_key_dist = rsp->init_key_dist; +} + +static u8 smp_cmd_pairing_req(struct l2cap_conn *conn, struct sk_buff *skb) +{ + struct smp_cmd_pairing rsp, *req = (void *) skb->data; + struct l2cap_chan *chan = conn->smp; + struct hci_dev *hdev = conn->hcon->hdev; + struct smp_chan *smp; + u8 key_size, auth, sec_level; + int ret; + + BT_DBG("conn %p", conn); + + if (skb->len < sizeof(*req)) + return SMP_INVALID_PARAMS; + + if (conn->hcon->role != HCI_ROLE_SLAVE) + return SMP_CMD_NOTSUPP; + + if (!chan->data) + smp = smp_chan_create(conn); + else + smp = chan->data; + + if (!smp) + return SMP_UNSPECIFIED; + + /* We didn't start the pairing, so match remote */ + auth = req->auth_req & AUTH_REQ_MASK(hdev); + + if (!hci_dev_test_flag(hdev, HCI_BONDABLE) && + (auth & SMP_AUTH_BONDING)) + return SMP_PAIRING_NOTSUPP; + + if (hci_dev_test_flag(hdev, HCI_SC_ONLY) && !(auth & SMP_AUTH_SC)) + return SMP_AUTH_REQUIREMENTS; + + smp->preq[0] = SMP_CMD_PAIRING_REQ; + memcpy(&smp->preq[1], req, sizeof(*req)); + skb_pull(skb, sizeof(*req)); + + /* If the remote side's OOB flag is set it means it has + * successfully received our local OOB data - therefore set the + * flag to indicate that local OOB is in use. + */ + if (req->oob_flag == SMP_OOB_PRESENT) + set_bit(SMP_FLAG_LOCAL_OOB, &smp->flags); + + /* SMP over BR/EDR requires special treatment */ + if (conn->hcon->type == ACL_LINK) { + /* We must have a BR/EDR SC link */ + if (!test_bit(HCI_CONN_AES_CCM, &conn->hcon->flags) && + !hci_dev_test_flag(hdev, HCI_FORCE_BREDR_SMP)) + return SMP_CROSS_TRANSP_NOT_ALLOWED; + + set_bit(SMP_FLAG_SC, &smp->flags); + + build_bredr_pairing_cmd(smp, req, &rsp); + + key_size = min(req->max_key_size, rsp.max_key_size); + if (check_enc_key_size(conn, key_size)) + return SMP_ENC_KEY_SIZE; + + /* Clear bits which are generated but not distributed */ + smp->remote_key_dist &= ~SMP_SC_NO_DIST; + + smp->prsp[0] = SMP_CMD_PAIRING_RSP; + memcpy(&smp->prsp[1], &rsp, sizeof(rsp)); + smp_send_cmd(conn, SMP_CMD_PAIRING_RSP, sizeof(rsp), &rsp); + + smp_distribute_keys(smp); + return 0; + } + + build_pairing_cmd(conn, req, &rsp, auth); + + if (rsp.auth_req & SMP_AUTH_SC) + set_bit(SMP_FLAG_SC, &smp->flags); + + if (conn->hcon->io_capability == HCI_IO_NO_INPUT_OUTPUT) + sec_level = BT_SECURITY_MEDIUM; + else + sec_level = authreq_to_seclevel(auth); + + if (sec_level > conn->hcon->pending_sec_level) + conn->hcon->pending_sec_level = sec_level; + + /* If we need MITM check that it can be achieved */ + if (conn->hcon->pending_sec_level >= BT_SECURITY_HIGH) { + u8 method; + + method = get_auth_method(smp, conn->hcon->io_capability, + req->io_capability); + if (method == JUST_WORKS || method == JUST_CFM) + return SMP_AUTH_REQUIREMENTS; + } + + key_size = min(req->max_key_size, rsp.max_key_size); + if (check_enc_key_size(conn, key_size)) + return SMP_ENC_KEY_SIZE; + + get_random_bytes(smp->prnd, sizeof(smp->prnd)); + + smp->prsp[0] = SMP_CMD_PAIRING_RSP; + memcpy(&smp->prsp[1], &rsp, sizeof(rsp)); + + smp_send_cmd(conn, SMP_CMD_PAIRING_RSP, sizeof(rsp), &rsp); + + clear_bit(SMP_FLAG_INITIATOR, &smp->flags); + + /* Strictly speaking we shouldn't allow Pairing Confirm for the + * SC case, however some implementations incorrectly copy RFU auth + * req bits from our security request, which may create a false + * positive SC enablement. + */ + SMP_ALLOW_CMD(smp, SMP_CMD_PAIRING_CONFIRM); + + if (test_bit(SMP_FLAG_SC, &smp->flags)) { + SMP_ALLOW_CMD(smp, SMP_CMD_PUBLIC_KEY); + /* Clear bits which are generated but not distributed */ + smp->remote_key_dist &= ~SMP_SC_NO_DIST; + /* Wait for Public Key from Initiating Device */ + return 0; + } + + /* Request setup of TK */ + ret = tk_request(conn, 0, auth, rsp.io_capability, req->io_capability); + if (ret) + return SMP_UNSPECIFIED; + + return 0; +} + +static u8 sc_send_public_key(struct smp_chan *smp) +{ + struct hci_dev *hdev = smp->conn->hcon->hdev; + + BT_DBG(""); + + if (test_bit(SMP_FLAG_LOCAL_OOB, &smp->flags)) { + struct l2cap_chan *chan = hdev->smp_data; + struct smp_dev *smp_dev; + + if (!chan || !chan->data) + return SMP_UNSPECIFIED; + + smp_dev = chan->data; + + memcpy(smp->local_pk, smp_dev->local_pk, 64); + memcpy(smp->local_sk, smp_dev->local_sk, 32); + memcpy(smp->lr, smp_dev->local_rand, 16); + + if (smp_dev->debug_key) + set_bit(SMP_FLAG_DEBUG_KEY, &smp->flags); + + goto done; + } + + if (hci_dev_test_flag(hdev, HCI_USE_DEBUG_KEYS)) { + BT_DBG("Using debug keys"); + memcpy(smp->local_pk, debug_pk, 64); + memcpy(smp->local_sk, debug_sk, 32); + set_bit(SMP_FLAG_DEBUG_KEY, &smp->flags); + } else { + while (true) { + /* Generate local key pair for Secure Connections */ + if (!ecc_make_key(smp->local_pk, smp->local_sk)) + return SMP_UNSPECIFIED; + + /* This is unlikely, but we need to check that + * we didn't accidentially generate a debug key. + */ + if (memcmp(smp->local_sk, debug_sk, 32)) + break; + } + } + +done: + SMP_DBG("Local Public Key X: %32phN", smp->local_pk); + SMP_DBG("Local Public Key Y: %32phN", smp->local_pk + 32); + SMP_DBG("Local Private Key: %32phN", smp->local_sk); + + smp_send_cmd(smp->conn, SMP_CMD_PUBLIC_KEY, 64, smp->local_pk); + + return 0; +} + +static u8 smp_cmd_pairing_rsp(struct l2cap_conn *conn, struct sk_buff *skb) +{ + struct smp_cmd_pairing *req, *rsp = (void *) skb->data; + struct l2cap_chan *chan = conn->smp; + struct smp_chan *smp = chan->data; + struct hci_dev *hdev = conn->hcon->hdev; + u8 key_size, auth; + int ret; + + BT_DBG("conn %p", conn); + + if (skb->len < sizeof(*rsp)) + return SMP_INVALID_PARAMS; + + if (conn->hcon->role != HCI_ROLE_MASTER) + return SMP_CMD_NOTSUPP; + + skb_pull(skb, sizeof(*rsp)); + + req = (void *) &smp->preq[1]; + + key_size = min(req->max_key_size, rsp->max_key_size); + if (check_enc_key_size(conn, key_size)) + return SMP_ENC_KEY_SIZE; + + auth = rsp->auth_req & AUTH_REQ_MASK(hdev); + + if (hci_dev_test_flag(hdev, HCI_SC_ONLY) && !(auth & SMP_AUTH_SC)) + return SMP_AUTH_REQUIREMENTS; + + /* If the remote side's OOB flag is set it means it has + * successfully received our local OOB data - therefore set the + * flag to indicate that local OOB is in use. + */ + if (rsp->oob_flag == SMP_OOB_PRESENT) + set_bit(SMP_FLAG_LOCAL_OOB, &smp->flags); + + smp->prsp[0] = SMP_CMD_PAIRING_RSP; + memcpy(&smp->prsp[1], rsp, sizeof(*rsp)); + + /* Update remote key distribution in case the remote cleared + * some bits that we had enabled in our request. + */ + smp->remote_key_dist &= rsp->resp_key_dist; + + /* For BR/EDR this means we're done and can start phase 3 */ + if (conn->hcon->type == ACL_LINK) { + /* Clear bits which are generated but not distributed */ + smp->remote_key_dist &= ~SMP_SC_NO_DIST; + smp_distribute_keys(smp); + return 0; + } + + if ((req->auth_req & SMP_AUTH_SC) && (auth & SMP_AUTH_SC)) + set_bit(SMP_FLAG_SC, &smp->flags); + else if (conn->hcon->pending_sec_level > BT_SECURITY_HIGH) + conn->hcon->pending_sec_level = BT_SECURITY_HIGH; + + /* If we need MITM check that it can be achieved */ + if (conn->hcon->pending_sec_level >= BT_SECURITY_HIGH) { + u8 method; + + method = get_auth_method(smp, req->io_capability, + rsp->io_capability); + if (method == JUST_WORKS || method == JUST_CFM) + return SMP_AUTH_REQUIREMENTS; + } + + get_random_bytes(smp->prnd, sizeof(smp->prnd)); + + /* Update remote key distribution in case the remote cleared + * some bits that we had enabled in our request. + */ + smp->remote_key_dist &= rsp->resp_key_dist; + + if (test_bit(SMP_FLAG_SC, &smp->flags)) { + /* Clear bits which are generated but not distributed */ + smp->remote_key_dist &= ~SMP_SC_NO_DIST; + SMP_ALLOW_CMD(smp, SMP_CMD_PUBLIC_KEY); + return sc_send_public_key(smp); + } + + auth |= req->auth_req; + + ret = tk_request(conn, 0, auth, req->io_capability, rsp->io_capability); + if (ret) + return SMP_UNSPECIFIED; + + set_bit(SMP_FLAG_CFM_PENDING, &smp->flags); + + /* Can't compose response until we have been confirmed */ + if (test_bit(SMP_FLAG_TK_VALID, &smp->flags)) + return smp_confirm(smp); + + return 0; +} + +static u8 sc_check_confirm(struct smp_chan *smp) +{ + struct l2cap_conn *conn = smp->conn; + + BT_DBG(""); + + if (smp->method == REQ_PASSKEY || smp->method == DSP_PASSKEY) + return sc_passkey_round(smp, SMP_CMD_PAIRING_CONFIRM); + + if (conn->hcon->out) { + smp_send_cmd(conn, SMP_CMD_PAIRING_RANDOM, sizeof(smp->prnd), + smp->prnd); + SMP_ALLOW_CMD(smp, SMP_CMD_PAIRING_RANDOM); + } + + return 0; +} + +/* Work-around for some implementations that incorrectly copy RFU bits + * from our security request and thereby create the impression that + * we're doing SC when in fact the remote doesn't support it. + */ +static int fixup_sc_false_positive(struct smp_chan *smp) +{ + struct l2cap_conn *conn = smp->conn; + struct hci_conn *hcon = conn->hcon; + struct hci_dev *hdev = hcon->hdev; + struct smp_cmd_pairing *req, *rsp; + u8 auth; + + /* The issue is only observed when we're in slave role */ + if (hcon->out) + return SMP_UNSPECIFIED; + + if (hci_dev_test_flag(hdev, HCI_SC_ONLY)) { + BT_ERR("Refusing SMP SC -> legacy fallback in SC-only mode"); + return SMP_UNSPECIFIED; + } + + BT_ERR("Trying to fall back to legacy SMP"); + + req = (void *) &smp->preq[1]; + rsp = (void *) &smp->prsp[1]; + + /* Rebuild key dist flags which may have been cleared for SC */ + smp->remote_key_dist = (req->init_key_dist & rsp->resp_key_dist); + + auth = req->auth_req & AUTH_REQ_MASK(hdev); + + if (tk_request(conn, 0, auth, rsp->io_capability, req->io_capability)) { + BT_ERR("Failed to fall back to legacy SMP"); + return SMP_UNSPECIFIED; + } + + clear_bit(SMP_FLAG_SC, &smp->flags); + + return 0; +} + +static u8 smp_cmd_pairing_confirm(struct l2cap_conn *conn, struct sk_buff *skb) +{ + struct l2cap_chan *chan = conn->smp; + struct smp_chan *smp = chan->data; + + BT_DBG("conn %p %s", conn, conn->hcon->out ? "master" : "slave"); + + if (skb->len < sizeof(smp->pcnf)) + return SMP_INVALID_PARAMS; + + memcpy(smp->pcnf, skb->data, sizeof(smp->pcnf)); + skb_pull(skb, sizeof(smp->pcnf)); + + if (test_bit(SMP_FLAG_SC, &smp->flags)) { + int ret; + + /* Public Key exchange must happen before any other steps */ + if (test_bit(SMP_FLAG_REMOTE_PK, &smp->flags)) + return sc_check_confirm(smp); + + BT_ERR("Unexpected SMP Pairing Confirm"); + + ret = fixup_sc_false_positive(smp); + if (ret) + return ret; + } + + if (conn->hcon->out) { + smp_send_cmd(conn, SMP_CMD_PAIRING_RANDOM, sizeof(smp->prnd), + smp->prnd); + SMP_ALLOW_CMD(smp, SMP_CMD_PAIRING_RANDOM); + return 0; + } + + if (test_bit(SMP_FLAG_TK_VALID, &smp->flags)) + return smp_confirm(smp); + + set_bit(SMP_FLAG_CFM_PENDING, &smp->flags); + + return 0; +} + +static u8 smp_cmd_pairing_random(struct l2cap_conn *conn, struct sk_buff *skb) +{ + struct l2cap_chan *chan = conn->smp; + struct smp_chan *smp = chan->data; + struct hci_conn *hcon = conn->hcon; + u8 *pkax, *pkbx, *na, *nb; + u32 passkey; + int err; + + BT_DBG("conn %p", conn); + + if (skb->len < sizeof(smp->rrnd)) + return SMP_INVALID_PARAMS; + + memcpy(smp->rrnd, skb->data, sizeof(smp->rrnd)); + skb_pull(skb, sizeof(smp->rrnd)); + + if (!test_bit(SMP_FLAG_SC, &smp->flags)) + return smp_random(smp); + + if (hcon->out) { + pkax = smp->local_pk; + pkbx = smp->remote_pk; + na = smp->prnd; + nb = smp->rrnd; + } else { + pkax = smp->remote_pk; + pkbx = smp->local_pk; + na = smp->rrnd; + nb = smp->prnd; + } + + if (smp->method == REQ_OOB) { + if (!hcon->out) + smp_send_cmd(conn, SMP_CMD_PAIRING_RANDOM, + sizeof(smp->prnd), smp->prnd); + SMP_ALLOW_CMD(smp, SMP_CMD_DHKEY_CHECK); + goto mackey_and_ltk; + } + + /* Passkey entry has special treatment */ + if (smp->method == REQ_PASSKEY || smp->method == DSP_PASSKEY) + return sc_passkey_round(smp, SMP_CMD_PAIRING_RANDOM); + + if (hcon->out) { + u8 cfm[16]; + + err = smp_f4(smp->tfm_cmac, smp->remote_pk, smp->local_pk, + smp->rrnd, 0, cfm); + if (err) + return SMP_UNSPECIFIED; + + if (memcmp(smp->pcnf, cfm, 16)) + return SMP_CONFIRM_FAILED; + } else { + smp_send_cmd(conn, SMP_CMD_PAIRING_RANDOM, sizeof(smp->prnd), + smp->prnd); + SMP_ALLOW_CMD(smp, SMP_CMD_DHKEY_CHECK); + } + +mackey_and_ltk: + /* Generate MacKey and LTK */ + err = sc_mackey_and_ltk(smp, smp->mackey, smp->tk); + if (err) + return SMP_UNSPECIFIED; + + if (smp->method == JUST_WORKS || smp->method == REQ_OOB) { + if (hcon->out) { + sc_dhkey_check(smp); + SMP_ALLOW_CMD(smp, SMP_CMD_DHKEY_CHECK); + } + return 0; + } + + err = smp_g2(smp->tfm_cmac, pkax, pkbx, na, nb, &passkey); + if (err) + return SMP_UNSPECIFIED; + + err = mgmt_user_confirm_request(hcon->hdev, &hcon->dst, hcon->type, + hcon->dst_type, passkey, 0); + if (err) + return SMP_UNSPECIFIED; + + set_bit(SMP_FLAG_WAIT_USER, &smp->flags); + + return 0; +} + +static bool smp_ltk_encrypt(struct l2cap_conn *conn, u8 sec_level) +{ + struct smp_ltk *key; + struct hci_conn *hcon = conn->hcon; + + key = hci_find_ltk(hcon->hdev, &hcon->dst, hcon->dst_type, hcon->role); + if (!key) + return false; + + if (smp_ltk_sec_level(key) < sec_level) + return false; + + if (test_and_set_bit(HCI_CONN_ENCRYPT_PEND, &hcon->flags)) + return true; + + hci_le_start_enc(hcon, key->ediv, key->rand, key->val); + hcon->enc_key_size = key->enc_size; + + /* We never store STKs for master role, so clear this flag */ + clear_bit(HCI_CONN_STK_ENCRYPT, &hcon->flags); + + return true; +} + +bool smp_sufficient_security(struct hci_conn *hcon, u8 sec_level, + enum smp_key_pref key_pref) +{ + if (sec_level == BT_SECURITY_LOW) + return true; + + /* If we're encrypted with an STK but the caller prefers using + * LTK claim insufficient security. This way we allow the + * connection to be re-encrypted with an LTK, even if the LTK + * provides the same level of security. Only exception is if we + * don't have an LTK (e.g. because of key distribution bits). + */ + if (key_pref == SMP_USE_LTK && + test_bit(HCI_CONN_STK_ENCRYPT, &hcon->flags) && + hci_find_ltk(hcon->hdev, &hcon->dst, hcon->dst_type, hcon->role)) + return false; + + if (hcon->sec_level >= sec_level) + return true; + + return false; +} + +static u8 smp_cmd_security_req(struct l2cap_conn *conn, struct sk_buff *skb) +{ + struct smp_cmd_security_req *rp = (void *) skb->data; + struct smp_cmd_pairing cp; + struct hci_conn *hcon = conn->hcon; + struct hci_dev *hdev = hcon->hdev; + struct smp_chan *smp; + u8 sec_level, auth; + + BT_DBG("conn %p", conn); + + if (skb->len < sizeof(*rp)) + return SMP_INVALID_PARAMS; + + if (hcon->role != HCI_ROLE_MASTER) + return SMP_CMD_NOTSUPP; + + auth = rp->auth_req & AUTH_REQ_MASK(hdev); + + if (hci_dev_test_flag(hdev, HCI_SC_ONLY) && !(auth & SMP_AUTH_SC)) + return SMP_AUTH_REQUIREMENTS; + + if (hcon->io_capability == HCI_IO_NO_INPUT_OUTPUT) + sec_level = BT_SECURITY_MEDIUM; + else + sec_level = authreq_to_seclevel(auth); + + if (smp_sufficient_security(hcon, sec_level, SMP_USE_LTK)) + return 0; + + if (sec_level > hcon->pending_sec_level) + hcon->pending_sec_level = sec_level; + + if (smp_ltk_encrypt(conn, hcon->pending_sec_level)) + return 0; + + smp = smp_chan_create(conn); + if (!smp) + return SMP_UNSPECIFIED; + + if (!hci_dev_test_flag(hdev, HCI_BONDABLE) && + (auth & SMP_AUTH_BONDING)) + return SMP_PAIRING_NOTSUPP; + + skb_pull(skb, sizeof(*rp)); + + memset(&cp, 0, sizeof(cp)); + build_pairing_cmd(conn, &cp, NULL, auth); + + smp->preq[0] = SMP_CMD_PAIRING_REQ; + memcpy(&smp->preq[1], &cp, sizeof(cp)); + + smp_send_cmd(conn, SMP_CMD_PAIRING_REQ, sizeof(cp), &cp); + SMP_ALLOW_CMD(smp, SMP_CMD_PAIRING_RSP); + + return 0; +} + +int smp_conn_security(struct hci_conn *hcon, __u8 sec_level) +{ + struct l2cap_conn *conn = hcon->l2cap_data; + struct l2cap_chan *chan; + struct smp_chan *smp; + __u8 authreq; + int ret; + + BT_DBG("conn %p hcon %p level 0x%2.2x", conn, hcon, sec_level); + + /* This may be NULL if there's an unexpected disconnection */ + if (!conn) + return 1; + + chan = conn->smp; + + if (!hci_dev_test_flag(hcon->hdev, HCI_LE_ENABLED)) + return 1; + + if (smp_sufficient_security(hcon, sec_level, SMP_USE_LTK)) + return 1; + + if (sec_level > hcon->pending_sec_level) + hcon->pending_sec_level = sec_level; + + if (hcon->role == HCI_ROLE_MASTER) + if (smp_ltk_encrypt(conn, hcon->pending_sec_level)) + return 0; + + l2cap_chan_lock(chan); + + /* If SMP is already in progress ignore this request */ + if (chan->data) { + ret = 0; + goto unlock; + } + + smp = smp_chan_create(conn); + if (!smp) { + ret = 1; + goto unlock; + } + + authreq = seclevel_to_authreq(sec_level); + + if (hci_dev_test_flag(hcon->hdev, HCI_SC_ENABLED)) + authreq |= SMP_AUTH_SC; + + /* Require MITM if IO Capability allows or the security level + * requires it. + */ + if (hcon->io_capability != HCI_IO_NO_INPUT_OUTPUT || + hcon->pending_sec_level > BT_SECURITY_MEDIUM) + authreq |= SMP_AUTH_MITM; + + if (hcon->role == HCI_ROLE_MASTER) { + struct smp_cmd_pairing cp; + + build_pairing_cmd(conn, &cp, NULL, authreq); + smp->preq[0] = SMP_CMD_PAIRING_REQ; + memcpy(&smp->preq[1], &cp, sizeof(cp)); + + smp_send_cmd(conn, SMP_CMD_PAIRING_REQ, sizeof(cp), &cp); + SMP_ALLOW_CMD(smp, SMP_CMD_PAIRING_RSP); + } else { + struct smp_cmd_security_req cp; + cp.auth_req = authreq; + smp_send_cmd(conn, SMP_CMD_SECURITY_REQ, sizeof(cp), &cp); + SMP_ALLOW_CMD(smp, SMP_CMD_PAIRING_REQ); + } + + set_bit(SMP_FLAG_INITIATOR, &smp->flags); + ret = 0; + +unlock: + l2cap_chan_unlock(chan); + return ret; +} + +static int smp_cmd_encrypt_info(struct l2cap_conn *conn, struct sk_buff *skb) +{ + struct smp_cmd_encrypt_info *rp = (void *) skb->data; + struct l2cap_chan *chan = conn->smp; + struct smp_chan *smp = chan->data; + + BT_DBG("conn %p", conn); + + if (skb->len < sizeof(*rp)) + return SMP_INVALID_PARAMS; + + SMP_ALLOW_CMD(smp, SMP_CMD_MASTER_IDENT); + + skb_pull(skb, sizeof(*rp)); + + memcpy(smp->tk, rp->ltk, sizeof(smp->tk)); + + return 0; +} + +static int smp_cmd_master_ident(struct l2cap_conn *conn, struct sk_buff *skb) +{ + struct smp_cmd_master_ident *rp = (void *) skb->data; + struct l2cap_chan *chan = conn->smp; + struct smp_chan *smp = chan->data; + struct hci_dev *hdev = conn->hcon->hdev; + struct hci_conn *hcon = conn->hcon; + struct smp_ltk *ltk; + u8 authenticated; + + BT_DBG("conn %p", conn); + + if (skb->len < sizeof(*rp)) + return SMP_INVALID_PARAMS; + + /* Mark the information as received */ + smp->remote_key_dist &= ~SMP_DIST_ENC_KEY; + + if (smp->remote_key_dist & SMP_DIST_ID_KEY) + SMP_ALLOW_CMD(smp, SMP_CMD_IDENT_INFO); + else if (smp->remote_key_dist & SMP_DIST_SIGN) + SMP_ALLOW_CMD(smp, SMP_CMD_SIGN_INFO); + + skb_pull(skb, sizeof(*rp)); + + authenticated = (hcon->sec_level == BT_SECURITY_HIGH); + ltk = hci_add_ltk(hdev, &hcon->dst, hcon->dst_type, SMP_LTK, + authenticated, smp->tk, smp->enc_key_size, + rp->ediv, rp->rand); + smp->ltk = ltk; + if (!(smp->remote_key_dist & KEY_DIST_MASK)) + smp_distribute_keys(smp); + + return 0; +} + +static int smp_cmd_ident_info(struct l2cap_conn *conn, struct sk_buff *skb) +{ + struct smp_cmd_ident_info *info = (void *) skb->data; + struct l2cap_chan *chan = conn->smp; + struct smp_chan *smp = chan->data; + + BT_DBG(""); + + if (skb->len < sizeof(*info)) + return SMP_INVALID_PARAMS; + + SMP_ALLOW_CMD(smp, SMP_CMD_IDENT_ADDR_INFO); + + skb_pull(skb, sizeof(*info)); + + memcpy(smp->irk, info->irk, 16); + + return 0; +} + +static int smp_cmd_ident_addr_info(struct l2cap_conn *conn, + struct sk_buff *skb) +{ + struct smp_cmd_ident_addr_info *info = (void *) skb->data; + struct l2cap_chan *chan = conn->smp; + struct smp_chan *smp = chan->data; + struct hci_conn *hcon = conn->hcon; + bdaddr_t rpa; + + BT_DBG(""); + + if (skb->len < sizeof(*info)) + return SMP_INVALID_PARAMS; + + /* Mark the information as received */ + smp->remote_key_dist &= ~SMP_DIST_ID_KEY; + + if (smp->remote_key_dist & SMP_DIST_SIGN) + SMP_ALLOW_CMD(smp, SMP_CMD_SIGN_INFO); + + skb_pull(skb, sizeof(*info)); + + /* Strictly speaking the Core Specification (4.1) allows sending + * an empty address which would force us to rely on just the IRK + * as "identity information". However, since such + * implementations are not known of and in order to not over + * complicate our implementation, simply pretend that we never + * received an IRK for such a device. + * + * The Identity Address must also be a Static Random or Public + * Address, which hci_is_identity_address() checks for. + */ + if (!bacmp(&info->bdaddr, BDADDR_ANY) || + !hci_is_identity_address(&info->bdaddr, info->addr_type)) { + BT_ERR("Ignoring IRK with no identity address"); + goto distribute; + } + + bacpy(&smp->id_addr, &info->bdaddr); + smp->id_addr_type = info->addr_type; + + if (hci_bdaddr_is_rpa(&hcon->dst, hcon->dst_type)) + bacpy(&rpa, &hcon->dst); + else + bacpy(&rpa, BDADDR_ANY); + + smp->remote_irk = hci_add_irk(conn->hcon->hdev, &smp->id_addr, + smp->id_addr_type, smp->irk, &rpa); + +distribute: + if (!(smp->remote_key_dist & KEY_DIST_MASK)) + smp_distribute_keys(smp); + + return 0; +} + +static int smp_cmd_sign_info(struct l2cap_conn *conn, struct sk_buff *skb) +{ + struct smp_cmd_sign_info *rp = (void *) skb->data; + struct l2cap_chan *chan = conn->smp; + struct smp_chan *smp = chan->data; + struct smp_csrk *csrk; + + BT_DBG("conn %p", conn); + + if (skb->len < sizeof(*rp)) + return SMP_INVALID_PARAMS; + + /* Mark the information as received */ + smp->remote_key_dist &= ~SMP_DIST_SIGN; + + skb_pull(skb, sizeof(*rp)); + + csrk = kzalloc(sizeof(*csrk), GFP_KERNEL); + if (csrk) { + if (conn->hcon->sec_level > BT_SECURITY_MEDIUM) + csrk->type = MGMT_CSRK_REMOTE_AUTHENTICATED; + else + csrk->type = MGMT_CSRK_REMOTE_UNAUTHENTICATED; + memcpy(csrk->val, rp->csrk, sizeof(csrk->val)); + } + smp->csrk = csrk; + smp_distribute_keys(smp); + + return 0; +} + +static u8 sc_select_method(struct smp_chan *smp) +{ + struct l2cap_conn *conn = smp->conn; + struct hci_conn *hcon = conn->hcon; + struct smp_cmd_pairing *local, *remote; + u8 local_mitm, remote_mitm, local_io, remote_io, method; + + if (test_bit(SMP_FLAG_REMOTE_OOB, &smp->flags) || + test_bit(SMP_FLAG_LOCAL_OOB, &smp->flags)) + return REQ_OOB; + + /* The preq/prsp contain the raw Pairing Request/Response PDUs + * which are needed as inputs to some crypto functions. To get + * the "struct smp_cmd_pairing" from them we need to skip the + * first byte which contains the opcode. + */ + if (hcon->out) { + local = (void *) &smp->preq[1]; + remote = (void *) &smp->prsp[1]; + } else { + local = (void *) &smp->prsp[1]; + remote = (void *) &smp->preq[1]; + } + + local_io = local->io_capability; + remote_io = remote->io_capability; + + local_mitm = (local->auth_req & SMP_AUTH_MITM); + remote_mitm = (remote->auth_req & SMP_AUTH_MITM); + + /* If either side wants MITM, look up the method from the table, + * otherwise use JUST WORKS. + */ + if (local_mitm || remote_mitm) + method = get_auth_method(smp, local_io, remote_io); + else + method = JUST_WORKS; + + /* Don't confirm locally initiated pairing attempts */ + if (method == JUST_CFM && test_bit(SMP_FLAG_INITIATOR, &smp->flags)) + method = JUST_WORKS; + + return method; +} + +static int smp_cmd_public_key(struct l2cap_conn *conn, struct sk_buff *skb) +{ + struct smp_cmd_public_key *key = (void *) skb->data; + struct hci_conn *hcon = conn->hcon; + struct l2cap_chan *chan = conn->smp; + struct smp_chan *smp = chan->data; + struct hci_dev *hdev = hcon->hdev; + struct smp_cmd_pairing_confirm cfm; + int err; + + BT_DBG("conn %p", conn); + + if (skb->len < sizeof(*key)) + return SMP_INVALID_PARAMS; + + memcpy(smp->remote_pk, key, 64); + + if (test_bit(SMP_FLAG_REMOTE_OOB, &smp->flags)) { + err = smp_f4(smp->tfm_cmac, smp->remote_pk, smp->remote_pk, + smp->rr, 0, cfm.confirm_val); + if (err) + return SMP_UNSPECIFIED; + + if (memcmp(cfm.confirm_val, smp->pcnf, 16)) + return SMP_CONFIRM_FAILED; + } + + /* Non-initiating device sends its public key after receiving + * the key from the initiating device. + */ + if (!hcon->out) { + err = sc_send_public_key(smp); + if (err) + return err; + } + + SMP_DBG("Remote Public Key X: %32phN", smp->remote_pk); + SMP_DBG("Remote Public Key Y: %32phN", smp->remote_pk + 32); + + if (!ecdh_shared_secret(smp->remote_pk, smp->local_sk, smp->dhkey)) + return SMP_UNSPECIFIED; + + SMP_DBG("DHKey %32phN", smp->dhkey); + + set_bit(SMP_FLAG_REMOTE_PK, &smp->flags); + + smp->method = sc_select_method(smp); + + BT_DBG("%s selected method 0x%02x", hdev->name, smp->method); + + /* JUST_WORKS and JUST_CFM result in an unauthenticated key */ + if (smp->method == JUST_WORKS || smp->method == JUST_CFM) + hcon->pending_sec_level = BT_SECURITY_MEDIUM; + else + hcon->pending_sec_level = BT_SECURITY_FIPS; + + if (!memcmp(debug_pk, smp->remote_pk, 64)) + set_bit(SMP_FLAG_DEBUG_KEY, &smp->flags); + + if (smp->method == DSP_PASSKEY) { + get_random_bytes(&hcon->passkey_notify, + sizeof(hcon->passkey_notify)); + hcon->passkey_notify %= 1000000; + hcon->passkey_entered = 0; + smp->passkey_round = 0; + if (mgmt_user_passkey_notify(hdev, &hcon->dst, hcon->type, + hcon->dst_type, + hcon->passkey_notify, + hcon->passkey_entered)) + return SMP_UNSPECIFIED; + SMP_ALLOW_CMD(smp, SMP_CMD_PAIRING_CONFIRM); + return sc_passkey_round(smp, SMP_CMD_PUBLIC_KEY); + } + + if (smp->method == REQ_OOB) { + if (hcon->out) + smp_send_cmd(conn, SMP_CMD_PAIRING_RANDOM, + sizeof(smp->prnd), smp->prnd); + + SMP_ALLOW_CMD(smp, SMP_CMD_PAIRING_RANDOM); + + return 0; + } + + if (hcon->out) + SMP_ALLOW_CMD(smp, SMP_CMD_PAIRING_CONFIRM); + + if (smp->method == REQ_PASSKEY) { + if (mgmt_user_passkey_request(hdev, &hcon->dst, hcon->type, + hcon->dst_type)) + return SMP_UNSPECIFIED; + SMP_ALLOW_CMD(smp, SMP_CMD_PAIRING_CONFIRM); + set_bit(SMP_FLAG_WAIT_USER, &smp->flags); + return 0; + } + + /* The Initiating device waits for the non-initiating device to + * send the confirm value. + */ + if (conn->hcon->out) + return 0; + + err = smp_f4(smp->tfm_cmac, smp->local_pk, smp->remote_pk, smp->prnd, + 0, cfm.confirm_val); + if (err) + return SMP_UNSPECIFIED; + + smp_send_cmd(conn, SMP_CMD_PAIRING_CONFIRM, sizeof(cfm), &cfm); + SMP_ALLOW_CMD(smp, SMP_CMD_PAIRING_RANDOM); + + return 0; +} + +static int smp_cmd_dhkey_check(struct l2cap_conn *conn, struct sk_buff *skb) +{ + struct smp_cmd_dhkey_check *check = (void *) skb->data; + struct l2cap_chan *chan = conn->smp; + struct hci_conn *hcon = conn->hcon; + struct smp_chan *smp = chan->data; + u8 a[7], b[7], *local_addr, *remote_addr; + u8 io_cap[3], r[16], e[16]; + int err; + + BT_DBG("conn %p", conn); + + if (skb->len < sizeof(*check)) + return SMP_INVALID_PARAMS; + + memcpy(a, &hcon->init_addr, 6); + memcpy(b, &hcon->resp_addr, 6); + a[6] = hcon->init_addr_type; + b[6] = hcon->resp_addr_type; + + if (hcon->out) { + local_addr = a; + remote_addr = b; + memcpy(io_cap, &smp->prsp[1], 3); + } else { + local_addr = b; + remote_addr = a; + memcpy(io_cap, &smp->preq[1], 3); + } + + memset(r, 0, sizeof(r)); + + if (smp->method == REQ_PASSKEY || smp->method == DSP_PASSKEY) + put_unaligned_le32(hcon->passkey_notify, r); + else if (smp->method == REQ_OOB) + memcpy(r, smp->lr, 16); + + err = smp_f6(smp->tfm_cmac, smp->mackey, smp->rrnd, smp->prnd, r, + io_cap, remote_addr, local_addr, e); + if (err) + return SMP_UNSPECIFIED; + + if (memcmp(check->e, e, 16)) + return SMP_DHKEY_CHECK_FAILED; + + if (!hcon->out) { + if (test_bit(SMP_FLAG_WAIT_USER, &smp->flags)) { + set_bit(SMP_FLAG_DHKEY_PENDING, &smp->flags); + return 0; + } + + /* Slave sends DHKey check as response to master */ + sc_dhkey_check(smp); + } + + sc_add_ltk(smp); + + if (hcon->out) { + hci_le_start_enc(hcon, 0, 0, smp->tk); + hcon->enc_key_size = smp->enc_key_size; + } + + return 0; +} + +static int smp_cmd_keypress_notify(struct l2cap_conn *conn, + struct sk_buff *skb) +{ + struct smp_cmd_keypress_notify *kp = (void *) skb->data; + + BT_DBG("value 0x%02x", kp->value); + + return 0; +} + +static int smp_sig_channel(struct l2cap_chan *chan, struct sk_buff *skb) +{ + struct l2cap_conn *conn = chan->conn; + struct hci_conn *hcon = conn->hcon; + struct smp_chan *smp; + __u8 code, reason; + int err = 0; + + if (skb->len < 1) + return -EILSEQ; + + if (!hci_dev_test_flag(hcon->hdev, HCI_LE_ENABLED)) { + reason = SMP_PAIRING_NOTSUPP; + goto done; + } + + code = skb->data[0]; + skb_pull(skb, sizeof(code)); + + smp = chan->data; + + if (code > SMP_CMD_MAX) + goto drop; + + if (smp && !test_and_clear_bit(code, &smp->allow_cmd)) + goto drop; + + /* If we don't have a context the only allowed commands are + * pairing request and security request. + */ + if (!smp && code != SMP_CMD_PAIRING_REQ && code != SMP_CMD_SECURITY_REQ) + goto drop; + + switch (code) { + case SMP_CMD_PAIRING_REQ: + reason = smp_cmd_pairing_req(conn, skb); + break; + + case SMP_CMD_PAIRING_FAIL: + smp_failure(conn, 0); + err = -EPERM; + break; + + case SMP_CMD_PAIRING_RSP: + reason = smp_cmd_pairing_rsp(conn, skb); + break; + + case SMP_CMD_SECURITY_REQ: + reason = smp_cmd_security_req(conn, skb); + break; + + case SMP_CMD_PAIRING_CONFIRM: + reason = smp_cmd_pairing_confirm(conn, skb); + break; + + case SMP_CMD_PAIRING_RANDOM: + reason = smp_cmd_pairing_random(conn, skb); + break; + + case SMP_CMD_ENCRYPT_INFO: + reason = smp_cmd_encrypt_info(conn, skb); + break; + + case SMP_CMD_MASTER_IDENT: + reason = smp_cmd_master_ident(conn, skb); + break; + + case SMP_CMD_IDENT_INFO: + reason = smp_cmd_ident_info(conn, skb); + break; + + case SMP_CMD_IDENT_ADDR_INFO: + reason = smp_cmd_ident_addr_info(conn, skb); + break; + + case SMP_CMD_SIGN_INFO: + reason = smp_cmd_sign_info(conn, skb); + break; + + case SMP_CMD_PUBLIC_KEY: + reason = smp_cmd_public_key(conn, skb); + break; + + case SMP_CMD_DHKEY_CHECK: + reason = smp_cmd_dhkey_check(conn, skb); + break; + + case SMP_CMD_KEYPRESS_NOTIFY: + reason = smp_cmd_keypress_notify(conn, skb); + break; + + default: + BT_DBG("Unknown command code 0x%2.2x", code); + reason = SMP_CMD_NOTSUPP; + goto done; + } + +done: + if (!err) { + if (reason) + smp_failure(conn, reason); + kfree_skb(skb); + } + + return err; + +drop: + BT_ERR("%s unexpected SMP command 0x%02x from %pMR", hcon->hdev->name, + code, &hcon->dst); + kfree_skb(skb); + return 0; +} + +static void smp_teardown_cb(struct l2cap_chan *chan, int err) +{ + struct l2cap_conn *conn = chan->conn; + + BT_DBG("chan %p", chan); + + if (chan->data) + smp_chan_destroy(conn); + + conn->smp = NULL; + l2cap_chan_put(chan); +} + +static void bredr_pairing(struct l2cap_chan *chan) +{ + struct l2cap_conn *conn = chan->conn; + struct hci_conn *hcon = conn->hcon; + struct hci_dev *hdev = hcon->hdev; + struct smp_cmd_pairing req; + struct smp_chan *smp; + + BT_DBG("chan %p", chan); + + /* Only new pairings are interesting */ + if (!test_bit(HCI_CONN_NEW_LINK_KEY, &hcon->flags)) + return; + + /* Don't bother if we're not encrypted */ + if (!test_bit(HCI_CONN_ENCRYPT, &hcon->flags)) + return; + + /* Only master may initiate SMP over BR/EDR */ + if (hcon->role != HCI_ROLE_MASTER) + return; + + /* Secure Connections support must be enabled */ + if (!hci_dev_test_flag(hdev, HCI_SC_ENABLED)) + return; + + /* BR/EDR must use Secure Connections for SMP */ + if (!test_bit(HCI_CONN_AES_CCM, &hcon->flags) && + !hci_dev_test_flag(hdev, HCI_FORCE_BREDR_SMP)) + return; + + /* If our LE support is not enabled don't do anything */ + if (!hci_dev_test_flag(hdev, HCI_LE_ENABLED)) + return; + + /* Don't bother if remote LE support is not enabled */ + if (!lmp_host_le_capable(hcon)) + return; + + /* Remote must support SMP fixed chan for BR/EDR */ + if (!(conn->remote_fixed_chan & L2CAP_FC_SMP_BREDR)) + return; + + /* Don't bother if SMP is already ongoing */ + if (chan->data) + return; + + smp = smp_chan_create(conn); + if (!smp) { + BT_ERR("%s unable to create SMP context for BR/EDR", + hdev->name); + return; + } + + set_bit(SMP_FLAG_SC, &smp->flags); + + BT_DBG("%s starting SMP over BR/EDR", hdev->name); + + /* Prepare and send the BR/EDR SMP Pairing Request */ + build_bredr_pairing_cmd(smp, &req, NULL); + + smp->preq[0] = SMP_CMD_PAIRING_REQ; + memcpy(&smp->preq[1], &req, sizeof(req)); + + smp_send_cmd(conn, SMP_CMD_PAIRING_REQ, sizeof(req), &req); + SMP_ALLOW_CMD(smp, SMP_CMD_PAIRING_RSP); +} + +static void smp_resume_cb(struct l2cap_chan *chan) +{ + struct smp_chan *smp = chan->data; + struct l2cap_conn *conn = chan->conn; + struct hci_conn *hcon = conn->hcon; + + BT_DBG("chan %p", chan); + + if (hcon->type == ACL_LINK) { + bredr_pairing(chan); + return; + } + + if (!smp) + return; + + if (!test_bit(HCI_CONN_ENCRYPT, &hcon->flags)) + return; + + cancel_delayed_work(&smp->security_timer); + + smp_distribute_keys(smp); +} + +static void smp_ready_cb(struct l2cap_chan *chan) +{ + struct l2cap_conn *conn = chan->conn; + struct hci_conn *hcon = conn->hcon; + + BT_DBG("chan %p", chan); + + conn->smp = chan; + l2cap_chan_hold(chan); + + if (hcon->type == ACL_LINK && test_bit(HCI_CONN_ENCRYPT, &hcon->flags)) + bredr_pairing(chan); +} + +static int smp_recv_cb(struct l2cap_chan *chan, struct sk_buff *skb) +{ + int err; + + BT_DBG("chan %p", chan); + + err = smp_sig_channel(chan, skb); + if (err) { + struct smp_chan *smp = chan->data; + + if (smp) + cancel_delayed_work_sync(&smp->security_timer); + + hci_disconnect(chan->conn->hcon, HCI_ERROR_AUTH_FAILURE); + } + + return err; +} + +static struct sk_buff *smp_alloc_skb_cb(struct l2cap_chan *chan, + unsigned long hdr_len, + unsigned long len, int nb) +{ + struct sk_buff *skb; + + skb = bt_skb_alloc(hdr_len + len, GFP_KERNEL); + if (!skb) + return ERR_PTR(-ENOMEM); + + skb->priority = HCI_PRIO_MAX; + bt_cb(skb)->l2cap.chan = chan; + + return skb; +} + +static const struct l2cap_ops smp_chan_ops = { + .name = "Security Manager", + .ready = smp_ready_cb, + .recv = smp_recv_cb, + .alloc_skb = smp_alloc_skb_cb, + .teardown = smp_teardown_cb, + .resume = smp_resume_cb, + + .new_connection = l2cap_chan_no_new_connection, + .state_change = l2cap_chan_no_state_change, + .close = l2cap_chan_no_close, + .defer = l2cap_chan_no_defer, + .suspend = l2cap_chan_no_suspend, + .set_shutdown = l2cap_chan_no_set_shutdown, + .get_sndtimeo = l2cap_chan_no_get_sndtimeo, +}; + +static inline struct l2cap_chan *smp_new_conn_cb(struct l2cap_chan *pchan) +{ + struct l2cap_chan *chan; + + BT_DBG("pchan %p", pchan); + + chan = l2cap_chan_create(); + if (!chan) + return NULL; + + chan->chan_type = pchan->chan_type; + chan->ops = &smp_chan_ops; + chan->scid = pchan->scid; + chan->dcid = chan->scid; + chan->imtu = pchan->imtu; + chan->omtu = pchan->omtu; + chan->mode = pchan->mode; + + /* Other L2CAP channels may request SMP routines in order to + * change the security level. This means that the SMP channel + * lock must be considered in its own category to avoid lockdep + * warnings. + */ + atomic_set(&chan->nesting, L2CAP_NESTING_SMP); + + BT_DBG("created chan %p", chan); + + return chan; +} + +static const struct l2cap_ops smp_root_chan_ops = { + .name = "Security Manager Root", + .new_connection = smp_new_conn_cb, + + /* None of these are implemented for the root channel */ + .close = l2cap_chan_no_close, + .alloc_skb = l2cap_chan_no_alloc_skb, + .recv = l2cap_chan_no_recv, + .state_change = l2cap_chan_no_state_change, + .teardown = l2cap_chan_no_teardown, + .ready = l2cap_chan_no_ready, + .defer = l2cap_chan_no_defer, + .suspend = l2cap_chan_no_suspend, + .resume = l2cap_chan_no_resume, + .set_shutdown = l2cap_chan_no_set_shutdown, + .get_sndtimeo = l2cap_chan_no_get_sndtimeo, +}; + +static struct l2cap_chan *smp_add_cid(struct hci_dev *hdev, u16 cid) +{ + struct l2cap_chan *chan; + struct smp_dev *smp; + struct crypto_blkcipher *tfm_aes; + struct crypto_hash *tfm_cmac; + + if (cid == L2CAP_CID_SMP_BREDR) { + smp = NULL; + goto create_chan; + } + + smp = kzalloc(sizeof(*smp), GFP_KERNEL); + if (!smp) + return ERR_PTR(-ENOMEM); + + tfm_aes = crypto_alloc_blkcipher("ecb(aes)", 0, CRYPTO_ALG_ASYNC); + if (IS_ERR(tfm_aes)) { + BT_ERR("Unable to create ECB crypto context"); + kzfree(smp); + return ERR_CAST(tfm_aes); + } + + tfm_cmac = crypto_alloc_hash("cmac(aes)", 0, CRYPTO_ALG_ASYNC); + if (IS_ERR(tfm_cmac)) { + BT_ERR("Unable to create CMAC crypto context"); + crypto_free_blkcipher(tfm_aes); + kzfree(smp); + return ERR_CAST(tfm_cmac); + } + + smp->tfm_aes = tfm_aes; + smp->tfm_cmac = tfm_cmac; + +create_chan: + chan = l2cap_chan_create(); + if (!chan) { + if (smp) { + crypto_free_blkcipher(smp->tfm_aes); + crypto_free_hash(smp->tfm_cmac); + kzfree(smp); + } + return ERR_PTR(-ENOMEM); + } + + chan->data = smp; + + l2cap_add_scid(chan, cid); + + l2cap_chan_set_defaults(chan); + + if (cid == L2CAP_CID_SMP) { + u8 bdaddr_type; + + hci_copy_identity_address(hdev, &chan->src, &bdaddr_type); + + if (bdaddr_type == ADDR_LE_DEV_PUBLIC) + chan->src_type = BDADDR_LE_PUBLIC; + else + chan->src_type = BDADDR_LE_RANDOM; + } else { + bacpy(&chan->src, &hdev->bdaddr); + chan->src_type = BDADDR_BREDR; + } + + chan->state = BT_LISTEN; + chan->mode = L2CAP_MODE_BASIC; + chan->imtu = L2CAP_DEFAULT_MTU; + chan->ops = &smp_root_chan_ops; + + /* Set correct nesting level for a parent/listening channel */ + atomic_set(&chan->nesting, L2CAP_NESTING_PARENT); + + return chan; +} + +static void smp_del_chan(struct l2cap_chan *chan) +{ + struct smp_dev *smp; + + BT_DBG("chan %p", chan); + + smp = chan->data; + if (smp) { + chan->data = NULL; + if (smp->tfm_aes) + crypto_free_blkcipher(smp->tfm_aes); + if (smp->tfm_cmac) + crypto_free_hash(smp->tfm_cmac); + kzfree(smp); + } + + l2cap_chan_put(chan); +} + +static ssize_t force_bredr_smp_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] = hci_dev_test_flag(hdev, HCI_FORCE_BREDR_SMP) ? 'Y': 'N'; + buf[1] = '\n'; + buf[2] = '\0'; + return simple_read_from_buffer(user_buf, count, ppos, buf, 2); +} + +static ssize_t force_bredr_smp_write(struct file *file, + const char __user *user_buf, + size_t count, loff_t *ppos) +{ + struct hci_dev *hdev = file->private_data; + char buf[32]; + size_t buf_size = min(count, (sizeof(buf)-1)); + bool enable; + + if (copy_from_user(buf, user_buf, buf_size)) + return -EFAULT; + + buf[buf_size] = '\0'; + if (strtobool(buf, &enable)) + return -EINVAL; + + if (enable == hci_dev_test_flag(hdev, HCI_FORCE_BREDR_SMP)) + return -EALREADY; + + if (enable) { + struct l2cap_chan *chan; + + chan = smp_add_cid(hdev, L2CAP_CID_SMP_BREDR); + if (IS_ERR(chan)) + return PTR_ERR(chan); + + hdev->smp_bredr_data = chan; + } else { + struct l2cap_chan *chan; + + chan = hdev->smp_bredr_data; + hdev->smp_bredr_data = NULL; + smp_del_chan(chan); + } + + hci_dev_change_flag(hdev, HCI_FORCE_BREDR_SMP); + + return count; +} + +static const struct file_operations force_bredr_smp_fops = { + .open = simple_open, + .read = force_bredr_smp_read, + .write = force_bredr_smp_write, + .llseek = default_llseek, +}; + +int smp_register(struct hci_dev *hdev) +{ + struct l2cap_chan *chan; + + BT_DBG("%s", hdev->name); + + /* If the controller does not support Low Energy operation, then + * there is also no need to register any SMP channel. + */ + if (!lmp_le_capable(hdev)) + return 0; + + if (WARN_ON(hdev->smp_data)) { + chan = hdev->smp_data; + hdev->smp_data = NULL; + smp_del_chan(chan); + } + + chan = smp_add_cid(hdev, L2CAP_CID_SMP); + if (IS_ERR(chan)) + return PTR_ERR(chan); + + hdev->smp_data = chan; + + /* If the controller does not support BR/EDR Secure Connections + * feature, then the BR/EDR SMP channel shall not be present. + * + * To test this with Bluetooth 4.0 controllers, create a debugfs + * switch that allows forcing BR/EDR SMP support and accepting + * cross-transport pairing on non-AES encrypted connections. + */ + if (!lmp_sc_capable(hdev)) { + debugfs_create_file("force_bredr_smp", 0644, hdev->debugfs, + hdev, &force_bredr_smp_fops); + return 0; + } + + if (WARN_ON(hdev->smp_bredr_data)) { + chan = hdev->smp_bredr_data; + hdev->smp_bredr_data = NULL; + smp_del_chan(chan); + } + + chan = smp_add_cid(hdev, L2CAP_CID_SMP_BREDR); + if (IS_ERR(chan)) { + int err = PTR_ERR(chan); + chan = hdev->smp_data; + hdev->smp_data = NULL; + smp_del_chan(chan); + return err; + } + + hdev->smp_bredr_data = chan; + + return 0; +} + +void smp_unregister(struct hci_dev *hdev) +{ + struct l2cap_chan *chan; + + if (hdev->smp_bredr_data) { + chan = hdev->smp_bredr_data; + hdev->smp_bredr_data = NULL; + smp_del_chan(chan); + } + + if (hdev->smp_data) { + chan = hdev->smp_data; + hdev->smp_data = NULL; + smp_del_chan(chan); + } +} + +#if IS_ENABLED(CONFIG_BT_SELFTEST_SMP) + +static int __init test_ah(struct crypto_blkcipher *tfm_aes) +{ + const u8 irk[16] = { + 0x9b, 0x7d, 0x39, 0x0a, 0xa6, 0x10, 0x10, 0x34, + 0x05, 0xad, 0xc8, 0x57, 0xa3, 0x34, 0x02, 0xec }; + const u8 r[3] = { 0x94, 0x81, 0x70 }; + const u8 exp[3] = { 0xaa, 0xfb, 0x0d }; + u8 res[3]; + int err; + + err = smp_ah(tfm_aes, irk, r, res); + if (err) + return err; + + if (memcmp(res, exp, 3)) + return -EINVAL; + + return 0; +} + +static int __init test_c1(struct crypto_blkcipher *tfm_aes) +{ + const u8 k[16] = { + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 }; + const u8 r[16] = { + 0xe0, 0x2e, 0x70, 0xc6, 0x4e, 0x27, 0x88, 0x63, + 0x0e, 0x6f, 0xad, 0x56, 0x21, 0xd5, 0x83, 0x57 }; + const u8 preq[7] = { 0x01, 0x01, 0x00, 0x00, 0x10, 0x07, 0x07 }; + const u8 pres[7] = { 0x02, 0x03, 0x00, 0x00, 0x08, 0x00, 0x05 }; + const u8 _iat = 0x01; + const u8 _rat = 0x00; + const bdaddr_t ra = { { 0xb6, 0xb5, 0xb4, 0xb3, 0xb2, 0xb1 } }; + const bdaddr_t ia = { { 0xa6, 0xa5, 0xa4, 0xa3, 0xa2, 0xa1 } }; + const u8 exp[16] = { + 0x86, 0x3b, 0xf1, 0xbe, 0xc5, 0x4d, 0xa7, 0xd2, + 0xea, 0x88, 0x89, 0x87, 0xef, 0x3f, 0x1e, 0x1e }; + u8 res[16]; + int err; + + err = smp_c1(tfm_aes, k, r, preq, pres, _iat, &ia, _rat, &ra, res); + if (err) + return err; + + if (memcmp(res, exp, 16)) + return -EINVAL; + + return 0; +} + +static int __init test_s1(struct crypto_blkcipher *tfm_aes) +{ + const u8 k[16] = { + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 }; + const u8 r1[16] = { + 0x88, 0x77, 0x66, 0x55, 0x44, 0x33, 0x22, 0x11 }; + const u8 r2[16] = { + 0x00, 0xff, 0xee, 0xdd, 0xcc, 0xbb, 0xaa, 0x99 }; + const u8 exp[16] = { + 0x62, 0xa0, 0x6d, 0x79, 0xae, 0x16, 0x42, 0x5b, + 0x9b, 0xf4, 0xb0, 0xe8, 0xf0, 0xe1, 0x1f, 0x9a }; + u8 res[16]; + int err; + + err = smp_s1(tfm_aes, k, r1, r2, res); + if (err) + return err; + + if (memcmp(res, exp, 16)) + return -EINVAL; + + return 0; +} + +static int __init test_f4(struct crypto_hash *tfm_cmac) +{ + const u8 u[32] = { + 0xe6, 0x9d, 0x35, 0x0e, 0x48, 0x01, 0x03, 0xcc, + 0xdb, 0xfd, 0xf4, 0xac, 0x11, 0x91, 0xf4, 0xef, + 0xb9, 0xa5, 0xf9, 0xe9, 0xa7, 0x83, 0x2c, 0x5e, + 0x2c, 0xbe, 0x97, 0xf2, 0xd2, 0x03, 0xb0, 0x20 }; + const u8 v[32] = { + 0xfd, 0xc5, 0x7f, 0xf4, 0x49, 0xdd, 0x4f, 0x6b, + 0xfb, 0x7c, 0x9d, 0xf1, 0xc2, 0x9a, 0xcb, 0x59, + 0x2a, 0xe7, 0xd4, 0xee, 0xfb, 0xfc, 0x0a, 0x90, + 0x9a, 0xbb, 0xf6, 0x32, 0x3d, 0x8b, 0x18, 0x55 }; + const u8 x[16] = { + 0xab, 0xae, 0x2b, 0x71, 0xec, 0xb2, 0xff, 0xff, + 0x3e, 0x73, 0x77, 0xd1, 0x54, 0x84, 0xcb, 0xd5 }; + const u8 z = 0x00; + const u8 exp[16] = { + 0x2d, 0x87, 0x74, 0xa9, 0xbe, 0xa1, 0xed, 0xf1, + 0x1c, 0xbd, 0xa9, 0x07, 0xf1, 0x16, 0xc9, 0xf2 }; + u8 res[16]; + int err; + + err = smp_f4(tfm_cmac, u, v, x, z, res); + if (err) + return err; + + if (memcmp(res, exp, 16)) + return -EINVAL; + + return 0; +} + +static int __init test_f5(struct crypto_hash *tfm_cmac) +{ + const u8 w[32] = { + 0x98, 0xa6, 0xbf, 0x73, 0xf3, 0x34, 0x8d, 0x86, + 0xf1, 0x66, 0xf8, 0xb4, 0x13, 0x6b, 0x79, 0x99, + 0x9b, 0x7d, 0x39, 0x0a, 0xa6, 0x10, 0x10, 0x34, + 0x05, 0xad, 0xc8, 0x57, 0xa3, 0x34, 0x02, 0xec }; + const u8 n1[16] = { + 0xab, 0xae, 0x2b, 0x71, 0xec, 0xb2, 0xff, 0xff, + 0x3e, 0x73, 0x77, 0xd1, 0x54, 0x84, 0xcb, 0xd5 }; + const u8 n2[16] = { + 0xcf, 0xc4, 0x3d, 0xff, 0xf7, 0x83, 0x65, 0x21, + 0x6e, 0x5f, 0xa7, 0x25, 0xcc, 0xe7, 0xe8, 0xa6 }; + const u8 a1[7] = { 0xce, 0xbf, 0x37, 0x37, 0x12, 0x56, 0x00 }; + const u8 a2[7] = { 0xc1, 0xcf, 0x2d, 0x70, 0x13, 0xa7, 0x00 }; + const u8 exp_ltk[16] = { + 0x38, 0x0a, 0x75, 0x94, 0xb5, 0x22, 0x05, 0x98, + 0x23, 0xcd, 0xd7, 0x69, 0x11, 0x79, 0x86, 0x69 }; + const u8 exp_mackey[16] = { + 0x20, 0x6e, 0x63, 0xce, 0x20, 0x6a, 0x3f, 0xfd, + 0x02, 0x4a, 0x08, 0xa1, 0x76, 0xf1, 0x65, 0x29 }; + u8 mackey[16], ltk[16]; + int err; + + err = smp_f5(tfm_cmac, w, n1, n2, a1, a2, mackey, ltk); + if (err) + return err; + + if (memcmp(mackey, exp_mackey, 16)) + return -EINVAL; + + if (memcmp(ltk, exp_ltk, 16)) + return -EINVAL; + + return 0; +} + +static int __init test_f6(struct crypto_hash *tfm_cmac) +{ + const u8 w[16] = { + 0x20, 0x6e, 0x63, 0xce, 0x20, 0x6a, 0x3f, 0xfd, + 0x02, 0x4a, 0x08, 0xa1, 0x76, 0xf1, 0x65, 0x29 }; + const u8 n1[16] = { + 0xab, 0xae, 0x2b, 0x71, 0xec, 0xb2, 0xff, 0xff, + 0x3e, 0x73, 0x77, 0xd1, 0x54, 0x84, 0xcb, 0xd5 }; + const u8 n2[16] = { + 0xcf, 0xc4, 0x3d, 0xff, 0xf7, 0x83, 0x65, 0x21, + 0x6e, 0x5f, 0xa7, 0x25, 0xcc, 0xe7, 0xe8, 0xa6 }; + const u8 r[16] = { + 0xc8, 0x0f, 0x2d, 0x0c, 0xd2, 0x42, 0xda, 0x08, + 0x54, 0xbb, 0x53, 0xb4, 0x3b, 0x34, 0xa3, 0x12 }; + const u8 io_cap[3] = { 0x02, 0x01, 0x01 }; + const u8 a1[7] = { 0xce, 0xbf, 0x37, 0x37, 0x12, 0x56, 0x00 }; + const u8 a2[7] = { 0xc1, 0xcf, 0x2d, 0x70, 0x13, 0xa7, 0x00 }; + const u8 exp[16] = { + 0x61, 0x8f, 0x95, 0xda, 0x09, 0x0b, 0x6c, 0xd2, + 0xc5, 0xe8, 0xd0, 0x9c, 0x98, 0x73, 0xc4, 0xe3 }; + u8 res[16]; + int err; + + err = smp_f6(tfm_cmac, w, n1, n2, r, io_cap, a1, a2, res); + if (err) + return err; + + if (memcmp(res, exp, 16)) + return -EINVAL; + + return 0; +} + +static int __init test_g2(struct crypto_hash *tfm_cmac) +{ + const u8 u[32] = { + 0xe6, 0x9d, 0x35, 0x0e, 0x48, 0x01, 0x03, 0xcc, + 0xdb, 0xfd, 0xf4, 0xac, 0x11, 0x91, 0xf4, 0xef, + 0xb9, 0xa5, 0xf9, 0xe9, 0xa7, 0x83, 0x2c, 0x5e, + 0x2c, 0xbe, 0x97, 0xf2, 0xd2, 0x03, 0xb0, 0x20 }; + const u8 v[32] = { + 0xfd, 0xc5, 0x7f, 0xf4, 0x49, 0xdd, 0x4f, 0x6b, + 0xfb, 0x7c, 0x9d, 0xf1, 0xc2, 0x9a, 0xcb, 0x59, + 0x2a, 0xe7, 0xd4, 0xee, 0xfb, 0xfc, 0x0a, 0x90, + 0x9a, 0xbb, 0xf6, 0x32, 0x3d, 0x8b, 0x18, 0x55 }; + const u8 x[16] = { + 0xab, 0xae, 0x2b, 0x71, 0xec, 0xb2, 0xff, 0xff, + 0x3e, 0x73, 0x77, 0xd1, 0x54, 0x84, 0xcb, 0xd5 }; + const u8 y[16] = { + 0xcf, 0xc4, 0x3d, 0xff, 0xf7, 0x83, 0x65, 0x21, + 0x6e, 0x5f, 0xa7, 0x25, 0xcc, 0xe7, 0xe8, 0xa6 }; + const u32 exp_val = 0x2f9ed5ba % 1000000; + u32 val; + int err; + + err = smp_g2(tfm_cmac, u, v, x, y, &val); + if (err) + return err; + + if (val != exp_val) + return -EINVAL; + + return 0; +} + +static int __init test_h6(struct crypto_hash *tfm_cmac) +{ + const u8 w[16] = { + 0x9b, 0x7d, 0x39, 0x0a, 0xa6, 0x10, 0x10, 0x34, + 0x05, 0xad, 0xc8, 0x57, 0xa3, 0x34, 0x02, 0xec }; + const u8 key_id[4] = { 0x72, 0x62, 0x65, 0x6c }; + const u8 exp[16] = { + 0x99, 0x63, 0xb1, 0x80, 0xe2, 0xa9, 0xd3, 0xe8, + 0x1c, 0xc9, 0x6d, 0xe7, 0x02, 0xe1, 0x9a, 0x2d }; + u8 res[16]; + int err; + + err = smp_h6(tfm_cmac, w, key_id, res); + if (err) + return err; + + if (memcmp(res, exp, 16)) + return -EINVAL; + + return 0; +} + +static char test_smp_buffer[32]; + +static ssize_t test_smp_read(struct file *file, char __user *user_buf, + size_t count, loff_t *ppos) +{ + return simple_read_from_buffer(user_buf, count, ppos, test_smp_buffer, + strlen(test_smp_buffer)); +} + +static const struct file_operations test_smp_fops = { + .open = simple_open, + .read = test_smp_read, + .llseek = default_llseek, +}; + +static int __init run_selftests(struct crypto_blkcipher *tfm_aes, + struct crypto_hash *tfm_cmac) +{ + ktime_t calltime, delta, rettime; + unsigned long long duration; + int err; + + calltime = ktime_get(); + + err = test_ah(tfm_aes); + if (err) { + BT_ERR("smp_ah test failed"); + goto done; + } + + err = test_c1(tfm_aes); + if (err) { + BT_ERR("smp_c1 test failed"); + goto done; + } + + err = test_s1(tfm_aes); + if (err) { + BT_ERR("smp_s1 test failed"); + goto done; + } + + err = test_f4(tfm_cmac); + if (err) { + BT_ERR("smp_f4 test failed"); + goto done; + } + + err = test_f5(tfm_cmac); + if (err) { + BT_ERR("smp_f5 test failed"); + goto done; + } + + err = test_f6(tfm_cmac); + if (err) { + BT_ERR("smp_f6 test failed"); + goto done; + } + + err = test_g2(tfm_cmac); + if (err) { + BT_ERR("smp_g2 test failed"); + goto done; + } + + err = test_h6(tfm_cmac); + if (err) { + BT_ERR("smp_h6 test failed"); + goto done; + } + + rettime = ktime_get(); + delta = ktime_sub(rettime, calltime); + duration = (unsigned long long) ktime_to_ns(delta) >> 10; + + BT_INFO("SMP test passed in %llu usecs", duration); + +done: + if (!err) + snprintf(test_smp_buffer, sizeof(test_smp_buffer), + "PASS (%llu usecs)\n", duration); + else + snprintf(test_smp_buffer, sizeof(test_smp_buffer), "FAIL\n"); + + debugfs_create_file("selftest_smp", 0444, bt_debugfs, NULL, + &test_smp_fops); + + return err; +} + +int __init bt_selftest_smp(void) +{ + struct crypto_blkcipher *tfm_aes; + struct crypto_hash *tfm_cmac; + int err; + + tfm_aes = crypto_alloc_blkcipher("ecb(aes)", 0, CRYPTO_ALG_ASYNC); + if (IS_ERR(tfm_aes)) { + BT_ERR("Unable to create ECB crypto context"); + return PTR_ERR(tfm_aes); + } + + tfm_cmac = crypto_alloc_hash("cmac(aes)", 0, CRYPTO_ALG_ASYNC); + if (IS_ERR(tfm_cmac)) { + BT_ERR("Unable to create CMAC crypto context"); + crypto_free_blkcipher(tfm_aes); + return PTR_ERR(tfm_cmac); + } + + err = run_selftests(tfm_aes, tfm_cmac); + + crypto_free_hash(tfm_cmac); + crypto_free_blkcipher(tfm_aes); + + return err; +} + +#endif diff --git a/kernel/net/bluetooth/smp.h b/kernel/net/bluetooth/smp.h new file mode 100644 index 000000000..6cf872563 --- /dev/null +++ b/kernel/net/bluetooth/smp.h @@ -0,0 +1,209 @@ +/* + BlueZ - Bluetooth protocol stack for Linux + Copyright (C) 2011 Nokia Corporation and/or its subsidiary(-ies). + + 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; + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS + OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT OF THIRD PARTY RIGHTS. + IN NO EVENT SHALL THE COPYRIGHT HOLDER(S) AND AUTHOR(S) BE LIABLE FOR ANY + CLAIM, OR ANY SPECIAL INDIRECT OR CONSEQUENTIAL DAMAGES, OR ANY DAMAGES + WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN + ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF + OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + + ALL LIABILITY, INCLUDING LIABILITY FOR INFRINGEMENT OF ANY PATENTS, + COPYRIGHTS, TRADEMARKS OR OTHER RIGHTS, RELATING TO USE OF THIS + SOFTWARE IS DISCLAIMED. +*/ + +#ifndef __SMP_H +#define __SMP_H + +struct smp_command_hdr { + __u8 code; +} __packed; + +#define SMP_CMD_PAIRING_REQ 0x01 +#define SMP_CMD_PAIRING_RSP 0x02 +struct smp_cmd_pairing { + __u8 io_capability; + __u8 oob_flag; + __u8 auth_req; + __u8 max_key_size; + __u8 init_key_dist; + __u8 resp_key_dist; +} __packed; + +#define SMP_IO_DISPLAY_ONLY 0x00 +#define SMP_IO_DISPLAY_YESNO 0x01 +#define SMP_IO_KEYBOARD_ONLY 0x02 +#define SMP_IO_NO_INPUT_OUTPUT 0x03 +#define SMP_IO_KEYBOARD_DISPLAY 0x04 + +#define SMP_OOB_NOT_PRESENT 0x00 +#define SMP_OOB_PRESENT 0x01 + +#define SMP_DIST_ENC_KEY 0x01 +#define SMP_DIST_ID_KEY 0x02 +#define SMP_DIST_SIGN 0x04 +#define SMP_DIST_LINK_KEY 0x08 + +#define SMP_AUTH_NONE 0x00 +#define SMP_AUTH_BONDING 0x01 +#define SMP_AUTH_MITM 0x04 +#define SMP_AUTH_SC 0x08 +#define SMP_AUTH_KEYPRESS 0x10 + +#define SMP_CMD_PAIRING_CONFIRM 0x03 +struct smp_cmd_pairing_confirm { + __u8 confirm_val[16]; +} __packed; + +#define SMP_CMD_PAIRING_RANDOM 0x04 +struct smp_cmd_pairing_random { + __u8 rand_val[16]; +} __packed; + +#define SMP_CMD_PAIRING_FAIL 0x05 +struct smp_cmd_pairing_fail { + __u8 reason; +} __packed; + +#define SMP_CMD_ENCRYPT_INFO 0x06 +struct smp_cmd_encrypt_info { + __u8 ltk[16]; +} __packed; + +#define SMP_CMD_MASTER_IDENT 0x07 +struct smp_cmd_master_ident { + __le16 ediv; + __le64 rand; +} __packed; + +#define SMP_CMD_IDENT_INFO 0x08 +struct smp_cmd_ident_info { + __u8 irk[16]; +} __packed; + +#define SMP_CMD_IDENT_ADDR_INFO 0x09 +struct smp_cmd_ident_addr_info { + __u8 addr_type; + bdaddr_t bdaddr; +} __packed; + +#define SMP_CMD_SIGN_INFO 0x0a +struct smp_cmd_sign_info { + __u8 csrk[16]; +} __packed; + +#define SMP_CMD_SECURITY_REQ 0x0b +struct smp_cmd_security_req { + __u8 auth_req; +} __packed; + +#define SMP_CMD_PUBLIC_KEY 0x0c +struct smp_cmd_public_key { + __u8 x[32]; + __u8 y[32]; +} __packed; + +#define SMP_CMD_DHKEY_CHECK 0x0d +struct smp_cmd_dhkey_check { + __u8 e[16]; +} __packed; + +#define SMP_CMD_KEYPRESS_NOTIFY 0x0e +struct smp_cmd_keypress_notify { + __u8 value; +} __packed; + +#define SMP_CMD_MAX 0x0e + +#define SMP_PASSKEY_ENTRY_FAILED 0x01 +#define SMP_OOB_NOT_AVAIL 0x02 +#define SMP_AUTH_REQUIREMENTS 0x03 +#define SMP_CONFIRM_FAILED 0x04 +#define SMP_PAIRING_NOTSUPP 0x05 +#define SMP_ENC_KEY_SIZE 0x06 +#define SMP_CMD_NOTSUPP 0x07 +#define SMP_UNSPECIFIED 0x08 +#define SMP_REPEATED_ATTEMPTS 0x09 +#define SMP_INVALID_PARAMS 0x0a +#define SMP_DHKEY_CHECK_FAILED 0x0b +#define SMP_NUMERIC_COMP_FAILED 0x0c +#define SMP_BREDR_PAIRING_IN_PROGRESS 0x0d +#define SMP_CROSS_TRANSP_NOT_ALLOWED 0x0e + +#define SMP_MIN_ENC_KEY_SIZE 7 +#define SMP_MAX_ENC_KEY_SIZE 16 + +/* LTK types used in internal storage (struct smp_ltk) */ +enum { + SMP_STK, + SMP_LTK, + SMP_LTK_SLAVE, + SMP_LTK_P256, + SMP_LTK_P256_DEBUG, +}; + +static inline bool smp_ltk_is_sc(struct smp_ltk *key) +{ + switch (key->type) { + case SMP_LTK_P256: + case SMP_LTK_P256_DEBUG: + return true; + } + + return false; +} + +static inline u8 smp_ltk_sec_level(struct smp_ltk *key) +{ + if (key->authenticated) { + if (smp_ltk_is_sc(key)) + return BT_SECURITY_FIPS; + else + return BT_SECURITY_HIGH; + } + + return BT_SECURITY_MEDIUM; +} + +/* Key preferences for smp_sufficient security */ +enum smp_key_pref { + SMP_ALLOW_STK, + SMP_USE_LTK, +}; + +/* SMP Commands */ +bool smp_sufficient_security(struct hci_conn *hcon, u8 sec_level, + enum smp_key_pref key_pref); +int smp_conn_security(struct hci_conn *hcon, __u8 sec_level); +int smp_user_confirm_reply(struct hci_conn *conn, u16 mgmt_op, __le32 passkey); + +bool smp_irk_matches(struct hci_dev *hdev, const u8 irk[16], + const bdaddr_t *bdaddr); +int smp_generate_rpa(struct hci_dev *hdev, const u8 irk[16], bdaddr_t *rpa); +int smp_generate_oob(struct hci_dev *hdev, u8 hash[16], u8 rand[16]); + +int smp_register(struct hci_dev *hdev); +void smp_unregister(struct hci_dev *hdev); + +#if IS_ENABLED(CONFIG_BT_SELFTEST_SMP) + +int bt_selftest_smp(void); + +#else + +static inline int bt_selftest_smp(void) +{ + return 0; +} + +#endif + +#endif /* __SMP_H */ |