Re: HCI Set custom bandwidth for AuriStream SCO codec

["Pali Rohár" <pali.rohar@gmail.com>]

[-- Attachment #1.1: Type: text/plain, Size: 1348 bytes --]

On Wednesday 20 November 2019 08:44:11 Pali Rohár wrote:
> I was playing with another suggestion for API:
> 
> +#define BT_VOICE_SETUP		14
> +struct bt_voice_pkt_type {
> +	__u8 capability; /* 0x01 - SCO; 0x02 - eSCO */
> +	__u8 retrans_effort;
> +	__u16 pkt_type;
> +	__u16 max_latency;
> +};
> +struct bt_voice_setup {
> +	__u16 voice_setting;
> +	__u16 pkt_types_count;
> +	__u32 tx_bandwidth;
> +	__u32 rx_bandwidth;
> +	struct bt_voice_pkt_type pkt_types[];
> +};
> 
> So voice_setttings, pkt_types_count and badwidth would not be repeated
> as it is same for every pkt_type/retrans_effors/max_latency.
> 
> But above uses C99 flexible arrays, so I do not know if API kernel <-->
> userspace API is allowed to use C99 flexible arrays.
> 
> But getsockopt/setsockopt functions are possible to write with above
> API.

I played more with C99 flexible arrays and seems that gcc supports it
without any problems. I'm sending another attempt of API implementation,
now with more fields which are needed for Enhanced Setup Synchronous
Connection command. This command is not supported by kernel yet, but
should be easy to add it. So my ioctl API is prepared for it. Enhanced
Setup Synchronous Connection command would be needed to use hardware
mSBC codec encoder/decoder.

-- 
Pali Rohár
pali.rohar@gmail.com

[-- Attachment #1.2: sco.patch --]
[-- Type: text/plain, Size: 27433 bytes --]

diff --git a/include/net/bluetooth/bluetooth.h b/include/net/bluetooth/bluetooth.h
index fabee6db0abb..29590c6749d5 100644
--- a/include/net/bluetooth/bluetooth.h
+++ b/include/net/bluetooth/bluetooth.h
@@ -116,12 +116,49 @@ struct bt_voice {
 	__u16 setting;
 };
 
-#define BT_VOICE_TRANSPARENT			0x0003
-#define BT_VOICE_CVSD_16BIT			0x0060
-
 #define BT_SNDMTU		12
 #define BT_RCVMTU		13
 
+#define BT_VOICE_SETUP		14
+#define BT_VOICE_PKT_TYPE_CAP_SCO	BIT(0)
+#define BT_VOICE_PKT_TYPE_CAP_ESCO	BIT(1)
+struct bt_voice_pkt_type {
+	__u8 capability; /* bitmask of BT_VOICE_PKT_TYPE_CAP_* */
+	__u8 retrans_effort;
+	__u16 pkt_type;
+	__u16 max_latency;
+};
+#define BT_VOICE_SETUP_FEATURE_CONFIG		BIT(0) /* Additional configuration fields after voice_settings are set (including other features) */
+#define BT_VOICE_SETUP_FEATURE_ADD_SCO		BIT(1) /* Can use Add Synchronous Connection */
+#define BT_VOICE_SETUP_FEATURE_SETUP_SCO	BIT(2) /* Can use Setup Synchronous Connection */
+#define BT_VOICE_SETUP_FEATURE_ENH_SETUP_SCO	BIT(3) /* Can use Enhanced Setup Synchronous Connection */
+struct bt_voice_setup {
+	__u16 voice_setting;
+	__u8 features; /* bitmask of BT_VOICE_SETUP_FEATURE_* */
+	__u8 pkt_types_count;
+	__u32 tx_bandwidth;
+	__u32 rx_bandwidth;
+	__u32 input_bandwidth;
+	__u32 output_bandwidth;
+	__u8 tx_coding_format[5];
+	__u8 rx_coding_format[5];
+	__u8 input_coding_format[5];
+	__u8 output_coding_format[5];
+	__u16 tx_codec_frame_size;
+	__u16 rx_codec_frame_size;
+	__u16 input_coded_data_size;
+	__u16 output_coded_data_size;
+	__u8 input_pcm_data_format;
+	__u8 output_pcm_data_format;
+	__u8 input_pcm_msb_position;
+	__u8 output_pcm_msb_position;
+	__u8 input_data_path;
+	__u8 output_data_path;
+	__u8 input_unit_size;
+	__u8 output_unit_size;
+	struct bt_voice_pkt_type pkt_types[];
+};
+
 __printf(1, 2)
 void bt_info(const char *fmt, ...);
 __printf(1, 2)
diff --git a/include/net/bluetooth/hci_core.h b/include/net/bluetooth/hci_core.h
index 094e61e07030..c99236e3a6d2 100644
--- a/include/net/bluetooth/hci_core.h
+++ b/include/net/bluetooth/hci_core.h
@@ -477,7 +477,7 @@ struct hci_conn {
 	__u8		passkey_entered;
 	__u16		disc_timeout;
 	__u16		conn_timeout;
-	__u16		setting;
+	const struct bt_voice_setup *voice_setup;
 	__u16		le_conn_min_interval;
 	__u16		le_conn_max_interval;
 	__u16		le_conn_interval;
@@ -897,8 +897,8 @@ static inline struct hci_conn *hci_lookup_le_connect(struct hci_dev *hdev)
 }
 
 int hci_disconnect(struct hci_conn *conn, __u8 reason);
-bool hci_setup_sync(struct hci_conn *conn, __u16 handle);
-void hci_sco_setup(struct hci_conn *conn, __u8 status);
+int hci_setup_sync(struct hci_conn *conn, __u16 handle);
+int hci_sco_setup(struct hci_conn *conn, __u8 status);
 
 struct hci_conn *hci_conn_add(struct hci_dev *hdev, int type, bdaddr_t *dst,
 			      u8 role);
@@ -920,7 +920,7 @@ struct hci_conn *hci_connect_le(struct hci_dev *hdev, bdaddr_t *dst,
 struct hci_conn *hci_connect_acl(struct hci_dev *hdev, bdaddr_t *dst,
 				 u8 sec_level, u8 auth_type);
 struct hci_conn *hci_connect_sco(struct hci_dev *hdev, int type, bdaddr_t *dst,
-				 __u16 setting);
+				 const struct bt_voice_setup *voice_setup);
 int hci_conn_check_link_mode(struct hci_conn *conn);
 int hci_conn_check_secure(struct hci_conn *conn, __u8 sec_level);
 int hci_conn_security(struct hci_conn *conn, __u8 sec_level, __u8 auth_type,
@@ -1164,6 +1164,9 @@ void hci_conn_del_sysfs(struct hci_conn *conn);
 #define lmp_lsto_capable(dev)      ((dev)->features[0][7] & LMP_LSTO)
 #define lmp_inq_tx_pwr_capable(dev) ((dev)->features[0][7] & LMP_INQ_TX_PWR)
 #define lmp_ext_feat_capable(dev)  ((dev)->features[0][7] & LMP_EXTFEATURES)
+#define lmp_ulaw_capable(dev)      ((dev)->features[0][1] & LMP_ULAW)
+#define lmp_alaw_capable(dev)      ((dev)->features[0][1] & LMP_ALAW)
+#define lmp_cvsd_capable(dev)      ((dev)->features[0][2] & LMP_CVSD)
 #define lmp_transp_capable(dev)    ((dev)->features[0][2] & LMP_TRANSPARENT)
 #define lmp_edr_2m_capable(dev)    ((dev)->features[0][3] & LMP_EDR_2M)
 #define lmp_edr_3m_capable(dev)    ((dev)->features[0][3] & LMP_EDR_3M)
@@ -1592,6 +1595,15 @@ void hci_copy_identity_address(struct hci_dev *hdev, bdaddr_t *bdaddr,
 
 #define SCO_AIRMODE_MASK       0x0003
 #define SCO_AIRMODE_CVSD       0x0000
+#define SCO_AIRMODE_ULAW       0x0001
+#define SCO_AIRMODE_ALAW       0x0002
 #define SCO_AIRMODE_TRANSP     0x0003
 
+#define lmp_voice_setting_compatible(dev, voice_setting) ( \
+	(((voice_setting) & SCO_AIRMODE_MASK) == SCO_AIRMODE_CVSD) ? lmp_cvsd_capable(dev) : \
+	(((voice_setting) & SCO_AIRMODE_MASK) == SCO_AIRMODE_ULAW) ? lmp_ulaw_capable(dev) : \
+	(((voice_setting) & SCO_AIRMODE_MASK) == SCO_AIRMODE_ALAW) ? lmp_alaw_capable(dev) : \
+	(((voice_setting) & SCO_AIRMODE_MASK) == SCO_AIRMODE_TRANSP) ? lmp_transp_capable(dev) : \
+	false)
+
 #endif /* __HCI_CORE_H */
diff --git a/net/bluetooth/hci_conn.c b/net/bluetooth/hci_conn.c
index bd4978ce8c45..ac0e3aceac01 100644
--- a/net/bluetooth/hci_conn.c
+++ b/net/bluetooth/hci_conn.c
@@ -35,30 +35,6 @@
 #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 */
-};
-
 /* This function requires the caller holds hdev->lock */
 static void hci_connect_le_scan_cleanup(struct hci_conn *conn)
 {
@@ -250,7 +226,7 @@ int hci_disconnect(struct hci_conn *conn, __u8 reason)
 	return hci_abort_conn(conn, reason);
 }
 
-static void hci_add_sco(struct hci_conn *conn, __u16 handle)
+static int hci_add_sco(struct hci_conn *conn, __u16 handle)
 {
 	struct hci_dev *hdev = conn->hdev;
 	struct hci_cp_add_sco cp;
@@ -262,17 +238,21 @@ static void hci_add_sco(struct hci_conn *conn, __u16 handle)
 
 	conn->attempt++;
 
+	if (conn->voice_setup->voice_setting != hdev->voice_setting)
+		return -EOPNOTSUPP;
+
 	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);
+	return hci_send_cmd(hdev, HCI_OP_ADD_SCO, sizeof(cp), &cp);
 }
 
-bool hci_setup_sync(struct hci_conn *conn, __u16 handle)
+int 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;
+	const struct bt_voice_setup *voice_setup = conn->voice_setup;
+	unsigned int i, j;
 
 	BT_DBG("hcon %p", conn);
 
@@ -281,41 +261,26 @@ bool hci_setup_sync(struct hci_conn *conn, __u16 handle)
 
 	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;
+	for (j = 0, i = 0; i < voice_setup->pkt_types_count && j < conn->attempt; i++) {
+		if (conn->type == ESCO_LINK && !(voice_setup->pkt_types[i].capability & BT_VOICE_PKT_TYPE_CAP_ESCO))
+			continue;
+		if (conn->type == SCO_LINK && !(voice_setup->pkt_types[i].capability & BT_VOICE_PKT_TYPE_CAP_SCO))
+			continue;
+		j++;
 	}
 
-	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 (j != conn->attempt)
+		return -EINVAL;
 
-	if (hci_send_cmd(hdev, HCI_OP_SETUP_SYNC_CONN, sizeof(cp), &cp) < 0)
-		return false;
+	cp.handle         = cpu_to_le16(handle);
+	cp.tx_bandwidth   = cpu_to_le32(voice_setup->tx_bandwidth);
+	cp.rx_bandwidth   = cpu_to_le32(voice_setup->rx_bandwidth);
+	cp.voice_setting  = cpu_to_le16(voice_setup->voice_setting);
+	cp.pkt_type       = cpu_to_le16(voice_setup->pkt_types[j-1].pkt_type);
+	cp.max_latency    = cpu_to_le16(voice_setup->pkt_types[j-1].max_latency);
+	cp.retrans_effort = voice_setup->pkt_types[j-1].retrans_effort;
 
-	return true;
+	return hci_send_cmd(hdev, HCI_OP_SETUP_SYNC_CONN, sizeof(cp), &cp);
 }
 
 u8 hci_le_conn_update(struct hci_conn *conn, u16 min, u16 max, u16 latency,
@@ -373,24 +338,38 @@ void hci_le_start_enc(struct hci_conn *conn, __le16 ediv, __le64 rand,
 }
 
 /* Device _must_ be locked */
-void hci_sco_setup(struct hci_conn *conn, __u8 status)
+int hci_sco_setup(struct hci_conn *conn, __u8 status)
 {
 	struct hci_conn *sco = conn->link;
+	int err = 0;
 
 	if (!sco)
-		return;
+		return -EINVAL;
 
 	BT_DBG("hcon %p", conn);
 
 	if (!status) {
-		if (lmp_esco_capable(conn->hdev))
-			hci_setup_sync(sco, conn->handle);
+		bool can_use_add_sco = !(conn->voice_setup->features & BT_VOICE_SETUP_FEATURE_CONFIG) || (conn->voice_setup->features & BT_VOICE_SETUP_FEATURE_ADD_SCO);
+		bool can_use_setup_sco = lmp_esco_capable(conn->hdev) && (!(conn->voice_setup->features & BT_VOICE_SETUP_FEATURE_CONFIG) || (conn->voice_setup->features & BT_VOICE_SETUP_FEATURE_SETUP_SCO));
+		bool can_use_enh_setup_sco = false /* (conn->hdev->commands[29] & BIT(3)) && (!(conn->voice_setup->features & BT_VOICE_SETUP_FEATURE_CONFIG) || (conn->voice_setup->features & BT_VOICE_SETUP_FEATURE_ENH_SETUP_SCO)) */ ; /* FIXME: Enhanced Setup Synchronous Connection is unimplemented */
+		if (!lmp_voice_setting_compatible(conn->hdev, conn->voice_setup->voice_setting))
+			err = -EOPNOTSUPP;
+#if 0
+		else if (can_use_enh_setup_sco)
+			err = hci_enh_setup_sync(sco, conn->handle); /* TODO */
+#endif
+		else if (can_use_setup_sco)
+			err = hci_setup_sync(sco, conn->handle);
+		else if (can_use_add_sco)
+			err = hci_add_sco(sco, conn->handle);
 		else
-			hci_add_sco(sco, conn->handle);
+			err = -EOPNOTSUPP;
 	} else {
 		hci_connect_cfm(sco, status);
 		hci_conn_del(sco);
 	}
+
+	return err;
 }
 
 static void hci_conn_timeout(struct work_struct *work)
@@ -1214,10 +1193,11 @@ struct hci_conn *hci_connect_acl(struct hci_dev *hdev, bdaddr_t *dst,
 }
 
 struct hci_conn *hci_connect_sco(struct hci_dev *hdev, int type, bdaddr_t *dst,
-				 __u16 setting)
+				 const struct bt_voice_setup *voice_setup)
 {
 	struct hci_conn *acl;
 	struct hci_conn *sco;
+	int err;
 
 	acl = hci_connect_acl(hdev, dst, BT_SECURITY_LOW, HCI_AT_NO_BONDING);
 	if (IS_ERR(acl))
@@ -1237,7 +1217,7 @@ struct hci_conn *hci_connect_sco(struct hci_dev *hdev, int type, bdaddr_t *dst,
 
 	hci_conn_hold(sco);
 
-	sco->setting = setting;
+	sco->voice_setup = voice_setup;
 
 	if (acl->state == BT_CONNECTED &&
 	    (sco->state == BT_OPEN || sco->state == BT_CLOSED)) {
@@ -1250,7 +1230,11 @@ struct hci_conn *hci_connect_sco(struct hci_dev *hdev, int type, bdaddr_t *dst,
 			return sco;
 		}
 
-		hci_sco_setup(acl, 0x00);
+		err = hci_sco_setup(acl, 0x00);
+		if (err) {
+			hci_conn_drop(sco);
+			return ERR_PTR(err);
+		}
 	}
 
 	return sco;
diff --git a/net/bluetooth/hci_event.c b/net/bluetooth/hci_event.c
index 609fd6871c5a..2e7156bcabc3 100644
--- a/net/bluetooth/hci_event.c
+++ b/net/bluetooth/hci_event.c
@@ -2601,6 +2601,7 @@ static void hci_conn_request_evt(struct hci_dev *hdev, struct sk_buff *skb)
 		bacpy(&cp.bdaddr, &ev->bdaddr);
 		cp.pkt_type = cpu_to_le16(conn->pkt_type);
 
+		/* FIXME: use voice_setup */
 		cp.tx_bandwidth   = cpu_to_le32(0x00001f40);
 		cp.rx_bandwidth   = cpu_to_le32(0x00001f40);
 		cp.max_latency    = cpu_to_le16(0xffff);
@@ -4137,7 +4138,7 @@ static void hci_sync_conn_complete_evt(struct hci_dev *hdev,
 		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))
+			if (hci_setup_sync(conn, conn->link->handle) == 0)
 				goto unlock;
 		}
 		/* fall through */
