1 files changed, 47 insertions, 36 deletions

diff --git a/net/bluetooth/l2cap.c b/net/bluetooth/l2cap.c
index cf4481f7f566..6b839d682143 100644
--- a/net/bluetooth/l2cap.c
+++ b/net/bluetooth/l2cap.c
@@ -3606,6 +3606,46 @@ disconnect:
         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);