diff options
| -rw-r--r-- | net/bluetooth/l2cap_core.c | 62 |
1 files changed, 38 insertions, 24 deletions
diff --git a/net/bluetooth/l2cap_core.c b/net/bluetooth/l2cap_core.c index bd5d9926bf4..f7ada4a2cc5 100644 --- a/net/bluetooth/l2cap_core.c +++ b/net/bluetooth/l2cap_core.c @@ -3227,22 +3227,26 @@ disconnect: return 0; } -static int l2cap_try_push_rx_skb(struct l2cap_chan *chan) +static void l2cap_ertm_enter_local_busy(struct l2cap_chan *chan) { - struct sk_buff *skb; u16 control; - int err; - while ((skb = skb_dequeue(&chan->busy_q))) { - control = bt_cb(skb)->sar << L2CAP_CTRL_SAR_SHIFT; - err = l2cap_ertm_reassembly_sdu(chan, skb, control); - if (err < 0) { - skb_queue_head(&chan->busy_q, skb); - return -EBUSY; - } + BT_DBG("chan %p, Enter local busy", chan); - chan->buffer_seq = (chan->buffer_seq + 1) % 64; - } + set_bit(CONN_LOCAL_BUSY, &chan->conn_state); + + control = chan->buffer_seq << L2CAP_CTRL_REQSEQ_SHIFT; + control |= L2CAP_SUPER_RCV_NOT_READY; + l2cap_send_sframe(chan, control); + + set_bit(CONN_RNR_SENT, &chan->conn_state); + + __clear_ack_timer(chan); +} + +static void l2cap_ertm_exit_local_busy(struct l2cap_chan *chan) +{ + u16 control; if (!test_bit(CONN_RNR_SENT, &chan->conn_state)) goto done; @@ -3262,6 +3266,26 @@ done: clear_bit(CONN_RNR_SENT, &chan->conn_state); BT_DBG("chan %p, Exit local busy", chan); +} + +static int l2cap_try_push_rx_skb(struct l2cap_chan *chan) +{ + struct sk_buff *skb; + u16 control; + int err; + + while ((skb = skb_dequeue(&chan->busy_q))) { + control = bt_cb(skb)->sar << L2CAP_CTRL_SAR_SHIFT; + err = l2cap_ertm_reassembly_sdu(chan, skb, control); + if (err < 0) { + skb_queue_head(&chan->busy_q, skb); + return -EBUSY; + } + + chan->buffer_seq = (chan->buffer_seq + 1) % 64; + } + + l2cap_ertm_exit_local_busy(chan); return 0; } @@ -3315,7 +3339,7 @@ static void l2cap_busy_work(struct work_struct *work) static int l2cap_push_rx_skb(struct l2cap_chan *chan, struct sk_buff *skb, u16 control) { - int sctrl, err; + int err; if (test_bit(CONN_LOCAL_BUSY, &chan->conn_state)) { bt_cb(skb)->sar = control >> L2CAP_CTRL_SAR_SHIFT; @@ -3331,21 +3355,11 @@ static int l2cap_push_rx_skb(struct l2cap_chan *chan, struct sk_buff *skb, u16 c return err; } - /* Busy Condition */ - BT_DBG("chan %p, Enter local busy", chan); + l2cap_ertm_enter_local_busy(chan); - set_bit(CONN_LOCAL_BUSY, &chan->conn_state); bt_cb(skb)->sar = control >> L2CAP_CTRL_SAR_SHIFT; __skb_queue_tail(&chan->busy_q, skb); - sctrl = chan->buffer_seq << L2CAP_CTRL_REQSEQ_SHIFT; - sctrl |= L2CAP_SUPER_RCV_NOT_READY; - l2cap_send_sframe(chan, sctrl); - - set_bit(CONN_RNR_SENT, &chan->conn_state); - - __clear_ack_timer(chan); - queue_work(_busy_wq, &chan->busy_work); return err; |