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.
Based on a patch from Nathan Holstein <nathan@xxxxxxxxxxxxxxxxxxx>
Signed-off-by: Gustavo F. Padovan <gustavo@xxxxxxxxxxxxxxxxx>
---
include/net/bluetooth/bluetooth.h | 3 +-
include/net/bluetooth/l2cap.h | 123 ++++++++++++-
net/bluetooth/l2cap.c | 368 +++++++++++++++++++++++++++++++------
3 files changed, 440 insertions(+), 54 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..521852f 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_CONTROL_SAR 0xC000
+#define L2CAP_CONTROL_REQSEQ 0x3F00
+#define L2CAP_CONTROL_TXSEQ 0x007E
+#define L2CAP_CONTROL_RETRANS 0x0080
+#define L2CAP_CONTROL_FINAL 0x0080
+#define L2CAP_CONTROL_POLL 0x0010
+#define L2CAP_CONTROL_SUPERVISE 0x000C
+#define L2CAP_CONTROL_FRAME_TYPE 0x0001 /* I- or S-Frame */
+
+#define L2CAP_SEQ_NUM_INC(seq) ((seq) = (seq + 1) % 64)
+#define L2CAP_CONTROL_TXSEQ_SHIFT 1
+#define L2CAP_CONTROL_REQSEQ_SHIFT 8
+#define L2CAP_NUM_TO_ACK_INC(seq) ((seq) = (seq + 1) % L2CAP_DEFAULT_NUM_TO_ACK)
+
+/* 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_SAR_UNSEGMENTED 0x0000
+#define L2CAP_SAR_SDU_START 0x4000
+#define L2CAP_SAR_SDU_END 0x8000
+#define L2CAP_SAR_SDU_CONTINUE 0xC000
+
/* L2CAP structures */
struct l2cap_hdr {
__le16 len;
@@ -284,6 +312,13 @@ struct l2cap_pinfo {
__u8 conf_req[64];
__u8 conf_len;
__u8 conf_state;
+ __u8 conn_state;
+
+ __u8 next_tx_seq;
+ __u8 expected_ack_seq;
+ __u8 req_seq;
+ __u8 expected_tx_seq;
+ __u8 num_to_ack;
__u8 ident;
@@ -295,6 +330,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 +347,91 @@ struct l2cap_pinfo {
#define L2CAP_CONF_MAX_CONF_REQ 2
#define L2CAP_CONF_MAX_CONF_RSP 2
+#define L2CAP_CONN_TRANSMITTER 0x01
+
+static inline void l2cap_init_send_head(struct sock *sk)
+{
+ sk->sk_send_head = NULL;
+}
+
+static inline struct sk_buff *l2cap_send_head(struct sock *sk)
+{
+ return sk->sk_send_head;
+}
+
+static inline void __l2cap_add_tx_queue_tail(struct sock *sk, struct sk_buff *skb)
+{
+ __skb_queue_tail(&l2cap_pi(sk)->tx_queue, skb);
+}
+
+static inline void l2cap_tx_queue_head_init(struct sock *sk)
+{
+ __skb_queue_head_init(&l2cap_pi(sk)->tx_queue);
+}
+
+static inline void l2cap_add_tx_queue_tail(struct sock *sk, struct sk_buff *skb)
+{
+ __l2cap_add_tx_queue_tail(sk, skb);
+
+ /* Queue it, remembering where we must start sending. */
+ if (sk->sk_send_head == NULL)
+ sk->sk_send_head = skb;
+}
+
+static inline struct sk_buff *l2cap_write_queue_next(struct sock *sk, struct sk_buff *skb)
+{
+ return skb_queue_next(&l2cap_pi(sk)->tx_queue, skb);
+}
+
+static inline bool l2cap_skb_is_last(const struct sock *sk,
+ const struct sk_buff *skb)
+{
+ return skb_queue_is_last(&l2cap_pi(sk)->tx_queue, skb);
+}
+
+static inline struct sk_buff *l2cap_skb_peek(struct sock *sk)
+{
+ return skb_peek(&l2cap_pi(sk)->tx_queue);
+}
+
+static inline struct sk_buff *l2cap_skb_dequeue(struct sock *sk)
+{
+ return skb_dequeue(&l2cap_pi(sk)->tx_queue);
+}
+
+static inline void l2cap_advance_send_head(struct sock *sk, struct sk_buff *skb)
+{
+ if (l2cap_skb_is_last(sk, skb))
+ sk->sk_send_head = NULL;
+ else
+ sk->sk_send_head = l2cap_write_queue_next(sk, skb);
+}
+
+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 l2cap_control_txseq(control) (control & L2CAP_CONTROL_TXSEQ) \
+ >> L2CAP_CONTROL_TXSEQ_SHIFT
+
+#define l2cap_control_reqseq(control) (control & L2CAP_CONTROL_REQSEQ) \
+ >> L2CAP_CONTROL_REQSEQ_SHIFT
+
+#define l2cap_txseq_to_reqseq(control) (control & L2CAP_CONTROL_TXSEQ) \
+ << (L2CAP_CONTROL_REQSEQ_SHIFT - L2CAP_CONTROL_TXSEQ_SHIFT)
+
+#define l2cap_is_iframe(control) !(control & L2CAP_CONTROL_FRAME_TYPE)
+
+#define l2cap_is_sframe(control) (control & L2CAP_CONTROL_FRAME_TYPE)
void l2cap_load(void);
diff --git a/net/bluetooth/l2cap.c b/net/bluetooth/l2cap.c
index 18b3c62..8106567 100644
--- a/net/bluetooth/l2cap.c
+++ b/net/bluetooth/l2cap.c
@@ -69,6 +69,7 @@ static void l2cap_sock_kill(struct sock *sk);
static struct sk_buff *l2cap_build_cmd(struct l2cap_conn *conn,
u8 code, u8 ident, u16 dlen, void *data);
+static struct sk_buff *l2cap_build_sframe(struct l2cap_pinfo *pi, u16 control);
/* ---- L2CAP timers ---- */
static void l2cap_sock_timeout(unsigned long arg)
@@ -333,6 +334,18 @@ 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 = l2cap_build_sframe(pi, control);
+
+ BT_DBG("control 0x%2.2x", control);
+
+ if (!skb)
+ return -ENOMEM;
+
+ 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 +1168,79 @@ 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 = l2cap_skb_peek(sk))) {
- /* First fragment (with L2CAP header) */
- if (sk->sk_type == SOCK_DGRAM)
- hlen = L2CAP_HDR_SIZE + 2;
- else
- hlen = L2CAP_HDR_SIZE;
+ if (bt_cb(skb)->tx_seq == l2cap_pi(sk)->expected_ack_seq)
+ break;
- count = min_t(unsigned int, (conn->mtu - hlen), len);
+ skb = l2cap_skb_dequeue(sk);
+ kfree_skb(skb);
+ }
- 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);
+
+ 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 = l2cap_send_head(sk)) && (!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_CONTROL_REQSEQ_SHIFT)
+ | (tx_seq << L2CAP_CONTROL_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;
+
+ l2cap_advance_send_head(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 +1253,68 @@ 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)
+{
+ struct l2cap_conn *conn = l2cap_pi(sk)->conn;
+ struct sk_buff *skb;
+ int err, count, hlen = L2CAP_HDR_SIZE;
+ struct l2cap_hdr *lh;
+
+ if (control)
+ hlen += 2;
+
+ BT_DBG("sk %p len %d", sk, (int)len);
+
+ /* First fragment (with L2CAP header) */
+ if (sk->sk_type == SOCK_DGRAM)
+ 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);
+ 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, (__le16 *) skb_put(skb, 2));
+
+ if (sk->sk_type == SOCK_DGRAM)
+ put_unaligned_le16(l2cap_pi(sk)->psm, (__le16 *) 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 +1326,64 @@ 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;
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_SAR_UNSEGMENTED;
+ skb = l2cap_create_pdu(sk, msg, len, &control);
+ if (IS_ERR(skb)) {
+ err = PTR_ERR(skb);
+ goto done;
+ }
+ }
+ else {
+ /* Segmentation will be added later */
+ err = -EINVAL;
+ goto done;
+ }
+ if (err < 0)
+ goto done;
+
+ l2cap_add_tx_queue_tail(sk, 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;
}
@@ -1684,6 +1820,30 @@ fail:
return NULL;
}
+static struct sk_buff *l2cap_build_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_CONTROL_FRAME_TYPE;
+
+ skb = bt_skb_alloc(count, GFP_ATOMIC);
+ if (!skb)
+ return NULL;
+
+ 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, (__le16 *) skb_put(skb, 2));
+
+ return skb;
+}
+
static inline int l2cap_get_conf_opt(void **ptr, int *type, int *olen, unsigned long *val)
{
struct l2cap_conf_opt *opt = *ptr;
@@ -2302,6 +2462,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)->conn_state |= L2CAP_CONN_TRANSMITTER;
+ l2cap_tx_queue_head_init(sk);
l2cap_chan_ready(sk);
goto unlock;
}
@@ -2376,6 +2540,8 @@ 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;
l2cap_chan_ready(sk);
}
@@ -2406,6 +2572,8 @@ static inline int l2cap_disconnect_req(struct l2cap_conn *conn, struct l2cap_cmd
sk->sk_shutdown = SHUTDOWN_MASK;
+ if (l2cap_pi(sk)->conn_state & L2CAP_CONN_TRANSMITTER)
+ skb_queue_purge(&l2cap_pi(sk)->tx_queue);
l2cap_chan_del(sk, ECONNRESET);
bh_unlock_sock(sk);
@@ -2428,6 +2596,8 @@ static inline int l2cap_disconnect_rsp(struct l2cap_conn *conn, struct l2cap_cmd
if (!sk)
return 0;
+ if (l2cap_pi(sk)->conn_state & L2CAP_CONN_TRANSMITTER)
+ skb_queue_purge(&l2cap_pi(sk)->tx_queue);
l2cap_chan_del(sk, 0);
bh_unlock_sock(sk);
@@ -2603,7 +2773,89 @@ static inline void l2cap_sig_channel(struct l2cap_conn *conn, struct sk_buff *sk
kfree_skb(skb);
}
-static inline int l2cap_data_channel(struct l2cap_conn *conn, u16 cid, struct sk_buff *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 = l2cap_control_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_CONTROL_FRAME_TYPE;
+ tx_control |= L2CAP_SUPER_RCV_READY;
+ tx_control |= pi->expected_tx_seq << L2CAP_CONTROL_REQSEQ_SHIFT;
+
+ if (l2cap_send_sframe(pi, tx_control))
+ return -EINVAL;
+ }
+
+ 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);
+ u16 tx_control;
+
+ BT_DBG("sk %p rx_control 0x%4.4x len %d", sk, rx_control, skb->len);
+
+ if (rx_control & L2CAP_CONTROL_POLL) {
+ tx_control = L2CAP_SUPER_RCV_READY | L2CAP_CONTROL_FINAL;
+ return l2cap_send_sframe(pi, tx_control);
+ }
+
+ switch (rx_control & L2CAP_CONTROL_SUPERVISE) {
+ case L2CAP_SUPER_RCV_READY:
+ pi->expected_ack_seq = l2cap_control_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_ertm_data_channel(struct sock *sk, struct l2cap_hdr *lh, struct sk_buff *skb)
+{
+ u16 control, len = skb->len;
+
+ BT_DBG("sk %p skb %p", sk, skb);
+
+ control = get_unaligned((__le16 *) skb->data);
+ skb_pull(skb, 2);
+ len -=2;
+ BT_DBG("control 0x%4.4x", control);
+
+ if (l2cap_pi(sk)->imtu < skb->len)
+ goto drop;
+
+ if (l2cap_is_iframe(control))
+ return l2cap_data_channel_iframe(sk, control, skb);
+ else
+ return l2cap_data_channel_sframe(sk, control, skb);
+
+drop:
+ kfree_skb(skb);
+ return 0;
+}
+
+static inline int l2cap_data_channel(struct l2cap_conn *conn, u16 cid, struct l2cap_hdr *lh, struct sk_buff *skb)
{
struct sock *sk;
@@ -2618,16 +2870,29 @@ 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:
+ if (!l2cap_ertm_data_channel(sk, lh, skb));
+ goto done;
+ break;
+
+ default:
+ BT_DBG("sk %p: bad mode 0x%2.2x", sk, l2cap_pi(sk)->mode);
+ break;
+ }
drop:
kfree_skb(skb);
@@ -2691,7 +2956,7 @@ static void l2cap_recv_frame(struct l2cap_conn *conn, struct sk_buff *skb)
break;
default:
- l2cap_data_channel(conn, cid, skb);
+ l2cap_data_channel(conn, cid, lh, skb);
break;
}
}
@@ -2872,7 +3137,6 @@ static int l2cap_recv_acldata(struct hci_conn *hcon, struct sk_buff *skb, u16 fl
goto drop;
BT_DBG("conn %p len %d flags 0x%x", conn, skb->len, flags);
-
if (flags & ACL_START) {
struct l2cap_hdr *hdr;
int len;
--
1.6.3.3
--
To unsubscribe from this list: send the line "unsubscribe linux-bluetooth" in
the body of a message to majordomo@xxxxxxxxxxxxxxx
More majordomo info at http://vger.kernel.org/majordomo-info.html
