Re: [PATCH 2/4] Bluetooth: Add support for Segmentation and Reassembly of SDUs

[Marcel Holtmann <marcel@holtmann.org>]

Hi Gustavo,

> ERTM should use Segmentation and Reassembly to break down a SDU in many
> PDUs on sending data to the other side.
> On sending packets we queue all 'segments' until end of segmentation and
> just the add them to the queue for sending.
> On receiving we create a new skb with the SDU reassembled.
> 
> Initially based on a patch from Nathan Holstein <nathan@lampreynetworks.com>
> 
> Signed-off-by: Gustavo F. Padovan <gustavo@las.ic.unicamp.br>
> ---
>  include/net/bluetooth/l2cap.h |   10 ++-
>  net/bluetooth/l2cap.c         |  180 ++++++++++++++++++++++++++++++++++++-----
>  2 files changed, 168 insertions(+), 22 deletions(-)
> 
> diff --git a/include/net/bluetooth/l2cap.h b/include/net/bluetooth/l2cap.h
> index cae561a..233a2fa 100644
> --- a/include/net/bluetooth/l2cap.h
> +++ b/include/net/bluetooth/l2cap.h
> @@ -34,7 +34,7 @@
>  #define L2CAP_DEFAULT_MAX_RECEIVE	1
>  #define L2CAP_DEFAULT_RETRANS_TO	300    /* 300 milliseconds */
>  #define L2CAP_DEFAULT_MONITOR_TO	1000   /* 1 second */
> -#define L2CAP_DEFAULT_MAX_RX_APDU	0xfff7
> +#define L2CAP_DEFAULT_MAX_PDU_SIZE	672
>  
>  #define L2CAP_CONN_TIMEOUT	(40000) /* 40 seconds */
>  #define L2CAP_INFO_TIMEOUT	(4000)  /*  4 seconds */
> @@ -313,6 +313,7 @@ struct l2cap_pinfo {
>  	__u8		conf_req[64];
>  	__u8		conf_len;
>  	__u8		conf_state;
> +	__u8		conn_state;
>  
>  	__u8		next_tx_seq;
>  	__u8		expected_ack_seq;
> @@ -320,6 +321,10 @@ struct l2cap_pinfo {
>  	__u8		expected_tx_seq;
>  	__u8		unacked_frames;
>  	__u8		num_to_ack;
> +	__u16		sdu_len;
> +	__u16		partial_sdu_len;
> +	__u8		start_txseq;
> +	struct sk_buff	*sdu;
>  
>  	__u8		ident;
>  
> @@ -348,6 +353,8 @@ struct l2cap_pinfo {
>  #define L2CAP_CONF_MAX_CONF_REQ 2
>  #define L2CAP_CONF_MAX_CONF_RSP 2
>  
> +#define L2CAP_CONN_SAR_SDU         0x01
> +
>  static inline int l2cap_tx_window_full(struct sock *sk)
>  {
>  	struct l2cap_pinfo *pi = l2cap_pi(sk);
> @@ -365,6 +372,7 @@ static inline int l2cap_tx_window_full(struct sock *sk)
>  #define __get_reqseq(ctrl) ((ctrl) & L2CAP_CTRL_REQSEQ) >> 8
>  #define __is_iframe(ctrl) !((ctrl) & L2CAP_CTRL_FRAME_TYPE)
>  #define __is_sframe(ctrl) (ctrl) & L2CAP_CTRL_FRAME_TYPE
> +#define __is_sar_sdu_start(ctrl) ((ctrl) & L2CAP_CTRL_SAR) == L2CAP_SDU_START

Using just __is_sar_start is enough here.
 
>  void l2cap_load(void);
>  
> diff --git a/net/bluetooth/l2cap.c b/net/bluetooth/l2cap.c
> index b2fd4f9..dcde60b 100644
> --- a/net/bluetooth/l2cap.c
> +++ b/net/bluetooth/l2cap.c
> @@ -1282,7 +1282,7 @@ static inline int l2cap_skbuff_fromiovec(struct sock *sk, struct msghdr *msg, in
>  	return sent;
>  }
>  
> -static struct sk_buff *l2cap_create_pdu(struct sock *sk, struct msghdr *msg, size_t len, u16 *control)
> +static struct sk_buff *l2cap_create_pdu(struct sock *sk, struct msghdr *msg, size_t len, u16 *control, u16 sdulen)
>  {
>  	struct l2cap_conn *conn = l2cap_pi(sk)->conn;
>  	struct sk_buff *skb;
> @@ -1291,8 +1291,11 @@ static struct sk_buff *l2cap_create_pdu(struct sock *sk, struct msghdr *msg, siz
>  
>  	BT_DBG("sk %p len %d", sk, (int)len);
>  
> -	if (control)
> +	if (control) {
>  		hlen += 2;
> +		if (sdulen)
> +			hlen +=2;
> +	}
>  	else if (sk->sk_type == SOCK_DGRAM)
>  		hlen += 2;

This is just ugly. We might really wanna split PDU creation here in
connection less, basic and ertm. Think about it.

> @@ -1307,8 +1310,11 @@ static struct sk_buff *l2cap_create_pdu(struct sock *sk, struct msghdr *msg, siz
>  	lh->cid = cpu_to_le16(l2cap_pi(sk)->dcid);
>  	lh->len = cpu_to_le16(len + (hlen - L2CAP_HDR_SIZE));
>  
> -	if (control)
> +	if (control) {
>  		put_unaligned_le16(*control, skb_put(skb, 2));
> +		if (sdulen)
> +			put_unaligned_le16(sdulen, skb_put(skb, 2));
> +	}
>  	else if (sk->sk_type == SOCK_DGRAM)
>  		put_unaligned_le16(l2cap_pi(sk)->psm, skb_put(skb, 2));
>  
> @@ -1317,10 +1323,58 @@ static struct sk_buff *l2cap_create_pdu(struct sock *sk, struct msghdr *msg, siz
>  		kfree_skb(skb);
>  		return ERR_PTR(err);
>  	}
> -
>  	return skb;
>  }
>  
> +static inline int l2cap_sar_segment_sdu(struct sock *sk, struct msghdr *msg, size_t len)
> +{
> +	struct l2cap_pinfo *pi = l2cap_pi(sk);
> +	struct sk_buff *skb;
> +	struct sk_buff_head sar_queue;
> +	u16 control;
> +	size_t size = 0;
> +
> +	__skb_queue_head_init(&sar_queue);
> +	control |= L2CAP_SDU_START;
> +	skb = l2cap_create_pdu(sk, msg, pi->max_pdu_size, &control, len);
> +	if (IS_ERR(skb))
> +		return PTR_ERR(skb);
> +
> +	__skb_queue_tail(&sar_queue, skb);
> +	len -= pi->max_pdu_size;
> +	size +=pi->max_pdu_size;
> +	control = 0;
> +
> +	while (len > 0) {
> +		size_t buflen;
> +
> +		if (len > pi->max_pdu_size) {
> +			control |= L2CAP_SDU_CONTINUE;
> +			buflen = pi->max_pdu_size;
> +		}
> +		else {

Coding style error. Must be  } else { all on the same line.

> +			control |= L2CAP_SDU_END;
> +			buflen = len;
> +		}
> +
> +		skb = l2cap_create_pdu(sk, msg, buflen ,&control, 0);

It is ,<space> and not <space>,

> +		if (IS_ERR(skb)) {
> +			skb_queue_purge(&sar_queue);
> +			return PTR_ERR(skb);
> +		}
> +
> +		__skb_queue_tail(&sar_queue, skb);
> +		len -= buflen;
> +		size += buflen;
> +		control = 0;
> +	}
> +	skb_queue_splice_tail(&sar_queue, TX_QUEUE(sk));
> +	if (sk->sk_send_head == NULL)
> +		sk->sk_send_head = sar_queue.next;
> +
> +	return size;
> +}
> +
>  static int l2cap_sock_sendmsg(struct kiocb *iocb, struct socket *sock, struct msghdr *msg, size_t len)
>  {
>  	struct sock *sk = sock->sk;
> @@ -1339,7 +1393,7 @@ static int l2cap_sock_sendmsg(struct kiocb *iocb, struct socket *sock, struct ms
>  		return -EOPNOTSUPP;
>  
>  	/* Check outgoing MTU */
> -	if ((sk->sk_type != SOCK_RAW || pi->mode == L2CAP_MODE_BASIC)
> +	if ((sk->sk_type != SOCK_RAW && pi->mode == L2CAP_MODE_BASIC)
>  			&& len > pi->omtu)
>  		return -EINVAL;

I don't understand this one. If it is right, then also the extra
brackets are pointless now.

>  
> @@ -1353,7 +1407,7 @@ static int l2cap_sock_sendmsg(struct kiocb *iocb, struct socket *sock, struct ms
>  	switch (pi->mode) {
>  	case L2CAP_MODE_BASIC:
>  		/* Create a basic PDU */
> -		skb = l2cap_create_pdu(sk, msg, len, NULL);
> +		skb = l2cap_create_pdu(sk, msg, len, NULL, 0);
>  		if (IS_ERR(skb)) {
>  			err = PTR_ERR(skb);
>  			goto done;
> @@ -1366,22 +1420,23 @@ static int l2cap_sock_sendmsg(struct kiocb *iocb, struct socket *sock, struct ms
>  
>  	case L2CAP_MODE_ERTM:
>  		/* Entire SDU fits into one PDU */
> -		if (len <= pi->omtu) {
> +		if (len <= pi->max_pdu_size) {
>  			control = L2CAP_SDU_UNSEGMENTED;
> -			skb = l2cap_create_pdu(sk, msg, len, &control);
> +			skb = l2cap_create_pdu(sk, msg, len, &control, 0);
>  			if (IS_ERR(skb)) {
>  				err = PTR_ERR(skb);
>  				goto done;
>  			}
> +			__skb_queue_tail(TX_QUEUE(sk), skb);
> +			if (sk->sk_send_head == NULL)
> +				sk->sk_send_head = skb;
>  		}
> +		/* Segment SDU into multiples PDUs */
>  		else {
> -			/* FIXME: Segmentation will be added later */
> -			err = -EINVAL;
> -			goto done;
> +			err = l2cap_sar_segment_sdu(sk, msg, len);
> +			if (unlikely(err < 0))
> +				goto done;
>  		}
> -		__skb_queue_tail(TX_QUEUE(sk), skb);
> -		if (sk->sk_send_head == NULL)
> -			sk->sk_send_head = skb;
>  
>  		err = l2cap_ertm_send(sk);
>  		if (!err)
> @@ -1959,7 +2014,7 @@ done:
>  		rfc.max_transmit    = L2CAP_DEFAULT_MAX_RECEIVE;
>  		rfc.retrans_timeout = 0;
>  		rfc.monitor_timeout = 0;
> -		rfc.max_pdu_size    = cpu_to_le16(L2CAP_DEFAULT_MAX_RX_APDU);
> +		rfc.max_pdu_size    = cpu_to_le16(L2CAP_DEFAULT_MAX_PDU_SIZE);
>  
>  		l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC,
>  					sizeof(rfc), (unsigned long) &rfc);
> @@ -1971,7 +2026,7 @@ done:
>  		rfc.max_transmit    = 0;
>  		rfc.retrans_timeout = 0;
>  		rfc.monitor_timeout = 0;
> -		rfc.max_pdu_size    = cpu_to_le16(L2CAP_DEFAULT_MAX_RX_APDU);
> +		rfc.max_pdu_size    = cpu_to_le16(L2CAP_DEFAULT_MAX_PDU_SIZE);
>  
>  		l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC,
>  					sizeof(rfc), (unsigned long) &rfc);
> @@ -2760,6 +2815,85 @@ static inline void l2cap_sig_channel(struct l2cap_conn *conn, struct sk_buff *sk
>  	kfree_skb(skb);
>  }
>  
> +static int l2cap_sar_reassembly_sdu(struct sock *sk, struct sk_buff *skb, u16 control, u8 txseq)
> +{
> +	struct l2cap_pinfo *pi = l2cap_pi(sk);
> +	struct sk_buff *_skb;
> +	int err = -EINVAL;
> +
> +	switch (control & L2CAP_CTRL_SAR) {
> +	case L2CAP_SDU_UNSEGMENTED:
> +		if (pi->conn_state & L2CAP_CONN_SAR_SDU)
> +			goto drop2;
> +
> +		err = sock_queue_rcv_skb(sk, skb);
> +		if (unlikely(err < 0))
> +			goto drop;

Drop the unlikely here. Not helping much.

> +		break;
> +
> +	case L2CAP_SDU_START:
> +		if (pi->conn_state & L2CAP_CONN_SAR_SDU)
> +			goto drop2;
> +
> +		pi->sdu_len = get_unaligned_le16(skb->data);
> +		skb_pull(skb, 2);
> +
> +		pi->sdu = bt_skb_alloc(pi->sdu_len, GFP_ATOMIC);
> +		if (!pi->sdu)
> +			goto drop;
> +
> +		memcpy(skb_put(pi->sdu, skb->len), skb->data, skb->len);
> +
> +		pi->conn_state |= L2CAP_CONN_SAR_SDU;
> +		pi->partial_sdu_len = skb->len;
> +		pi->start_txseq = txseq;
> +		kfree_skb(skb);
> +		break;
> +
> +	case L2CAP_SDU_CONTINUE:
> +		if (!(pi->conn_state & L2CAP_CONN_SAR_SDU))
> +			goto drop;
> +
> +		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 drop2;
> +
> +		kfree_skb(skb);
> +		break;
> +
> +	case L2CAP_SDU_END:
> +		if (!(pi->conn_state & L2CAP_CONN_SAR_SDU))
> +			goto drop;
> +
> +		memcpy(skb_put(pi->sdu, skb->len), skb->data, skb->len);
> +
> +		pi->conn_state &= !L2CAP_CONN_SAR_SDU;

What is this. Removing a flag is &= ~ and not &= !.

> +		pi->partial_sdu_len += skb->len;
> +
> +		if (pi->partial_sdu_len != pi->sdu_len)
> +			goto drop2;
> +
> +		_skb = skb_clone(pi->sdu, GFP_ATOMIC);
> +		err = sock_queue_rcv_skb(sk, _skb);
> +		if (unlikely(err < 0))
> +			kfree_skb(_skb);

Drop the Unlikely here, too.

> +		kfree_skb(pi->sdu);
> +		kfree_skb(skb);
> +		break;
> +	}
> +	return 0;
> +
> +drop2:
> +	kfree_skb(pi->sdu);
> +
> +drop:
> +	kfree_skb(skb);
> +	return err;
> +}
> +
>  static inline int l2cap_data_channel_iframe(struct sock *sk, u16 rx_control, struct sk_buff *skb)
>  {
>  	struct l2cap_pinfo *pi = l2cap_pi(sk);
> @@ -2772,11 +2906,11 @@ static inline int l2cap_data_channel_iframe(struct sock *sk, u16 rx_control, str
>  	if (tx_seq != pi->expected_tx_seq)
>  		return -EINVAL;
>  
> -	L2CAP_SEQ_NUM_INC(pi->expected_tx_seq);
> -	err = sock_queue_rcv_skb(sk, skb);
> -	if (err)
> +	err = l2cap_sar_reassembly_sdu(sk, skb, rx_control, tx_seq);
> +	if (unlikely(err < 0))
>  		return err;

No unlikely please.

>  
> +	L2CAP_SEQ_NUM_INC(pi->expected_tx_seq);
>  	L2CAP_NUM_TO_ACK_INC(pi->num_to_ack);
>  	if (pi->num_to_ack == L2CAP_DEFAULT_NUM_TO_ACK - 1) {
>  		tx_control |= L2CAP_CTRL_FRAME_TYPE;
> @@ -2815,7 +2949,7 @@ static inline int l2cap_data_channel_sframe(struct sock *sk, u16 rx_control, str
>  static inline int l2cap_data_channel(struct l2cap_conn *conn, u16 cid, struct sk_buff *skb)
>  {
>  	struct sock *sk;
> -	u16 control;
> +	u16 control, len;
>  	int err;
>  
>  	sk = l2cap_get_chan_by_scid(&conn->chan_list, cid);
> @@ -2846,8 +2980,12 @@ static inline int l2cap_data_channel(struct l2cap_conn *conn, u16 cid, struct sk
>  	case L2CAP_MODE_ERTM:
>  		control = get_unaligned_le16(skb->data);
>  		skb_pull(skb, 2);
> +		len = skb->len;
>  
> -		if (l2cap_pi(sk)->imtu < skb->len)
> +		if (__is_sar_sdu_start(control))
> +			len -= 2;
> +
> +		if (L2CAP_DEFAULT_MAX_PDU_SIZE < len)
>  			goto drop;
>  
>  		if (__is_iframe(control))

Regards

Marcel