From f8d53a1a48e7683200d68c7cf42aa5aad619a5bc Mon Sep 17 00:00:00 2001
From: "Gustavo F. Padovan" <padovan@profusion.mobi>
Date: Mon, 21 Jun 2010 19:39:50 -0300
Subject: [PATCH] --- yaml --- r: 203859 b: refs/heads/master c:
 712132eb541e4a76afad97898dc0ce6b6c0032d8 h: refs/heads/master i:   203857:
 2fcd8e786eca2612fb7134169c536c5603a53073   203855:
 db4846895b0b03598e4bbf7797afc71facbb45f5 v: v3

---
 [refs]                      |  2 +-
 trunk/net/bluetooth/l2cap.c | 83 +++++++++++++++++++++----------------
 2 files changed, 48 insertions(+), 37 deletions(-)

diff --git a/[refs] b/[refs]
index 4e500b6e9941..ace6863a446d 100644
--- a/[refs]
+++ b/[refs]
@@ -1,2 +1,2 @@
 ---
-refs/heads/master: 218bb9dfd21472128f86b38ad2eab123205c2991
+refs/heads/master: 712132eb541e4a76afad97898dc0ce6b6c0032d8
diff --git a/trunk/net/bluetooth/l2cap.c b/trunk/net/bluetooth/l2cap.c
index cf4481f7f566..6b839d682143 100644
--- a/trunk/net/bluetooth/l2cap.c
+++ b/trunk/net/bluetooth/l2cap.c
@@ -3606,6 +3606,46 @@ static int l2cap_ertm_reassembly_sdu(struct sock *sk, struct sk_buff *skb, u16 c
 	return 0;
 }
 
+static int l2cap_try_push_rx_skb(struct sock *sk)
+{
+	struct l2cap_pinfo *pi = l2cap_pi(sk);
+	struct sk_buff *skb;
+	u16 control;
+	int err;
+
+	while ((skb = skb_dequeue(BUSY_QUEUE(sk)))) {
+		control = bt_cb(skb)->sar << L2CAP_CTRL_SAR_SHIFT;
+		err = l2cap_ertm_reassembly_sdu(sk, skb, control);
+		if (err < 0) {
+			skb_queue_head(BUSY_QUEUE(sk), skb);
+			return -EBUSY;
+		}
+
+		pi->buffer_seq = (pi->buffer_seq + 1) % 64;
+	}
+
+	if (!(pi->conn_state & L2CAP_CONN_RNR_SENT))
+		goto done;
+
+	control = pi->buffer_seq << L2CAP_CTRL_REQSEQ_SHIFT;
+	control |= L2CAP_SUPER_RCV_READY | L2CAP_CTRL_POLL;
+	l2cap_send_sframe(pi, control);
+	l2cap_pi(sk)->retry_count = 1;
+
+	del_timer(&pi->retrans_timer);
+	__mod_monitor_timer();
+
+	l2cap_pi(sk)->conn_state |= L2CAP_CONN_WAIT_F;
+
+done:
+	pi->conn_state &= ~L2CAP_CONN_LOCAL_BUSY;
+	pi->conn_state &= ~L2CAP_CONN_RNR_SENT;
+
+	BT_DBG("sk %p, Exit local busy", sk);
+
+	return 0;
+}
+
 static void l2cap_busy_work(struct work_struct *work)
 {
 	DECLARE_WAITQUEUE(wait, current);
@@ -3614,7 +3654,6 @@ static void l2cap_busy_work(struct work_struct *work)
 	struct sock *sk = (struct sock *)pi;
 	int n_tries = 0, timeo = HZ/5, err;
 	struct sk_buff *skb;
-	u16 control;
 
 	lock_sock(sk);
 
@@ -3625,7 +3664,7 @@ static void l2cap_busy_work(struct work_struct *work)
 		if (n_tries++ > L2CAP_LOCAL_BUSY_TRIES) {
 			err = -EBUSY;
 			l2cap_send_disconn_req(pi->conn, sk, EBUSY);
-			goto done;
+			break;
 		}
 
 		if (!timeo)
@@ -3633,7 +3672,7 @@ static void l2cap_busy_work(struct work_struct *work)
 
 		if (signal_pending(current)) {
 			err = sock_intr_errno(timeo);
-			goto done;
+			break;
 		}
 
 		release_sock(sk);
@@ -3642,42 +3681,12 @@ static void l2cap_busy_work(struct work_struct *work)
 
 		err = sock_error(sk);
 		if (err)
-			goto done;
-
-		while ((skb = skb_dequeue(BUSY_QUEUE(sk)))) {
-			control = bt_cb(skb)->sar << L2CAP_CTRL_SAR_SHIFT;
-			err = l2cap_ertm_reassembly_sdu(sk, skb, control);
-			if (err < 0) {
-				skb_queue_head(BUSY_QUEUE(sk), skb);
-				break;
-			}
-
-			pi->buffer_seq = (pi->buffer_seq + 1) % 64;
-		}
+			break;
 
-		if (!skb)
+		if (l2cap_try_push_rx_skb(sk) == 0)
 			break;
 	}
 
-	if (!(pi->conn_state & L2CAP_CONN_RNR_SENT))
-		goto done;
-
-	control = pi->buffer_seq << L2CAP_CTRL_REQSEQ_SHIFT;
-	control |= L2CAP_SUPER_RCV_READY | L2CAP_CTRL_POLL;
-	l2cap_send_sframe(pi, control);
-	l2cap_pi(sk)->retry_count = 1;
-
-	del_timer(&pi->retrans_timer);
-	__mod_monitor_timer();
-
-	l2cap_pi(sk)->conn_state |= L2CAP_CONN_WAIT_F;
-
-done:
-	pi->conn_state &= ~L2CAP_CONN_LOCAL_BUSY;
-	pi->conn_state &= ~L2CAP_CONN_RNR_SENT;
-
-	BT_DBG("sk %p, Exit local busy", sk);
-
 	set_current_state(TASK_RUNNING);
 	remove_wait_queue(sk_sleep(sk), &wait);
 
@@ -3692,7 +3701,9 @@ static int l2cap_push_rx_skb(struct sock *sk, struct sk_buff *skb, u16 control)
 	if (pi->conn_state & L2CAP_CONN_LOCAL_BUSY) {
 		bt_cb(skb)->sar = control >> L2CAP_CTRL_SAR_SHIFT;
 		__skb_queue_tail(BUSY_QUEUE(sk), skb);
-		return -EBUSY;
+		return l2cap_try_push_rx_skb(sk);
+
+
 	}
 
 	err = l2cap_ertm_reassembly_sdu(sk, skb, control);