diff --git a/net/bluetooth/hci_request.c b/net/bluetooth/hci_request.c
index ca73d36cc149..7d8443d7e30e 100644
--- a/net/bluetooth/hci_request.c
+++ b/net/bluetooth/hci_request.c
@@ -2205,6 +2205,16 @@ void __hci_abort_conn(struct hci_request *req, struct hci_conn *conn,
 			 */
 			rej.reason = HCI_ERROR_REJ_LIMITED_RESOURCES;
 
+			if (!lmp_esco_capable(conn->hdev)) {
+				/* If HCI_OP_REJECT_SYNC_CONN_REQ is not supported then use HCI_OP_REJECT_CONN_REQ */
+				struct hci_cp_reject_conn_req rej2;
+				bacpy(&rej2.bdaddr, &rej.bdaddr);
+				rej2.reason = rej.reason;
+				hci_req_add(req, HCI_OP_REJECT_CONN_REQ,
+					    sizeof(rej2), &rej2);
+				break;
+			}
+
 			hci_req_add(req, HCI_OP_REJECT_SYNC_CONN_REQ,
 				    sizeof(rej), &rej);
 		}
diff --git a/net/bluetooth/sco.c b/net/bluetooth/sco.c
index 9a580999ca57..a3c5cae7a2a2 100644
--- a/net/bluetooth/sco.c
+++ b/net/bluetooth/sco.c
@@ -33,6 +33,39 @@
 #include <net/bluetooth/hci_core.h>
 #include <net/bluetooth/sco.h>
 
