Re: HCI Set custom bandwidth for AuriStream SCO codec

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

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

On Thursday 18 July 2019 12:09:39 Pali Rohár wrote:
> On Friday 12 July 2019 20:59:22 Marcel Holtmann wrote:
> > Hi Pali,
> > 
> > >>>>>>>>> to be honest, I would rather see WBS implementation finally
> > >>>>>>>>> reach PA before we start digging into this.
> > >>>>>>>> 
> > >>>>>>>> First I want to finish improving A2DP codec support in pulseaudio. Later
> > >>>>>>>> I can look at HSP/HFP profiles. Ideally it should have modular/plugin
> > >>>>>>>> extensible design. So the aim is that adding new codec would be very
> > >>>>>>>> simple, without need to hack something related to mSBC/WBC, AuriStream
> > >>>>>>>> or any other codec.
> > >>>>>>> 
> > >>>>>>> Well HSP don't have support for codec negotiation, but yes a modular
> > >>>>>>> design is probably recommended.
> > >>>>>>> 
> > >>>>>>>> But for AuriStream I need to set custom SCO parameters as described
> > >>>>>>>> below and currently kernel does not support it. This is why I'm asking
> > >>>>>>>> how kernel can export for userspace configuration of SCO parameters...
> > >>>>>>> 
> > >>>>>>> We can always come up with socket options but we got to see the value
> > >>>>>>> it would bring since AuriStream don't look that popular among
> > >>>>>>> headsets, at least Ive never seem any device advertising it like
> > >>>>>>> apt-X, etc.
> > >>>>>> 
> > >>>>>> Pali clearly has such device and he is willing to work on it. Surely
> > >>>>>> that means it is popular enough to be supported...?
> > >>>>> 
> > >>>>> Just put AT+CSRSF=0,0,0,0,0,7 to google search and you would see that
> > >>>>> not only I have such device...
> > >>>>> 
> > >>>>> So I would really would like to see that kernel finally stops blocking
> > >>>>> usage of this AuriStream codec.
> > >>>> 
> > >>>> we need to figure out on how we do the kernel API to allow you this specific setting.
> > >>> 
> > >>> Hi Marcel! Kernel API for userspace should be simple. Just add two
> > >>> ioctls for retrieving and setting structure with custom parameters:
> > >>> 
> > >>> syncPktTypes = 0x003F
> > >>> bandwidth = 4000
> > >>> max_latency = 16
> > >>> voice_settings = 0x63
> > >>> retx_effort = 2
> > >>> 
> > >>> Or add more ioctls, one ioctl per parameter. There is already only ioctl
> > >>> for voice settings and moreover it is whitelisted only for two values.
> > >> 
> > >> it is not that simple actually. Most profiles define a certain set of parameters and then they try to configure better settings and only fallback to a specification defined default as last resort.
> > > 
> > > Ok. I see that there is another "example" configuration for AuriStream
> > > with just different syncPktTypes = 0x02BF and bandwidth = 3850.
> > > 
> > > So it really is not simple as it can be seen.
> > 
> > currently the stepping for mSBC and CVSD are hard-coded in esco_param_cvsd and esco_param_msbc arrays in hci_conn.c and then selected by the ->setting parameter.
> > 
> > So either we provide an new socket option (for example BT_VOICE_EXT) or we extend BT_VOICE to allow providing the needed information. However this needs to be flexible array size since we should then be able to encode multiple stepping that are tried in order.
> > 
> > My preference is that we extend BT_VOICE and not introduce a new socket option. So feel free to propose how we can load the full tables into the SCO socket. I mean we are not really far off actually. The only difference is that currently the tables are in the hci_conn.c file and selected by the provided voice->setting. However nothing really stops us from providing the full table via user space.
> 
> Ok. I will look at it and I will try to propose how to extend current
> BT_VOICE ioctl API for supporting all those new parameters.

Below is inline MIME part with POC patch which try to implement a new
IOCTL (currently named BT_VOICE_SETUP) for configuring voice sco
settings.

It uses flexible array of parameters <tx_bandwidth, rx_bandwidth,
voice_setting, pkt_type, max_latency, retrans_effort>, but with
maximally 10 array members (due to usage of static array storage). cvsd
codec uses 7 different fallback settings (see voice_setup_cvsd), so for
POC 10 should be enough.

