[v3,08/13] Bluetooth: Implement MSFT avdtp open command

diff --git a/include/net/bluetooth/bluetooth.h b/include/net/bluetooth/bluetooth.h
index 2f31e571f34c..5e07cfed941d 100644
--- a/include/net/bluetooth/bluetooth.h
+++ b/include/net/bluetooth/bluetooth.h
@@ -177,6 +177,8 @@  struct bt_codecs {
 #define BT_CODEC_TRANSPARENT	0x03
 #define BT_CODEC_MSBC		0x05
 
+#define BT_MSFT_OPEN		20
+
 __printf(1, 2)
 void bt_info(const char *fmt, ...);
 __printf(1, 2)
diff --git a/include/net/bluetooth/hci.h b/include/net/bluetooth/hci.h
index 7ea1bfce204f..a7dad0125c10 100644
--- a/include/net/bluetooth/hci.h
+++ b/include/net/bluetooth/hci.h
@@ -2009,6 +2009,22 @@  struct hci_cp_le_reject_cis {
 	__u8    reason;
 } __packed;
 
+#define HCI_MSFT_AVDTP_CMD			0xfc1e
+
+#define HCI_MSFT_AVDTP_OPEN			0x08
+struct hci_media_service_caps {
+	__u8	category;
+	__u8	len;
+	__u8	data[0];
+} __packed;
+
+struct msft_cp_avdtp_open {
+	__u8	sub_opcode;
+	__le16	handle;
+	__le16	dcid;
+	__le16	omtu;
+} __packed;
+
 /* ---- HCI Events ---- */
 #define HCI_EV_INQUIRY_COMPLETE		0x01
 
diff --git a/net/bluetooth/hci_codec.c b/net/bluetooth/hci_codec.c
index c6bd934dcf36..e179f3bfb494 100644
--- a/net/bluetooth/hci_codec.c
+++ b/net/bluetooth/hci_codec.c
@@ -355,3 +355,46 @@  int hci_get_supported_codecs(struct hci_dev *hdev, u8 type, char __user *optval,
 error:
 	return err;
 }
+
+int hci_configure_msft_avdtp_open(struct hci_dev *hdev, struct l2cap_chan *chan,
+				  sockptr_t optval, int optlen)
+{
+	struct msft_cp_avdtp_open *cmd = NULL;
+	struct hci_media_service_caps *caps;
+	int err;
+
+	if (!optlen || optlen < sizeof(*caps)) {
+		err = -EINVAL;
+		goto fail;
+	}
+
+	cmd = kzalloc(sizeof(*cmd) + optlen, GFP_KERNEL);
+	if (!cmd) {
+		err = -ENOMEM;
+		goto fail;
+	}
+
+	cmd->sub_opcode = HCI_MSFT_AVDTP_OPEN;
+	cmd->handle = __cpu_to_le16(chan->conn->hcon->handle);
+	cmd->dcid = cpu_to_le16(chan->dcid);
+	cmd->omtu = cpu_to_le16(chan->omtu);
+	caps = (void *)(cmd + 1);
+
+	if (copy_from_sockptr(caps, optval, optlen)) {
+		err = -EFAULT;
+		goto fail;
+	}
+
+	if (caps->category != 0x07) {
+		err = -EINVAL;
+		goto fail;
+	}
+
+	hci_send_cmd(hdev, HCI_MSFT_AVDTP_CMD, sizeof(*cmd) + optlen, cmd);
+
+	/* wait until we get avdtp handle or timeout */
+
+fail:
+	kfree(cmd);
+	return err;
+}
diff --git a/net/bluetooth/hci_codec.h b/net/bluetooth/hci_codec.h
index 6e849c7d75b9..123b46a6a8ce 100644
--- a/net/bluetooth/hci_codec.h
+++ b/net/bluetooth/hci_codec.h
@@ -2,8 +2,12 @@ 
 
 /* Copyright (C) 2014 Intel Corporation */
 
+#include <net/bluetooth/l2cap.h>
+
 void hci_read_supported_codecs(struct hci_dev *hdev);
 void hci_read_supported_codecs_v2(struct hci_dev *hdev);
 void hci_codec_list_clear(struct list_head *codec_list);
 int hci_get_supported_codecs(struct hci_dev *hdev, u8 type, char __user *optval,
 			     int __user *optlen, int len);
+int hci_configure_msft_avdtp_open(struct hci_dev *hdev, struct l2cap_chan *chan,
+				  sockptr_t optval, int optlen);
diff --git a/net/bluetooth/l2cap_sock.c b/net/bluetooth/l2cap_sock.c
index a883acf33e3c..fa689e576576 100644
--- a/net/bluetooth/l2cap_sock.c
+++ b/net/bluetooth/l2cap_sock.c
@@ -909,6 +909,7 @@  static int l2cap_sock_setsockopt(struct socket *sock, int level, int optname,
 	struct l2cap_conn *conn;
 	int len, err = 0;
 	u32 opt;
+	struct hci_dev *hdev;
 
 	BT_DBG("sk %p", sk);
 
@@ -1137,6 +1138,29 @@  static int l2cap_sock_setsockopt(struct socket *sock, int level, int optname,
 
 		break;
 
+	case BT_MSFT_OPEN:
+		if (sk->sk_state != BT_CONNECTED) {
+			err = -ENOTCONN;
+			break;
+		}
+
+		hdev = hci_get_route(BDADDR_ANY, &chan->src, BDADDR_BREDR);
+		if (!hdev) {
+			err = -EBADFD;
+			break;
+		}
+
+		if (!hci_dev_test_flag(hdev, HCI_OFFLOAD_CODECS_ENABLED) ||
+		    !hdev->get_data_path_id) {
+			err = -EOPNOTSUPP;
+			hci_dev_put(hdev);
+			break;
+		}
+
+		err = hci_configure_msft_avdtp_open(hdev, chan, optval, optlen);
+		hci_dev_put(hdev);
+		break;
+
 	default:
 		err = -ENOPROTOOPT;
 		break;