2 files changed, 23 insertions, 2 deletions

diff --git a/net/bluetooth/hci_conn.c b/net/bluetooth/hci_conn.c
index 98f97a1e9bbb..2435e830ba60 100644
--- a/net/bluetooth/hci_conn.c
+++ b/net/bluetooth/hci_conn.c
@@ -123,6 +123,8 @@ void hci_add_sco(struct hci_conn *conn, __u16 handle)
         conn->state = BT_CONNECT;
         conn->out = 1;
 
+        conn->attempt++;
+
         cp.handle   = cpu_to_le16(handle);
         cp.pkt_type = cpu_to_le16(conn->pkt_type);
 
@@ -139,6 +141,8 @@ void hci_setup_sync(struct hci_conn *conn, __u16 handle)
         conn->state = BT_CONNECT;
         conn->out = 1;
 
+        conn->attempt++;
+
         cp.handle   = cpu_to_le16(handle);
         cp.pkt_type = cpu_to_le16(conn->pkt_type);
 
@@ -216,12 +220,13 @@ struct hci_conn *hci_conn_add(struct hci_dev *hdev, int type, bdaddr_t *dst)
                 break;
         case SCO_LINK:
                 if (lmp_esco_capable(hdev))
-                        conn->pkt_type = hdev->esco_type & SCO_ESCO_MASK;
+                        conn->pkt_type = (hdev->esco_type & SCO_ESCO_MASK) |
+                                        (hdev->esco_type & EDR_ESCO_MASK);
                 else
                         conn->pkt_type = hdev->pkt_type & SCO_PTYPE_MASK;
                 break;
         case ESCO_LINK:
-                conn->pkt_type = hdev->esco_type;
+                conn->pkt_type = hdev->esco_type & ~EDR_ESCO_MASK;
                 break;
         }
 
diff --git a/net/bluetooth/hci_event.c b/net/bluetooth/hci_event.c
index 014fc8b320ba..899b8991a466 100644
--- a/net/bluetooth/hci_event.c
+++ b/net/bluetooth/hci_event.c
@@ -484,6 +484,15 @@ static void hci_cc_read_local_features(struct hci_dev *hdev, struct sk_buff *skb
         if (hdev->features[4] & LMP_EV5)
                 hdev->esco_type |= (ESCO_EV5);
 
+        if (hdev->features[5] & LMP_EDR_ESCO_2M)
+                hdev->esco_type |= (ESCO_2EV3);
+
+        if (hdev->features[5] & LMP_EDR_ESCO_3M)
+                hdev->esco_type |= (ESCO_3EV3);
+
+        if (hdev->features[5] & LMP_EDR_3S_ESCO)
+                hdev->esco_type |= (ESCO_2EV5 | ESCO_3EV5);
+
         BT_DBG("%s features 0x%.2x%.2x%.2x%.2x%.2x%.2x%.2x%.2x", hdev->name,
                                         hdev->features[0], hdev->features[1],
                                         hdev->features[2], hdev->features[3],
@@ -1639,6 +1648,13 @@ static inline void hci_sync_conn_complete_evt(struct hci_dev *hdev, struct sk_bu
                 conn->type = SCO_LINK;
         }
 
+        if (conn->out && ev->status == 0x1c && conn->attempt < 2) {
+                conn->pkt_type = (hdev->esco_type & SCO_ESCO_MASK) |
+                                        (hdev->esco_type & EDR_ESCO_MASK);
+                hci_setup_sync(conn, conn->link->handle);
+                goto unlock;
+        }
+
         if (!ev->status) {
                 conn->handle = __le16_to_cpu(ev->handle);
                 conn->state  = BT_CONNECTED;