Bluetooth: Improve ERTM local busy handling

Now we also check if can push skb userspace just after receive a new
skb instead of only wait the l2cap_busy_work wake up from time to time
to check the local busy condition.

Signed-off-by: Gustavo F. Padovan <padovan@profusion.mobi>
Signed-off-by: Marcel Holtmann <marcel@holtmann.org>

diff --git a/net/bluetooth/l2cap.c b/net/bluetooth/l2cap.c
index cf4481f..6b839d6 100644 (file)
--- 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);