1 files changed, 50 insertions, 12 deletions

diff --git a/net/bluetooth/hci_conn.c b/net/bluetooth/hci_conn.c
index 6c7f36379722..f0817121ec5e 100644
--- a/net/bluetooth/hci_conn.c
+++ b/net/bluetooth/hci_conn.c
@@ -31,6 +31,24 @@
 #include <net/bluetooth/a2mp.h>
 #include <net/bluetooth/smp.h>
 
+struct sco_param {
+        u16 pkt_type;
+        u16 max_latency;
+};
+
+static const struct sco_param sco_param_cvsd[] = {
+        { EDR_ESCO_MASK & ~ESCO_2EV3, 0x000a }, /* S3 */
+        { EDR_ESCO_MASK & ~ESCO_2EV3, 0x0007 }, /* S2 */
+        { EDR_ESCO_MASK | ESCO_EV3,   0x0007 }, /* S1 */
+        { EDR_ESCO_MASK | ESCO_HV3,   0xffff }, /* D1 */
+        { EDR_ESCO_MASK | ESCO_HV1,   0xffff }, /* D0 */
+};
+
+static const struct sco_param sco_param_wideband[] = {
+        { EDR_ESCO_MASK & ~ESCO_2EV3, 0x000d }, /* T2 */
+        { EDR_ESCO_MASK | ESCO_EV3,   0x0008 }, /* T1 */
+};
+
 static void hci_le_create_connection(struct hci_conn *conn)
 {
         struct hci_dev *hdev = conn->hdev;
@@ -172,10 +190,11 @@ static void hci_add_sco(struct hci_conn *conn, __u16 handle)
         hci_send_cmd(hdev, HCI_OP_ADD_SCO, sizeof(cp), &cp);
 }
 
-void hci_setup_sync(struct hci_conn *conn, __u16 handle)
+bool 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;
 
         BT_DBG("hcon %p", conn);
 
@@ -185,15 +204,35 @@ void hci_setup_sync(struct hci_conn *conn, __u16 handle)
         conn->attempt++;
 
         cp.handle   = cpu_to_le16(handle);
-        cp.pkt_type = cpu_to_le16(conn->pkt_type);
 
         cp.tx_bandwidth   = __constant_cpu_to_le32(0x00001f40);
         cp.rx_bandwidth   = __constant_cpu_to_le32(0x00001f40);
-        cp.max_latency    = __constant_cpu_to_le16(0xffff);
-        cp.voice_setting  = cpu_to_le16(hdev->voice_setting);
-        cp.retrans_effort = 0xff;
+        cp.voice_setting  = cpu_to_le16(conn->setting);
+
+        switch (conn->setting & SCO_AIRMODE_MASK) {
+        case SCO_AIRMODE_TRANSP:
+                if (conn->attempt > ARRAY_SIZE(sco_param_wideband))
+                        return false;
+                cp.retrans_effort = 0x02;
+                param = &sco_param_wideband[conn->attempt - 1];
+                break;
+        case SCO_AIRMODE_CVSD:
+                if (conn->attempt > ARRAY_SIZE(sco_param_cvsd))
+                        return false;
+                cp.retrans_effort = 0x01;
+                param = &sco_param_cvsd[conn->attempt - 1];
+                break;
+        default:
+                return false;
+        }
 
-        hci_send_cmd(hdev, HCI_OP_SETUP_SYNC_CONN, sizeof(cp), &cp);
+        cp.pkt_type = __cpu_to_le16(param->pkt_type);
+        cp.max_latency = __cpu_to_le16(param->max_latency);
+
+        if (hci_send_cmd(hdev, HCI_OP_SETUP_SYNC_CONN, sizeof(cp), &cp) < 0)
+                return false;
+
+        return true;
 }
 
 void hci_le_conn_update(struct hci_conn *conn, u16 min, u16 max,
@@ -560,13 +599,13 @@ static struct hci_conn *hci_connect_acl(struct hci_dev *hdev, bdaddr_t *dst,
         return acl;
 }
 
-static struct hci_conn *hci_connect_sco(struct hci_dev *hdev, int type,
-                                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 hci_conn *acl;
         struct hci_conn *sco;
 
-        acl = hci_connect_acl(hdev, dst, sec_level, auth_type);
+        acl = hci_connect_acl(hdev, dst, BT_SECURITY_LOW, HCI_AT_NO_BONDING);
         if (IS_ERR(acl))
                 return acl;
 
@@ -584,6 +623,8 @@ static struct hci_conn *hci_connect_sco(struct hci_dev *hdev, int type,
 
         hci_conn_hold(sco);
 
+        sco->setting = setting;
+
         if (acl->state == BT_CONNECTED &&
             (sco->state == BT_OPEN || sco->state == BT_CLOSED)) {
                 set_bit(HCI_CONN_POWER_SAVE, &acl->flags);
@@ -612,9 +653,6 @@ struct hci_conn *hci_connect(struct hci_dev *hdev, int type, bdaddr_t *dst,
                 return hci_connect_le(hdev, dst, dst_type, sec_level, auth_type);
         case ACL_LINK:
                 return hci_connect_acl(hdev, dst, sec_level, auth_type);
-        case SCO_LINK:
-        case ESCO_LINK:
-                return hci_connect_sco(hdev, type, dst, sec_level, auth_type);
         }
 
         return ERR_PTR(-EINVAL);