4 files changed, 150 insertions, 22 deletions

diff --git a/net/bluetooth/hci_conn.c b/net/bluetooth/hci_conn.c
index 797a30bec6fd..9483320f6dad 100644
--- a/net/bluetooth/hci_conn.c
+++ b/net/bluetooth/hci_conn.c
@@ -78,9 +78,9 @@ void hci_acl_connect(struct hci_conn *conn)
 
         cp.pkt_type = cpu_to_le16(hdev->pkt_type & ACL_PTYPE_MASK);
         if (lmp_rswitch_capable(hdev) && !(hdev->link_mode & HCI_LM_MASTER))
-                cp.role_switch  = 0x01;
+                cp.role_switch = 0x01;
         else
-                cp.role_switch  = 0x00;
+                cp.role_switch = 0x00;
 
         hci_send_cmd(hdev, HCI_OP_CREATE_CONN, sizeof(cp), &cp);
 }
@@ -127,6 +127,28 @@ 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)
+{
+        struct hci_dev *hdev = conn->hdev;
+        struct hci_cp_setup_sync_conn cp;
+
+        BT_DBG("%p", conn);
+
+        conn->state = BT_CONNECT;
+        conn->out = 1;
+
+        cp.handle   = cpu_to_le16(handle);
+        cp.pkt_type = cpu_to_le16(hdev->esco_type);
+
+        cp.tx_bandwidth   = cpu_to_le32(0x00001f40);
+        cp.rx_bandwidth   = cpu_to_le32(0x00001f40);
+        cp.max_latency    = cpu_to_le16(0xffff);
+        cp.voice_setting  = cpu_to_le16(hdev->voice_setting);
+        cp.retrans_effort = 0xff;
+
+        hci_send_cmd(hdev, HCI_OP_SETUP_SYNC_CONN, sizeof(cp), &cp);
+}
+
 static void hci_conn_timeout(unsigned long arg)
 {
         struct hci_conn *conn = (void *) arg;
@@ -141,7 +163,10 @@ static void hci_conn_timeout(unsigned long arg)
 
         switch (conn->state) {
         case BT_CONNECT:
-                hci_acl_connect_cancel(conn);
+                if (conn->type == ACL_LINK)
+                        hci_acl_connect_cancel(conn);
+                else
+                        hci_acl_disconn(conn, 0x13);
                 break;
         case BT_CONNECTED:
                 hci_acl_disconn(conn, 0x13);
@@ -328,8 +353,12 @@ struct hci_conn *hci_connect(struct hci_dev *hdev, int type, bdaddr_t *dst)
         hci_conn_hold(sco);
 
         if (acl->state == BT_CONNECTED &&
-                        (sco->state == BT_OPEN || sco->state == BT_CLOSED))
-                hci_add_sco(sco, acl->handle);
+                        (sco->state == BT_OPEN || sco->state == BT_CLOSED)) {
+                if (lmp_esco_capable(hdev))
+                        hci_setup_sync(sco, acl->handle);
+                else
+                        hci_add_sco(sco, acl->handle);
+        }
 
         return sco;
 }
diff --git a/net/bluetooth/hci_core.c b/net/bluetooth/hci_core.c
index 2894382dd26d..372b0d3b75a8 100644
--- a/net/bluetooth/hci_core.c
+++ b/net/bluetooth/hci_core.c
@@ -1361,6 +1361,26 @@ static inline void hci_sched_sco(struct hci_dev *hdev)
         }
 }
 