+static const struct bt_voice_setup voice_setup_cvsd = {
+	.features = BT_VOICE_SETUP_FEATURE_CONFIG | BT_VOICE_SETUP_FEATURE_ADD_SCO | BT_VOICE_SETUP_FEATURE_SETUP_SCO,
+	/* TODO: Add configuration for BT_VOICE_SETUP_FEATURE_ENH_SETUP_SCO */
+	.voice_setting = 0x0060,
+	.tx_bandwidth = 8000,
+	.rx_bandwidth = 8000,
+	.pkt_types_count = 7,
+	.pkt_types = {
+		{ BT_VOICE_PKT_TYPE_CAP_ESCO, 0x01, EDR_ESCO_MASK & ~ESCO_2EV3, 0x000a }, /* S3 */
+		{ BT_VOICE_PKT_TYPE_CAP_ESCO, 0x01, EDR_ESCO_MASK & ~ESCO_2EV3, 0x0007 }, /* S2 */
+		{ BT_VOICE_PKT_TYPE_CAP_ESCO, 0x01, EDR_ESCO_MASK | ESCO_EV3,   0x0007 }, /* S1 */
+		{ BT_VOICE_PKT_TYPE_CAP_ESCO, 0x01, EDR_ESCO_MASK | ESCO_HV3,   0xffff }, /* D1 */
+		{ BT_VOICE_PKT_TYPE_CAP_ESCO, 0x01, EDR_ESCO_MASK | ESCO_HV1,   0xffff }, /* D0 */
+		{ BT_VOICE_PKT_TYPE_CAP_ESCO, 0xff, EDR_ESCO_MASK | ESCO_HV3,   0xffff }, /* D1 */
+		{ BT_VOICE_PKT_TYPE_CAP_ESCO, 0xff, EDR_ESCO_MASK | ESCO_HV1,   0xffff }, /* D0 */
+		{ BT_VOICE_PKT_TYPE_CAP_SCO,  0xff, EDR_ESCO_MASK | ESCO_HV3,   0xffff }, /* D1 */
+		{ BT_VOICE_PKT_TYPE_CAP_SCO,  0xff, EDR_ESCO_MASK | ESCO_HV1,   0xffff }, /* D0 */
+	},
+};
+
+static const struct bt_voice_setup voice_setup_msbc = {
+	.features = BT_VOICE_SETUP_FEATURE_CONFIG | BT_VOICE_SETUP_FEATURE_SETUP_SCO,
+	/* TODO: Add configuration for BT_VOICE_SETUP_FEATURE_ENH_SETUP_SCO */
+	.voice_setting = 0x0003,
+	.tx_bandwidth = 8000,
+	.rx_bandwidth = 8000,
+	.pkt_types_count = 2,
+	.pkt_types = {
+		{ BT_VOICE_PKT_TYPE_CAP_ESCO, 0x02, EDR_ESCO_MASK & ~ESCO_2EV3, 0x000d }, /* T2 */
+		{ BT_VOICE_PKT_TYPE_CAP_ESCO, 0x02, EDR_ESCO_MASK | ESCO_EV3,   0x0008 }, /* T1 */
+	},
+};
+
 static bool disable_esco;
 
 static const struct proto_ops sco_sock_ops;
