From 1bf9ba249cce51c47fab3934585ca9a4cffdd81a Mon Sep 17 00:00:00 2001
From: Marcel Holtmann <marcel@holtmann.org>
Date: Fri, 6 Feb 2009 09:13:37 +0100
Subject: [PATCH] --- yaml --- r: 134729 b: refs/heads/master c:
 efc7688b557dd1be10eead7399b315efcb1dbc74 h: refs/heads/master i:   134727:
 39b1a2426e2bf11e7b1082e82dc22411fa160f21 v: v3

---
 [refs]                            |  2 +-
 trunk/include/net/bluetooth/hci.h |  8 ++++++++
 trunk/net/bluetooth/hci_conn.c    |  9 +++++++--
 trunk/net/bluetooth/hci_event.c   | 16 ++++++++++++++++
 4 files changed, 32 insertions(+), 3 deletions(-)

diff --git a/[refs] b/[refs]
index b08f343ddc36..9cfd67bacaba 100644
--- a/[refs]
+++ b/[refs]
@@ -1,2 +1,2 @@
 ---
-refs/heads/master: 255c76014af74165428e7aa16414b857e2bdccf2
+refs/heads/master: efc7688b557dd1be10eead7399b315efcb1dbc74
diff --git a/trunk/include/net/bluetooth/hci.h b/trunk/include/net/bluetooth/hci.h
index 3645139e68c7..f69f015bbcc0 100644
--- a/trunk/include/net/bluetooth/hci.h
+++ b/trunk/include/net/bluetooth/hci.h
@@ -133,8 +133,13 @@ enum {
 #define ESCO_EV3	0x0008
 #define ESCO_EV4	0x0010
 #define ESCO_EV5	0x0020
+#define ESCO_2EV3	0x0040
+#define ESCO_3EV3	0x0080
+#define ESCO_2EV5	0x0100
+#define ESCO_3EV5	0x0200
 
 #define SCO_ESCO_MASK  (ESCO_HV1 | ESCO_HV2 | ESCO_HV3)
+#define EDR_ESCO_MASK  (ESCO_2EV3 | ESCO_3EV3 | ESCO_2EV5 | ESCO_3EV5)
 
 /* ACL flags */
 #define ACL_CONT		0x01
@@ -176,6 +181,9 @@ enum {
 #define LMP_EV5		0x02
 
 #define LMP_SNIFF_SUBR	0x02
+#define LMP_EDR_ESCO_2M	0x20
+#define LMP_EDR_ESCO_3M	0x40
+#define LMP_EDR_3S_ESCO	0x80
 
 #define LMP_SIMPLE_PAIR	0x08
 
diff --git a/trunk/net/bluetooth/hci_conn.c b/trunk/net/bluetooth/hci_conn.c
index 98f97a1e9bbb..2435e830ba60 100644
--- a/trunk/net/bluetooth/hci_conn.c
+++ b/trunk/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/trunk/net/bluetooth/hci_event.c b/trunk/net/bluetooth/hci_event.c
index 014fc8b320ba..899b8991a466 100644
--- a/trunk/net/bluetooth/hci_event.c
+++ b/trunk/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;