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 <[email protected]>
Signed-off-by: Gustavo F. Padovan <[email protected]>
---
include/net/bluetooth/bluetooth.h | 3 +-
include/net/bluetooth/l2cap.h | 54 +++++-
net/bluetooth/l2cap.c | 389 ++++++++++++++++++++++++++++++++-----
3 files changed, 395 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..9bbfbe7 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,31 @@ 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_CTRL_TXSEQ_SHIFT 1
+#define L2CAP_CTRL_REQSEQ_SHIFT 8
+
+/* 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 +288,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 +312,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 +329,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 +346,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..a4e3cdc 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 (err < 0)
+ kfree_skb(skb);
+
+ 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);
- if (sk->sk_type == SOCK_DGRAM)
- put_unaligned(l2cap_pi(sk)->psm, (__le16 *) skb_put(skb, 2));
+ 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;
+ }
+
+ pi->next_tx_seq = (pi->next_tx_seq + 1) % 64;
+ 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;
+}
+
+
+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,112 @@ 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_conless_pdu(struct sock *sk, struct msghdr *msg, size_t len)
+{
+ struct l2cap_conn *conn = l2cap_pi(sk)->conn;
+ struct sk_buff *skb;
+ int err, count, hlen = L2CAP_HDR_SIZE + 2;
+ struct l2cap_hdr *lh;
+
+ BT_DBG("sk %p len %d", sk, (int)len);
+
+ 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));
+ 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 struct sk_buff *l2cap_create_basic_pdu(struct sock *sk, struct msghdr *msg, size_t len)
+{
+ 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);
+
+ 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));
+
+ err = l2cap_skbuff_fromiovec(sk, msg, len, count, skb);
+ if (unlikely(err < 0)) {
+ kfree_skb(skb);
+ return ERR_PTR(err);
+ }
+ return skb;
+}
+
+static struct sk_buff *l2cap_create_ertm_pdu(struct sock *sk, struct msghdr *msg, size_t len, u16 control)
+{
+ struct l2cap_conn *conn = l2cap_pi(sk)->conn;
+ struct sk_buff *skb;
+ int err, count, hlen = L2CAP_HDR_SIZE + 2;
+ struct l2cap_hdr *lh;
+
+ BT_DBG("sk %p len %d", sk, (int)len);
+
+ 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));
+ put_unaligned_le16(control, 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 +1386,68 @@ 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_SEQPACKET && pi->mode == L2CAP_MODE_BASIC
+ && len > pi->omtu)
return -EINVAL;
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;
+ }
+
+ /* Connectionless channel */
+ if (sk->sk_type == SOCK_DGRAM) {
+ skb = l2cap_create_conless_pdu(sk, msg, len);
+ err = l2cap_do_send(sk, skb);
+ goto done;
+ }
+ switch (pi->mode) {
+ case L2CAP_MODE_BASIC:
+ /* Create a basic PDU */
+ skb = l2cap_create_basic_pdu(sk, msg, len);
+ 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_ertm_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 +2502,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 +2580,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 +2613,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 +2637,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 +2814,60 @@ 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 = 0;
+
+ 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;
+
+ pi->expected_tx_seq = (pi->expected_tx_seq + 1) % 64;
+ err = sock_queue_rcv_skb(sk, skb);
+ if (err)
+ 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_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);
+ }
+ return err;
+}
+
+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 +2880,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 +2963,11 @@ 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) {
+ kfree_skb(skb);
+ return;
+ }
+
BT_DBG("len %d, cid 0x%4.4x", len, cid);
switch (cid) {
--
1.6.3.3
L2CAP uses rentransmission and monitor timers to inquiry the other side
about unacked I-frames. After send each I-frame we (re)start the
retransmission timer, if it expires, we start a monitor timer that send a
S-frame with P bit set and wait for S-frame with F bit set.
If monitor timer expires, it try again, at a maximum of
L2CAP_DEFAULT_MAX_TX.
Signed-off-by: Gustavo F. Padovan <[email protected]>
---
include/net/bluetooth/bluetooth.h | 1 +
include/net/bluetooth/l2cap.h | 15 +++++-
net/bluetooth/l2cap.c | 81 +++++++++++++++++++++++++++++++++++--
3 files changed, 90 insertions(+), 7 deletions(-)
diff --git a/include/net/bluetooth/bluetooth.h b/include/net/bluetooth/bluetooth.h
index 65a5cf8..b8b9a84 100644
--- a/include/net/bluetooth/bluetooth.h
+++ b/include/net/bluetooth/bluetooth.h
@@ -139,6 +139,7 @@ struct bt_skb_cb {
__u8 pkt_type;
__u8 incoming;
__u8 tx_seq;
+ __u8 retries;
};
#define bt_cb(skb) ((struct bt_skb_cb *)((skb)->cb))
diff --git a/include/net/bluetooth/l2cap.h b/include/net/bluetooth/l2cap.h
index c54b72a..aeadd7d 100644
--- a/include/net/bluetooth/l2cap.h
+++ b/include/net/bluetooth/l2cap.h
@@ -31,9 +31,9 @@
#define L2CAP_DEFAULT_FLUSH_TO 0xffff
#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 */
+#define L2CAP_DEFAULT_MAX_TX 3
+#define L2CAP_DEFAULT_RETRANS_TO 1000 /* 1 second */
+#define L2CAP_DEFAULT_MONITOR_TO 12000 /* 12 seconds */
#define L2CAP_DEFAULT_MAX_PDU_SIZE 672
#define L2CAP_CONN_TIMEOUT (40000) /* 40 seconds */
@@ -318,6 +318,7 @@ struct l2cap_pinfo {
__u8 req_seq;
__u8 expected_tx_seq;
__u8 unacked_frames;
+ __u8 retry_count;
__u8 num_to_ack;
__u16 sdu_len;
__u16 partial_sdu_len;
@@ -334,6 +335,8 @@ struct l2cap_pinfo {
__le16 sport;
+ struct timer_list retrans_timer;
+ struct timer_list monitor_timer;
struct sk_buff_head tx_queue;
struct l2cap_conn *conn;
struct sock *next_c;
@@ -353,6 +356,12 @@ struct l2cap_pinfo {
#define L2CAP_CONN_SAR_SDU 0x01
#define L2CAP_CONN_UNDER_REJ 0x02
+#define L2CAP_CONN_WAIT_ACK 0x04
+
+#define __mod_retrans_timer() mod_timer(&l2cap_pi(sk)->retrans_timer, \
+ jiffies + msecs_to_jiffies(L2CAP_DEFAULT_RETRANS_TO));
+#define __mod_monitor_timer() mod_timer(&l2cap_pi(sk)->monitor_timer, \
+ jiffies + msecs_to_jiffies(L2CAP_DEFAULT_MONITOR_TO));
static inline int l2cap_tx_window_full(struct sock *sk)
{
diff --git a/net/bluetooth/l2cap.c b/net/bluetooth/l2cap.c
index 918cc15..0ce89c3 100644
--- a/net/bluetooth/l2cap.c
+++ b/net/bluetooth/l2cap.c
@@ -1179,6 +1179,37 @@ static int l2cap_sock_getname(struct socket *sock, struct sockaddr *addr, int *l
return 0;
}
+static void l2cap_monitor_timeout(unsigned long arg)
+{
+ struct sock *sk = (void *) arg;
+ u16 control;
+
+ if (l2cap_pi(sk)->retry_count >= l2cap_pi(sk)->remote_max_tx) {
+ l2cap_send_disconn_req(l2cap_pi(sk)->conn, sk);
+ return;
+ }
+
+ l2cap_pi(sk)->retry_count++;
+ __mod_monitor_timer();
+
+ control = L2CAP_CTRL_POLL;
+ l2cap_send_sframe(l2cap_pi(sk), control);
+}
+
+static void l2cap_retrans_timeout(unsigned long arg)
+{
+ struct sock *sk = (void *) arg;
+ u16 control;
+
+ l2cap_pi(sk)->retry_count = 1;
+ __mod_monitor_timer();
+
+ l2cap_pi(sk)->conn_state |= L2CAP_CONN_WAIT_ACK;
+
+ control = L2CAP_CTRL_POLL;
+ l2cap_send_sframe(l2cap_pi(sk), control);
+}
+
static void l2cap_drop_acked_frames(struct sock *sk)
{
struct sk_buff *skb;
@@ -1193,6 +1224,9 @@ static void l2cap_drop_acked_frames(struct sock *sk)
l2cap_pi(sk)->unacked_frames--;
}
+ if (!l2cap_pi(sk)->unacked_frames)
+ del_timer(&l2cap_pi(sk)->retrans_timer);
+
return;
}
@@ -1219,10 +1253,21 @@ static int l2cap_ertm_send(struct sock *sk)
u16 *control;
int err;
+ if (pi->conn_state & L2CAP_CONN_WAIT_ACK)
+ return 0;
+
while ((skb = sk->sk_send_head) && (!l2cap_tx_window_full(sk))) {
tx_seq = pi->next_tx_seq;
tx_skb = skb_clone(skb, GFP_ATOMIC);
+ if (pi->remote_max_tx &&
+ bt_cb(skb)->retries == pi->remote_max_tx) {
+ l2cap_send_disconn_req(pi->conn, sk);
+ break;
+ }
+
+ bt_cb(skb)->retries++;
+
control = (u16 *)(skb->data + L2CAP_HDR_SIZE);
*control |= cpu_to_le16(
(pi->req_seq << L2CAP_CTRL_REQSEQ_SHIFT)
@@ -1234,6 +1279,8 @@ static int l2cap_ertm_send(struct sock *sk)
return err;
}
+ __mod_retrans_timer();
+
pi->next_tx_seq = (pi->next_tx_seq + 1) % 64;
bt_cb(skb)->tx_seq = tx_seq;
@@ -1370,6 +1417,8 @@ static struct sk_buff *l2cap_create_ertm_pdu(struct sock *sk, struct msghdr *msg
kfree_skb(skb);
return ERR_PTR(err);
}
+
+ bt_cb(skb)->retries = 0;
return skb;
}
@@ -2064,7 +2113,7 @@ done:
case L2CAP_MODE_ERTM:
rfc.mode = L2CAP_MODE_ERTM;
rfc.txwin_size = L2CAP_DEFAULT_TX_WINDOW;
- rfc.max_transmit = L2CAP_DEFAULT_MAX_RECEIVE;
+ rfc.max_transmit = L2CAP_DEFAULT_MAX_TX;
rfc.retrans_timeout = 0;
rfc.monitor_timeout = 0;
rfc.max_pdu_size = cpu_to_le16(L2CAP_DEFAULT_MAX_PDU_SIZE);
@@ -2559,6 +2608,12 @@ static inline int l2cap_config_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr
l2cap_pi(sk)->next_tx_seq = 0;
l2cap_pi(sk)->expected_ack_seq = 0;
l2cap_pi(sk)->unacked_frames = 0;
+
+ setup_timer(&l2cap_pi(sk)->retrans_timer,
+ l2cap_retrans_timeout, (unsigned long) sk);
+ setup_timer(&l2cap_pi(sk)->monitor_timer,
+ l2cap_monitor_timeout, (unsigned long) sk);
+
__skb_queue_head_init(TX_QUEUE(sk));
l2cap_chan_ready(sk);
goto unlock;
@@ -2668,6 +2723,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));
+ del_timer(&l2cap_pi(sk)->retrans_timer);
+ del_timer(&l2cap_pi(sk)->monitor_timer);
l2cap_chan_del(sk, ECONNRESET);
bh_unlock_sock(sk);
@@ -2692,6 +2749,8 @@ static inline int l2cap_disconnect_rsp(struct l2cap_conn *conn, struct l2cap_cmd
return 0;
skb_queue_purge(TX_QUEUE(sk));
+ del_timer(&l2cap_pi(sk)->retrans_timer);
+ del_timer(&l2cap_pi(sk)->monitor_timer);
l2cap_chan_del(sk, 0);
bh_unlock_sock(sk);
@@ -2996,9 +3055,23 @@ static inline int l2cap_data_channel_sframe(struct sock *sk, u16 rx_control, str
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);
+ if (rx_control & L2CAP_CTRL_POLL) {
+ u16 control = L2CAP_CTRL_FINAL;
+ l2cap_send_sframe(l2cap_pi(sk), control);
+ } else if (rx_control & L2CAP_CTRL_FINAL) {
+ if (pi->conn_state & L2CAP_CONN_WAIT_ACK)
+ break;
+
+ pi->conn_state &= !L2CAP_CONN_WAIT_ACK;
+ del_timer(&pi->monitor_timer);
+
+ if (pi->unacked_frames > 0)
+ __mod_retrans_timer();
+ } else {
+ pi->expected_ack_seq = __get_reqseq(rx_control);
+ l2cap_drop_acked_frames(sk);
+ l2cap_ertm_send(sk);
+ }
break;
case L2CAP_SUPER_REJECT:
--
1.6.3.3
When receiving an I-frame with unexpected txSeq, receiver side start the
recovery procedure by sending a REJ S-frame to the transmitter side. So
the transmitter can re-send the lost I-frame.
This patch just adds a basic support for retransmission, it doesn't
mean that ERTM now has full support to packet retransmission.
Signed-off-by: Gustavo F. Padovan <[email protected]>
---
include/net/bluetooth/l2cap.h | 1 +
net/bluetooth/l2cap.c | 57 +++++++++++++++++++++++++++++++----------
2 files changed, 44 insertions(+), 14 deletions(-)
diff --git a/include/net/bluetooth/l2cap.h b/include/net/bluetooth/l2cap.h
index 9189328..c54b72a 100644
--- a/include/net/bluetooth/l2cap.h
+++ b/include/net/bluetooth/l2cap.h
@@ -352,6 +352,7 @@ struct l2cap_pinfo {
#define L2CAP_CONF_MAX_CONF_RSP 2
#define L2CAP_CONN_SAR_SDU 0x01
+#define L2CAP_CONN_UNDER_REJ 0x02
static inline int l2cap_tx_window_full(struct sock *sk)
{
diff --git a/net/bluetooth/l2cap.c b/net/bluetooth/l2cap.c
index 16c448d..918cc15 100644
--- a/net/bluetooth/l2cap.c
+++ b/net/bluetooth/l2cap.c
@@ -2956,22 +2956,36 @@ static inline int l2cap_data_channel_iframe(struct sock *sk, u16 rx_control, str
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;
+ if (tx_seq == pi->expected_tx_seq) {
+ if (pi->conn_state & L2CAP_CONN_UNDER_REJ)
+ pi->conn_state &= ~L2CAP_CONN_UNDER_REJ;
- err = l2cap_sar_reassembly_sdu(sk, skb, rx_control, tx_seq);
- if (err < 0)
- return err;
+ err = l2cap_sar_reassembly_sdu(sk, skb, rx_control, tx_seq);
+ if (err < 0)
+ return err;
+
+ 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;
+ }
+ } 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->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_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);
+ goto send;
+ }
}
- return err;
+ return 0;
+
+send:
+ return l2cap_send_sframe(pi, tx_control);
}
static inline int l2cap_data_channel_sframe(struct sock *sk, u16 rx_control, struct sk_buff *skb)
@@ -2987,8 +3001,18 @@ static inline int l2cap_data_channel_sframe(struct sock *sk, u16 rx_control, str
l2cap_ertm_send(sk);
break;
- case L2CAP_SUPER_RCV_NOT_READY:
case L2CAP_SUPER_REJECT:
+ pi->expected_ack_seq = __get_reqseq(rx_control);
+ l2cap_drop_acked_frames(sk);
+
+ sk->sk_send_head = TX_QUEUE(sk)->next;
+ pi->next_tx_seq = pi->expected_ack_seq;
+
+ l2cap_ertm_send(sk);
+
+ break;
+
+ case L2CAP_SUPER_RCV_NOT_READY:
case L2CAP_SUPER_SELECT_REJECT:
break;
}
@@ -3035,6 +3059,11 @@ static inline int l2cap_data_channel(struct l2cap_conn *conn, u16 cid, struct sk
if (__is_sar_start(control))
len -= 2;
+ /*
+ * We can just drop the corrupted I-frame here.
+ * Receiver will miss it and start proper recovery
+ * procedures and ask retransmission.
+ */
if (len > L2CAP_DEFAULT_MAX_PDU_SIZE)
goto drop;
--
1.6.3.3
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 <[email protected]>
Signed-off-by: Gustavo F. Padovan <[email protected]>
---
include/net/bluetooth/l2cap.h | 10 ++-
net/bluetooth/l2cap.c | 169 +++++++++++++++++++++++++++++++++++++----
2 files changed, 162 insertions(+), 17 deletions(-)
diff --git a/include/net/bluetooth/l2cap.h b/include/net/bluetooth/l2cap.h
index 9bbfbe7..9189328 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 */
@@ -311,6 +311,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;
@@ -318,6 +319,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;
@@ -346,6 +351,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);
@@ -363,6 +370,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_start(ctrl) ((ctrl) & L2CAP_CTRL_SAR) == L2CAP_SDU_START
void l2cap_load(void);
diff --git a/net/bluetooth/l2cap.c b/net/bluetooth/l2cap.c
index a4e3cdc..16c448d 100644
--- a/net/bluetooth/l2cap.c
+++ b/net/bluetooth/l2cap.c
@@ -1339,7 +1339,7 @@ static struct sk_buff *l2cap_create_basic_pdu(struct sock *sk, struct msghdr *ms
return skb;
}
-static struct sk_buff *l2cap_create_ertm_pdu(struct sock *sk, struct msghdr *msg, size_t len, u16 control)
+static struct sk_buff *l2cap_create_ertm_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;
@@ -1348,6 +1348,9 @@ static struct sk_buff *l2cap_create_ertm_pdu(struct sock *sk, struct msghdr *msg
BT_DBG("sk %p len %d", sk, (int)len);
+ if (sdulen)
+ hlen +=2;
+
count = min_t(unsigned int, (conn->mtu - hlen), len);
skb = bt_skb_send_alloc(sk, count + hlen,
msg->msg_flags & MSG_DONTWAIT, &err);
@@ -1359,6 +1362,8 @@ static struct sk_buff *l2cap_create_ertm_pdu(struct sock *sk, struct msghdr *msg
lh->cid = cpu_to_le16(l2cap_pi(sk)->dcid);
lh->len = cpu_to_le16(len + (hlen - L2CAP_HDR_SIZE));
put_unaligned_le16(control, skb_put(skb, 2));
+ if (sdulen)
+ put_unaligned_le16(sdulen, skb_put(skb, 2));
err = l2cap_skbuff_fromiovec(sk, msg, len, count, skb);
if (unlikely(err < 0)) {
@@ -1368,6 +1373,54 @@ static struct sk_buff *l2cap_create_ertm_pdu(struct sock *sk, struct msghdr *msg
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_ertm_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 {
+ control |= L2CAP_SDU_END;
+ buflen = len;
+ }
+
+ skb = l2cap_create_ertm_pdu(sk, msg, buflen, control, 0);
+ 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;
@@ -1420,22 +1473,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_ertm_pdu(sk, msg, len, control);
+ skb = l2cap_create_ertm_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 (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)
@@ -2013,7 +2067,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);
@@ -2025,7 +2079,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);
@@ -2814,6 +2868,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 (err < 0)
+ goto drop;
+ 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;
+ 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 (err < 0)
+ kfree_skb(_skb);
+
+ 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);
@@ -2826,11 +2959,11 @@ static inline int l2cap_data_channel_iframe(struct sock *sk, u16 rx_control, str
if (tx_seq != pi->expected_tx_seq)
return -EINVAL;
- pi->expected_tx_seq = (pi->expected_tx_seq + 1) % 64;
- err = sock_queue_rcv_skb(sk, skb);
- if (err)
+ err = l2cap_sar_reassembly_sdu(sk, skb, rx_control, tx_seq);
+ if (err < 0)
return err;
+ 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_CTRL_FRAME_TYPE;
@@ -2866,7 +2999,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);
@@ -2897,8 +3030,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_start(control))
+ len -= 2;
+
+ if (len > L2CAP_DEFAULT_MAX_PDU_SIZE)
goto drop;
if (__is_iframe(control))
--
1.6.3.3
On Mon, Jul 27, 2009 at 7:05 AM, Marcel Holtmann<[email protected]> wrote=
:
> 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 <[email protected]>
>> ---
>> =A0include/net/bluetooth/l2cap.h | =A0 10 ++-
>> =A0net/bluetooth/l2cap.c =A0 =A0 =A0 =A0 | =A0180 ++++++++++++++++++++++=
++++++++++++++-----
>> =A02 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 @@
>> =A0#define L2CAP_DEFAULT_MAX_RECEIVE =A0 =A01
>> =A0#define L2CAP_DEFAULT_RETRANS_TO =A0 =A0 300 =A0 =A0/* 300 millisecon=
ds */
>> =A0#define L2CAP_DEFAULT_MONITOR_TO =A0 =A0 1000 =A0 /* 1 second */
>> -#define L2CAP_DEFAULT_MAX_RX_APDU =A0 =A00xfff7
>> +#define L2CAP_DEFAULT_MAX_PDU_SIZE =A0 672
>>
>> =A0#define L2CAP_CONN_TIMEOUT =A0 (40000) /* 40 seconds */
>> =A0#define L2CAP_INFO_TIMEOUT =A0 (4000) =A0/* =A04 seconds */
>> @@ -313,6 +313,7 @@ struct l2cap_pinfo {
>> =A0 =A0 =A0 __u8 =A0 =A0 =A0 =A0 =A0 =A0conf_req[64];
>> =A0 =A0 =A0 __u8 =A0 =A0 =A0 =A0 =A0 =A0conf_len;
>> =A0 =A0 =A0 __u8 =A0 =A0 =A0 =A0 =A0 =A0conf_state;
>> + =A0 =A0 __u8 =A0 =A0 =A0 =A0 =A0 =A0conn_state;
>>
>> =A0 =A0 =A0 __u8 =A0 =A0 =A0 =A0 =A0 =A0next_tx_seq;
>> =A0 =A0 =A0 __u8 =A0 =A0 =A0 =A0 =A0 =A0expected_ack_seq;
>> @@ -320,6 +321,10 @@ struct l2cap_pinfo {
>> =A0 =A0 =A0 __u8 =A0 =A0 =A0 =A0 =A0 =A0expected_tx_seq;
>> =A0 =A0 =A0 __u8 =A0 =A0 =A0 =A0 =A0 =A0unacked_frames;
>> =A0 =A0 =A0 __u8 =A0 =A0 =A0 =A0 =A0 =A0num_to_ack;
>> + =A0 =A0 __u16 =A0 =A0 =A0 =A0 =A0 sdu_len;
>> + =A0 =A0 __u16 =A0 =A0 =A0 =A0 =A0 partial_sdu_len;
>> + =A0 =A0 __u8 =A0 =A0 =A0 =A0 =A0 =A0start_txseq;
>> + =A0 =A0 struct sk_buff =A0*sdu;
>>
>> =A0 =A0 =A0 __u8 =A0 =A0 =A0 =A0 =A0 =A0ident;
>>
>> @@ -348,6 +353,8 @@ struct l2cap_pinfo {
>> =A0#define L2CAP_CONF_MAX_CONF_REQ 2
>> =A0#define L2CAP_CONF_MAX_CONF_RSP 2
>>
>> +#define L2CAP_CONN_SAR_SDU =A0 =A0 =A0 =A0 0x01
>> +
>> =A0static inline int l2cap_tx_window_full(struct sock *sk)
>> =A0{
>> =A0 =A0 =A0 struct l2cap_pinfo *pi =3D l2cap_pi(sk);
>> @@ -365,6 +372,7 @@ static inline int l2cap_tx_window_full(struct sock *=
sk)
>> =A0#define __get_reqseq(ctrl) ((ctrl) & L2CAP_CTRL_REQSEQ) >> 8
>> =A0#define __is_iframe(ctrl) !((ctrl) & L2CAP_CTRL_FRAME_TYPE)
>> =A0#define __is_sframe(ctrl) (ctrl) & L2CAP_CTRL_FRAME_TYPE
>> +#define __is_sar_sdu_start(ctrl) ((ctrl) & L2CAP_CTRL_SAR) =3D=3D L2CAP=
_SDU_START
>
> Using just __is_sar_start is enough here.
>
>> =A0void 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 so=
ck *sk, struct msghdr *msg, in
>> =A0 =A0 =A0 return sent;
>> =A0}
>>
>> -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)
>> =A0{
>> =A0 =A0 =A0 struct l2cap_conn *conn =3D l2cap_pi(sk)->conn;
>> =A0 =A0 =A0 struct sk_buff *skb;
>> @@ -1291,8 +1291,11 @@ static struct sk_buff *l2cap_create_pdu(struct so=
ck *sk, struct msghdr *msg, siz
>>
>> =A0 =A0 =A0 BT_DBG("sk %p len %d", sk, (int)len);
>>
>> - =A0 =A0 if (control)
>> + =A0 =A0 if (control) {
>> =A0 =A0 =A0 =A0 =A0 =A0 =A0 hlen +=3D 2;
>> + =A0 =A0 =A0 =A0 =A0 =A0 if (sdulen)
>> + =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 hlen +=3D2;
>> + =A0 =A0 }
>> =A0 =A0 =A0 else if (sk->sk_type =3D=3D SOCK_DGRAM)
>> =A0 =A0 =A0 =A0 =A0 =A0 =A0 hlen +=3D 2;
>
> This is just ugly. We might really wanna split PDU creation here in
> connection less, basic and ertm. Think about it.
Maybe it's better we split the PDU create, we also solve the problem
of control be a pointer.
We will duplicate some code, but things will look better.
>
>> @@ -1307,8 +1310,11 @@ static struct sk_buff *l2cap_create_pdu(struct so=
ck *sk, struct msghdr *msg, siz
>> =A0 =A0 =A0 lh->cid =3D cpu_to_le16(l2cap_pi(sk)->dcid);
>> =A0 =A0 =A0 lh->len =3D cpu_to_le16(len + (hlen - L2CAP_HDR_SIZE));
>>
>> - =A0 =A0 if (control)
>> + =A0 =A0 if (control) {
>> =A0 =A0 =A0 =A0 =A0 =A0 =A0 put_unaligned_le16(*control, skb_put(skb, 2)=
);
>> + =A0 =A0 =A0 =A0 =A0 =A0 if (sdulen)
>> + =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 put_unaligned_le16(sdulen, skb=
_put(skb, 2));
>> + =A0 =A0 }
>> =A0 =A0 =A0 else if (sk->sk_type =3D=3D SOCK_DGRAM)
>> =A0 =A0 =A0 =A0 =A0 =A0 =A0 put_unaligned_le16(l2cap_pi(sk)->psm, skb_pu=
t(skb, 2));
>>
>> @@ -1317,10 +1323,58 @@ static struct sk_buff *l2cap_create_pdu(struct s=
ock *sk, struct msghdr *msg, siz
>> =A0 =A0 =A0 =A0 =A0 =A0 =A0 kfree_skb(skb);
>> =A0 =A0 =A0 =A0 =A0 =A0 =A0 return ERR_PTR(err);
>> =A0 =A0 =A0 }
>> -
>> =A0 =A0 =A0 return skb;
>> =A0}
>>
>> +static inline int l2cap_sar_segment_sdu(struct sock *sk, struct msghdr =
*msg, size_t len)
>> +{
>> + =A0 =A0 struct l2cap_pinfo *pi =3D l2cap_pi(sk);
>> + =A0 =A0 struct sk_buff *skb;
>> + =A0 =A0 struct sk_buff_head sar_queue;
>> + =A0 =A0 u16 control;
>> + =A0 =A0 size_t size =3D 0;
>> +
>> + =A0 =A0 __skb_queue_head_init(&sar_queue);
>> + =A0 =A0 control |=3D L2CAP_SDU_START;
>> + =A0 =A0 skb =3D l2cap_create_pdu(sk, msg, pi->max_pdu_size, &control, =
len);
>> + =A0 =A0 if (IS_ERR(skb))
>> + =A0 =A0 =A0 =A0 =A0 =A0 return PTR_ERR(skb);
>> +
>> + =A0 =A0 __skb_queue_tail(&sar_queue, skb);
>> + =A0 =A0 len -=3D pi->max_pdu_size;
>> + =A0 =A0 size +=3Dpi->max_pdu_size;
>> + =A0 =A0 control =3D 0;
>> +
>> + =A0 =A0 while (len > 0) {
>> + =A0 =A0 =A0 =A0 =A0 =A0 size_t buflen;
>> +
>> + =A0 =A0 =A0 =A0 =A0 =A0 if (len > pi->max_pdu_size) {
>> + =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 control |=3D L2CAP_SDU_CONTINU=
E;
>> + =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 buflen =3D pi->max_pdu_size;
>> + =A0 =A0 =A0 =A0 =A0 =A0 }
>> + =A0 =A0 =A0 =A0 =A0 =A0 else {
>
> Coding style error. Must be =A0} else { all on the same line.
>
>> + =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 control |=3D L2CAP_SDU_END;
>> + =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 buflen =3D len;
>> + =A0 =A0 =A0 =A0 =A0 =A0 }
>> +
>> + =A0 =A0 =A0 =A0 =A0 =A0 skb =3D l2cap_create_pdu(sk, msg, buflen ,&con=
trol, 0);
>
> It is ,<space> and not <space>,
>
>> + =A0 =A0 =A0 =A0 =A0 =A0 if (IS_ERR(skb)) {
>> + =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 skb_queue_purge(&sar_queue);
>> + =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 return PTR_ERR(skb);
>> + =A0 =A0 =A0 =A0 =A0 =A0 }
>> +
>> + =A0 =A0 =A0 =A0 =A0 =A0 __skb_queue_tail(&sar_queue, skb);
>> + =A0 =A0 =A0 =A0 =A0 =A0 len -=3D buflen;
>> + =A0 =A0 =A0 =A0 =A0 =A0 size +=3D buflen;
>> + =A0 =A0 =A0 =A0 =A0 =A0 control =3D 0;
>> + =A0 =A0 }
>> + =A0 =A0 skb_queue_splice_tail(&sar_queue, TX_QUEUE(sk));
>> + =A0 =A0 if (sk->sk_send_head =3D=3D NULL)
>> + =A0 =A0 =A0 =A0 =A0 =A0 sk->sk_send_head =3D sar_queue.next;
>> +
>> + =A0 =A0 return size;
>> +}
>> +
>> =A0static int l2cap_sock_sendmsg(struct kiocb *iocb, struct socket *sock=
, struct msghdr *msg, size_t len)
>> =A0{
>> =A0 =A0 =A0 struct sock *sk =3D sock->sk;
>> @@ -1339,7 +1393,7 @@ static int l2cap_sock_sendmsg(struct kiocb *iocb, =
struct socket *sock, struct ms
>> =A0 =A0 =A0 =A0 =A0 =A0 =A0 return -EOPNOTSUPP;
>>
>> =A0 =A0 =A0 /* Check outgoing MTU */
>> - =A0 =A0 if ((sk->sk_type !=3D SOCK_RAW || pi->mode =3D=3D L2CAP_MODE_B=
ASIC)
>> + =A0 =A0 if ((sk->sk_type !=3D SOCK_RAW && pi->mode =3D=3D L2CAP_MODE_B=
ASIC)
>> =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 && len > pi->omtu)
>> =A0 =A0 =A0 =A0 =A0 =A0 =A0 return -EINVAL;
>
> I don't understand this one. If it is right, then also the extra
> brackets are pointless now.
It's wrong! Need to be:
+if ((sk->sk_type !=3D SOCK_SEQPACKET&& pi->mode =3D=3D L2CAP_MODE_BASIC)
So we will need another check for SOCK_RAW as I said on a comment on 1/4.
>
>>
>> @@ -1353,7 +1407,7 @@ static int l2cap_sock_sendmsg(struct kiocb *iocb, =
struct socket *sock, struct ms
>> =A0 =A0 =A0 switch (pi->mode) {
>> =A0 =A0 =A0 case L2CAP_MODE_BASIC:
>> =A0 =A0 =A0 =A0 =A0 =A0 =A0 /* Create a basic PDU */
>> - =A0 =A0 =A0 =A0 =A0 =A0 skb =3D l2cap_create_pdu(sk, msg, len, NULL);
>> + =A0 =A0 =A0 =A0 =A0 =A0 skb =3D l2cap_create_pdu(sk, msg, len, NULL, 0=
);
>> =A0 =A0 =A0 =A0 =A0 =A0 =A0 if (IS_ERR(skb)) {
>> =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 err =3D PTR_ERR(skb);
>> =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 goto done;
>> @@ -1366,22 +1420,23 @@ static int l2cap_sock_sendmsg(struct kiocb *iocb=
, struct socket *sock, struct ms
>>
>> =A0 =A0 =A0 case L2CAP_MODE_ERTM:
>> =A0 =A0 =A0 =A0 =A0 =A0 =A0 /* Entire SDU fits into one PDU */
>> - =A0 =A0 =A0 =A0 =A0 =A0 if (len <=3D pi->omtu) {
>> + =A0 =A0 =A0 =A0 =A0 =A0 if (len <=3D pi->max_pdu_size) {
>> =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 control =3D L2CAP_SDU_UNSEGM=
ENTED;
>> - =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 skb =3D l2cap_create_pdu(sk, m=
sg, len, &control);
>> + =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 skb =3D l2cap_create_pdu(sk, m=
sg, len, &control, 0);
>> =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 if (IS_ERR(skb)) {
>> =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 err =3D PTR_=
ERR(skb);
>> =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 goto done;
>> =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 }
>> + =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 __skb_queue_tail(TX_QUEUE(sk),=
skb);
>> + =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 if (sk->sk_send_head =3D=3D NU=
LL)
>> + =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 sk->sk_send_he=
ad =3D skb;
>> =A0 =A0 =A0 =A0 =A0 =A0 =A0 }
>> + =A0 =A0 =A0 =A0 =A0 =A0 /* Segment SDU into multiples PDUs */
>> =A0 =A0 =A0 =A0 =A0 =A0 =A0 else {
>> - =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 /* FIXME: Segmentation will be=
added later */
>> - =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 err =3D -EINVAL;
>> - =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 goto done;
>> + =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 err =3D l2cap_sar_segment_sdu(=
sk, msg, len);
>> + =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 if (unlikely(err < 0))
>> + =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 goto done;
>> =A0 =A0 =A0 =A0 =A0 =A0 =A0 }
>> - =A0 =A0 =A0 =A0 =A0 =A0 __skb_queue_tail(TX_QUEUE(sk), skb);
>> - =A0 =A0 =A0 =A0 =A0 =A0 if (sk->sk_send_head =3D=3D NULL)
>> - =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 sk->sk_send_head =3D skb;
>>
>> =A0 =A0 =A0 =A0 =A0 =A0 =A0 err =3D l2cap_ertm_send(sk);
>> =A0 =A0 =A0 =A0 =A0 =A0 =A0 if (!err)
>> @@ -1959,7 +2014,7 @@ done:
>> =A0 =A0 =A0 =A0 =A0 =A0 =A0 rfc.max_transmit =A0 =A0=3D L2CAP_DEFAULT_MA=
X_RECEIVE;
>> =A0 =A0 =A0 =A0 =A0 =A0 =A0 rfc.retrans_timeout =3D 0;
>> =A0 =A0 =A0 =A0 =A0 =A0 =A0 rfc.monitor_timeout =3D 0;
>> - =A0 =A0 =A0 =A0 =A0 =A0 rfc.max_pdu_size =A0 =A0=3D cpu_to_le16(L2CAP_=
DEFAULT_MAX_RX_APDU);
>> + =A0 =A0 =A0 =A0 =A0 =A0 rfc.max_pdu_size =A0 =A0=3D cpu_to_le16(L2CAP_=
DEFAULT_MAX_PDU_SIZE);
>>
>> =A0 =A0 =A0 =A0 =A0 =A0 =A0 l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC,
>> =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 =
=A0 sizeof(rfc), (unsigned long) &rfc);
>> @@ -1971,7 +2026,7 @@ done:
>> =A0 =A0 =A0 =A0 =A0 =A0 =A0 rfc.max_transmit =A0 =A0=3D 0;
>> =A0 =A0 =A0 =A0 =A0 =A0 =A0 rfc.retrans_timeout =3D 0;
>> =A0 =A0 =A0 =A0 =A0 =A0 =A0 rfc.monitor_timeout =3D 0;
>> - =A0 =A0 =A0 =A0 =A0 =A0 rfc.max_pdu_size =A0 =A0=3D cpu_to_le16(L2CAP_=
DEFAULT_MAX_RX_APDU);
>> + =A0 =A0 =A0 =A0 =A0 =A0 rfc.max_pdu_size =A0 =A0=3D cpu_to_le16(L2CAP_=
DEFAULT_MAX_PDU_SIZE);
>>
>> =A0 =A0 =A0 =A0 =A0 =A0 =A0 l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC,
>> =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 =
=A0 sizeof(rfc), (unsigned long) &rfc);
>> @@ -2760,6 +2815,85 @@ static inline void l2cap_sig_channel(struct l2cap=
_conn *conn, struct sk_buff *sk
>> =A0 =A0 =A0 kfree_skb(skb);
>> =A0}
>>
>> +static int l2cap_sar_reassembly_sdu(struct sock *sk, struct sk_buff *sk=
b, u16 control, u8 txseq)
>> +{
>> + =A0 =A0 struct l2cap_pinfo *pi =3D l2cap_pi(sk);
>> + =A0 =A0 struct sk_buff *_skb;
>> + =A0 =A0 int err =3D -EINVAL;
>> +
>> + =A0 =A0 switch (control & L2CAP_CTRL_SAR) {
>> + =A0 =A0 case L2CAP_SDU_UNSEGMENTED:
>> + =A0 =A0 =A0 =A0 =A0 =A0 if (pi->conn_state & L2CAP_CONN_SAR_SDU)
>> + =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 goto drop2;
>> +
>> + =A0 =A0 =A0 =A0 =A0 =A0 err =3D sock_queue_rcv_skb(sk, skb);
>> + =A0 =A0 =A0 =A0 =A0 =A0 if (unlikely(err < 0))
>> + =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 goto drop;
>
> Drop the unlikely here. Not helping much.
>
>> + =A0 =A0 =A0 =A0 =A0 =A0 break;
>> +
>> + =A0 =A0 case L2CAP_SDU_START:
>> + =A0 =A0 =A0 =A0 =A0 =A0 if (pi->conn_state & L2CAP_CONN_SAR_SDU)
>> + =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 goto drop2;
>> +
>> + =A0 =A0 =A0 =A0 =A0 =A0 pi->sdu_len =3D get_unaligned_le16(skb->data);
>> + =A0 =A0 =A0 =A0 =A0 =A0 skb_pull(skb, 2);
>> +
>> + =A0 =A0 =A0 =A0 =A0 =A0 pi->sdu =3D bt_skb_alloc(pi->sdu_len, GFP_ATOM=
IC);
>> + =A0 =A0 =A0 =A0 =A0 =A0 if (!pi->sdu)
>> + =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 goto drop;
>> +
>> + =A0 =A0 =A0 =A0 =A0 =A0 memcpy(skb_put(pi->sdu, skb->len), skb->data, =
skb->len);
>> +
>> + =A0 =A0 =A0 =A0 =A0 =A0 pi->conn_state |=3D L2CAP_CONN_SAR_SDU;
>> + =A0 =A0 =A0 =A0 =A0 =A0 pi->partial_sdu_len =3D skb->len;
>> + =A0 =A0 =A0 =A0 =A0 =A0 pi->start_txseq =3D txseq;
>> + =A0 =A0 =A0 =A0 =A0 =A0 kfree_skb(skb);
>> + =A0 =A0 =A0 =A0 =A0 =A0 break;
>> +
>> + =A0 =A0 case L2CAP_SDU_CONTINUE:
>> + =A0 =A0 =A0 =A0 =A0 =A0 if (!(pi->conn_state & L2CAP_CONN_SAR_SDU))
>> + =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 goto drop;
>> +
>> + =A0 =A0 =A0 =A0 =A0 =A0 memcpy(skb_put(pi->sdu, skb->len), skb->data, =
skb->len);
>> +
>> + =A0 =A0 =A0 =A0 =A0 =A0 pi->partial_sdu_len +=3D skb->len;
>> + =A0 =A0 =A0 =A0 =A0 =A0 if (pi->partial_sdu_len > pi->sdu_len)
>> + =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 goto drop2;
>> +
>> + =A0 =A0 =A0 =A0 =A0 =A0 kfree_skb(skb);
>> + =A0 =A0 =A0 =A0 =A0 =A0 break;
>> +
>> + =A0 =A0 case L2CAP_SDU_END:
>> + =A0 =A0 =A0 =A0 =A0 =A0 if (!(pi->conn_state & L2CAP_CONN_SAR_SDU))
>> + =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 goto drop;
>> +
>> + =A0 =A0 =A0 =A0 =A0 =A0 memcpy(skb_put(pi->sdu, skb->len), skb->data, =
skb->len);
>> +
>> + =A0 =A0 =A0 =A0 =A0 =A0 pi->conn_state &=3D !L2CAP_CONN_SAR_SDU;
>
> What is this. Removing a flag is &=3D ~ and not &=3D !.
>
>> + =A0 =A0 =A0 =A0 =A0 =A0 pi->partial_sdu_len +=3D skb->len;
>> +
>> + =A0 =A0 =A0 =A0 =A0 =A0 if (pi->partial_sdu_len !=3D pi->sdu_len)
>> + =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 goto drop2;
>> +
>> + =A0 =A0 =A0 =A0 =A0 =A0 _skb =3D skb_clone(pi->sdu, GFP_ATOMIC);
>> + =A0 =A0 =A0 =A0 =A0 =A0 err =3D sock_queue_rcv_skb(sk, _skb);
>> + =A0 =A0 =A0 =A0 =A0 =A0 if (unlikely(err < 0))
>> + =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 kfree_skb(_skb);
>
> Drop the Unlikely here, too.
>
>> + =A0 =A0 =A0 =A0 =A0 =A0 kfree_skb(pi->sdu);
>> + =A0 =A0 =A0 =A0 =A0 =A0 kfree_skb(skb);
>> + =A0 =A0 =A0 =A0 =A0 =A0 break;
>> + =A0 =A0 }
>> + =A0 =A0 return 0;
>> +
>> +drop2:
>> + =A0 =A0 kfree_skb(pi->sdu);
>> +
>> +drop:
>> + =A0 =A0 kfree_skb(skb);
>> + =A0 =A0 return err;
>> +}
>> +
>> =A0static inline int l2cap_data_channel_iframe(struct sock *sk, u16 rx_c=
ontrol, struct sk_buff *skb)
>> =A0{
>> =A0 =A0 =A0 struct l2cap_pinfo *pi =3D l2cap_pi(sk);
>> @@ -2772,11 +2906,11 @@ static inline int l2cap_data_channel_iframe(stru=
ct sock *sk, u16 rx_control, str
>> =A0 =A0 =A0 if (tx_seq !=3D pi->expected_tx_seq)
>> =A0 =A0 =A0 =A0 =A0 =A0 =A0 return -EINVAL;
>>
>> - =A0 =A0 L2CAP_SEQ_NUM_INC(pi->expected_tx_seq);
>> - =A0 =A0 err =3D sock_queue_rcv_skb(sk, skb);
>> - =A0 =A0 if (err)
>> + =A0 =A0 err =3D l2cap_sar_reassembly_sdu(sk, skb, rx_control, tx_seq);
>> + =A0 =A0 if (unlikely(err < 0))
>> =A0 =A0 =A0 =A0 =A0 =A0 =A0 return err;
>
> No unlikely please.
>
>>
>> + =A0 =A0 L2CAP_SEQ_NUM_INC(pi->expected_tx_seq);
>> =A0 =A0 =A0 L2CAP_NUM_TO_ACK_INC(pi->num_to_ack);
>> =A0 =A0 =A0 if (pi->num_to_ack =3D=3D L2CAP_DEFAULT_NUM_TO_ACK - 1) {
>> =A0 =A0 =A0 =A0 =A0 =A0 =A0 tx_control |=3D L2CAP_CTRL_FRAME_TYPE;
>> @@ -2815,7 +2949,7 @@ static inline int l2cap_data_channel_sframe(struct=
sock *sk, u16 rx_control, str
>> =A0static inline int l2cap_data_channel(struct l2cap_conn *conn, u16 cid=
, struct sk_buff *skb)
>> =A0{
>> =A0 =A0 =A0 struct sock *sk;
>> - =A0 =A0 u16 control;
>> + =A0 =A0 u16 control, len;
>> =A0 =A0 =A0 int err;
>>
>> =A0 =A0 =A0 sk =3D 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
>> =A0 =A0 =A0 case L2CAP_MODE_ERTM:
>> =A0 =A0 =A0 =A0 =A0 =A0 =A0 control =3D get_unaligned_le16(skb->data);
>> =A0 =A0 =A0 =A0 =A0 =A0 =A0 skb_pull(skb, 2);
>> + =A0 =A0 =A0 =A0 =A0 =A0 len =3D skb->len;
>>
>> - =A0 =A0 =A0 =A0 =A0 =A0 if (l2cap_pi(sk)->imtu < skb->len)
>> + =A0 =A0 =A0 =A0 =A0 =A0 if (__is_sar_sdu_start(control))
>> + =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 len -=3D 2;
>> +
>> + =A0 =A0 =A0 =A0 =A0 =A0 if (L2CAP_DEFAULT_MAX_PDU_SIZE < len)
>> =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 goto drop;
>>
>> =A0 =A0 =A0 =A0 =A0 =A0 =A0 if (__is_iframe(control))
>
> Regards
>
> Marcel
>
>
>
--=20
Gustavo F. Padovan
http://padovan.org
On Mon, Jul 27, 2009 at 6:59 AM, Marcel Holtmann<[email protected]> wrote=
:
> Hi Gustavo,
>
>> +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.
Because control could be 0 too. So I pass the control =3D NULL when we
don't want control field.
>
>> +{
>> + =A0 =A0 struct l2cap_conn *conn =3D l2cap_pi(sk)->conn;
>> + =A0 =A0 struct sk_buff *skb;
>> + =A0 =A0 int err, count, hlen =3D L2CAP_HDR_SIZE;
>> + =A0 =A0 struct l2cap_hdr *lh;
>> +
>> + =A0 =A0 BT_DBG("sk %p len %d", sk, (int)len);
>> +
>> + =A0 =A0 if (control)
>> + =A0 =A0 =A0 =A0 =A0 =A0 hlen +=3D 2;
>> + =A0 =A0 else if (sk->sk_type =3D=3D SOCK_DGRAM)
>> + =A0 =A0 =A0 =A0 =A0 =A0 hlen +=3D 2;
>
> An if (control || sk->sk_type =3D=3D ...) would do the same here. I know
> wanna make it the same as below, but it is confusing.
>
>> +
>> + =A0 =A0 count =3D min_t(unsigned int, (conn->mtu - hlen), len);
>> + =A0 =A0 skb =3D bt_skb_send_alloc(sk, count + hlen,
>> + =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 msg->msg_flags & MSG_DONTWAIT,=
&err);
>> + =A0 =A0 if (!skb)
>> + =A0 =A0 =A0 =A0 =A0 =A0 return ERR_PTR(-ENOMEM);
>> +
>> + =A0 =A0 /* Create L2CAP header */
>> + =A0 =A0 lh =3D (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE);
>> + =A0 =A0 lh->cid =3D cpu_to_le16(l2cap_pi(sk)->dcid);
>> + =A0 =A0 lh->len =3D cpu_to_le16(len + (hlen - L2CAP_HDR_SIZE));
>> +
>> + =A0 =A0 if (control)
>> + =A0 =A0 =A0 =A0 =A0 =A0 put_unaligned_le16(*control, skb_put(skb, 2));
>> + =A0 =A0 else if (sk->sk_type =3D=3D SOCK_DGRAM)
>> + =A0 =A0 =A0 =A0 =A0 =A0 put_unaligned_le16(l2cap_pi(sk)->psm, skb_put(=
skb, 2));
>> +
>> + =A0 =A0 err =3D l2cap_skbuff_fromiovec(sk, msg, len, count, skb);
>> + =A0 =A0 if (unlikely(err < 0)) {
>> + =A0 =A0 =A0 =A0 =A0 =A0 kfree_skb(skb);
>> + =A0 =A0 =A0 =A0 =A0 =A0 return ERR_PTR(err);
>> + =A0 =A0 }
>> +
>> + =A0 =A0 return skb;
>> =A0}
>>
>> =A0static int l2cap_sock_sendmsg(struct kiocb *iocb, struct socket *sock=
, struct msghdr *msg, size_t len)
>> =A0{
>> =A0 =A0 =A0 struct sock *sk =3D sock->sk;
>> - =A0 =A0 int err =3D 0;
>> + =A0 =A0 struct l2cap_pinfo *pi =3D l2cap_pi(sk);
>> + =A0 =A0 struct sk_buff *skb;
>> + =A0 =A0 u16 control;
>> + =A0 =A0 int err;
>>
>> =A0 =A0 =A0 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
>> =A0 =A0 =A0 =A0 =A0 =A0 =A0 return -EOPNOTSUPP;
>>
>> =A0 =A0 =A0 /* Check outgoing MTU */
>> - =A0 =A0 if (sk->sk_type !=3D SOCK_RAW && len > l2cap_pi(sk)->omtu)
>> + =A0 =A0 if ((sk->sk_type !=3D SOCK_RAW || pi->mode =3D=3D L2CAP_MODE_B=
ASIC)
>> + =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 && len > pi->omtu)
>> =A0 =A0 =A0 =A0 =A0 =A0 =A0 return -EINVAL;
>
> I am confused now. Wouldn't it be better to check for =3D=3D SOCK_SEQPACK=
ET
> and that it is basic mode and that the len in smaller than the MTU.
So do we need to add a new check? One for SOCK_RAW (the old) and for
the other you told?
>
>> =A0 =A0 =A0 lock_sock(sk);
>>
>> - =A0 =A0 if (sk->sk_state =3D=3D BT_CONNECTED)
>> - =A0 =A0 =A0 =A0 =A0 =A0 err =3D l2cap_do_send(sk, msg, len);
>> - =A0 =A0 else
>> + =A0 =A0 if (sk->sk_state !=3D BT_CONNECTED) {
>> =A0 =A0 =A0 =A0 =A0 =A0 =A0 err =3D -ENOTCONN;
>> + =A0 =A0 =A0 =A0 =A0 =A0 goto done;
>> + =A0 =A0 }
>> +
>> + =A0 =A0 switch (pi->mode) {
>> + =A0 =A0 case L2CAP_MODE_BASIC:
>> + =A0 =A0 =A0 =A0 =A0 =A0 /* Create a basic PDU */
>> + =A0 =A0 =A0 =A0 =A0 =A0 skb =3D l2cap_create_pdu(sk, msg, len, NULL);
>> + =A0 =A0 =A0 =A0 =A0 =A0 if (IS_ERR(skb)) {
>> + =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 err =3D PTR_ERR(skb);
>> + =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 goto done;
>> + =A0 =A0 =A0 =A0 =A0 =A0 }
>> +
>> + =A0 =A0 =A0 =A0 =A0 =A0 err =3D l2cap_do_send(sk, skb);
>> + =A0 =A0 =A0 =A0 =A0 =A0 if (!err)
>> + =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 err =3D len;
>> + =A0 =A0 =A0 =A0 =A0 =A0 break;
>> +
>> + =A0 =A0 case L2CAP_MODE_ERTM:
>> + =A0 =A0 =A0 =A0 =A0 =A0 /* Entire SDU fits into one PDU */
>> + =A0 =A0 =A0 =A0 =A0 =A0 if (len <=3D pi->omtu) {
>> + =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 control =3D L2CAP_SDU_UNSEGMEN=
TED;
>> + =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 skb =3D l2cap_create_pdu(sk, m=
sg, len, &control);
>> + =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 if (IS_ERR(skb)) {
>> + =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 err =3D PTR_ER=
R(skb);
>> + =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 goto done;
>> + =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 }
>> + =A0 =A0 =A0 =A0 =A0 =A0 }
>> + =A0 =A0 =A0 =A0 =A0 =A0 else {
>> + =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 /* FIXME: Segmentation will be=
added later */
>> + =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 err =3D -EINVAL;
>> + =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 goto done;
>> + =A0 =A0 =A0 =A0 =A0 =A0 }
>> + =A0 =A0 =A0 =A0 =A0 =A0 __skb_queue_tail(TX_QUEUE(sk), skb);
>> + =A0 =A0 =A0 =A0 =A0 =A0 if (sk->sk_send_head =3D=3D NULL)
>> + =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 sk->sk_send_head =3D skb;
>>
>> + =A0 =A0 =A0 =A0 =A0 =A0 err =3D l2cap_ertm_send(sk);
>> + =A0 =A0 =A0 =A0 =A0 =A0 if (!err)
>> + =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 err =3D len;
>> + =A0 =A0 =A0 =A0 =A0 =A0 break;
>> +
>> + =A0 =A0 default:
>> + =A0 =A0 =A0 =A0 =A0 =A0 BT_DBG("bad state %1.1x", pi->mode);
>> + =A0 =A0 =A0 =A0 =A0 =A0 err =3D -EINVAL;
>> + =A0 =A0 }
>> +
>> +done:
>> =A0 =A0 =A0 release_sock(sk);
>> =A0 =A0 =A0 return err;
>> =A0}
>> @@ -2302,6 +2448,10 @@ static inline int l2cap_config_req(struct l2cap_c=
onn *conn, struct l2cap_cmd_hdr
>>
>> =A0 =A0 =A0 if (l2cap_pi(sk)->conf_state & L2CAP_CONF_INPUT_DONE) {
>> =A0 =A0 =A0 =A0 =A0 =A0 =A0 sk->sk_state =3D BT_CONNECTED;
>> + =A0 =A0 =A0 =A0 =A0 =A0 l2cap_pi(sk)->next_tx_seq =3D 0;
>> + =A0 =A0 =A0 =A0 =A0 =A0 l2cap_pi(sk)->expected_ack_seq =3D 0;
>> + =A0 =A0 =A0 =A0 =A0 =A0 l2cap_pi(sk)->unacked_frames =3D 0;
>> + =A0 =A0 =A0 =A0 =A0 =A0 __skb_queue_head_init(TX_QUEUE(sk));
>> =A0 =A0 =A0 =A0 =A0 =A0 =A0 l2cap_chan_ready(sk);
>> =A0 =A0 =A0 =A0 =A0 =A0 =A0 goto unlock;
>> =A0 =A0 =A0 }
>> @@ -2376,6 +2526,9 @@ static inline int l2cap_config_rsp(struct l2cap_co=
nn *conn, struct l2cap_cmd_hdr
>>
>> =A0 =A0 =A0 if (l2cap_pi(sk)->conf_state & L2CAP_CONF_OUTPUT_DONE) {
>> =A0 =A0 =A0 =A0 =A0 =A0 =A0 sk->sk_state =3D BT_CONNECTED;
>> + =A0 =A0 =A0 =A0 =A0 =A0 l2cap_pi(sk)->expected_tx_seq =3D 0;
>> + =A0 =A0 =A0 =A0 =A0 =A0 l2cap_pi(sk)->num_to_ack =3D 0;
>> + =A0 =A0 =A0 =A0 =A0 =A0 __skb_queue_head_init(TX_QUEUE(sk));
>> =A0 =A0 =A0 =A0 =A0 =A0 =A0 l2cap_chan_ready(sk);
>> =A0 =A0 =A0 }
>>
>> @@ -2406,6 +2559,8 @@ static inline int l2cap_disconnect_req(struct l2ca=
p_conn *conn, struct l2cap_cmd
>>
>> =A0 =A0 =A0 sk->sk_shutdown =3D SHUTDOWN_MASK;
>>
>> + =A0 =A0 skb_queue_purge(TX_QUEUE(sk));
>> +
>> =A0 =A0 =A0 l2cap_chan_del(sk, ECONNRESET);
>> =A0 =A0 =A0 bh_unlock_sock(sk);
>>
>> @@ -2428,6 +2583,8 @@ static inline int l2cap_disconnect_rsp(struct l2ca=
p_conn *conn, struct l2cap_cmd
>> =A0 =A0 =A0 if (!sk)
>> =A0 =A0 =A0 =A0 =A0 =A0 =A0 return 0;
>>
>> + =A0 =A0 skb_queue_purge(TX_QUEUE(sk));
>> +
>> =A0 =A0 =A0 l2cap_chan_del(sk, 0);
>> =A0 =A0 =A0 bh_unlock_sock(sk);
>>
>> @@ -2603,9 +2760,63 @@ static inline void l2cap_sig_channel(struct l2cap=
_conn *conn, struct sk_buff *sk
>> =A0 =A0 =A0 kfree_skb(skb);
>> =A0}
>>
>> +static inline int l2cap_data_channel_iframe(struct sock *sk, u16 rx_con=
trol, struct sk_buff *skb)
>> +{
>> + =A0 =A0 struct l2cap_pinfo *pi =3D l2cap_pi(sk);
>> + =A0 =A0 u8 tx_seq =3D __get_txseq(rx_control);
>> + =A0 =A0 u16 tx_control =3D 0;
>> + =A0 =A0 int err;
>> +
>> + =A0 =A0 BT_DBG("sk %p rx_control 0x%4.4x len %d", sk, rx_control, skb-=
>len);
>> +
>> + =A0 =A0 if (tx_seq !=3D pi->expected_tx_seq)
>> + =A0 =A0 =A0 =A0 =A0 =A0 return -EINVAL;
>> +
>> + =A0 =A0 L2CAP_SEQ_NUM_INC(pi->expected_tx_seq);
>> + =A0 =A0 err =3D sock_queue_rcv_skb(sk, skb);
>> + =A0 =A0 if (err)
>> + =A0 =A0 =A0 =A0 =A0 =A0 return err;
>> +
>> + =A0 =A0 L2CAP_NUM_TO_ACK_INC(pi->num_to_ack);
>> + =A0 =A0 if (pi->num_to_ack =3D=3D L2CAP_DEFAULT_NUM_TO_ACK - 1) {
>> + =A0 =A0 =A0 =A0 =A0 =A0 tx_control |=3D L2CAP_CTRL_FRAME_TYPE;
>> + =A0 =A0 =A0 =A0 =A0 =A0 tx_control |=3D L2CAP_SUPER_RCV_READY;
>> + =A0 =A0 =A0 =A0 =A0 =A0 tx_control |=3D pi->expected_tx_seq << L2CAP_C=
TRL_REQSEQ_SHIFT;
>> +
>> + =A0 =A0 =A0 =A0 =A0 =A0 err =3D l2cap_send_sframe(pi, tx_control);
>> + =A0 =A0 =A0 =A0 =A0 =A0 if (unlikely(err))
>> + =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 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.
>
>> + =A0 =A0 }
>> + =A0 =A0 return 0;
>> +}
>> +
>> +static inline int l2cap_data_channel_sframe(struct sock *sk, u16 rx_con=
trol, struct sk_buff *skb)
>> +{
>> + =A0 =A0 struct l2cap_pinfo *pi =3D l2cap_pi(sk);
>> +
>> + =A0 =A0 BT_DBG("sk %p rx_control 0x%4.4x len %d", sk, rx_control, skb-=
>len);
>> +
>> + =A0 =A0 switch (rx_control & L2CAP_CTRL_SUPERVISE) {
>> + =A0 =A0 case L2CAP_SUPER_RCV_READY:
>> + =A0 =A0 =A0 =A0 =A0 =A0 pi->expected_ack_seq =3D __get_reqseq(rx_contr=
ol);
>> + =A0 =A0 =A0 =A0 =A0 =A0 l2cap_drop_acked_frames(sk);
>> + =A0 =A0 =A0 =A0 =A0 =A0 l2cap_ertm_send(sk);
>> + =A0 =A0 =A0 =A0 =A0 =A0 break;
>> +
>> + =A0 =A0 case L2CAP_SUPER_RCV_NOT_READY:
>> + =A0 =A0 case L2CAP_SUPER_REJECT:
>> + =A0 =A0 case L2CAP_SUPER_SELECT_REJECT:
>> + =A0 =A0 =A0 =A0 =A0 =A0 break;
>> + =A0 =A0 }
>> +
>> + =A0 =A0 return 0;
>> +}
>> +
>> =A0static inline int l2cap_data_channel(struct l2cap_conn *conn, u16 cid=
, struct sk_buff *skb)
>> =A0{
>> =A0 =A0 =A0 struct sock *sk;
>> + =A0 =A0 u16 control;
>> + =A0 =A0 int err;
>>
>> =A0 =A0 =A0 sk =3D l2cap_get_chan_by_scid(&conn->chan_list, cid);
>> =A0 =A0 =A0 if (!sk) {
>> @@ -2618,16 +2829,40 @@ static inline int l2cap_data_channel(struct l2ca=
p_conn *conn, u16 cid, struct sk
>> =A0 =A0 =A0 if (sk->sk_state !=3D BT_CONNECTED)
>> =A0 =A0 =A0 =A0 =A0 =A0 =A0 goto drop;
>>
>> - =A0 =A0 if (l2cap_pi(sk)->imtu < skb->len)
>> - =A0 =A0 =A0 =A0 =A0 =A0 goto drop;
>> + =A0 =A0 switch (l2cap_pi(sk)->mode) {
>> + =A0 =A0 case L2CAP_MODE_BASIC:
>> + =A0 =A0 =A0 =A0 =A0 =A0 /* If socket recv buffers overflows we drop da=
ta here
>> + =A0 =A0 =A0 =A0 =A0 =A0 =A0* which is *bad* because L2CAP has to be re=
liable.
>> + =A0 =A0 =A0 =A0 =A0 =A0 =A0* But we don't have any other choice. L2CAP=
doesn't
>> + =A0 =A0 =A0 =A0 =A0 =A0 =A0* provide flow control mechanism. */
>>
>> - =A0 =A0 /* If socket recv buffers overflows we drop data here
>> - =A0 =A0 =A0* which is *bad* because L2CAP has to be reliable.
>> - =A0 =A0 =A0* But we don't have any other choice. L2CAP doesn't
>> - =A0 =A0 =A0* provide flow control mechanism. */
>> + =A0 =A0 =A0 =A0 =A0 =A0 if (l2cap_pi(sk)->imtu < skb->len)
>> + =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 goto drop;
>>
>> - =A0 =A0 if (!sock_queue_rcv_skb(sk, skb))
>> - =A0 =A0 =A0 =A0 =A0 =A0 goto done;
>> + =A0 =A0 =A0 =A0 =A0 =A0 if (!sock_queue_rcv_skb(sk, skb))
>> + =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 goto done;
>> + =A0 =A0 =A0 =A0 =A0 =A0 break;
>> +
>> + =A0 =A0 case L2CAP_MODE_ERTM:
>> + =A0 =A0 =A0 =A0 =A0 =A0 control =3D get_unaligned_le16(skb->data);
>> + =A0 =A0 =A0 =A0 =A0 =A0 skb_pull(skb, 2);
>> +
>> + =A0 =A0 =A0 =A0 =A0 =A0 if (l2cap_pi(sk)->imtu < skb->len)
>> + =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 goto drop;
>> +
>> + =A0 =A0 =A0 =A0 =A0 =A0 if (__is_iframe(control))
>> + =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 err =3D l2cap_data_channel_ifr=
ame(sk, control, skb);
>> + =A0 =A0 =A0 =A0 =A0 =A0 else
>> + =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 err =3D l2cap_data_channel_sfr=
ame(sk, control, skb);
>> +
>> + =A0 =A0 =A0 =A0 =A0 =A0 if (!err)
>> + =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 =A0 goto done;
>> + =A0 =A0 =A0 =A0 =A0 =A0 break;
>> +
>> + =A0 =A0 default:
>> + =A0 =A0 =A0 =A0 =A0 =A0 BT_DBG("sk %p: bad mode 0x%2.2x", sk, l2cap_pi=
(sk)->mode);
>> + =A0 =A0 =A0 =A0 =A0 =A0 break;
>> + =A0 =A0 }
>>
>> =A0drop:
>> =A0 =A0 =A0 kfree_skb(skb);
>> @@ -2677,6 +2912,9 @@ static void l2cap_recv_frame(struct l2cap_conn *co=
nn, struct sk_buff *skb)
>> =A0 =A0 =A0 cid =3D __le16_to_cpu(lh->cid);
>> =A0 =A0 =A0 len =3D __le16_to_cpu(lh->len);
>>
>> + =A0 =A0 if (len !=3D skb->len)
>> + =A0 =A0 =A0 =A0 =A0 =A0 goto drop;
>> +
>> =A0 =A0 =A0 BT_DBG("len %d, cid 0x%4.4x", len, cid);
>>
>> =A0 =A0 =A0 switch (cid) {
>> @@ -2694,6 +2932,10 @@ static void l2cap_recv_frame(struct l2cap_conn *c=
onn, struct sk_buff *skb)
>> =A0 =A0 =A0 =A0 =A0 =A0 =A0 l2cap_data_channel(conn, cid, skb);
>> =A0 =A0 =A0 =A0 =A0 =A0 =A0 break;
>> =A0 =A0 =A0 }
>> + =A0 =A0 return;
>> +
>> +drop:
>> + =A0 =A0 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
>
>
>
--=20
Gustavo F. Padovan
http://padovan.org
Hi Gustavo,
> L2CAP uses rentransmission and monitor timers to inquiry the other side
> about unacked I-frames. After send each I-frame we (re)start the
> retransmission timer, if it expires, we start a monitor timer that send a
> S-frame with P bit set and wait for S-frame with F bit set.
> If monitor timer expires, it try again, at a maximum of
> L2CAP_DEFAULT_MAX_TX.
>
> Signed-off-by: Gustavo F. Padovan <[email protected]>
> ---
> include/net/bluetooth/bluetooth.h | 1 +
> include/net/bluetooth/l2cap.h | 15 +++++-
> net/bluetooth/l2cap.c | 83 +++++++++++++++++++++++++++++++++++--
> 3 files changed, 92 insertions(+), 7 deletions(-)
>
> diff --git a/include/net/bluetooth/bluetooth.h b/include/net/bluetooth/bluetooth.h
> index 65a5cf8..b8b9a84 100644
> --- a/include/net/bluetooth/bluetooth.h
> +++ b/include/net/bluetooth/bluetooth.h
> @@ -139,6 +139,7 @@ struct bt_skb_cb {
> __u8 pkt_type;
> __u8 incoming;
> __u8 tx_seq;
> + __u8 retries;
> };
> #define bt_cb(skb) ((struct bt_skb_cb *)((skb)->cb))
>
> diff --git a/include/net/bluetooth/l2cap.h b/include/net/bluetooth/l2cap.h
> index 653c313..02871df 100644
> --- a/include/net/bluetooth/l2cap.h
> +++ b/include/net/bluetooth/l2cap.h
> @@ -31,9 +31,9 @@
> #define L2CAP_DEFAULT_FLUSH_TO 0xffff
> #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 */
> +#define L2CAP_DEFAULT_MAX_TX 3
> +#define L2CAP_DEFAULT_RETRANS_TO 1000 /* 1 second */
> +#define L2CAP_DEFAULT_MONITOR_TO 12000 /* 12 seconds */
> #define L2CAP_DEFAULT_MAX_PDU_SIZE 672
>
> #define L2CAP_CONN_TIMEOUT (40000) /* 40 seconds */
> @@ -320,6 +320,7 @@ struct l2cap_pinfo {
> __u8 req_seq;
> __u8 expected_tx_seq;
> __u8 unacked_frames;
> + __u8 retry_count;
> __u8 num_to_ack;
> __u16 sdu_len;
> __u16 partial_sdu_len;
> @@ -336,6 +337,8 @@ struct l2cap_pinfo {
>
> __le16 sport;
>
> + struct timer_list retrans_timer;
> + struct timer_list monitor_timer;
> struct sk_buff_head tx_queue;
> struct l2cap_conn *conn;
> struct sock *next_c;
> @@ -355,6 +358,12 @@ struct l2cap_pinfo {
>
> #define L2CAP_CONN_SAR_SDU 0x01
> #define L2CAP_CONN_UNDER_REJ 0x02
> +#define L2CAP_CONN_WAIT_ACK 0x04
> +
> +#define __mod_retrans_timer() mod_timer(&l2cap_pi(sk)->retrans_timer, \
> + jiffies + msecs_to_jiffies(L2CAP_DEFAULT_RETRANS_TO));
> +#define __mod_monitor_timer() mod_timer(&l2cap_pi(sk)->monitor_timer, \
> + jiffies + msecs_to_jiffies(L2CAP_DEFAULT_MONITOR_TO));
>
> static inline int l2cap_tx_window_full(struct sock *sk)
> {
> diff --git a/net/bluetooth/l2cap.c b/net/bluetooth/l2cap.c
> index 938d6db..bcfa9b9 100644
> --- a/net/bluetooth/l2cap.c
> +++ b/net/bluetooth/l2cap.c
> @@ -1179,6 +1179,37 @@ static int l2cap_sock_getname(struct socket *sock, struct sockaddr *addr, int *l
> return 0;
> }
>
> +static void l2cap_monitor_timeout(unsigned long arg)
> +{
> + struct sock *sk = (void *) arg;
> + u16 control;
> +
> + if (l2cap_pi(sk)->retry_count >= l2cap_pi(sk)->remote_max_tx) {
> + l2cap_send_disconn_req(l2cap_pi(sk)->conn, sk);
> + return;
> + }
> +
> + l2cap_pi(sk)->retry_count++;
> + __mod_monitor_timer();
> +
> + control = L2CAP_CTRL_POLL;
> + l2cap_send_sframe(l2cap_pi(sk), control);
> +}
> +
> +static void l2cap_retrans_timeout(unsigned long arg)
> +{
> + struct sock *sk = (void *) arg;
> + u16 control;
> +
> + l2cap_pi(sk)->retry_count = 1;
> + __mod_monitor_timer();
> +
> + l2cap_pi(sk)->conn_state |= L2CAP_CONN_WAIT_ACK;
> +
> + control = L2CAP_CTRL_POLL;
> + l2cap_send_sframe(l2cap_pi(sk), control);
> +}
> +
> static void l2cap_drop_acked_frames(struct sock *sk)
> {
> struct sk_buff *skb;
> @@ -1193,6 +1224,9 @@ static void l2cap_drop_acked_frames(struct sock *sk)
> l2cap_pi(sk)->unacked_frames--;
> }
>
> + if (!l2cap_pi(sk)->unacked_frames)
> + del_timer(&l2cap_pi(sk)->retrans_timer);
> +
> return;
> }
>
> @@ -1219,10 +1253,21 @@ static int l2cap_ertm_send(struct sock *sk)
> u16 *control;
> int err;
>
> + if (pi->conn_state & L2CAP_CONN_WAIT_ACK)
> + return 0;
> +
> while ((skb = sk->sk_send_head) && (!l2cap_tx_window_full(sk))) {
> tx_seq = pi->next_tx_seq;
> tx_skb = skb_clone(skb, GFP_ATOMIC);
>
> + if (pi->remote_max_tx &&
> + bt_cb(skb)->retries == pi->remote_max_tx) {
> + l2cap_send_disconn_req(pi->conn, sk);
> + break;
> + }
> +
> + bt_cb(skb)->retries++;
> +
> control = (u16 *)(skb->data + L2CAP_HDR_SIZE);
> *control |= cpu_to_le16(
> (pi->req_seq << L2CAP_CTRL_REQSEQ_SHIFT)
> @@ -1234,6 +1279,8 @@ static int l2cap_ertm_send(struct sock *sk)
> return err;
> }
>
> + __mod_retrans_timer();
> +
> L2CAP_SEQ_NUM_INC(pi->next_tx_seq);
> bt_cb(skb)->tx_seq = tx_seq;
>
> @@ -1323,6 +1370,8 @@ static struct sk_buff *l2cap_create_pdu(struct sock *sk, struct msghdr *msg, siz
> kfree_skb(skb);
> return ERR_PTR(err);
> }
> +
> + bt_cb(skb)->retries = 0;
> return skb;
> }
>
> @@ -2011,7 +2060,7 @@ done:
> case L2CAP_MODE_ERTM:
> rfc.mode = L2CAP_MODE_ERTM;
> rfc.txwin_size = L2CAP_DEFAULT_TX_WINDOW;
> - rfc.max_transmit = L2CAP_DEFAULT_MAX_RECEIVE;
> + rfc.max_transmit = L2CAP_DEFAULT_MAX_TX;
> rfc.retrans_timeout = 0;
> rfc.monitor_timeout = 0;
> rfc.max_pdu_size = cpu_to_le16(L2CAP_DEFAULT_MAX_PDU_SIZE);
> @@ -2506,6 +2555,12 @@ static inline int l2cap_config_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr
> l2cap_pi(sk)->next_tx_seq = 0;
> l2cap_pi(sk)->expected_ack_seq = 0;
> l2cap_pi(sk)->unacked_frames = 0;
> +
> + setup_timer(&l2cap_pi(sk)->retrans_timer,
> + l2cap_retrans_timeout, (unsigned long) sk);
> + setup_timer(&l2cap_pi(sk)->monitor_timer,
> + l2cap_monitor_timeout, (unsigned long) sk);
> +
> __skb_queue_head_init(TX_QUEUE(sk));
> l2cap_chan_ready(sk);
> goto unlock;
> @@ -2615,6 +2670,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));
> + del_timer(&l2cap_pi(sk)->retrans_timer);
> + del_timer(&l2cap_pi(sk)->monitor_timer);
>
> l2cap_chan_del(sk, ECONNRESET);
> bh_unlock_sock(sk);
> @@ -2639,6 +2696,8 @@ static inline int l2cap_disconnect_rsp(struct l2cap_conn *conn, struct l2cap_cmd
> return 0;
>
> skb_queue_purge(TX_QUEUE(sk));
> + del_timer(&l2cap_pi(sk)->retrans_timer);
> + del_timer(&l2cap_pi(sk)->monitor_timer);
>
> l2cap_chan_del(sk, 0);
> bh_unlock_sock(sk);
> @@ -2944,9 +3003,25 @@ static inline int l2cap_data_channel_sframe(struct sock *sk, u16 rx_control, str
>
> 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);
> + if (rx_control & L2CAP_CTRL_POLL) {
> + u16 control = L2CAP_CTRL_FINAL;
> + l2cap_send_sframe(l2cap_pi(sk), control);
> + }
> + else if (rx_control & L2CAP_CTRL_FINAL) {
Coding style is } else if.
> + if (pi->conn_state & L2CAP_CONN_WAIT_ACK)
> + break;
> +
> + pi->conn_state &= !L2CAP_CONN_WAIT_ACK;
> + del_timer(&pi->monitor_timer);
> +
> + if (pi->unacked_frames > 0)
> + __mod_retrans_timer();
> + }
> + else {
Same here.
> + pi->expected_ack_seq = __get_reqseq(rx_control);
> + l2cap_drop_acked_frames(sk);
> + l2cap_ertm_send(sk);
> + }
> break;
>
> case L2CAP_SUPER_REJECT:
Regards
Marcel
Hi Gustavo,
> When receiving an I-frame with unexpected txSeq, receiver side start the
> recovery procedure by send a REJ S-frame to the transmitter side. So the
> transmitter can re-send the lost I-frame.
> This patch just adds a basic support for retransmission, it doesn't
> mean that ERTM now has full support to packet retransmission.
>
> Signed-off-by: Gustavo F. Padovan <[email protected]>
> ---
> include/net/bluetooth/l2cap.h | 1 +
> net/bluetooth/l2cap.c | 57 ++++++++++++++++++++++++++++++-----------
> 2 files changed, 43 insertions(+), 15 deletions(-)
>
> diff --git a/include/net/bluetooth/l2cap.h b/include/net/bluetooth/l2cap.h
> index 233a2fa..653c313 100644
> --- a/include/net/bluetooth/l2cap.h
> +++ b/include/net/bluetooth/l2cap.h
> @@ -354,6 +354,7 @@ struct l2cap_pinfo {
> #define L2CAP_CONF_MAX_CONF_RSP 2
>
> #define L2CAP_CONN_SAR_SDU 0x01
> +#define L2CAP_CONN_UNDER_REJ 0x02
>
> static inline int l2cap_tx_window_full(struct sock *sk)
> {
> diff --git a/net/bluetooth/l2cap.c b/net/bluetooth/l2cap.c
> index dcde60b..938d6db 100644
> --- a/net/bluetooth/l2cap.c
> +++ b/net/bluetooth/l2cap.c
> @@ -2903,25 +2903,37 @@ static inline int l2cap_data_channel_iframe(struct sock *sk, u16 rx_control, str
>
> 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;
> + if (tx_seq == pi->expected_tx_seq) {
> + if (pi->conn_state & L2CAP_CONN_UNDER_REJ)
> + pi->conn_state &= !L2CAP_CONN_UNDER_REJ;
This is &= ~ again.
>
> - err = l2cap_sar_reassembly_sdu(sk, skb, rx_control, tx_seq);
> - if (unlikely(err < 0))
> - return 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;
> - tx_control |= L2CAP_SUPER_RCV_READY;
> - tx_control |= pi->expected_tx_seq << L2CAP_CTRL_REQSEQ_SHIFT;
> + 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_SUPER_RCV_READY;
> + tx_control |= pi->expected_tx_seq << L2CAP_CTRL_REQSEQ_SHIFT;
> + goto send;
> + }
> + }
> + 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;
>
> - err = l2cap_send_sframe(pi, tx_control);
> - if (unlikely(err))
> - return err;
> + goto send;
> + }
> }
> return 0;
> +
> +send:
> + return l2cap_send_sframe(pi, tx_control);
> }
>
> static inline int l2cap_data_channel_sframe(struct sock *sk, u16 rx_control, struct sk_buff *skb)
> @@ -2937,8 +2949,18 @@ static inline int l2cap_data_channel_sframe(struct sock *sk, u16 rx_control, str
> l2cap_ertm_send(sk);
> break;
>
> - case L2CAP_SUPER_RCV_NOT_READY:
> case L2CAP_SUPER_REJECT:
> + pi->expected_ack_seq = __get_reqseq(rx_control);
> + l2cap_drop_acked_frames(sk);
> +
> + sk->sk_send_head = TX_QUEUE(sk)->next;
> + pi->next_tx_seq = pi->expected_ack_seq;
> +
> + l2cap_ertm_send(sk);
> +
> + break;
> +
> + case L2CAP_SUPER_RCV_NOT_READY:
> case L2CAP_SUPER_SELECT_REJECT:
> break;
> }
> @@ -2985,6 +3007,11 @@ static inline int l2cap_data_channel(struct l2cap_conn *conn, u16 cid, struct sk
> if (__is_sar_sdu_start(control))
> len -= 2;
>
> + /*
> + * We can just drop the corrupted I-frames here.
> + * Receiver will miss it and start proper recovery
> + * procedures and ask retransmission.
> + */
> if (L2CAP_DEFAULT_MAX_PDU_SIZE < len)
> goto drop;
>
Regards
Marcel
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 <[email protected]>
>
> Signed-off-by: Gustavo F. Padovan <[email protected]>
> ---
> 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
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 <[email protected]>
>
> Signed-off-by: Gustavo F. Padovan <[email protected]>
> ---
> 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