Because a new IOCL has different members then old BT_VOICE I rather
decided to introduce a new IOCTL and not hacking old IOCTL to accept two
different structures.

Please let me know what do you think about this API, if this is a way
how to continue or if something different is needed.

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

[-- Attachment #1.2: sco.patch --]
[-- Type: text/x-diff, Size: 16919 bytes --]

diff --git a/include/net/bluetooth/bluetooth.h b/include/net/bluetooth/bluetooth.h
index fabee6db0abb..0e9f4ac07220 100644
--- a/include/net/bluetooth/bluetooth.h
+++ b/include/net/bluetooth/bluetooth.h
@@ -122,6 +122,19 @@ struct bt_voice {
 #define BT_SNDMTU		12
 #define BT_RCVMTU		13
 
+#define BT_VOICE_SETUP		14
+#define BT_VOICE_SETUP_ARRAY_SIZE 10
+struct bt_voice_setup {
+	__u8 sco_capable:1;
+	__u8 esco_capable:1;
+	__u32 tx_bandwidth;
+	__u32 rx_bandwidth;
+	__u16 voice_setting;
+	__u16 pkt_type;
+	__u16 max_latency;
+	__u8 retrans_effort;
+};
+
 __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..8f3c161da1c4 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;
+	struct bt_voice_setup voice_setup[BT_VOICE_SETUP_ARRAY_SIZE];
 	__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);
+				 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,
diff --git a/net/bluetooth/hci_conn.c b/net/bluetooth/hci_conn.c
index bd4978ce8c45..0aa2ad98eb80 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[0].voice_setting & SCO_AIRMODE_MASK) == SCO_AIRMODE_TRANSP)
+		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;
+	int err;
 
 	BT_DBG("hcon %p", conn);
 
@@ -281,41 +261,41 @@ bool hci_setup_sync(struct hci_conn *conn, __u16 handle)
 
 	conn->attempt++;
 
-	cp.handle   = cpu_to_le16(handle);
+	err = -EINVAL;
+	for (; conn->attempt <= ARRAY_SIZE(conn->voice_setup); conn->attempt++) {
+		voice_setup = &conn->voice_setup[conn->attempt - 1];
 
-	cp.tx_bandwidth   = cpu_to_le32(0x00001f40);
-	cp.rx_bandwidth   = cpu_to_le32(0x00001f40);
-	cp.voice_setting  = cpu_to_le16(conn->setting);
+		/* last valid entry in array */
+		if (!voice_setup->sco_capable && !voice_setup->esco_capable)
+			return err;
 
-	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];
+		if (!lmp_esco_capable(conn->link) && !voice_setup->sco_capable)
+			continue;
+
+		if (lmp_esco_capable(conn->link) && !voice_setup->esco_capable)
+			continue;
+
+		if ((voice_setup->voice_setting & SCO_AIRMODE_MASK) == SCO_AIRMODE_TRANSP &&
+		    (!lmp_esco_capable(conn->link) || !lmp_transp_capable(hdev) || !lmp_esco_capable(hdev))) {
+			err = -EOPNOTSUPP;
+			continue;
 		}
+
 		break;
-	default:
-		return false;
 	}
 
-	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 (conn->attempt > ARRAY_SIZE(conn->voice_setup))
+		return err;
 
-	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_type);
+	cp.max_latency    = cpu_to_le16(voice_setup->max_latency);
+	cp.retrans_effort = voice_setup->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 +353,27 @@ 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);
+			err = hci_setup_sync(sco, conn->handle);
 		else
-			hci_add_sco(sco, conn->handle);
+			err = hci_add_sco(sco, conn->handle);
 	} else {
 		hci_connect_cfm(sco, status);
 		hci_conn_del(sco);
 	}
+
+	return err;
 }
 
 static void hci_conn_timeout(struct work_struct *work)
