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

[Kiran K <kiran.k@xxxxxxxxx>]

In A2DP offload use case, controller needs to configured
with selected codec capabilities, dcid of media transport
channel and ACL connection handle.

Controller responds with avdtp handle which needs to be
sent in other avdtp commands like start, suspend and close.

Signed-off-by: Kiran K <kiran.k@xxxxxxxxx>
Reviewed-by: Chethan T N <chethan.tumkur.narayan@xxxxxxxxx>
Reviewed-by: Srivatsa Ravishankar <ravishankar.srivatsa@xxxxxxxxx>
---
 include/net/bluetooth/bluetooth.h |  2 ++
 include/net/bluetooth/hci.h       | 16 ++++++++++++
 net/bluetooth/hci_codec.c         | 43 +++++++++++++++++++++++++++++++
 net/bluetooth/hci_codec.h         |  4 +++
 net/bluetooth/l2cap_sock.c        | 24 +++++++++++++++++
 5 files changed, 89 insertions(+)

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;
-- 
2.17.1