2 files changed, 43 insertions, 20 deletions

diff --git a/net/bluetooth/hci_conn.c b/net/bluetooth/hci_conn.c
index e9fef83449f8..0b1e460fe440 100644
--- a/net/bluetooth/hci_conn.c
+++ b/net/bluetooth/hci_conn.c
@@ -155,6 +155,27 @@ void hci_setup_sync(struct hci_conn *conn, __u16 handle)
         hci_send_cmd(hdev, HCI_OP_SETUP_SYNC_CONN, sizeof(cp), &cp);
 }
 
+/* Device _must_ be locked */
+void hci_sco_setup(struct hci_conn *conn, __u8 status)
+{
+        struct hci_conn *sco = conn->link;
+
+        BT_DBG("%p", conn);
+
+        if (!sco)
+                return;
+
+        if (!status) {
+                if (lmp_esco_capable(conn->hdev))
+                        hci_setup_sync(sco, conn->handle);
+                else
+                        hci_add_sco(sco, conn->handle);
+        } else {
+                hci_proto_connect_cfm(sco, status);
+                hci_conn_del(sco);
+        }
+}
+
 static void hci_conn_timeout(unsigned long arg)
 {
         struct hci_conn *conn = (void *) arg;
@@ -385,10 +406,13 @@ struct hci_conn *hci_connect(struct hci_dev *hdev, int type, bdaddr_t *dst, __u8
                 acl->power_save = 1;
                 hci_conn_enter_active_mode(acl);
 
-                if (lmp_esco_capable(hdev))
-                        hci_setup_sync(sco, acl->handle);
-                else
-                        hci_add_sco(sco, acl->handle);
+                if (test_bit(HCI_CONN_MODE_CHANGE_PEND, &acl->pend)) {
+                        /* defer SCO setup until mode change completed */
+                        set_bit(HCI_CONN_SCO_SETUP_PEND, &acl->pend);
+                        return sco;
+                }
+
+                hci_sco_setup(acl, 0x00);
         }
 
         return sco;
diff --git a/net/bluetooth/hci_event.c b/net/bluetooth/hci_event.c
index 2069c3b05fda..bfef5bae0b3a 100644
--- a/net/bluetooth/hci_event.c
+++ b/net/bluetooth/hci_event.c
@@ -785,9 +785,13 @@ static void hci_cs_sniff_mode(struct hci_dev *hdev, __u8 status)
         hci_dev_lock(hdev);
 
         conn = hci_conn_hash_lookup_handle(hdev, __le16_to_cpu(cp->handle));
-        if (conn)
+        if (conn) {
                 clear_bit(HCI_CONN_MODE_CHANGE_PEND, &conn->pend);
 
+                if (test_and_clear_bit(HCI_CONN_SCO_SETUP_PEND, &conn->pend))
+                        hci_sco_setup(conn, status);
+        }
+
         hci_dev_unlock(hdev);
 }
 
@@ -808,9 +812,13 @@ static void hci_cs_exit_sniff_mode(struct hci_dev *hdev, __u8 status)
         hci_dev_lock(hdev);
 
         conn = hci_conn_hash_lookup_handle(hdev, __le16_to_cpu(cp->handle));
-        if (conn)
+        if (conn) {
                 clear_bit(HCI_CONN_MODE_CHANGE_PEND, &conn->pend);
 
+                if (test_and_clear_bit(HCI_CONN_SCO_SETUP_PEND, &conn->pend))
+                        hci_sco_setup(conn, status);
+        }
+
         hci_dev_unlock(hdev);
 }
 
@@ -915,20 +923,8 @@ static inline void hci_conn_complete_evt(struct hci_dev *hdev, struct sk_buff *s
         } else
                 conn->state = BT_CLOSED;
 
-        if (conn->type == ACL_LINK) {
-                struct hci_conn *sco = conn->link;
-                if (sco) {
-                        if (!ev->status) {
-                                if (lmp_esco_capable(hdev))
-                                        hci_setup_sync(sco, conn->handle);
-                                else
-                                        hci_add_sco(sco, conn->handle);
-                        } else {
-                                hci_proto_connect_cfm(sco, ev->status);
-                                hci_conn_del(sco);
-                        }
-                }
-        }
+        if (conn->type == ACL_LINK)
+                hci_sco_setup(conn, ev->status);
 
         if (ev->status) {
                 hci_proto_connect_cfm(conn, ev->status);
@@ -1481,6 +1477,9 @@ static inline void hci_mode_change_evt(struct hci_dev *hdev, struct sk_buff *skb
                         else
                                 conn->power_save = 0;
                 }
+
+                if (test_and_clear_bit(HCI_CONN_SCO_SETUP_PEND, &conn->pend))
+                        hci_sco_setup(conn, ev->status);
         }
 
         hci_dev_unlock(hdev);