forked from Minki/linux
Bluetooth: Add support for L2CAP SREJ exception
When L2CAP loses an I-frame we send a SREJ frame to the transmitter side requesting the lost packet. This patch implement all Recv I-frame events on SREJ_SENT state table except the ones that deal with SendRej (the REJ exception at receiver side is yet not implemented). Signed-off-by: Gustavo F. Padovan <gustavo@las.ic.unicamp.br> Signed-off-by: Marcel Holtmann <marcel@holtmann.org>
This commit is contained in:
Gustavo F. Padovan
Marcel Holtmann
parent
fcc203c30d
commit
8f17154f1f
@ -140,6 +140,7 @@ struct bt_skb_cb {
|
||||
__u8 incoming;
|
||||
__u8 tx_seq;
|
||||
__u8 retries;
|
||||
__u8 sar;
|
||||
};
|
||||
#define bt_cb(skb) ((struct bt_skb_cb *)((skb)->cb))
|
||||
|
||||
|
||||
@ -108,6 +108,7 @@ struct l2cap_conninfo {
|
||||
|
||||
#define L2CAP_CTRL_TXSEQ_SHIFT 1
|
||||
#define L2CAP_CTRL_REQSEQ_SHIFT 8
|
||||
#define L2CAP_CTRL_SAR_SHIFT 14
|
||||
|
||||
/* L2CAP Supervisory Function */
|
||||
#define L2CAP_SUPER_RCV_READY 0x0000
|
||||
@ -290,6 +291,13 @@ struct l2cap_conn {
|
||||
/* ----- L2CAP channel and socket info ----- */
|
||||
#define l2cap_pi(sk) ((struct l2cap_pinfo *) sk)
|
||||
#define TX_QUEUE(sk) (&l2cap_pi(sk)->tx_queue)
|
||||
#define SREJ_QUEUE(sk) (&l2cap_pi(sk)->srej_queue)
|
||||
#define SREJ_LIST(sk) (&l2cap_pi(sk)->srej_l.list)
|
||||
|
||||
struct srej_list {
|
||||
__u8 tx_seq;
|
||||
struct list_head list;
|
||||
};
|
||||
|
||||
struct l2cap_pinfo {
|
||||
struct bt_sock bt;
|
||||
@ -318,6 +326,8 @@ struct l2cap_pinfo {
|
||||
__u8 expected_ack_seq;
|
||||
__u8 req_seq;
|
||||
__u8 expected_tx_seq;
|
||||
__u8 buffer_seq;
|
||||
__u8 buffer_seq_srej;
|
||||
__u8 unacked_frames;
|
||||
__u8 retry_count;
|
||||
__u8 num_to_ack;
|
||||
@ -338,6 +348,8 @@ struct l2cap_pinfo {
|
||||
struct timer_list retrans_timer;
|
||||
struct timer_list monitor_timer;
|
||||
struct sk_buff_head tx_queue;
|
||||
struct sk_buff_head srej_queue;
|
||||
struct srej_list srej_l;
|
||||
struct l2cap_conn *conn;
|
||||
struct sock *next_c;
|
||||
struct sock *prev_c;
|
||||
@ -356,7 +368,7 @@ struct l2cap_pinfo {
|
||||
#define L2CAP_CONF_MAX_CONF_RSP 2
|
||||
|
||||
#define L2CAP_CONN_SAR_SDU 0x01
|
||||
#define L2CAP_CONN_UNDER_REJ 0x02
|
||||
#define L2CAP_CONN_SREJ_SENT 0x02
|
||||
#define L2CAP_CONN_WAIT_F 0x04
|
||||
|
||||
#define __mod_retrans_timer() mod_timer(&l2cap_pi(sk)->retrans_timer, \
|
||||
|
||||
@ -1292,6 +1292,50 @@ static int l2cap_streaming_send(struct sock *sk)
|
||||
return 0;
|
||||
}
|
||||
|
||||
static int l2cap_retransmit_frame(struct sock *sk, u8 tx_seq)
|
||||
{
|
||||
struct l2cap_pinfo *pi = l2cap_pi(sk);
|
||||
struct sk_buff *skb, *tx_skb;
|
||||
u16 control, fcs;
|
||||
int err;
|
||||
|
||||
skb = skb_peek(TX_QUEUE(sk));
|
||||
do {
|
||||
if (bt_cb(skb)->tx_seq != tx_seq) {
|
||||
if (skb_queue_is_last(TX_QUEUE(sk), skb))
|
||||
break;
|
||||
skb = skb_queue_next(TX_QUEUE(sk), skb);
|
||||
continue;
|
||||
}
|
||||
|
||||
if (pi->remote_max_tx &&
|
||||
bt_cb(skb)->retries == pi->remote_max_tx) {
|
||||
l2cap_send_disconn_req(pi->conn, sk);
|
||||
break;
|
||||
}
|
||||
|
||||
tx_skb = skb_clone(skb, GFP_ATOMIC);
|
||||
bt_cb(skb)->retries++;
|
||||
control = get_unaligned_le16(tx_skb->data + L2CAP_HDR_SIZE);
|
||||
control |= (pi->req_seq << L2CAP_CTRL_REQSEQ_SHIFT)
|
||||
| (tx_seq << L2CAP_CTRL_TXSEQ_SHIFT);
|
||||
put_unaligned_le16(control, tx_skb->data + L2CAP_HDR_SIZE);
|
||||
|
||||
if (l2cap_pi(sk)->fcs == L2CAP_FCS_CRC16) {
|
||||
fcs = crc16(0, (u8 *)tx_skb->data, tx_skb->len - 2);
|
||||
put_unaligned_le16(fcs, tx_skb->data + tx_skb->len - 2);
|
||||
}
|
||||
|
||||
err = l2cap_do_send(sk, tx_skb);
|
||||
if (err < 0) {
|
||||
l2cap_send_disconn_req(pi->conn, sk);
|
||||
return err;
|
||||
}
|
||||
break;
|
||||
} while(1);
|
||||
return 0;
|
||||
}
|
||||
|
||||
static int l2cap_ertm_send(struct sock *sk)
|
||||
{
|
||||
struct sk_buff *skb, *tx_skb;
|
||||
@ -2705,6 +2749,7 @@ static inline int l2cap_config_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr
|
||||
l2cap_monitor_timeout, (unsigned long) sk);
|
||||
|
||||
__skb_queue_head_init(TX_QUEUE(sk));
|
||||
__skb_queue_head_init(SREJ_QUEUE(sk));
|
||||
l2cap_chan_ready(sk);
|
||||
goto unlock;
|
||||
}
|
||||
@ -2784,8 +2829,10 @@ static inline int l2cap_config_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hdr
|
||||
|
||||
sk->sk_state = BT_CONNECTED;
|
||||
l2cap_pi(sk)->expected_tx_seq = 0;
|
||||
l2cap_pi(sk)->buffer_seq = 0;
|
||||
l2cap_pi(sk)->num_to_ack = 0;
|
||||
__skb_queue_head_init(TX_QUEUE(sk));
|
||||
__skb_queue_head_init(SREJ_QUEUE(sk));
|
||||
l2cap_chan_ready(sk);
|
||||
}
|
||||
|
||||
@ -2817,6 +2864,7 @@ static inline int l2cap_disconnect_req(struct l2cap_conn *conn, struct l2cap_cmd
|
||||
sk->sk_shutdown = SHUTDOWN_MASK;
|
||||
|
||||
skb_queue_purge(TX_QUEUE(sk));
|
||||
skb_queue_purge(SREJ_QUEUE(sk));
|
||||
del_timer(&l2cap_pi(sk)->retrans_timer);
|
||||
del_timer(&l2cap_pi(sk)->monitor_timer);
|
||||
|
||||
@ -2843,6 +2891,7 @@ static inline int l2cap_disconnect_rsp(struct l2cap_conn *conn, struct l2cap_cmd
|
||||
return 0;
|
||||
|
||||
skb_queue_purge(TX_QUEUE(sk));
|
||||
skb_queue_purge(SREJ_QUEUE(sk));
|
||||
del_timer(&l2cap_pi(sk)->retrans_timer);
|
||||
del_timer(&l2cap_pi(sk)->monitor_timer);
|
||||
|
||||
@ -3038,6 +3087,33 @@ static int l2cap_check_fcs(struct l2cap_pinfo *pi, struct sk_buff *skb)
|
||||
return 0;
|
||||
}
|
||||
|
||||
static void l2cap_add_to_srej_queue(struct sock *sk, struct sk_buff *skb, u8 tx_seq, u8 sar)
|
||||
{
|
||||
struct sk_buff *next_skb;
|
||||
|
||||
bt_cb(skb)->tx_seq = tx_seq;
|
||||
bt_cb(skb)->sar = sar;
|
||||
|
||||
next_skb = skb_peek(SREJ_QUEUE(sk));
|
||||
if (!next_skb) {
|
||||
__skb_queue_tail(SREJ_QUEUE(sk), skb);
|
||||
return;
|
||||
}
|
||||
|
||||
do {
|
||||
if (bt_cb(next_skb)->tx_seq > tx_seq) {
|
||||
__skb_queue_before(SREJ_QUEUE(sk), next_skb, skb);
|
||||
return;
|
||||
}
|
||||
|
||||
if (skb_queue_is_last(SREJ_QUEUE(sk), next_skb))
|
||||
break;
|
||||
|
||||
} while((next_skb = skb_queue_next(SREJ_QUEUE(sk), next_skb)));
|
||||
|
||||
__skb_queue_tail(SREJ_QUEUE(sk), skb);
|
||||
}
|
||||
|
||||
static int l2cap_sar_reassembly_sdu(struct sock *sk, struct sk_buff *skb, u16 control)
|
||||
{
|
||||
struct l2cap_pinfo *pi = l2cap_pi(sk);
|
||||
@ -3118,50 +3194,143 @@ static int l2cap_sar_reassembly_sdu(struct sock *sk, struct sk_buff *skb, u16 co
|
||||
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;
|
||||
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);
|
||||
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);
|
||||
|
||||
if (tx_seq == pi->expected_tx_seq) {
|
||||
if (pi->conn_state & L2CAP_CONN_UNDER_REJ)
|
||||
pi->conn_state &= ~L2CAP_CONN_UNDER_REJ;
|
||||
if (tx_seq == pi->expected_tx_seq)
|
||||
goto expected;
|
||||
|
||||
err = l2cap_sar_reassembly_sdu(sk, skb, rx_control);
|
||||
if (err < 0)
|
||||
return err;
|
||||
if (pi->conn_state & L2CAP_CONN_SREJ_SENT) {
|
||||
struct srej_list *first;
|
||||
|
||||
pi->expected_tx_seq = (pi->expected_tx_seq + 1) % 64;
|
||||
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->expected_tx_seq << L2CAP_CTRL_REQSEQ_SHIFT;
|
||||
goto send;
|
||||
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 {
|
||||
/* Unexpected txSeq. Send a REJ S-frame */
|
||||
kfree_skb(skb);
|
||||
if (!(pi->conn_state & L2CAP_CONN_UNDER_REJ)) {
|
||||
tx_control |= L2CAP_SUPER_REJECT;
|
||||
tx_control |= pi->expected_tx_seq << L2CAP_CTRL_REQSEQ_SHIFT;
|
||||
pi->conn_state |= L2CAP_CONN_UNDER_REJ;
|
||||
pi->conn_state |= L2CAP_CONN_SREJ_SENT;
|
||||
|
||||
goto send;
|
||||
}
|
||||
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);
|
||||
|
||||
l2cap_send_srejframe(sk, tx_seq);
|
||||
}
|
||||
return 0;
|
||||
|
||||
send:
|
||||
return l2cap_send_sframe(pi, tx_control);
|
||||
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;
|
||||
}
|
||||
|
||||
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);
|
||||
|
||||
@ -3181,7 +3350,7 @@ static inline int l2cap_data_channel_sframe(struct sock *sk, u16 rx_control, str
|
||||
if (pi->unacked_frames > 0)
|
||||
__mod_retrans_timer();
|
||||
} else {
|
||||
pi->expected_ack_seq = __get_reqseq(rx_control);
|
||||
pi->expected_ack_seq = tx_seq;
|
||||
l2cap_drop_acked_frames(sk);
|
||||
if (pi->unacked_frames > 0)
|
||||
__mod_retrans_timer();
|
||||
@ -3200,8 +3369,11 @@ static inline int l2cap_data_channel_sframe(struct sock *sk, u16 rx_control, str
|
||||
|
||||
break;
|
||||
|
||||
case L2CAP_SUPER_RCV_NOT_READY:
|
||||
case L2CAP_SUPER_SELECT_REJECT:
|
||||
l2cap_retransmit_frame(sk, tx_seq);
|
||||
break;
|
||||
|
||||
case L2CAP_SUPER_RCV_NOT_READY:
|
||||
break;
|
||||
}
|
||||
|
||||
|
||||
Loading…
Reference in New Issue
Block a user