1 files changed, 383 insertions, 86 deletions

diff --git a/net/bluetooth/hci_sock.c b/net/bluetooth/hci_sock.c
index 0dcc96266779..63afd234283e 100644
--- a/net/bluetooth/hci_sock.c
+++ b/net/bluetooth/hci_sock.c
@@ -48,8 +48,9 @@
 
 #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 +86,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 +107,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 +133,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 +442,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 +471,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 +488,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 +621,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:
@@ -461,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);
 
@@ -495,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;
@@ -574,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)) {
@@ -636,6 +968,7 @@ static int hci_sock_setsockopt(struct socket *sock, int level, int optname, char
 		break;
 	}
 
+done:
 	release_sock(sk);
 	return err;
 }
@@ -644,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)
@@ -657,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:
@@ -667,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:
@@ -682,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 = {
@@ -748,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;
@@ -806,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;
@@ -823,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");