+ return err;
+}
+
+static void l2cap_check_srej_gap(struct sock *sk, u8 tx_seq)
+{
+ struct sk_buff *skb;
+ u16 control = 0;
+
+ while((skb = skb_peek(SREJ_QUEUE(sk)))) {
+ if (bt_cb(skb)->tx_seq != tx_seq)
+ break;
+
+ skb = skb_dequeue(SREJ_QUEUE(sk));
+ control |= bt_cb(skb)->sar << L2CAP_CTRL_SAR_SHIFT;
+ l2cap_sar_reassembly_sdu(sk, skb, control);
+ l2cap_pi(sk)->buffer_seq_srej =
+ (l2cap_pi(sk)->buffer_seq_srej + 1) % 64;
+ tx_seq++;
+ }
+}
+
+static void l2cap_resend_srejframe(struct sock *sk, u8 tx_seq)
+{
+ struct l2cap_pinfo *pi = l2cap_pi(sk);
+ struct srej_list *l, *tmp;
+ u16 control;
+
+ list_for_each_entry_safe(l,tmp, SREJ_LIST(sk), list) {
+ if (l->tx_seq == tx_seq) {
+ list_del(&l->list);
+ kfree(l);
+ return;
+ }
+ control = L2CAP_SUPER_SELECT_REJECT;
+ control |= l->tx_seq << L2CAP_CTRL_REQSEQ_SHIFT;
+ l2cap_send_sframe(pi, control);
+ list_del(&l->list);
+ list_add_tail(&l->list, SREJ_LIST(sk));
+ }
+}
+
+static void l2cap_send_srejframe(struct sock *sk, u8 tx_seq)
+{
+ struct l2cap_pinfo *pi = l2cap_pi(sk);
+ struct srej_list *new;
+ u16 control;
+
+ while (tx_seq != pi->expected_tx_seq) {
+ control = L2CAP_SUPER_SELECT_REJECT;
+ control |= pi->expected_tx_seq << L2CAP_CTRL_REQSEQ_SHIFT;
+ if (pi->conn_state & L2CAP_CONN_SEND_PBIT) {
+ control |= L2CAP_CTRL_POLL;
+ pi->conn_state &= ~L2CAP_CONN_SEND_PBIT;
+ }
+ l2cap_send_sframe(pi, control);
+
+ new = kzalloc(sizeof(struct srej_list), GFP_ATOMIC);
+ new->tx_seq = pi->expected_tx_seq++;
+ list_add_tail(&new->list, SREJ_LIST(sk));
+ }
+ pi->expected_tx_seq++;
+}
+
+static inline int l2cap_data_channel_iframe(struct sock *sk, u16 rx_control, struct sk_buff *skb)
+{
+ struct l2cap_pinfo *pi = l2cap_pi(sk);
+ u8 tx_seq = __get_txseq(rx_control);
+ u8 req_seq = __get_reqseq(rx_control);
+ u16 tx_control = 0;
+ u8 sar = rx_control >> L2CAP_CTRL_SAR_SHIFT;
+ int err = 0;
+
+ BT_DBG("sk %p rx_control 0x%4.4x len %d", sk, rx_control, skb->len);
+
+ pi->expected_ack_seq = req_seq;
+ l2cap_drop_acked_frames(sk);
+
+ if (tx_seq == pi->expected_tx_seq)
+ goto expected;
+
+ if (pi->conn_state & L2CAP_CONN_SREJ_SENT) {
+ struct srej_list *first;
+
+ first = list_first_entry(SREJ_LIST(sk),
+ struct srej_list, list);
+ if (tx_seq == first->tx_seq) {
+ l2cap_add_to_srej_queue(sk, skb, tx_seq, sar);
+ l2cap_check_srej_gap(sk, tx_seq);
+
+ list_del(&first->list);
+ kfree(first);
+
+ if (list_empty(SREJ_LIST(sk))) {
+ pi->buffer_seq = pi->buffer_seq_srej;
+ pi->conn_state &= ~L2CAP_CONN_SREJ_SENT;
+ }
+ } else {
+ struct srej_list *l;
+ l2cap_add_to_srej_queue(sk, skb, tx_seq, sar);
+
+ list_for_each_entry(l, SREJ_LIST(sk), list) {
+ if (l->tx_seq == tx_seq) {
+ l2cap_resend_srejframe(sk, tx_seq);
+ return 0;
+ }
+ }
+ l2cap_send_srejframe(sk, tx_seq);
+ }
+ } else {
+ pi->conn_state |= L2CAP_CONN_SREJ_SENT;
+
+ INIT_LIST_HEAD(SREJ_LIST(sk));
+ pi->buffer_seq_srej = pi->buffer_seq;
+
+ __skb_queue_head_init(SREJ_QUEUE(sk));
+ l2cap_add_to_srej_queue(sk, skb, tx_seq, sar);
+
+ pi->conn_state |= L2CAP_CONN_SEND_PBIT;
+
+ l2cap_send_srejframe(sk, tx_seq);
+ }
+ return 0;
+
+expected:
+ pi->expected_tx_seq = (pi->expected_tx_seq + 1) % 64;
+
+ if (pi->conn_state & L2CAP_CONN_SREJ_SENT) {
+ l2cap_add_to_srej_queue(sk, skb, tx_seq, sar);
+ return 0;
+ }
+
+ if (rx_control & L2CAP_CTRL_FINAL) {
+ if (pi->conn_state & L2CAP_CONN_REJ_ACT)
+ pi->conn_state &= ~L2CAP_CONN_REJ_ACT;
+ else {
+ sk->sk_send_head = TX_QUEUE(sk)->next;
+ pi->next_tx_seq = pi->expected_ack_seq;
+ l2cap_ertm_send(sk);
+ }
+ }
+
+ pi->buffer_seq = (pi->buffer_seq + 1) % 64;
+
+ err = l2cap_sar_reassembly_sdu(sk, skb, rx_control);
+ if (err < 0)
+ return err;
+
+ pi->num_to_ack = (pi->num_to_ack + 1) % L2CAP_DEFAULT_NUM_TO_ACK;
+ if (pi->num_to_ack == L2CAP_DEFAULT_NUM_TO_ACK - 1) {
+ tx_control |= L2CAP_SUPER_RCV_READY;
+ tx_control |= pi->buffer_seq << L2CAP_CTRL_REQSEQ_SHIFT;
+ l2cap_send_sframe(pi, tx_control);
+ }
+ return 0;
+}
+
+static inline int l2cap_data_channel_sframe(struct sock *sk, u16 rx_control, struct sk_buff *skb)
+{
+ struct l2cap_pinfo *pi = l2cap_pi(sk);
+ u8 tx_seq = __get_reqseq(rx_control);
+
+ BT_DBG("sk %p rx_control 0x%4.4x len %d", sk, rx_control, skb->len);
+
+ switch (rx_control & L2CAP_CTRL_SUPERVISE) {
+ case L2CAP_SUPER_RCV_READY:
+ if (rx_control & L2CAP_CTRL_POLL) {
+ u16 control = L2CAP_CTRL_FINAL;
+ control |= L2CAP_SUPER_RCV_READY |
+ (pi->buffer_seq << L2CAP_CTRL_REQSEQ_SHIFT);
+ l2cap_send_sframe(l2cap_pi(sk), control);
+ pi->conn_state &= ~L2CAP_CONN_REMOTE_BUSY;
+
+ } else if (rx_control & L2CAP_CTRL_FINAL) {
+ pi->conn_state &= ~L2CAP_CONN_REMOTE_BUSY;
+ pi->expected_ack_seq = tx_seq;
+ l2cap_drop_acked_frames(sk);
+
+ if (pi->conn_state & L2CAP_CONN_REJ_ACT)
+ pi->conn_state &= ~L2CAP_CONN_REJ_ACT;
+ else {
+ sk->sk_send_head = TX_QUEUE(sk)->next;
+ pi->next_tx_seq = pi->expected_ack_seq;
+ l2cap_ertm_send(sk);
+ }
+
+ if (!(pi->conn_state & L2CAP_CONN_WAIT_F))
+ break;
+
+ pi->conn_state &= ~L2CAP_CONN_WAIT_F;
+ del_timer(&pi->monitor_timer);
+
+ if (pi->unacked_frames > 0)
+ __mod_retrans_timer();
+ } else {
+ pi->expected_ack_seq = tx_seq;
+ l2cap_drop_acked_frames(sk);
+
+ if ((pi->conn_state & L2CAP_CONN_REMOTE_BUSY) &&
+ (pi->unacked_frames > 0))
+ __mod_retrans_timer();
+
+ l2cap_ertm_send(sk);
+ pi->conn_state &= ~L2CAP_CONN_REMOTE_BUSY;
+ }
+ break;
+
+ case L2CAP_SUPER_REJECT:
+ pi->conn_state &= ~L2CAP_CONN_REMOTE_BUSY;
+
+ pi->expected_ack_seq = __get_reqseq(rx_control);
+ l2cap_drop_acked_frames(sk);
+
+ if (rx_control & L2CAP_CTRL_FINAL) {
+ if (pi->conn_state & L2CAP_CONN_REJ_ACT)
+ pi->conn_state &= ~L2CAP_CONN_REJ_ACT;
+ else {
+ sk->sk_send_head = TX_QUEUE(sk)->next;
+ pi->next_tx_seq = pi->expected_ack_seq;
+ l2cap_ertm_send(sk);
+ }
+ } else {
+ sk->sk_send_head = TX_QUEUE(sk)->next;
+ pi->next_tx_seq = pi->expected_ack_seq;
+ l2cap_ertm_send(sk);
+
+ if (pi->conn_state & L2CAP_CONN_WAIT_F) {
+ pi->srej_save_reqseq = tx_seq;
+ pi->conn_state |= L2CAP_CONN_REJ_ACT;
+ }
+ }
+
+ break;
+
+ case L2CAP_SUPER_SELECT_REJECT:
+ pi->conn_state &= ~L2CAP_CONN_REMOTE_BUSY;
+
+ if (rx_control & L2CAP_CTRL_POLL) {
+ l2cap_retransmit_frame(sk, tx_seq);
+ pi->expected_ack_seq = tx_seq;
+ l2cap_drop_acked_frames(sk);
+ l2cap_ertm_send(sk);
+ if (pi->conn_state & L2CAP_CONN_WAIT_F) {
+ pi->srej_save_reqseq = tx_seq;
+ pi->conn_state |= L2CAP_CONN_SREJ_ACT;
+ }
+ } else if (rx_control & L2CAP_CTRL_FINAL) {
+ if ((pi->conn_state & L2CAP_CONN_SREJ_ACT) &&
+ pi->srej_save_reqseq == tx_seq)
+ pi->conn_state &= ~L2CAP_CONN_SREJ_ACT;
+ else
+ l2cap_retransmit_frame(sk, tx_seq);
+ }
+ else {
+ l2cap_retransmit_frame(sk, tx_seq);
+ if (pi->conn_state & L2CAP_CONN_WAIT_F) {
+ pi->srej_save_reqseq = tx_seq;
+ pi->conn_state |= L2CAP_CONN_SREJ_ACT;
+ }
+ }
+ break;
+
+ case L2CAP_SUPER_RCV_NOT_READY:
+ pi->conn_state |= L2CAP_CONN_REMOTE_BUSY;
+ pi->expected_ack_seq = tx_seq;
+ l2cap_drop_acked_frames(sk);
+
+ del_timer(&l2cap_pi(sk)->retrans_timer);
+ if (rx_control & L2CAP_CTRL_POLL) {
+ u16 control = L2CAP_CTRL_FINAL;
+ l2cap_send_rr_or_rnr(l2cap_pi(sk), control);
+ }
+ break;
+ }
+
+ return 0;
git://ftp.safe.ca / safe / jmp / linux-2.6 / blobdiff