#define VERSION "2.14"
+#ifdef CONFIG_BT_L2CAP_EXT_FEATURES
+static int enable_ertm = 1;
+#else
static int enable_ertm = 0;
+#endif
static int max_transmit = L2CAP_DEFAULT_MAX_TX;
static int tx_window = L2CAP_DEFAULT_TX_WINDOW;
static const struct proto_ops l2cap_sock_ops;
+static struct workqueue_struct *_busy_wq;
+
static struct bt_sock_list l2cap_sk_list = {
.lock = __RW_LOCK_UNLOCKED(l2cap_sk_list.lock)
};
+static void l2cap_busy_work(struct work_struct *work);
+
static void __l2cap_sock_close(struct sock *sk, int reason);
static void l2cap_sock_close(struct sock *sk);
static void l2cap_sock_kill(struct sock *sk);
l2cap_pi(sk)->conn = conn;
- if (sk->sk_type == SOCK_SEQPACKET) {
+ if (sk->sk_type == SOCK_SEQPACKET || sk->sk_type == SOCK_STREAM) {
/* Alloc CID for connection-oriented socket */
l2cap_pi(sk)->scid = l2cap_alloc_cid(l);
} else if (sk->sk_type == SOCK_DGRAM) {
return id;
}
-static inline int l2cap_send_cmd(struct l2cap_conn *conn, u8 ident, u8 code, u16 len, void *data)
+static inline void l2cap_send_cmd(struct l2cap_conn *conn, u8 ident, u8 code, u16 len, void *data)
{
struct sk_buff *skb = l2cap_build_cmd(conn, code, ident, len, data);
BT_DBG("code 0x%2.2x", code);
if (!skb)
- return -ENOMEM;
+ return;
- return hci_send_acl(conn->hcon, skb, 0);
+ hci_send_acl(conn->hcon, skb, 0);
}
-static inline int l2cap_send_sframe(struct l2cap_pinfo *pi, u16 control)
+static inline void l2cap_send_sframe(struct l2cap_pinfo *pi, u16 control)
{
struct sk_buff *skb;
struct l2cap_hdr *lh;
skb = bt_skb_alloc(count, GFP_ATOMIC);
if (!skb)
- return -ENOMEM;
+ return;
lh = (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE);
lh->len = cpu_to_le16(hlen - L2CAP_HDR_SIZE);
put_unaligned_le16(fcs, skb_put(skb, 2));
}
- return hci_send_acl(pi->conn->hcon, skb, 0);
+ hci_send_acl(pi->conn->hcon, skb, 0);
}
-static inline int l2cap_send_rr_or_rnr(struct l2cap_pinfo *pi, u16 control)
+static inline void l2cap_send_rr_or_rnr(struct l2cap_pinfo *pi, u16 control)
{
- if (pi->conn_state & L2CAP_CONN_LOCAL_BUSY)
+ if (pi->conn_state & L2CAP_CONN_LOCAL_BUSY) {
control |= L2CAP_SUPER_RCV_NOT_READY;
- else
+ pi->conn_state |= L2CAP_CONN_RNR_SENT;
+ } else
control |= L2CAP_SUPER_RCV_READY;
control |= pi->buffer_seq << L2CAP_CTRL_REQSEQ_SHIFT;
- return l2cap_send_sframe(pi, control);
+ l2cap_send_sframe(pi, control);
}
static void l2cap_do_start(struct sock *sk)
for (sk = l->head; sk; sk = l2cap_pi(sk)->next_c) {
bh_lock_sock(sk);
- if (sk->sk_type != SOCK_SEQPACKET) {
+ if (sk->sk_type != SOCK_SEQPACKET &&
+ sk->sk_type != SOCK_STREAM) {
bh_unlock_sock(sk);
continue;
}
for (sk = l->head; sk; sk = l2cap_pi(sk)->next_c) {
bh_lock_sock(sk);
- if (sk->sk_type != SOCK_SEQPACKET) {
+ if (sk->sk_type != SOCK_SEQPACKET &&
+ sk->sk_type != SOCK_STREAM) {
l2cap_sock_clear_timer(sk);
sk->sk_state = BT_CONNECTED;
sk->sk_state_change(sk);
case BT_CONNECTED:
case BT_CONFIG:
- if (sk->sk_type == SOCK_SEQPACKET) {
+ if (sk->sk_type == SOCK_SEQPACKET ||
+ sk->sk_type == SOCK_STREAM) {
struct l2cap_conn *conn = l2cap_pi(sk)->conn;
sk->sk_state = BT_DISCONN;
break;
case BT_CONNECT2:
- if (sk->sk_type == SOCK_SEQPACKET) {
+ if (sk->sk_type == SOCK_SEQPACKET ||
+ sk->sk_type == SOCK_STREAM) {
struct l2cap_conn *conn = l2cap_pi(sk)->conn;
struct l2cap_conn_rsp rsp;
__u16 result;
} else {
pi->imtu = L2CAP_DEFAULT_MTU;
pi->omtu = 0;
- pi->mode = L2CAP_MODE_BASIC;
+ if (enable_ertm && sk->sk_type == SOCK_STREAM)
+ pi->mode = L2CAP_MODE_ERTM;
+ else
+ pi->mode = L2CAP_MODE_BASIC;
pi->max_tx = max_transmit;
pi->fcs = L2CAP_FCS_CRC16;
pi->tx_win = tx_window;
pi->flush_to = L2CAP_DEFAULT_FLUSH_TO;
skb_queue_head_init(TX_QUEUE(sk));
skb_queue_head_init(SREJ_QUEUE(sk));
+ skb_queue_head_init(BUSY_QUEUE(sk));
INIT_LIST_HEAD(SREJ_LIST(sk));
}
sock->state = SS_UNCONNECTED;
- if (sock->type != SOCK_SEQPACKET &&
+ if (sock->type != SOCK_SEQPACKET && sock->type != SOCK_STREAM &&
sock->type != SOCK_DGRAM && sock->type != SOCK_RAW)
return -ESOCKTNOSUPPORT;
l2cap_sock_set_timer(sk, sk->sk_sndtimeo);
if (hcon->state == BT_CONNECTED) {
- if (sk->sk_type != SOCK_SEQPACKET) {
+ if (sk->sk_type != SOCK_SEQPACKET &&
+ sk->sk_type != SOCK_STREAM) {
l2cap_sock_clear_timer(sk);
sk->sk_state = BT_CONNECTED;
} else
lock_sock(sk);
- if (sk->sk_type == SOCK_SEQPACKET && !la.l2_psm) {
+ if ((sk->sk_type == SOCK_SEQPACKET || sk->sk_type == SOCK_STREAM)
+ && !la.l2_psm) {
err = -EINVAL;
goto done;
}
lock_sock(sk);
- if (sk->sk_state != BT_BOUND || sock->type != SOCK_SEQPACKET) {
+ if ((sock->type != SOCK_SEQPACKET && sock->type != SOCK_STREAM)
+ || sk->sk_state != BT_BOUND) {
err = -EBADFD;
goto done;
}
return 0;
}
+static int __l2cap_wait_ack(struct sock *sk)
+{
+ DECLARE_WAITQUEUE(wait, current);
+ int err = 0;
+ int timeo = HZ/5;
+
+ add_wait_queue(sk_sleep(sk), &wait);
+ while ((l2cap_pi(sk)->unacked_frames > 0 && l2cap_pi(sk)->conn)) {
+ set_current_state(TASK_INTERRUPTIBLE);
+
+ if (!timeo)
+ timeo = HZ/5;
+
+ if (signal_pending(current)) {
+ err = sock_intr_errno(timeo);
+ break;
+ }
+
+ release_sock(sk);
+ timeo = schedule_timeout(timeo);
+ lock_sock(sk);
+
+ err = sock_error(sk);
+ if (err)
+ break;
+ }
+ set_current_state(TASK_RUNNING);
+ remove_wait_queue(sk_sleep(sk), &wait);
+ return err;
+}
+
static void l2cap_monitor_timeout(unsigned long arg)
{
struct sock *sk = (void *) arg;
- u16 control;
bh_lock_sock(sk);
if (l2cap_pi(sk)->retry_count >= l2cap_pi(sk)->remote_max_tx) {
l2cap_pi(sk)->retry_count++;
__mod_monitor_timer();
- control = L2CAP_CTRL_POLL;
- l2cap_send_rr_or_rnr(l2cap_pi(sk), control);
+ l2cap_send_rr_or_rnr(l2cap_pi(sk), L2CAP_CTRL_POLL);
bh_unlock_sock(sk);
}
static void l2cap_retrans_timeout(unsigned long arg)
{
struct sock *sk = (void *) arg;
- u16 control;
bh_lock_sock(sk);
l2cap_pi(sk)->retry_count = 1;
l2cap_pi(sk)->conn_state |= L2CAP_CONN_WAIT_F;
- control = L2CAP_CTRL_POLL;
- l2cap_send_rr_or_rnr(l2cap_pi(sk), control);
+ l2cap_send_rr_or_rnr(l2cap_pi(sk), L2CAP_CTRL_POLL);
bh_unlock_sock(sk);
}
{
struct sk_buff *skb;
- while ((skb = skb_peek(TX_QUEUE(sk)))) {
+ while ((skb = skb_peek(TX_QUEUE(sk))) &&
+ l2cap_pi(sk)->unacked_frames) {
if (bt_cb(skb)->tx_seq == l2cap_pi(sk)->expected_ack_seq)
break;
return;
}
-static inline int l2cap_do_send(struct sock *sk, struct sk_buff *skb)
+static inline void 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;
+ hci_send_acl(pi->conn->hcon, skb, 0);
}
static int l2cap_streaming_send(struct sock *sk)
struct sk_buff *skb, *tx_skb;
struct l2cap_pinfo *pi = l2cap_pi(sk);
u16 control, fcs;
- int err;
while ((skb = sk->sk_send_head)) {
tx_skb = skb_clone(skb, GFP_ATOMIC);
put_unaligned_le16(fcs, tx_skb->data + tx_skb->len - 2);
}
- err = l2cap_do_send(sk, tx_skb);
- if (err < 0) {
- l2cap_send_disconn_req(pi->conn, sk);
- return err;
- }
+ l2cap_do_send(sk, tx_skb);
pi->next_tx_seq = (pi->next_tx_seq + 1) % 64;
return 0;
}
-static int l2cap_retransmit_frame(struct sock *sk, u8 tx_seq)
+static void l2cap_retransmit_one_frame(struct sock *sk, u8 tx_seq)
{
struct l2cap_pinfo *pi = l2cap_pi(sk);
struct sk_buff *skb, *tx_skb;
u16 control, fcs;
- int err;
skb = skb_peek(TX_QUEUE(sk));
- do {
- if (bt_cb(skb)->tx_seq != tx_seq) {
- if (skb_queue_is_last(TX_QUEUE(sk), skb))
- break;
- skb = skb_queue_next(TX_QUEUE(sk), skb);
- continue;
- }
+ if (!skb)
+ return;
- if (pi->remote_max_tx &&
- bt_cb(skb)->retries == pi->remote_max_tx) {
- l2cap_send_disconn_req(pi->conn, sk);
+ do {
+ if (bt_cb(skb)->tx_seq == tx_seq)
break;
- }
- tx_skb = skb_clone(skb, GFP_ATOMIC);
- bt_cb(skb)->retries++;
- control = get_unaligned_le16(tx_skb->data + L2CAP_HDR_SIZE);
- control |= (pi->buffer_seq << L2CAP_CTRL_REQSEQ_SHIFT)
- | (tx_seq << L2CAP_CTRL_TXSEQ_SHIFT);
- put_unaligned_le16(control, tx_skb->data + L2CAP_HDR_SIZE);
+ if (skb_queue_is_last(TX_QUEUE(sk), skb))
+ return;
- if (pi->fcs == L2CAP_FCS_CRC16) {
- fcs = crc16(0, (u8 *)tx_skb->data, tx_skb->len - 2);
- put_unaligned_le16(fcs, tx_skb->data + tx_skb->len - 2);
- }
+ } while ((skb = skb_queue_next(TX_QUEUE(sk), skb)));
- err = l2cap_do_send(sk, tx_skb);
- if (err < 0) {
- l2cap_send_disconn_req(pi->conn, sk);
- return err;
- }
- break;
- } while(1);
- return 0;
+ if (pi->remote_max_tx &&
+ bt_cb(skb)->retries == pi->remote_max_tx) {
+ l2cap_send_disconn_req(pi->conn, sk);
+ return;
+ }
+
+ tx_skb = skb_clone(skb, GFP_ATOMIC);
+ bt_cb(skb)->retries++;
+ control = get_unaligned_le16(tx_skb->data + L2CAP_HDR_SIZE);
+ control |= (pi->buffer_seq << L2CAP_CTRL_REQSEQ_SHIFT)
+ | (tx_seq << L2CAP_CTRL_TXSEQ_SHIFT);
+ put_unaligned_le16(control, tx_skb->data + L2CAP_HDR_SIZE);
+
+ if (pi->fcs == L2CAP_FCS_CRC16) {
+ fcs = crc16(0, (u8 *)tx_skb->data, tx_skb->len - 2);
+ put_unaligned_le16(fcs, tx_skb->data + tx_skb->len - 2);
+ }
+
+ l2cap_do_send(sk, tx_skb);
}
static int l2cap_ertm_send(struct sock *sk)
struct sk_buff *skb, *tx_skb;
struct l2cap_pinfo *pi = l2cap_pi(sk);
u16 control, fcs;
- int err, nsent = 0;
+ int nsent = 0;
if (pi->conn_state & L2CAP_CONN_WAIT_F)
return 0;
while ((skb = sk->sk_send_head) && (!l2cap_tx_window_full(sk)) &&
- !(pi->conn_state & L2CAP_CONN_REMOTE_BUSY)) {
+ !(pi->conn_state & L2CAP_CONN_REMOTE_BUSY)) {
if (pi->remote_max_tx &&
bt_cb(skb)->retries == pi->remote_max_tx) {
put_unaligned_le16(fcs, skb->data + tx_skb->len - 2);
}
- err = l2cap_do_send(sk, tx_skb);
- if (err < 0) {
- l2cap_send_disconn_req(pi->conn, sk);
- return err;
- }
+ l2cap_do_send(sk, tx_skb);
+
__mod_retrans_timer();
bt_cb(skb)->tx_seq = pi->next_tx_seq;
return nsent;
}
-static int l2cap_send_ack(struct l2cap_pinfo *pi)
+static int l2cap_retransmit_frames(struct sock *sk)
+{
+ struct l2cap_pinfo *pi = l2cap_pi(sk);
+ int ret;
+
+ spin_lock_bh(&pi->send_lock);
+
+ if (!skb_queue_empty(TX_QUEUE(sk)))
+ sk->sk_send_head = TX_QUEUE(sk)->next;
+
+ pi->next_tx_seq = pi->expected_ack_seq;
+ ret = l2cap_ertm_send(sk);
+
+ spin_unlock_bh(&pi->send_lock);
+
+ return ret;
+}
+
+static void l2cap_send_ack(struct l2cap_pinfo *pi)
{
struct sock *sk = (struct sock *)pi;
u16 control = 0;
+ int nframes;
control |= pi->buffer_seq << L2CAP_CTRL_REQSEQ_SHIFT;
if (pi->conn_state & L2CAP_CONN_LOCAL_BUSY) {
control |= L2CAP_SUPER_RCV_NOT_READY;
- return l2cap_send_sframe(pi, control);
- } else if (l2cap_ertm_send(sk) == 0) {
- control |= L2CAP_SUPER_RCV_READY;
- return l2cap_send_sframe(pi, control);
+ pi->conn_state |= L2CAP_CONN_RNR_SENT;
+ l2cap_send_sframe(pi, control);
+ return;
}
- return 0;
+
+ spin_lock_bh(&pi->send_lock);
+ nframes = l2cap_ertm_send(sk);
+ spin_unlock_bh(&pi->send_lock);
+
+ if (nframes > 0)
+ return;
+
+ control |= L2CAP_SUPER_RCV_READY;
+ l2cap_send_sframe(pi, control);
}
-static int l2cap_send_srejtail(struct sock *sk)
+static void l2cap_send_srejtail(struct sock *sk)
{
struct srej_list *tail;
u16 control;
control |= tail->tx_seq << L2CAP_CTRL_REQSEQ_SHIFT;
l2cap_send_sframe(l2cap_pi(sk), control);
-
- return 0;
}
static inline int l2cap_skbuff_fromiovec(struct sock *sk, struct msghdr *msg, int len, int count, struct sk_buff *skb)
struct sk_buff **frag;
int err, sent = 0;
- if (memcpy_fromiovec(skb_put(skb, count), msg->msg_iov, count)) {
+ if (memcpy_fromiovec(skb_put(skb, count), msg->msg_iov, count))
return -EFAULT;
- }
sent += count;
len -= count;
BT_DBG("sk %p len %d", sk, (int)len);
+ if (!conn)
+ return ERR_PTR(-ENOTCONN);
+
if (sdulen)
hlen += 2;
u16 control;
size_t size = 0;
- __skb_queue_head_init(&sar_queue);
+ skb_queue_head_init(&sar_queue);
control = L2CAP_SDU_START;
skb = l2cap_create_iframe_pdu(sk, msg, pi->remote_mps, control, len);
if (IS_ERR(skb))
__skb_queue_tail(&sar_queue, skb);
len -= pi->remote_mps;
size += pi->remote_mps;
- control = 0;
while (len > 0) {
size_t buflen;
if (len > pi->remote_mps) {
- control |= L2CAP_SDU_CONTINUE;
+ control = L2CAP_SDU_CONTINUE;
buflen = pi->remote_mps;
} else {
- control |= L2CAP_SDU_END;
+ control = L2CAP_SDU_END;
buflen = len;
}
__skb_queue_tail(&sar_queue, skb);
len -= buflen;
size += buflen;
- control = 0;
}
skb_queue_splice_tail(&sar_queue, TX_QUEUE(sk));
+ spin_lock_bh(&pi->send_lock);
if (sk->sk_send_head == NULL)
sk->sk_send_head = sar_queue.next;
+ spin_unlock_bh(&pi->send_lock);
return size;
}
/* Connectionless channel */
if (sk->sk_type == SOCK_DGRAM) {
skb = l2cap_create_connless_pdu(sk, msg, len);
- if (IS_ERR(skb))
+ if (IS_ERR(skb)) {
err = PTR_ERR(skb);
- else
- err = l2cap_do_send(sk, skb);
+ } else {
+ l2cap_do_send(sk, skb);
+ err = len;
+ }
goto done;
}
goto done;
}
- err = l2cap_do_send(sk, skb);
- if (!err)
- err = len;
+ l2cap_do_send(sk, skb);
+ err = len;
break;
case L2CAP_MODE_ERTM:
goto done;
}
__skb_queue_tail(TX_QUEUE(sk), skb);
+
+ if (pi->mode == L2CAP_MODE_ERTM)
+ spin_lock_bh(&pi->send_lock);
+
if (sk->sk_send_head == NULL)
sk->sk_send_head = skb;
+
+ if (pi->mode == L2CAP_MODE_ERTM)
+ spin_unlock_bh(&pi->send_lock);
} else {
/* Segment SDU into multiples PDUs */
err = l2cap_sar_segment_sdu(sk, msg, len);
goto done;
}
- if (pi->mode == L2CAP_MODE_STREAMING)
+ if (pi->mode == L2CAP_MODE_STREAMING) {
err = l2cap_streaming_send(sk);
- else
+ } else {
+ spin_lock_bh(&pi->send_lock);
err = l2cap_ertm_send(sk);
+ spin_unlock_bh(&pi->send_lock);
+ }
if (err >= 0)
err = len;
break;
}
+ l2cap_pi(sk)->mode = opts.mode;
+ switch (l2cap_pi(sk)->mode) {
+ case L2CAP_MODE_BASIC:
+ break;
+ case L2CAP_MODE_ERTM:
+ case L2CAP_MODE_STREAMING:
+ if (enable_ertm)
+ break;
+ /* fall through */
+ default:
+ err = -EINVAL;
+ break;
+ }
+
l2cap_pi(sk)->imtu = opts.imtu;
l2cap_pi(sk)->omtu = opts.omtu;
- l2cap_pi(sk)->mode = opts.mode;
l2cap_pi(sk)->fcs = opts.fcs;
l2cap_pi(sk)->max_tx = opts.max_tx;
l2cap_pi(sk)->tx_win = (__u8)opts.txwin_size;
switch (optname) {
case BT_SECURITY:
- if (sk->sk_type != SOCK_SEQPACKET && sk->sk_type != SOCK_RAW) {
+ if (sk->sk_type != SOCK_SEQPACKET && sk->sk_type != SOCK_STREAM
+ && sk->sk_type != SOCK_RAW) {
err = -EINVAL;
break;
}
switch (optname) {
case BT_SECURITY:
- if (sk->sk_type != SOCK_SEQPACKET && sk->sk_type != SOCK_RAW) {
+ if (sk->sk_type != SOCK_SEQPACKET && sk->sk_type != SOCK_STREAM
+ && sk->sk_type != SOCK_RAW) {
err = -EINVAL;
break;
}
lock_sock(sk);
if (!sk->sk_shutdown) {
+ if (l2cap_pi(sk)->mode == L2CAP_MODE_ERTM)
+ err = __l2cap_wait_ack(sk);
+
sk->sk_shutdown = SHUTDOWN_MASK;
l2cap_sock_clear_timer(sk);
__l2cap_sock_close(sk, 0);
l2cap_ack_timeout, (unsigned long) sk);
__skb_queue_head_init(SREJ_QUEUE(sk));
+ __skb_queue_head_init(BUSY_QUEUE(sk));
+ spin_lock_init(&l2cap_pi(sk)->send_lock);
+
+ INIT_WORK(&l2cap_pi(sk)->busy_work, l2cap_busy_work);
}
static int l2cap_mode_supported(__u8 mode, __u32 feat_mask)
{
struct l2cap_pinfo *pi = l2cap_pi(sk);
struct l2cap_conf_req *req = data;
- struct l2cap_conf_rfc rfc = { .mode = L2CAP_MODE_BASIC };
+ struct l2cap_conf_rfc rfc = { .mode = pi->mode };
void *ptr = req->data;
BT_DBG("sk %p", sk);
if (l2cap_pi(sk)->mode == L2CAP_MODE_ERTM) {
skb_queue_purge(SREJ_QUEUE(sk));
+ skb_queue_purge(BUSY_QUEUE(sk));
del_timer(&l2cap_pi(sk)->retrans_timer);
del_timer(&l2cap_pi(sk)->monitor_timer);
del_timer(&l2cap_pi(sk)->ack_timer);
if (l2cap_pi(sk)->mode == L2CAP_MODE_ERTM) {
skb_queue_purge(SREJ_QUEUE(sk));
+ skb_queue_purge(BUSY_QUEUE(sk));
del_timer(&l2cap_pi(sk)->retrans_timer);
del_timer(&l2cap_pi(sk)->monitor_timer);
del_timer(&l2cap_pi(sk)->ack_timer);
if (pi->conn_state & L2CAP_CONN_LOCAL_BUSY) {
control |= L2CAP_SUPER_RCV_NOT_READY | L2CAP_CTRL_FINAL;
l2cap_send_sframe(pi, control);
+ pi->conn_state |= L2CAP_CONN_RNR_SENT;
pi->conn_state &= ~L2CAP_CONN_SEND_FBIT;
}
if (pi->conn_state & L2CAP_CONN_REMOTE_BUSY && pi->unacked_frames > 0)
__mod_retrans_timer();
+ pi->conn_state &= ~L2CAP_CONN_REMOTE_BUSY;
+
+ spin_lock_bh(&pi->send_lock);
l2cap_ertm_send(sk);
+ spin_unlock_bh(&pi->send_lock);
if (!(pi->conn_state & L2CAP_CONN_LOCAL_BUSY) &&
pi->frames_sent == 0) {
}
}
-static void l2cap_add_to_srej_queue(struct sock *sk, struct sk_buff *skb, u8 tx_seq, u8 sar)
+static int l2cap_add_to_srej_queue(struct sock *sk, struct sk_buff *skb, u8 tx_seq, u8 sar)
{
struct sk_buff *next_skb;
next_skb = skb_peek(SREJ_QUEUE(sk));
if (!next_skb) {
__skb_queue_tail(SREJ_QUEUE(sk), skb);
- return;
+ return 0;
}
do {
+ if (bt_cb(next_skb)->tx_seq == tx_seq)
+ return -EINVAL;
+
if (bt_cb(next_skb)->tx_seq > tx_seq) {
__skb_queue_before(SREJ_QUEUE(sk), next_skb, skb);
- return;
+ return 0;
}
if (skb_queue_is_last(SREJ_QUEUE(sk), next_skb))
break;
- } while((next_skb = skb_queue_next(SREJ_QUEUE(sk), next_skb)));
+ } while ((next_skb = skb_queue_next(SREJ_QUEUE(sk), next_skb)));
__skb_queue_tail(SREJ_QUEUE(sk), skb);
+
+ return 0;
+}
+
+static int l2cap_ertm_reassembly_sdu(struct sock *sk, struct sk_buff *skb, u16 control)
+{
+ struct l2cap_pinfo *pi = l2cap_pi(sk);
+ struct sk_buff *_skb;
+ int err;
+
+ switch (control & L2CAP_CTRL_SAR) {
+ case L2CAP_SDU_UNSEGMENTED:
+ if (pi->conn_state & L2CAP_CONN_SAR_SDU)
+ goto drop;
+
+ err = sock_queue_rcv_skb(sk, skb);
+ if (!err)
+ return err;
+
+ break;
+
+ case L2CAP_SDU_START:
+ if (pi->conn_state & L2CAP_CONN_SAR_SDU)
+ goto drop;
+
+ pi->sdu_len = get_unaligned_le16(skb->data);
+
+ if (pi->sdu_len > pi->imtu)
+ goto disconnect;
+
+ pi->sdu = bt_skb_alloc(pi->sdu_len, GFP_ATOMIC);
+ if (!pi->sdu)
+ return -ENOMEM;
+
+ /* pull sdu_len bytes only after alloc, because of Local Busy
+ * condition we have to be sure that this will be executed
+ * only once, i.e., when alloc does not fail */
+ skb_pull(skb, 2);
+
+ memcpy(skb_put(pi->sdu, skb->len), skb->data, skb->len);
+
+ pi->conn_state |= L2CAP_CONN_SAR_SDU;
+ pi->partial_sdu_len = skb->len;
+ break;
+
+ case L2CAP_SDU_CONTINUE:
+ if (!(pi->conn_state & L2CAP_CONN_SAR_SDU))
+ goto disconnect;
+
+ if (!pi->sdu)
+ goto disconnect;
+
+ pi->partial_sdu_len += skb->len;
+ if (pi->partial_sdu_len > pi->sdu_len)
+ goto drop;
+
+ memcpy(skb_put(pi->sdu, skb->len), skb->data, skb->len);
+
+ break;
+
+ case L2CAP_SDU_END:
+ if (!(pi->conn_state & L2CAP_CONN_SAR_SDU))
+ goto disconnect;
+
+ if (!pi->sdu)
+ goto disconnect;
+
+ if (!(pi->conn_state & L2CAP_CONN_SAR_RETRY)) {
+ pi->partial_sdu_len += skb->len;
+
+ if (pi->partial_sdu_len > pi->imtu)
+ goto drop;
+
+ if (pi->partial_sdu_len != pi->sdu_len)
+ goto drop;
+
+ memcpy(skb_put(pi->sdu, skb->len), skb->data, skb->len);
+ }
+
+ _skb = skb_clone(pi->sdu, GFP_ATOMIC);
+ if (!_skb) {
+ pi->conn_state |= L2CAP_CONN_SAR_RETRY;
+ return -ENOMEM;
+ }
+
+ err = sock_queue_rcv_skb(sk, _skb);
+ if (err < 0) {
+ kfree_skb(_skb);
+ pi->conn_state |= L2CAP_CONN_SAR_RETRY;
+ return err;
+ }
+
+ pi->conn_state &= ~L2CAP_CONN_SAR_RETRY;
+ pi->conn_state &= ~L2CAP_CONN_SAR_SDU;
+
+ kfree_skb(pi->sdu);
+ break;
+ }
+
+ kfree_skb(skb);
+ return 0;
+
+drop:
+ kfree_skb(pi->sdu);
+ pi->sdu = NULL;
+
+disconnect:
+ l2cap_send_disconn_req(pi->conn, sk);
+ kfree_skb(skb);
+ return 0;
}
-static int l2cap_sar_reassembly_sdu(struct sock *sk, struct sk_buff *skb, u16 control)
+static void l2cap_busy_work(struct work_struct *work)
+{
+ DECLARE_WAITQUEUE(wait, current);
+ struct l2cap_pinfo *pi =
+ container_of(work, struct l2cap_pinfo, busy_work);
+ struct sock *sk = (struct sock *)pi;
+ int n_tries = 0, timeo = HZ/5, err;
+ struct sk_buff *skb;
+ u16 control;
+
+ lock_sock(sk);
+
+ add_wait_queue(sk_sleep(sk), &wait);
+ while ((skb = skb_peek(BUSY_QUEUE(sk)))) {
+ set_current_state(TASK_INTERRUPTIBLE);
+
+ if (n_tries++ > L2CAP_LOCAL_BUSY_TRIES) {
+ err = -EBUSY;
+ l2cap_send_disconn_req(pi->conn, sk);
+ goto done;
+ }
+
+ if (!timeo)
+ timeo = HZ/5;
+
+ if (signal_pending(current)) {
+ err = sock_intr_errno(timeo);
+ goto done;
+ }
+
+ release_sock(sk);
+ timeo = schedule_timeout(timeo);
+ lock_sock(sk);
+
+ err = sock_error(sk);
+ if (err)
+ goto done;
+
+ while ((skb = skb_dequeue(BUSY_QUEUE(sk)))) {
+ control = bt_cb(skb)->sar << L2CAP_CTRL_SAR_SHIFT;
+ err = l2cap_ertm_reassembly_sdu(sk, skb, control);
+ if (err < 0) {
+ skb_queue_head(BUSY_QUEUE(sk), skb);
+ break;
+ }
+
+ pi->buffer_seq = (pi->buffer_seq + 1) % 64;
+ }
+
+ if (!skb)
+ break;
+ }
+
+ if (!(pi->conn_state & L2CAP_CONN_RNR_SENT))
+ goto done;
+
+ control = pi->buffer_seq << L2CAP_CTRL_REQSEQ_SHIFT;
+ control |= L2CAP_SUPER_RCV_READY | L2CAP_CTRL_POLL;
+ l2cap_send_sframe(pi, control);
+ l2cap_pi(sk)->retry_count = 1;
+
+ del_timer(&pi->retrans_timer);
+ __mod_monitor_timer();
+
+ l2cap_pi(sk)->conn_state |= L2CAP_CONN_WAIT_F;
+
+done:
+ pi->conn_state &= ~L2CAP_CONN_LOCAL_BUSY;
+ pi->conn_state &= ~L2CAP_CONN_RNR_SENT;
+
+ set_current_state(TASK_RUNNING);
+ remove_wait_queue(sk_sleep(sk), &wait);
+
+ release_sock(sk);
+}
+
+static int l2cap_push_rx_skb(struct sock *sk, struct sk_buff *skb, u16 control)
+{
+ struct l2cap_pinfo *pi = l2cap_pi(sk);
+ int sctrl, err;
+
+ if (pi->conn_state & L2CAP_CONN_LOCAL_BUSY) {
+ bt_cb(skb)->sar = control >> L2CAP_CTRL_SAR_SHIFT;
+ __skb_queue_tail(BUSY_QUEUE(sk), skb);
+ return -EBUSY;
+ }
+
+ err = l2cap_ertm_reassembly_sdu(sk, skb, control);
+ if (err >= 0) {
+ pi->buffer_seq = (pi->buffer_seq + 1) % 64;
+ return err;
+ }
+
+ /* Busy Condition */
+ pi->conn_state |= L2CAP_CONN_LOCAL_BUSY;
+ bt_cb(skb)->sar = control >> L2CAP_CTRL_SAR_SHIFT;
+ __skb_queue_tail(BUSY_QUEUE(sk), skb);
+
+ sctrl = pi->buffer_seq << L2CAP_CTRL_REQSEQ_SHIFT;
+ sctrl |= L2CAP_SUPER_RCV_NOT_READY;
+ l2cap_send_sframe(pi, sctrl);
+
+ pi->conn_state |= L2CAP_CONN_RNR_SENT;
+
+ queue_work(_busy_wq, &pi->busy_work);
+
+ return err;
+}
+
+static int l2cap_streaming_reassembly_sdu(struct sock *sk, struct sk_buff *skb, u16 control)
{
struct l2cap_pinfo *pi = l2cap_pi(sk);
struct sk_buff *_skb;
int err = -EINVAL;
+ /*
+ * TODO: We have to notify the userland if some data is lost with the
+ * Streaming Mode.
+ */
+
switch (control & L2CAP_CTRL_SAR) {
case L2CAP_SDU_UNSEGMENTED:
if (pi->conn_state & L2CAP_CONN_SAR_SDU) {
static void l2cap_check_srej_gap(struct sock *sk, u8 tx_seq)
{
struct sk_buff *skb;
- u16 control = 0;
+ u16 control;
- while((skb = skb_peek(SREJ_QUEUE(sk)))) {
+ while ((skb = skb_peek(SREJ_QUEUE(sk)))) {
if (bt_cb(skb)->tx_seq != tx_seq)
break;
skb = skb_dequeue(SREJ_QUEUE(sk));
- control |= bt_cb(skb)->sar << L2CAP_CTRL_SAR_SHIFT;
- l2cap_sar_reassembly_sdu(sk, skb, control);
+ control = bt_cb(skb)->sar << L2CAP_CTRL_SAR_SHIFT;
+ l2cap_ertm_reassembly_sdu(sk, skb, control);
l2cap_pi(sk)->buffer_seq_srej =
(l2cap_pi(sk)->buffer_seq_srej + 1) % 64;
tx_seq++;
struct srej_list *l, *tmp;
u16 control;
- list_for_each_entry_safe(l,tmp, SREJ_LIST(sk), list) {
+ list_for_each_entry_safe(l, tmp, SREJ_LIST(sk), list) {
if (l->tx_seq == tx_seq) {
list_del(&l->list);
kfree(l);
u8 tx_seq = __get_txseq(rx_control);
u8 req_seq = __get_reqseq(rx_control);
u8 sar = rx_control >> L2CAP_CTRL_SAR_SHIFT;
+ u8 tx_seq_offset, expected_tx_seq_offset;
int num_to_ack = (pi->tx_win/6) + 1;
int err = 0;
BT_DBG("sk %p rx_control 0x%4.4x len %d", sk, rx_control, skb->len);
- if (L2CAP_CTRL_FINAL & rx_control) {
+ if (L2CAP_CTRL_FINAL & rx_control &&
+ l2cap_pi(sk)->conn_state & L2CAP_CONN_WAIT_F) {
del_timer(&pi->monitor_timer);
if (pi->unacked_frames > 0)
__mod_retrans_timer();
if (tx_seq == pi->expected_tx_seq)
goto expected;
+ tx_seq_offset = (tx_seq - pi->buffer_seq) % 64;
+ if (tx_seq_offset < 0)
+ tx_seq_offset += 64;
+
+ /* invalid tx_seq */
+ if (tx_seq_offset >= pi->tx_win) {
+ l2cap_send_disconn_req(pi->conn, sk);
+ goto drop;
+ }
+
+ if (pi->conn_state == L2CAP_CONN_LOCAL_BUSY)
+ goto drop;
+
if (pi->conn_state & L2CAP_CONN_SREJ_SENT) {
struct srej_list *first;
}
} else {
struct srej_list *l;
- l2cap_add_to_srej_queue(sk, skb, tx_seq, sar);
+
+ /* duplicated tx_seq */
+ if (l2cap_add_to_srej_queue(sk, skb, tx_seq, sar) < 0)
+ goto drop;
list_for_each_entry(l, SREJ_LIST(sk), list) {
if (l->tx_seq == tx_seq) {
l2cap_send_srejframe(sk, tx_seq);
}
} else {
+ expected_tx_seq_offset =
+ (pi->expected_tx_seq - pi->buffer_seq) % 64;
+ if (expected_tx_seq_offset < 0)
+ expected_tx_seq_offset += 64;
+
+ /* duplicated tx_seq */
+ if (tx_seq_offset < expected_tx_seq_offset)
+ goto drop;
+
pi->conn_state |= L2CAP_CONN_SREJ_SENT;
INIT_LIST_HEAD(SREJ_LIST(sk));
pi->buffer_seq_srej = pi->buffer_seq;
__skb_queue_head_init(SREJ_QUEUE(sk));
+ __skb_queue_head_init(BUSY_QUEUE(sk));
l2cap_add_to_srej_queue(sk, skb, tx_seq, sar);
pi->conn_state |= L2CAP_CONN_SEND_PBIT;
pi->expected_tx_seq = (pi->expected_tx_seq + 1) % 64;
if (pi->conn_state & L2CAP_CONN_SREJ_SENT) {
- l2cap_add_to_srej_queue(sk, skb, tx_seq, sar);
+ bt_cb(skb)->tx_seq = tx_seq;
+ bt_cb(skb)->sar = sar;
+ __skb_queue_tail(SREJ_QUEUE(sk), skb);
return 0;
}
if (rx_control & L2CAP_CTRL_FINAL) {
if (pi->conn_state & L2CAP_CONN_REJ_ACT)
pi->conn_state &= ~L2CAP_CONN_REJ_ACT;
- else {
- sk->sk_send_head = TX_QUEUE(sk)->next;
- pi->next_tx_seq = pi->expected_ack_seq;
- l2cap_ertm_send(sk);
- }
+ else
+ l2cap_retransmit_frames(sk);
}
- pi->buffer_seq = (pi->buffer_seq + 1) % 64;
-
- err = l2cap_sar_reassembly_sdu(sk, skb, rx_control);
+ err = l2cap_push_rx_skb(sk, skb, rx_control);
if (err < 0)
- return err;
+ return 0;
__mod_ack_timer();
l2cap_send_ack(pi);
return 0;
+
+drop:
+ kfree_skb(skb);
+ return 0;
}
static inline void l2cap_data_channel_rrframe(struct sock *sk, u16 rx_control)
l2cap_send_srejtail(sk);
} else {
l2cap_send_i_or_rr_or_rnr(sk);
- pi->conn_state &= ~L2CAP_CONN_REMOTE_BUSY;
}
} else if (rx_control & L2CAP_CTRL_FINAL) {
if (pi->conn_state & L2CAP_CONN_REJ_ACT)
pi->conn_state &= ~L2CAP_CONN_REJ_ACT;
- else {
- sk->sk_send_head = TX_QUEUE(sk)->next;
- pi->next_tx_seq = pi->expected_ack_seq;
- l2cap_ertm_send(sk);
- }
+ else
+ l2cap_retransmit_frames(sk);
} else {
if ((pi->conn_state & L2CAP_CONN_REMOTE_BUSY) &&
__mod_retrans_timer();
pi->conn_state &= ~L2CAP_CONN_REMOTE_BUSY;
- if (pi->conn_state & L2CAP_CONN_SREJ_SENT)
+ if (pi->conn_state & L2CAP_CONN_SREJ_SENT) {
l2cap_send_ack(pi);
- else
+ } else {
+ spin_lock_bh(&pi->send_lock);
l2cap_ertm_send(sk);
+ spin_unlock_bh(&pi->send_lock);
+ }
}
}
if (rx_control & L2CAP_CTRL_FINAL) {
if (pi->conn_state & L2CAP_CONN_REJ_ACT)
pi->conn_state &= ~L2CAP_CONN_REJ_ACT;
- else {
- sk->sk_send_head = TX_QUEUE(sk)->next;
- pi->next_tx_seq = pi->expected_ack_seq;
- l2cap_ertm_send(sk);
- }
+ else
+ l2cap_retransmit_frames(sk);
} else {
- sk->sk_send_head = TX_QUEUE(sk)->next;
- pi->next_tx_seq = pi->expected_ack_seq;
- l2cap_ertm_send(sk);
+ l2cap_retransmit_frames(sk);
- if (pi->conn_state & L2CAP_CONN_WAIT_F) {
- pi->srej_save_reqseq = tx_seq;
+ if (pi->conn_state & L2CAP_CONN_WAIT_F)
pi->conn_state |= L2CAP_CONN_REJ_ACT;
- }
}
}
static inline void l2cap_data_channel_srejframe(struct sock *sk, u16 rx_control)
if (rx_control & L2CAP_CTRL_POLL) {
pi->expected_ack_seq = tx_seq;
l2cap_drop_acked_frames(sk);
- l2cap_retransmit_frame(sk, tx_seq);
+ l2cap_retransmit_one_frame(sk, tx_seq);
+
+ spin_lock_bh(&pi->send_lock);
l2cap_ertm_send(sk);
+ spin_unlock_bh(&pi->send_lock);
+
if (pi->conn_state & L2CAP_CONN_WAIT_F) {
pi->srej_save_reqseq = tx_seq;
pi->conn_state |= L2CAP_CONN_SREJ_ACT;
pi->srej_save_reqseq == tx_seq)
pi->conn_state &= ~L2CAP_CONN_SREJ_ACT;
else
- l2cap_retransmit_frame(sk, tx_seq);
+ l2cap_retransmit_one_frame(sk, tx_seq);
} else {
- l2cap_retransmit_frame(sk, tx_seq);
+ l2cap_retransmit_one_frame(sk, tx_seq);
if (pi->conn_state & L2CAP_CONN_WAIT_F) {
pi->srej_save_reqseq = tx_seq;
pi->conn_state |= L2CAP_CONN_SREJ_ACT;
if (!(pi->conn_state & L2CAP_CONN_SREJ_SENT)) {
del_timer(&pi->retrans_timer);
- if (rx_control & L2CAP_CTRL_POLL) {
- u16 control = L2CAP_CTRL_FINAL;
- l2cap_send_rr_or_rnr(pi, control);
- }
+ if (rx_control & L2CAP_CTRL_POLL)
+ l2cap_send_rr_or_rnr(pi, L2CAP_CTRL_FINAL);
return;
}
{
BT_DBG("sk %p rx_control 0x%4.4x len %d", sk, rx_control, skb->len);
- if (L2CAP_CTRL_FINAL & rx_control) {
+ if (L2CAP_CTRL_FINAL & rx_control &&
+ l2cap_pi(sk)->conn_state & L2CAP_CONN_WAIT_F) {
del_timer(&l2cap_pi(sk)->monitor_timer);
if (l2cap_pi(sk)->unacked_frames > 0)
__mod_retrans_timer();
struct sock *sk;
struct l2cap_pinfo *pi;
u16 control, len;
- u8 tx_seq;
+ u8 tx_seq, req_seq, next_tx_seq_offset, req_seq_offset;
sk = l2cap_get_chan_by_scid(&conn->chan_list, cid);
if (!sk) {
* Receiver will miss it and start proper recovery
* procedures and ask retransmission.
*/
- if (len > pi->mps)
+ if (len > pi->mps) {
+ l2cap_send_disconn_req(pi->conn, sk);
goto drop;
+ }
if (l2cap_check_fcs(pi, skb))
goto drop;
+ req_seq = __get_reqseq(control);
+ req_seq_offset = (req_seq - pi->expected_ack_seq) % 64;
+ if (req_seq_offset < 0)
+ req_seq_offset += 64;
+
+ next_tx_seq_offset =
+ (pi->next_tx_seq - pi->expected_ack_seq) % 64;
+ if (next_tx_seq_offset < 0)
+ next_tx_seq_offset += 64;
+
+ /* check for invalid req-seq */
+ if (req_seq_offset > next_tx_seq_offset) {
+ l2cap_send_disconn_req(pi->conn, sk);
+ goto drop;
+ }
+
if (__is_iframe(control)) {
- if (len < 4)
+ if (len < 4) {
+ l2cap_send_disconn_req(pi->conn, sk);
goto drop;
+ }
l2cap_data_channel_iframe(sk, control, skb);
} else {
- if (len != 0)
+ if (len != 0) {
+ l2cap_send_disconn_req(pi->conn, sk);
goto drop;
+ }
l2cap_data_channel_sframe(sk, control, skb);
}
else
pi->expected_tx_seq = (tx_seq + 1) % 64;
- l2cap_sar_reassembly_sdu(sk, skb, control);
+ l2cap_streaming_reassembly_sdu(sk, skb, control);
goto done;
static inline void l2cap_check_encryption(struct sock *sk, u8 encrypt)
{
- if (sk->sk_type != SOCK_SEQPACKET)
+ if (sk->sk_type != SOCK_SEQPACKET && sk->sk_type != SOCK_STREAM)
return;
if (encrypt == 0x00) {
if (err < 0)
return err;
+ _busy_wq = create_singlethread_workqueue("l2cap");
+ if (!_busy_wq)
+ goto error;
+
err = bt_sock_register(BTPROTO_L2CAP, &l2cap_sock_family_ops);
if (err < 0) {
BT_ERR("L2CAP socket registration failed");
{
debugfs_remove(l2cap_debugfs);
+ flush_workqueue(_busy_wq);
+ destroy_workqueue(_busy_wq);
+
if (bt_sock_unregister(BTPROTO_L2CAP) < 0)
BT_ERR("L2CAP socket unregistration failed");
git.openpandora.org / pandora-kernel.git / blobdiff