1 files changed, 383 insertions, 87 deletions

diff --git a/net/bluetooth/hci_sock.c b/net/bluetooth/hci_sock.c
index b2eb2b93580f..5914623f426a 100644
--- a/net/bluetooth/hci_sock.c
+++ b/net/bluetooth/hci_sock.c
@@ -42,14 +42,14 @@
 #include <linux/ioctl.h>
 #include <net/sock.h>
 
-#include <asm/system.h>
 #include <linux/uaccess.h>
 #include <asm/unaligned.h>
 
 #include <net/bluetooth/bluetooth.h>
 #include <net/bluetooth/hci_core.h>
+#include <net/bluetooth/hci_mon.h>
 
-static bool enable_mgmt;
+static atomic_t monitor_promisc = ATOMIC_INIT(0);
 
 /* ----- HCI socket interface ----- */
 
@@ -85,22 +85,20 @@ static struct bt_sock_list hci_sk_list = {
 };
 
 /* Send frame to RAW socket */
-void hci_send_to_sock(struct hci_dev *hdev, struct sk_buff *skb,
-                                                        struct sock *skip_sk)
+void hci_send_to_sock(struct hci_dev *hdev, struct sk_buff *skb)
 {
         struct sock *sk;
         struct hlist_node *node;
+        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, node, &hci_sk_list.head) {
                 struct hci_filter *flt;
                 struct sk_buff *nskb;
 
-                if (sk == skip_sk)
-                        continue;
-
                 if (sk->sk_state != BT_BOUND || hci_pi(sk)->hdev != hdev)
                         continue;
 
@@ -108,12 +106,9 @@ void hci_send_to_sock(struct hci_dev *hdev, struct sk_buff *skb,
                 if (skb->sk == sk)
                         continue;
 
-                if (bt_cb(skb)->channel != hci_pi(sk)->channel)
+                if (hci_pi(sk)->channel != HCI_CHANNEL_RAW)
                         continue;
 
-                if (bt_cb(skb)->channel == HCI_CHANNEL_CONTROL)
-                        goto clone;
-
                 /* Apply filter */
                 flt = &hci_pi(sk)->filter;
 
@@ -137,21 +132,303 @@ void hci_send_to_sock(struct hci_dev *hdev, struct sk_buff *skb,
                                 continue;
                 }
 
-clone:
+                if (!skb_copy) {
+                        /* Create a private copy with headroom */
+                        skb_copy = __pskb_copy(skb, 1, GFP_ATOMIC);
+                        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 control socket */
+void hci_send_to_control(struct sk_buff *skb, struct sock *skip_sk)
+{
+        struct sock *sk;
+        struct hlist_node *node;
+
+        BT_DBG("len %d", skb->len);
+
+        read_lock(&hci_sk_list.lock);
+
+        sk_for_each(sk, node, &hci_sk_list.head) {
+                struct sk_buff *nskb;
+
+                /* Skip the original socket */
+                if (sk == skip_sk)
+                        continue;
+
+                if (sk->sk_state != BT_BOUND)
+                        continue;
+
+                if (hci_pi(sk)->channel != HCI_CHANNEL_CONTROL)
+                        continue;
+
                 nskb = skb_clone(skb, GFP_ATOMIC);
                 if (!nskb)
                         continue;
 
-                /* Put type byte before the data */
-                if (bt_cb(skb)->channel == HCI_CHANNEL_RAW)
-                        memcpy(skb_push(nskb, 1), &bt_cb(nskb)->pkt_type, 1);
+                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 sock *sk;
+        struct hlist_node *node;
+        struct sk_buff *skb_copy = NULL;
+        __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 = __constant_cpu_to_le16(HCI_MON_COMMAND_PKT);
+                break;
+        case HCI_EVENT_PKT:
+                opcode = __constant_cpu_to_le16(HCI_MON_EVENT_PKT);
+                break;
+        case HCI_ACLDATA_PKT:
+                if (bt_cb(skb)->incoming)
+                        opcode = __constant_cpu_to_le16(HCI_MON_ACL_RX_PKT);
+                else
+                        opcode = __constant_cpu_to_le16(HCI_MON_ACL_TX_PKT);
+                break;
+        case HCI_SCODATA_PKT:
+                if (bt_cb(skb)->incoming)
+                        opcode = __constant_cpu_to_le16(HCI_MON_SCO_RX_PKT);
+                else
+                        opcode = __constant_cpu_to_le16(HCI_MON_SCO_TX_PKT);
+                break;
+        default:
+                return;
+        }
+
+        read_lock(&hci_sk_list.lock);
+
+        sk_for_each(sk, node, &hci_sk_list.head) {
+                struct sk_buff *nskb;
+
+                if (sk->sk_state != BT_BOUND)
+                        continue;
+
+                if (hci_pi(sk)->channel != HCI_CHANNEL_MONITOR)
+                        continue;
+
+                if (!skb_copy) {
+                        struct hci_mon_hdr *hdr;
+
+                        /* Create a private copy with headroom */
+                        skb_copy = __pskb_copy(skb, HCI_MON_HDR_SIZE, GFP_ATOMIC);
+                        if (!skb_copy)
+                                continue;
+
+                        /* 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);
+                }
+
+                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);
+}
+
+static void send_monitor_event(struct sk_buff *skb)
+{
+        struct sock *sk;
+        struct hlist_node *node;
+
+        BT_DBG("len %d", skb->len);
+
+        read_lock(&hci_sk_list.lock);
+
+        sk_for_each(sk, node, &hci_sk_list.head) {
+                struct sk_buff *nskb;
+
+                if (sk->sk_state != BT_BOUND)
+                        continue;
+
+                if (hci_pi(sk)->channel != HCI_CHANNEL_MONITOR)
+                        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);
 }
 
+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 = __constant_cpu_to_le16(HCI_MON_NEW_INDEX);
+                break;
+
+        case HCI_DEV_UNREG:
+                skb = bt_skb_alloc(0, GFP_ATOMIC);
+                if (!skb)
+                        return NULL;
+
+                opcode = __constant_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;
+        skb->dev = (void *) hdev;
+        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) {
+                        send_monitor_event(skb);
+                        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;
+                struct hlist_node *node;
+
+                /* Detach sockets from device */
+                read_lock(&hci_sk_list.lock);
+                sk_for_each(sk, node, &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 int hci_sock_release(struct socket *sock)
 {
         struct sock *sk = sock->sk;
@@ -164,6 +441,9 @@ static int hci_sock_release(struct socket *sock)
 
         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) {
@@ -190,7 +470,7 @@ static int hci_sock_blacklist_add(struct hci_dev *hdev, void __user *arg)
 
         hci_dev_lock(hdev);
 
-        err = hci_blacklist_add(hdev, &bdaddr);
+        err = hci_blacklist_add(hdev, &bdaddr, 0);
 
         hci_dev_unlock(hdev);
 
@@ -207,7 +487,7 @@ static int hci_sock_blacklist_del(struct hci_dev *hdev, void __user *arg)
 
         hci_dev_lock(hdev);
 
-        err = hci_blacklist_del(hdev, &bdaddr);
+        err = hci_blacklist_del(hdev, &bdaddr, 0);
 
         hci_dev_unlock(hdev);
 
@@ -340,34 +620,69 @@ static int hci_sock_bind(struct socket *sock, struct sockaddr *addr, int addr_le
         if (haddr.hci_family != AF_BLUETOOTH)
                 return -EINVAL;
 
-        if (haddr.hci_channel > HCI_CHANNEL_CONTROL)
-                return -EINVAL;
-
-        if (haddr.hci_channel == HCI_CHANNEL_CONTROL) {
-                if (!enable_mgmt)
-                        return -EINVAL;
-                set_bit(HCI_PI_MGMT_INIT, &hci_pi(sk)->flags);
-        }
-
         lock_sock(sk);
 
-        if (sk->sk_state == BT_BOUND || hci_pi(sk)->hdev) {
+        if (sk->sk_state == BT_BOUND) {
                 err = -EALREADY;
                 goto done;
         }
 
-        if (haddr.hci_dev != HCI_DEV_NONE) {
-                hdev = hci_dev_get(haddr.hci_dev);
-                if (!hdev) {
-                        err = -ENODEV;
+        switch (haddr.hci_channel) {
+        case HCI_CHANNEL_RAW:
+                if (hci_pi(sk)->hdev) {
+                        err = -EALREADY;
                         goto done;
                 }
 
-                atomic_inc(&hdev->promisc);
+                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_CONTROL:
+                if (haddr.hci_dev != HCI_DEV_NONE) {
+                        err = -EINVAL;
+                        goto done;
+                }
+
+                if (!capable(CAP_NET_ADMIN)) {
+                        err = -EPERM;
+                        goto done;
+                }
+
+                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;
+                }
+
+                send_monitor_replay(sk);
+
+                atomic_inc(&monitor_promisc);
+                break;
+
+        default:
+                err = -EINVAL;
+                goto done;
         }
 
+
         hci_pi(sk)->channel = haddr.hci_channel;
-        hci_pi(sk)->hdev = hdev;
         sk->sk_state = BT_BOUND;
 
 done:
@@ -462,7 +777,15 @@ static int hci_sock_recvmsg(struct kiocb *iocb, struct socket *sock,
         skb_reset_transport_header(skb);
         err = skb_copy_datagram_iovec(skb, 0, msg->msg_iov, copied);
 
-        hci_sock_cmsg(sk, msg, skb);
+        switch (hci_pi(sk)->channel) {
+        case HCI_CHANNEL_RAW:
+                hci_sock_cmsg(sk, msg, skb);
+                break;
+        case HCI_CHANNEL_CONTROL:
+        case HCI_CHANNEL_MONITOR:
+                sock_recv_timestamp(msg, sk, skb);
+                break;
+        }
 
         skb_free_datagram(sk, skb);
 
@@ -496,6 +819,9 @@ static int hci_sock_sendmsg(struct kiocb *iocb, struct socket *sock,
         case HCI_CHANNEL_CONTROL:
                 err = mgmt_control(sk, msg, len);
                 goto done;
+        case HCI_CHANNEL_MONITOR:
+                err = -EOPNOTSUPP;
+                goto done;
         default:
                 err = -EINVAL;
                 goto done;
@@ -575,6 +901,11 @@ static int hci_sock_setsockopt(struct socket *sock, int level, int optname, char
 
         lock_sock(sk);
 
+        if (hci_pi(sk)->channel != HCI_CHANNEL_RAW) {
+                err = -EINVAL;
+                goto done;
+        }
+
         switch (optname) {
         case HCI_DATA_DIR:
                 if (get_user(opt, (int __user *)optval)) {
@@ -637,6 +968,7 @@ static int hci_sock_setsockopt(struct socket *sock, int level, int optname, char
                 break;
         }
 
+done:
         release_sock(sk);
         return err;
 }
@@ -645,11 +977,20 @@ static int hci_sock_getsockopt(struct socket *sock, int level, int optname, char
 {
         struct hci_ufilter uf;
         struct sock *sk = sock->sk;
-        int len, opt;
+        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 = -EINVAL;
+                goto done;
+        }
+
         switch (optname) {
         case HCI_DATA_DIR:
                 if (hci_pi(sk)->cmsg_mask & HCI_CMSG_DIR)
@@ -658,7 +999,7 @@ static int hci_sock_getsockopt(struct socket *sock, int level, int optname, char
                         opt = 0;
 
                 if (put_user(opt, optval))
-                        return -EFAULT;
+                        err = -EFAULT;
                 break;
 
         case HCI_TIME_STAMP:
@@ -668,7 +1009,7 @@ static int hci_sock_getsockopt(struct socket *sock, int level, int optname, char
                         opt = 0;
 
                 if (put_user(opt, optval))
-                        return -EFAULT;
+                        err = -EFAULT;
                 break;
 
         case HCI_FILTER:
@@ -683,15 +1024,17 @@ static int hci_sock_getsockopt(struct socket *sock, int level, int optname, char
 
                 len = min_t(unsigned int, len, sizeof(uf));
                 if (copy_to_user(optval, &uf, len))
-                        return -EFAULT;
+                        err = -EFAULT;
                 break;
 
         default:
-                return -ENOPROTOOPT;
+                err = -ENOPROTOOPT;
                 break;
         }
 
-        return 0;
+done:
+        release_sock(sk);
+        return err;
 }
 
 static const struct proto_ops hci_sock_ops = {
@@ -749,52 +1092,12 @@ static int hci_sock_create(struct net *net, struct socket *sock, int protocol,
         return 0;
 }
 
-static int hci_sock_dev_event(struct notifier_block *this, unsigned long event, void *ptr)
-{
-        struct hci_dev *hdev = (struct hci_dev *) ptr;
-        struct hci_ev_si_device ev;
-
-        BT_DBG("hdev %s event %ld", hdev->name, event);
-
-        /* 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;
-                struct hlist_node *node;
-
-                /* Detach sockets from device */
-                read_lock(&hci_sk_list.lock);
-                sk_for_each(sk, node, &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);
-        }
-
-        return NOTIFY_DONE;
-}
-
 static const struct net_proto_family hci_sock_family_ops = {
         .family = PF_BLUETOOTH,
         .owner  = THIS_MODULE,
         .create = hci_sock_create,
 };
 
-static struct notifier_block hci_sock_nblock = {
-        .notifier_call = hci_sock_dev_event
-};
-
 int __init hci_sock_init(void)
 {
         int err;
@@ -807,8 +1110,6 @@ int __init hci_sock_init(void)
         if (err < 0)
                 goto error;
 
-        hci_register_notifier(&hci_sock_nblock);
-
         BT_INFO("HCI socket layer initialized");
 
         return 0;
@@ -824,10 +1125,5 @@ void hci_sock_cleanup(void)
         if (bt_sock_unregister(BTPROTO_HCI) < 0)
                 BT_ERR("HCI socket unregistration failed");
 
-        hci_unregister_notifier(&hci_sock_nblock);
-
         proto_unregister(&hci_sk_proto);
 }
-
-module_param(enable_mgmt, bool, 0644);
-MODULE_PARM_DESC(enable_mgmt, "Enable Management interface");