@@ -65,8 +98,8 @@ struct sco_pinfo {
 	bdaddr_t	src;
 	bdaddr_t	dst;
 	__u32		flags;
-	__u16		setting;
 	struct sco_conn	*conn;
+	struct bt_voice_setup *voice_setup;
 };
 
 /* ---- SCO timers ---- */
@@ -231,14 +264,31 @@ static int sco_connect(struct sock *sk)
 	else
 		type = SCO_LINK;
 
-	if (sco_pi(sk)->setting == BT_VOICE_TRANSPARENT &&
-	    (!lmp_transp_capable(hdev) || !lmp_esco_capable(hdev))) {
-		err = -EOPNOTSUPP;
-		goto done;
+	/* If setsockopt BT_VOICE_SETUP was not called or called without additional features then use default parameters from CVSD or mSBC codec based on voice_setting */
+	if (!sco_pi(sk)->voice_setup || !(sco_pi(sk)->voice_setup->features & BT_VOICE_SETUP_FEATURE_CONFIG) || !sco_pi(sk)->voice_setup->pkt_types_count) {
+		/* If setsockopt BT_VOICE or BT_VOICE_SETUP was not called then use global voice_setting */
+		__u16 voice_setting = sco_pi(sk)->voice_setup ? sco_pi(sk)->voice_setup->voice_setting : hdev->voice_setting;
+		const struct bt_voice_setup *source_voice_setup = ((voice_setting & SCO_AIRMODE_MASK) == SCO_AIRMODE_TRANSP) ? &voice_setup_msbc : &voice_setup_cvsd;
+		size_t voice_setup_size = sizeof(struct bt_voice_setup) + sizeof(struct bt_voice_pkt_type)*source_voice_setup->pkt_types_count;
+		bool set_only_pkt_types = (sco_pi(sk)->voice_setup && (sco_pi(sk)->voice_setup->features & BT_VOICE_SETUP_FEATURE_CONFIG));
+		struct bt_voice_setup *new_voice_setup = krealloc(sco_pi(sk)->voice_setup, voice_setup_size, GFP_KERNEL);
+		if (!new_voice_setup) {
+			err = -ENOMEM;
+			goto done;
+		}
+		sco_pi(sk)->voice_setup = new_voice_setup;
+		if (set_only_pkt_types) {
+			sco_pi(sk)->voice_setup->pkt_types_count = source_voice_setup->pkt_types_count;
+			memcpy(sco_pi(sk)->voice_setup->pkt_types, source_voice_setup->pkt_types, sizeof(struct bt_voice_pkt_type)*source_voice_setup->pkt_types_count);
+		} else {
+			memcpy(sco_pi(sk)->voice_setup, source_voice_setup, voice_setup_size);
+			memcpy(sco_pi(sk)->voice_setup, source_voice_setup, voice_setup_size);
+			sco_pi(sk)->voice_setup->voice_setting = voice_setting;
+		}
 	}
 
 	hcon = hci_connect_sco(hdev, type, &sco_pi(sk)->dst,
-			       sco_pi(sk)->setting);
+			       sco_pi(sk)->voice_setup);
 	if (IS_ERR(hcon)) {
 		err = PTR_ERR(hcon);
 		goto done;
@@ -368,6 +418,7 @@ static void sco_sock_destruct(struct sock *sk)
 {
 	BT_DBG("sk %p", sk);
 
+	kfree(sco_pi(sk)->voice_setup);
 	skb_queue_purge(&sk->sk_receive_queue);
 	skb_queue_purge(&sk->sk_write_queue);
 }
@@ -486,8 +537,6 @@ static struct sock *sco_sock_alloc(struct net *net, struct socket *sock,
 	sk->sk_protocol = proto;
 	sk->sk_state    = BT_OPEN;
 
-	sco_pi(sk)->setting = BT_VOICE_CVSD_16BIT;
-
 	timer_setup(&sk->sk_timer, sco_sock_timeout, 0);
 
 	bt_sock_link(&sco_sk_list, sk);
@@ -724,47 +773,100 @@ static int sco_sock_sendmsg(struct socket *sock, struct msghdr *msg,
 	return err;
 }
 
-static void sco_conn_defer_accept(struct hci_conn *conn, u16 setting)
+static void sco_conn_defer_accept(struct hci_conn *conn, const struct bt_voice_setup *voice_setup)
 {
 	struct hci_dev *hdev = conn->hdev;
+	bool can_use_add_sco = !(voice_setup && (voice_setup->features & BT_VOICE_SETUP_FEATURE_CONFIG)) || (voice_setup->features & BT_VOICE_SETUP_FEATURE_ADD_SCO);
+	bool can_use_setup_sco = lmp_esco_capable(hdev) && (!voice_setup || !(voice_setup->features & BT_VOICE_SETUP_FEATURE_CONFIG) || (voice_setup->features & BT_VOICE_SETUP_FEATURE_SETUP_SCO));
+	bool can_use_enh_setup_sco = false /* (hdev->commands[29] & BIT(4)) && (!voice_setup || !(voice_setup->features & BT_VOICE_SETUP_FEATURE_CONFIG) || (voice_setup->features & BT_VOICE_SETUP_FEATURE_ENH_SETUP_SCO)) */ ; /* FIXME: Enhanced Setup Synchronous Connection is unimplemented */
 
 	BT_DBG("conn %p", conn);
 
 	conn->state = BT_CONFIG;
 
-	if (!lmp_esco_capable(hdev)) {
+	if (voice_setup && !lmp_voice_setting_compatible(hdev, voice_setup->voice_setting)) {
+		/* TODO: Throw error about incompatible setup for accept() */
+	}
+
+	if (conn->type == ESCO_LINK && disable_esco) {
+		/* TODO: Throw error about incompatible setup for accept() */
+	}
+
+	if (can_use_add_sco && !can_use_setup_sco && !can_use_enh_setup_sco) {
 		struct hci_cp_accept_conn_req cp;
 
+		if (voice_setup && (voice_setup->voice_setting != hdev->voice_setting)) {
+			/* TODO: Throw error about incompatible setup for accept() */
+		}
+
 		bacpy(&cp.bdaddr, &conn->dst);
 		cp.role = 0x00; /* Ignored */
 
 		hci_send_cmd(hdev, HCI_OP_ACCEPT_CONN_REQ, sizeof(cp), &cp);
-	} else {
+	} else if (can_use_setup_sco && !can_use_enh_setup_sco) {
 		struct hci_cp_accept_sync_conn_req cp;
+		const struct bt_voice_pkt_type *pkt_types;
+		unsigned int i, pkt_type_next, pkt_types_count;
 
 		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);
+		if (!voice_setup) {
+			cp.content_format = cpu_to_le16(hdev->voice_setting);
+			if ((hdev->voice_setting & SCO_AIRMODE_MASK) == SCO_AIRMODE_TRANSP)
+				voice_setup = &voice_setup_msbc;
 			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;
+				voice_setup = &voice_setup_cvsd;
+		} else {
+			cp.content_format = cpu_to_le16(voice_setup->voice_setting);
+		}
+
+		if (voice_setup->features & BT_VOICE_SETUP_FEATURE_CONFIG) {
+			cp.tx_bandwidth = cpu_to_le32(voice_setup->tx_bandwidth);
+			cp.rx_bandwidth = cpu_to_le32(voice_setup->rx_bandwidth);
+		} else if ((voice_setup->voice_setting & SCO_AIRMODE_MASK) == SCO_AIRMODE_TRANSP) {
+			cp.tx_bandwidth = cpu_to_le32(voice_setup_msbc.tx_bandwidth);
+			cp.rx_bandwidth = cpu_to_le32(voice_setup_msbc.rx_bandwidth);
+		} else {
+			cp.tx_bandwidth = cpu_to_le32(voice_setup_cvsd.tx_bandwidth);
+			cp.rx_bandwidth = cpu_to_le32(voice_setup_cvsd.rx_bandwidth);
+		}
+
+		if ((voice_setup->features & BT_VOICE_SETUP_FEATURE_CONFIG) && voice_setup->pkt_types_count) {
+			pkt_types = voice_setup->pkt_types;
+			pkt_types_count = voice_setup->pkt_types_count;
+		} else if ((voice_setup->voice_setting & SCO_AIRMODE_MASK) == SCO_AIRMODE_TRANSP) {
+			pkt_types = voice_setup_msbc.pkt_types;
+			pkt_types_count = voice_setup_msbc.pkt_types_count;
+		} else {
+			pkt_types = voice_setup_cvsd.pkt_types;
+			pkt_types_count = voice_setup_cvsd.pkt_types_count;
+		}
+
+		for (pkt_type_next = 0, i = 0; i < pkt_types_count; ++i) {
+			if (conn->type == ESCO_LINK && !(pkt_types[i].capability & BT_VOICE_PKT_TYPE_CAP_ESCO))
+				continue;
+			if (conn->type == SCO_LINK && !(pkt_types[i].capability & BT_VOICE_PKT_TYPE_CAP_SCO))
+				continue;
+			if (!pkt_type_next)
+				pkt_type_next = i;
+			if (!(pkt_types[i].pkt_type & conn->pkt_type))
+				continue;
 			break;
 		}
 
+		if (i == pkt_types_count)
+			i = pkt_type_next;
+
+		cp.max_latency = cpu_to_le16(pkt_types[i].max_latency);
+		cp.retrans_effort = pkt_types[i].retrans_effort;
+
 		hci_send_cmd(hdev, HCI_OP_ACCEPT_SYNC_CONN_REQ,
 			     sizeof(cp), &cp);
+	} else if (can_use_enh_setup_sco) {
+		/* TODO */
+	} else {
+		/* TODO: Throw error about incompatible setup for accept() */
 	}
 }
 
@@ -778,9 +880,7 @@ static int sco_sock_recvmsg(struct socket *sock, struct msghdr *msg,
 
 	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;
-
+		sco_conn_defer_accept(pi->conn->hcon, pi->voice_setup);
 		release_sock(sk);
 		return 0;
 	}
@@ -794,8 +894,10 @@ 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;
+	int err = 0;
 	struct bt_voice voice;
+	char buffer[1536]; /* FIXME: max size */
+	struct bt_voice_setup *new_voice_setup;
 	u32 opt;
 
 	BT_DBG("sk %p", sk);
@@ -828,22 +930,62 @@ static int sco_sock_setsockopt(struct socket *sock, int level, int optname,
 			break;
 		}
 
-		voice.setting = sco_pi(sk)->setting;
+		if (optlen != sizeof(voice)) {
+			err = -EINVAL;
+			break;
+		}
 
-		len = min_t(unsigned int, sizeof(voice), optlen);
-		if (copy_from_user((char *)&voice, optval, len)) {
+		if (copy_from_user((char *)&voice, optval, optlen)) {
 			err = -EFAULT;
 			break;
 		}
 
-		/* Explicitly check for these values */
-		if (voice.setting != BT_VOICE_TRANSPARENT &&
-		    voice.setting != BT_VOICE_CVSD_16BIT) {
+		if (!sco_pi(sk)->voice_setup) {
+			sco_pi(sk)->voice_setup = kzalloc(sizeof(struct bt_voice_setup), GFP_KERNEL);
+			if (!sco_pi(sk)->voice_setup) {
+				err = -ENOMEM;
+				break;
+			}
+		}
+
+		sco_pi(sk)->voice_setup->voice_setting = voice.setting;
+
+		break;
+
+	case BT_VOICE_SETUP:
+		if (sk->sk_state != BT_OPEN && sk->sk_state != BT_BOUND &&
+		    sk->sk_state != BT_CONNECT2) {
+			err = -EINVAL;
+			break;
+		}
+
+		/* FIXME: max size limit */
+		if (optlen < sizeof(struct bt_voice_setup) || (optlen-sizeof(struct bt_voice_setup)) % sizeof(struct bt_voice_pkt_type) != 0 || optlen > sizeof(buffer)) {
+			err = -EINVAL;
+			break;
+		}
+
+		if (copy_from_user((char *)&buffer, optval, optlen)) {
+			err = -EFAULT;
+			break;
+		}
+
+		if (((struct bt_voice_setup *)&buffer)->pkt_types_count > (optlen - sizeof(struct bt_voice_setup)) / sizeof(struct bt_voice_pkt_type)) {
 			err = -EINVAL;
 			break;
 		}
 
-		sco_pi(sk)->setting = voice.setting;
+		/* FIXME: should we check if disable_esco is set and in voice_setup is at least one BT_VOICE_PKT_TYPE_CAP_SCO pkt_type? */
+
+		new_voice_setup = krealloc(sco_pi(sk)->voice_setup, optlen, GFP_KERNEL);
+		if (!new_voice_setup) {
+			err = -ENOMEM;
+			break;
+		}
+
+		memcpy(new_voice_setup, &buffer, optlen);
+		sco_pi(sk)->voice_setup = new_voice_setup;
+
 		break;
 
 	default:
@@ -948,7 +1090,13 @@ static int sco_sock_getsockopt(struct socket *sock, int level, int optname,
 		break;
 
 	case BT_VOICE:
-		voice.setting = sco_pi(sk)->setting;
+		/* When voice setting were not set then due to backward compatibility return something: either voice settings (if are available) or fallback to CVSD voice setting */
+		if (sco_pi(sk)->voice_setup)
+			voice.setting = sco_pi(sk)->voice_setup->voice_setting;
+		else if (sco_pi(sk)->conn)
+			voice.setting = sco_pi(sk)->conn->hcon->hdev->voice_setting;
+		else
+			voice.setting = voice_setup_cvsd.voice_setting;
 
 		len = min_t(unsigned int, len, sizeof(voice));
 		if (copy_to_user(optval, (char *)&voice, len))
@@ -956,6 +1104,24 @@ static int sco_sock_getsockopt(struct socket *sock, int level, int optname,
 
 		break;
 
+	case BT_VOICE_SETUP:
+		if (sco_pi(sk)->voice_setup) {
+			len = min_t(unsigned int, len, sizeof(struct bt_voice_setup) + sizeof(struct bt_voice_pkt_type)*sco_pi(sk)->voice_setup->pkt_types_count);
+			if (copy_to_user(optval, (char *)sco_pi(sk)->voice_setup, len)) {
+				err = -EFAULT;
+				break;
+			}
+		} else {
+			len = 0;
+		}
+
+		if (put_user(len, optlen))  {
+			err = -EFAULT;
+			break;
+		}
+
+		break;
+
 	default:
 		err = -ENOPROTOOPT;
 		break;

[-- Attachment #2: signature.asc --]
[-- Type: application/pgp-signature, Size: 195 bytes --]