@@ -1214,10 +1197,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)
+				 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 +1221,7 @@ struct hci_conn *hci_connect_sco(struct hci_dev *hdev, int type, bdaddr_t *dst,
 
 	hci_conn_hold(sco);
 
-	sco->setting = setting;
+	memcpy(sco->voice_setup, voice_setup, sizeof(sco->voice_setup));
 
 	if (acl->state == BT_CONNECTED &&
 	    (sco->state == BT_OPEN || sco->state == BT_CLOSED)) {
@@ -1250,7 +1234,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..9c39e5607a79 100644
--- a/net/bluetooth/hci_event.c
+++ b/net/bluetooth/hci_event.c
@@ -4137,7 +4137,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/sco.c b/net/bluetooth/sco.c
index 9a580999ca57..e7bb8ae297f7 100644
--- a/net/bluetooth/sco.c
+++ b/net/bluetooth/sco.c
@@ -33,6 +33,21 @@
 #include <net/bluetooth/hci_core.h>
 #include <net/bluetooth/sco.h>
 
+static const struct bt_voice_setup voice_setup_cvsd[BT_VOICE_SETUP_ARRAY_SIZE] = {
+	{ 0, 1, 8000, 8000, BT_VOICE_CVSD_16BIT,  EDR_ESCO_MASK & ~ESCO_2EV3, 0x000a, 0x01 }, /* S3 */
+	{ 0, 1, 8000, 8000, BT_VOICE_CVSD_16BIT,  EDR_ESCO_MASK & ~ESCO_2EV3, 0x0007, 0x01 }, /* S2 */
+	{ 0, 1, 8000, 8000, BT_VOICE_CVSD_16BIT,  EDR_ESCO_MASK | ESCO_EV3,   0x0007, 0x01 }, /* S1 */
+	{ 0, 1, 8000, 8000, BT_VOICE_CVSD_16BIT,  EDR_ESCO_MASK | ESCO_HV3,   0xffff, 0x01 }, /* D1 */
+	{ 0, 1, 8000, 8000, BT_VOICE_CVSD_16BIT,  EDR_ESCO_MASK | ESCO_HV1,   0xffff, 0x01 }, /* D0 */
+	{ 1, 0, 8000, 8000, BT_VOICE_CVSD_16BIT,  EDR_ESCO_MASK | ESCO_HV3,   0xffff, 0xff }, /* D1 */
+	{ 1, 0, 8000, 8000, BT_VOICE_CVSD_16BIT,  EDR_ESCO_MASK | ESCO_HV1,   0xffff, 0xff }, /* D0 */
+};
+
+static const struct bt_voice_setup voice_setup_msbc[BT_VOICE_SETUP_ARRAY_SIZE] = {
+	{ 0, 1, 8000, 8000, BT_VOICE_TRANSPARENT, EDR_ESCO_MASK & ~ESCO_2EV3, 0x000d, 0x02 }, /* T2 */
+	{ 0, 1, 8000, 8000, BT_VOICE_TRANSPARENT, EDR_ESCO_MASK | ESCO_EV3,   0x0008, 0x02 }, /* T1 */
+};
+
 static bool disable_esco;
 
 static const struct proto_ops sco_sock_ops;
@@ -65,7 +80,7 @@ struct sco_pinfo {
 	bdaddr_t	src;
 	bdaddr_t	dst;
 	__u32		flags;
-	__u16		setting;
+	struct bt_voice_setup voice_setup[BT_VOICE_SETUP_ARRAY_SIZE];
 	struct sco_conn	*conn;
 };
 
@@ -231,14 +246,8 @@ 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;
-	}
-
 	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;
@@ -486,7 +495,7 @@ 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;
+	memcpy(sco_pi(sk)->voice_setup, voice_setup_cvsd, sizeof(voice_setup_cvsd));
 
 	timer_setup(&sk->sk_timer, sco_sock_timeout, 0);
 
@@ -724,7 +733,7 @@ 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, struct bt_voice_setup *voice_setup)
 {
 	struct hci_dev *hdev = conn->hdev;
 
@@ -743,25 +752,12 @@ static void sco_conn_defer_accept(struct hci_conn *conn, u16 setting)
 		struct hci_cp_accept_sync_conn_req cp;
 
 		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);
-			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;
-			break;
-		}
+		cp.pkt_type       = cpu_to_le16(voice_setup->pkt_type);
+		cp.tx_bandwidth   = cpu_to_le32(voice_setup->tx_bandwidth);
+		cp.rx_bandwidth   = cpu_to_le32(voice_setup->rx_bandwidth);
+		cp.content_format = cpu_to_le16(voice_setup->voice_setting);
+		cp.max_latency    = cpu_to_le16(voice_setup->max_latency);
+		cp.retrans_effort = voice_setup->retrans_effort;
 
 		hci_send_cmd(hdev, HCI_OP_ACCEPT_SYNC_CONN_REQ,
 			     sizeof(cp), &cp);
