Hi Gustavo, > This patch adds support for ERTM transfers, without retransmission, with > txWindow up to 63 and with acknowledgement of packets received. Now the > packets are queued before call l2cap_do_send(), so packets couldn't be > sent at the time we call l2cap_sock_sendmsg(). They will be sent in > an asynchronous way on later calls of l2cap_ertm_send(). Besides if an > error occurs on calling l2cap_do_send() we disconnect the channel. > > Initially based on a patch from Nathan Holstein <nathan@xxxxxxxxxxxxxxxxxxx> > > Signed-off-by: Gustavo F. Padovan <gustavo@xxxxxxxxxxxxxxxxx> > --- > include/net/bluetooth/bluetooth.h | 3 +- > include/net/bluetooth/l2cap.h | 56 ++++++- > net/bluetooth/l2cap.c | 340 +++++++++++++++++++++++++++++++------ > 3 files changed, 348 insertions(+), 51 deletions(-) > > diff --git a/include/net/bluetooth/bluetooth.h b/include/net/bluetooth/bluetooth.h > index 968166a..65a5cf8 100644 > --- a/include/net/bluetooth/bluetooth.h > +++ b/include/net/bluetooth/bluetooth.h > @@ -138,8 +138,9 @@ struct sock *bt_accept_dequeue(struct sock *parent, struct socket *newsock); > struct bt_skb_cb { > __u8 pkt_type; > __u8 incoming; > + __u8 tx_seq; > }; > -#define bt_cb(skb) ((struct bt_skb_cb *)(skb->cb)) > +#define bt_cb(skb) ((struct bt_skb_cb *)((skb)->cb)) > > static inline struct sk_buff *bt_skb_alloc(unsigned int len, gfp_t how) > { > diff --git a/include/net/bluetooth/l2cap.h b/include/net/bluetooth/l2cap.h > index 6fc7698..cae561a 100644 > --- a/include/net/bluetooth/l2cap.h > +++ b/include/net/bluetooth/l2cap.h > @@ -29,7 +29,8 @@ > #define L2CAP_DEFAULT_MTU 672 > #define L2CAP_DEFAULT_MIN_MTU 48 > #define L2CAP_DEFAULT_FLUSH_TO 0xffff > -#define L2CAP_DEFAULT_TX_WINDOW 1 > +#define L2CAP_DEFAULT_TX_WINDOW 63 > +#define L2CAP_DEFAULT_NUM_TO_ACK (L2CAP_DEFAULT_TX_WINDOW/5) > #define L2CAP_DEFAULT_MAX_RECEIVE 1 > #define L2CAP_DEFAULT_RETRANS_TO 300 /* 300 milliseconds */ > #define L2CAP_DEFAULT_MONITOR_TO 1000 /* 1 second */ > @@ -94,6 +95,33 @@ struct l2cap_conninfo { > #define L2CAP_FCS_NONE 0x00 > #define L2CAP_FCS_CRC16 0x01 > > +/* L2CAP Control Field bit masks */ > +#define L2CAP_CTRL_SAR 0xC000 > +#define L2CAP_CTRL_REQSEQ 0x3F00 > +#define L2CAP_CTRL_TXSEQ 0x007E > +#define L2CAP_CTRL_RETRANS 0x0080 > +#define L2CAP_CTRL_FINAL 0x0080 > +#define L2CAP_CTRL_POLL 0x0010 > +#define L2CAP_CTRL_SUPERVISE 0x000C > +#define L2CAP_CTRL_FRAME_TYPE 0x0001 /* I- or S-Frame */ > + > +#define L2CAP_SEQ_NUM_INC(seq) ((seq) = (seq + 1) % 64) the second seq needs to be (seq) too. I would also drop the L2CAP_ prefix here. And then again, why are we using this and not do it directly in the code? > +#define L2CAP_CTRL_TXSEQ_SHIFT 1 > +#define L2CAP_CTRL_REQSEQ_SHIFT 8 > +#define L2CAP_NUM_TO_ACK_INC(seq) ((seq) = (seq + 1) % L2CAP_DEFAULT_NUM_TO_ACK) I don't see a point in this one. It is only used once and it is more obfuscating the code than helping it. > +/* L2CAP Supervisory Function */ > +#define L2CAP_SUPER_RCV_READY 0x0000 > +#define L2CAP_SUPER_REJECT 0x0004 > +#define L2CAP_SUPER_RCV_NOT_READY 0x0008 > +#define L2CAP_SUPER_SELECT_REJECT 0x000C > + > +/* L2CAP Segmentation and Reassembly */ > +#define L2CAP_SDU_UNSEGMENTED 0x0000 > +#define L2CAP_SDU_START 0x4000 > +#define L2CAP_SDU_END 0x8000 > +#define L2CAP_SDU_CONTINUE 0xC000 > + > /* L2CAP structures */ > struct l2cap_hdr { > __le16 len; > @@ -262,6 +290,7 @@ 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) > > struct l2cap_pinfo { > struct bt_sock bt; > @@ -285,6 +314,13 @@ struct l2cap_pinfo { > __u8 conf_len; > __u8 conf_state; > > + __u8 next_tx_seq; > + __u8 expected_ack_seq; > + __u8 req_seq; > + __u8 expected_tx_seq; > + __u8 unacked_frames; > + __u8 num_to_ack; > + > __u8 ident; > > __u8 remote_tx_win; > @@ -295,6 +331,7 @@ struct l2cap_pinfo { > > __le16 sport; > > + struct sk_buff_head tx_queue; > struct l2cap_conn *conn; > struct sock *next_c; > struct sock *prev_c; > @@ -311,6 +348,23 @@ struct l2cap_pinfo { > #define L2CAP_CONF_MAX_CONF_REQ 2 > #define L2CAP_CONF_MAX_CONF_RSP 2 > > +static inline int l2cap_tx_window_full(struct sock *sk) > +{ > + struct l2cap_pinfo *pi = l2cap_pi(sk); > + int sub; > + > + sub = (pi->next_tx_seq - pi->expected_ack_seq) % 64; > + > + if (sub < 0) > + sub += 64; > + > + return (sub == pi->remote_tx_win); > +} > + > +#define __get_txseq(ctrl) ((ctrl) & L2CAP_CTRL_TXSEQ) >> 1 > +#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 > > void l2cap_load(void); > > diff --git a/net/bluetooth/l2cap.c b/net/bluetooth/l2cap.c > index 18b3c62..b2fd4f9 100644 > --- a/net/bluetooth/l2cap.c > +++ b/net/bluetooth/l2cap.c > @@ -333,6 +333,30 @@ static inline int l2cap_send_cmd(struct l2cap_conn *conn, u8 ident, u8 code, u16 > return hci_send_acl(conn->hcon, skb, 0); > } > > +static inline int l2cap_send_sframe(struct l2cap_pinfo *pi, u16 control) > +{ > + struct sk_buff *skb; > + struct l2cap_hdr *lh; > + struct l2cap_conn *conn = pi->conn; > + int count; > + > + BT_DBG("pi %p, control 0x%2.2x", pi, control); > + > + count = min_t(unsigned int, conn->mtu, L2CAP_HDR_SIZE + 2); > + control |= L2CAP_CTRL_FRAME_TYPE; > + > + skb = bt_skb_alloc(count, GFP_ATOMIC); > + if (!skb) > + return -ENOMEM; > + > + lh = (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE); > + lh->len = cpu_to_le16(2); > + lh->cid = cpu_to_le16(pi->dcid); > + put_unaligned_le16(control, skb_put(skb, 2)); > + > + return hci_send_acl(pi->conn->hcon, skb, 0); > +} > + > static void l2cap_do_start(struct sock *sk) > { > struct l2cap_conn *conn = l2cap_pi(sk)->conn; > @@ -1155,39 +1179,84 @@ static int l2cap_sock_getname(struct socket *sock, struct sockaddr *addr, int *l > return 0; > } > > -static inline int l2cap_do_send(struct sock *sk, struct msghdr *msg, int len) > +static void l2cap_drop_acked_frames(struct sock *sk) > { > - struct l2cap_conn *conn = l2cap_pi(sk)->conn; > - struct sk_buff *skb, **frag; > - int err, hlen, count, sent = 0; > - struct l2cap_hdr *lh; > + struct sk_buff *skb; > > - BT_DBG("sk %p len %d", sk, len); > + while ((skb = skb_peek(TX_QUEUE(sk)))) { > + if (bt_cb(skb)->tx_seq == l2cap_pi(sk)->expected_ack_seq) > + break; > > - /* First fragment (with L2CAP header) */ > - if (sk->sk_type == SOCK_DGRAM) > - hlen = L2CAP_HDR_SIZE + 2; > - else > - hlen = L2CAP_HDR_SIZE; > + skb = skb_dequeue(TX_QUEUE(sk)); > + kfree_skb(skb); > > - count = min_t(unsigned int, (conn->mtu - hlen), len); > + l2cap_pi(sk)->unacked_frames--; > + } > > - skb = bt_skb_send_alloc(sk, hlen + count, > - msg->msg_flags & MSG_DONTWAIT, &err); > - if (!skb) > - return err; > + return; > +} > > - /* Create L2CAP header */ > - lh = (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE); > - lh->cid = cpu_to_le16(l2cap_pi(sk)->dcid); > - lh->len = cpu_to_le16(len + (hlen - L2CAP_HDR_SIZE)); > +static inline int l2cap_do_send(struct sock *sk, struct sk_buff *skb) > +{ > + struct l2cap_pinfo *pi = l2cap_pi(sk); > + int err; > + > + BT_DBG("sk %p, skb %p len %d", sk, skb, skb->len); > + > + err = hci_send_acl(pi->conn->hcon, skb, 0); > + > + if (unlikely(err) < 0) > + kfree_skb(skb); Drop this unlikely. It is not worth it. > + > + return err; > +} > + > +static int l2cap_ertm_send(struct sock *sk) > +{ > + struct sk_buff *skb, *tx_skb; > + struct l2cap_pinfo *pi = l2cap_pi(sk); > + u8 tx_seq; > + u16 *control; > + int err; > + > + while ((skb = sk->sk_send_head) && (!l2cap_tx_window_full(sk))) { > + tx_seq = pi->next_tx_seq; > + tx_skb = skb_clone(skb, GFP_ATOMIC); > + > + control = (u16 *)(skb->data + L2CAP_HDR_SIZE); > + *control |= cpu_to_le16( > + (pi->req_seq << L2CAP_CTRL_REQSEQ_SHIFT) > + | (tx_seq << L2CAP_CTRL_TXSEQ_SHIFT)); > + > + err = l2cap_do_send(sk, tx_skb); > + if (err < 0) { > + l2cap_send_disconn_req(pi->conn, sk); > + return err; > + } > + > + L2CAP_SEQ_NUM_INC(pi->next_tx_seq); > + bt_cb(skb)->tx_seq = tx_seq; > + > + pi->unacked_frames++; > + > + if (skb_queue_is_last(TX_QUEUE(sk), skb)) > + sk->sk_send_head = NULL; > + else > + sk->sk_send_head = skb_queue_next(TX_QUEUE(sk), skb); > + } > + > + return 0; > +} > > - if (sk->sk_type == SOCK_DGRAM) > - put_unaligned(l2cap_pi(sk)->psm, (__le16 *) skb_put(skb, 2)); > + > +static inline int l2cap_skbuff_fromiovec(struct sock *sk, struct msghdr *msg, int len, int count, struct sk_buff *skb) > +{ > + struct l2cap_conn *conn = l2cap_pi(sk)->conn; > + struct sk_buff **frag; > + int err, sent = 0; > > if (memcpy_fromiovec(skb_put(skb, count), msg->msg_iov, count)) { > - err = -EFAULT; > - goto fail; > + return -EFAULT; > } > > sent += count; > @@ -1200,33 +1269,65 @@ static inline int l2cap_do_send(struct sock *sk, struct msghdr *msg, int len) > > *frag = bt_skb_send_alloc(sk, count, msg->msg_flags & MSG_DONTWAIT, &err); > if (!*frag) > - goto fail; > - > - if (memcpy_fromiovec(skb_put(*frag, count), msg->msg_iov, count)) { > - err = -EFAULT; > - goto fail; > - } > + return -EFAULT; > + if (memcpy_fromiovec(skb_put(*frag, count), msg->msg_iov, count)) > + return -EFAULT; > > sent += count; > len -= count; > > frag = &(*frag)->next; > } > - err = hci_send_acl(conn->hcon, skb, 0); > - if (err < 0) > - goto fail; > > return sent; > +} > > -fail: > - kfree_skb(skb); > - return err; > +static struct sk_buff *l2cap_create_pdu(struct sock *sk, struct msghdr *msg, size_t len, u16 *control) Why does control have to be a pointer here. You are not modifying its content anyway. > +{ > + struct l2cap_conn *conn = l2cap_pi(sk)->conn; > + struct sk_buff *skb; > + int err, count, hlen = L2CAP_HDR_SIZE; > + struct l2cap_hdr *lh; > + > + BT_DBG("sk %p len %d", sk, (int)len); > + > + if (control) > + hlen += 2; > + else if (sk->sk_type == SOCK_DGRAM) > + hlen += 2; An if (control || sk->sk_type == ...) would do the same here. I know wanna make it the same as below, but it is confusing. > + > + count = min_t(unsigned int, (conn->mtu - hlen), len); > + skb = bt_skb_send_alloc(sk, count + hlen, > + msg->msg_flags & MSG_DONTWAIT, &err); > + if (!skb) > + return ERR_PTR(-ENOMEM); > + > + /* Create L2CAP header */ > + lh = (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE); > + lh->cid = cpu_to_le16(l2cap_pi(sk)->dcid); > + lh->len = cpu_to_le16(len + (hlen - L2CAP_HDR_SIZE)); > + > + if (control) > + put_unaligned_le16(*control, skb_put(skb, 2)); > + else if (sk->sk_type == SOCK_DGRAM) > + put_unaligned_le16(l2cap_pi(sk)->psm, skb_put(skb, 2)); > + > + err = l2cap_skbuff_fromiovec(sk, msg, len, count, skb); > + if (unlikely(err < 0)) { > + kfree_skb(skb); > + return ERR_PTR(err); > + } > + > + return skb; > } > > static int l2cap_sock_sendmsg(struct kiocb *iocb, struct socket *sock, struct msghdr *msg, size_t len) > { > struct sock *sk = sock->sk; > - int err = 0; > + struct l2cap_pinfo *pi = l2cap_pi(sk); > + struct sk_buff *skb; > + u16 control; > + int err; > > BT_DBG("sock %p, sk %p", sock, sk); > > @@ -1238,16 +1339,61 @@ static int l2cap_sock_sendmsg(struct kiocb *iocb, struct socket *sock, struct ms > return -EOPNOTSUPP; > > /* Check outgoing MTU */ > - if (sk->sk_type != SOCK_RAW && len > l2cap_pi(sk)->omtu) > + if ((sk->sk_type != SOCK_RAW || pi->mode == L2CAP_MODE_BASIC) > + && len > pi->omtu) > return -EINVAL; I am confused now. Wouldn't it be better to check for == SOCK_SEQPACKET and that it is basic mode and that the len in smaller than the MTU. > lock_sock(sk); > > - if (sk->sk_state == BT_CONNECTED) > - err = l2cap_do_send(sk, msg, len); > - else > + if (sk->sk_state != BT_CONNECTED) { > err = -ENOTCONN; > + goto done; > + } > + > + switch (pi->mode) { > + case L2CAP_MODE_BASIC: > + /* Create a basic PDU */ > + skb = l2cap_create_pdu(sk, msg, len, NULL); > + if (IS_ERR(skb)) { > + err = PTR_ERR(skb); > + goto done; > + } > + > + err = l2cap_do_send(sk, skb); > + if (!err) > + err = len; > + break; > + > + case L2CAP_MODE_ERTM: > + /* Entire SDU fits into one PDU */ > + if (len <= pi->omtu) { > + control = L2CAP_SDU_UNSEGMENTED; > + skb = l2cap_create_pdu(sk, msg, len, &control); > + if (IS_ERR(skb)) { > + err = PTR_ERR(skb); > + goto done; > + } > + } > + else { > + /* FIXME: Segmentation will be added later */ > + err = -EINVAL; > + 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) > + err = len; > + break; > + > + default: > + BT_DBG("bad state %1.1x", pi->mode); > + err = -EINVAL; > + } > + > +done: > release_sock(sk); > return err; > } > @@ -2302,6 +2448,10 @@ static inline int l2cap_config_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr > > if (l2cap_pi(sk)->conf_state & L2CAP_CONF_INPUT_DONE) { > sk->sk_state = BT_CONNECTED; > + l2cap_pi(sk)->next_tx_seq = 0; > + l2cap_pi(sk)->expected_ack_seq = 0; > + l2cap_pi(sk)->unacked_frames = 0; > + __skb_queue_head_init(TX_QUEUE(sk)); > l2cap_chan_ready(sk); > goto unlock; > } > @@ -2376,6 +2526,9 @@ static inline int l2cap_config_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hdr > > if (l2cap_pi(sk)->conf_state & L2CAP_CONF_OUTPUT_DONE) { > sk->sk_state = BT_CONNECTED; > + l2cap_pi(sk)->expected_tx_seq = 0; > + l2cap_pi(sk)->num_to_ack = 0; > + __skb_queue_head_init(TX_QUEUE(sk)); > l2cap_chan_ready(sk); > } > > @@ -2406,6 +2559,8 @@ static inline int l2cap_disconnect_req(struct l2cap_conn *conn, struct l2cap_cmd > > sk->sk_shutdown = SHUTDOWN_MASK; > > + skb_queue_purge(TX_QUEUE(sk)); > + > l2cap_chan_del(sk, ECONNRESET); > bh_unlock_sock(sk); > > @@ -2428,6 +2583,8 @@ static inline int l2cap_disconnect_rsp(struct l2cap_conn *conn, struct l2cap_cmd > if (!sk) > return 0; > > + skb_queue_purge(TX_QUEUE(sk)); > + > l2cap_chan_del(sk, 0); > bh_unlock_sock(sk); > > @@ -2603,9 +2760,63 @@ static inline void l2cap_sig_channel(struct l2cap_conn *conn, struct sk_buff *sk > kfree_skb(skb); > } > > +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; > + int err; > + > + BT_DBG("sk %p rx_control 0x%4.4x len %d", sk, rx_control, skb->len); > + > + 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) > + return err; > + > + 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; > + tx_control |= L2CAP_SUPER_RCV_READY; > + tx_control |= pi->expected_tx_seq << L2CAP_CTRL_REQSEQ_SHIFT; > + > + err = l2cap_send_sframe(pi, tx_control); > + if (unlikely(err)) > + return err; Drop this unlikely, too. Just return err. Actually at this point in the flow there is no need to check it at all. > + } > + 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); > + > + 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: > + pi->expected_ack_seq = __get_reqseq(rx_control); > + l2cap_drop_acked_frames(sk); > + l2cap_ertm_send(sk); > + break; > + > + case L2CAP_SUPER_RCV_NOT_READY: > + case L2CAP_SUPER_REJECT: > + case L2CAP_SUPER_SELECT_REJECT: > + break; > + } > + > + return 0; > +} > + > static inline int l2cap_data_channel(struct l2cap_conn *conn, u16 cid, struct sk_buff *skb) > { > struct sock *sk; > + u16 control; > + int err; > > sk = l2cap_get_chan_by_scid(&conn->chan_list, cid); > if (!sk) { > @@ -2618,16 +2829,40 @@ static inline int l2cap_data_channel(struct l2cap_conn *conn, u16 cid, struct sk > if (sk->sk_state != BT_CONNECTED) > goto drop; > > - if (l2cap_pi(sk)->imtu < skb->len) > - goto drop; > + switch (l2cap_pi(sk)->mode) { > + case L2CAP_MODE_BASIC: > + /* If socket recv buffers overflows we drop data here > + * which is *bad* because L2CAP has to be reliable. > + * But we don't have any other choice. L2CAP doesn't > + * provide flow control mechanism. */ > > - /* If socket recv buffers overflows we drop data here > - * which is *bad* because L2CAP has to be reliable. > - * But we don't have any other choice. L2CAP doesn't > - * provide flow control mechanism. */ > + if (l2cap_pi(sk)->imtu < skb->len) > + goto drop; > > - if (!sock_queue_rcv_skb(sk, skb)) > - goto done; > + if (!sock_queue_rcv_skb(sk, skb)) > + goto done; > + break; > + > + case L2CAP_MODE_ERTM: > + control = get_unaligned_le16(skb->data); > + skb_pull(skb, 2); > + > + if (l2cap_pi(sk)->imtu < skb->len) > + goto drop; > + > + if (__is_iframe(control)) > + err = l2cap_data_channel_iframe(sk, control, skb); > + else > + err = l2cap_data_channel_sframe(sk, control, skb); > + > + if (!err) > + goto done; > + break; > + > + default: > + BT_DBG("sk %p: bad mode 0x%2.2x", sk, l2cap_pi(sk)->mode); > + break; > + } > > drop: > kfree_skb(skb); > @@ -2677,6 +2912,9 @@ static void l2cap_recv_frame(struct l2cap_conn *conn, struct sk_buff *skb) > cid = __le16_to_cpu(lh->cid); > len = __le16_to_cpu(lh->len); > > + if (len != skb->len) > + goto drop; > + > BT_DBG("len %d, cid 0x%4.4x", len, cid); > > switch (cid) { > @@ -2694,6 +2932,10 @@ static void l2cap_recv_frame(struct l2cap_conn *conn, struct sk_buff *skb) > l2cap_data_channel(conn, cid, skb); > break; > } > + return; > + > +drop: > + kfree_skb(skb); So this change is pointless. Just free the skb and return from the actual length check. There is no benefit to go to the end of the function here. Regards Marcel -- To unsubscribe from this list: send the line "unsubscribe linux-bluetooth" in the body of a message to majordomo@xxxxxxxxxxxxxxx More majordomo info at http://vger.kernel.org/majordomo-info.html