Donate to e Foundation | Murena handsets with /e/OS | Own a part of Murena! Learn more

Commit 18778a63 authored by Gustavo Padovan's avatar Gustavo Padovan Committed by Marcel Holtmann
Browse files

Bluetooth: Implement missing parts of the Invalid Frame Detection 



There is a plenty of situation where ERTM shall close the channel, this
commit treats the cases regarding Invalid Frame Detection.
It create one reassembly SDU function for ERTM and other for Streaming
Mode to make the Invalid Frame Detection handling less complex.

Signed-off-by: default avatarGustavo F. Padovan <padovan@profusion.mobi>
Reviewed-by: default avatarJoão Paulo Rechi Vita <jprvita@profusion.mobi>
Signed-off-by: default avatarMarcel Holtmann <marcel@holtmann.org>
parent f11d676d

net/bluetooth/l2cap.c

+112 −7
Original line number Diff line number Diff line
@@ -3329,12 +3329,111 @@ static void l2cap_add_to_srej_queue(struct sock *sk, struct sk_buff *skb, u8 tx_ 
	__skb_queue_tail(SREJ_QUEUE(sk), skb);

}



static int l2cap_sar_reassembly_sdu(struct sock *sk, struct sk_buff *skb, u16 control)

static int l2cap_ertm_reassembly_sdu(struct sock *sk, struct sk_buff *skb, u16 control)

{

	struct l2cap_pinfo *pi = l2cap_pi(sk);

	struct sk_buff *_skb;

	int err = 0;



	switch (control & L2CAP_CTRL_SAR) {

	case L2CAP_SDU_UNSEGMENTED:

		if (pi->conn_state & L2CAP_CONN_SAR_SDU)

			goto drop;



		err = sock_queue_rcv_skb(sk, skb);

		if (!err)

			return err;



		break;



	case L2CAP_SDU_START:

		if (pi->conn_state & L2CAP_CONN_SAR_SDU)

			goto drop;



		pi->sdu_len = get_unaligned_le16(skb->data);

		skb_pull(skb, 2);



		if (pi->sdu_len > pi->imtu)

			goto disconnect;



		pi->sdu = bt_skb_alloc(pi->sdu_len, GFP_ATOMIC);

		if (!pi->sdu) {

			err = -ENOMEM;

			break;

		}



		memcpy(skb_put(pi->sdu, skb->len), skb->data, skb->len);



		pi->conn_state |= L2CAP_CONN_SAR_SDU;

		pi->partial_sdu_len = skb->len;

		break;



	case L2CAP_SDU_CONTINUE:

		if (!(pi->conn_state & L2CAP_CONN_SAR_SDU))

			goto disconnect;



		if (!pi->sdu)

			goto disconnect;



		memcpy(skb_put(pi->sdu, skb->len), skb->data, skb->len);



		pi->partial_sdu_len += skb->len;

		if (pi->partial_sdu_len > pi->sdu_len)

			goto drop;



		break;



	case L2CAP_SDU_END:

		if (!(pi->conn_state & L2CAP_CONN_SAR_SDU))

			goto disconnect;



		if (!pi->sdu)

			goto disconnect;



		memcpy(skb_put(pi->sdu, skb->len), skb->data, skb->len);



		pi->conn_state &= ~L2CAP_CONN_SAR_SDU;

		pi->partial_sdu_len += skb->len;



		if (pi->partial_sdu_len > pi->imtu)

			goto drop;



		if (pi->partial_sdu_len != pi->sdu_len)

			goto drop;



		_skb = skb_clone(pi->sdu, GFP_ATOMIC);

		err = sock_queue_rcv_skb(sk, _skb);

		if (err < 0)

			kfree_skb(_skb);



		kfree_skb(pi->sdu);

		break;

	}



	kfree_skb(skb);

	return err;



drop:

	kfree_skb(pi->sdu);

	pi->sdu = NULL;



disconnect:

	l2cap_send_disconn_req(pi->conn, sk);

	kfree_skb(skb);

	return 0;

}



static int l2cap_streaming_reassembly_sdu(struct sock *sk, struct sk_buff *skb, u16 control)

{

	struct l2cap_pinfo *pi = l2cap_pi(sk);

	struct sk_buff *_skb;

	int err = -EINVAL;



	/*

	 * TODO: We have to notify the userland if some data is lost with the

	 * Streaming Mode.

	 */



	switch (control & L2CAP_CTRL_SAR) {

	case L2CAP_SDU_UNSEGMENTED:

		if (pi->conn_state & L2CAP_CONN_SAR_SDU) {
@@ -3429,7 +3528,7 @@ static void l2cap_check_srej_gap(struct sock *sk, u8 tx_seq) 


		skb = skb_dequeue(SREJ_QUEUE(sk));

		control = bt_cb(skb)->sar << L2CAP_CTRL_SAR_SHIFT;

		l2cap_sar_reassembly_sdu(sk, skb, control);

		l2cap_ertm_reassembly_sdu(sk, skb, control);

		l2cap_pi(sk)->buffer_seq_srej =

			(l2cap_pi(sk)->buffer_seq_srej + 1) % 64;

		tx_seq++;
@@ -3566,7 +3665,7 @@ static inline int l2cap_data_channel_iframe(struct sock *sk, u16 rx_control, str 


	pi->buffer_seq = (pi->buffer_seq + 1) % 64;



	err = l2cap_sar_reassembly_sdu(sk, skb, rx_control);

	err = l2cap_ertm_reassembly_sdu(sk, skb, rx_control);

	if (err < 0)

		return err;
@@ -3790,8 +3889,10 @@ static inline int l2cap_data_channel(struct l2cap_conn *conn, u16 cid, struct sk 
		 * Receiver will miss it and start proper recovery

		 * procedures and ask retransmission.

		 */

		if (len > pi->mps)

		if (len > pi->mps) {

			l2cap_send_disconn_req(pi->conn, sk);

			goto drop;

		}



		if (l2cap_check_fcs(pi, skb))

			goto drop;
@@ -3813,13 +3914,17 @@ static inline int l2cap_data_channel(struct l2cap_conn *conn, u16 cid, struct sk 
		}



		if (__is_iframe(control)) {

			if (len < 4)

			if (len < 4) {

				l2cap_send_disconn_req(pi->conn, sk);

				goto drop;

			}



			l2cap_data_channel_iframe(sk, control, skb);

		} else {

			if (len != 0)

			if (len != 0) {

				l2cap_send_disconn_req(pi->conn, sk);

				goto drop;

			}



			l2cap_data_channel_sframe(sk, control, skb);

		}
@@ -3850,7 +3955,7 @@ static inline int l2cap_data_channel(struct l2cap_conn *conn, u16 cid, struct sk 
		else

			pi->expected_tx_seq = (tx_seq + 1) % 64;



		l2cap_sar_reassembly_sdu(sk, skb, control);

		l2cap_streaming_reassembly_sdu(sk, skb, control);



		goto done;