@@ -778,7 +774,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);
+		sco_conn_defer_accept(pi->conn->hcon, &pi->voice_setup[0]);
 		sk->sk_state = BT_CONFIG;
 
 		release_sock(sk);
@@ -794,8 +790,9 @@ 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 i, err = 0;
 	struct bt_voice voice;
+	struct bt_voice_setup voice_setup[BT_VOICE_SETUP_ARRAY_SIZE];
 	u32 opt;
 
 	BT_DBG("sk %p", sk);
@@ -828,22 +825,56 @@ 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;
+		}
+
+		if (copy_from_user((char *)&voice, optval, optlen)) {
+			err = -EFAULT;
+			break;
+		}
+
+		if (voice.setting == BT_VOICE_CVSD_16BIT)
+			memcpy(sco_pi(sk)->voice_setup, voice_setup_cvsd, sizeof(voice_setup_cvsd));
+		else if (voice.setting == BT_VOICE_TRANSPARENT)
+			memcpy(sco_pi(sk)->voice_setup, voice_setup_msbc, sizeof(voice_setup_msbc));
+		else {
+			err = -EINVAL;
+			break;
+		}
+
+		break;
+
+	case BT_VOICE_SETUP:
+		if (sk->sk_state != BT_OPEN && sk->sk_state != BT_BOUND &&
+		    sk->sk_state != BT_CONNECT2) {
+			err = -EINVAL;
+			break;
+		}
+
+		if (!optlen || optlen % sizeof(voice_setup[0]) != 0 || optlen > sizeof(voice_setup)) {
+			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_setup, optval, optlen)) {
 			err = -EFAULT;
 			break;
 		}
 
-		/* Explicitly check for these values */
-		if (voice.setting != BT_VOICE_TRANSPARENT &&
-		    voice.setting != BT_VOICE_CVSD_16BIT) {
+		for (i = 0; i < optlen / sizeof(voice_setup[0]); i++) {
+			if (!voice_setup[i].sco_capable && !voice_setup[i].esco_capable)
+				break;
+		}
+		if (i != optlen / sizeof(voice_setup[0])) {
 			err = -EINVAL;
 			break;
 		}
 
-		sco_pi(sk)->setting = voice.setting;
+		memcpy(&sco_pi(sk)->voice_setup, voice_setup, optlen);
+		memset(&sco_pi(sk)->voice_setup + optlen, 0, sizeof(sco_pi(sk)->voice_setup) - optlen);
+
 		break;
 
 	default:
@@ -921,6 +952,7 @@ static int sco_sock_getsockopt(struct socket *sock, int level, int optname,
 {
 	struct sock *sk = sock->sk;
 	int len, err = 0;
+	unsigned int voice_setup_count;
 	struct bt_voice voice;
 
 	BT_DBG("sk %p", sk);
@@ -948,7 +980,7 @@ static int sco_sock_getsockopt(struct socket *sock, int level, int optname,
 		break;
 
 	case BT_VOICE:
-		voice.setting = sco_pi(sk)->setting;
+		voice.setting = sco_pi(sk)->voice_setup[0].voice_setting;
 
 		len = min_t(unsigned int, len, sizeof(voice));
 		if (copy_to_user(optval, (char *)&voice, len))
@@ -956,6 +988,25 @@ static int sco_sock_getsockopt(struct socket *sock, int level, int optname,
 
 		break;
 
+	case BT_VOICE_SETUP:
+		for (voice_setup_count = 0; voice_setup_count < ARRAY_SIZE(sco_pi(sk)->voice_setup); voice_setup_count++) {
+			if (!sco_pi(sk)->voice_setup[voice_setup_count].sco_capable && !sco_pi(sk)->voice_setup[voice_setup_count].esco_capable)
+				break;
+		}
+
+		len = min_t(unsigned int, len, voice_setup_count*sizeof(sco_pi(sk)->voice_setup[0]));
+		if (copy_to_user(optval, (char *)&sco_pi(sk)->voice_setup, len)) {
+			err = -EFAULT;
+			break;
+		}
+
+		if (put_user(len, optlen))  {
+			err = -EFAULT;
+			break;
+		}
+
+		break;
+
 	default:
 		err = -ENOPROTOOPT;
 		break;

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