+static inline void hci_sched_esco(struct hci_dev *hdev)
+{
+        struct hci_conn *conn;
+        struct sk_buff *skb;
+        int quote;
+
+        BT_DBG("%s", hdev->name);
+
+        while (hdev->sco_cnt && (conn = hci_low_sent(hdev, ESCO_LINK, &quote))) {
+                while (quote-- && (skb = skb_dequeue(&conn->data_q))) {
+                        BT_DBG("skb %p len %d", skb, skb->len);
+                        hci_send_frame(skb);
+
+                        conn->sent++;
+                        if (conn->sent == ~0)
+                                conn->sent = 0;
+                }
+        }
+}
+
 static void hci_tx_task(unsigned long arg)
 {
         struct hci_dev *hdev = (struct hci_dev *) arg;
@@ -1376,6 +1396,8 @@ static void hci_tx_task(unsigned long arg)
 
         hci_sched_sco(hdev);
 
+        hci_sched_esco(hdev);
+
         /* Send next queued raw (unknown type) packet */
         while ((skb = skb_dequeue(&hdev->raw_q)))
                 hci_send_frame(skb);
diff --git a/net/bluetooth/hci_event.c b/net/bluetooth/hci_event.c
index e2cfeea5ee72..46df2e403df8 100644
--- a/net/bluetooth/hci_event.c
+++ b/net/bluetooth/hci_event.c
@@ -511,11 +511,11 @@ static void hci_cs_add_sco(struct hci_dev *hdev, __u8 status)
         struct hci_conn *acl, *sco;
         __u16 handle;
 
+        BT_DBG("%s status 0x%x", hdev->name, status);
+
         if (!status)
                 return;
 
-        BT_DBG("%s status 0x%x", hdev->name, status);
-
         cp = hci_sent_cmd_data(hdev, HCI_OP_ADD_SCO);
         if (!cp)
                 return;
@@ -544,7 +544,34 @@ static void hci_cs_remote_name_req(struct hci_dev *hdev, __u8 status)
 
 static void hci_cs_setup_sync_conn(struct hci_dev *hdev, __u8 status)
 {
+        struct hci_cp_setup_sync_conn *cp;
+        struct hci_conn *acl, *sco;
+        __u16 handle;
+
         BT_DBG("%s status 0x%x", hdev->name, status);
+
+        if (!status)
+                return;
+
+        cp = hci_sent_cmd_data(hdev, HCI_OP_SETUP_SYNC_CONN);
+        if (!cp)
+                return;
+
+        handle = __le16_to_cpu(cp->handle);
+
+        BT_DBG("%s handle %d", hdev->name, handle);
+
+        hci_dev_lock(hdev);
+
+        acl = hci_conn_hash_lookup_handle(hdev, handle);
+        if (acl && (sco = acl->link)) {
+                sco->state = BT_CLOSED;
+
+                hci_proto_connect_cfm(sco, status);
+                hci_conn_del(sco);
+        }
+
+        hci_dev_unlock(hdev);
 }
 
 static void hci_cs_sniff_mode(struct hci_dev *hdev, __u8 status)
@@ -692,9 +719,12 @@ static inline void hci_conn_complete_evt(struct hci_dev *hdev, struct sk_buff *s
         if (conn->type == ACL_LINK) {
                 struct hci_conn *sco = conn->link;
                 if (sco) {
-                        if (!ev->status)
-                                hci_add_sco(sco, conn->handle);
-                        else {
+                        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);
                         }
@@ -724,9 +754,9 @@ static inline void hci_conn_request_evt(struct hci_dev *hdev, struct sk_buff *sk
         if (mask & HCI_LM_ACCEPT) {
                 /* Connection accepted */
                 struct hci_conn *conn;
-                struct hci_cp_accept_conn_req cp;
 
                 hci_dev_lock(hdev);
+
                 conn = hci_conn_hash_lookup_ba(hdev, ev->link_type, &ev->bdaddr);
                 if (!conn) {
                         if (!(conn = hci_conn_add(hdev, ev->link_type, &ev->bdaddr))) {
@@ -735,18 +765,39 @@ static inline void hci_conn_request_evt(struct hci_dev *hdev, struct sk_buff *sk
                                 return;
                         }
                 }
+
                 memcpy(conn->dev_class, ev->dev_class, 3);
                 conn->state = BT_CONNECT;
+
                 hci_dev_unlock(hdev);
 
-                bacpy(&cp.bdaddr, &ev->bdaddr);
+                if (ev->link_type == ACL_LINK || !lmp_esco_capable(hdev)) {
+                        struct hci_cp_accept_conn_req cp;
 
-                if (lmp_rswitch_capable(hdev) && (mask & HCI_LM_MASTER))
-                        cp.role = 0x00; /* Become master */
-                else
-                        cp.role = 0x01; /* Remain slave */
+                        bacpy(&cp.bdaddr, &ev->bdaddr);
+
+                        if (lmp_rswitch_capable(hdev) && (mask & HCI_LM_MASTER))
+                                cp.role = 0x00; /* Become master */
+                        else
+                                cp.role = 0x01; /* Remain slave */
+
+                        hci_send_cmd(hdev, HCI_OP_ACCEPT_CONN_REQ,
+                                                        sizeof(cp), &cp);
+                } else {
+                        struct hci_cp_accept_sync_conn_req cp;
+
+                        bacpy(&cp.bdaddr, &ev->bdaddr);
+                        cp.pkt_type = cpu_to_le16(hdev->esco_type);
+
+                        cp.tx_bandwidth   = cpu_to_le32(0x00001f40);
+                        cp.rx_bandwidth   = cpu_to_le32(0x00001f40);
+                        cp.max_latency    = cpu_to_le16(0xffff);
+                        cp.content_format = cpu_to_le16(hdev->voice_setting);
+                        cp.retrans_effort = 0xff;
 
-                hci_send_cmd(hdev, HCI_OP_ACCEPT_CONN_REQ, sizeof(cp), &cp);
+                        hci_send_cmd(hdev, HCI_OP_ACCEPT_SYNC_CONN_REQ,
+                                                        sizeof(cp), &cp);
+                }
         } else {
                 /* Connection rejected */
                 struct hci_cp_reject_conn_req cp;
@@ -1254,7 +1305,29 @@ static inline void hci_remote_ext_features_evt(struct hci_dev *hdev, struct sk_b
 
 static inline void hci_sync_conn_complete_evt(struct hci_dev *hdev, struct sk_buff *skb)
 {
-        BT_DBG("%s", hdev->name);
+        struct hci_ev_sync_conn_complete *ev = (void *) skb->data;
+        struct hci_conn *conn;
+
+        BT_DBG("%s status %d", hdev->name, ev->status);
+
+        hci_dev_lock(hdev);
+
+        conn = hci_conn_hash_lookup_ba(hdev, ev->link_type, &ev->bdaddr);
+        if (!conn)
+                goto unlock;
+
+        if (!ev->status) {
+                conn->handle = __le16_to_cpu(ev->handle);
+                conn->state  = BT_CONNECTED;
+        } else
+                conn->state = BT_CLOSED;
+
+        hci_proto_connect_cfm(conn, ev->status);
+        if (ev->status)
+                hci_conn_del(conn);
+
+unlock:
+        hci_dev_unlock(hdev);
 }
 
 static inline void hci_sync_conn_changed_evt(struct hci_dev *hdev, struct sk_buff *skb)
diff --git a/net/bluetooth/sco.c b/net/bluetooth/sco.c
index 65b6fb1c4154..82d0dfdfa7e2 100644
--- a/net/bluetooth/sco.c
+++ b/net/bluetooth/sco.c
@@ -189,7 +189,7 @@ static int sco_connect(struct sock *sk)
         struct sco_conn *conn;
         struct hci_conn *hcon;
         struct hci_dev  *hdev;
-        int err = 0;
+        int err, type;
 
         BT_DBG("%s -> %s", batostr(src), batostr(dst));
 
@@ -200,7 +200,9 @@ static int sco_connect(struct sock *sk)
 
         err = -ENOMEM;
 
-        hcon = hci_connect(hdev, SCO_LINK, dst);
+        type = lmp_esco_capable(hdev) ? ESCO_LINK : SCO_LINK;
+
+        hcon = hci_connect(hdev, type, dst);
         if (!hcon)
                 goto done;
 
@@ -224,6 +226,7 @@ static int sco_connect(struct sock *sk)
                 sk->sk_state = BT_CONNECT;
                 sco_sock_set_timer(sk, sk->sk_sndtimeo);
         }
+
 done:
         hci_dev_unlock_bh(hdev);
         hci_dev_put(hdev);
@@ -846,7 +849,7 @@ static int sco_connect_cfm(struct hci_conn *hcon, __u8 status)
 {
         BT_DBG("hcon %p bdaddr %s status %d", hcon, batostr(&hcon->dst), status);
 
-        if (hcon->type != SCO_LINK)
+        if (hcon->type != SCO_LINK && hcon->type != ESCO_LINK)
                 return 0;
 
         if (!status) {
@@ -865,10 +868,11 @@ static int sco_disconn_ind(struct hci_conn *hcon, __u8 reason)
 {
         BT_DBG("hcon %p reason %d", hcon, reason);
 
-        if (hcon->type != SCO_LINK)
+        if (hcon->type != SCO_LINK && hcon->type != ESCO_LINK)
                 return 0;
 
         sco_conn_del(hcon, bt_err(reason));
+
         return 0;
 }