diff options
Diffstat (limited to 'net/bluetooth/l2cap_core.c')
| -rw-r--r-- | net/bluetooth/l2cap_core.c | 7529 |
1 files changed, 5510 insertions, 2019 deletions
diff --git a/net/bluetooth/l2cap_core.c b/net/bluetooth/l2cap_core.c index ca27f3a4153..323f23cd2c3 100644 --- a/net/bluetooth/l2cap_core.c +++ b/net/bluetooth/l2cap_core.c @@ -3,6 +3,8 @@ Copyright (C) 2000-2001 Qualcomm Incorporated Copyright (C) 2009-2010 Gustavo F. Padovan <gustavo@padovan.org> Copyright (C) 2010 Google Inc. + Copyright (C) 2011 ProFUSION Embedded Systems + Copyright (c) 2012 Code Aurora Forum. All rights reserved. Written 2000,2001 by Maxim Krasnyansky <maxk@qualcomm.com> @@ -28,280 +30,760 @@ #include <linux/module.h> -#include <linux/types.h> -#include <linux/capability.h> -#include <linux/errno.h> -#include <linux/kernel.h> -#include <linux/sched.h> -#include <linux/slab.h> -#include <linux/poll.h> -#include <linux/fcntl.h> -#include <linux/init.h> -#include <linux/interrupt.h> -#include <linux/socket.h> -#include <linux/skbuff.h> -#include <linux/list.h> -#include <linux/device.h> #include <linux/debugfs.h> -#include <linux/seq_file.h> -#include <linux/uaccess.h> #include <linux/crc16.h> -#include <net/sock.h> - -#include <asm/system.h> -#include <asm/unaligned.h> #include <net/bluetooth/bluetooth.h> #include <net/bluetooth/hci_core.h> #include <net/bluetooth/l2cap.h> -int disable_ertm; +#include "smp.h" +#include "a2mp.h" +#include "amp.h" +#include "6lowpan.h" -static u32 l2cap_feat_mask = L2CAP_FEAT_FIXED_CHAN; -static u8 l2cap_fixed_chan[8] = { 0x02, }; +#define LE_FLOWCTL_MAX_CREDITS 65535 -static struct workqueue_struct *_busy_wq; +bool disable_ertm; -struct bt_sock_list l2cap_sk_list = { - .lock = __RW_LOCK_UNLOCKED(l2cap_sk_list.lock) -}; +static u32 l2cap_feat_mask = L2CAP_FEAT_FIXED_CHAN | L2CAP_FEAT_UCD; +static u8 l2cap_fixed_chan[8] = { L2CAP_FC_L2CAP | L2CAP_FC_CONNLESS, }; + +static LIST_HEAD(chan_list); +static DEFINE_RWLOCK(chan_list_lock); -static void l2cap_busy_work(struct work_struct *work); +static u16 le_max_credits = L2CAP_LE_MAX_CREDITS; +static u16 le_default_mps = L2CAP_LE_DEFAULT_MPS; static struct sk_buff *l2cap_build_cmd(struct l2cap_conn *conn, - u8 code, u8 ident, u16 dlen, void *data); + u8 code, u8 ident, u16 dlen, void *data); +static void l2cap_send_cmd(struct l2cap_conn *conn, u8 ident, u8 code, u16 len, + void *data); +static int l2cap_build_conf_req(struct l2cap_chan *chan, void *data); +static void l2cap_send_disconn_req(struct l2cap_chan *chan, int err); -static int l2cap_ertm_data_rcv(struct sock *sk, struct sk_buff *skb); +static void l2cap_tx(struct l2cap_chan *chan, struct l2cap_ctrl *control, + struct sk_buff_head *skbs, u8 event); + +static inline __u8 bdaddr_type(struct hci_conn *hcon, __u8 type) +{ + if (hcon->type == LE_LINK) { + if (type == ADDR_LE_DEV_PUBLIC) + return BDADDR_LE_PUBLIC; + else + return BDADDR_LE_RANDOM; + } + + return BDADDR_BREDR; +} /* ---- L2CAP channels ---- */ -static struct sock *__l2cap_get_chan_by_dcid(struct l2cap_chan_list *l, u16 cid) + +static struct l2cap_chan *__l2cap_get_chan_by_dcid(struct l2cap_conn *conn, + u16 cid) { - struct sock *s; - for (s = l->head; s; s = l2cap_pi(s)->next_c) { - if (l2cap_pi(s)->dcid == cid) - break; + struct l2cap_chan *c; + + list_for_each_entry(c, &conn->chan_l, list) { + if (c->dcid == cid) + return c; } - return s; + return NULL; } -static struct sock *__l2cap_get_chan_by_scid(struct l2cap_chan_list *l, u16 cid) +static struct l2cap_chan *__l2cap_get_chan_by_scid(struct l2cap_conn *conn, + u16 cid) { - struct sock *s; - for (s = l->head; s; s = l2cap_pi(s)->next_c) { - if (l2cap_pi(s)->scid == cid) - break; + struct l2cap_chan *c; + + list_for_each_entry(c, &conn->chan_l, list) { + if (c->scid == cid) + return c; } - return s; + return NULL; } /* Find channel with given SCID. - * Returns locked socket */ -static inline struct sock *l2cap_get_chan_by_scid(struct l2cap_chan_list *l, u16 cid) + * Returns locked channel. */ +static struct l2cap_chan *l2cap_get_chan_by_scid(struct l2cap_conn *conn, + u16 cid) { - struct sock *s; - read_lock(&l->lock); - s = __l2cap_get_chan_by_scid(l, cid); - if (s) - bh_lock_sock(s); - read_unlock(&l->lock); - return s; + struct l2cap_chan *c; + + mutex_lock(&conn->chan_lock); + c = __l2cap_get_chan_by_scid(conn, cid); + if (c) + l2cap_chan_lock(c); + mutex_unlock(&conn->chan_lock); + + return c; } -static struct sock *__l2cap_get_chan_by_ident(struct l2cap_chan_list *l, u8 ident) +/* Find channel with given DCID. + * Returns locked channel. + */ +static struct l2cap_chan *l2cap_get_chan_by_dcid(struct l2cap_conn *conn, + u16 cid) { - struct sock *s; - for (s = l->head; s; s = l2cap_pi(s)->next_c) { - if (l2cap_pi(s)->ident == ident) - break; + struct l2cap_chan *c; + + mutex_lock(&conn->chan_lock); + c = __l2cap_get_chan_by_dcid(conn, cid); + if (c) + l2cap_chan_lock(c); + mutex_unlock(&conn->chan_lock); + + return c; +} + +static struct l2cap_chan *__l2cap_get_chan_by_ident(struct l2cap_conn *conn, + u8 ident) +{ + struct l2cap_chan *c; + + list_for_each_entry(c, &conn->chan_l, list) { + if (c->ident == ident) + return c; + } + return NULL; +} + +static struct l2cap_chan *l2cap_get_chan_by_ident(struct l2cap_conn *conn, + u8 ident) +{ + struct l2cap_chan *c; + + mutex_lock(&conn->chan_lock); + c = __l2cap_get_chan_by_ident(conn, ident); + if (c) + l2cap_chan_lock(c); + mutex_unlock(&conn->chan_lock); + + return c; +} + +static struct l2cap_chan *__l2cap_global_chan_by_addr(__le16 psm, bdaddr_t *src) +{ + struct l2cap_chan *c; + + list_for_each_entry(c, &chan_list, global_l) { + if (c->sport == psm && !bacmp(&c->src, src)) + return c; } - return s; + return NULL; } -static inline struct sock *l2cap_get_chan_by_ident(struct l2cap_chan_list *l, u8 ident) +int l2cap_add_psm(struct l2cap_chan *chan, bdaddr_t *src, __le16 psm) { - struct sock *s; - read_lock(&l->lock); - s = __l2cap_get_chan_by_ident(l, ident); - if (s) - bh_lock_sock(s); - read_unlock(&l->lock); - return s; + int err; + + write_lock(&chan_list_lock); + + if (psm && __l2cap_global_chan_by_addr(psm, src)) { + err = -EADDRINUSE; + goto done; + } + + if (psm) { + chan->psm = psm; + chan->sport = psm; + err = 0; + } else { + u16 p; + + err = -EINVAL; + for (p = 0x1001; p < 0x1100; p += 2) + if (!__l2cap_global_chan_by_addr(cpu_to_le16(p), src)) { + chan->psm = cpu_to_le16(p); + chan->sport = cpu_to_le16(p); + err = 0; + break; + } + } + +done: + write_unlock(&chan_list_lock); + return err; } -static u16 l2cap_alloc_cid(struct l2cap_chan_list *l) +int l2cap_add_scid(struct l2cap_chan *chan, __u16 scid) { - u16 cid = L2CAP_CID_DYN_START; + write_lock(&chan_list_lock); + + chan->scid = scid; + + write_unlock(&chan_list_lock); - for (; cid < L2CAP_CID_DYN_END; cid++) { - if (!__l2cap_get_chan_by_scid(l, cid)) + return 0; +} + +static u16 l2cap_alloc_cid(struct l2cap_conn *conn) +{ + u16 cid, dyn_end; + + if (conn->hcon->type == LE_LINK) + dyn_end = L2CAP_CID_LE_DYN_END; + else + dyn_end = L2CAP_CID_DYN_END; + + for (cid = L2CAP_CID_DYN_START; cid < dyn_end; cid++) { + if (!__l2cap_get_chan_by_scid(conn, cid)) return cid; } return 0; } -static inline void __l2cap_chan_link(struct l2cap_chan_list *l, struct sock *sk) +static void l2cap_state_change(struct l2cap_chan *chan, int state) +{ + BT_DBG("chan %p %s -> %s", chan, state_to_string(chan->state), + state_to_string(state)); + + chan->state = state; + chan->ops->state_change(chan, state, 0); +} + +static inline void l2cap_state_change_and_error(struct l2cap_chan *chan, + int state, int err) +{ + chan->state = state; + chan->ops->state_change(chan, chan->state, err); +} + +static inline void l2cap_chan_set_err(struct l2cap_chan *chan, int err) +{ + chan->ops->state_change(chan, chan->state, err); +} + +static void __set_retrans_timer(struct l2cap_chan *chan) +{ + if (!delayed_work_pending(&chan->monitor_timer) && + chan->retrans_timeout) { + l2cap_set_timer(chan, &chan->retrans_timer, + msecs_to_jiffies(chan->retrans_timeout)); + } +} + +static void __set_monitor_timer(struct l2cap_chan *chan) +{ + __clear_retrans_timer(chan); + if (chan->monitor_timeout) { + l2cap_set_timer(chan, &chan->monitor_timer, + msecs_to_jiffies(chan->monitor_timeout)); + } +} + +static struct sk_buff *l2cap_ertm_seq_in_queue(struct sk_buff_head *head, + u16 seq) +{ + struct sk_buff *skb; + + skb_queue_walk(head, skb) { + if (bt_cb(skb)->control.txseq == seq) + return skb; + } + + return NULL; +} + +/* ---- L2CAP sequence number lists ---- */ + +/* For ERTM, ordered lists of sequence numbers must be tracked for + * SREJ requests that are received and for frames that are to be + * retransmitted. These seq_list functions implement a singly-linked + * list in an array, where membership in the list can also be checked + * in constant time. Items can also be added to the tail of the list + * and removed from the head in constant time, without further memory + * allocs or frees. + */ + +static int l2cap_seq_list_init(struct l2cap_seq_list *seq_list, u16 size) +{ + size_t alloc_size, i; + + /* Allocated size is a power of 2 to map sequence numbers + * (which may be up to 14 bits) in to a smaller array that is + * sized for the negotiated ERTM transmit windows. + */ + alloc_size = roundup_pow_of_two(size); + + seq_list->list = kmalloc(sizeof(u16) * alloc_size, GFP_KERNEL); + if (!seq_list->list) + return -ENOMEM; + + seq_list->mask = alloc_size - 1; + seq_list->head = L2CAP_SEQ_LIST_CLEAR; + seq_list->tail = L2CAP_SEQ_LIST_CLEAR; + for (i = 0; i < alloc_size; i++) + seq_list->list[i] = L2CAP_SEQ_LIST_CLEAR; + + return 0; +} + +static inline void l2cap_seq_list_free(struct l2cap_seq_list *seq_list) +{ + kfree(seq_list->list); +} + +static inline bool l2cap_seq_list_contains(struct l2cap_seq_list *seq_list, + u16 seq) +{ + /* Constant-time check for list membership */ + return seq_list->list[seq & seq_list->mask] != L2CAP_SEQ_LIST_CLEAR; +} + +static inline u16 l2cap_seq_list_pop(struct l2cap_seq_list *seq_list) +{ + u16 seq = seq_list->head; + u16 mask = seq_list->mask; + + seq_list->head = seq_list->list[seq & mask]; + seq_list->list[seq & mask] = L2CAP_SEQ_LIST_CLEAR; + + if (seq_list->head == L2CAP_SEQ_LIST_TAIL) { + seq_list->head = L2CAP_SEQ_LIST_CLEAR; + seq_list->tail = L2CAP_SEQ_LIST_CLEAR; + } + + return seq; +} + +static void l2cap_seq_list_clear(struct l2cap_seq_list *seq_list) +{ + u16 i; + + if (seq_list->head == L2CAP_SEQ_LIST_CLEAR) + return; + + for (i = 0; i <= seq_list->mask; i++) + seq_list->list[i] = L2CAP_SEQ_LIST_CLEAR; + + seq_list->head = L2CAP_SEQ_LIST_CLEAR; + seq_list->tail = L2CAP_SEQ_LIST_CLEAR; +} + +static void l2cap_seq_list_append(struct l2cap_seq_list *seq_list, u16 seq) +{ + u16 mask = seq_list->mask; + + /* All appends happen in constant time */ + + if (seq_list->list[seq & mask] != L2CAP_SEQ_LIST_CLEAR) + return; + + if (seq_list->tail == L2CAP_SEQ_LIST_CLEAR) + seq_list->head = seq; + else + seq_list->list[seq_list->tail & mask] = seq; + + seq_list->tail = seq; + seq_list->list[seq & mask] = L2CAP_SEQ_LIST_TAIL; +} + +static void l2cap_chan_timeout(struct work_struct *work) +{ + struct l2cap_chan *chan = container_of(work, struct l2cap_chan, + chan_timer.work); + struct l2cap_conn *conn = chan->conn; + int reason; + + BT_DBG("chan %p state %s", chan, state_to_string(chan->state)); + + mutex_lock(&conn->chan_lock); + l2cap_chan_lock(chan); + + if (chan->state == BT_CONNECTED || chan->state == BT_CONFIG) + reason = ECONNREFUSED; + else if (chan->state == BT_CONNECT && + chan->sec_level != BT_SECURITY_SDP) + reason = ECONNREFUSED; + else + reason = ETIMEDOUT; + + l2cap_chan_close(chan, reason); + + l2cap_chan_unlock(chan); + + chan->ops->close(chan); + mutex_unlock(&conn->chan_lock); + + l2cap_chan_put(chan); +} + +struct l2cap_chan *l2cap_chan_create(void) { - sock_hold(sk); + struct l2cap_chan *chan; + + chan = kzalloc(sizeof(*chan), GFP_ATOMIC); + if (!chan) + return NULL; + + mutex_init(&chan->lock); + + write_lock(&chan_list_lock); + list_add(&chan->global_l, &chan_list); + write_unlock(&chan_list_lock); + + INIT_DELAYED_WORK(&chan->chan_timer, l2cap_chan_timeout); + + chan->state = BT_OPEN; + + kref_init(&chan->kref); - if (l->head) - l2cap_pi(l->head)->prev_c = sk; + /* This flag is cleared in l2cap_chan_ready() */ + set_bit(CONF_NOT_COMPLETE, &chan->conf_state); - l2cap_pi(sk)->next_c = l->head; - l2cap_pi(sk)->prev_c = NULL; - l->head = sk; + BT_DBG("chan %p", chan); + + return chan; } -static inline void l2cap_chan_unlink(struct l2cap_chan_list *l, struct sock *sk) +static void l2cap_chan_destroy(struct kref *kref) { - struct sock *next = l2cap_pi(sk)->next_c, *prev = l2cap_pi(sk)->prev_c; + struct l2cap_chan *chan = container_of(kref, struct l2cap_chan, kref); + + BT_DBG("chan %p", chan); - write_lock_bh(&l->lock); - if (sk == l->head) - l->head = next; + write_lock(&chan_list_lock); + list_del(&chan->global_l); + write_unlock(&chan_list_lock); - if (next) - l2cap_pi(next)->prev_c = prev; - if (prev) - l2cap_pi(prev)->next_c = next; - write_unlock_bh(&l->lock); + kfree(chan); +} + +void l2cap_chan_hold(struct l2cap_chan *c) +{ + BT_DBG("chan %p orig refcnt %d", c, atomic_read(&c->kref.refcount)); - __sock_put(sk); + kref_get(&c->kref); } -static void __l2cap_chan_add(struct l2cap_conn *conn, struct sock *sk, struct sock *parent) +void l2cap_chan_put(struct l2cap_chan *c) { - struct l2cap_chan_list *l = &conn->chan_list; + BT_DBG("chan %p orig refcnt %d", c, atomic_read(&c->kref.refcount)); + kref_put(&c->kref, l2cap_chan_destroy); +} + +void l2cap_chan_set_defaults(struct l2cap_chan *chan) +{ + chan->fcs = L2CAP_FCS_CRC16; + chan->max_tx = L2CAP_DEFAULT_MAX_TX; + chan->tx_win = L2CAP_DEFAULT_TX_WINDOW; + chan->tx_win_max = L2CAP_DEFAULT_TX_WINDOW; + chan->remote_max_tx = chan->max_tx; + chan->remote_tx_win = chan->tx_win; + chan->ack_win = L2CAP_DEFAULT_TX_WINDOW; + chan->sec_level = BT_SECURITY_LOW; + chan->flush_to = L2CAP_DEFAULT_FLUSH_TO; + chan->retrans_timeout = L2CAP_DEFAULT_RETRANS_TO; + chan->monitor_timeout = L2CAP_DEFAULT_MONITOR_TO; + chan->conf_state = 0; + + set_bit(FLAG_FORCE_ACTIVE, &chan->flags); +} + +static void l2cap_le_flowctl_init(struct l2cap_chan *chan) +{ + chan->sdu = NULL; + chan->sdu_last_frag = NULL; + chan->sdu_len = 0; + chan->tx_credits = 0; + chan->rx_credits = le_max_credits; + chan->mps = min_t(u16, chan->imtu, le_default_mps); + + skb_queue_head_init(&chan->tx_q); +} + +void __l2cap_chan_add(struct l2cap_conn *conn, struct l2cap_chan *chan) +{ BT_DBG("conn %p, psm 0x%2.2x, dcid 0x%4.4x", conn, - l2cap_pi(sk)->psm, l2cap_pi(sk)->dcid); + __le16_to_cpu(chan->psm), chan->dcid); - conn->disc_reason = 0x13; + conn->disc_reason = HCI_ERROR_REMOTE_USER_TERM; - l2cap_pi(sk)->conn = conn; + chan->conn = conn; - if (sk->sk_type == SOCK_SEQPACKET || sk->sk_type == SOCK_STREAM) { - if (conn->hcon->type == LE_LINK) { - /* LE connection */ - l2cap_pi(sk)->omtu = L2CAP_LE_DEFAULT_MTU; - l2cap_pi(sk)->scid = L2CAP_CID_LE_DATA; - l2cap_pi(sk)->dcid = L2CAP_CID_LE_DATA; - } else { - /* Alloc CID for connection-oriented socket */ - l2cap_pi(sk)->scid = l2cap_alloc_cid(l); - l2cap_pi(sk)->omtu = L2CAP_DEFAULT_MTU; - } - } else if (sk->sk_type == SOCK_DGRAM) { + switch (chan->chan_type) { + case L2CAP_CHAN_CONN_ORIENTED: + /* Alloc CID for connection-oriented socket */ + chan->scid = l2cap_alloc_cid(conn); + if (conn->hcon->type == ACL_LINK) + chan->omtu = L2CAP_DEFAULT_MTU; + break; + + case L2CAP_CHAN_CONN_LESS: /* Connectionless socket */ - l2cap_pi(sk)->scid = L2CAP_CID_CONN_LESS; - l2cap_pi(sk)->dcid = L2CAP_CID_CONN_LESS; - l2cap_pi(sk)->omtu = L2CAP_DEFAULT_MTU; - } else { + chan->scid = L2CAP_CID_CONN_LESS; + chan->dcid = L2CAP_CID_CONN_LESS; + chan->omtu = L2CAP_DEFAULT_MTU; + break; + + case L2CAP_CHAN_FIXED: + /* Caller will set CID and CID specific MTU values */ + break; + + default: /* Raw socket can send/recv signalling messages only */ - l2cap_pi(sk)->scid = L2CAP_CID_SIGNALING; - l2cap_pi(sk)->dcid = L2CAP_CID_SIGNALING; - l2cap_pi(sk)->omtu = L2CAP_DEFAULT_MTU; + chan->scid = L2CAP_CID_SIGNALING; + chan->dcid = L2CAP_CID_SIGNALING; + chan->omtu = L2CAP_DEFAULT_MTU; } - __l2cap_chan_link(l, sk); + chan->local_id = L2CAP_BESTEFFORT_ID; + chan->local_stype = L2CAP_SERV_BESTEFFORT; + chan->local_msdu = L2CAP_DEFAULT_MAX_SDU_SIZE; + chan->local_sdu_itime = L2CAP_DEFAULT_SDU_ITIME; + chan->local_acc_lat = L2CAP_DEFAULT_ACC_LAT; + chan->local_flush_to = L2CAP_EFS_DEFAULT_FLUSH_TO; + + l2cap_chan_hold(chan); + + hci_conn_hold(conn->hcon); - if (parent) - bt_accept_enqueue(parent, sk); + list_add(&chan->list, &conn->chan_l); } -/* Delete channel. - * Must be called on the locked socket. */ -void l2cap_chan_del(struct sock *sk, int err) +void l2cap_chan_add(struct l2cap_conn *conn, struct l2cap_chan *chan) { - struct l2cap_conn *conn = l2cap_pi(sk)->conn; - struct sock *parent = bt_sk(sk)->parent; + mutex_lock(&conn->chan_lock); + __l2cap_chan_add(conn, chan); + mutex_unlock(&conn->chan_lock); +} - l2cap_sock_clear_timer(sk); +void l2cap_chan_del(struct l2cap_chan *chan, int err) +{ + struct l2cap_conn *conn = chan->conn; - BT_DBG("sk %p, conn %p, err %d", sk, conn, err); + __clear_chan_timer(chan); + + BT_DBG("chan %p, conn %p, err %d", chan, conn, err); if (conn) { - /* Unlink from channel list */ - l2cap_chan_unlink(&conn->chan_list, sk); - l2cap_pi(sk)->conn = NULL; - hci_conn_put(conn->hcon); + struct amp_mgr *mgr = conn->hcon->amp_mgr; + /* Delete from channel list */ + list_del(&chan->list); + + l2cap_chan_put(chan); + + chan->conn = NULL; + + if (chan->scid != L2CAP_CID_A2MP) + hci_conn_drop(conn->hcon); + + if (mgr && mgr->bredr_chan == chan) + mgr->bredr_chan = NULL; } - sk->sk_state = BT_CLOSED; - sock_set_flag(sk, SOCK_ZAPPED); + if (chan->hs_hchan) { + struct hci_chan *hs_hchan = chan->hs_hchan; - if (err) - sk->sk_err = err; + BT_DBG("chan %p disconnect hs_hchan %p", chan, hs_hchan); + amp_disconnect_logical_link(hs_hchan); + } + + chan->ops->teardown(chan, err); + + if (test_bit(CONF_NOT_COMPLETE, &chan->conf_state)) + return; + + switch(chan->mode) { + case L2CAP_MODE_BASIC: + break; + + case L2CAP_MODE_LE_FLOWCTL: + skb_queue_purge(&chan->tx_q); + break; + + case L2CAP_MODE_ERTM: + __clear_retrans_timer(chan); + __clear_monitor_timer(chan); + __clear_ack_timer(chan); + + skb_queue_purge(&chan->srej_q); + + l2cap_seq_list_free(&chan->srej_list); + l2cap_seq_list_free(&chan->retrans_list); + + /* fall through */ + + case L2CAP_MODE_STREAMING: + skb_queue_purge(&chan->tx_q); + break; + } + + return; +} + +void l2cap_conn_update_id_addr(struct hci_conn *hcon) +{ + struct l2cap_conn *conn = hcon->l2cap_data; + struct l2cap_chan *chan; + + mutex_lock(&conn->chan_lock); + + list_for_each_entry(chan, &conn->chan_l, list) { + l2cap_chan_lock(chan); + bacpy(&chan->dst, &hcon->dst); + chan->dst_type = bdaddr_type(hcon, hcon->dst_type); + l2cap_chan_unlock(chan); + } + + mutex_unlock(&conn->chan_lock); +} + +static void l2cap_chan_le_connect_reject(struct l2cap_chan *chan) +{ + struct l2cap_conn *conn = chan->conn; + struct l2cap_le_conn_rsp rsp; + u16 result; + + if (test_bit(FLAG_DEFER_SETUP, &chan->flags)) + result = L2CAP_CR_AUTHORIZATION; + else + result = L2CAP_CR_BAD_PSM; + + l2cap_state_change(chan, BT_DISCONN); - if (parent) { - bt_accept_unlink(sk); - parent->sk_data_ready(parent, 0); - } else - sk->sk_state_change(sk); + rsp.dcid = cpu_to_le16(chan->scid); + rsp.mtu = cpu_to_le16(chan->imtu); + rsp.mps = cpu_to_le16(chan->mps); + rsp.credits = cpu_to_le16(chan->rx_credits); + rsp.result = cpu_to_le16(result); - skb_queue_purge(TX_QUEUE(sk)); + l2cap_send_cmd(conn, chan->ident, L2CAP_LE_CONN_RSP, sizeof(rsp), + &rsp); +} - if (l2cap_pi(sk)->mode == L2CAP_MODE_ERTM) { - struct srej_list *l, *tmp; +static void l2cap_chan_connect_reject(struct l2cap_chan *chan) +{ + struct l2cap_conn *conn = chan->conn; + struct l2cap_conn_rsp rsp; + u16 result; - del_timer(&l2cap_pi(sk)->retrans_timer); - del_timer(&l2cap_pi(sk)->monitor_timer); - del_timer(&l2cap_pi(sk)->ack_timer); + if (test_bit(FLAG_DEFER_SETUP, &chan->flags)) + result = L2CAP_CR_SEC_BLOCK; + else + result = L2CAP_CR_BAD_PSM; + + l2cap_state_change(chan, BT_DISCONN); - skb_queue_purge(SREJ_QUEUE(sk)); - skb_queue_purge(BUSY_QUEUE(sk)); + rsp.scid = cpu_to_le16(chan->dcid); + rsp.dcid = cpu_to_le16(chan->scid); + rsp.result = cpu_to_le16(result); + rsp.status = cpu_to_le16(L2CAP_CS_NO_INFO); - list_for_each_entry_safe(l, tmp, SREJ_LIST(sk), list) { - list_del(&l->list); - kfree(l); + l2cap_send_cmd(conn, chan->ident, L2CAP_CONN_RSP, sizeof(rsp), &rsp); +} + +void l2cap_chan_close(struct l2cap_chan *chan, int reason) +{ + struct l2cap_conn *conn = chan->conn; + + BT_DBG("chan %p state %s", chan, state_to_string(chan->state)); + + switch (chan->state) { + case BT_LISTEN: + chan->ops->teardown(chan, 0); + break; + + case BT_CONNECTED: + case BT_CONFIG: + if (chan->chan_type == L2CAP_CHAN_CONN_ORIENTED) { + __set_chan_timer(chan, chan->ops->get_sndtimeo(chan)); + l2cap_send_disconn_req(chan, reason); + } else + l2cap_chan_del(chan, reason); + break; + + case BT_CONNECT2: + if (chan->chan_type == L2CAP_CHAN_CONN_ORIENTED) { + if (conn->hcon->type == ACL_LINK) + l2cap_chan_connect_reject(chan); + else if (conn->hcon->type == LE_LINK) + l2cap_chan_le_connect_reject(chan); } + + l2cap_chan_del(chan, reason); + break; + + case BT_CONNECT: + case BT_DISCONN: + l2cap_chan_del(chan, reason); + break; + + default: + chan->ops->teardown(chan, 0); + break; } } -static inline u8 l2cap_get_auth_type(struct sock *sk) +static inline u8 l2cap_get_auth_type(struct l2cap_chan *chan) { - if (sk->sk_type == SOCK_RAW) { - switch (l2cap_pi(sk)->sec_level) { + switch (chan->chan_type) { + case L2CAP_CHAN_RAW: + switch (chan->sec_level) { case BT_SECURITY_HIGH: + case BT_SECURITY_FIPS: return HCI_AT_DEDICATED_BONDING_MITM; case BT_SECURITY_MEDIUM: return HCI_AT_DEDICATED_BONDING; default: return HCI_AT_NO_BONDING; } - } else if (l2cap_pi(sk)->psm == cpu_to_le16(0x0001)) { - if (l2cap_pi(sk)->sec_level == BT_SECURITY_LOW) - l2cap_pi(sk)->sec_level = BT_SECURITY_SDP; - - if (l2cap_pi(sk)->sec_level == BT_SECURITY_HIGH) + break; + case L2CAP_CHAN_CONN_LESS: + if (chan->psm == cpu_to_le16(L2CAP_PSM_3DSP)) { + if (chan->sec_level == BT_SECURITY_LOW) + chan->sec_level = BT_SECURITY_SDP; + } + if (chan->sec_level == BT_SECURITY_HIGH || + chan->sec_level == BT_SECURITY_FIPS) return HCI_AT_NO_BONDING_MITM; else return HCI_AT_NO_BONDING; - } else { - switch (l2cap_pi(sk)->sec_level) { + break; + case L2CAP_CHAN_CONN_ORIENTED: + if (chan->psm == cpu_to_le16(L2CAP_PSM_SDP)) { + if (chan->sec_level == BT_SECURITY_LOW) + chan->sec_level = BT_SECURITY_SDP; + + if (chan->sec_level == BT_SECURITY_HIGH || + chan->sec_level == BT_SECURITY_FIPS) + return HCI_AT_NO_BONDING_MITM; + else + return HCI_AT_NO_BONDING; + } + /* fall through */ + default: + switch (chan->sec_level) { case BT_SECURITY_HIGH: + case BT_SECURITY_FIPS: return HCI_AT_GENERAL_BONDING_MITM; case BT_SECURITY_MEDIUM: return HCI_AT_GENERAL_BONDING; default: return HCI_AT_NO_BONDING; } + break; } } /* Service level security */ -static inline int l2cap_check_security(struct sock *sk) +int l2cap_chan_check_security(struct l2cap_chan *chan) { - struct l2cap_conn *conn = l2cap_pi(sk)->conn; + struct l2cap_conn *conn = chan->conn; __u8 auth_type; - auth_type = l2cap_get_auth_type(sk); + if (conn->hcon->type == LE_LINK) + return smp_conn_security(conn->hcon, chan->sec_level); + + auth_type = l2cap_get_auth_type(chan); - return hci_conn_security(conn->hcon, l2cap_pi(sk)->sec_level, - auth_type); + return hci_conn_security(conn->hcon, chan->sec_level, auth_type); } -u8 l2cap_get_ident(struct l2cap_conn *conn) +static u8 l2cap_get_ident(struct l2cap_conn *conn) { u8 id; @@ -311,19 +793,20 @@ u8 l2cap_get_ident(struct l2cap_conn *conn) * 200 - 254 are used by utilities like l2ping, etc. */ - spin_lock_bh(&conn->lock); + spin_lock(&conn->lock); if (++conn->tx_ident > 128) conn->tx_ident = 1; id = conn->tx_ident; - spin_unlock_bh(&conn->lock); + spin_unlock(&conn->lock); return id; } -void l2cap_send_cmd(struct l2cap_conn *conn, u8 ident, u8 code, u16 len, void *data) +static 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); u8 flags; @@ -338,97 +821,461 @@ void l2cap_send_cmd(struct l2cap_conn *conn, u8 ident, u8 code, u16 len, void *d else flags = ACL_START; - hci_send_acl(conn->hcon, skb, flags); + bt_cb(skb)->force_active = BT_POWER_FORCE_ACTIVE_ON; + skb->priority = HCI_PRIO_MAX; + + hci_send_acl(conn->hchan, skb, flags); } -static inline void l2cap_send_sframe(struct l2cap_pinfo *pi, u16 control) +static bool __chan_is_moving(struct l2cap_chan *chan) { - struct sk_buff *skb; - struct l2cap_hdr *lh; - struct l2cap_conn *conn = pi->conn; - struct sock *sk = (struct sock *)pi; - int count, hlen = L2CAP_HDR_SIZE + 2; - u8 flags; + return chan->move_state != L2CAP_MOVE_STABLE && + chan->move_state != L2CAP_MOVE_WAIT_PREPARE; +} + +static void l2cap_do_send(struct l2cap_chan *chan, struct sk_buff *skb) +{ + struct hci_conn *hcon = chan->conn->hcon; + u16 flags; + + BT_DBG("chan %p, skb %p len %d priority %u", chan, skb, skb->len, + skb->priority); + + if (chan->hs_hcon && !__chan_is_moving(chan)) { + if (chan->hs_hchan) + hci_send_acl(chan->hs_hchan, skb, ACL_COMPLETE); + else + kfree_skb(skb); - if (sk->sk_state != BT_CONNECTED) return; + } - if (pi->fcs == L2CAP_FCS_CRC16) - hlen += 2; + if (!test_bit(FLAG_FLUSHABLE, &chan->flags) && + lmp_no_flush_capable(hcon->hdev)) + flags = ACL_START_NO_FLUSH; + else + flags = ACL_START; - BT_DBG("pi %p, control 0x%2.2x", pi, control); + bt_cb(skb)->force_active = test_bit(FLAG_FORCE_ACTIVE, &chan->flags); + hci_send_acl(chan->conn->hchan, skb, flags); +} - count = min_t(unsigned int, conn->mtu, hlen); - control |= L2CAP_CTRL_FRAME_TYPE; +static void __unpack_enhanced_control(u16 enh, struct l2cap_ctrl *control) +{ + control->reqseq = (enh & L2CAP_CTRL_REQSEQ) >> L2CAP_CTRL_REQSEQ_SHIFT; + control->final = (enh & L2CAP_CTRL_FINAL) >> L2CAP_CTRL_FINAL_SHIFT; - if (pi->conn_state & L2CAP_CONN_SEND_FBIT) { - control |= L2CAP_CTRL_FINAL; - pi->conn_state &= ~L2CAP_CONN_SEND_FBIT; + if (enh & L2CAP_CTRL_FRAME_TYPE) { + /* S-Frame */ + control->sframe = 1; + control->poll = (enh & L2CAP_CTRL_POLL) >> L2CAP_CTRL_POLL_SHIFT; + control->super = (enh & L2CAP_CTRL_SUPERVISE) >> L2CAP_CTRL_SUPER_SHIFT; + + control->sar = 0; + control->txseq = 0; + } else { + /* I-Frame */ + control->sframe = 0; + control->sar = (enh & L2CAP_CTRL_SAR) >> L2CAP_CTRL_SAR_SHIFT; + control->txseq = (enh & L2CAP_CTRL_TXSEQ) >> L2CAP_CTRL_TXSEQ_SHIFT; + + control->poll = 0; + control->super = 0; } +} - if (pi->conn_state & L2CAP_CONN_SEND_PBIT) { - control |= L2CAP_CTRL_POLL; - pi->conn_state &= ~L2CAP_CONN_SEND_PBIT; +static void __unpack_extended_control(u32 ext, struct l2cap_ctrl *control) +{ + control->reqseq = (ext & L2CAP_EXT_CTRL_REQSEQ) >> L2CAP_EXT_CTRL_REQSEQ_SHIFT; + control->final = (ext & L2CAP_EXT_CTRL_FINAL) >> L2CAP_EXT_CTRL_FINAL_SHIFT; + + if (ext & L2CAP_EXT_CTRL_FRAME_TYPE) { + /* S-Frame */ + control->sframe = 1; + control->poll = (ext & L2CAP_EXT_CTRL_POLL) >> L2CAP_EXT_CTRL_POLL_SHIFT; + control->super = (ext & L2CAP_EXT_CTRL_SUPERVISE) >> L2CAP_EXT_CTRL_SUPER_SHIFT; + + control->sar = 0; + control->txseq = 0; + } else { + /* I-Frame */ + control->sframe = 0; + control->sar = (ext & L2CAP_EXT_CTRL_SAR) >> L2CAP_EXT_CTRL_SAR_SHIFT; + control->txseq = (ext & L2CAP_EXT_CTRL_TXSEQ) >> L2CAP_EXT_CTRL_TXSEQ_SHIFT; + + control->poll = 0; + control->super = 0; + } +} + +static inline void __unpack_control(struct l2cap_chan *chan, + struct sk_buff *skb) +{ + if (test_bit(FLAG_EXT_CTRL, &chan->flags)) { + __unpack_extended_control(get_unaligned_le32(skb->data), + &bt_cb(skb)->control); + skb_pull(skb, L2CAP_EXT_CTRL_SIZE); + } else { + __unpack_enhanced_control(get_unaligned_le16(skb->data), + &bt_cb(skb)->control); + skb_pull(skb, L2CAP_ENH_CTRL_SIZE); + } +} + +static u32 __pack_extended_control(struct l2cap_ctrl *control) +{ + u32 packed; + + packed = control->reqseq << L2CAP_EXT_CTRL_REQSEQ_SHIFT; + packed |= control->final << L2CAP_EXT_CTRL_FINAL_SHIFT; + + if (control->sframe) { + packed |= control->poll << L2CAP_EXT_CTRL_POLL_SHIFT; + packed |= control->super << L2CAP_EXT_CTRL_SUPER_SHIFT; + packed |= L2CAP_EXT_CTRL_FRAME_TYPE; + } else { + packed |= control->sar << L2CAP_EXT_CTRL_SAR_SHIFT; + packed |= control->txseq << L2CAP_EXT_CTRL_TXSEQ_SHIFT; } - skb = bt_skb_alloc(count, GFP_ATOMIC); + return packed; +} + +static u16 __pack_enhanced_control(struct l2cap_ctrl *control) +{ + u16 packed; + + packed = control->reqseq << L2CAP_CTRL_REQSEQ_SHIFT; + packed |= control->final << L2CAP_CTRL_FINAL_SHIFT; + + if (control->sframe) { + packed |= control->poll << L2CAP_CTRL_POLL_SHIFT; + packed |= control->super << L2CAP_CTRL_SUPER_SHIFT; + packed |= L2CAP_CTRL_FRAME_TYPE; + } else { + packed |= control->sar << L2CAP_CTRL_SAR_SHIFT; + packed |= control->txseq << L2CAP_CTRL_TXSEQ_SHIFT; + } + + return packed; +} + +static inline void __pack_control(struct l2cap_chan *chan, + struct l2cap_ctrl *control, + struct sk_buff *skb) +{ + if (test_bit(FLAG_EXT_CTRL, &chan->flags)) { + put_unaligned_le32(__pack_extended_control(control), + skb->data + L2CAP_HDR_SIZE); + } else { + put_unaligned_le16(__pack_enhanced_control(control), + skb->data + L2CAP_HDR_SIZE); + } +} + +static inline unsigned int __ertm_hdr_size(struct l2cap_chan *chan) +{ + if (test_bit(FLAG_EXT_CTRL, &chan->flags)) + return L2CAP_EXT_HDR_SIZE; + else + return L2CAP_ENH_HDR_SIZE; +} + +static struct sk_buff *l2cap_create_sframe_pdu(struct l2cap_chan *chan, + u32 control) +{ + struct sk_buff *skb; + struct l2cap_hdr *lh; + int hlen = __ertm_hdr_size(chan); + + if (chan->fcs == L2CAP_FCS_CRC16) + hlen += L2CAP_FCS_SIZE; + + skb = bt_skb_alloc(hlen, GFP_KERNEL); + if (!skb) - return; + return ERR_PTR(-ENOMEM); lh = (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE); lh->len = cpu_to_le16(hlen - L2CAP_HDR_SIZE); - lh->cid = cpu_to_le16(pi->dcid); - put_unaligned_le16(control, skb_put(skb, 2)); + lh->cid = cpu_to_le16(chan->dcid); - if (pi->fcs == L2CAP_FCS_CRC16) { - u16 fcs = crc16(0, (u8 *)lh, count - 2); - put_unaligned_le16(fcs, skb_put(skb, 2)); + if (test_bit(FLAG_EXT_CTRL, &chan->flags)) + put_unaligned_le32(control, skb_put(skb, L2CAP_EXT_CTRL_SIZE)); + else + put_unaligned_le16(control, skb_put(skb, L2CAP_ENH_CTRL_SIZE)); + + if (chan->fcs == L2CAP_FCS_CRC16) { + u16 fcs = crc16(0, (u8 *)skb->data, skb->len); + put_unaligned_le16(fcs, skb_put(skb, L2CAP_FCS_SIZE)); } - if (lmp_no_flush_capable(conn->hcon->hdev)) - flags = ACL_START_NO_FLUSH; + skb->priority = HCI_PRIO_MAX; + return skb; +} + +static void l2cap_send_sframe(struct l2cap_chan *chan, + struct l2cap_ctrl *control) +{ + struct sk_buff *skb; + u32 control_field; + + BT_DBG("chan %p, control %p", chan, control); + + if (!control->sframe) + return; + + if (__chan_is_moving(chan)) + return; + + if (test_and_clear_bit(CONN_SEND_FBIT, &chan->conn_state) && + !control->poll) + control->final = 1; + + if (control->super == L2CAP_SUPER_RR) + clear_bit(CONN_RNR_SENT, &chan->conn_state); + else if (control->super == L2CAP_SUPER_RNR) + set_bit(CONN_RNR_SENT, &chan->conn_state); + + if (control->super != L2CAP_SUPER_SREJ) { + chan->last_acked_seq = control->reqseq; + __clear_ack_timer(chan); + } + + BT_DBG("reqseq %d, final %d, poll %d, super %d", control->reqseq, + control->final, control->poll, control->super); + + if (test_bit(FLAG_EXT_CTRL, &chan->flags)) + control_field = __pack_extended_control(control); else - flags = ACL_START; + control_field = __pack_enhanced_control(control); + + skb = l2cap_create_sframe_pdu(chan, control_field); + if (!IS_ERR(skb)) + l2cap_do_send(chan, skb); +} + +static void l2cap_send_rr_or_rnr(struct l2cap_chan *chan, bool poll) +{ + struct l2cap_ctrl control; + + BT_DBG("chan %p, poll %d", chan, poll); + + memset(&control, 0, sizeof(control)); + control.sframe = 1; + control.poll = poll; + + if (test_bit(CONN_LOCAL_BUSY, &chan->conn_state)) + control.super = L2CAP_SUPER_RNR; + else + control.super = L2CAP_SUPER_RR; + + control.reqseq = chan->buffer_seq; + l2cap_send_sframe(chan, &control); +} - hci_send_acl(pi->conn->hcon, skb, flags); +static inline int __l2cap_no_conn_pending(struct l2cap_chan *chan) +{ + return !test_bit(CONF_CONNECT_PEND, &chan->conf_state); +} + +static bool __amp_capable(struct l2cap_chan *chan) +{ + struct l2cap_conn *conn = chan->conn; + struct hci_dev *hdev; + bool amp_available = false; + + if (!conn->hs_enabled) + return false; + + if (!(conn->fixed_chan_mask & L2CAP_FC_A2MP)) + return false; + + read_lock(&hci_dev_list_lock); + list_for_each_entry(hdev, &hci_dev_list, list) { + if (hdev->amp_type != AMP_TYPE_BREDR && + test_bit(HCI_UP, &hdev->flags)) { + amp_available = true; + break; + } + } + read_unlock(&hci_dev_list_lock); + + if (chan->chan_policy == BT_CHANNEL_POLICY_AMP_PREFERRED) + return amp_available; + + return false; +} + +static bool l2cap_check_efs(struct l2cap_chan *chan) +{ + /* Check EFS parameters */ + return true; +} + +void l2cap_send_conn_req(struct l2cap_chan *chan) +{ + struct l2cap_conn *conn = chan->conn; + struct l2cap_conn_req req; + + req.scid = cpu_to_le16(chan->scid); + req.psm = chan->psm; + + chan->ident = l2cap_get_ident(conn); + + set_bit(CONF_CONNECT_PEND, &chan->conf_state); + + l2cap_send_cmd(conn, chan->ident, L2CAP_CONN_REQ, sizeof(req), &req); +} + +static void l2cap_send_create_chan_req(struct l2cap_chan *chan, u8 amp_id) +{ + struct l2cap_create_chan_req req; + req.scid = cpu_to_le16(chan->scid); + req.psm = chan->psm; + req.amp_id = amp_id; + + chan->ident = l2cap_get_ident(chan->conn); + + l2cap_send_cmd(chan->conn, chan->ident, L2CAP_CREATE_CHAN_REQ, + sizeof(req), &req); +} + +static void l2cap_move_setup(struct l2cap_chan *chan) +{ + struct sk_buff *skb; + + BT_DBG("chan %p", chan); + + if (chan->mode != L2CAP_MODE_ERTM) + return; + + __clear_retrans_timer(chan); + __clear_monitor_timer(chan); + __clear_ack_timer(chan); + + chan->retry_count = 0; + skb_queue_walk(&chan->tx_q, skb) { + if (bt_cb(skb)->control.retries) + bt_cb(skb)->control.retries = 1; + else + break; + } + + chan->expected_tx_seq = chan->buffer_seq; + + clear_bit(CONN_REJ_ACT, &chan->conn_state); + clear_bit(CONN_SREJ_ACT, &chan->conn_state); + l2cap_seq_list_clear(&chan->retrans_list); + l2cap_seq_list_clear(&chan->srej_list); + skb_queue_purge(&chan->srej_q); + + chan->tx_state = L2CAP_TX_STATE_XMIT; + chan->rx_state = L2CAP_RX_STATE_MOVE; + + set_bit(CONN_REMOTE_BUSY, &chan->conn_state); } -static inline void l2cap_send_rr_or_rnr(struct l2cap_pinfo *pi, u16 control) +static void l2cap_move_done(struct l2cap_chan *chan) { - if (pi->conn_state & L2CAP_CONN_LOCAL_BUSY) { - control |= L2CAP_SUPER_RCV_NOT_READY; - pi->conn_state |= L2CAP_CONN_RNR_SENT; - } else - control |= L2CAP_SUPER_RCV_READY; + u8 move_role = chan->move_role; + BT_DBG("chan %p", chan); - control |= pi->buffer_seq << L2CAP_CTRL_REQSEQ_SHIFT; + chan->move_state = L2CAP_MOVE_STABLE; + chan->move_role = L2CAP_MOVE_ROLE_NONE; - l2cap_send_sframe(pi, control); + if (chan->mode != L2CAP_MODE_ERTM) + return; + + switch (move_role) { + case L2CAP_MOVE_ROLE_INITIATOR: + l2cap_tx(chan, NULL, NULL, L2CAP_EV_EXPLICIT_POLL); + chan->rx_state = L2CAP_RX_STATE_WAIT_F; + break; + case L2CAP_MOVE_ROLE_RESPONDER: + chan->rx_state = L2CAP_RX_STATE_WAIT_P; + break; + } } -static inline int __l2cap_no_conn_pending(struct sock *sk) +static void l2cap_chan_ready(struct l2cap_chan *chan) { - return !(l2cap_pi(sk)->conf_state & L2CAP_CONF_CONNECT_PEND); + /* This clears all conf flags, including CONF_NOT_COMPLETE */ + chan->conf_state = 0; + __clear_chan_timer(chan); + + if (chan->mode == L2CAP_MODE_LE_FLOWCTL && !chan->tx_credits) + chan->ops->suspend(chan); + + chan->state = BT_CONNECTED; + + chan->ops->ready(chan); } -static void l2cap_do_start(struct sock *sk) +static void l2cap_le_connect(struct l2cap_chan *chan) { - struct l2cap_conn *conn = l2cap_pi(sk)->conn; + struct l2cap_conn *conn = chan->conn; + struct l2cap_le_conn_req req; + + if (test_and_set_bit(FLAG_LE_CONN_REQ_SENT, &chan->flags)) + return; + + req.psm = chan->psm; + req.scid = cpu_to_le16(chan->scid); + req.mtu = cpu_to_le16(chan->imtu); + req.mps = cpu_to_le16(chan->mps); + req.credits = cpu_to_le16(chan->rx_credits); + + chan->ident = l2cap_get_ident(conn); + + l2cap_send_cmd(conn, chan->ident, L2CAP_LE_CONN_REQ, + sizeof(req), &req); +} + +static void l2cap_le_start(struct l2cap_chan *chan) +{ + struct l2cap_conn *conn = chan->conn; + + if (!smp_conn_security(conn->hcon, chan->sec_level)) + return; + + if (!chan->psm) { + l2cap_chan_ready(chan); + return; + } + + if (chan->state == BT_CONNECT) + l2cap_le_connect(chan); +} + +static void l2cap_start_connection(struct l2cap_chan *chan) +{ + if (__amp_capable(chan)) { + BT_DBG("chan %p AMP capable: discover AMPs", chan); + a2mp_discover_amp(chan); + } else if (chan->conn->hcon->type == LE_LINK) { + l2cap_le_start(chan); + } else { + l2cap_send_conn_req(chan); + } +} + +static void l2cap_do_start(struct l2cap_chan *chan) +{ + struct l2cap_conn *conn = chan->conn; + + if (conn->hcon->type == LE_LINK) { + l2cap_le_start(chan); + return; + } if (conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_SENT) { if (!(conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_DONE)) return; - if (l2cap_check_security(sk) && __l2cap_no_conn_pending(sk)) { - struct l2cap_conn_req req; - req.scid = cpu_to_le16(l2cap_pi(sk)->scid); - req.psm = l2cap_pi(sk)->psm; - - l2cap_pi(sk)->ident = l2cap_get_ident(conn); - l2cap_pi(sk)->conf_state |= L2CAP_CONF_CONNECT_PEND; - - l2cap_send_cmd(conn, l2cap_pi(sk)->ident, - L2CAP_CONN_REQ, sizeof(req), &req); + if (l2cap_chan_check_security(chan) && + __l2cap_no_conn_pending(chan)) { + l2cap_start_connection(chan); } } else { struct l2cap_info_req req; @@ -437,11 +1284,10 @@ static void l2cap_do_start(struct sock *sk) conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_SENT; conn->info_ident = l2cap_get_ident(conn); - mod_timer(&conn->info_timer, jiffies + - msecs_to_jiffies(L2CAP_INFO_TIMEOUT)); + schedule_delayed_work(&conn->info_timer, L2CAP_INFO_TIMEOUT); - l2cap_send_cmd(conn, conn->info_ident, - L2CAP_INFO_REQ, sizeof(req), &req); + l2cap_send_cmd(conn, conn->info_ident, L2CAP_INFO_REQ, + sizeof(req), &req); } } @@ -461,97 +1307,81 @@ static inline int l2cap_mode_supported(__u8 mode, __u32 feat_mask) } } -void l2cap_send_disconn_req(struct l2cap_conn *conn, struct sock *sk, int err) +static void l2cap_send_disconn_req(struct l2cap_chan *chan, int err) { + struct l2cap_conn *conn = chan->conn; struct l2cap_disconn_req req; if (!conn) return; - skb_queue_purge(TX_QUEUE(sk)); + if (chan->mode == L2CAP_MODE_ERTM && chan->state == BT_CONNECTED) { + __clear_retrans_timer(chan); + __clear_monitor_timer(chan); + __clear_ack_timer(chan); + } - if (l2cap_pi(sk)->mode == L2CAP_MODE_ERTM) { - del_timer(&l2cap_pi(sk)->retrans_timer); - del_timer(&l2cap_pi(sk)->monitor_timer); - del_timer(&l2cap_pi(sk)->ack_timer); + if (chan->scid == L2CAP_CID_A2MP) { + l2cap_state_change(chan, BT_DISCONN); + return; } - req.dcid = cpu_to_le16(l2cap_pi(sk)->dcid); - req.scid = cpu_to_le16(l2cap_pi(sk)->scid); - l2cap_send_cmd(conn, l2cap_get_ident(conn), - L2CAP_DISCONN_REQ, sizeof(req), &req); + req.dcid = cpu_to_le16(chan->dcid); + req.scid = cpu_to_le16(chan->scid); + l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_DISCONN_REQ, + sizeof(req), &req); - sk->sk_state = BT_DISCONN; - sk->sk_err = err; + l2cap_state_change_and_error(chan, BT_DISCONN, err); } /* ---- L2CAP connections ---- */ static void l2cap_conn_start(struct l2cap_conn *conn) { - struct l2cap_chan_list *l = &conn->chan_list; - struct sock_del_list del, *tmp1, *tmp2; - struct sock *sk; + struct l2cap_chan *chan, *tmp; BT_DBG("conn %p", conn); - INIT_LIST_HEAD(&del.list); - - read_lock(&l->lock); + mutex_lock(&conn->chan_lock); - for (sk = l->head; sk; sk = l2cap_pi(sk)->next_c) { - bh_lock_sock(sk); + list_for_each_entry_safe(chan, tmp, &conn->chan_l, list) { + l2cap_chan_lock(chan); - if (sk->sk_type != SOCK_SEQPACKET && - sk->sk_type != SOCK_STREAM) { - bh_unlock_sock(sk); + if (chan->chan_type != L2CAP_CHAN_CONN_ORIENTED) { + l2cap_chan_unlock(chan); continue; } - if (sk->sk_state == BT_CONNECT) { - struct l2cap_conn_req req; - - if (!l2cap_check_security(sk) || - !__l2cap_no_conn_pending(sk)) { - bh_unlock_sock(sk); + if (chan->state == BT_CONNECT) { + if (!l2cap_chan_check_security(chan) || + !__l2cap_no_conn_pending(chan)) { + l2cap_chan_unlock(chan); continue; } - if (!l2cap_mode_supported(l2cap_pi(sk)->mode, - conn->feat_mask) - && l2cap_pi(sk)->conf_state & - L2CAP_CONF_STATE2_DEVICE) { - tmp1 = kzalloc(sizeof(struct sock_del_list), - GFP_ATOMIC); - tmp1->sk = sk; - list_add_tail(&tmp1->list, &del.list); - bh_unlock_sock(sk); + if (!l2cap_mode_supported(chan->mode, conn->feat_mask) + && test_bit(CONF_STATE2_DEVICE, + &chan->conf_state)) { + l2cap_chan_close(chan, ECONNRESET); + l2cap_chan_unlock(chan); continue; } - req.scid = cpu_to_le16(l2cap_pi(sk)->scid); - req.psm = l2cap_pi(sk)->psm; - - l2cap_pi(sk)->ident = l2cap_get_ident(conn); - l2cap_pi(sk)->conf_state |= L2CAP_CONF_CONNECT_PEND; + l2cap_start_connection(chan); - l2cap_send_cmd(conn, l2cap_pi(sk)->ident, - L2CAP_CONN_REQ, sizeof(req), &req); - - } else if (sk->sk_state == BT_CONNECT2) { + } else if (chan->state == BT_CONNECT2) { struct l2cap_conn_rsp rsp; char buf[128]; - rsp.scid = cpu_to_le16(l2cap_pi(sk)->dcid); - rsp.dcid = cpu_to_le16(l2cap_pi(sk)->scid); + rsp.scid = cpu_to_le16(chan->dcid); + rsp.dcid = cpu_to_le16(chan->scid); - if (l2cap_check_security(sk)) { - if (bt_sk(sk)->defer_setup) { - struct sock *parent = bt_sk(sk)->parent; + if (l2cap_chan_check_security(chan)) { + if (test_bit(FLAG_DEFER_SETUP, &chan->flags)) { rsp.result = cpu_to_le16(L2CAP_CR_PEND); rsp.status = cpu_to_le16(L2CAP_CS_AUTHOR_PEND); - parent->sk_data_ready(parent, 0); + chan->ops->defer(chan); } else { - sk->sk_state = BT_CONFIG; + l2cap_state_change(chan, BT_CONFIG); rsp.result = cpu_to_le16(L2CAP_CR_SUCCESS); rsp.status = cpu_to_le16(L2CAP_CS_NO_INFO); } @@ -560,167 +1390,176 @@ static void l2cap_conn_start(struct l2cap_conn *conn) rsp.status = cpu_to_le16(L2CAP_CS_AUTHEN_PEND); } - l2cap_send_cmd(conn, l2cap_pi(sk)->ident, - L2CAP_CONN_RSP, sizeof(rsp), &rsp); + l2cap_send_cmd(conn, chan->ident, L2CAP_CONN_RSP, + sizeof(rsp), &rsp); - if (l2cap_pi(sk)->conf_state & L2CAP_CONF_REQ_SENT || - rsp.result != L2CAP_CR_SUCCESS) { - bh_unlock_sock(sk); + if (test_bit(CONF_REQ_SENT, &chan->conf_state) || + rsp.result != L2CAP_CR_SUCCESS) { + l2cap_chan_unlock(chan); continue; } - l2cap_pi(sk)->conf_state |= L2CAP_CONF_REQ_SENT; + set_bit(CONF_REQ_SENT, &chan->conf_state); l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ, - l2cap_build_conf_req(sk, buf), buf); - l2cap_pi(sk)->num_conf_req++; + l2cap_build_conf_req(chan, buf), buf); + chan->num_conf_req++; } - bh_unlock_sock(sk); + l2cap_chan_unlock(chan); } - read_unlock(&l->lock); - - list_for_each_entry_safe(tmp1, tmp2, &del.list, list) { - bh_lock_sock(tmp1->sk); - __l2cap_sock_close(tmp1->sk, ECONNRESET); - bh_unlock_sock(tmp1->sk); - list_del(&tmp1->list); - kfree(tmp1); - } + mutex_unlock(&conn->chan_lock); } -/* Find socket with cid and source bdaddr. +/* Find socket with cid and source/destination bdaddr. * Returns closest match, locked. */ -static struct sock *l2cap_get_sock_by_scid(int state, __le16 cid, bdaddr_t *src) +static struct l2cap_chan *l2cap_global_chan_by_scid(int state, u16 cid, + bdaddr_t *src, + bdaddr_t *dst) { - struct sock *s, *sk = NULL, *sk1 = NULL; - struct hlist_node *node; + struct l2cap_chan *c, *c1 = NULL; - read_lock(&l2cap_sk_list.lock); + read_lock(&chan_list_lock); - sk_for_each(sk, node, &l2cap_sk_list.head) { - if (state && sk->sk_state != state) + list_for_each_entry(c, &chan_list, global_l) { + if (state && c->state != state) continue; - if (l2cap_pi(sk)->scid == cid) { + if (c->scid == cid) { + int src_match, dst_match; + int src_any, dst_any; + /* Exact match. */ - if (!bacmp(&bt_sk(sk)->src, src)) - break; + src_match = !bacmp(&c->src, src); + dst_match = !bacmp(&c->dst, dst); + if (src_match && dst_match) { + read_unlock(&chan_list_lock); + return c; + } /* Closest match */ - if (!bacmp(&bt_sk(sk)->src, BDADDR_ANY)) - sk1 = sk; + src_any = !bacmp(&c->src, BDADDR_ANY); + dst_any = !bacmp(&c->dst, BDADDR_ANY); + if ((src_match && dst_any) || (src_any && dst_match) || + (src_any && dst_any)) + c1 = c; } } - s = node ? sk : sk1; - if (s) - bh_lock_sock(s); - read_unlock(&l2cap_sk_list.lock); - return s; + read_unlock(&chan_list_lock); + + return c1; } static void l2cap_le_conn_ready(struct l2cap_conn *conn) { - struct l2cap_chan_list *list = &conn->chan_list; - struct sock *parent, *uninitialized_var(sk); + struct hci_conn *hcon = conn->hcon; + struct l2cap_chan *chan, *pchan; + u8 dst_type; BT_DBG(""); + bt_6lowpan_add_conn(conn); + /* Check if we have socket listening on cid */ - parent = l2cap_get_sock_by_scid(BT_LISTEN, L2CAP_CID_LE_DATA, - conn->src); - if (!parent) + pchan = l2cap_global_chan_by_scid(BT_LISTEN, L2CAP_CID_ATT, + &hcon->src, &hcon->dst); + if (!pchan) return; - /* Check for backlog size */ - if (sk_acceptq_is_full(parent)) { - BT_DBG("backlog full %d", parent->sk_ack_backlog); - goto clean; - } - - sk = l2cap_sock_alloc(sock_net(parent), NULL, BTPROTO_L2CAP, GFP_ATOMIC); - if (!sk) - goto clean; - - write_lock_bh(&list->lock); + /* Client ATT sockets should override the server one */ + if (__l2cap_get_chan_by_dcid(conn, L2CAP_CID_ATT)) + return; - hci_conn_hold(conn->hcon); + dst_type = bdaddr_type(hcon, hcon->dst_type); - l2cap_sock_init(sk, parent); - bacpy(&bt_sk(sk)->src, conn->src); - bacpy(&bt_sk(sk)->dst, conn->dst); + /* If device is blocked, do not create a channel for it */ + if (hci_blacklist_lookup(hcon->hdev, &hcon->dst, dst_type)) + return; - __l2cap_chan_add(conn, sk, parent); + l2cap_chan_lock(pchan); - l2cap_sock_set_timer(sk, sk->sk_sndtimeo); + chan = pchan->ops->new_connection(pchan); + if (!chan) + goto clean; - sk->sk_state = BT_CONNECTED; - parent->sk_data_ready(parent, 0); + bacpy(&chan->src, &hcon->src); + bacpy(&chan->dst, &hcon->dst); + chan->src_type = bdaddr_type(hcon, hcon->src_type); + chan->dst_type = dst_type; - write_unlock_bh(&list->lock); + __l2cap_chan_add(conn, chan); clean: - bh_unlock_sock(parent); + l2cap_chan_unlock(pchan); } static void l2cap_conn_ready(struct l2cap_conn *conn) { - struct l2cap_chan_list *l = &conn->chan_list; - struct sock *sk; + struct l2cap_chan *chan; + struct hci_conn *hcon = conn->hcon; BT_DBG("conn %p", conn); - if (!conn->hcon->out && conn->hcon->type == LE_LINK) + /* For outgoing pairing which doesn't necessarily have an + * associated socket (e.g. mgmt_pair_device). + */ + if (hcon->out && hcon->type == LE_LINK) + smp_conn_security(hcon, hcon->pending_sec_level); + + mutex_lock(&conn->chan_lock); + + if (hcon->type == LE_LINK) l2cap_le_conn_ready(conn); - read_lock(&l->lock); + list_for_each_entry(chan, &conn->chan_l, list) { - for (sk = l->head; sk; sk = l2cap_pi(sk)->next_c) { - bh_lock_sock(sk); + l2cap_chan_lock(chan); - if (conn->hcon->type == LE_LINK) { - l2cap_sock_clear_timer(sk); - sk->sk_state = BT_CONNECTED; - sk->sk_state_change(sk); + if (chan->scid == L2CAP_CID_A2MP) { + l2cap_chan_unlock(chan); + continue; } - 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); - } else if (sk->sk_state == BT_CONNECT) - l2cap_do_start(sk); + if (hcon->type == LE_LINK) { + l2cap_le_start(chan); + } else if (chan->chan_type != L2CAP_CHAN_CONN_ORIENTED) { + l2cap_chan_ready(chan); - bh_unlock_sock(sk); + } else if (chan->state == BT_CONNECT) { + l2cap_do_start(chan); + } + + l2cap_chan_unlock(chan); } - read_unlock(&l->lock); + mutex_unlock(&conn->chan_lock); + + queue_work(hcon->hdev->workqueue, &conn->pending_rx_work); } /* Notify sockets that we cannot guaranty reliability anymore */ static void l2cap_conn_unreliable(struct l2cap_conn *conn, int err) { - struct l2cap_chan_list *l = &conn->chan_list; - struct sock *sk; + struct l2cap_chan *chan; BT_DBG("conn %p", conn); - read_lock(&l->lock); + mutex_lock(&conn->chan_lock); - for (sk = l->head; sk; sk = l2cap_pi(sk)->next_c) { - if (l2cap_pi(sk)->force_reliable) - sk->sk_err = err; + list_for_each_entry(chan, &conn->chan_l, list) { + if (test_bit(FLAG_FORCE_RELIABLE, &chan->flags)) + l2cap_chan_set_err(chan, err); } - read_unlock(&l->lock); + mutex_unlock(&conn->chan_lock); } -static void l2cap_info_timeout(unsigned long arg) +static void l2cap_info_timeout(struct work_struct *work) { - struct l2cap_conn *conn = (void *) arg; + struct l2cap_conn *conn = container_of(work, struct l2cap_conn, + info_timer.work); conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_DONE; conn->info_ident = 0; @@ -728,48 +1567,93 @@ static void l2cap_info_timeout(unsigned long arg) l2cap_conn_start(conn); } -static struct l2cap_conn *l2cap_conn_add(struct hci_conn *hcon, u8 status) +/* + * l2cap_user + * External modules can register l2cap_user objects on l2cap_conn. The ->probe + * callback is called during registration. The ->remove callback is called + * during unregistration. + * An l2cap_user object can either be explicitly unregistered or when the + * underlying l2cap_conn object is deleted. This guarantees that l2cap->hcon, + * l2cap->hchan, .. are valid as long as the remove callback hasn't been called. + * External modules must own a reference to the l2cap_conn object if they intend + * to call l2cap_unregister_user(). The l2cap_conn object might get destroyed at + * any time if they don't. + */ + +int l2cap_register_user(struct l2cap_conn *conn, struct l2cap_user *user) { - struct l2cap_conn *conn = hcon->l2cap_data; + struct hci_dev *hdev = conn->hcon->hdev; + int ret; - if (conn || status) - return conn; + /* We need to check whether l2cap_conn is registered. If it is not, we + * must not register the l2cap_user. l2cap_conn_del() is unregisters + * l2cap_conn objects, but doesn't provide its own locking. Instead, it + * relies on the parent hci_conn object to be locked. This itself relies + * on the hci_dev object to be locked. So we must lock the hci device + * here, too. */ - conn = kzalloc(sizeof(struct l2cap_conn), GFP_ATOMIC); - if (!conn) - return NULL; + hci_dev_lock(hdev); - hcon->l2cap_data = conn; - conn->hcon = hcon; + if (user->list.next || user->list.prev) { + ret = -EINVAL; + goto out_unlock; + } - BT_DBG("hcon %p conn %p", hcon, conn); + /* conn->hchan is NULL after l2cap_conn_del() was called */ + if (!conn->hchan) { + ret = -ENODEV; + goto out_unlock; + } - if (hcon->hdev->le_mtu && hcon->type == LE_LINK) - conn->mtu = hcon->hdev->le_mtu; - else - conn->mtu = hcon->hdev->acl_mtu; + ret = user->probe(conn, user); + if (ret) + goto out_unlock; - conn->src = &hcon->hdev->bdaddr; - conn->dst = &hcon->dst; + list_add(&user->list, &conn->users); + ret = 0; - conn->feat_mask = 0; +out_unlock: + hci_dev_unlock(hdev); + return ret; +} +EXPORT_SYMBOL(l2cap_register_user); - spin_lock_init(&conn->lock); - rwlock_init(&conn->chan_list.lock); +void l2cap_unregister_user(struct l2cap_conn *conn, struct l2cap_user *user) +{ + struct hci_dev *hdev = conn->hcon->hdev; - if (hcon->type != LE_LINK) - setup_timer(&conn->info_timer, l2cap_info_timeout, - (unsigned long) conn); + hci_dev_lock(hdev); - conn->disc_reason = 0x13; + if (!user->list.next || !user->list.prev) + goto out_unlock; - return conn; + list_del(&user->list); + user->list.next = NULL; + user->list.prev = NULL; + user->remove(conn, user); + +out_unlock: + hci_dev_unlock(hdev); +} +EXPORT_SYMBOL(l2cap_unregister_user); + +static void l2cap_unregister_all_users(struct l2cap_conn *conn) +{ + struct l2cap_user *user; + + while (!list_empty(&conn->users)) { + user = list_first_entry(&conn->users, struct l2cap_user, list); + list_del(&user->list); + user->list.next = NULL; + user->list.prev = NULL; + user->remove(conn, user); + } } static void l2cap_conn_del(struct hci_conn *hcon, int err) { struct l2cap_conn *conn = hcon->l2cap_data; - struct sock *sk; + struct l2cap_chan *chan, *l; if (!conn) return; @@ -778,413 +1662,461 @@ static void l2cap_conn_del(struct hci_conn *hcon, int err) kfree_skb(conn->rx_skb); + skb_queue_purge(&conn->pending_rx); + + /* We can not call flush_work(&conn->pending_rx_work) here since we + * might block if we are running on a worker from the same workqueue + * pending_rx_work is waiting on. + */ + if (work_pending(&conn->pending_rx_work)) + cancel_work_sync(&conn->pending_rx_work); + + l2cap_unregister_all_users(conn); + + mutex_lock(&conn->chan_lock); + /* Kill channels */ - while ((sk = conn->chan_list.head)) { - bh_lock_sock(sk); - l2cap_chan_del(sk, err); - bh_unlock_sock(sk); - l2cap_sock_kill(sk); + list_for_each_entry_safe(chan, l, &conn->chan_l, list) { + l2cap_chan_hold(chan); + l2cap_chan_lock(chan); + + l2cap_chan_del(chan, err); + + l2cap_chan_unlock(chan); + + chan->ops->close(chan); + l2cap_chan_put(chan); } + mutex_unlock(&conn->chan_lock); + + hci_chan_del(conn->hchan); + if (conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_SENT) - del_timer_sync(&conn->info_timer); + cancel_delayed_work_sync(&conn->info_timer); + + if (test_and_clear_bit(HCI_CONN_LE_SMP_PEND, &hcon->flags)) { + cancel_delayed_work_sync(&conn->security_timer); + smp_chan_destroy(conn); + } hcon->l2cap_data = NULL; + conn->hchan = NULL; + l2cap_conn_put(conn); +} + +static void security_timeout(struct work_struct *work) +{ + struct l2cap_conn *conn = container_of(work, struct l2cap_conn, + security_timer.work); + + BT_DBG("conn %p", conn); + + if (test_and_clear_bit(HCI_CONN_LE_SMP_PEND, &conn->hcon->flags)) { + smp_chan_destroy(conn); + l2cap_conn_del(conn->hcon, ETIMEDOUT); + } +} + +static void l2cap_conn_free(struct kref *ref) +{ + struct l2cap_conn *conn = container_of(ref, struct l2cap_conn, ref); + + hci_conn_put(conn->hcon); kfree(conn); } -static inline void l2cap_chan_add(struct l2cap_conn *conn, struct sock *sk, struct sock *parent) +void l2cap_conn_get(struct l2cap_conn *conn) { - struct l2cap_chan_list *l = &conn->chan_list; - write_lock_bh(&l->lock); - __l2cap_chan_add(conn, sk, parent); - write_unlock_bh(&l->lock); + kref_get(&conn->ref); } +EXPORT_SYMBOL(l2cap_conn_get); + +void l2cap_conn_put(struct l2cap_conn *conn) +{ + kref_put(&conn->ref, l2cap_conn_free); +} +EXPORT_SYMBOL(l2cap_conn_put); /* ---- Socket interface ---- */ -/* Find socket with psm and source bdaddr. +/* Find socket with psm and source / destination bdaddr. * Returns closest match. */ -static struct sock *l2cap_get_sock_by_psm(int state, __le16 psm, bdaddr_t *src) +static struct l2cap_chan *l2cap_global_chan_by_psm(int state, __le16 psm, + bdaddr_t *src, + bdaddr_t *dst, + u8 link_type) { - struct sock *sk = NULL, *sk1 = NULL; - struct hlist_node *node; + struct l2cap_chan *c, *c1 = NULL; - read_lock(&l2cap_sk_list.lock); + read_lock(&chan_list_lock); - sk_for_each(sk, node, &l2cap_sk_list.head) { - if (state && sk->sk_state != state) + list_for_each_entry(c, &chan_list, global_l) { + if (state && c->state != state) continue; - if (l2cap_pi(sk)->psm == psm) { + if (link_type == ACL_LINK && c->src_type != BDADDR_BREDR) + continue; + + if (link_type == LE_LINK && c->src_type == BDADDR_BREDR) + continue; + + if (c->psm == psm) { + int src_match, dst_match; + int src_any, dst_any; + /* Exact match. */ - if (!bacmp(&bt_sk(sk)->src, src)) - break; + src_match = !bacmp(&c->src, src); + dst_match = !bacmp(&c->dst, dst); + if (src_match && dst_match) { + read_unlock(&chan_list_lock); + return c; + } /* Closest match */ - if (!bacmp(&bt_sk(sk)->src, BDADDR_ANY)) - sk1 = sk; + src_any = !bacmp(&c->src, BDADDR_ANY); + dst_any = !bacmp(&c->dst, BDADDR_ANY); + if ((src_match && dst_any) || (src_any && dst_match) || + (src_any && dst_any)) + c1 = c; } } - read_unlock(&l2cap_sk_list.lock); + read_unlock(&chan_list_lock); - return node ? sk : sk1; + return c1; } -int l2cap_do_connect(struct sock *sk) +static void l2cap_monitor_timeout(struct work_struct *work) { - bdaddr_t *src = &bt_sk(sk)->src; - bdaddr_t *dst = &bt_sk(sk)->dst; - struct l2cap_conn *conn; - struct hci_conn *hcon; - struct hci_dev *hdev; - __u8 auth_type; - int err; + struct l2cap_chan *chan = container_of(work, struct l2cap_chan, + monitor_timer.work); - BT_DBG("%s -> %s psm 0x%2.2x", batostr(src), batostr(dst), - l2cap_pi(sk)->psm); + BT_DBG("chan %p", chan); - hdev = hci_get_route(dst, src); - if (!hdev) - return -EHOSTUNREACH; - - hci_dev_lock_bh(hdev); - - auth_type = l2cap_get_auth_type(sk); - - if (l2cap_pi(sk)->dcid == L2CAP_CID_LE_DATA) - hcon = hci_connect(hdev, LE_LINK, dst, - l2cap_pi(sk)->sec_level, auth_type); - else - hcon = hci_connect(hdev, ACL_LINK, dst, - l2cap_pi(sk)->sec_level, auth_type); - - if (IS_ERR(hcon)) { - err = PTR_ERR(hcon); - goto done; - } - - conn = l2cap_conn_add(hcon, 0); - if (!conn) { - hci_conn_put(hcon); - err = -ENOMEM; - goto done; - } - - /* Update source addr of the socket */ - bacpy(src, conn->src); + l2cap_chan_lock(chan); - l2cap_chan_add(conn, sk, NULL); - - sk->sk_state = BT_CONNECT; - l2cap_sock_set_timer(sk, sk->sk_sndtimeo); - - if (hcon->state == BT_CONNECTED) { - if (sk->sk_type != SOCK_SEQPACKET && - sk->sk_type != SOCK_STREAM) { - l2cap_sock_clear_timer(sk); - if (l2cap_check_security(sk)) - sk->sk_state = BT_CONNECTED; - } else - l2cap_do_start(sk); + if (!chan->conn) { + l2cap_chan_unlock(chan); + l2cap_chan_put(chan); + return; } - err = 0; + l2cap_tx(chan, NULL, NULL, L2CAP_EV_MONITOR_TO); -done: - hci_dev_unlock_bh(hdev); - hci_dev_put(hdev); - return err; + l2cap_chan_unlock(chan); + l2cap_chan_put(chan); } -int __l2cap_wait_ack(struct sock *sk) +static void l2cap_retrans_timeout(struct work_struct *work) { - 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); + struct l2cap_chan *chan = container_of(work, struct l2cap_chan, + retrans_timer.work); - if (!timeo) - timeo = HZ/5; - - if (signal_pending(current)) { - err = sock_intr_errno(timeo); - break; - } + BT_DBG("chan %p", chan); - release_sock(sk); - timeo = schedule_timeout(timeo); - lock_sock(sk); + l2cap_chan_lock(chan); - err = sock_error(sk); - if (err) - break; + if (!chan->conn) { + l2cap_chan_unlock(chan); + l2cap_chan_put(chan); + return; } - set_current_state(TASK_RUNNING); - remove_wait_queue(sk_sleep(sk), &wait); - return err; + + l2cap_tx(chan, NULL, NULL, L2CAP_EV_RETRANS_TO); + l2cap_chan_unlock(chan); + l2cap_chan_put(chan); } -static void l2cap_monitor_timeout(unsigned long arg) +static void l2cap_streaming_send(struct l2cap_chan *chan, + struct sk_buff_head *skbs) { - struct sock *sk = (void *) arg; + struct sk_buff *skb; + struct l2cap_ctrl *control; - BT_DBG("sk %p", sk); + BT_DBG("chan %p, skbs %p", chan, skbs); - bh_lock_sock(sk); - if (l2cap_pi(sk)->retry_count >= l2cap_pi(sk)->remote_max_tx) { - l2cap_send_disconn_req(l2cap_pi(sk)->conn, sk, ECONNABORTED); - bh_unlock_sock(sk); + if (__chan_is_moving(chan)) return; - } - l2cap_pi(sk)->retry_count++; - __mod_monitor_timer(); + skb_queue_splice_tail_init(skbs, &chan->tx_q); - l2cap_send_rr_or_rnr(l2cap_pi(sk), L2CAP_CTRL_POLL); - bh_unlock_sock(sk); -} + while (!skb_queue_empty(&chan->tx_q)) { -static void l2cap_retrans_timeout(unsigned long arg) -{ - struct sock *sk = (void *) arg; - - BT_DBG("sk %p", sk); + skb = skb_dequeue(&chan->tx_q); - bh_lock_sock(sk); - l2cap_pi(sk)->retry_count = 1; - __mod_monitor_timer(); + bt_cb(skb)->control.retries = 1; + control = &bt_cb(skb)->control; - l2cap_pi(sk)->conn_state |= L2CAP_CONN_WAIT_F; + control->reqseq = 0; + control->txseq = chan->next_tx_seq; - l2cap_send_rr_or_rnr(l2cap_pi(sk), L2CAP_CTRL_POLL); - bh_unlock_sock(sk); -} + __pack_control(chan, control, skb); -static void l2cap_drop_acked_frames(struct sock *sk) -{ - struct sk_buff *skb; + if (chan->fcs == L2CAP_FCS_CRC16) { + u16 fcs = crc16(0, (u8 *) skb->data, skb->len); + put_unaligned_le16(fcs, skb_put(skb, L2CAP_FCS_SIZE)); + } - 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; + l2cap_do_send(chan, skb); - skb = skb_dequeue(TX_QUEUE(sk)); - kfree_skb(skb); + BT_DBG("Sent txseq %u", control->txseq); - l2cap_pi(sk)->unacked_frames--; + chan->next_tx_seq = __next_seq(chan, chan->next_tx_seq); + chan->frames_sent++; } - - if (!l2cap_pi(sk)->unacked_frames) - del_timer(&l2cap_pi(sk)->retrans_timer); } -void l2cap_do_send(struct sock *sk, struct sk_buff *skb) +static int l2cap_ertm_send(struct l2cap_chan *chan) { - struct l2cap_pinfo *pi = l2cap_pi(sk); - struct hci_conn *hcon = pi->conn->hcon; - u16 flags; - - BT_DBG("sk %p, skb %p len %d", sk, skb, skb->len); - - if (!pi->flushable && lmp_no_flush_capable(hcon->hdev)) - flags = ACL_START_NO_FLUSH; - else - flags = ACL_START; + struct sk_buff *skb, *tx_skb; + struct l2cap_ctrl *control; + int sent = 0; - hci_send_acl(hcon, skb, flags); -} + BT_DBG("chan %p", chan); -void l2cap_streaming_send(struct sock *sk) -{ - struct sk_buff *skb; - struct l2cap_pinfo *pi = l2cap_pi(sk); - u16 control, fcs; + if (chan->state != BT_CONNECTED) + return -ENOTCONN; - while ((skb = skb_dequeue(TX_QUEUE(sk)))) { - control = get_unaligned_le16(skb->data + L2CAP_HDR_SIZE); - control |= pi->next_tx_seq << L2CAP_CTRL_TXSEQ_SHIFT; - put_unaligned_le16(control, skb->data + L2CAP_HDR_SIZE); + if (test_bit(CONN_REMOTE_BUSY, &chan->conn_state)) + return 0; - if (pi->fcs == L2CAP_FCS_CRC16) { - fcs = crc16(0, (u8 *)skb->data, skb->len - 2); - put_unaligned_le16(fcs, skb->data + skb->len - 2); - } + if (__chan_is_moving(chan)) + return 0; - l2cap_do_send(sk, skb); + while (chan->tx_send_head && + chan->unacked_frames < chan->remote_tx_win && + chan->tx_state == L2CAP_TX_STATE_XMIT) { - pi->next_tx_seq = (pi->next_tx_seq + 1) % 64; - } -} + skb = chan->tx_send_head; -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; + bt_cb(skb)->control.retries = 1; + control = &bt_cb(skb)->control; - skb = skb_peek(TX_QUEUE(sk)); - if (!skb) - return; + if (test_and_clear_bit(CONN_SEND_FBIT, &chan->conn_state)) + control->final = 1; - do { - if (bt_cb(skb)->tx_seq == tx_seq) - break; + control->reqseq = chan->buffer_seq; + chan->last_acked_seq = chan->buffer_seq; + control->txseq = chan->next_tx_seq; - if (skb_queue_is_last(TX_QUEUE(sk), skb)) - return; + __pack_control(chan, control, skb); - } while ((skb = skb_queue_next(TX_QUEUE(sk), skb))); + if (chan->fcs == L2CAP_FCS_CRC16) { + u16 fcs = crc16(0, (u8 *) skb->data, skb->len); + put_unaligned_le16(fcs, skb_put(skb, L2CAP_FCS_SIZE)); + } - if (pi->remote_max_tx && - bt_cb(skb)->retries == pi->remote_max_tx) { - l2cap_send_disconn_req(pi->conn, sk, ECONNABORTED); - return; - } + /* Clone after data has been modified. Data is assumed to be + read-only (for locking purposes) on cloned sk_buffs. + */ + tx_skb = skb_clone(skb, GFP_KERNEL); - tx_skb = skb_clone(skb, GFP_ATOMIC); - bt_cb(skb)->retries++; - control = get_unaligned_le16(tx_skb->data + L2CAP_HDR_SIZE); + if (!tx_skb) + break; - if (pi->conn_state & L2CAP_CONN_SEND_FBIT) { - control |= L2CAP_CTRL_FINAL; - pi->conn_state &= ~L2CAP_CONN_SEND_FBIT; - } + __set_retrans_timer(chan); - control |= (pi->buffer_seq << L2CAP_CTRL_REQSEQ_SHIFT) - | (tx_seq << L2CAP_CTRL_TXSEQ_SHIFT); + chan->next_tx_seq = __next_seq(chan, chan->next_tx_seq); + chan->unacked_frames++; + chan->frames_sent++; + sent++; - put_unaligned_le16(control, tx_skb->data + L2CAP_HDR_SIZE); + if (skb_queue_is_last(&chan->tx_q, skb)) + chan->tx_send_head = NULL; + else + chan->tx_send_head = skb_queue_next(&chan->tx_q, skb); - 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(chan, tx_skb); + BT_DBG("Sent txseq %u", control->txseq); } - l2cap_do_send(sk, tx_skb); + BT_DBG("Sent %d, %u unacked, %u in ERTM queue", sent, + chan->unacked_frames, skb_queue_len(&chan->tx_q)); + + return sent; } -int l2cap_ertm_send(struct sock *sk) +static void l2cap_ertm_resend(struct l2cap_chan *chan) { - struct sk_buff *skb, *tx_skb; - struct l2cap_pinfo *pi = l2cap_pi(sk); - u16 control, fcs; - int nsent = 0; + struct l2cap_ctrl control; + struct sk_buff *skb; + struct sk_buff *tx_skb; + u16 seq; - if (sk->sk_state != BT_CONNECTED) - return -ENOTCONN; + BT_DBG("chan %p", chan); - while ((skb = sk->sk_send_head) && (!l2cap_tx_window_full(sk))) { + if (test_bit(CONN_REMOTE_BUSY, &chan->conn_state)) + return; - if (pi->remote_max_tx && - bt_cb(skb)->retries == pi->remote_max_tx) { - l2cap_send_disconn_req(pi->conn, sk, ECONNABORTED); - break; - } + if (__chan_is_moving(chan)) + return; - tx_skb = skb_clone(skb, GFP_ATOMIC); + while (chan->retrans_list.head != L2CAP_SEQ_LIST_CLEAR) { + seq = l2cap_seq_list_pop(&chan->retrans_list); - bt_cb(skb)->retries++; + skb = l2cap_ertm_seq_in_queue(&chan->tx_q, seq); + if (!skb) { + BT_DBG("Error: Can't retransmit seq %d, frame missing", + seq); + continue; + } - control = get_unaligned_le16(tx_skb->data + L2CAP_HDR_SIZE); - control &= L2CAP_CTRL_SAR; + bt_cb(skb)->control.retries++; + control = bt_cb(skb)->control; - if (pi->conn_state & L2CAP_CONN_SEND_FBIT) { - control |= L2CAP_CTRL_FINAL; - pi->conn_state &= ~L2CAP_CONN_SEND_FBIT; + if (chan->max_tx != 0 && + bt_cb(skb)->control.retries > chan->max_tx) { + BT_DBG("Retry limit exceeded (%d)", chan->max_tx); + l2cap_send_disconn_req(chan, ECONNRESET); + l2cap_seq_list_clear(&chan->retrans_list); + break; } - control |= (pi->buffer_seq << L2CAP_CTRL_REQSEQ_SHIFT) - | (pi->next_tx_seq << L2CAP_CTRL_TXSEQ_SHIFT); - put_unaligned_le16(control, tx_skb->data + L2CAP_HDR_SIZE); + control.reqseq = chan->buffer_seq; + if (test_and_clear_bit(CONN_SEND_FBIT, &chan->conn_state)) + control.final = 1; + else + control.final = 0; - if (pi->fcs == L2CAP_FCS_CRC16) { - fcs = crc16(0, (u8 *)skb->data, tx_skb->len - 2); - put_unaligned_le16(fcs, skb->data + tx_skb->len - 2); + if (skb_cloned(skb)) { + /* Cloned sk_buffs are read-only, so we need a + * writeable copy + */ + tx_skb = skb_copy(skb, GFP_KERNEL); + } else { + tx_skb = skb_clone(skb, GFP_KERNEL); } - l2cap_do_send(sk, tx_skb); - - __mod_retrans_timer(); + if (!tx_skb) { + l2cap_seq_list_clear(&chan->retrans_list); + break; + } - bt_cb(skb)->tx_seq = pi->next_tx_seq; - pi->next_tx_seq = (pi->next_tx_seq + 1) % 64; + /* Update skb contents */ + if (test_bit(FLAG_EXT_CTRL, &chan->flags)) { + put_unaligned_le32(__pack_extended_control(&control), + tx_skb->data + L2CAP_HDR_SIZE); + } else { + put_unaligned_le16(__pack_enhanced_control(&control), + tx_skb->data + L2CAP_HDR_SIZE); + } - if (bt_cb(skb)->retries == 1) - pi->unacked_frames++; + if (chan->fcs == L2CAP_FCS_CRC16) { + u16 fcs = crc16(0, (u8 *) tx_skb->data, tx_skb->len); + put_unaligned_le16(fcs, skb_put(tx_skb, + L2CAP_FCS_SIZE)); + } - pi->frames_sent++; + l2cap_do_send(chan, tx_skb); - 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); + BT_DBG("Resent txseq %d", control.txseq); - nsent++; + chan->last_acked_seq = chan->buffer_seq; } - - return nsent; } -static int l2cap_retransmit_frames(struct sock *sk) +static void l2cap_retransmit(struct l2cap_chan *chan, + struct l2cap_ctrl *control) { - struct l2cap_pinfo *pi = l2cap_pi(sk); - int ret; - - if (!skb_queue_empty(TX_QUEUE(sk))) - sk->sk_send_head = TX_QUEUE(sk)->next; + BT_DBG("chan %p, control %p", chan, control); - pi->next_tx_seq = pi->expected_ack_seq; - ret = l2cap_ertm_send(sk); - return ret; + l2cap_seq_list_append(&chan->retrans_list, control->reqseq); + l2cap_ertm_resend(chan); } -static void l2cap_send_ack(struct l2cap_pinfo *pi) +static void l2cap_retransmit_all(struct l2cap_chan *chan, + struct l2cap_ctrl *control) { - struct sock *sk = (struct sock *)pi; - u16 control = 0; + struct sk_buff *skb; - control |= pi->buffer_seq << L2CAP_CTRL_REQSEQ_SHIFT; + BT_DBG("chan %p, control %p", chan, control); - if (pi->conn_state & L2CAP_CONN_LOCAL_BUSY) { - control |= L2CAP_SUPER_RCV_NOT_READY; - pi->conn_state |= L2CAP_CONN_RNR_SENT; - l2cap_send_sframe(pi, control); - return; - } + if (control->poll) + set_bit(CONN_SEND_FBIT, &chan->conn_state); + + l2cap_seq_list_clear(&chan->retrans_list); - if (l2cap_ertm_send(sk) > 0) + if (test_bit(CONN_REMOTE_BUSY, &chan->conn_state)) return; - control |= L2CAP_SUPER_RCV_READY; - l2cap_send_sframe(pi, control); + if (chan->unacked_frames) { + skb_queue_walk(&chan->tx_q, skb) { + if (bt_cb(skb)->control.txseq == control->reqseq || + skb == chan->tx_send_head) + break; + } + + skb_queue_walk_from(&chan->tx_q, skb) { + if (skb == chan->tx_send_head) + break; + + l2cap_seq_list_append(&chan->retrans_list, + bt_cb(skb)->control.txseq); + } + + l2cap_ertm_resend(chan); + } } -static void l2cap_send_srejtail(struct sock *sk) +static void l2cap_send_ack(struct l2cap_chan *chan) { - struct srej_list *tail; - u16 control; - - control = L2CAP_SUPER_SELECT_REJECT; - control |= L2CAP_CTRL_FINAL; + struct l2cap_ctrl control; + u16 frames_to_ack = __seq_offset(chan, chan->buffer_seq, + chan->last_acked_seq); + int threshold; + + BT_DBG("chan %p last_acked_seq %d buffer_seq %d", + chan, chan->last_acked_seq, chan->buffer_seq); + + memset(&control, 0, sizeof(control)); + control.sframe = 1; + + if (test_bit(CONN_LOCAL_BUSY, &chan->conn_state) && + chan->rx_state == L2CAP_RX_STATE_RECV) { + __clear_ack_timer(chan); + control.super = L2CAP_SUPER_RNR; + control.reqseq = chan->buffer_seq; + l2cap_send_sframe(chan, &control); + } else { + if (!test_bit(CONN_REMOTE_BUSY, &chan->conn_state)) { + l2cap_ertm_send(chan); + /* If any i-frames were sent, they included an ack */ + if (chan->buffer_seq == chan->last_acked_seq) + frames_to_ack = 0; + } - tail = list_entry(SREJ_LIST(sk)->prev, struct srej_list, list); - control |= tail->tx_seq << L2CAP_CTRL_REQSEQ_SHIFT; + /* Ack now if the window is 3/4ths full. + * Calculate without mul or div + */ + threshold = chan->ack_win; + threshold += threshold << 1; + threshold >>= 2; + + BT_DBG("frames_to_ack %u, threshold %d", frames_to_ack, + threshold); + + if (frames_to_ack >= threshold) { + __clear_ack_timer(chan); + control.super = L2CAP_SUPER_RR; + control.reqseq = chan->buffer_seq; + l2cap_send_sframe(chan, &control); + frames_to_ack = 0; + } - l2cap_send_sframe(l2cap_pi(sk), control); + if (frames_to_ack) + __set_ack_timer(chan); + } } -static inline int l2cap_skbuff_fromiovec(struct sock *sk, struct msghdr *msg, int len, int count, struct sk_buff *skb) +static inline int l2cap_skbuff_fromiovec(struct l2cap_chan *chan, + struct msghdr *msg, int len, + int count, struct sk_buff *skb) { - struct l2cap_conn *conn = l2cap_pi(sk)->conn; + struct l2cap_conn *conn = chan->conn; struct sk_buff **frag; - int err, sent = 0; + int sent = 0; if (memcpy_fromiovec(skb_put(skb, count), msg->msg_iov, count)) return -EFAULT; @@ -1195,45 +2127,62 @@ static inline int l2cap_skbuff_fromiovec(struct sock *sk, struct msghdr *msg, in /* Continuation fragments (no L2CAP header) */ frag = &skb_shinfo(skb)->frag_list; while (len) { + struct sk_buff *tmp; + count = min_t(unsigned int, conn->mtu, len); - *frag = bt_skb_send_alloc(sk, count, msg->msg_flags & MSG_DONTWAIT, &err); - if (!*frag) - return err; + tmp = chan->ops->alloc_skb(chan, count, + msg->msg_flags & MSG_DONTWAIT); + if (IS_ERR(tmp)) + return PTR_ERR(tmp); + + *frag = tmp; + if (memcpy_fromiovec(skb_put(*frag, count), msg->msg_iov, count)) return -EFAULT; + (*frag)->priority = skb->priority; + sent += count; len -= count; + skb->len += (*frag)->len; + skb->data_len += (*frag)->len; + frag = &(*frag)->next; } return sent; } -struct sk_buff *l2cap_create_connless_pdu(struct sock *sk, struct msghdr *msg, size_t len) +static struct sk_buff *l2cap_create_connless_pdu(struct l2cap_chan *chan, + struct msghdr *msg, size_t len, + u32 priority) { - struct l2cap_conn *conn = l2cap_pi(sk)->conn; + struct l2cap_conn *conn = chan->conn; struct sk_buff *skb; - int err, count, hlen = L2CAP_HDR_SIZE + 2; + int err, count, hlen = L2CAP_HDR_SIZE + L2CAP_PSMLEN_SIZE; struct l2cap_hdr *lh; - BT_DBG("sk %p len %d", sk, (int)len); + BT_DBG("chan %p psm 0x%2.2x len %zu priority %u", chan, + __le16_to_cpu(chan->psm), len, priority); 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(err); + + skb = chan->ops->alloc_skb(chan, count + hlen, + msg->msg_flags & MSG_DONTWAIT); + if (IS_ERR(skb)) + return skb; + + skb->priority = priority; /* 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)); + lh->cid = cpu_to_le16(chan->dcid); + lh->len = cpu_to_le16(len + L2CAP_PSMLEN_SIZE); + put_unaligned(chan->psm, (__le16 *) skb_put(skb, L2CAP_PSMLEN_SIZE)); - err = l2cap_skbuff_fromiovec(sk, msg, len, count, skb); + err = l2cap_skbuff_fromiovec(chan, msg, len, count, skb); if (unlikely(err < 0)) { kfree_skb(skb); return ERR_PTR(err); @@ -1241,27 +2190,32 @@ struct sk_buff *l2cap_create_connless_pdu(struct sock *sk, struct msghdr *msg, s return skb; } -struct sk_buff *l2cap_create_basic_pdu(struct sock *sk, struct msghdr *msg, size_t len) +static struct sk_buff *l2cap_create_basic_pdu(struct l2cap_chan *chan, + struct msghdr *msg, size_t len, + u32 priority) { - struct l2cap_conn *conn = l2cap_pi(sk)->conn; + struct l2cap_conn *conn = chan->conn; struct sk_buff *skb; - int err, count, hlen = L2CAP_HDR_SIZE; + int err, count; struct l2cap_hdr *lh; - BT_DBG("sk %p len %d", sk, (int)len); + BT_DBG("chan %p len %zu", chan, 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(err); + count = min_t(unsigned int, (conn->mtu - L2CAP_HDR_SIZE), len); + + skb = chan->ops->alloc_skb(chan, count + L2CAP_HDR_SIZE, + msg->msg_flags & MSG_DONTWAIT); + if (IS_ERR(skb)) + return skb; + + skb->priority = priority; /* 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)); + lh->cid = cpu_to_le16(chan->dcid); + lh->len = cpu_to_le16(len); - err = l2cap_skbuff_fromiovec(sk, msg, len, count, skb); + err = l2cap_skbuff_fromiovec(chan, msg, len, count, skb); if (unlikely(err < 0)) { kfree_skb(skb); return ERR_PTR(err); @@ -1269,163 +2223,683 @@ struct sk_buff *l2cap_create_basic_pdu(struct sock *sk, struct msghdr *msg, size return skb; } -struct sk_buff *l2cap_create_iframe_pdu(struct sock *sk, struct msghdr *msg, size_t len, u16 control, u16 sdulen) +static struct sk_buff *l2cap_create_iframe_pdu(struct l2cap_chan *chan, + struct msghdr *msg, size_t len, + u16 sdulen) { - struct l2cap_conn *conn = l2cap_pi(sk)->conn; + struct l2cap_conn *conn = chan->conn; struct sk_buff *skb; - int err, count, hlen = L2CAP_HDR_SIZE + 2; + int err, count, hlen; struct l2cap_hdr *lh; - BT_DBG("sk %p len %d", sk, (int)len); + BT_DBG("chan %p len %zu", chan, len); if (!conn) return ERR_PTR(-ENOTCONN); + hlen = __ertm_hdr_size(chan); + if (sdulen) - hlen += 2; + hlen += L2CAP_SDULEN_SIZE; - if (l2cap_pi(sk)->fcs == L2CAP_FCS_CRC16) - hlen += 2; + if (chan->fcs == L2CAP_FCS_CRC16) + hlen += L2CAP_FCS_SIZE; 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(err); + + skb = chan->ops->alloc_skb(chan, count + hlen, + msg->msg_flags & MSG_DONTWAIT); + if (IS_ERR(skb)) + return skb; /* Create L2CAP header */ lh = (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE); - lh->cid = cpu_to_le16(l2cap_pi(sk)->dcid); + lh->cid = cpu_to_le16(chan->dcid); lh->len = cpu_to_le16(len + (hlen - L2CAP_HDR_SIZE)); - put_unaligned_le16(control, skb_put(skb, 2)); + + /* Control header is populated later */ + if (test_bit(FLAG_EXT_CTRL, &chan->flags)) + put_unaligned_le32(0, skb_put(skb, L2CAP_EXT_CTRL_SIZE)); + else + put_unaligned_le16(0, skb_put(skb, L2CAP_ENH_CTRL_SIZE)); + if (sdulen) - put_unaligned_le16(sdulen, skb_put(skb, 2)); + put_unaligned_le16(sdulen, skb_put(skb, L2CAP_SDULEN_SIZE)); - err = l2cap_skbuff_fromiovec(sk, msg, len, count, skb); + err = l2cap_skbuff_fromiovec(chan, msg, len, count, skb); if (unlikely(err < 0)) { kfree_skb(skb); return ERR_PTR(err); } - if (l2cap_pi(sk)->fcs == L2CAP_FCS_CRC16) - put_unaligned_le16(0, skb_put(skb, 2)); - - bt_cb(skb)->retries = 0; + bt_cb(skb)->control.fcs = chan->fcs; + bt_cb(skb)->control.retries = 0; return skb; } -int l2cap_sar_segment_sdu(struct sock *sk, struct msghdr *msg, size_t len) +static int l2cap_segment_sdu(struct l2cap_chan *chan, + struct sk_buff_head *seg_queue, + 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; + u16 sdu_len; + size_t pdu_len; + u8 sar; - 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)) - return PTR_ERR(skb); + BT_DBG("chan %p, msg %p, len %zu", chan, msg, len); + + /* It is critical that ERTM PDUs fit in a single HCI fragment, + * so fragmented skbs are not used. The HCI layer's handling + * of fragmented skbs is not compatible with ERTM's queueing. + */ + + /* PDU size is derived from the HCI MTU */ + pdu_len = chan->conn->mtu; + + /* Constrain PDU size for BR/EDR connections */ + if (!chan->hs_hcon) + pdu_len = min_t(size_t, pdu_len, L2CAP_BREDR_MAX_PAYLOAD); + + /* Adjust for largest possible L2CAP overhead. */ + if (chan->fcs) + pdu_len -= L2CAP_FCS_SIZE; + + pdu_len -= __ertm_hdr_size(chan); - __skb_queue_tail(&sar_queue, skb); - len -= pi->remote_mps; - size += pi->remote_mps; + /* Remote device may have requested smaller PDUs */ + pdu_len = min_t(size_t, pdu_len, chan->remote_mps); + + if (len <= pdu_len) { + sar = L2CAP_SAR_UNSEGMENTED; + sdu_len = 0; + pdu_len = len; + } else { + sar = L2CAP_SAR_START; + sdu_len = len; + pdu_len -= L2CAP_SDULEN_SIZE; + } while (len > 0) { - size_t buflen; + skb = l2cap_create_iframe_pdu(chan, msg, pdu_len, sdu_len); - if (len > pi->remote_mps) { - control = L2CAP_SDU_CONTINUE; - buflen = pi->remote_mps; + if (IS_ERR(skb)) { + __skb_queue_purge(seg_queue); + return PTR_ERR(skb); + } + + bt_cb(skb)->control.sar = sar; + __skb_queue_tail(seg_queue, skb); + + len -= pdu_len; + if (sdu_len) { + sdu_len = 0; + pdu_len += L2CAP_SDULEN_SIZE; + } + + if (len <= pdu_len) { + sar = L2CAP_SAR_END; + pdu_len = len; } else { - control = L2CAP_SDU_END; - buflen = len; + sar = L2CAP_SAR_CONTINUE; } + } + + return 0; +} - skb = l2cap_create_iframe_pdu(sk, msg, buflen, control, 0); +static struct sk_buff *l2cap_create_le_flowctl_pdu(struct l2cap_chan *chan, + struct msghdr *msg, + size_t len, u16 sdulen) +{ + struct l2cap_conn *conn = chan->conn; + struct sk_buff *skb; + int err, count, hlen; + struct l2cap_hdr *lh; + + BT_DBG("chan %p len %zu", chan, len); + + if (!conn) + return ERR_PTR(-ENOTCONN); + + hlen = L2CAP_HDR_SIZE; + + if (sdulen) + hlen += L2CAP_SDULEN_SIZE; + + count = min_t(unsigned int, (conn->mtu - hlen), len); + + skb = chan->ops->alloc_skb(chan, count + hlen, + msg->msg_flags & MSG_DONTWAIT); + if (IS_ERR(skb)) + return skb; + + /* Create L2CAP header */ + lh = (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE); + lh->cid = cpu_to_le16(chan->dcid); + lh->len = cpu_to_le16(len + (hlen - L2CAP_HDR_SIZE)); + + if (sdulen) + put_unaligned_le16(sdulen, skb_put(skb, L2CAP_SDULEN_SIZE)); + + err = l2cap_skbuff_fromiovec(chan, msg, len, count, skb); + if (unlikely(err < 0)) { + kfree_skb(skb); + return ERR_PTR(err); + } + + return skb; +} + +static int l2cap_segment_le_sdu(struct l2cap_chan *chan, + struct sk_buff_head *seg_queue, + struct msghdr *msg, size_t len) +{ + struct sk_buff *skb; + size_t pdu_len; + u16 sdu_len; + + BT_DBG("chan %p, msg %p, len %zu", chan, msg, len); + + pdu_len = chan->conn->mtu - L2CAP_HDR_SIZE; + + pdu_len = min_t(size_t, pdu_len, chan->remote_mps); + + sdu_len = len; + pdu_len -= L2CAP_SDULEN_SIZE; + + while (len > 0) { + if (len <= pdu_len) + pdu_len = len; + + skb = l2cap_create_le_flowctl_pdu(chan, msg, pdu_len, sdu_len); if (IS_ERR(skb)) { - skb_queue_purge(&sar_queue); + __skb_queue_purge(seg_queue); return PTR_ERR(skb); } - __skb_queue_tail(&sar_queue, skb); - len -= buflen; - size += buflen; + __skb_queue_tail(seg_queue, skb); + + len -= pdu_len; + + if (sdu_len) { + sdu_len = 0; + pdu_len += L2CAP_SDULEN_SIZE; + } } - skb_queue_splice_tail(&sar_queue, TX_QUEUE(sk)); - if (sk->sk_send_head == NULL) - sk->sk_send_head = sar_queue.next; - return size; + return 0; } -static void l2cap_chan_ready(struct sock *sk) +int l2cap_chan_send(struct l2cap_chan *chan, struct msghdr *msg, size_t len, + u32 priority) { - struct sock *parent = bt_sk(sk)->parent; + struct sk_buff *skb; + int err; + struct sk_buff_head seg_queue; - BT_DBG("sk %p, parent %p", sk, parent); + if (!chan->conn) + return -ENOTCONN; - l2cap_pi(sk)->conf_state = 0; - l2cap_sock_clear_timer(sk); + /* Connectionless channel */ + if (chan->chan_type == L2CAP_CHAN_CONN_LESS) { + skb = l2cap_create_connless_pdu(chan, msg, len, priority); + if (IS_ERR(skb)) + return PTR_ERR(skb); - if (!parent) { - /* Outgoing channel. - * Wake up socket sleeping on connect. + /* Channel lock is released before requesting new skb and then + * reacquired thus we need to recheck channel state. */ - sk->sk_state = BT_CONNECTED; - sk->sk_state_change(sk); - } else { - /* Incoming channel. - * Wake up socket sleeping on accept. + if (chan->state != BT_CONNECTED) { + kfree_skb(skb); + return -ENOTCONN; + } + + l2cap_do_send(chan, skb); + return len; + } + + switch (chan->mode) { + case L2CAP_MODE_LE_FLOWCTL: + /* Check outgoing MTU */ + if (len > chan->omtu) + return -EMSGSIZE; + + if (!chan->tx_credits) + return -EAGAIN; + + __skb_queue_head_init(&seg_queue); + + err = l2cap_segment_le_sdu(chan, &seg_queue, msg, len); + + if (chan->state != BT_CONNECTED) { + __skb_queue_purge(&seg_queue); + err = -ENOTCONN; + } + + if (err) + return err; + + skb_queue_splice_tail_init(&seg_queue, &chan->tx_q); + + while (chan->tx_credits && !skb_queue_empty(&chan->tx_q)) { + l2cap_do_send(chan, skb_dequeue(&chan->tx_q)); + chan->tx_credits--; + } + + if (!chan->tx_credits) + chan->ops->suspend(chan); + + err = len; + + break; + + case L2CAP_MODE_BASIC: + /* Check outgoing MTU */ + if (len > chan->omtu) + return -EMSGSIZE; + + /* Create a basic PDU */ + skb = l2cap_create_basic_pdu(chan, msg, len, priority); + if (IS_ERR(skb)) + return PTR_ERR(skb); + + /* Channel lock is released before requesting new skb and then + * reacquired thus we need to recheck channel state. */ - parent->sk_data_ready(parent, 0); + if (chan->state != BT_CONNECTED) { + kfree_skb(skb); + return -ENOTCONN; + } + + l2cap_do_send(chan, skb); + err = len; + break; + + case L2CAP_MODE_ERTM: + case L2CAP_MODE_STREAMING: + /* Check outgoing MTU */ + if (len > chan->omtu) { + err = -EMSGSIZE; + break; + } + + __skb_queue_head_init(&seg_queue); + + /* Do segmentation before calling in to the state machine, + * since it's possible to block while waiting for memory + * allocation. + */ + err = l2cap_segment_sdu(chan, &seg_queue, msg, len); + + /* The channel could have been closed while segmenting, + * check that it is still connected. + */ + if (chan->state != BT_CONNECTED) { + __skb_queue_purge(&seg_queue); + err = -ENOTCONN; + } + + if (err) + break; + + if (chan->mode == L2CAP_MODE_ERTM) + l2cap_tx(chan, NULL, &seg_queue, L2CAP_EV_DATA_REQUEST); + else + l2cap_streaming_send(chan, &seg_queue); + + err = len; + + /* If the skbs were not queued for sending, they'll still be in + * seg_queue and need to be purged. + */ + __skb_queue_purge(&seg_queue); + break; + + default: + BT_DBG("bad state %1.1x", chan->mode); + err = -EBADFD; + } + + return err; +} + +static void l2cap_send_srej(struct l2cap_chan *chan, u16 txseq) +{ + struct l2cap_ctrl control; + u16 seq; + + BT_DBG("chan %p, txseq %u", chan, txseq); + + memset(&control, 0, sizeof(control)); + control.sframe = 1; + control.super = L2CAP_SUPER_SREJ; + + for (seq = chan->expected_tx_seq; seq != txseq; + seq = __next_seq(chan, seq)) { + if (!l2cap_ertm_seq_in_queue(&chan->srej_q, seq)) { + control.reqseq = seq; + l2cap_send_sframe(chan, &control); + l2cap_seq_list_append(&chan->srej_list, seq); + } + } + + chan->expected_tx_seq = __next_seq(chan, txseq); +} + +static void l2cap_send_srej_tail(struct l2cap_chan *chan) +{ + struct l2cap_ctrl control; + + BT_DBG("chan %p", chan); + + if (chan->srej_list.tail == L2CAP_SEQ_LIST_CLEAR) + return; + + memset(&control, 0, sizeof(control)); + control.sframe = 1; + control.super = L2CAP_SUPER_SREJ; + control.reqseq = chan->srej_list.tail; + l2cap_send_sframe(chan, &control); +} + +static void l2cap_send_srej_list(struct l2cap_chan *chan, u16 txseq) +{ + struct l2cap_ctrl control; + u16 initial_head; + u16 seq; + + BT_DBG("chan %p, txseq %u", chan, txseq); + + memset(&control, 0, sizeof(control)); + control.sframe = 1; + control.super = L2CAP_SUPER_SREJ; + + /* Capture initial list head to allow only one pass through the list. */ + initial_head = chan->srej_list.head; + + do { + seq = l2cap_seq_list_pop(&chan->srej_list); + if (seq == txseq || seq == L2CAP_SEQ_LIST_CLEAR) + break; + + control.reqseq = seq; + l2cap_send_sframe(chan, &control); + l2cap_seq_list_append(&chan->srej_list, seq); + } while (chan->srej_list.head != initial_head); +} + +static void l2cap_process_reqseq(struct l2cap_chan *chan, u16 reqseq) +{ + struct sk_buff *acked_skb; + u16 ackseq; + + BT_DBG("chan %p, reqseq %u", chan, reqseq); + + if (chan->unacked_frames == 0 || reqseq == chan->expected_ack_seq) + return; + + BT_DBG("expected_ack_seq %u, unacked_frames %u", + chan->expected_ack_seq, chan->unacked_frames); + + for (ackseq = chan->expected_ack_seq; ackseq != reqseq; + ackseq = __next_seq(chan, ackseq)) { + + acked_skb = l2cap_ertm_seq_in_queue(&chan->tx_q, ackseq); + if (acked_skb) { + skb_unlink(acked_skb, &chan->tx_q); + kfree_skb(acked_skb); + chan->unacked_frames--; + } + } + + chan->expected_ack_seq = reqseq; + + if (chan->unacked_frames == 0) + __clear_retrans_timer(chan); + + BT_DBG("unacked_frames %u", chan->unacked_frames); +} + +static void l2cap_abort_rx_srej_sent(struct l2cap_chan *chan) +{ + BT_DBG("chan %p", chan); + + chan->expected_tx_seq = chan->buffer_seq; + l2cap_seq_list_clear(&chan->srej_list); + skb_queue_purge(&chan->srej_q); + chan->rx_state = L2CAP_RX_STATE_RECV; +} + +static void l2cap_tx_state_xmit(struct l2cap_chan *chan, + struct l2cap_ctrl *control, + struct sk_buff_head *skbs, u8 event) +{ + BT_DBG("chan %p, control %p, skbs %p, event %d", chan, control, skbs, + event); + + switch (event) { + case L2CAP_EV_DATA_REQUEST: + if (chan->tx_send_head == NULL) + chan->tx_send_head = skb_peek(skbs); + + skb_queue_splice_tail_init(skbs, &chan->tx_q); + l2cap_ertm_send(chan); + break; + case L2CAP_EV_LOCAL_BUSY_DETECTED: + BT_DBG("Enter LOCAL_BUSY"); + set_bit(CONN_LOCAL_BUSY, &chan->conn_state); + + if (chan->rx_state == L2CAP_RX_STATE_SREJ_SENT) { + /* The SREJ_SENT state must be aborted if we are to + * enter the LOCAL_BUSY state. + */ + l2cap_abort_rx_srej_sent(chan); + } + + l2cap_send_ack(chan); + + break; + case L2CAP_EV_LOCAL_BUSY_CLEAR: + BT_DBG("Exit LOCAL_BUSY"); + clear_bit(CONN_LOCAL_BUSY, &chan->conn_state); + + if (test_bit(CONN_RNR_SENT, &chan->conn_state)) { + struct l2cap_ctrl local_control; + + memset(&local_control, 0, sizeof(local_control)); + local_control.sframe = 1; + local_control.super = L2CAP_SUPER_RR; + local_control.poll = 1; + local_control.reqseq = chan->buffer_seq; + l2cap_send_sframe(chan, &local_control); + + chan->retry_count = 1; + __set_monitor_timer(chan); + chan->tx_state = L2CAP_TX_STATE_WAIT_F; + } + break; + case L2CAP_EV_RECV_REQSEQ_AND_FBIT: + l2cap_process_reqseq(chan, control->reqseq); + break; + case L2CAP_EV_EXPLICIT_POLL: + l2cap_send_rr_or_rnr(chan, 1); + chan->retry_count = 1; + __set_monitor_timer(chan); + __clear_ack_timer(chan); + chan->tx_state = L2CAP_TX_STATE_WAIT_F; + break; + case L2CAP_EV_RETRANS_TO: + l2cap_send_rr_or_rnr(chan, 1); + chan->retry_count = 1; + __set_monitor_timer(chan); + chan->tx_state = L2CAP_TX_STATE_WAIT_F; + break; + case L2CAP_EV_RECV_FBIT: + /* Nothing to process */ + break; + default: + break; + } +} + +static void l2cap_tx_state_wait_f(struct l2cap_chan *chan, + struct l2cap_ctrl *control, + struct sk_buff_head *skbs, u8 event) +{ + BT_DBG("chan %p, control %p, skbs %p, event %d", chan, control, skbs, + event); + + switch (event) { + case L2CAP_EV_DATA_REQUEST: + if (chan->tx_send_head == NULL) + chan->tx_send_head = skb_peek(skbs); + /* Queue data, but don't send. */ + skb_queue_splice_tail_init(skbs, &chan->tx_q); + break; + case L2CAP_EV_LOCAL_BUSY_DETECTED: + BT_DBG("Enter LOCAL_BUSY"); + set_bit(CONN_LOCAL_BUSY, &chan->conn_state); + + if (chan->rx_state == L2CAP_RX_STATE_SREJ_SENT) { + /* The SREJ_SENT state must be aborted if we are to + * enter the LOCAL_BUSY state. + */ + l2cap_abort_rx_srej_sent(chan); + } + + l2cap_send_ack(chan); + + break; + case L2CAP_EV_LOCAL_BUSY_CLEAR: + BT_DBG("Exit LOCAL_BUSY"); + clear_bit(CONN_LOCAL_BUSY, &chan->conn_state); + + if (test_bit(CONN_RNR_SENT, &chan->conn_state)) { + struct l2cap_ctrl local_control; + memset(&local_control, 0, sizeof(local_control)); + local_control.sframe = 1; + local_control.super = L2CAP_SUPER_RR; + local_control.poll = 1; + local_control.reqseq = chan->buffer_seq; + l2cap_send_sframe(chan, &local_control); + + chan->retry_count = 1; + __set_monitor_timer(chan); + chan->tx_state = L2CAP_TX_STATE_WAIT_F; + } + break; + case L2CAP_EV_RECV_REQSEQ_AND_FBIT: + l2cap_process_reqseq(chan, control->reqseq); + + /* Fall through */ + + case L2CAP_EV_RECV_FBIT: + if (control && control->final) { + __clear_monitor_timer(chan); + if (chan->unacked_frames > 0) + __set_retrans_timer(chan); + chan->retry_count = 0; + chan->tx_state = L2CAP_TX_STATE_XMIT; + BT_DBG("recv fbit tx_state 0x2.2%x", chan->tx_state); + } + break; + case L2CAP_EV_EXPLICIT_POLL: + /* Ignore */ + break; + case L2CAP_EV_MONITOR_TO: + if (chan->max_tx == 0 || chan->retry_count < chan->max_tx) { + l2cap_send_rr_or_rnr(chan, 1); + __set_monitor_timer(chan); + chan->retry_count++; + } else { + l2cap_send_disconn_req(chan, ECONNABORTED); + } + break; + default: + break; + } +} + +static void l2cap_tx(struct l2cap_chan *chan, struct l2cap_ctrl *control, + struct sk_buff_head *skbs, u8 event) +{ + BT_DBG("chan %p, control %p, skbs %p, event %d, state %d", + chan, control, skbs, event, chan->tx_state); + + switch (chan->tx_state) { + case L2CAP_TX_STATE_XMIT: + l2cap_tx_state_xmit(chan, control, skbs, event); + break; + case L2CAP_TX_STATE_WAIT_F: + l2cap_tx_state_wait_f(chan, control, skbs, event); + break; + default: + /* Ignore event */ + break; } } +static void l2cap_pass_to_tx(struct l2cap_chan *chan, + struct l2cap_ctrl *control) +{ + BT_DBG("chan %p, control %p", chan, control); + l2cap_tx(chan, control, NULL, L2CAP_EV_RECV_REQSEQ_AND_FBIT); +} + +static void l2cap_pass_to_tx_fbit(struct l2cap_chan *chan, + struct l2cap_ctrl *control) +{ + BT_DBG("chan %p, control %p", chan, control); + l2cap_tx(chan, control, NULL, L2CAP_EV_RECV_FBIT); +} + /* Copy frame to all raw sockets on that connection */ static void l2cap_raw_recv(struct l2cap_conn *conn, struct sk_buff *skb) { - struct l2cap_chan_list *l = &conn->chan_list; struct sk_buff *nskb; - struct sock *sk; + struct l2cap_chan *chan; BT_DBG("conn %p", conn); - read_lock(&l->lock); - for (sk = l->head; sk; sk = l2cap_pi(sk)->next_c) { - if (sk->sk_type != SOCK_RAW) + mutex_lock(&conn->chan_lock); + + list_for_each_entry(chan, &conn->chan_l, list) { + if (chan->chan_type != L2CAP_CHAN_RAW) continue; - /* Don't send frame to the socket it came from */ - if (skb->sk == sk) + /* Don't send frame to the channel it came from */ + if (bt_cb(skb)->chan == chan) continue; - nskb = skb_clone(skb, GFP_ATOMIC); + + nskb = skb_clone(skb, GFP_KERNEL); if (!nskb) continue; - - if (sock_queue_rcv_skb(sk, nskb)) + if (chan->ops->recv(chan, nskb)) kfree_skb(nskb); } - read_unlock(&l->lock); + + mutex_unlock(&conn->chan_lock); } /* ---- L2CAP signalling commands ---- */ -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_cmd(struct l2cap_conn *conn, u8 code, + u8 ident, u16 dlen, void *data) { struct sk_buff *skb, **frag; struct l2cap_cmd_hdr *cmd; struct l2cap_hdr *lh; int len, count; - BT_DBG("conn %p, code 0x%2.2x, ident 0x%2.2x, len %d", - conn, code, ident, dlen); + BT_DBG("conn %p, code 0x%2.2x, ident 0x%2.2x, len %u", + conn, code, ident, dlen); + + if (conn->mtu < L2CAP_HDR_SIZE + L2CAP_CMD_HDR_SIZE) + return NULL; len = L2CAP_HDR_SIZE + L2CAP_CMD_HDR_SIZE + dlen; count = min_t(unsigned int, conn->mtu, len); - skb = bt_skb_alloc(count, GFP_ATOMIC); + skb = bt_skb_alloc(count, GFP_KERNEL); if (!skb) return NULL; @@ -1455,7 +2929,7 @@ static struct sk_buff *l2cap_build_cmd(struct l2cap_conn *conn, while (len) { count = min_t(unsigned int, conn->mtu, len); - *frag = bt_skb_alloc(count, GFP_ATOMIC); + *frag = bt_skb_alloc(count, GFP_KERNEL); if (!*frag) goto fail; @@ -1474,7 +2948,8 @@ fail: return NULL; } -static inline int l2cap_get_conf_opt(void **ptr, int *type, int *olen, unsigned long *val) +static inline int l2cap_get_conf_opt(void **ptr, int *type, int *olen, + unsigned long *val) { struct l2cap_conf_opt *opt = *ptr; int len; @@ -1503,7 +2978,7 @@ static inline int l2cap_get_conf_opt(void **ptr, int *type, int *olen, unsigned break; } - BT_DBG("type 0x%2.2x len %d val 0x%lx", *type, opt->len, *val); + BT_DBG("type 0x%2.2x len %u val 0x%lx", *type, opt->len, *val); return len; } @@ -1511,7 +2986,7 @@ static void l2cap_add_conf_opt(void **ptr, u8 type, u8 len, unsigned long val) { struct l2cap_conf_opt *opt = *ptr; - BT_DBG("type 0x%2.2x len %d val 0x%lx", type, len, val); + BT_DBG("type 0x%2.2x len %u val 0x%lx", type, len, val); opt->type = type; opt->len = len; @@ -1537,36 +3012,100 @@ static void l2cap_add_conf_opt(void **ptr, u8 type, u8 len, unsigned long val) *ptr += L2CAP_CONF_OPT_SIZE + len; } -static void l2cap_ack_timeout(unsigned long arg) +static void l2cap_add_opt_efs(void **ptr, struct l2cap_chan *chan) +{ + struct l2cap_conf_efs efs; + + switch (chan->mode) { + case L2CAP_MODE_ERTM: + efs.id = chan->local_id; + efs.stype = chan->local_stype; + efs.msdu = cpu_to_le16(chan->local_msdu); + efs.sdu_itime = cpu_to_le32(chan->local_sdu_itime); + efs.acc_lat = cpu_to_le32(L2CAP_DEFAULT_ACC_LAT); + efs.flush_to = cpu_to_le32(L2CAP_EFS_DEFAULT_FLUSH_TO); + break; + + case L2CAP_MODE_STREAMING: + efs.id = 1; + efs.stype = L2CAP_SERV_BESTEFFORT; + efs.msdu = cpu_to_le16(chan->local_msdu); + efs.sdu_itime = cpu_to_le32(chan->local_sdu_itime); + efs.acc_lat = 0; + efs.flush_to = 0; + break; + + default: + return; + } + + l2cap_add_conf_opt(ptr, L2CAP_CONF_EFS, sizeof(efs), + (unsigned long) &efs); +} + +static void l2cap_ack_timeout(struct work_struct *work) { - struct sock *sk = (void *) arg; + struct l2cap_chan *chan = container_of(work, struct l2cap_chan, + ack_timer.work); + u16 frames_to_ack; + + BT_DBG("chan %p", chan); + + l2cap_chan_lock(chan); + + frames_to_ack = __seq_offset(chan, chan->buffer_seq, + chan->last_acked_seq); + + if (frames_to_ack) + l2cap_send_rr_or_rnr(chan, 0); - bh_lock_sock(sk); - l2cap_send_ack(l2cap_pi(sk)); - bh_unlock_sock(sk); + l2cap_chan_unlock(chan); + l2cap_chan_put(chan); } -static inline void l2cap_ertm_init(struct sock *sk) +int l2cap_ertm_init(struct l2cap_chan *chan) { - l2cap_pi(sk)->expected_ack_seq = 0; - l2cap_pi(sk)->unacked_frames = 0; - l2cap_pi(sk)->buffer_seq = 0; - l2cap_pi(sk)->num_acked = 0; - l2cap_pi(sk)->frames_sent = 0; + int err; + + chan->next_tx_seq = 0; + chan->expected_tx_seq = 0; + chan->expected_ack_seq = 0; + chan->unacked_frames = 0; + chan->buffer_seq = 0; + chan->frames_sent = 0; + chan->last_acked_seq = 0; + chan->sdu = NULL; + chan->sdu_last_frag = NULL; + chan->sdu_len = 0; + + skb_queue_head_init(&chan->tx_q); + + chan->local_amp_id = AMP_ID_BREDR; + chan->move_id = AMP_ID_BREDR; + chan->move_state = L2CAP_MOVE_STABLE; + chan->move_role = L2CAP_MOVE_ROLE_NONE; + + if (chan->mode != L2CAP_MODE_ERTM) + return 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); - setup_timer(&l2cap_pi(sk)->ack_timer, - l2cap_ack_timeout, (unsigned long) sk); + chan->rx_state = L2CAP_RX_STATE_RECV; + chan->tx_state = L2CAP_TX_STATE_XMIT; - __skb_queue_head_init(SREJ_QUEUE(sk)); - __skb_queue_head_init(BUSY_QUEUE(sk)); + INIT_DELAYED_WORK(&chan->retrans_timer, l2cap_retrans_timeout); + INIT_DELAYED_WORK(&chan->monitor_timer, l2cap_monitor_timeout); + INIT_DELAYED_WORK(&chan->ack_timer, l2cap_ack_timeout); - INIT_WORK(&l2cap_pi(sk)->busy_work, l2cap_busy_work); + skb_queue_head_init(&chan->srej_q); - sk->sk_backlog_rcv = l2cap_ertm_data_rcv; + err = l2cap_seq_list_init(&chan->srej_list, chan->tx_win); + if (err < 0) + return err; + + err = l2cap_seq_list_init(&chan->retrans_list, chan->remote_tx_win); + if (err < 0) + l2cap_seq_list_free(&chan->srej_list); + + return err; } static inline __u8 l2cap_select_mode(__u8 mode, __u16 remote_feat_mask) @@ -1582,38 +3121,104 @@ static inline __u8 l2cap_select_mode(__u8 mode, __u16 remote_feat_mask) } } -int l2cap_build_conf_req(struct sock *sk, void *data) +static inline bool __l2cap_ews_supported(struct l2cap_conn *conn) +{ + return conn->hs_enabled && conn->feat_mask & L2CAP_FEAT_EXT_WINDOW; +} + +static inline bool __l2cap_efs_supported(struct l2cap_conn *conn) +{ + return conn->hs_enabled && conn->feat_mask & L2CAP_FEAT_EXT_FLOW; +} + +static void __l2cap_set_ertm_timeouts(struct l2cap_chan *chan, + struct l2cap_conf_rfc *rfc) +{ + if (chan->local_amp_id != AMP_ID_BREDR && chan->hs_hcon) { + u64 ertm_to = chan->hs_hcon->hdev->amp_be_flush_to; + + /* Class 1 devices have must have ERTM timeouts + * exceeding the Link Supervision Timeout. The + * default Link Supervision Timeout for AMP + * controllers is 10 seconds. + * + * Class 1 devices use 0xffffffff for their + * best-effort flush timeout, so the clamping logic + * will result in a timeout that meets the above + * requirement. ERTM timeouts are 16-bit values, so + * the maximum timeout is 65.535 seconds. + */ + + /* Convert timeout to milliseconds and round */ + ertm_to = DIV_ROUND_UP_ULL(ertm_to, 1000); + + /* This is the recommended formula for class 2 devices + * that start ERTM timers when packets are sent to the + * controller. + */ + ertm_to = 3 * ertm_to + 500; + + if (ertm_to > 0xffff) + ertm_to = 0xffff; + + rfc->retrans_timeout = cpu_to_le16((u16) ertm_to); + rfc->monitor_timeout = rfc->retrans_timeout; + } else { + rfc->retrans_timeout = cpu_to_le16(L2CAP_DEFAULT_RETRANS_TO); + rfc->monitor_timeout = cpu_to_le16(L2CAP_DEFAULT_MONITOR_TO); + } +} + +static inline void l2cap_txwin_setup(struct l2cap_chan *chan) +{ + if (chan->tx_win > L2CAP_DEFAULT_TX_WINDOW && + __l2cap_ews_supported(chan->conn)) { + /* use extended control field */ + set_bit(FLAG_EXT_CTRL, &chan->flags); + chan->tx_win_max = L2CAP_DEFAULT_EXT_WINDOW; + } else { + chan->tx_win = min_t(u16, chan->tx_win, + L2CAP_DEFAULT_TX_WINDOW); + chan->tx_win_max = L2CAP_DEFAULT_TX_WINDOW; + } + chan->ack_win = chan->tx_win; +} + +static int l2cap_build_conf_req(struct l2cap_chan *chan, void *data) { - struct l2cap_pinfo *pi = l2cap_pi(sk); struct l2cap_conf_req *req = data; - struct l2cap_conf_rfc rfc = { .mode = pi->mode }; + struct l2cap_conf_rfc rfc = { .mode = chan->mode }; void *ptr = req->data; + u16 size; - BT_DBG("sk %p", sk); + BT_DBG("chan %p", chan); - if (pi->num_conf_req || pi->num_conf_rsp) + if (chan->num_conf_req || chan->num_conf_rsp) goto done; - switch (pi->mode) { + switch (chan->mode) { case L2CAP_MODE_STREAMING: case L2CAP_MODE_ERTM: - if (pi->conf_state & L2CAP_CONF_STATE2_DEVICE) + if (test_bit(CONF_STATE2_DEVICE, &chan->conf_state)) break; + if (__l2cap_efs_supported(chan->conn)) + set_bit(FLAG_EFS_ENABLE, &chan->flags); + /* fall through */ default: - pi->mode = l2cap_select_mode(rfc.mode, pi->conn->feat_mask); + chan->mode = l2cap_select_mode(rfc.mode, chan->conn->feat_mask); break; } done: - if (pi->imtu != L2CAP_DEFAULT_MTU) - l2cap_add_conf_opt(&ptr, L2CAP_CONF_MTU, 2, pi->imtu); + if (chan->imtu != L2CAP_DEFAULT_MTU) + l2cap_add_conf_opt(&ptr, L2CAP_CONF_MTU, 2, chan->imtu); - switch (pi->mode) { + switch (chan->mode) { case L2CAP_MODE_BASIC: - if (!(pi->conn->feat_mask & L2CAP_FEAT_ERTM) && - !(pi->conn->feat_mask & L2CAP_FEAT_STREAMING)) + if (!(chan->conn->feat_mask & L2CAP_FEAT_ERTM) && + !(chan->conn->feat_mask & L2CAP_FEAT_STREAMING)) break; rfc.mode = L2CAP_MODE_BASIC; @@ -1624,76 +3229,95 @@ done: rfc.max_pdu_size = 0; l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC, sizeof(rfc), - (unsigned long) &rfc); + (unsigned long) &rfc); break; case L2CAP_MODE_ERTM: rfc.mode = L2CAP_MODE_ERTM; - rfc.txwin_size = pi->tx_win; - rfc.max_transmit = pi->max_tx; - rfc.retrans_timeout = 0; - rfc.monitor_timeout = 0; - rfc.max_pdu_size = cpu_to_le16(L2CAP_DEFAULT_MAX_PDU_SIZE); - if (L2CAP_DEFAULT_MAX_PDU_SIZE > pi->conn->mtu - 10) - rfc.max_pdu_size = cpu_to_le16(pi->conn->mtu - 10); + rfc.max_transmit = chan->max_tx; + + __l2cap_set_ertm_timeouts(chan, &rfc); + + size = min_t(u16, L2CAP_DEFAULT_MAX_PDU_SIZE, chan->conn->mtu - + L2CAP_EXT_HDR_SIZE - L2CAP_SDULEN_SIZE - + L2CAP_FCS_SIZE); + rfc.max_pdu_size = cpu_to_le16(size); + + l2cap_txwin_setup(chan); + + rfc.txwin_size = min_t(u16, chan->tx_win, + L2CAP_DEFAULT_TX_WINDOW); l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC, sizeof(rfc), - (unsigned long) &rfc); + (unsigned long) &rfc); - if (!(pi->conn->feat_mask & L2CAP_FEAT_FCS)) - break; + if (test_bit(FLAG_EFS_ENABLE, &chan->flags)) + l2cap_add_opt_efs(&ptr, chan); - if (pi->fcs == L2CAP_FCS_NONE || - pi->conf_state & L2CAP_CONF_NO_FCS_RECV) { - pi->fcs = L2CAP_FCS_NONE; - l2cap_add_conf_opt(&ptr, L2CAP_CONF_FCS, 1, pi->fcs); - } + if (test_bit(FLAG_EXT_CTRL, &chan->flags)) + l2cap_add_conf_opt(&ptr, L2CAP_CONF_EWS, 2, + chan->tx_win); + + if (chan->conn->feat_mask & L2CAP_FEAT_FCS) + if (chan->fcs == L2CAP_FCS_NONE || + test_bit(CONF_RECV_NO_FCS, &chan->conf_state)) { + chan->fcs = L2CAP_FCS_NONE; + l2cap_add_conf_opt(&ptr, L2CAP_CONF_FCS, 1, + chan->fcs); + } break; case L2CAP_MODE_STREAMING: + l2cap_txwin_setup(chan); rfc.mode = L2CAP_MODE_STREAMING; rfc.txwin_size = 0; rfc.max_transmit = 0; rfc.retrans_timeout = 0; rfc.monitor_timeout = 0; - rfc.max_pdu_size = cpu_to_le16(L2CAP_DEFAULT_MAX_PDU_SIZE); - if (L2CAP_DEFAULT_MAX_PDU_SIZE > pi->conn->mtu - 10) - rfc.max_pdu_size = cpu_to_le16(pi->conn->mtu - 10); + + size = min_t(u16, L2CAP_DEFAULT_MAX_PDU_SIZE, chan->conn->mtu - + L2CAP_EXT_HDR_SIZE - L2CAP_SDULEN_SIZE - + L2CAP_FCS_SIZE); + rfc.max_pdu_size = cpu_to_le16(size); l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC, sizeof(rfc), - (unsigned long) &rfc); + (unsigned long) &rfc); - if (!(pi->conn->feat_mask & L2CAP_FEAT_FCS)) - break; + if (test_bit(FLAG_EFS_ENABLE, &chan->flags)) + l2cap_add_opt_efs(&ptr, chan); - if (pi->fcs == L2CAP_FCS_NONE || - pi->conf_state & L2CAP_CONF_NO_FCS_RECV) { - pi->fcs = L2CAP_FCS_NONE; - l2cap_add_conf_opt(&ptr, L2CAP_CONF_FCS, 1, pi->fcs); - } + if (chan->conn->feat_mask & L2CAP_FEAT_FCS) + if (chan->fcs == L2CAP_FCS_NONE || + test_bit(CONF_RECV_NO_FCS, &chan->conf_state)) { + chan->fcs = L2CAP_FCS_NONE; + l2cap_add_conf_opt(&ptr, L2CAP_CONF_FCS, 1, + chan->fcs); + } break; } - req->dcid = cpu_to_le16(pi->dcid); + req->dcid = cpu_to_le16(chan->dcid); req->flags = cpu_to_le16(0); return ptr - data; } -static int l2cap_parse_conf_req(struct sock *sk, void *data) +static int l2cap_parse_conf_req(struct l2cap_chan *chan, void *data) { - struct l2cap_pinfo *pi = l2cap_pi(sk); struct l2cap_conf_rsp *rsp = data; void *ptr = rsp->data; - void *req = pi->conf_req; - int len = pi->conf_len; + void *req = chan->conf_req; + int len = chan->conf_len; int type, hint, olen; unsigned long val; struct l2cap_conf_rfc rfc = { .mode = L2CAP_MODE_BASIC }; + struct l2cap_conf_efs efs; + u8 remote_efs = 0; u16 mtu = L2CAP_DEFAULT_MTU; u16 result = L2CAP_CONF_SUCCESS; + u16 size; - BT_DBG("sk %p", sk); + BT_DBG("chan %p", chan); while (len >= L2CAP_CONF_OPT_SIZE) { len -= l2cap_get_conf_opt(&req, &type, &olen, &val); @@ -1707,7 +3331,7 @@ static int l2cap_parse_conf_req(struct sock *sk, void *data) break; case L2CAP_CONF_FLUSH_TO: - pi->flush_to = val; + chan->flush_to = val; break; case L2CAP_CONF_QOS: @@ -1720,8 +3344,23 @@ static int l2cap_parse_conf_req(struct sock *sk, void *data) case L2CAP_CONF_FCS: if (val == L2CAP_FCS_NONE) - pi->conf_state |= L2CAP_CONF_NO_FCS_RECV; + set_bit(CONF_RECV_NO_FCS, &chan->conf_state); + break; + case L2CAP_CONF_EFS: + remote_efs = 1; + if (olen == sizeof(efs)) + memcpy(&efs, (void *) val, olen); + break; + + case L2CAP_CONF_EWS: + if (!chan->conn->hs_enabled) + return -ECONNREFUSED; + + set_bit(FLAG_EXT_CTRL, &chan->flags); + set_bit(CONF_EWS_RECV, &chan->conf_state); + chan->tx_win_max = L2CAP_DEFAULT_EXT_WINDOW; + chan->remote_tx_win = val; break; default: @@ -1734,37 +3373,43 @@ static int l2cap_parse_conf_req(struct sock *sk, void *data) } } - if (pi->num_conf_rsp || pi->num_conf_req > 1) + if (chan->num_conf_rsp || chan->num_conf_req > 1) goto done; - switch (pi->mode) { + switch (chan->mode) { case L2CAP_MODE_STREAMING: case L2CAP_MODE_ERTM: - if (!(pi->conf_state & L2CAP_CONF_STATE2_DEVICE)) { - pi->mode = l2cap_select_mode(rfc.mode, - pi->conn->feat_mask); + if (!test_bit(CONF_STATE2_DEVICE, &chan->conf_state)) { + chan->mode = l2cap_select_mode(rfc.mode, + chan->conn->feat_mask); break; } - if (pi->mode != rfc.mode) + if (remote_efs) { + if (__l2cap_efs_supported(chan->conn)) + set_bit(FLAG_EFS_ENABLE, &chan->flags); + else + return -ECONNREFUSED; + } + + if (chan->mode != rfc.mode) return -ECONNREFUSED; break; } done: - if (pi->mode != rfc.mode) { + if (chan->mode != rfc.mode) { result = L2CAP_CONF_UNACCEPT; - rfc.mode = pi->mode; + rfc.mode = chan->mode; - if (pi->num_conf_rsp == 1) + if (chan->num_conf_rsp == 1) return -ECONNREFUSED; - l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC, - sizeof(rfc), (unsigned long) &rfc); + l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC, sizeof(rfc), + (unsigned long) &rfc); } - if (result == L2CAP_CONF_SUCCESS) { /* Configure output options and let the other side know * which ones we don't like. */ @@ -1772,48 +3417,85 @@ done: if (mtu < L2CAP_DEFAULT_MIN_MTU) result = L2CAP_CONF_UNACCEPT; else { - pi->omtu = mtu; - pi->conf_state |= L2CAP_CONF_MTU_DONE; + chan->omtu = mtu; + set_bit(CONF_MTU_DONE, &chan->conf_state); + } + l2cap_add_conf_opt(&ptr, L2CAP_CONF_MTU, 2, chan->omtu); + + if (remote_efs) { + if (chan->local_stype != L2CAP_SERV_NOTRAFIC && + efs.stype != L2CAP_SERV_NOTRAFIC && + efs.stype != chan->local_stype) { + + result = L2CAP_CONF_UNACCEPT; + + if (chan->num_conf_req >= 1) + return -ECONNREFUSED; + + l2cap_add_conf_opt(&ptr, L2CAP_CONF_EFS, + sizeof(efs), + (unsigned long) &efs); + } else { + /* Send PENDING Conf Rsp */ + result = L2CAP_CONF_PENDING; + set_bit(CONF_LOC_CONF_PEND, &chan->conf_state); + } } - l2cap_add_conf_opt(&ptr, L2CAP_CONF_MTU, 2, pi->omtu); switch (rfc.mode) { case L2CAP_MODE_BASIC: - pi->fcs = L2CAP_FCS_NONE; - pi->conf_state |= L2CAP_CONF_MODE_DONE; + chan->fcs = L2CAP_FCS_NONE; + set_bit(CONF_MODE_DONE, &chan->conf_state); break; case L2CAP_MODE_ERTM: - pi->remote_tx_win = rfc.txwin_size; - pi->remote_max_tx = rfc.max_transmit; + if (!test_bit(CONF_EWS_RECV, &chan->conf_state)) + chan->remote_tx_win = rfc.txwin_size; + else + rfc.txwin_size = L2CAP_DEFAULT_TX_WINDOW; - if (le16_to_cpu(rfc.max_pdu_size) > pi->conn->mtu - 10) - rfc.max_pdu_size = cpu_to_le16(pi->conn->mtu - 10); + chan->remote_max_tx = rfc.max_transmit; - pi->remote_mps = le16_to_cpu(rfc.max_pdu_size); + size = min_t(u16, le16_to_cpu(rfc.max_pdu_size), + chan->conn->mtu - L2CAP_EXT_HDR_SIZE - + L2CAP_SDULEN_SIZE - L2CAP_FCS_SIZE); + rfc.max_pdu_size = cpu_to_le16(size); + chan->remote_mps = size; - rfc.retrans_timeout = - le16_to_cpu(L2CAP_DEFAULT_RETRANS_TO); - rfc.monitor_timeout = - le16_to_cpu(L2CAP_DEFAULT_MONITOR_TO); + __l2cap_set_ertm_timeouts(chan, &rfc); - pi->conf_state |= L2CAP_CONF_MODE_DONE; + set_bit(CONF_MODE_DONE, &chan->conf_state); l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC, - sizeof(rfc), (unsigned long) &rfc); - + sizeof(rfc), (unsigned long) &rfc); + + if (test_bit(FLAG_EFS_ENABLE, &chan->flags)) { + chan->remote_id = efs.id; + chan->remote_stype = efs.stype; + chan->remote_msdu = le16_to_cpu(efs.msdu); + chan->remote_flush_to = + le32_to_cpu(efs.flush_to); + chan->remote_acc_lat = + le32_to_cpu(efs.acc_lat); + chan->remote_sdu_itime = + le32_to_cpu(efs.sdu_itime); + l2cap_add_conf_opt(&ptr, L2CAP_CONF_EFS, + sizeof(efs), + (unsigned long) &efs); + } break; case L2CAP_MODE_STREAMING: - if (le16_to_cpu(rfc.max_pdu_size) > pi->conn->mtu - 10) - rfc.max_pdu_size = cpu_to_le16(pi->conn->mtu - 10); + size = min_t(u16, le16_to_cpu(rfc.max_pdu_size), + chan->conn->mtu - L2CAP_EXT_HDR_SIZE - + L2CAP_SDULEN_SIZE - L2CAP_FCS_SIZE); + rfc.max_pdu_size = cpu_to_le16(size); + chan->remote_mps = size; - pi->remote_mps = le16_to_cpu(rfc.max_pdu_size); + set_bit(CONF_MODE_DONE, &chan->conf_state); - pi->conf_state |= L2CAP_CONF_MODE_DONE; - - l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC, - sizeof(rfc), (unsigned long) &rfc); + l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC, sizeof(rfc), + (unsigned long) &rfc); break; @@ -1821,29 +3503,30 @@ done: result = L2CAP_CONF_UNACCEPT; memset(&rfc, 0, sizeof(rfc)); - rfc.mode = pi->mode; + rfc.mode = chan->mode; } if (result == L2CAP_CONF_SUCCESS) - pi->conf_state |= L2CAP_CONF_OUTPUT_DONE; + set_bit(CONF_OUTPUT_DONE, &chan->conf_state); } - rsp->scid = cpu_to_le16(pi->dcid); + rsp->scid = cpu_to_le16(chan->dcid); rsp->result = cpu_to_le16(result); - rsp->flags = cpu_to_le16(0x0000); + rsp->flags = cpu_to_le16(0); return ptr - data; } -static int l2cap_parse_conf_rsp(struct sock *sk, void *rsp, int len, void *data, u16 *result) +static int l2cap_parse_conf_rsp(struct l2cap_chan *chan, void *rsp, int len, + void *data, u16 *result) { - struct l2cap_pinfo *pi = l2cap_pi(sk); struct l2cap_conf_req *req = data; void *ptr = req->data; int type, olen; unsigned long val; - struct l2cap_conf_rfc rfc; + struct l2cap_conf_rfc rfc = { .mode = L2CAP_MODE_BASIC }; + struct l2cap_conf_efs efs; - BT_DBG("sk %p, rsp %p, len %d, req %p", sk, rsp, len, data); + BT_DBG("chan %p, rsp %p, len %d, req %p", chan, rsp, len, data); while (len >= L2CAP_CONF_OPT_SIZE) { len -= l2cap_get_conf_opt(&rsp, &type, &olen, &val); @@ -1852,81 +3535,176 @@ static int l2cap_parse_conf_rsp(struct sock *sk, void *rsp, int len, void *data, case L2CAP_CONF_MTU: if (val < L2CAP_DEFAULT_MIN_MTU) { *result = L2CAP_CONF_UNACCEPT; - pi->imtu = L2CAP_DEFAULT_MIN_MTU; + chan->imtu = L2CAP_DEFAULT_MIN_MTU; } else - pi->imtu = val; - l2cap_add_conf_opt(&ptr, L2CAP_CONF_MTU, 2, pi->imtu); + chan->imtu = val; + l2cap_add_conf_opt(&ptr, L2CAP_CONF_MTU, 2, chan->imtu); break; case L2CAP_CONF_FLUSH_TO: - pi->flush_to = val; + chan->flush_to = val; l2cap_add_conf_opt(&ptr, L2CAP_CONF_FLUSH_TO, - 2, pi->flush_to); + 2, chan->flush_to); break; case L2CAP_CONF_RFC: if (olen == sizeof(rfc)) memcpy(&rfc, (void *)val, olen); - if ((pi->conf_state & L2CAP_CONF_STATE2_DEVICE) && - rfc.mode != pi->mode) + if (test_bit(CONF_STATE2_DEVICE, &chan->conf_state) && + rfc.mode != chan->mode) return -ECONNREFUSED; - pi->fcs = 0; + chan->fcs = 0; l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC, - sizeof(rfc), (unsigned long) &rfc); + sizeof(rfc), (unsigned long) &rfc); + break; + + case L2CAP_CONF_EWS: + chan->ack_win = min_t(u16, val, chan->ack_win); + l2cap_add_conf_opt(&ptr, L2CAP_CONF_EWS, 2, + chan->tx_win); + break; + + case L2CAP_CONF_EFS: + if (olen == sizeof(efs)) + memcpy(&efs, (void *)val, olen); + + if (chan->local_stype != L2CAP_SERV_NOTRAFIC && + efs.stype != L2CAP_SERV_NOTRAFIC && + efs.stype != chan->local_stype) + return -ECONNREFUSED; + + l2cap_add_conf_opt(&ptr, L2CAP_CONF_EFS, sizeof(efs), + (unsigned long) &efs); + break; + + case L2CAP_CONF_FCS: + if (*result == L2CAP_CONF_PENDING) + if (val == L2CAP_FCS_NONE) + set_bit(CONF_RECV_NO_FCS, + &chan->conf_state); break; } } - if (pi->mode == L2CAP_MODE_BASIC && pi->mode != rfc.mode) + if (chan->mode == L2CAP_MODE_BASIC && chan->mode != rfc.mode) return -ECONNREFUSED; - pi->mode = rfc.mode; + chan->mode = rfc.mode; - if (*result == L2CAP_CONF_SUCCESS) { + if (*result == L2CAP_CONF_SUCCESS || *result == L2CAP_CONF_PENDING) { switch (rfc.mode) { case L2CAP_MODE_ERTM: - pi->retrans_timeout = le16_to_cpu(rfc.retrans_timeout); - pi->monitor_timeout = le16_to_cpu(rfc.monitor_timeout); - pi->mps = le16_to_cpu(rfc.max_pdu_size); + chan->retrans_timeout = le16_to_cpu(rfc.retrans_timeout); + chan->monitor_timeout = le16_to_cpu(rfc.monitor_timeout); + chan->mps = le16_to_cpu(rfc.max_pdu_size); + if (!test_bit(FLAG_EXT_CTRL, &chan->flags)) + chan->ack_win = min_t(u16, chan->ack_win, + rfc.txwin_size); + + if (test_bit(FLAG_EFS_ENABLE, &chan->flags)) { + chan->local_msdu = le16_to_cpu(efs.msdu); + chan->local_sdu_itime = + le32_to_cpu(efs.sdu_itime); + chan->local_acc_lat = le32_to_cpu(efs.acc_lat); + chan->local_flush_to = + le32_to_cpu(efs.flush_to); + } break; + case L2CAP_MODE_STREAMING: - pi->mps = le16_to_cpu(rfc.max_pdu_size); + chan->mps = le16_to_cpu(rfc.max_pdu_size); } } - req->dcid = cpu_to_le16(pi->dcid); - req->flags = cpu_to_le16(0x0000); + req->dcid = cpu_to_le16(chan->dcid); + req->flags = cpu_to_le16(0); return ptr - data; } -static int l2cap_build_conf_rsp(struct sock *sk, void *data, u16 result, u16 flags) +static int l2cap_build_conf_rsp(struct l2cap_chan *chan, void *data, + u16 result, u16 flags) { struct l2cap_conf_rsp *rsp = data; void *ptr = rsp->data; - BT_DBG("sk %p", sk); + BT_DBG("chan %p", chan); - rsp->scid = cpu_to_le16(l2cap_pi(sk)->dcid); + rsp->scid = cpu_to_le16(chan->dcid); rsp->result = cpu_to_le16(result); rsp->flags = cpu_to_le16(flags); return ptr - data; } -static void l2cap_conf_rfc_get(struct sock *sk, void *rsp, int len) +void __l2cap_le_connect_rsp_defer(struct l2cap_chan *chan) { - struct l2cap_pinfo *pi = l2cap_pi(sk); - int type, olen; - unsigned long val; - struct l2cap_conf_rfc rfc; + struct l2cap_le_conn_rsp rsp; + struct l2cap_conn *conn = chan->conn; + + BT_DBG("chan %p", chan); + + rsp.dcid = cpu_to_le16(chan->scid); + rsp.mtu = cpu_to_le16(chan->imtu); + rsp.mps = cpu_to_le16(chan->mps); + rsp.credits = cpu_to_le16(chan->rx_credits); + rsp.result = cpu_to_le16(L2CAP_CR_SUCCESS); + + l2cap_send_cmd(conn, chan->ident, L2CAP_LE_CONN_RSP, sizeof(rsp), + &rsp); +} + +void __l2cap_connect_rsp_defer(struct l2cap_chan *chan) +{ + struct l2cap_conn_rsp rsp; + struct l2cap_conn *conn = chan->conn; + u8 buf[128]; + u8 rsp_code; + + rsp.scid = cpu_to_le16(chan->dcid); + rsp.dcid = cpu_to_le16(chan->scid); + rsp.result = cpu_to_le16(L2CAP_CR_SUCCESS); + rsp.status = cpu_to_le16(L2CAP_CS_NO_INFO); + + if (chan->hs_hcon) + rsp_code = L2CAP_CREATE_CHAN_RSP; + else + rsp_code = L2CAP_CONN_RSP; + + BT_DBG("chan %p rsp_code %u", chan, rsp_code); + + l2cap_send_cmd(conn, chan->ident, rsp_code, sizeof(rsp), &rsp); + + if (test_and_set_bit(CONF_REQ_SENT, &chan->conf_state)) + return; - BT_DBG("sk %p, rsp %p, len %d", sk, rsp, len); + l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ, + l2cap_build_conf_req(chan, buf), buf); + chan->num_conf_req++; +} - if ((pi->mode != L2CAP_MODE_ERTM) && (pi->mode != L2CAP_MODE_STREAMING)) +static void l2cap_conf_rfc_get(struct l2cap_chan *chan, void *rsp, int len) +{ + int type, olen; + unsigned long val; + /* Use sane default values in case a misbehaving remote device + * did not send an RFC or extended window size option. + */ + u16 txwin_ext = chan->ack_win; + struct l2cap_conf_rfc rfc = { + .mode = chan->mode, + .retrans_timeout = cpu_to_le16(L2CAP_DEFAULT_RETRANS_TO), + .monitor_timeout = cpu_to_le16(L2CAP_DEFAULT_MONITOR_TO), + .max_pdu_size = cpu_to_le16(chan->imtu), + .txwin_size = min_t(u16, chan->ack_win, L2CAP_DEFAULT_TX_WINDOW), + }; + + BT_DBG("chan %p, rsp %p, len %d", chan, rsp, len); + + if ((chan->mode != L2CAP_MODE_ERTM) && (chan->mode != L2CAP_MODE_STREAMING)) return; while (len >= L2CAP_CONF_OPT_SIZE) { @@ -1936,32 +3714,44 @@ static void l2cap_conf_rfc_get(struct sock *sk, void *rsp, int len) case L2CAP_CONF_RFC: if (olen == sizeof(rfc)) memcpy(&rfc, (void *)val, olen); - goto done; + break; + case L2CAP_CONF_EWS: + txwin_ext = val; + break; } } -done: switch (rfc.mode) { case L2CAP_MODE_ERTM: - pi->retrans_timeout = le16_to_cpu(rfc.retrans_timeout); - pi->monitor_timeout = le16_to_cpu(rfc.monitor_timeout); - pi->mps = le16_to_cpu(rfc.max_pdu_size); + chan->retrans_timeout = le16_to_cpu(rfc.retrans_timeout); + chan->monitor_timeout = le16_to_cpu(rfc.monitor_timeout); + chan->mps = le16_to_cpu(rfc.max_pdu_size); + if (test_bit(FLAG_EXT_CTRL, &chan->flags)) + chan->ack_win = min_t(u16, chan->ack_win, txwin_ext); + else + chan->ack_win = min_t(u16, chan->ack_win, + rfc.txwin_size); break; case L2CAP_MODE_STREAMING: - pi->mps = le16_to_cpu(rfc.max_pdu_size); + chan->mps = le16_to_cpu(rfc.max_pdu_size); } } -static inline int l2cap_command_rej(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data) +static inline int l2cap_command_rej(struct l2cap_conn *conn, + struct l2cap_cmd_hdr *cmd, u16 cmd_len, + u8 *data) { - struct l2cap_cmd_rej *rej = (struct l2cap_cmd_rej *) data; + struct l2cap_cmd_rej_unk *rej = (struct l2cap_cmd_rej_unk *) data; + + if (cmd_len < sizeof(*rej)) + return -EPROTO; - if (rej->reason != 0x0000) + if (rej->reason != L2CAP_REJ_NOT_UNDERSTOOD) return 0; if ((conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_SENT) && - cmd->ident == conn->info_ident) { - del_timer(&conn->info_timer); + cmd->ident == conn->info_ident) { + cancel_delayed_work(&conn->info_timer); conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_DONE; conn->info_ident = 0; @@ -1972,107 +3762,114 @@ static inline int l2cap_command_rej(struct l2cap_conn *conn, struct l2cap_cmd_hd return 0; } -static inline int l2cap_connect_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data) +static struct l2cap_chan *l2cap_connect(struct l2cap_conn *conn, + struct l2cap_cmd_hdr *cmd, + u8 *data, u8 rsp_code, u8 amp_id) { - struct l2cap_chan_list *list = &conn->chan_list; struct l2cap_conn_req *req = (struct l2cap_conn_req *) data; struct l2cap_conn_rsp rsp; - struct sock *parent, *sk = NULL; + struct l2cap_chan *chan = NULL, *pchan; int result, status = L2CAP_CS_NO_INFO; u16 dcid = 0, scid = __le16_to_cpu(req->scid); __le16 psm = req->psm; - BT_DBG("psm 0x%2.2x scid 0x%4.4x", psm, scid); + BT_DBG("psm 0x%2.2x scid 0x%4.4x", __le16_to_cpu(psm), scid); /* Check if we have socket listening on psm */ - parent = l2cap_get_sock_by_psm(BT_LISTEN, psm, conn->src); - if (!parent) { + pchan = l2cap_global_chan_by_psm(BT_LISTEN, psm, &conn->hcon->src, + &conn->hcon->dst, ACL_LINK); + if (!pchan) { result = L2CAP_CR_BAD_PSM; goto sendresp; } - bh_lock_sock(parent); + mutex_lock(&conn->chan_lock); + l2cap_chan_lock(pchan); /* Check if the ACL is secure enough (if not SDP) */ - if (psm != cpu_to_le16(0x0001) && - !hci_conn_check_link_mode(conn->hcon)) { - conn->disc_reason = 0x05; + if (psm != cpu_to_le16(L2CAP_PSM_SDP) && + !hci_conn_check_link_mode(conn->hcon)) { + conn->disc_reason = HCI_ERROR_AUTH_FAILURE; result = L2CAP_CR_SEC_BLOCK; goto response; } result = L2CAP_CR_NO_MEM; - /* Check for backlog size */ - if (sk_acceptq_is_full(parent)) { - BT_DBG("backlog full %d", parent->sk_ack_backlog); + /* Check if we already have channel with that dcid */ + if (__l2cap_get_chan_by_dcid(conn, scid)) goto response; - } - sk = l2cap_sock_alloc(sock_net(parent), NULL, BTPROTO_L2CAP, GFP_ATOMIC); - if (!sk) + chan = pchan->ops->new_connection(pchan); + if (!chan) goto response; - write_lock_bh(&list->lock); - - /* Check if we already have channel with that dcid */ - if (__l2cap_get_chan_by_dcid(list, scid)) { - write_unlock_bh(&list->lock); - sock_set_flag(sk, SOCK_ZAPPED); - l2cap_sock_kill(sk); - goto response; - } + /* For certain devices (ex: HID mouse), support for authentication, + * pairing and bonding is optional. For such devices, inorder to avoid + * the ACL alive for too long after L2CAP disconnection, reset the ACL + * disc_timeout back to HCI_DISCONN_TIMEOUT during L2CAP connect. + */ + conn->hcon->disc_timeout = HCI_DISCONN_TIMEOUT; - hci_conn_hold(conn->hcon); + bacpy(&chan->src, &conn->hcon->src); + bacpy(&chan->dst, &conn->hcon->dst); + chan->src_type = bdaddr_type(conn->hcon, conn->hcon->src_type); + chan->dst_type = bdaddr_type(conn->hcon, conn->hcon->dst_type); + chan->psm = psm; + chan->dcid = scid; + chan->local_amp_id = amp_id; - l2cap_sock_init(sk, parent); - bacpy(&bt_sk(sk)->src, conn->src); - bacpy(&bt_sk(sk)->dst, conn->dst); - l2cap_pi(sk)->psm = psm; - l2cap_pi(sk)->dcid = scid; + __l2cap_chan_add(conn, chan); - __l2cap_chan_add(conn, sk, parent); - dcid = l2cap_pi(sk)->scid; + dcid = chan->scid; - l2cap_sock_set_timer(sk, sk->sk_sndtimeo); + __set_chan_timer(chan, chan->ops->get_sndtimeo(chan)); - l2cap_pi(sk)->ident = cmd->ident; + chan->ident = cmd->ident; if (conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_DONE) { - if (l2cap_check_security(sk)) { - if (bt_sk(sk)->defer_setup) { - sk->sk_state = BT_CONNECT2; + if (l2cap_chan_check_security(chan)) { + if (test_bit(FLAG_DEFER_SETUP, &chan->flags)) { + l2cap_state_change(chan, BT_CONNECT2); result = L2CAP_CR_PEND; status = L2CAP_CS_AUTHOR_PEND; - parent->sk_data_ready(parent, 0); + chan->ops->defer(chan); } else { - sk->sk_state = BT_CONFIG; - result = L2CAP_CR_SUCCESS; + /* Force pending result for AMP controllers. + * The connection will succeed after the + * physical link is up. + */ + if (amp_id == AMP_ID_BREDR) { + l2cap_state_change(chan, BT_CONFIG); + result = L2CAP_CR_SUCCESS; + } else { + l2cap_state_change(chan, BT_CONNECT2); + result = L2CAP_CR_PEND; + } status = L2CAP_CS_NO_INFO; } } else { - sk->sk_state = BT_CONNECT2; + l2cap_state_change(chan, BT_CONNECT2); result = L2CAP_CR_PEND; status = L2CAP_CS_AUTHEN_PEND; } } else { - sk->sk_state = BT_CONNECT2; + l2cap_state_change(chan, BT_CONNECT2); result = L2CAP_CR_PEND; status = L2CAP_CS_NO_INFO; } - write_unlock_bh(&list->lock); - response: - bh_unlock_sock(parent); + l2cap_chan_unlock(pchan); + mutex_unlock(&conn->chan_lock); sendresp: rsp.scid = cpu_to_le16(scid); rsp.dcid = cpu_to_le16(dcid); rsp.result = cpu_to_le16(result); rsp.status = cpu_to_le16(status); - l2cap_send_cmd(conn, cmd->ident, L2CAP_CONN_RSP, sizeof(rsp), &rsp); + l2cap_send_cmd(conn, cmd->ident, rsp_code, sizeof(rsp), &rsp); if (result == L2CAP_CR_PEND && status == L2CAP_CS_NO_INFO) { struct l2cap_info_req info; @@ -2081,342 +3878,474 @@ sendresp: conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_SENT; conn->info_ident = l2cap_get_ident(conn); - mod_timer(&conn->info_timer, jiffies + - msecs_to_jiffies(L2CAP_INFO_TIMEOUT)); + schedule_delayed_work(&conn->info_timer, L2CAP_INFO_TIMEOUT); - l2cap_send_cmd(conn, conn->info_ident, - L2CAP_INFO_REQ, sizeof(info), &info); + l2cap_send_cmd(conn, conn->info_ident, L2CAP_INFO_REQ, + sizeof(info), &info); } - if (sk && !(l2cap_pi(sk)->conf_state & L2CAP_CONF_REQ_SENT) && - result == L2CAP_CR_SUCCESS) { + if (chan && !test_bit(CONF_REQ_SENT, &chan->conf_state) && + result == L2CAP_CR_SUCCESS) { u8 buf[128]; - l2cap_pi(sk)->conf_state |= L2CAP_CONF_REQ_SENT; + set_bit(CONF_REQ_SENT, &chan->conf_state); l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ, - l2cap_build_conf_req(sk, buf), buf); - l2cap_pi(sk)->num_conf_req++; + l2cap_build_conf_req(chan, buf), buf); + chan->num_conf_req++; } + return chan; +} + +static int l2cap_connect_req(struct l2cap_conn *conn, + struct l2cap_cmd_hdr *cmd, u16 cmd_len, u8 *data) +{ + struct hci_dev *hdev = conn->hcon->hdev; + struct hci_conn *hcon = conn->hcon; + + if (cmd_len < sizeof(struct l2cap_conn_req)) + return -EPROTO; + + hci_dev_lock(hdev); + if (test_bit(HCI_MGMT, &hdev->dev_flags) && + !test_and_set_bit(HCI_CONN_MGMT_CONNECTED, &hcon->flags)) + mgmt_device_connected(hdev, &hcon->dst, hcon->type, + hcon->dst_type, 0, NULL, 0, + hcon->dev_class); + hci_dev_unlock(hdev); + + l2cap_connect(conn, cmd, data, L2CAP_CONN_RSP, 0); return 0; } -static inline int l2cap_connect_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data) +static int l2cap_connect_create_rsp(struct l2cap_conn *conn, + struct l2cap_cmd_hdr *cmd, u16 cmd_len, + u8 *data) { struct l2cap_conn_rsp *rsp = (struct l2cap_conn_rsp *) data; u16 scid, dcid, result, status; - struct sock *sk; + struct l2cap_chan *chan; u8 req[128]; + int err; + + if (cmd_len < sizeof(*rsp)) + return -EPROTO; scid = __le16_to_cpu(rsp->scid); dcid = __le16_to_cpu(rsp->dcid); result = __le16_to_cpu(rsp->result); status = __le16_to_cpu(rsp->status); - BT_DBG("dcid 0x%4.4x scid 0x%4.4x result 0x%2.2x status 0x%2.2x", dcid, scid, result, status); + BT_DBG("dcid 0x%4.4x scid 0x%4.4x result 0x%2.2x status 0x%2.2x", + dcid, scid, result, status); + + mutex_lock(&conn->chan_lock); if (scid) { - sk = l2cap_get_chan_by_scid(&conn->chan_list, scid); - if (!sk) - return -EFAULT; + chan = __l2cap_get_chan_by_scid(conn, scid); + if (!chan) { + err = -EBADSLT; + goto unlock; + } } else { - sk = l2cap_get_chan_by_ident(&conn->chan_list, cmd->ident); - if (!sk) - return -EFAULT; + chan = __l2cap_get_chan_by_ident(conn, cmd->ident); + if (!chan) { + err = -EBADSLT; + goto unlock; + } } + err = 0; + + l2cap_chan_lock(chan); + switch (result) { case L2CAP_CR_SUCCESS: - sk->sk_state = BT_CONFIG; - l2cap_pi(sk)->ident = 0; - l2cap_pi(sk)->dcid = dcid; - l2cap_pi(sk)->conf_state &= ~L2CAP_CONF_CONNECT_PEND; + l2cap_state_change(chan, BT_CONFIG); + chan->ident = 0; + chan->dcid = dcid; + clear_bit(CONF_CONNECT_PEND, &chan->conf_state); - if (l2cap_pi(sk)->conf_state & L2CAP_CONF_REQ_SENT) + if (test_and_set_bit(CONF_REQ_SENT, &chan->conf_state)) break; - l2cap_pi(sk)->conf_state |= L2CAP_CONF_REQ_SENT; - l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ, - l2cap_build_conf_req(sk, req), req); - l2cap_pi(sk)->num_conf_req++; + l2cap_build_conf_req(chan, req), req); + chan->num_conf_req++; break; case L2CAP_CR_PEND: - l2cap_pi(sk)->conf_state |= L2CAP_CONF_CONNECT_PEND; + set_bit(CONF_CONNECT_PEND, &chan->conf_state); break; default: - /* don't delete l2cap channel if sk is owned by user */ - if (sock_owned_by_user(sk)) { - sk->sk_state = BT_DISCONN; - l2cap_sock_clear_timer(sk); - l2cap_sock_set_timer(sk, HZ / 5); - break; - } - - l2cap_chan_del(sk, ECONNREFUSED); + l2cap_chan_del(chan, ECONNREFUSED); break; } - bh_unlock_sock(sk); - return 0; + l2cap_chan_unlock(chan); + +unlock: + mutex_unlock(&conn->chan_lock); + + return err; } -static inline void set_default_fcs(struct l2cap_pinfo *pi) +static inline void set_default_fcs(struct l2cap_chan *chan) { /* FCS is enabled only in ERTM or streaming mode, if one or both * sides request it. */ - if (pi->mode != L2CAP_MODE_ERTM && pi->mode != L2CAP_MODE_STREAMING) - pi->fcs = L2CAP_FCS_NONE; - else if (!(pi->conf_state & L2CAP_CONF_NO_FCS_RECV)) - pi->fcs = L2CAP_FCS_CRC16; + if (chan->mode != L2CAP_MODE_ERTM && chan->mode != L2CAP_MODE_STREAMING) + chan->fcs = L2CAP_FCS_NONE; + else if (!test_bit(CONF_RECV_NO_FCS, &chan->conf_state)) + chan->fcs = L2CAP_FCS_CRC16; } -static inline int l2cap_config_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u16 cmd_len, u8 *data) +static void l2cap_send_efs_conf_rsp(struct l2cap_chan *chan, void *data, + u8 ident, u16 flags) +{ + struct l2cap_conn *conn = chan->conn; + + BT_DBG("conn %p chan %p ident %d flags 0x%4.4x", conn, chan, ident, + flags); + + clear_bit(CONF_LOC_CONF_PEND, &chan->conf_state); + set_bit(CONF_OUTPUT_DONE, &chan->conf_state); + + l2cap_send_cmd(conn, ident, L2CAP_CONF_RSP, + l2cap_build_conf_rsp(chan, data, + L2CAP_CONF_SUCCESS, flags), data); +} + +static void cmd_reject_invalid_cid(struct l2cap_conn *conn, u8 ident, + u16 scid, u16 dcid) +{ + struct l2cap_cmd_rej_cid rej; + + rej.reason = cpu_to_le16(L2CAP_REJ_INVALID_CID); + rej.scid = __cpu_to_le16(scid); + rej.dcid = __cpu_to_le16(dcid); + + l2cap_send_cmd(conn, ident, L2CAP_COMMAND_REJ, sizeof(rej), &rej); +} + +static inline int l2cap_config_req(struct l2cap_conn *conn, + struct l2cap_cmd_hdr *cmd, u16 cmd_len, + u8 *data) { struct l2cap_conf_req *req = (struct l2cap_conf_req *) data; u16 dcid, flags; u8 rsp[64]; - struct sock *sk; - int len; + struct l2cap_chan *chan; + int len, err = 0; + + if (cmd_len < sizeof(*req)) + return -EPROTO; dcid = __le16_to_cpu(req->dcid); flags = __le16_to_cpu(req->flags); BT_DBG("dcid 0x%4.4x flags 0x%2.2x", dcid, flags); - sk = l2cap_get_chan_by_scid(&conn->chan_list, dcid); - if (!sk) - return -ENOENT; - - if (sk->sk_state != BT_CONFIG) { - struct l2cap_cmd_rej rej; + chan = l2cap_get_chan_by_scid(conn, dcid); + if (!chan) { + cmd_reject_invalid_cid(conn, cmd->ident, dcid, 0); + return 0; + } - rej.reason = cpu_to_le16(0x0002); - l2cap_send_cmd(conn, cmd->ident, L2CAP_COMMAND_REJ, - sizeof(rej), &rej); + if (chan->state != BT_CONFIG && chan->state != BT_CONNECT2) { + cmd_reject_invalid_cid(conn, cmd->ident, chan->scid, + chan->dcid); goto unlock; } /* Reject if config buffer is too small. */ len = cmd_len - sizeof(*req); - if (l2cap_pi(sk)->conf_len + len > sizeof(l2cap_pi(sk)->conf_req)) { + if (chan->conf_len + len > sizeof(chan->conf_req)) { l2cap_send_cmd(conn, cmd->ident, L2CAP_CONF_RSP, - l2cap_build_conf_rsp(sk, rsp, - L2CAP_CONF_REJECT, flags), rsp); + l2cap_build_conf_rsp(chan, rsp, + L2CAP_CONF_REJECT, flags), rsp); goto unlock; } /* Store config. */ - memcpy(l2cap_pi(sk)->conf_req + l2cap_pi(sk)->conf_len, req->data, len); - l2cap_pi(sk)->conf_len += len; + memcpy(chan->conf_req + chan->conf_len, req->data, len); + chan->conf_len += len; - if (flags & 0x0001) { + if (flags & L2CAP_CONF_FLAG_CONTINUATION) { /* Incomplete config. Send empty response. */ l2cap_send_cmd(conn, cmd->ident, L2CAP_CONF_RSP, - l2cap_build_conf_rsp(sk, rsp, - L2CAP_CONF_SUCCESS, 0x0001), rsp); + l2cap_build_conf_rsp(chan, rsp, + L2CAP_CONF_SUCCESS, flags), rsp); goto unlock; } /* Complete config. */ - len = l2cap_parse_conf_req(sk, rsp); + len = l2cap_parse_conf_req(chan, rsp); if (len < 0) { - l2cap_send_disconn_req(conn, sk, ECONNRESET); + l2cap_send_disconn_req(chan, ECONNRESET); goto unlock; } + chan->ident = cmd->ident; l2cap_send_cmd(conn, cmd->ident, L2CAP_CONF_RSP, len, rsp); - l2cap_pi(sk)->num_conf_rsp++; + chan->num_conf_rsp++; /* Reset config buffer. */ - l2cap_pi(sk)->conf_len = 0; + chan->conf_len = 0; - if (!(l2cap_pi(sk)->conf_state & L2CAP_CONF_OUTPUT_DONE)) + if (!test_bit(CONF_OUTPUT_DONE, &chan->conf_state)) goto unlock; - if (l2cap_pi(sk)->conf_state & L2CAP_CONF_INPUT_DONE) { - set_default_fcs(l2cap_pi(sk)); + if (test_bit(CONF_INPUT_DONE, &chan->conf_state)) { + set_default_fcs(chan); - sk->sk_state = BT_CONNECTED; + if (chan->mode == L2CAP_MODE_ERTM || + chan->mode == L2CAP_MODE_STREAMING) + err = l2cap_ertm_init(chan); - l2cap_pi(sk)->next_tx_seq = 0; - l2cap_pi(sk)->expected_tx_seq = 0; - __skb_queue_head_init(TX_QUEUE(sk)); - if (l2cap_pi(sk)->mode == L2CAP_MODE_ERTM) - l2cap_ertm_init(sk); + if (err < 0) + l2cap_send_disconn_req(chan, -err); + else + l2cap_chan_ready(chan); - l2cap_chan_ready(sk); goto unlock; } - if (!(l2cap_pi(sk)->conf_state & L2CAP_CONF_REQ_SENT)) { + if (!test_and_set_bit(CONF_REQ_SENT, &chan->conf_state)) { u8 buf[64]; - l2cap_pi(sk)->conf_state |= L2CAP_CONF_REQ_SENT; l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ, - l2cap_build_conf_req(sk, buf), buf); - l2cap_pi(sk)->num_conf_req++; + l2cap_build_conf_req(chan, buf), buf); + chan->num_conf_req++; + } + + /* Got Conf Rsp PENDING from remote side and asume we sent + Conf Rsp PENDING in the code above */ + if (test_bit(CONF_REM_CONF_PEND, &chan->conf_state) && + test_bit(CONF_LOC_CONF_PEND, &chan->conf_state)) { + + /* check compatibility */ + + /* Send rsp for BR/EDR channel */ + if (!chan->hs_hcon) + l2cap_send_efs_conf_rsp(chan, rsp, cmd->ident, flags); + else + chan->ident = cmd->ident; } unlock: - bh_unlock_sock(sk); - return 0; + l2cap_chan_unlock(chan); + return err; } -static inline int l2cap_config_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data) +static inline int l2cap_config_rsp(struct l2cap_conn *conn, + struct l2cap_cmd_hdr *cmd, u16 cmd_len, + u8 *data) { struct l2cap_conf_rsp *rsp = (struct l2cap_conf_rsp *)data; u16 scid, flags, result; - struct sock *sk; - int len = cmd->len - sizeof(*rsp); + struct l2cap_chan *chan; + int len = cmd_len - sizeof(*rsp); + int err = 0; + + if (cmd_len < sizeof(*rsp)) + return -EPROTO; scid = __le16_to_cpu(rsp->scid); flags = __le16_to_cpu(rsp->flags); result = __le16_to_cpu(rsp->result); - BT_DBG("scid 0x%4.4x flags 0x%2.2x result 0x%2.2x", - scid, flags, result); + BT_DBG("scid 0x%4.4x flags 0x%2.2x result 0x%2.2x len %d", scid, flags, + result, len); - sk = l2cap_get_chan_by_scid(&conn->chan_list, scid); - if (!sk) + chan = l2cap_get_chan_by_scid(conn, scid); + if (!chan) return 0; switch (result) { case L2CAP_CONF_SUCCESS: - l2cap_conf_rfc_get(sk, rsp->data, len); + l2cap_conf_rfc_get(chan, rsp->data, len); + clear_bit(CONF_REM_CONF_PEND, &chan->conf_state); break; + case L2CAP_CONF_PENDING: + set_bit(CONF_REM_CONF_PEND, &chan->conf_state); + + if (test_bit(CONF_LOC_CONF_PEND, &chan->conf_state)) { + char buf[64]; + + len = l2cap_parse_conf_rsp(chan, rsp->data, len, + buf, &result); + if (len < 0) { + l2cap_send_disconn_req(chan, ECONNRESET); + goto done; + } + + if (!chan->hs_hcon) { + l2cap_send_efs_conf_rsp(chan, buf, cmd->ident, + 0); + } else { + if (l2cap_check_efs(chan)) { + amp_create_logical_link(chan); + chan->ident = cmd->ident; + } + } + } + goto done; + case L2CAP_CONF_UNACCEPT: - if (l2cap_pi(sk)->num_conf_rsp <= L2CAP_CONF_MAX_CONF_RSP) { + if (chan->num_conf_rsp <= L2CAP_CONF_MAX_CONF_RSP) { char req[64]; if (len > sizeof(req) - sizeof(struct l2cap_conf_req)) { - l2cap_send_disconn_req(conn, sk, ECONNRESET); + l2cap_send_disconn_req(chan, ECONNRESET); goto done; } /* throw out any old stored conf requests */ result = L2CAP_CONF_SUCCESS; - len = l2cap_parse_conf_rsp(sk, rsp->data, - len, req, &result); + len = l2cap_parse_conf_rsp(chan, rsp->data, len, + req, &result); if (len < 0) { - l2cap_send_disconn_req(conn, sk, ECONNRESET); + l2cap_send_disconn_req(chan, ECONNRESET); goto done; } l2cap_send_cmd(conn, l2cap_get_ident(conn), - L2CAP_CONF_REQ, len, req); - l2cap_pi(sk)->num_conf_req++; + L2CAP_CONF_REQ, len, req); + chan->num_conf_req++; if (result != L2CAP_CONF_SUCCESS) goto done; break; } default: - sk->sk_err = ECONNRESET; - l2cap_sock_set_timer(sk, HZ * 5); - l2cap_send_disconn_req(conn, sk, ECONNRESET); + l2cap_chan_set_err(chan, ECONNRESET); + + __set_chan_timer(chan, L2CAP_DISC_REJ_TIMEOUT); + l2cap_send_disconn_req(chan, ECONNRESET); goto done; } - if (flags & 0x01) + if (flags & L2CAP_CONF_FLAG_CONTINUATION) goto done; - l2cap_pi(sk)->conf_state |= L2CAP_CONF_INPUT_DONE; + set_bit(CONF_INPUT_DONE, &chan->conf_state); - if (l2cap_pi(sk)->conf_state & L2CAP_CONF_OUTPUT_DONE) { - set_default_fcs(l2cap_pi(sk)); + if (test_bit(CONF_OUTPUT_DONE, &chan->conf_state)) { + set_default_fcs(chan); - sk->sk_state = BT_CONNECTED; - l2cap_pi(sk)->next_tx_seq = 0; - l2cap_pi(sk)->expected_tx_seq = 0; - __skb_queue_head_init(TX_QUEUE(sk)); - if (l2cap_pi(sk)->mode == L2CAP_MODE_ERTM) - l2cap_ertm_init(sk); + if (chan->mode == L2CAP_MODE_ERTM || + chan->mode == L2CAP_MODE_STREAMING) + err = l2cap_ertm_init(chan); - l2cap_chan_ready(sk); + if (err < 0) + l2cap_send_disconn_req(chan, -err); + else + l2cap_chan_ready(chan); } done: - bh_unlock_sock(sk); - return 0; + l2cap_chan_unlock(chan); + return err; } -static inline int l2cap_disconnect_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data) +static inline int l2cap_disconnect_req(struct l2cap_conn *conn, + struct l2cap_cmd_hdr *cmd, u16 cmd_len, + u8 *data) { struct l2cap_disconn_req *req = (struct l2cap_disconn_req *) data; struct l2cap_disconn_rsp rsp; u16 dcid, scid; - struct sock *sk; + struct l2cap_chan *chan; + + if (cmd_len != sizeof(*req)) + return -EPROTO; scid = __le16_to_cpu(req->scid); dcid = __le16_to_cpu(req->dcid); BT_DBG("scid 0x%4.4x dcid 0x%4.4x", scid, dcid); - sk = l2cap_get_chan_by_scid(&conn->chan_list, dcid); - if (!sk) + mutex_lock(&conn->chan_lock); + + chan = __l2cap_get_chan_by_scid(conn, dcid); + if (!chan) { + mutex_unlock(&conn->chan_lock); + cmd_reject_invalid_cid(conn, cmd->ident, dcid, scid); return 0; + } - rsp.dcid = cpu_to_le16(l2cap_pi(sk)->scid); - rsp.scid = cpu_to_le16(l2cap_pi(sk)->dcid); + l2cap_chan_lock(chan); + + rsp.dcid = cpu_to_le16(chan->scid); + rsp.scid = cpu_to_le16(chan->dcid); l2cap_send_cmd(conn, cmd->ident, L2CAP_DISCONN_RSP, sizeof(rsp), &rsp); - sk->sk_shutdown = SHUTDOWN_MASK; + chan->ops->set_shutdown(chan); - /* don't delete l2cap channel if sk is owned by user */ - if (sock_owned_by_user(sk)) { - sk->sk_state = BT_DISCONN; - l2cap_sock_clear_timer(sk); - l2cap_sock_set_timer(sk, HZ / 5); - bh_unlock_sock(sk); - return 0; - } + l2cap_chan_hold(chan); + l2cap_chan_del(chan, ECONNRESET); + + l2cap_chan_unlock(chan); - l2cap_chan_del(sk, ECONNRESET); - bh_unlock_sock(sk); + chan->ops->close(chan); + l2cap_chan_put(chan); + + mutex_unlock(&conn->chan_lock); - l2cap_sock_kill(sk); return 0; } -static inline int l2cap_disconnect_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data) +static inline int l2cap_disconnect_rsp(struct l2cap_conn *conn, + struct l2cap_cmd_hdr *cmd, u16 cmd_len, + u8 *data) { struct l2cap_disconn_rsp *rsp = (struct l2cap_disconn_rsp *) data; u16 dcid, scid; - struct sock *sk; + struct l2cap_chan *chan; + + if (cmd_len != sizeof(*rsp)) + return -EPROTO; scid = __le16_to_cpu(rsp->scid); dcid = __le16_to_cpu(rsp->dcid); BT_DBG("dcid 0x%4.4x scid 0x%4.4x", dcid, scid); - sk = l2cap_get_chan_by_scid(&conn->chan_list, scid); - if (!sk) - return 0; + mutex_lock(&conn->chan_lock); - /* don't delete l2cap channel if sk is owned by user */ - if (sock_owned_by_user(sk)) { - sk->sk_state = BT_DISCONN; - l2cap_sock_clear_timer(sk); - l2cap_sock_set_timer(sk, HZ / 5); - bh_unlock_sock(sk); + chan = __l2cap_get_chan_by_scid(conn, scid); + if (!chan) { + mutex_unlock(&conn->chan_lock); return 0; } - l2cap_chan_del(sk, 0); - bh_unlock_sock(sk); + l2cap_chan_lock(chan); + + l2cap_chan_hold(chan); + l2cap_chan_del(chan, 0); + + l2cap_chan_unlock(chan); + + chan->ops->close(chan); + l2cap_chan_put(chan); + + mutex_unlock(&conn->chan_lock); - l2cap_sock_kill(sk); return 0; } -static inline int l2cap_information_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data) +static inline int l2cap_information_req(struct l2cap_conn *conn, + struct l2cap_cmd_hdr *cmd, u16 cmd_len, + u8 *data) { struct l2cap_info_req *req = (struct l2cap_info_req *) data; u16 type; + if (cmd_len != sizeof(*req)) + return -EPROTO; + type = __le16_to_cpu(req->type); BT_DBG("type 0x%4.4x", type); @@ -2429,40 +4358,60 @@ static inline int l2cap_information_req(struct l2cap_conn *conn, struct l2cap_cm rsp->result = cpu_to_le16(L2CAP_IR_SUCCESS); if (!disable_ertm) feat_mask |= L2CAP_FEAT_ERTM | L2CAP_FEAT_STREAMING - | L2CAP_FEAT_FCS; + | L2CAP_FEAT_FCS; + if (conn->hs_enabled) + feat_mask |= L2CAP_FEAT_EXT_FLOW + | L2CAP_FEAT_EXT_WINDOW; + put_unaligned_le32(feat_mask, rsp->data); - l2cap_send_cmd(conn, cmd->ident, - L2CAP_INFO_RSP, sizeof(buf), buf); + l2cap_send_cmd(conn, cmd->ident, L2CAP_INFO_RSP, sizeof(buf), + buf); } else if (type == L2CAP_IT_FIXED_CHAN) { u8 buf[12]; struct l2cap_info_rsp *rsp = (struct l2cap_info_rsp *) buf; + + if (conn->hs_enabled) + l2cap_fixed_chan[0] |= L2CAP_FC_A2MP; + else + l2cap_fixed_chan[0] &= ~L2CAP_FC_A2MP; + rsp->type = cpu_to_le16(L2CAP_IT_FIXED_CHAN); rsp->result = cpu_to_le16(L2CAP_IR_SUCCESS); - memcpy(buf + 4, l2cap_fixed_chan, 8); - l2cap_send_cmd(conn, cmd->ident, - L2CAP_INFO_RSP, sizeof(buf), buf); + memcpy(rsp->data, l2cap_fixed_chan, sizeof(l2cap_fixed_chan)); + l2cap_send_cmd(conn, cmd->ident, L2CAP_INFO_RSP, sizeof(buf), + buf); } else { struct l2cap_info_rsp rsp; rsp.type = cpu_to_le16(type); rsp.result = cpu_to_le16(L2CAP_IR_NOTSUPP); - l2cap_send_cmd(conn, cmd->ident, - L2CAP_INFO_RSP, sizeof(rsp), &rsp); + l2cap_send_cmd(conn, cmd->ident, L2CAP_INFO_RSP, sizeof(rsp), + &rsp); } return 0; } -static inline int l2cap_information_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data) +static inline int l2cap_information_rsp(struct l2cap_conn *conn, + struct l2cap_cmd_hdr *cmd, u16 cmd_len, + u8 *data) { struct l2cap_info_rsp *rsp = (struct l2cap_info_rsp *) data; u16 type, result; + if (cmd_len < sizeof(*rsp)) + return -EPROTO; + type = __le16_to_cpu(rsp->type); result = __le16_to_cpu(rsp->result); BT_DBG("type 0x%4.4x result 0x%2.2x", type, result); - del_timer(&conn->info_timer); + /* L2CAP Info req/rsp are unbound to channels, add extra checks */ + if (cmd->ident != conn->info_ident || + conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_DONE) + return 0; + + cancel_delayed_work(&conn->info_timer); if (result != L2CAP_IR_SUCCESS) { conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_DONE; @@ -2473,7 +4422,8 @@ static inline int l2cap_information_rsp(struct l2cap_conn *conn, struct l2cap_cm return 0; } - if (type == L2CAP_IT_FEAT_MASK) { + switch (type) { + case L2CAP_IT_FEAT_MASK: conn->feat_mask = get_unaligned_le32(rsp->data); if (conn->feat_mask & L2CAP_FEAT_FIXED_CHAN) { @@ -2483,25 +4433,772 @@ static inline int l2cap_information_rsp(struct l2cap_conn *conn, struct l2cap_cm conn->info_ident = l2cap_get_ident(conn); l2cap_send_cmd(conn, conn->info_ident, - L2CAP_INFO_REQ, sizeof(req), &req); + L2CAP_INFO_REQ, sizeof(req), &req); } else { conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_DONE; conn->info_ident = 0; l2cap_conn_start(conn); } - } else if (type == L2CAP_IT_FIXED_CHAN) { + break; + + case L2CAP_IT_FIXED_CHAN: + conn->fixed_chan_mask = rsp->data[0]; conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_DONE; conn->info_ident = 0; l2cap_conn_start(conn); + break; + } + + return 0; +} + +static int l2cap_create_channel_req(struct l2cap_conn *conn, + struct l2cap_cmd_hdr *cmd, + u16 cmd_len, void *data) +{ + struct l2cap_create_chan_req *req = data; + struct l2cap_create_chan_rsp rsp; + struct l2cap_chan *chan; + struct hci_dev *hdev; + u16 psm, scid; + + if (cmd_len != sizeof(*req)) + return -EPROTO; + + if (!conn->hs_enabled) + return -EINVAL; + + psm = le16_to_cpu(req->psm); + scid = le16_to_cpu(req->scid); + + BT_DBG("psm 0x%2.2x, scid 0x%4.4x, amp_id %d", psm, scid, req->amp_id); + + /* For controller id 0 make BR/EDR connection */ + if (req->amp_id == AMP_ID_BREDR) { + l2cap_connect(conn, cmd, data, L2CAP_CREATE_CHAN_RSP, + req->amp_id); + return 0; + } + + /* Validate AMP controller id */ + hdev = hci_dev_get(req->amp_id); + if (!hdev) + goto error; + + if (hdev->dev_type != HCI_AMP || !test_bit(HCI_UP, &hdev->flags)) { + hci_dev_put(hdev); + goto error; + } + + chan = l2cap_connect(conn, cmd, data, L2CAP_CREATE_CHAN_RSP, + req->amp_id); + if (chan) { + struct amp_mgr *mgr = conn->hcon->amp_mgr; + struct hci_conn *hs_hcon; + + hs_hcon = hci_conn_hash_lookup_ba(hdev, AMP_LINK, + &conn->hcon->dst); + if (!hs_hcon) { + hci_dev_put(hdev); + cmd_reject_invalid_cid(conn, cmd->ident, chan->scid, + chan->dcid); + return 0; + } + + BT_DBG("mgr %p bredr_chan %p hs_hcon %p", mgr, chan, hs_hcon); + + mgr->bredr_chan = chan; + chan->hs_hcon = hs_hcon; + chan->fcs = L2CAP_FCS_NONE; + conn->mtu = hdev->block_mtu; + } + + hci_dev_put(hdev); + + return 0; + +error: + rsp.dcid = 0; + rsp.scid = cpu_to_le16(scid); + rsp.result = cpu_to_le16(L2CAP_CR_BAD_AMP); + rsp.status = cpu_to_le16(L2CAP_CS_NO_INFO); + + l2cap_send_cmd(conn, cmd->ident, L2CAP_CREATE_CHAN_RSP, + sizeof(rsp), &rsp); + + return 0; +} + +static void l2cap_send_move_chan_req(struct l2cap_chan *chan, u8 dest_amp_id) +{ + struct l2cap_move_chan_req req; + u8 ident; + + BT_DBG("chan %p, dest_amp_id %d", chan, dest_amp_id); + + ident = l2cap_get_ident(chan->conn); + chan->ident = ident; + + req.icid = cpu_to_le16(chan->scid); + req.dest_amp_id = dest_amp_id; + + l2cap_send_cmd(chan->conn, ident, L2CAP_MOVE_CHAN_REQ, sizeof(req), + &req); + + __set_chan_timer(chan, L2CAP_MOVE_TIMEOUT); +} + +static void l2cap_send_move_chan_rsp(struct l2cap_chan *chan, u16 result) +{ + struct l2cap_move_chan_rsp rsp; + + BT_DBG("chan %p, result 0x%4.4x", chan, result); + + rsp.icid = cpu_to_le16(chan->dcid); + rsp.result = cpu_to_le16(result); + + l2cap_send_cmd(chan->conn, chan->ident, L2CAP_MOVE_CHAN_RSP, + sizeof(rsp), &rsp); +} + +static void l2cap_send_move_chan_cfm(struct l2cap_chan *chan, u16 result) +{ + struct l2cap_move_chan_cfm cfm; + + BT_DBG("chan %p, result 0x%4.4x", chan, result); + + chan->ident = l2cap_get_ident(chan->conn); + + cfm.icid = cpu_to_le16(chan->scid); + cfm.result = cpu_to_le16(result); + + l2cap_send_cmd(chan->conn, chan->ident, L2CAP_MOVE_CHAN_CFM, + sizeof(cfm), &cfm); + + __set_chan_timer(chan, L2CAP_MOVE_TIMEOUT); +} + +static void l2cap_send_move_chan_cfm_icid(struct l2cap_conn *conn, u16 icid) +{ + struct l2cap_move_chan_cfm cfm; + + BT_DBG("conn %p, icid 0x%4.4x", conn, icid); + + cfm.icid = cpu_to_le16(icid); + cfm.result = cpu_to_le16(L2CAP_MC_UNCONFIRMED); + + l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_MOVE_CHAN_CFM, + sizeof(cfm), &cfm); +} + +static void l2cap_send_move_chan_cfm_rsp(struct l2cap_conn *conn, u8 ident, + u16 icid) +{ + struct l2cap_move_chan_cfm_rsp rsp; + + BT_DBG("icid 0x%4.4x", icid); + + rsp.icid = cpu_to_le16(icid); + l2cap_send_cmd(conn, ident, L2CAP_MOVE_CHAN_CFM_RSP, sizeof(rsp), &rsp); +} + +static void __release_logical_link(struct l2cap_chan *chan) +{ + chan->hs_hchan = NULL; + chan->hs_hcon = NULL; + + /* Placeholder - release the logical link */ +} + +static void l2cap_logical_fail(struct l2cap_chan *chan) +{ + /* Logical link setup failed */ + if (chan->state != BT_CONNECTED) { + /* Create channel failure, disconnect */ + l2cap_send_disconn_req(chan, ECONNRESET); + return; + } + + switch (chan->move_role) { + case L2CAP_MOVE_ROLE_RESPONDER: + l2cap_move_done(chan); + l2cap_send_move_chan_rsp(chan, L2CAP_MR_NOT_SUPP); + break; + case L2CAP_MOVE_ROLE_INITIATOR: + if (chan->move_state == L2CAP_MOVE_WAIT_LOGICAL_COMP || + chan->move_state == L2CAP_MOVE_WAIT_LOGICAL_CFM) { + /* Remote has only sent pending or + * success responses, clean up + */ + l2cap_move_done(chan); + } + + /* Other amp move states imply that the move + * has already aborted + */ + l2cap_send_move_chan_cfm(chan, L2CAP_MC_UNCONFIRMED); + break; + } +} + +static void l2cap_logical_finish_create(struct l2cap_chan *chan, + struct hci_chan *hchan) +{ + struct l2cap_conf_rsp rsp; + + chan->hs_hchan = hchan; + chan->hs_hcon->l2cap_data = chan->conn; + + l2cap_send_efs_conf_rsp(chan, &rsp, chan->ident, 0); + + if (test_bit(CONF_INPUT_DONE, &chan->conf_state)) { + int err; + + set_default_fcs(chan); + + err = l2cap_ertm_init(chan); + if (err < 0) + l2cap_send_disconn_req(chan, -err); + else + l2cap_chan_ready(chan); + } +} + +static void l2cap_logical_finish_move(struct l2cap_chan *chan, + struct hci_chan *hchan) +{ + chan->hs_hcon = hchan->conn; + chan->hs_hcon->l2cap_data = chan->conn; + + BT_DBG("move_state %d", chan->move_state); + + switch (chan->move_state) { + case L2CAP_MOVE_WAIT_LOGICAL_COMP: + /* Move confirm will be sent after a success + * response is received + */ + chan->move_state = L2CAP_MOVE_WAIT_RSP_SUCCESS; + break; + case L2CAP_MOVE_WAIT_LOGICAL_CFM: + if (test_bit(CONN_LOCAL_BUSY, &chan->conn_state)) { + chan->move_state = L2CAP_MOVE_WAIT_LOCAL_BUSY; + } else if (chan->move_role == L2CAP_MOVE_ROLE_INITIATOR) { + chan->move_state = L2CAP_MOVE_WAIT_CONFIRM_RSP; + l2cap_send_move_chan_cfm(chan, L2CAP_MC_CONFIRMED); + } else if (chan->move_role == L2CAP_MOVE_ROLE_RESPONDER) { + chan->move_state = L2CAP_MOVE_WAIT_CONFIRM; + l2cap_send_move_chan_rsp(chan, L2CAP_MR_SUCCESS); + } + break; + default: + /* Move was not in expected state, free the channel */ + __release_logical_link(chan); + + chan->move_state = L2CAP_MOVE_STABLE; + } +} + +/* Call with chan locked */ +void l2cap_logical_cfm(struct l2cap_chan *chan, struct hci_chan *hchan, + u8 status) +{ + BT_DBG("chan %p, hchan %p, status %d", chan, hchan, status); + + if (status) { + l2cap_logical_fail(chan); + __release_logical_link(chan); + return; + } + + if (chan->state != BT_CONNECTED) { + /* Ignore logical link if channel is on BR/EDR */ + if (chan->local_amp_id != AMP_ID_BREDR) + l2cap_logical_finish_create(chan, hchan); + } else { + l2cap_logical_finish_move(chan, hchan); } +} + +void l2cap_move_start(struct l2cap_chan *chan) +{ + BT_DBG("chan %p", chan); + + if (chan->local_amp_id == AMP_ID_BREDR) { + if (chan->chan_policy != BT_CHANNEL_POLICY_AMP_PREFERRED) + return; + chan->move_role = L2CAP_MOVE_ROLE_INITIATOR; + chan->move_state = L2CAP_MOVE_WAIT_PREPARE; + /* Placeholder - start physical link setup */ + } else { + chan->move_role = L2CAP_MOVE_ROLE_INITIATOR; + chan->move_state = L2CAP_MOVE_WAIT_RSP_SUCCESS; + chan->move_id = 0; + l2cap_move_setup(chan); + l2cap_send_move_chan_req(chan, 0); + } +} + +static void l2cap_do_create(struct l2cap_chan *chan, int result, + u8 local_amp_id, u8 remote_amp_id) +{ + BT_DBG("chan %p state %s %u -> %u", chan, state_to_string(chan->state), + local_amp_id, remote_amp_id); + + chan->fcs = L2CAP_FCS_NONE; + + /* Outgoing channel on AMP */ + if (chan->state == BT_CONNECT) { + if (result == L2CAP_CR_SUCCESS) { + chan->local_amp_id = local_amp_id; + l2cap_send_create_chan_req(chan, remote_amp_id); + } else { + /* Revert to BR/EDR connect */ + l2cap_send_conn_req(chan); + } + + return; + } + + /* Incoming channel on AMP */ + if (__l2cap_no_conn_pending(chan)) { + struct l2cap_conn_rsp rsp; + char buf[128]; + rsp.scid = cpu_to_le16(chan->dcid); + rsp.dcid = cpu_to_le16(chan->scid); + + if (result == L2CAP_CR_SUCCESS) { + /* Send successful response */ + rsp.result = cpu_to_le16(L2CAP_CR_SUCCESS); + rsp.status = cpu_to_le16(L2CAP_CS_NO_INFO); + } else { + /* Send negative response */ + rsp.result = cpu_to_le16(L2CAP_CR_NO_MEM); + rsp.status = cpu_to_le16(L2CAP_CS_NO_INFO); + } + + l2cap_send_cmd(chan->conn, chan->ident, L2CAP_CREATE_CHAN_RSP, + sizeof(rsp), &rsp); + + if (result == L2CAP_CR_SUCCESS) { + l2cap_state_change(chan, BT_CONFIG); + set_bit(CONF_REQ_SENT, &chan->conf_state); + l2cap_send_cmd(chan->conn, l2cap_get_ident(chan->conn), + L2CAP_CONF_REQ, + l2cap_build_conf_req(chan, buf), buf); + chan->num_conf_req++; + } + } +} + +static void l2cap_do_move_initiate(struct l2cap_chan *chan, u8 local_amp_id, + u8 remote_amp_id) +{ + l2cap_move_setup(chan); + chan->move_id = local_amp_id; + chan->move_state = L2CAP_MOVE_WAIT_RSP; + + l2cap_send_move_chan_req(chan, remote_amp_id); +} + +static void l2cap_do_move_respond(struct l2cap_chan *chan, int result) +{ + struct hci_chan *hchan = NULL; + + /* Placeholder - get hci_chan for logical link */ + + if (hchan) { + if (hchan->state == BT_CONNECTED) { + /* Logical link is ready to go */ + chan->hs_hcon = hchan->conn; + chan->hs_hcon->l2cap_data = chan->conn; + chan->move_state = L2CAP_MOVE_WAIT_CONFIRM; + l2cap_send_move_chan_rsp(chan, L2CAP_MR_SUCCESS); + + l2cap_logical_cfm(chan, hchan, L2CAP_MR_SUCCESS); + } else { + /* Wait for logical link to be ready */ + chan->move_state = L2CAP_MOVE_WAIT_LOGICAL_CFM; + } + } else { + /* Logical link not available */ + l2cap_send_move_chan_rsp(chan, L2CAP_MR_NOT_ALLOWED); + } +} + +static void l2cap_do_move_cancel(struct l2cap_chan *chan, int result) +{ + if (chan->move_role == L2CAP_MOVE_ROLE_RESPONDER) { + u8 rsp_result; + if (result == -EINVAL) + rsp_result = L2CAP_MR_BAD_ID; + else + rsp_result = L2CAP_MR_NOT_ALLOWED; + + l2cap_send_move_chan_rsp(chan, rsp_result); + } + + chan->move_role = L2CAP_MOVE_ROLE_NONE; + chan->move_state = L2CAP_MOVE_STABLE; + + /* Restart data transmission */ + l2cap_ertm_send(chan); +} + +/* Invoke with locked chan */ +void __l2cap_physical_cfm(struct l2cap_chan *chan, int result) +{ + u8 local_amp_id = chan->local_amp_id; + u8 remote_amp_id = chan->remote_amp_id; + + BT_DBG("chan %p, result %d, local_amp_id %d, remote_amp_id %d", + chan, result, local_amp_id, remote_amp_id); + + if (chan->state == BT_DISCONN || chan->state == BT_CLOSED) { + l2cap_chan_unlock(chan); + return; + } + + if (chan->state != BT_CONNECTED) { + l2cap_do_create(chan, result, local_amp_id, remote_amp_id); + } else if (result != L2CAP_MR_SUCCESS) { + l2cap_do_move_cancel(chan, result); + } else { + switch (chan->move_role) { + case L2CAP_MOVE_ROLE_INITIATOR: + l2cap_do_move_initiate(chan, local_amp_id, + remote_amp_id); + break; + case L2CAP_MOVE_ROLE_RESPONDER: + l2cap_do_move_respond(chan, result); + break; + default: + l2cap_do_move_cancel(chan, result); + break; + } + } +} + +static inline int l2cap_move_channel_req(struct l2cap_conn *conn, + struct l2cap_cmd_hdr *cmd, + u16 cmd_len, void *data) +{ + struct l2cap_move_chan_req *req = data; + struct l2cap_move_chan_rsp rsp; + struct l2cap_chan *chan; + u16 icid = 0; + u16 result = L2CAP_MR_NOT_ALLOWED; + + if (cmd_len != sizeof(*req)) + return -EPROTO; + + icid = le16_to_cpu(req->icid); + + BT_DBG("icid 0x%4.4x, dest_amp_id %d", icid, req->dest_amp_id); + + if (!conn->hs_enabled) + return -EINVAL; + + chan = l2cap_get_chan_by_dcid(conn, icid); + if (!chan) { + rsp.icid = cpu_to_le16(icid); + rsp.result = cpu_to_le16(L2CAP_MR_NOT_ALLOWED); + l2cap_send_cmd(conn, cmd->ident, L2CAP_MOVE_CHAN_RSP, + sizeof(rsp), &rsp); + return 0; + } + + chan->ident = cmd->ident; + + if (chan->scid < L2CAP_CID_DYN_START || + chan->chan_policy == BT_CHANNEL_POLICY_BREDR_ONLY || + (chan->mode != L2CAP_MODE_ERTM && + chan->mode != L2CAP_MODE_STREAMING)) { + result = L2CAP_MR_NOT_ALLOWED; + goto send_move_response; + } + + if (chan->local_amp_id == req->dest_amp_id) { + result = L2CAP_MR_SAME_ID; + goto send_move_response; + } + + if (req->dest_amp_id != AMP_ID_BREDR) { + struct hci_dev *hdev; + hdev = hci_dev_get(req->dest_amp_id); + if (!hdev || hdev->dev_type != HCI_AMP || + !test_bit(HCI_UP, &hdev->flags)) { + if (hdev) + hci_dev_put(hdev); + + result = L2CAP_MR_BAD_ID; + goto send_move_response; + } + hci_dev_put(hdev); + } + + /* Detect a move collision. Only send a collision response + * if this side has "lost", otherwise proceed with the move. + * The winner has the larger bd_addr. + */ + if ((__chan_is_moving(chan) || + chan->move_role != L2CAP_MOVE_ROLE_NONE) && + bacmp(&conn->hcon->src, &conn->hcon->dst) > 0) { + result = L2CAP_MR_COLLISION; + goto send_move_response; + } + + chan->move_role = L2CAP_MOVE_ROLE_RESPONDER; + l2cap_move_setup(chan); + chan->move_id = req->dest_amp_id; + icid = chan->dcid; + + if (req->dest_amp_id == AMP_ID_BREDR) { + /* Moving to BR/EDR */ + if (test_bit(CONN_LOCAL_BUSY, &chan->conn_state)) { + chan->move_state = L2CAP_MOVE_WAIT_LOCAL_BUSY; + result = L2CAP_MR_PEND; + } else { + chan->move_state = L2CAP_MOVE_WAIT_CONFIRM; + result = L2CAP_MR_SUCCESS; + } + } else { + chan->move_state = L2CAP_MOVE_WAIT_PREPARE; + /* Placeholder - uncomment when amp functions are available */ + /*amp_accept_physical(chan, req->dest_amp_id);*/ + result = L2CAP_MR_PEND; + } + +send_move_response: + l2cap_send_move_chan_rsp(chan, result); + + l2cap_chan_unlock(chan); + + return 0; +} + +static void l2cap_move_continue(struct l2cap_conn *conn, u16 icid, u16 result) +{ + struct l2cap_chan *chan; + struct hci_chan *hchan = NULL; + + chan = l2cap_get_chan_by_scid(conn, icid); + if (!chan) { + l2cap_send_move_chan_cfm_icid(conn, icid); + return; + } + + __clear_chan_timer(chan); + if (result == L2CAP_MR_PEND) + __set_chan_timer(chan, L2CAP_MOVE_ERTX_TIMEOUT); + + switch (chan->move_state) { + case L2CAP_MOVE_WAIT_LOGICAL_COMP: + /* Move confirm will be sent when logical link + * is complete. + */ + chan->move_state = L2CAP_MOVE_WAIT_LOGICAL_CFM; + break; + case L2CAP_MOVE_WAIT_RSP_SUCCESS: + if (result == L2CAP_MR_PEND) { + break; + } else if (test_bit(CONN_LOCAL_BUSY, + &chan->conn_state)) { + chan->move_state = L2CAP_MOVE_WAIT_LOCAL_BUSY; + } else { + /* Logical link is up or moving to BR/EDR, + * proceed with move + */ + chan->move_state = L2CAP_MOVE_WAIT_CONFIRM_RSP; + l2cap_send_move_chan_cfm(chan, L2CAP_MC_CONFIRMED); + } + break; + case L2CAP_MOVE_WAIT_RSP: + /* Moving to AMP */ + if (result == L2CAP_MR_SUCCESS) { + /* Remote is ready, send confirm immediately + * after logical link is ready + */ + chan->move_state = L2CAP_MOVE_WAIT_LOGICAL_CFM; + } else { + /* Both logical link and move success + * are required to confirm + */ + chan->move_state = L2CAP_MOVE_WAIT_LOGICAL_COMP; + } + + /* Placeholder - get hci_chan for logical link */ + if (!hchan) { + /* Logical link not available */ + l2cap_send_move_chan_cfm(chan, L2CAP_MC_UNCONFIRMED); + break; + } + + /* If the logical link is not yet connected, do not + * send confirmation. + */ + if (hchan->state != BT_CONNECTED) + break; + + /* Logical link is already ready to go */ + + chan->hs_hcon = hchan->conn; + chan->hs_hcon->l2cap_data = chan->conn; + + if (result == L2CAP_MR_SUCCESS) { + /* Can confirm now */ + l2cap_send_move_chan_cfm(chan, L2CAP_MC_CONFIRMED); + } else { + /* Now only need move success + * to confirm + */ + chan->move_state = L2CAP_MOVE_WAIT_RSP_SUCCESS; + } + + l2cap_logical_cfm(chan, hchan, L2CAP_MR_SUCCESS); + break; + default: + /* Any other amp move state means the move failed. */ + chan->move_id = chan->local_amp_id; + l2cap_move_done(chan); + l2cap_send_move_chan_cfm(chan, L2CAP_MC_UNCONFIRMED); + } + + l2cap_chan_unlock(chan); +} + +static void l2cap_move_fail(struct l2cap_conn *conn, u8 ident, u16 icid, + u16 result) +{ + struct l2cap_chan *chan; + + chan = l2cap_get_chan_by_ident(conn, ident); + if (!chan) { + /* Could not locate channel, icid is best guess */ + l2cap_send_move_chan_cfm_icid(conn, icid); + return; + } + + __clear_chan_timer(chan); + + if (chan->move_role == L2CAP_MOVE_ROLE_INITIATOR) { + if (result == L2CAP_MR_COLLISION) { + chan->move_role = L2CAP_MOVE_ROLE_RESPONDER; + } else { + /* Cleanup - cancel move */ + chan->move_id = chan->local_amp_id; + l2cap_move_done(chan); + } + } + + l2cap_send_move_chan_cfm(chan, L2CAP_MC_UNCONFIRMED); + + l2cap_chan_unlock(chan); +} + +static int l2cap_move_channel_rsp(struct l2cap_conn *conn, + struct l2cap_cmd_hdr *cmd, + u16 cmd_len, void *data) +{ + struct l2cap_move_chan_rsp *rsp = data; + u16 icid, result; + + if (cmd_len != sizeof(*rsp)) + return -EPROTO; + + icid = le16_to_cpu(rsp->icid); + result = le16_to_cpu(rsp->result); + + BT_DBG("icid 0x%4.4x, result 0x%4.4x", icid, result); + + if (result == L2CAP_MR_SUCCESS || result == L2CAP_MR_PEND) + l2cap_move_continue(conn, icid, result); + else + l2cap_move_fail(conn, cmd->ident, icid, result); + + return 0; +} + +static int l2cap_move_channel_confirm(struct l2cap_conn *conn, + struct l2cap_cmd_hdr *cmd, + u16 cmd_len, void *data) +{ + struct l2cap_move_chan_cfm *cfm = data; + struct l2cap_chan *chan; + u16 icid, result; + + if (cmd_len != sizeof(*cfm)) + return -EPROTO; + + icid = le16_to_cpu(cfm->icid); + result = le16_to_cpu(cfm->result); + + BT_DBG("icid 0x%4.4x, result 0x%4.4x", icid, result); + + chan = l2cap_get_chan_by_dcid(conn, icid); + if (!chan) { + /* Spec requires a response even if the icid was not found */ + l2cap_send_move_chan_cfm_rsp(conn, cmd->ident, icid); + return 0; + } + + if (chan->move_state == L2CAP_MOVE_WAIT_CONFIRM) { + if (result == L2CAP_MC_CONFIRMED) { + chan->local_amp_id = chan->move_id; + if (chan->local_amp_id == AMP_ID_BREDR) + __release_logical_link(chan); + } else { + chan->move_id = chan->local_amp_id; + } + + l2cap_move_done(chan); + } + + l2cap_send_move_chan_cfm_rsp(conn, cmd->ident, icid); + + l2cap_chan_unlock(chan); + + return 0; +} + +static inline int l2cap_move_channel_confirm_rsp(struct l2cap_conn *conn, + struct l2cap_cmd_hdr *cmd, + u16 cmd_len, void *data) +{ + struct l2cap_move_chan_cfm_rsp *rsp = data; + struct l2cap_chan *chan; + u16 icid; + + if (cmd_len != sizeof(*rsp)) + return -EPROTO; + + icid = le16_to_cpu(rsp->icid); + + BT_DBG("icid 0x%4.4x", icid); + + chan = l2cap_get_chan_by_scid(conn, icid); + if (!chan) + return 0; + + __clear_chan_timer(chan); + + if (chan->move_state == L2CAP_MOVE_WAIT_CONFIRM_RSP) { + chan->local_amp_id = chan->move_id; + + if (chan->local_amp_id == AMP_ID_BREDR && chan->hs_hchan) + __release_logical_link(chan); + + l2cap_move_done(chan); + } + + l2cap_chan_unlock(chan); return 0; } static inline int l2cap_check_conn_param(u16 min, u16 max, u16 latency, - u16 to_multiplier) + u16 to_multiplier) { u16 max_latency; @@ -2522,18 +5219,18 @@ static inline int l2cap_check_conn_param(u16 min, u16 max, u16 latency, } static inline int l2cap_conn_param_update_req(struct l2cap_conn *conn, - struct l2cap_cmd_hdr *cmd, u8 *data) + struct l2cap_cmd_hdr *cmd, + u16 cmd_len, u8 *data) { struct hci_conn *hcon = conn->hcon; struct l2cap_conn_param_update_req *req; struct l2cap_conn_param_update_rsp rsp; - u16 min, max, latency, to_multiplier, cmd_len; + u16 min, max, latency, to_multiplier; int err; if (!(hcon->link_mode & HCI_LM_MASTER)) return -EINVAL; - cmd_len = __le16_to_cpu(cmd->len); if (cmd_len != sizeof(struct l2cap_conn_param_update_req)) return -EPROTO; @@ -2544,7 +5241,7 @@ static inline int l2cap_conn_param_update_req(struct l2cap_conn *conn, to_multiplier = __le16_to_cpu(req->to_multiplier); BT_DBG("min 0x%4.4x max 0x%4.4x latency: 0x%4.4x Timeout: 0x%4.4x", - min, max, latency, to_multiplier); + min, max, latency, to_multiplier); memset(&rsp, 0, sizeof(rsp)); @@ -2555,7 +5252,7 @@ static inline int l2cap_conn_param_update_req(struct l2cap_conn *conn, rsp.result = cpu_to_le16(L2CAP_CONN_PARAM_ACCEPTED); l2cap_send_cmd(conn, cmd->ident, L2CAP_CONN_PARAM_UPDATE_RSP, - sizeof(rsp), &rsp); + sizeof(rsp), &rsp); if (!err) hci_le_conn_update(hcon, min, max, latency, to_multiplier); @@ -2563,22 +5260,83 @@ static inline int l2cap_conn_param_update_req(struct l2cap_conn *conn, return 0; } +static int l2cap_le_connect_rsp(struct l2cap_conn *conn, + struct l2cap_cmd_hdr *cmd, u16 cmd_len, + u8 *data) +{ + struct l2cap_le_conn_rsp *rsp = (struct l2cap_le_conn_rsp *) data; + u16 dcid, mtu, mps, credits, result; + struct l2cap_chan *chan; + int err; + + if (cmd_len < sizeof(*rsp)) + return -EPROTO; + + dcid = __le16_to_cpu(rsp->dcid); + mtu = __le16_to_cpu(rsp->mtu); + mps = __le16_to_cpu(rsp->mps); + credits = __le16_to_cpu(rsp->credits); + result = __le16_to_cpu(rsp->result); + + if (result == L2CAP_CR_SUCCESS && (mtu < 23 || mps < 23)) + return -EPROTO; + + BT_DBG("dcid 0x%4.4x mtu %u mps %u credits %u result 0x%2.2x", + dcid, mtu, mps, credits, result); + + mutex_lock(&conn->chan_lock); + + chan = __l2cap_get_chan_by_ident(conn, cmd->ident); + if (!chan) { + err = -EBADSLT; + goto unlock; + } + + err = 0; + + l2cap_chan_lock(chan); + + switch (result) { + case L2CAP_CR_SUCCESS: + chan->ident = 0; + chan->dcid = dcid; + chan->omtu = mtu; + chan->remote_mps = mps; + chan->tx_credits = credits; + l2cap_chan_ready(chan); + break; + + default: + l2cap_chan_del(chan, ECONNREFUSED); + break; + } + + l2cap_chan_unlock(chan); + +unlock: + mutex_unlock(&conn->chan_lock); + + return err; +} + static inline int l2cap_bredr_sig_cmd(struct l2cap_conn *conn, - struct l2cap_cmd_hdr *cmd, u16 cmd_len, u8 *data) + struct l2cap_cmd_hdr *cmd, u16 cmd_len, + u8 *data) { int err = 0; switch (cmd->code) { case L2CAP_COMMAND_REJ: - l2cap_command_rej(conn, cmd, data); + l2cap_command_rej(conn, cmd, cmd_len, data); break; case L2CAP_CONN_REQ: - err = l2cap_connect_req(conn, cmd, data); + err = l2cap_connect_req(conn, cmd, cmd_len, data); break; case L2CAP_CONN_RSP: - err = l2cap_connect_rsp(conn, cmd, data); + case L2CAP_CREATE_CHAN_RSP: + l2cap_connect_create_rsp(conn, cmd, cmd_len, data); break; case L2CAP_CONF_REQ: @@ -2586,15 +5344,15 @@ static inline int l2cap_bredr_sig_cmd(struct l2cap_conn *conn, break; case L2CAP_CONF_RSP: - err = l2cap_config_rsp(conn, cmd, data); + l2cap_config_rsp(conn, cmd, cmd_len, data); break; case L2CAP_DISCONN_REQ: - err = l2cap_disconnect_req(conn, cmd, data); + err = l2cap_disconnect_req(conn, cmd, cmd_len, data); break; case L2CAP_DISCONN_RSP: - err = l2cap_disconnect_rsp(conn, cmd, data); + l2cap_disconnect_rsp(conn, cmd, cmd_len, data); break; case L2CAP_ECHO_REQ: @@ -2605,11 +5363,31 @@ static inline int l2cap_bredr_sig_cmd(struct l2cap_conn *conn, break; case L2CAP_INFO_REQ: - err = l2cap_information_req(conn, cmd, data); + err = l2cap_information_req(conn, cmd, cmd_len, data); break; case L2CAP_INFO_RSP: - err = l2cap_information_rsp(conn, cmd, data); + l2cap_information_rsp(conn, cmd, cmd_len, data); + break; + + case L2CAP_CREATE_CHAN_REQ: + err = l2cap_create_channel_req(conn, cmd, cmd_len, data); + break; + + case L2CAP_MOVE_CHAN_REQ: + err = l2cap_move_channel_req(conn, cmd, cmd_len, data); + break; + + case L2CAP_MOVE_CHAN_RSP: + l2cap_move_channel_rsp(conn, cmd, cmd_len, data); + break; + + case L2CAP_MOVE_CHAN_CFM: + err = l2cap_move_channel_confirm(conn, cmd, cmd_len, data); + break; + + case L2CAP_MOVE_CHAN_CFM_RSP: + l2cap_move_channel_confirm_rsp(conn, cmd, cmd_len, data); break; default: @@ -2621,28 +5399,282 @@ static inline int l2cap_bredr_sig_cmd(struct l2cap_conn *conn, return err; } +static int l2cap_le_connect_req(struct l2cap_conn *conn, + struct l2cap_cmd_hdr *cmd, u16 cmd_len, + u8 *data) +{ + struct l2cap_le_conn_req *req = (struct l2cap_le_conn_req *) data; + struct l2cap_le_conn_rsp rsp; + struct l2cap_chan *chan, *pchan; + u16 dcid, scid, credits, mtu, mps; + __le16 psm; + u8 result; + + if (cmd_len != sizeof(*req)) + return -EPROTO; + + scid = __le16_to_cpu(req->scid); + mtu = __le16_to_cpu(req->mtu); + mps = __le16_to_cpu(req->mps); + psm = req->psm; + dcid = 0; + credits = 0; + + if (mtu < 23 || mps < 23) + return -EPROTO; + + BT_DBG("psm 0x%2.2x scid 0x%4.4x mtu %u mps %u", __le16_to_cpu(psm), + scid, mtu, mps); + + /* Check if we have socket listening on psm */ + pchan = l2cap_global_chan_by_psm(BT_LISTEN, psm, &conn->hcon->src, + &conn->hcon->dst, LE_LINK); + if (!pchan) { + result = L2CAP_CR_BAD_PSM; + chan = NULL; + goto response; + } + + mutex_lock(&conn->chan_lock); + l2cap_chan_lock(pchan); + + if (!smp_sufficient_security(conn->hcon, pchan->sec_level)) { + result = L2CAP_CR_AUTHENTICATION; + chan = NULL; + goto response_unlock; + } + + /* Check if we already have channel with that dcid */ + if (__l2cap_get_chan_by_dcid(conn, scid)) { + result = L2CAP_CR_NO_MEM; + chan = NULL; + goto response_unlock; + } + + chan = pchan->ops->new_connection(pchan); + if (!chan) { + result = L2CAP_CR_NO_MEM; + goto response_unlock; + } + + l2cap_le_flowctl_init(chan); + + bacpy(&chan->src, &conn->hcon->src); + bacpy(&chan->dst, &conn->hcon->dst); + chan->src_type = bdaddr_type(conn->hcon, conn->hcon->src_type); + chan->dst_type = bdaddr_type(conn->hcon, conn->hcon->dst_type); + chan->psm = psm; + chan->dcid = scid; + chan->omtu = mtu; + chan->remote_mps = mps; + chan->tx_credits = __le16_to_cpu(req->credits); + + __l2cap_chan_add(conn, chan); + dcid = chan->scid; + credits = chan->rx_credits; + + __set_chan_timer(chan, chan->ops->get_sndtimeo(chan)); + + chan->ident = cmd->ident; + + if (test_bit(FLAG_DEFER_SETUP, &chan->flags)) { + l2cap_state_change(chan, BT_CONNECT2); + result = L2CAP_CR_PEND; + chan->ops->defer(chan); + } else { + l2cap_chan_ready(chan); + result = L2CAP_CR_SUCCESS; + } + +response_unlock: + l2cap_chan_unlock(pchan); + mutex_unlock(&conn->chan_lock); + + if (result == L2CAP_CR_PEND) + return 0; + +response: + if (chan) { + rsp.mtu = cpu_to_le16(chan->imtu); + rsp.mps = cpu_to_le16(chan->mps); + } else { + rsp.mtu = 0; + rsp.mps = 0; + } + + rsp.dcid = cpu_to_le16(dcid); + rsp.credits = cpu_to_le16(credits); + rsp.result = cpu_to_le16(result); + + l2cap_send_cmd(conn, cmd->ident, L2CAP_LE_CONN_RSP, sizeof(rsp), &rsp); + + return 0; +} + +static inline int l2cap_le_credits(struct l2cap_conn *conn, + struct l2cap_cmd_hdr *cmd, u16 cmd_len, + u8 *data) +{ + struct l2cap_le_credits *pkt; + struct l2cap_chan *chan; + u16 cid, credits, max_credits; + + if (cmd_len != sizeof(*pkt)) + return -EPROTO; + + pkt = (struct l2cap_le_credits *) data; + cid = __le16_to_cpu(pkt->cid); + credits = __le16_to_cpu(pkt->credits); + + BT_DBG("cid 0x%4.4x credits 0x%4.4x", cid, credits); + + chan = l2cap_get_chan_by_dcid(conn, cid); + if (!chan) + return -EBADSLT; + + max_credits = LE_FLOWCTL_MAX_CREDITS - chan->tx_credits; + if (credits > max_credits) { + BT_ERR("LE credits overflow"); + l2cap_send_disconn_req(chan, ECONNRESET); + + /* Return 0 so that we don't trigger an unnecessary + * command reject packet. + */ + return 0; + } + + chan->tx_credits += credits; + + while (chan->tx_credits && !skb_queue_empty(&chan->tx_q)) { + l2cap_do_send(chan, skb_dequeue(&chan->tx_q)); + chan->tx_credits--; + } + + if (chan->tx_credits) + chan->ops->resume(chan); + + l2cap_chan_unlock(chan); + + return 0; +} + +static inline int l2cap_le_command_rej(struct l2cap_conn *conn, + struct l2cap_cmd_hdr *cmd, u16 cmd_len, + u8 *data) +{ + struct l2cap_cmd_rej_unk *rej = (struct l2cap_cmd_rej_unk *) data; + struct l2cap_chan *chan; + + if (cmd_len < sizeof(*rej)) + return -EPROTO; + + mutex_lock(&conn->chan_lock); + + chan = __l2cap_get_chan_by_ident(conn, cmd->ident); + if (!chan) + goto done; + + l2cap_chan_lock(chan); + l2cap_chan_del(chan, ECONNREFUSED); + l2cap_chan_unlock(chan); + +done: + mutex_unlock(&conn->chan_lock); + return 0; +} + static inline int l2cap_le_sig_cmd(struct l2cap_conn *conn, - struct l2cap_cmd_hdr *cmd, u8 *data) + struct l2cap_cmd_hdr *cmd, u16 cmd_len, + u8 *data) { + int err = 0; + switch (cmd->code) { case L2CAP_COMMAND_REJ: - return 0; + l2cap_le_command_rej(conn, cmd, cmd_len, data); + break; case L2CAP_CONN_PARAM_UPDATE_REQ: - return l2cap_conn_param_update_req(conn, cmd, data); + err = l2cap_conn_param_update_req(conn, cmd, cmd_len, data); + break; case L2CAP_CONN_PARAM_UPDATE_RSP: - return 0; + break; + + case L2CAP_LE_CONN_RSP: + l2cap_le_connect_rsp(conn, cmd, cmd_len, data); + break; + + case L2CAP_LE_CONN_REQ: + err = l2cap_le_connect_req(conn, cmd, cmd_len, data); + break; + + case L2CAP_LE_CREDITS: + err = l2cap_le_credits(conn, cmd, cmd_len, data); + break; + + case L2CAP_DISCONN_REQ: + err = l2cap_disconnect_req(conn, cmd, cmd_len, data); + break; + + case L2CAP_DISCONN_RSP: + l2cap_disconnect_rsp(conn, cmd, cmd_len, data); + break; default: BT_ERR("Unknown LE signaling command 0x%2.2x", cmd->code); - return -EINVAL; + err = -EINVAL; + break; } + + return err; +} + +static inline void l2cap_le_sig_channel(struct l2cap_conn *conn, + struct sk_buff *skb) +{ + struct hci_conn *hcon = conn->hcon; + struct l2cap_cmd_hdr *cmd; + u16 len; + int err; + + if (hcon->type != LE_LINK) + goto drop; + + if (skb->len < L2CAP_CMD_HDR_SIZE) + goto drop; + + cmd = (void *) skb->data; + skb_pull(skb, L2CAP_CMD_HDR_SIZE); + + len = le16_to_cpu(cmd->len); + + BT_DBG("code 0x%2.2x len %d id 0x%2.2x", cmd->code, len, cmd->ident); + + if (len != skb->len || !cmd->ident) { + BT_DBG("corrupted command"); + goto drop; + } + + err = l2cap_le_sig_cmd(conn, cmd, len, skb->data); + if (err) { + struct l2cap_cmd_rej_unk rej; + + BT_ERR("Wrong link type (%d)", err); + + rej.reason = cpu_to_le16(L2CAP_REJ_NOT_UNDERSTOOD); + l2cap_send_cmd(conn, cmd->ident, L2CAP_COMMAND_REJ, + sizeof(rej), &rej); + } + +drop: + kfree_skb(skb); } static inline void l2cap_sig_channel(struct l2cap_conn *conn, - struct sk_buff *skb) + struct sk_buff *skb) { + struct hci_conn *hcon = conn->hcon; u8 *data = skb->data; int len = skb->len; struct l2cap_cmd_hdr cmd; @@ -2650,6 +5682,9 @@ static inline void l2cap_sig_channel(struct l2cap_conn *conn, l2cap_raw_recv(conn, skb); + if (hcon->type != ACL_LINK) + goto drop; + while (len >= L2CAP_CMD_HDR_SIZE) { u16 cmd_len; memcpy(&cmd, data, L2CAP_CMD_HDR_SIZE); @@ -2658,41 +5693,45 @@ static inline void l2cap_sig_channel(struct l2cap_conn *conn, cmd_len = le16_to_cpu(cmd.len); - BT_DBG("code 0x%2.2x len %d id 0x%2.2x", cmd.code, cmd_len, cmd.ident); + BT_DBG("code 0x%2.2x len %d id 0x%2.2x", cmd.code, cmd_len, + cmd.ident); if (cmd_len > len || !cmd.ident) { BT_DBG("corrupted command"); break; } - if (conn->hcon->type == LE_LINK) - err = l2cap_le_sig_cmd(conn, &cmd, data); - else - err = l2cap_bredr_sig_cmd(conn, &cmd, cmd_len, data); - + err = l2cap_bredr_sig_cmd(conn, &cmd, cmd_len, data); if (err) { - struct l2cap_cmd_rej rej; - BT_DBG("error %d", err); + struct l2cap_cmd_rej_unk rej; + + BT_ERR("Wrong link type (%d)", err); - /* FIXME: Map err to a valid reason */ - rej.reason = cpu_to_le16(0); - l2cap_send_cmd(conn, cmd.ident, L2CAP_COMMAND_REJ, sizeof(rej), &rej); + rej.reason = cpu_to_le16(L2CAP_REJ_NOT_UNDERSTOOD); + l2cap_send_cmd(conn, cmd.ident, L2CAP_COMMAND_REJ, + sizeof(rej), &rej); } data += cmd_len; len -= cmd_len; } +drop: kfree_skb(skb); } -static int l2cap_check_fcs(struct l2cap_pinfo *pi, struct sk_buff *skb) +static int l2cap_check_fcs(struct l2cap_chan *chan, struct sk_buff *skb) { u16 our_fcs, rcv_fcs; - int hdr_size = L2CAP_HDR_SIZE + 2; + int hdr_size; + + if (test_bit(FLAG_EXT_CTRL, &chan->flags)) + hdr_size = L2CAP_EXT_HDR_SIZE; + else + hdr_size = L2CAP_ENH_HDR_SIZE; - if (pi->fcs == L2CAP_FCS_CRC16) { - skb_trim(skb, skb->len - 2); + if (chan->fcs == L2CAP_FCS_CRC16) { + skb_trim(skb, skb->len - L2CAP_FCS_SIZE); rcv_fcs = get_unaligned_le16(skb->data + skb->len); our_fcs = crc16(0, skb->data - hdr_size, skb->len + hdr_size); @@ -2702,957 +5741,1170 @@ static int l2cap_check_fcs(struct l2cap_pinfo *pi, struct sk_buff *skb) return 0; } -static inline void l2cap_send_i_or_rr_or_rnr(struct sock *sk) +static void l2cap_send_i_or_rr_or_rnr(struct l2cap_chan *chan) { - struct l2cap_pinfo *pi = l2cap_pi(sk); - u16 control = 0; + struct l2cap_ctrl control; - pi->frames_sent = 0; + BT_DBG("chan %p", chan); - control |= pi->buffer_seq << L2CAP_CTRL_REQSEQ_SHIFT; + memset(&control, 0, sizeof(control)); + control.sframe = 1; + control.final = 1; + control.reqseq = chan->buffer_seq; + set_bit(CONN_SEND_FBIT, &chan->conn_state); - if (pi->conn_state & L2CAP_CONN_LOCAL_BUSY) { - control |= L2CAP_SUPER_RCV_NOT_READY; - l2cap_send_sframe(pi, control); - pi->conn_state |= L2CAP_CONN_RNR_SENT; + if (test_bit(CONN_LOCAL_BUSY, &chan->conn_state)) { + control.super = L2CAP_SUPER_RNR; + l2cap_send_sframe(chan, &control); } - if (pi->conn_state & L2CAP_CONN_REMOTE_BUSY) - l2cap_retransmit_frames(sk); + if (test_and_clear_bit(CONN_REMOTE_BUSY, &chan->conn_state) && + chan->unacked_frames > 0) + __set_retrans_timer(chan); - l2cap_ertm_send(sk); + /* Send pending iframes */ + l2cap_ertm_send(chan); - if (!(pi->conn_state & L2CAP_CONN_LOCAL_BUSY) && - pi->frames_sent == 0) { - control |= L2CAP_SUPER_RCV_READY; - l2cap_send_sframe(pi, control); + if (!test_bit(CONN_LOCAL_BUSY, &chan->conn_state) && + test_bit(CONN_SEND_FBIT, &chan->conn_state)) { + /* F-bit wasn't sent in an s-frame or i-frame yet, so + * send it now. + */ + control.super = L2CAP_SUPER_RR; + l2cap_send_sframe(chan, &control); } } -static int l2cap_add_to_srej_queue(struct sock *sk, struct sk_buff *skb, u8 tx_seq, u8 sar) +static void append_skb_frag(struct sk_buff *skb, struct sk_buff *new_frag, + struct sk_buff **last_frag) { - struct sk_buff *next_skb; - struct l2cap_pinfo *pi = l2cap_pi(sk); - int tx_seq_offset, next_tx_seq_offset; + /* skb->len reflects data in skb as well as all fragments + * skb->data_len reflects only data in fragments + */ + if (!skb_has_frag_list(skb)) + skb_shinfo(skb)->frag_list = new_frag; - bt_cb(skb)->tx_seq = tx_seq; - bt_cb(skb)->sar = sar; + new_frag->next = NULL; - next_skb = skb_peek(SREJ_QUEUE(sk)); - if (!next_skb) { - __skb_queue_tail(SREJ_QUEUE(sk), skb); - return 0; - } + (*last_frag)->next = new_frag; + *last_frag = new_frag; - tx_seq_offset = (tx_seq - pi->buffer_seq) % 64; - if (tx_seq_offset < 0) - tx_seq_offset += 64; + skb->len += new_frag->len; + skb->data_len += new_frag->len; + skb->truesize += new_frag->truesize; +} - do { - if (bt_cb(next_skb)->tx_seq == tx_seq) - return -EINVAL; +static int l2cap_reassemble_sdu(struct l2cap_chan *chan, struct sk_buff *skb, + struct l2cap_ctrl *control) +{ + int err = -EINVAL; - next_tx_seq_offset = (bt_cb(next_skb)->tx_seq - - pi->buffer_seq) % 64; - if (next_tx_seq_offset < 0) - next_tx_seq_offset += 64; + switch (control->sar) { + case L2CAP_SAR_UNSEGMENTED: + if (chan->sdu) + break; - if (next_tx_seq_offset > tx_seq_offset) { - __skb_queue_before(SREJ_QUEUE(sk), next_skb, skb); - return 0; - } + err = chan->ops->recv(chan, skb); + break; - if (skb_queue_is_last(SREJ_QUEUE(sk), next_skb)) + case L2CAP_SAR_START: + if (chan->sdu) break; - } while ((next_skb = skb_queue_next(SREJ_QUEUE(sk), next_skb))); - - __skb_queue_tail(SREJ_QUEUE(sk), skb); + chan->sdu_len = get_unaligned_le16(skb->data); + skb_pull(skb, L2CAP_SDULEN_SIZE); - return 0; -} + if (chan->sdu_len > chan->imtu) { + err = -EMSGSIZE; + break; + } -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; + if (skb->len >= chan->sdu_len) + break; - 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; + chan->sdu = skb; + chan->sdu_last_frag = skb; + skb = NULL; + err = 0; 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; + case L2CAP_SAR_CONTINUE: + if (!chan->sdu) + break; - /* 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); + append_skb_frag(chan->sdu, skb, + &chan->sdu_last_frag); + skb = NULL; - memcpy(skb_put(pi->sdu, skb->len), skb->data, skb->len); + if (chan->sdu->len >= chan->sdu_len) + break; - pi->conn_state |= L2CAP_CONN_SAR_SDU; - pi->partial_sdu_len = skb->len; + err = 0; break; - case L2CAP_SDU_CONTINUE: - if (!(pi->conn_state & L2CAP_CONN_SAR_SDU)) - goto disconnect; + case L2CAP_SAR_END: + if (!chan->sdu) + break; - if (!pi->sdu) - goto disconnect; + append_skb_frag(chan->sdu, skb, + &chan->sdu_last_frag); + skb = NULL; - pi->partial_sdu_len += skb->len; - if (pi->partial_sdu_len > pi->sdu_len) - goto drop; + if (chan->sdu->len != chan->sdu_len) + break; - memcpy(skb_put(pi->sdu, skb->len), skb->data, skb->len); + err = chan->ops->recv(chan, chan->sdu); + if (!err) { + /* Reassembly complete */ + chan->sdu = NULL; + chan->sdu_last_frag = NULL; + chan->sdu_len = 0; + } break; + } - case L2CAP_SDU_END: - if (!(pi->conn_state & L2CAP_CONN_SAR_SDU)) - goto disconnect; + if (err) { + kfree_skb(skb); + kfree_skb(chan->sdu); + chan->sdu = NULL; + chan->sdu_last_frag = NULL; + chan->sdu_len = 0; + } - if (!pi->sdu) - goto disconnect; + return err; +} - if (!(pi->conn_state & L2CAP_CONN_SAR_RETRY)) { - pi->partial_sdu_len += skb->len; +static int l2cap_resegment(struct l2cap_chan *chan) +{ + /* Placeholder */ + return 0; +} - if (pi->partial_sdu_len > pi->imtu) - goto drop; +void l2cap_chan_busy(struct l2cap_chan *chan, int busy) +{ + u8 event; - if (pi->partial_sdu_len != pi->sdu_len) - goto drop; + if (chan->mode != L2CAP_MODE_ERTM) + return; - memcpy(skb_put(pi->sdu, skb->len), skb->data, skb->len); - } + event = busy ? L2CAP_EV_LOCAL_BUSY_DETECTED : L2CAP_EV_LOCAL_BUSY_CLEAR; + l2cap_tx(chan, NULL, NULL, event); +} - _skb = skb_clone(pi->sdu, GFP_ATOMIC); - if (!_skb) { - pi->conn_state |= L2CAP_CONN_SAR_RETRY; - return -ENOMEM; - } +static int l2cap_rx_queued_iframes(struct l2cap_chan *chan) +{ + int err = 0; + /* Pass sequential frames to l2cap_reassemble_sdu() + * until a gap is encountered. + */ - err = sock_queue_rcv_skb(sk, _skb); - if (err < 0) { - kfree_skb(_skb); - pi->conn_state |= L2CAP_CONN_SAR_RETRY; - return err; - } + BT_DBG("chan %p", chan); - pi->conn_state &= ~L2CAP_CONN_SAR_RETRY; - pi->conn_state &= ~L2CAP_CONN_SAR_SDU; + while (!test_bit(CONN_LOCAL_BUSY, &chan->conn_state)) { + struct sk_buff *skb; + BT_DBG("Searching for skb with txseq %d (queue len %d)", + chan->buffer_seq, skb_queue_len(&chan->srej_q)); - kfree_skb(pi->sdu); - break; - } + skb = l2cap_ertm_seq_in_queue(&chan->srej_q, chan->buffer_seq); - kfree_skb(skb); - return 0; + if (!skb) + break; -drop: - kfree_skb(pi->sdu); - pi->sdu = NULL; + skb_unlink(skb, &chan->srej_q); + chan->buffer_seq = __next_seq(chan, chan->buffer_seq); + err = l2cap_reassemble_sdu(chan, skb, &bt_cb(skb)->control); + if (err) + break; + } -disconnect: - l2cap_send_disconn_req(pi->conn, sk, ECONNRESET); - kfree_skb(skb); - return 0; + if (skb_queue_empty(&chan->srej_q)) { + chan->rx_state = L2CAP_RX_STATE_RECV; + l2cap_send_ack(chan); + } + + return err; } -static int l2cap_try_push_rx_skb(struct sock *sk) +static void l2cap_handle_srej(struct l2cap_chan *chan, + struct l2cap_ctrl *control) { - struct l2cap_pinfo *pi = l2cap_pi(sk); struct sk_buff *skb; - u16 control; - int err; - 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); - return -EBUSY; - } + BT_DBG("chan %p, control %p", chan, control); - pi->buffer_seq = (pi->buffer_seq + 1) % 64; + if (control->reqseq == chan->next_tx_seq) { + BT_DBG("Invalid reqseq %d, disconnecting", control->reqseq); + l2cap_send_disconn_req(chan, ECONNRESET); + return; } - if (!(pi->conn_state & L2CAP_CONN_RNR_SENT)) - goto done; + skb = l2cap_ertm_seq_in_queue(&chan->tx_q, control->reqseq); - 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; + if (skb == NULL) { + BT_DBG("Seq %d not available for retransmission", + control->reqseq); + return; + } - del_timer(&pi->retrans_timer); - __mod_monitor_timer(); + if (chan->max_tx != 0 && bt_cb(skb)->control.retries >= chan->max_tx) { + BT_DBG("Retry limit exceeded (%d)", chan->max_tx); + l2cap_send_disconn_req(chan, ECONNRESET); + return; + } - l2cap_pi(sk)->conn_state |= L2CAP_CONN_WAIT_F; + clear_bit(CONN_REMOTE_BUSY, &chan->conn_state); -done: - pi->conn_state &= ~L2CAP_CONN_LOCAL_BUSY; - pi->conn_state &= ~L2CAP_CONN_RNR_SENT; + if (control->poll) { + l2cap_pass_to_tx(chan, control); - BT_DBG("sk %p, Exit local busy", sk); + set_bit(CONN_SEND_FBIT, &chan->conn_state); + l2cap_retransmit(chan, control); + l2cap_ertm_send(chan); - return 0; + if (chan->tx_state == L2CAP_TX_STATE_WAIT_F) { + set_bit(CONN_SREJ_ACT, &chan->conn_state); + chan->srej_save_reqseq = control->reqseq; + } + } else { + l2cap_pass_to_tx_fbit(chan, control); + + if (control->final) { + if (chan->srej_save_reqseq != control->reqseq || + !test_and_clear_bit(CONN_SREJ_ACT, + &chan->conn_state)) + l2cap_retransmit(chan, control); + } else { + l2cap_retransmit(chan, control); + if (chan->tx_state == L2CAP_TX_STATE_WAIT_F) { + set_bit(CONN_SREJ_ACT, &chan->conn_state); + chan->srej_save_reqseq = control->reqseq; + } + } + } } -static void l2cap_busy_work(struct work_struct *work) +static void l2cap_handle_rej(struct l2cap_chan *chan, + struct l2cap_ctrl *control) { - 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; - lock_sock(sk); - - add_wait_queue(sk_sleep(sk), &wait); - while ((skb = skb_peek(BUSY_QUEUE(sk)))) { - set_current_state(TASK_INTERRUPTIBLE); + BT_DBG("chan %p, control %p", chan, control); - if (n_tries++ > L2CAP_LOCAL_BUSY_TRIES) { - err = -EBUSY; - l2cap_send_disconn_req(pi->conn, sk, EBUSY); - break; - } + if (control->reqseq == chan->next_tx_seq) { + BT_DBG("Invalid reqseq %d, disconnecting", control->reqseq); + l2cap_send_disconn_req(chan, ECONNRESET); + return; + } - if (!timeo) - timeo = HZ/5; + skb = l2cap_ertm_seq_in_queue(&chan->tx_q, control->reqseq); - if (signal_pending(current)) { - err = sock_intr_errno(timeo); - break; - } + if (chan->max_tx && skb && + bt_cb(skb)->control.retries >= chan->max_tx) { + BT_DBG("Retry limit exceeded (%d)", chan->max_tx); + l2cap_send_disconn_req(chan, ECONNRESET); + return; + } - release_sock(sk); - timeo = schedule_timeout(timeo); - lock_sock(sk); + clear_bit(CONN_REMOTE_BUSY, &chan->conn_state); - err = sock_error(sk); - if (err) - break; + l2cap_pass_to_tx(chan, control); - if (l2cap_try_push_rx_skb(sk) == 0) - break; + if (control->final) { + if (!test_and_clear_bit(CONN_REJ_ACT, &chan->conn_state)) + l2cap_retransmit_all(chan, control); + } else { + l2cap_retransmit_all(chan, control); + l2cap_ertm_send(chan); + if (chan->tx_state == L2CAP_TX_STATE_WAIT_F) + set_bit(CONN_REJ_ACT, &chan->conn_state); } - - 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) +static u8 l2cap_classify_txseq(struct l2cap_chan *chan, u16 txseq) { - struct l2cap_pinfo *pi = l2cap_pi(sk); - int sctrl, err; + BT_DBG("chan %p, txseq %d", chan, txseq); + + BT_DBG("last_acked_seq %d, expected_tx_seq %d", chan->last_acked_seq, + chan->expected_tx_seq); + + if (chan->rx_state == L2CAP_RX_STATE_SREJ_SENT) { + if (__seq_offset(chan, txseq, chan->last_acked_seq) >= + chan->tx_win) { + /* See notes below regarding "double poll" and + * invalid packets. + */ + if (chan->tx_win <= ((chan->tx_win_max + 1) >> 1)) { + BT_DBG("Invalid/Ignore - after SREJ"); + return L2CAP_TXSEQ_INVALID_IGNORE; + } else { + BT_DBG("Invalid - in window after SREJ sent"); + return L2CAP_TXSEQ_INVALID; + } + } - 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 l2cap_try_push_rx_skb(sk); + if (chan->srej_list.head == txseq) { + BT_DBG("Expected SREJ"); + return L2CAP_TXSEQ_EXPECTED_SREJ; + } + if (l2cap_ertm_seq_in_queue(&chan->srej_q, txseq)) { + BT_DBG("Duplicate SREJ - txseq already stored"); + return L2CAP_TXSEQ_DUPLICATE_SREJ; + } + if (l2cap_seq_list_contains(&chan->srej_list, txseq)) { + BT_DBG("Unexpected SREJ - not requested"); + return L2CAP_TXSEQ_UNEXPECTED_SREJ; + } } - err = l2cap_ertm_reassembly_sdu(sk, skb, control); - if (err >= 0) { - pi->buffer_seq = (pi->buffer_seq + 1) % 64; - return err; + if (chan->expected_tx_seq == txseq) { + if (__seq_offset(chan, txseq, chan->last_acked_seq) >= + chan->tx_win) { + BT_DBG("Invalid - txseq outside tx window"); + return L2CAP_TXSEQ_INVALID; + } else { + BT_DBG("Expected"); + return L2CAP_TXSEQ_EXPECTED; + } } - /* Busy Condition */ - BT_DBG("sk %p, Enter local busy", sk); + if (__seq_offset(chan, txseq, chan->last_acked_seq) < + __seq_offset(chan, chan->expected_tx_seq, chan->last_acked_seq)) { + BT_DBG("Duplicate - expected_tx_seq later than txseq"); + return L2CAP_TXSEQ_DUPLICATE; + } - pi->conn_state |= L2CAP_CONN_LOCAL_BUSY; - bt_cb(skb)->sar = control >> L2CAP_CTRL_SAR_SHIFT; - __skb_queue_tail(BUSY_QUEUE(sk), skb); + if (__seq_offset(chan, txseq, chan->last_acked_seq) >= chan->tx_win) { + /* A source of invalid packets is a "double poll" condition, + * where delays cause us to send multiple poll packets. If + * the remote stack receives and processes both polls, + * sequence numbers can wrap around in such a way that a + * resent frame has a sequence number that looks like new data + * with a sequence gap. This would trigger an erroneous SREJ + * request. + * + * Fortunately, this is impossible with a tx window that's + * less than half of the maximum sequence number, which allows + * invalid frames to be safely ignored. + * + * With tx window sizes greater than half of the tx window + * maximum, the frame is invalid and cannot be ignored. This + * causes a disconnect. + */ - sctrl = pi->buffer_seq << L2CAP_CTRL_REQSEQ_SHIFT; - sctrl |= L2CAP_SUPER_RCV_NOT_READY; - l2cap_send_sframe(pi, sctrl); + if (chan->tx_win <= ((chan->tx_win_max + 1) >> 1)) { + BT_DBG("Invalid/Ignore - txseq outside tx window"); + return L2CAP_TXSEQ_INVALID_IGNORE; + } else { + BT_DBG("Invalid - txseq outside tx window"); + return L2CAP_TXSEQ_INVALID; + } + } else { + BT_DBG("Unexpected - txseq indicates missing frames"); + return L2CAP_TXSEQ_UNEXPECTED; + } +} - pi->conn_state |= L2CAP_CONN_RNR_SENT; +static int l2cap_rx_state_recv(struct l2cap_chan *chan, + struct l2cap_ctrl *control, + struct sk_buff *skb, u8 event) +{ + int err = 0; + bool skb_in_use = false; - del_timer(&pi->ack_timer); + BT_DBG("chan %p, control %p, skb %p, event %d", chan, control, skb, + event); - queue_work(_busy_wq, &pi->busy_work); + switch (event) { + case L2CAP_EV_RECV_IFRAME: + switch (l2cap_classify_txseq(chan, control->txseq)) { + case L2CAP_TXSEQ_EXPECTED: + l2cap_pass_to_tx(chan, control); - return err; -} + if (test_bit(CONN_LOCAL_BUSY, &chan->conn_state)) { + BT_DBG("Busy, discarding expected seq %d", + control->txseq); + break; + } -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; + chan->expected_tx_seq = __next_seq(chan, + control->txseq); - /* - * TODO: We have to notify the userland if some data is lost with the - * Streaming Mode. - */ + chan->buffer_seq = chan->expected_tx_seq; + skb_in_use = true; + + err = l2cap_reassemble_sdu(chan, skb, control); + if (err) + break; - switch (control & L2CAP_CTRL_SAR) { - case L2CAP_SDU_UNSEGMENTED: - if (pi->conn_state & L2CAP_CONN_SAR_SDU) { - kfree_skb(pi->sdu); + if (control->final) { + if (!test_and_clear_bit(CONN_REJ_ACT, + &chan->conn_state)) { + control->final = 0; + l2cap_retransmit_all(chan, control); + l2cap_ertm_send(chan); + } + } + + if (!test_bit(CONN_LOCAL_BUSY, &chan->conn_state)) + l2cap_send_ack(chan); break; - } + case L2CAP_TXSEQ_UNEXPECTED: + l2cap_pass_to_tx(chan, control); + + /* Can't issue SREJ frames in the local busy state. + * Drop this frame, it will be seen as missing + * when local busy is exited. + */ + if (test_bit(CONN_LOCAL_BUSY, &chan->conn_state)) { + BT_DBG("Busy, discarding unexpected seq %d", + control->txseq); + break; + } - err = sock_queue_rcv_skb(sk, skb); - if (!err) - return 0; + /* There was a gap in the sequence, so an SREJ + * must be sent for each missing frame. The + * current frame is stored for later use. + */ + skb_queue_tail(&chan->srej_q, skb); + skb_in_use = true; + BT_DBG("Queued %p (queue len %d)", skb, + skb_queue_len(&chan->srej_q)); - break; + clear_bit(CONN_SREJ_ACT, &chan->conn_state); + l2cap_seq_list_clear(&chan->srej_list); + l2cap_send_srej(chan, control->txseq); - case L2CAP_SDU_START: - if (pi->conn_state & L2CAP_CONN_SAR_SDU) { - kfree_skb(pi->sdu); + chan->rx_state = L2CAP_RX_STATE_SREJ_SENT; + break; + case L2CAP_TXSEQ_DUPLICATE: + l2cap_pass_to_tx(chan, control); + break; + case L2CAP_TXSEQ_INVALID_IGNORE: + break; + case L2CAP_TXSEQ_INVALID: + default: + l2cap_send_disconn_req(chan, ECONNRESET); break; } + break; + case L2CAP_EV_RECV_RR: + l2cap_pass_to_tx(chan, control); + if (control->final) { + clear_bit(CONN_REMOTE_BUSY, &chan->conn_state); + + if (!test_and_clear_bit(CONN_REJ_ACT, &chan->conn_state) && + !__chan_is_moving(chan)) { + control->final = 0; + l2cap_retransmit_all(chan, control); + } - pi->sdu_len = get_unaligned_le16(skb->data); - skb_pull(skb, 2); + l2cap_ertm_send(chan); + } else if (control->poll) { + l2cap_send_i_or_rr_or_rnr(chan); + } else { + if (test_and_clear_bit(CONN_REMOTE_BUSY, + &chan->conn_state) && + chan->unacked_frames) + __set_retrans_timer(chan); - if (pi->sdu_len > pi->imtu) { - err = -EMSGSIZE; - break; + l2cap_ertm_send(chan); } - - pi->sdu = bt_skb_alloc(pi->sdu_len, GFP_ATOMIC); - if (!pi->sdu) { - err = -ENOMEM; - break; + break; + case L2CAP_EV_RECV_RNR: + set_bit(CONN_REMOTE_BUSY, &chan->conn_state); + l2cap_pass_to_tx(chan, control); + if (control && control->poll) { + set_bit(CONN_SEND_FBIT, &chan->conn_state); + l2cap_send_rr_or_rnr(chan, 0); } + __clear_retrans_timer(chan); + l2cap_seq_list_clear(&chan->retrans_list); + break; + case L2CAP_EV_RECV_REJ: + l2cap_handle_rej(chan, control); + break; + case L2CAP_EV_RECV_SREJ: + l2cap_handle_srej(chan, control); + break; + default: + break; + } - memcpy(skb_put(pi->sdu, skb->len), skb->data, skb->len); + if (skb && !skb_in_use) { + BT_DBG("Freeing %p", skb); + kfree_skb(skb); + } - pi->conn_state |= L2CAP_CONN_SAR_SDU; - pi->partial_sdu_len = skb->len; - err = 0; - break; + return err; +} - case L2CAP_SDU_CONTINUE: - if (!(pi->conn_state & L2CAP_CONN_SAR_SDU)) +static int l2cap_rx_state_srej_sent(struct l2cap_chan *chan, + struct l2cap_ctrl *control, + struct sk_buff *skb, u8 event) +{ + int err = 0; + u16 txseq = control->txseq; + bool skb_in_use = false; + + BT_DBG("chan %p, control %p, skb %p, event %d", chan, control, skb, + event); + + switch (event) { + case L2CAP_EV_RECV_IFRAME: + switch (l2cap_classify_txseq(chan, txseq)) { + case L2CAP_TXSEQ_EXPECTED: + /* Keep frame for reassembly later */ + l2cap_pass_to_tx(chan, control); + skb_queue_tail(&chan->srej_q, skb); + skb_in_use = true; + BT_DBG("Queued %p (queue len %d)", skb, + skb_queue_len(&chan->srej_q)); + + chan->expected_tx_seq = __next_seq(chan, txseq); break; + case L2CAP_TXSEQ_EXPECTED_SREJ: + l2cap_seq_list_pop(&chan->srej_list); - memcpy(skb_put(pi->sdu, skb->len), skb->data, skb->len); + l2cap_pass_to_tx(chan, control); + skb_queue_tail(&chan->srej_q, skb); + skb_in_use = true; + BT_DBG("Queued %p (queue len %d)", skb, + skb_queue_len(&chan->srej_q)); - pi->partial_sdu_len += skb->len; - if (pi->partial_sdu_len > pi->sdu_len) - kfree_skb(pi->sdu); - else - err = 0; - - break; + err = l2cap_rx_queued_iframes(chan); + if (err) + break; - case L2CAP_SDU_END: - if (!(pi->conn_state & L2CAP_CONN_SAR_SDU)) break; + case L2CAP_TXSEQ_UNEXPECTED: + /* Got a frame that can't be reassembled yet. + * Save it for later, and send SREJs to cover + * the missing frames. + */ + skb_queue_tail(&chan->srej_q, skb); + skb_in_use = true; + BT_DBG("Queued %p (queue len %d)", skb, + skb_queue_len(&chan->srej_q)); + + l2cap_pass_to_tx(chan, control); + l2cap_send_srej(chan, control->txseq); + break; + case L2CAP_TXSEQ_UNEXPECTED_SREJ: + /* This frame was requested with an SREJ, but + * some expected retransmitted frames are + * missing. Request retransmission of missing + * SREJ'd frames. + */ + skb_queue_tail(&chan->srej_q, skb); + skb_in_use = true; + BT_DBG("Queued %p (queue len %d)", skb, + skb_queue_len(&chan->srej_q)); + + l2cap_pass_to_tx(chan, control); + l2cap_send_srej_list(chan, control->txseq); + break; + case L2CAP_TXSEQ_DUPLICATE_SREJ: + /* We've already queued this frame. Drop this copy. */ + l2cap_pass_to_tx(chan, control); + break; + case L2CAP_TXSEQ_DUPLICATE: + /* Expecting a later sequence number, so this frame + * was already received. Ignore it completely. + */ + break; + case L2CAP_TXSEQ_INVALID_IGNORE: + break; + case L2CAP_TXSEQ_INVALID: + default: + l2cap_send_disconn_req(chan, ECONNRESET); + break; + } + break; + case L2CAP_EV_RECV_RR: + l2cap_pass_to_tx(chan, control); + if (control->final) { + clear_bit(CONN_REMOTE_BUSY, &chan->conn_state); + + if (!test_and_clear_bit(CONN_REJ_ACT, + &chan->conn_state)) { + control->final = 0; + l2cap_retransmit_all(chan, control); + } - memcpy(skb_put(pi->sdu, skb->len), skb->data, skb->len); - - pi->conn_state &= ~L2CAP_CONN_SAR_SDU; - pi->partial_sdu_len += skb->len; + l2cap_ertm_send(chan); + } else if (control->poll) { + if (test_and_clear_bit(CONN_REMOTE_BUSY, + &chan->conn_state) && + chan->unacked_frames) { + __set_retrans_timer(chan); + } - if (pi->partial_sdu_len > pi->imtu) - goto drop; + set_bit(CONN_SEND_FBIT, &chan->conn_state); + l2cap_send_srej_tail(chan); + } else { + if (test_and_clear_bit(CONN_REMOTE_BUSY, + &chan->conn_state) && + chan->unacked_frames) + __set_retrans_timer(chan); - if (pi->partial_sdu_len == pi->sdu_len) { - _skb = skb_clone(pi->sdu, GFP_ATOMIC); - err = sock_queue_rcv_skb(sk, _skb); - if (err < 0) - kfree_skb(_skb); + l2cap_send_ack(chan); + } + break; + case L2CAP_EV_RECV_RNR: + set_bit(CONN_REMOTE_BUSY, &chan->conn_state); + l2cap_pass_to_tx(chan, control); + if (control->poll) { + l2cap_send_srej_tail(chan); + } else { + struct l2cap_ctrl rr_control; + memset(&rr_control, 0, sizeof(rr_control)); + rr_control.sframe = 1; + rr_control.super = L2CAP_SUPER_RR; + rr_control.reqseq = chan->buffer_seq; + l2cap_send_sframe(chan, &rr_control); } - err = 0; -drop: - kfree_skb(pi->sdu); + break; + case L2CAP_EV_RECV_REJ: + l2cap_handle_rej(chan, control); + break; + case L2CAP_EV_RECV_SREJ: + l2cap_handle_srej(chan, control); break; } - kfree_skb(skb); + if (skb && !skb_in_use) { + BT_DBG("Freeing %p", skb); + kfree_skb(skb); + } + return err; } -static void l2cap_check_srej_gap(struct sock *sk, u8 tx_seq) +static int l2cap_finish_move(struct l2cap_chan *chan) { - struct sk_buff *skb; - u16 control; - - while ((skb = skb_peek(SREJ_QUEUE(sk)))) { - if (bt_cb(skb)->tx_seq != tx_seq) - break; + BT_DBG("chan %p", chan); - skb = skb_dequeue(SREJ_QUEUE(sk)); - 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 = (tx_seq + 1) % 64; - } -} + chan->rx_state = L2CAP_RX_STATE_RECV; -static void l2cap_resend_srejframe(struct sock *sk, u8 tx_seq) -{ - struct l2cap_pinfo *pi = l2cap_pi(sk); - struct srej_list *l, *tmp; - u16 control; + if (chan->hs_hcon) + chan->conn->mtu = chan->hs_hcon->hdev->block_mtu; + else + chan->conn->mtu = chan->conn->hcon->hdev->acl_mtu; - list_for_each_entry_safe(l, tmp, SREJ_LIST(sk), list) { - if (l->tx_seq == tx_seq) { - list_del(&l->list); - kfree(l); - return; - } - control = L2CAP_SUPER_SELECT_REJECT; - control |= l->tx_seq << L2CAP_CTRL_REQSEQ_SHIFT; - l2cap_send_sframe(pi, control); - list_del(&l->list); - list_add_tail(&l->list, SREJ_LIST(sk)); - } + return l2cap_resegment(chan); } -static void l2cap_send_srejframe(struct sock *sk, u8 tx_seq) +static int l2cap_rx_state_wait_p(struct l2cap_chan *chan, + struct l2cap_ctrl *control, + struct sk_buff *skb, u8 event) { - struct l2cap_pinfo *pi = l2cap_pi(sk); - struct srej_list *new; - u16 control; + int err; - while (tx_seq != pi->expected_tx_seq) { - control = L2CAP_SUPER_SELECT_REJECT; - control |= pi->expected_tx_seq << L2CAP_CTRL_REQSEQ_SHIFT; - l2cap_send_sframe(pi, control); + BT_DBG("chan %p, control %p, skb %p, event %d", chan, control, skb, + event); - new = kzalloc(sizeof(struct srej_list), GFP_ATOMIC); - new->tx_seq = pi->expected_tx_seq; - pi->expected_tx_seq = (pi->expected_tx_seq + 1) % 64; - list_add_tail(&new->list, SREJ_LIST(sk)); - } - pi->expected_tx_seq = (pi->expected_tx_seq + 1) % 64; -} + if (!control->poll) + return -EPROTO; -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); - u8 req_seq = __get_reqseq(rx_control); - u8 sar = rx_control >> L2CAP_CTRL_SAR_SHIFT; - int tx_seq_offset, expected_tx_seq_offset; - int num_to_ack = (pi->tx_win/6) + 1; - int err = 0; + l2cap_process_reqseq(chan, control->reqseq); - BT_DBG("sk %p len %d tx_seq %d rx_control 0x%4.4x", sk, skb->len, tx_seq, - rx_control); + if (!skb_queue_empty(&chan->tx_q)) + chan->tx_send_head = skb_peek(&chan->tx_q); + else + chan->tx_send_head = NULL; - 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(); - pi->conn_state &= ~L2CAP_CONN_WAIT_F; - } + /* Rewind next_tx_seq to the point expected + * by the receiver. + */ + chan->next_tx_seq = control->reqseq; + chan->unacked_frames = 0; - pi->expected_ack_seq = req_seq; - l2cap_drop_acked_frames(sk); + err = l2cap_finish_move(chan); + if (err) + return err; - if (tx_seq == pi->expected_tx_seq) - goto expected; + set_bit(CONN_SEND_FBIT, &chan->conn_state); + l2cap_send_i_or_rr_or_rnr(chan); - tx_seq_offset = (tx_seq - pi->buffer_seq) % 64; - if (tx_seq_offset < 0) - tx_seq_offset += 64; + if (event == L2CAP_EV_RECV_IFRAME) + return -EPROTO; - /* invalid tx_seq */ - if (tx_seq_offset >= pi->tx_win) { - l2cap_send_disconn_req(pi->conn, sk, ECONNRESET); - goto drop; - } + return l2cap_rx_state_recv(chan, control, NULL, event); +} - if (pi->conn_state == L2CAP_CONN_LOCAL_BUSY) - goto drop; +static int l2cap_rx_state_wait_f(struct l2cap_chan *chan, + struct l2cap_ctrl *control, + struct sk_buff *skb, u8 event) +{ + int err; - if (pi->conn_state & L2CAP_CONN_SREJ_SENT) { - struct srej_list *first; + if (!control->final) + return -EPROTO; - first = list_first_entry(SREJ_LIST(sk), - struct srej_list, list); - if (tx_seq == first->tx_seq) { - l2cap_add_to_srej_queue(sk, skb, tx_seq, sar); - l2cap_check_srej_gap(sk, tx_seq); + clear_bit(CONN_REMOTE_BUSY, &chan->conn_state); - list_del(&first->list); - kfree(first); + chan->rx_state = L2CAP_RX_STATE_RECV; + l2cap_process_reqseq(chan, control->reqseq); - if (list_empty(SREJ_LIST(sk))) { - pi->buffer_seq = pi->buffer_seq_srej; - pi->conn_state &= ~L2CAP_CONN_SREJ_SENT; - l2cap_send_ack(pi); - BT_DBG("sk %p, Exit SREJ_SENT", sk); - } - } else { - struct srej_list *l; + if (!skb_queue_empty(&chan->tx_q)) + chan->tx_send_head = skb_peek(&chan->tx_q); + else + chan->tx_send_head = NULL; - /* duplicated tx_seq */ - if (l2cap_add_to_srej_queue(sk, skb, tx_seq, sar) < 0) - goto drop; + /* Rewind next_tx_seq to the point expected + * by the receiver. + */ + chan->next_tx_seq = control->reqseq; + chan->unacked_frames = 0; - list_for_each_entry(l, SREJ_LIST(sk), list) { - if (l->tx_seq == tx_seq) { - l2cap_resend_srejframe(sk, tx_seq); - return 0; - } - } - 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; + if (chan->hs_hcon) + chan->conn->mtu = chan->hs_hcon->hdev->block_mtu; + else + chan->conn->mtu = chan->conn->hcon->hdev->acl_mtu; - /* duplicated tx_seq */ - if (tx_seq_offset < expected_tx_seq_offset) - goto drop; + err = l2cap_resegment(chan); - pi->conn_state |= L2CAP_CONN_SREJ_SENT; + if (!err) + err = l2cap_rx_state_recv(chan, control, skb, event); - BT_DBG("sk %p, Enter SREJ", sk); + return err; +} - INIT_LIST_HEAD(SREJ_LIST(sk)); - pi->buffer_seq_srej = pi->buffer_seq; +static bool __valid_reqseq(struct l2cap_chan *chan, u16 reqseq) +{ + /* Make sure reqseq is for a packet that has been sent but not acked */ + u16 unacked; - __skb_queue_head_init(SREJ_QUEUE(sk)); - __skb_queue_head_init(BUSY_QUEUE(sk)); - l2cap_add_to_srej_queue(sk, skb, tx_seq, sar); + unacked = __seq_offset(chan, chan->next_tx_seq, chan->expected_ack_seq); + return __seq_offset(chan, chan->next_tx_seq, reqseq) <= unacked; +} - pi->conn_state |= L2CAP_CONN_SEND_PBIT; +static int l2cap_rx(struct l2cap_chan *chan, struct l2cap_ctrl *control, + struct sk_buff *skb, u8 event) +{ + int err = 0; - l2cap_send_srejframe(sk, tx_seq); + BT_DBG("chan %p, control %p, skb %p, event %d, state %d", chan, + control, skb, event, chan->rx_state); - del_timer(&pi->ack_timer); + if (__valid_reqseq(chan, control->reqseq)) { + switch (chan->rx_state) { + case L2CAP_RX_STATE_RECV: + err = l2cap_rx_state_recv(chan, control, skb, event); + break; + case L2CAP_RX_STATE_SREJ_SENT: + err = l2cap_rx_state_srej_sent(chan, control, skb, + event); + break; + case L2CAP_RX_STATE_WAIT_P: + err = l2cap_rx_state_wait_p(chan, control, skb, event); + break; + case L2CAP_RX_STATE_WAIT_F: + err = l2cap_rx_state_wait_f(chan, control, skb, event); + break; + default: + /* shut it down */ + break; + } + } else { + BT_DBG("Invalid reqseq %d (next_tx_seq %d, expected_ack_seq %d", + control->reqseq, chan->next_tx_seq, + chan->expected_ack_seq); + l2cap_send_disconn_req(chan, ECONNRESET); } - return 0; -expected: - pi->expected_tx_seq = (pi->expected_tx_seq + 1) % 64; + return err; +} - if (pi->conn_state & L2CAP_CONN_SREJ_SENT) { - bt_cb(skb)->tx_seq = tx_seq; - bt_cb(skb)->sar = sar; - __skb_queue_tail(SREJ_QUEUE(sk), skb); - return 0; - } +static int l2cap_stream_rx(struct l2cap_chan *chan, struct l2cap_ctrl *control, + struct sk_buff *skb) +{ + int err = 0; - err = l2cap_push_rx_skb(sk, skb, rx_control); - if (err < 0) - return 0; + BT_DBG("chan %p, control %p, skb %p, state %d", chan, control, skb, + chan->rx_state); - if (rx_control & L2CAP_CTRL_FINAL) { - if (pi->conn_state & L2CAP_CONN_REJ_ACT) - pi->conn_state &= ~L2CAP_CONN_REJ_ACT; - else - l2cap_retransmit_frames(sk); - } + if (l2cap_classify_txseq(chan, control->txseq) == + L2CAP_TXSEQ_EXPECTED) { + l2cap_pass_to_tx(chan, control); - __mod_ack_timer(); + BT_DBG("buffer_seq %d->%d", chan->buffer_seq, + __next_seq(chan, chan->buffer_seq)); - pi->num_acked = (pi->num_acked + 1) % num_to_ack; - if (pi->num_acked == num_to_ack - 1) - l2cap_send_ack(pi); + chan->buffer_seq = __next_seq(chan, chan->buffer_seq); - return 0; + l2cap_reassemble_sdu(chan, skb, control); + } else { + if (chan->sdu) { + kfree_skb(chan->sdu); + chan->sdu = NULL; + } + chan->sdu_last_frag = NULL; + chan->sdu_len = 0; -drop: - kfree_skb(skb); - return 0; -} + if (skb) { + BT_DBG("Freeing %p", skb); + kfree_skb(skb); + } + } -static inline void l2cap_data_channel_rrframe(struct sock *sk, u16 rx_control) -{ - struct l2cap_pinfo *pi = l2cap_pi(sk); + chan->last_acked_seq = control->txseq; + chan->expected_tx_seq = __next_seq(chan, control->txseq); - BT_DBG("sk %p, req_seq %d ctrl 0x%4.4x", sk, __get_reqseq(rx_control), - rx_control); + return err; +} - pi->expected_ack_seq = __get_reqseq(rx_control); - l2cap_drop_acked_frames(sk); +static int l2cap_data_rcv(struct l2cap_chan *chan, struct sk_buff *skb) +{ + struct l2cap_ctrl *control = &bt_cb(skb)->control; + u16 len; + u8 event; - if (rx_control & L2CAP_CTRL_POLL) { - pi->conn_state |= L2CAP_CONN_SEND_FBIT; - if (pi->conn_state & L2CAP_CONN_SREJ_SENT) { - if ((pi->conn_state & L2CAP_CONN_REMOTE_BUSY) && - (pi->unacked_frames > 0)) - __mod_retrans_timer(); + __unpack_control(chan, skb); - pi->conn_state &= ~L2CAP_CONN_REMOTE_BUSY; - l2cap_send_srejtail(sk); - } else { - l2cap_send_i_or_rr_or_rnr(sk); - } + len = skb->len; - } else if (rx_control & L2CAP_CTRL_FINAL) { - pi->conn_state &= ~L2CAP_CONN_REMOTE_BUSY; + /* + * We can just drop the corrupted I-frame here. + * Receiver will miss it and start proper recovery + * procedures and ask for retransmission. + */ + if (l2cap_check_fcs(chan, skb)) + goto drop; - if (pi->conn_state & L2CAP_CONN_REJ_ACT) - pi->conn_state &= ~L2CAP_CONN_REJ_ACT; - else - l2cap_retransmit_frames(sk); + if (!control->sframe && control->sar == L2CAP_SAR_START) + len -= L2CAP_SDULEN_SIZE; - } else { - if ((pi->conn_state & L2CAP_CONN_REMOTE_BUSY) && - (pi->unacked_frames > 0)) - __mod_retrans_timer(); + if (chan->fcs == L2CAP_FCS_CRC16) + len -= L2CAP_FCS_SIZE; - pi->conn_state &= ~L2CAP_CONN_REMOTE_BUSY; - if (pi->conn_state & L2CAP_CONN_SREJ_SENT) - l2cap_send_ack(pi); - else - l2cap_ertm_send(sk); + if (len > chan->mps) { + l2cap_send_disconn_req(chan, ECONNRESET); + goto drop; } -} -static inline void l2cap_data_channel_rejframe(struct sock *sk, u16 rx_control) -{ - struct l2cap_pinfo *pi = l2cap_pi(sk); - u8 tx_seq = __get_reqseq(rx_control); + if (!control->sframe) { + int err; - BT_DBG("sk %p, req_seq %d ctrl 0x%4.4x", sk, tx_seq, rx_control); + BT_DBG("iframe sar %d, reqseq %d, final %d, txseq %d", + control->sar, control->reqseq, control->final, + control->txseq); - pi->conn_state &= ~L2CAP_CONN_REMOTE_BUSY; + /* Validate F-bit - F=0 always valid, F=1 only + * valid in TX WAIT_F + */ + if (control->final && chan->tx_state != L2CAP_TX_STATE_WAIT_F) + goto drop; - pi->expected_ack_seq = tx_seq; - l2cap_drop_acked_frames(sk); + if (chan->mode != L2CAP_MODE_STREAMING) { + event = L2CAP_EV_RECV_IFRAME; + err = l2cap_rx(chan, control, skb, event); + } else { + err = l2cap_stream_rx(chan, control, skb); + } - if (rx_control & L2CAP_CTRL_FINAL) { - if (pi->conn_state & L2CAP_CONN_REJ_ACT) - pi->conn_state &= ~L2CAP_CONN_REJ_ACT; - else - l2cap_retransmit_frames(sk); + if (err) + l2cap_send_disconn_req(chan, ECONNRESET); } else { - l2cap_retransmit_frames(sk); + const u8 rx_func_to_event[4] = { + L2CAP_EV_RECV_RR, L2CAP_EV_RECV_REJ, + L2CAP_EV_RECV_RNR, L2CAP_EV_RECV_SREJ + }; - 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) -{ - struct l2cap_pinfo *pi = l2cap_pi(sk); - u8 tx_seq = __get_reqseq(rx_control); + /* Only I-frames are expected in streaming mode */ + if (chan->mode == L2CAP_MODE_STREAMING) + goto drop; - BT_DBG("sk %p, req_seq %d ctrl 0x%4.4x", sk, tx_seq, rx_control); + BT_DBG("sframe reqseq %d, final %d, poll %d, super %d", + control->reqseq, control->final, control->poll, + control->super); - pi->conn_state &= ~L2CAP_CONN_REMOTE_BUSY; + if (len != 0) { + BT_ERR("Trailing bytes: %d in sframe", len); + l2cap_send_disconn_req(chan, ECONNRESET); + goto drop; + } - if (rx_control & L2CAP_CTRL_POLL) { - pi->expected_ack_seq = tx_seq; - l2cap_drop_acked_frames(sk); + /* Validate F and P bits */ + if (control->final && (control->poll || + chan->tx_state != L2CAP_TX_STATE_WAIT_F)) + goto drop; - pi->conn_state |= L2CAP_CONN_SEND_FBIT; - l2cap_retransmit_one_frame(sk, tx_seq); + event = rx_func_to_event[control->super]; + if (l2cap_rx(chan, control, skb, event)) + l2cap_send_disconn_req(chan, ECONNRESET); + } - l2cap_ertm_send(sk); + return 0; - if (pi->conn_state & L2CAP_CONN_WAIT_F) { - pi->srej_save_reqseq = tx_seq; - pi->conn_state |= L2CAP_CONN_SREJ_ACT; - } - } else if (rx_control & L2CAP_CTRL_FINAL) { - if ((pi->conn_state & L2CAP_CONN_SREJ_ACT) && - pi->srej_save_reqseq == tx_seq) - pi->conn_state &= ~L2CAP_CONN_SREJ_ACT; - else - l2cap_retransmit_one_frame(sk, tx_seq); - } else { - 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; - } - } +drop: + kfree_skb(skb); + return 0; } -static inline void l2cap_data_channel_rnrframe(struct sock *sk, u16 rx_control) +static void l2cap_chan_le_send_credits(struct l2cap_chan *chan) { - struct l2cap_pinfo *pi = l2cap_pi(sk); - u8 tx_seq = __get_reqseq(rx_control); + struct l2cap_conn *conn = chan->conn; + struct l2cap_le_credits pkt; + u16 return_credits; - BT_DBG("sk %p, req_seq %d ctrl 0x%4.4x", sk, tx_seq, rx_control); + /* We return more credits to the sender only after the amount of + * credits falls below half of the initial amount. + */ + if (chan->rx_credits >= (le_max_credits + 1) / 2) + return; - pi->conn_state |= L2CAP_CONN_REMOTE_BUSY; - pi->expected_ack_seq = tx_seq; - l2cap_drop_acked_frames(sk); + return_credits = le_max_credits - chan->rx_credits; - if (rx_control & L2CAP_CTRL_POLL) - pi->conn_state |= L2CAP_CONN_SEND_FBIT; + BT_DBG("chan %p returning %u credits to sender", chan, return_credits); - if (!(pi->conn_state & L2CAP_CONN_SREJ_SENT)) { - del_timer(&pi->retrans_timer); - if (rx_control & L2CAP_CTRL_POLL) - l2cap_send_rr_or_rnr(pi, L2CAP_CTRL_FINAL); - return; - } + chan->rx_credits += return_credits; - if (rx_control & L2CAP_CTRL_POLL) - l2cap_send_srejtail(sk); - else - l2cap_send_sframe(pi, L2CAP_SUPER_RCV_READY); + pkt.cid = cpu_to_le16(chan->scid); + pkt.credits = cpu_to_le16(return_credits); + + chan->ident = l2cap_get_ident(conn); + + l2cap_send_cmd(conn, chan->ident, L2CAP_LE_CREDITS, sizeof(pkt), &pkt); } -static inline int l2cap_data_channel_sframe(struct sock *sk, u16 rx_control, struct sk_buff *skb) +static int l2cap_le_data_rcv(struct l2cap_chan *chan, struct sk_buff *skb) { - BT_DBG("sk %p rx_control 0x%4.4x len %d", sk, rx_control, skb->len); + int err; - 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(); - l2cap_pi(sk)->conn_state &= ~L2CAP_CONN_WAIT_F; + if (!chan->rx_credits) { + BT_ERR("No credits to receive LE L2CAP data"); + l2cap_send_disconn_req(chan, ECONNRESET); + return -ENOBUFS; } - switch (rx_control & L2CAP_CTRL_SUPERVISE) { - case L2CAP_SUPER_RCV_READY: - l2cap_data_channel_rrframe(sk, rx_control); - break; + if (chan->imtu < skb->len) { + BT_ERR("Too big LE L2CAP PDU"); + return -ENOBUFS; + } - case L2CAP_SUPER_REJECT: - l2cap_data_channel_rejframe(sk, rx_control); - break; + chan->rx_credits--; + BT_DBG("rx_credits %u -> %u", chan->rx_credits + 1, chan->rx_credits); - case L2CAP_SUPER_SELECT_REJECT: - l2cap_data_channel_srejframe(sk, rx_control); - break; + l2cap_chan_le_send_credits(chan); - case L2CAP_SUPER_RCV_NOT_READY: - l2cap_data_channel_rnrframe(sk, rx_control); - break; - } + err = 0; - kfree_skb(skb); - return 0; -} + if (!chan->sdu) { + u16 sdu_len; -static int l2cap_ertm_data_rcv(struct sock *sk, struct sk_buff *skb) -{ - struct l2cap_pinfo *pi = l2cap_pi(sk); - u16 control; - u8 req_seq; - int len, next_tx_seq_offset, req_seq_offset; + sdu_len = get_unaligned_le16(skb->data); + skb_pull(skb, L2CAP_SDULEN_SIZE); - control = get_unaligned_le16(skb->data); - skb_pull(skb, 2); - len = skb->len; + BT_DBG("Start of new SDU. sdu_len %u skb->len %u imtu %u", + sdu_len, skb->len, chan->imtu); - /* - * We can just drop the corrupted I-frame here. - * Receiver will miss it and start proper recovery - * procedures and ask retransmission. - */ - if (l2cap_check_fcs(pi, skb)) - goto drop; + if (sdu_len > chan->imtu) { + BT_ERR("Too big LE L2CAP SDU length received"); + err = -EMSGSIZE; + goto failed; + } - if (__is_sar_start(control) && __is_iframe(control)) - len -= 2; + if (skb->len > sdu_len) { + BT_ERR("Too much LE L2CAP data received"); + err = -EINVAL; + goto failed; + } - if (pi->fcs == L2CAP_FCS_CRC16) - len -= 2; + if (skb->len == sdu_len) + return chan->ops->recv(chan, skb); - if (len > pi->mps) { - l2cap_send_disconn_req(pi->conn, sk, ECONNRESET); - goto drop; - } + chan->sdu = skb; + chan->sdu_len = sdu_len; + chan->sdu_last_frag = skb; - req_seq = __get_reqseq(control); - req_seq_offset = (req_seq - pi->expected_ack_seq) % 64; - if (req_seq_offset < 0) - req_seq_offset += 64; + return 0; + } - next_tx_seq_offset = - (pi->next_tx_seq - pi->expected_ack_seq) % 64; - if (next_tx_seq_offset < 0) - next_tx_seq_offset += 64; + BT_DBG("SDU fragment. chan->sdu->len %u skb->len %u chan->sdu_len %u", + chan->sdu->len, skb->len, chan->sdu_len); - /* check for invalid req-seq */ - if (req_seq_offset > next_tx_seq_offset) { - l2cap_send_disconn_req(pi->conn, sk, ECONNRESET); - goto drop; + if (chan->sdu->len + skb->len > chan->sdu_len) { + BT_ERR("Too much LE L2CAP data received"); + err = -EINVAL; + goto failed; } - if (__is_iframe(control)) { - if (len < 0) { - l2cap_send_disconn_req(pi->conn, sk, ECONNRESET); - goto drop; - } + append_skb_frag(chan->sdu, skb, &chan->sdu_last_frag); + skb = NULL; - l2cap_data_channel_iframe(sk, control, skb); - } else { - if (len != 0) { - BT_ERR("%d", len); - l2cap_send_disconn_req(pi->conn, sk, ECONNRESET); - goto drop; + if (chan->sdu->len == chan->sdu_len) { + err = chan->ops->recv(chan, chan->sdu); + if (!err) { + chan->sdu = NULL; + chan->sdu_last_frag = NULL; + chan->sdu_len = 0; } - - l2cap_data_channel_sframe(sk, control, skb); } - return 0; +failed: + if (err) { + kfree_skb(skb); + kfree_skb(chan->sdu); + chan->sdu = NULL; + chan->sdu_last_frag = NULL; + chan->sdu_len = 0; + } -drop: - kfree_skb(skb); + /* We can't return an error here since we took care of the skb + * freeing internally. An error return would cause the caller to + * do a double-free of the skb. + */ return 0; } -static inline int l2cap_data_channel(struct l2cap_conn *conn, u16 cid, struct sk_buff *skb) +static void l2cap_data_channel(struct l2cap_conn *conn, u16 cid, + struct sk_buff *skb) { - struct sock *sk; - struct l2cap_pinfo *pi; - u16 control; - u8 tx_seq; - int len; + struct l2cap_chan *chan; + + chan = l2cap_get_chan_by_scid(conn, cid); + if (!chan) { + if (cid == L2CAP_CID_A2MP) { + chan = a2mp_channel_create(conn, skb); + if (!chan) { + kfree_skb(skb); + return; + } - sk = l2cap_get_chan_by_scid(&conn->chan_list, cid); - if (!sk) { - BT_DBG("unknown cid 0x%4.4x", cid); - goto drop; + l2cap_chan_lock(chan); + } else { + BT_DBG("unknown cid 0x%4.4x", cid); + /* Drop packet and return */ + kfree_skb(skb); + return; + } } - pi = l2cap_pi(sk); + BT_DBG("chan %p, len %d", chan, skb->len); - BT_DBG("sk %p, len %d", sk, skb->len); - - if (sk->sk_state != BT_CONNECTED) + if (chan->state != BT_CONNECTED) goto drop; - switch (pi->mode) { + switch (chan->mode) { + case L2CAP_MODE_LE_FLOWCTL: + if (l2cap_le_data_rcv(chan, skb) < 0) + goto drop; + + goto done; + 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 (pi->imtu < skb->len) + if (chan->imtu < skb->len) { + BT_ERR("Dropping L2CAP data: receive buffer overflow"); goto drop; + } - if (!sock_queue_rcv_skb(sk, skb)) + if (!chan->ops->recv(chan, skb)) goto done; break; case L2CAP_MODE_ERTM: - if (!sock_owned_by_user(sk)) { - l2cap_ertm_data_rcv(sk, skb); - } else { - if (sk_add_backlog(sk, skb)) - goto drop; - } - + case L2CAP_MODE_STREAMING: + l2cap_data_rcv(chan, skb); goto done; - case L2CAP_MODE_STREAMING: - control = get_unaligned_le16(skb->data); - skb_pull(skb, 2); - len = skb->len; + default: + BT_DBG("chan %p: bad mode 0x%2.2x", chan, chan->mode); + break; + } - if (l2cap_check_fcs(pi, skb)) - goto drop; +drop: + kfree_skb(skb); - if (__is_sar_start(control)) - len -= 2; +done: + l2cap_chan_unlock(chan); +} - if (pi->fcs == L2CAP_FCS_CRC16) - len -= 2; +static void l2cap_conless_channel(struct l2cap_conn *conn, __le16 psm, + struct sk_buff *skb) +{ + struct hci_conn *hcon = conn->hcon; + struct l2cap_chan *chan; - if (len > pi->mps || len < 0 || __is_sframe(control)) - goto drop; + if (hcon->type != ACL_LINK) + goto drop; - tx_seq = __get_txseq(control); + chan = l2cap_global_chan_by_psm(0, psm, &hcon->src, &hcon->dst, + ACL_LINK); + if (!chan) + goto drop; - if (pi->expected_tx_seq == tx_seq) - pi->expected_tx_seq = (pi->expected_tx_seq + 1) % 64; - else - pi->expected_tx_seq = (tx_seq + 1) % 64; + BT_DBG("chan %p, len %d", chan, skb->len); - l2cap_streaming_reassembly_sdu(sk, skb, control); + if (chan->state != BT_BOUND && chan->state != BT_CONNECTED) + goto drop; - goto done; + if (chan->imtu < skb->len) + goto drop; - default: - BT_DBG("sk %p: bad mode 0x%2.2x", sk, pi->mode); - break; - } + /* Store remote BD_ADDR and PSM for msg_name */ + bacpy(&bt_cb(skb)->bdaddr, &hcon->dst); + bt_cb(skb)->psm = psm; + + if (!chan->ops->recv(chan, skb)) + return; drop: kfree_skb(skb); - -done: - if (sk) - bh_unlock_sock(sk); - - return 0; } -static inline int l2cap_conless_channel(struct l2cap_conn *conn, __le16 psm, struct sk_buff *skb) +static void l2cap_att_channel(struct l2cap_conn *conn, + struct sk_buff *skb) { - struct sock *sk; + struct hci_conn *hcon = conn->hcon; + struct l2cap_chan *chan; - sk = l2cap_get_sock_by_psm(0, psm, conn->src); - if (!sk) + if (hcon->type != LE_LINK) goto drop; - bh_lock_sock(sk); + chan = l2cap_global_chan_by_scid(BT_CONNECTED, L2CAP_CID_ATT, + &hcon->src, &hcon->dst); + if (!chan) + goto drop; - BT_DBG("sk %p, len %d", sk, skb->len); + BT_DBG("chan %p, len %d", chan, skb->len); - if (sk->sk_state != BT_BOUND && sk->sk_state != BT_CONNECTED) + if (hci_blacklist_lookup(hcon->hdev, &hcon->dst, hcon->dst_type)) goto drop; - if (l2cap_pi(sk)->imtu < skb->len) + if (chan->imtu < skb->len) goto drop; - if (!sock_queue_rcv_skb(sk, skb)) - goto done; + if (!chan->ops->recv(chan, skb)) + return; drop: kfree_skb(skb); - -done: - if (sk) - bh_unlock_sock(sk); - return 0; } static void l2cap_recv_frame(struct l2cap_conn *conn, struct sk_buff *skb) { struct l2cap_hdr *lh = (void *) skb->data; + struct hci_conn *hcon = conn->hcon; u16 cid, len; __le16 psm; + if (hcon->state != BT_CONNECTED) { + BT_DBG("queueing pending rx skb"); + skb_queue_tail(&conn->pending_rx, skb); + return; + } + skb_pull(skb, L2CAP_HDR_SIZE); cid = __le16_to_cpu(lh->cid); len = __le16_to_cpu(lh->len); @@ -3665,210 +6917,490 @@ static void l2cap_recv_frame(struct l2cap_conn *conn, struct sk_buff *skb) BT_DBG("len %d, cid 0x%4.4x", len, cid); switch (cid) { - case L2CAP_CID_LE_SIGNALING: case L2CAP_CID_SIGNALING: l2cap_sig_channel(conn, skb); break; case L2CAP_CID_CONN_LESS: - psm = get_unaligned_le16(skb->data); - skb_pull(skb, 2); + psm = get_unaligned((__le16 *) skb->data); + skb_pull(skb, L2CAP_PSMLEN_SIZE); l2cap_conless_channel(conn, psm, skb); break; + case L2CAP_CID_ATT: + l2cap_att_channel(conn, skb); + break; + + case L2CAP_CID_LE_SIGNALING: + l2cap_le_sig_channel(conn, skb); + break; + + case L2CAP_CID_SMP: + if (smp_sig_channel(conn, skb)) + l2cap_conn_del(conn->hcon, EACCES); + break; + + case L2CAP_FC_6LOWPAN: + bt_6lowpan_recv(conn, skb); + break; + default: l2cap_data_channel(conn, cid, skb); break; } } +static void process_pending_rx(struct work_struct *work) +{ + struct l2cap_conn *conn = container_of(work, struct l2cap_conn, + pending_rx_work); + struct sk_buff *skb; + + BT_DBG(""); + + while ((skb = skb_dequeue(&conn->pending_rx))) + l2cap_recv_frame(conn, skb); +} + +static struct l2cap_conn *l2cap_conn_add(struct hci_conn *hcon) +{ + struct l2cap_conn *conn = hcon->l2cap_data; + struct hci_chan *hchan; + + if (conn) + return conn; + + hchan = hci_chan_create(hcon); + if (!hchan) + return NULL; + + conn = kzalloc(sizeof(struct l2cap_conn), GFP_KERNEL); + if (!conn) { + hci_chan_del(hchan); + return NULL; + } + + kref_init(&conn->ref); + hcon->l2cap_data = conn; + conn->hcon = hcon; + hci_conn_get(conn->hcon); + conn->hchan = hchan; + + BT_DBG("hcon %p conn %p hchan %p", hcon, conn, hchan); + + switch (hcon->type) { + case LE_LINK: + if (hcon->hdev->le_mtu) { + conn->mtu = hcon->hdev->le_mtu; + break; + } + /* fall through */ + default: + conn->mtu = hcon->hdev->acl_mtu; + break; + } + + conn->feat_mask = 0; + + if (hcon->type == ACL_LINK) + conn->hs_enabled = test_bit(HCI_HS_ENABLED, + &hcon->hdev->dev_flags); + + spin_lock_init(&conn->lock); + mutex_init(&conn->chan_lock); + + INIT_LIST_HEAD(&conn->chan_l); + INIT_LIST_HEAD(&conn->users); + + if (hcon->type == LE_LINK) + INIT_DELAYED_WORK(&conn->security_timer, security_timeout); + else + INIT_DELAYED_WORK(&conn->info_timer, l2cap_info_timeout); + + skb_queue_head_init(&conn->pending_rx); + INIT_WORK(&conn->pending_rx_work, process_pending_rx); + + conn->disc_reason = HCI_ERROR_REMOTE_USER_TERM; + + return conn; +} + +static bool is_valid_psm(u16 psm, u8 dst_type) { + if (!psm) + return false; + + if (bdaddr_type_is_le(dst_type)) + return (psm <= 0x00ff); + + /* PSM must be odd and lsb of upper byte must be 0 */ + return ((psm & 0x0101) == 0x0001); +} + +int l2cap_chan_connect(struct l2cap_chan *chan, __le16 psm, u16 cid, + bdaddr_t *dst, u8 dst_type) +{ + struct l2cap_conn *conn; + struct hci_conn *hcon; + struct hci_dev *hdev; + __u8 auth_type; + int err; + + BT_DBG("%pMR -> %pMR (type %u) psm 0x%2.2x", &chan->src, dst, + dst_type, __le16_to_cpu(psm)); + + hdev = hci_get_route(dst, &chan->src); + if (!hdev) + return -EHOSTUNREACH; + + hci_dev_lock(hdev); + + l2cap_chan_lock(chan); + + if (!is_valid_psm(__le16_to_cpu(psm), dst_type) && !cid && + chan->chan_type != L2CAP_CHAN_RAW) { + err = -EINVAL; + goto done; + } + + if (chan->chan_type == L2CAP_CHAN_CONN_ORIENTED && !psm) { + err = -EINVAL; + goto done; + } + + if (chan->chan_type == L2CAP_CHAN_FIXED && !cid) { + err = -EINVAL; + goto done; + } + + switch (chan->mode) { + case L2CAP_MODE_BASIC: + break; + case L2CAP_MODE_LE_FLOWCTL: + l2cap_le_flowctl_init(chan); + break; + case L2CAP_MODE_ERTM: + case L2CAP_MODE_STREAMING: + if (!disable_ertm) + break; + /* fall through */ + default: + err = -ENOTSUPP; + goto done; + } + + switch (chan->state) { + case BT_CONNECT: + case BT_CONNECT2: + case BT_CONFIG: + /* Already connecting */ + err = 0; + goto done; + + case BT_CONNECTED: + /* Already connected */ + err = -EISCONN; + goto done; + + case BT_OPEN: + case BT_BOUND: + /* Can connect */ + break; + + default: + err = -EBADFD; + goto done; + } + + /* Set destination address and psm */ + bacpy(&chan->dst, dst); + chan->dst_type = dst_type; + + chan->psm = psm; + chan->dcid = cid; + + auth_type = l2cap_get_auth_type(chan); + + if (bdaddr_type_is_le(dst_type)) { + /* Convert from L2CAP channel address type to HCI address type + */ + if (dst_type == BDADDR_LE_PUBLIC) + dst_type = ADDR_LE_DEV_PUBLIC; + else + dst_type = ADDR_LE_DEV_RANDOM; + + hcon = hci_connect_le(hdev, dst, dst_type, chan->sec_level, + auth_type); + } else { + hcon = hci_connect_acl(hdev, dst, chan->sec_level, auth_type); + } + + if (IS_ERR(hcon)) { + err = PTR_ERR(hcon); + goto done; + } + + conn = l2cap_conn_add(hcon); + if (!conn) { + hci_conn_drop(hcon); + err = -ENOMEM; + goto done; + } + + if (cid && __l2cap_get_chan_by_dcid(conn, cid)) { + hci_conn_drop(hcon); + err = -EBUSY; + goto done; + } + + /* Update source addr of the socket */ + bacpy(&chan->src, &hcon->src); + chan->src_type = bdaddr_type(hcon, hcon->src_type); + + l2cap_chan_unlock(chan); + l2cap_chan_add(conn, chan); + l2cap_chan_lock(chan); + + /* l2cap_chan_add takes its own ref so we can drop this one */ + hci_conn_drop(hcon); + + l2cap_state_change(chan, BT_CONNECT); + __set_chan_timer(chan, chan->ops->get_sndtimeo(chan)); + + /* Release chan->sport so that it can be reused by other + * sockets (as it's only used for listening sockets). + */ + write_lock(&chan_list_lock); + chan->sport = 0; + write_unlock(&chan_list_lock); + + if (hcon->state == BT_CONNECTED) { + if (chan->chan_type != L2CAP_CHAN_CONN_ORIENTED) { + __clear_chan_timer(chan); + if (l2cap_chan_check_security(chan)) + l2cap_state_change(chan, BT_CONNECTED); + } else + l2cap_do_start(chan); + } + + err = 0; + +done: + l2cap_chan_unlock(chan); + hci_dev_unlock(hdev); + hci_dev_put(hdev); + return err; +} + /* ---- L2CAP interface with lower layer (HCI) ---- */ -static int l2cap_connect_ind(struct hci_dev *hdev, bdaddr_t *bdaddr, u8 type) +int l2cap_connect_ind(struct hci_dev *hdev, bdaddr_t *bdaddr) { int exact = 0, lm1 = 0, lm2 = 0; - register struct sock *sk; - struct hlist_node *node; - - if (type != ACL_LINK) - return -EINVAL; + struct l2cap_chan *c; - BT_DBG("hdev %s, bdaddr %s", hdev->name, batostr(bdaddr)); + BT_DBG("hdev %s, bdaddr %pMR", hdev->name, bdaddr); /* Find listening sockets and check their link_mode */ - read_lock(&l2cap_sk_list.lock); - sk_for_each(sk, node, &l2cap_sk_list.head) { - if (sk->sk_state != BT_LISTEN) + read_lock(&chan_list_lock); + list_for_each_entry(c, &chan_list, global_l) { + if (c->state != BT_LISTEN) continue; - if (!bacmp(&bt_sk(sk)->src, &hdev->bdaddr)) { + if (!bacmp(&c->src, &hdev->bdaddr)) { lm1 |= HCI_LM_ACCEPT; - if (l2cap_pi(sk)->role_switch) + if (test_bit(FLAG_ROLE_SWITCH, &c->flags)) lm1 |= HCI_LM_MASTER; exact++; - } else if (!bacmp(&bt_sk(sk)->src, BDADDR_ANY)) { + } else if (!bacmp(&c->src, BDADDR_ANY)) { lm2 |= HCI_LM_ACCEPT; - if (l2cap_pi(sk)->role_switch) + if (test_bit(FLAG_ROLE_SWITCH, &c->flags)) lm2 |= HCI_LM_MASTER; } } - read_unlock(&l2cap_sk_list.lock); + read_unlock(&chan_list_lock); return exact ? lm1 : lm2; } -static int l2cap_connect_cfm(struct hci_conn *hcon, u8 status) +void l2cap_connect_cfm(struct hci_conn *hcon, u8 status) { struct l2cap_conn *conn; - BT_DBG("hcon %p bdaddr %s status %d", hcon, batostr(&hcon->dst), status); - - if (!(hcon->type == ACL_LINK || hcon->type == LE_LINK)) - return -EINVAL; + BT_DBG("hcon %p bdaddr %pMR status %d", hcon, &hcon->dst, status); if (!status) { - conn = l2cap_conn_add(hcon, status); + conn = l2cap_conn_add(hcon); if (conn) l2cap_conn_ready(conn); - } else - l2cap_conn_del(hcon, bt_err(status)); - - return 0; + } else { + l2cap_conn_del(hcon, bt_to_errno(status)); + } } -static int l2cap_disconn_ind(struct hci_conn *hcon) +int l2cap_disconn_ind(struct hci_conn *hcon) { struct l2cap_conn *conn = hcon->l2cap_data; BT_DBG("hcon %p", hcon); - if (hcon->type != ACL_LINK || !conn) - return 0x13; - + if (!conn) + return HCI_ERROR_REMOTE_USER_TERM; return conn->disc_reason; } -static int l2cap_disconn_cfm(struct hci_conn *hcon, u8 reason) +void l2cap_disconn_cfm(struct hci_conn *hcon, u8 reason) { BT_DBG("hcon %p reason %d", hcon, reason); - if (!(hcon->type == ACL_LINK || hcon->type == LE_LINK)) - return -EINVAL; + bt_6lowpan_del_conn(hcon->l2cap_data); - l2cap_conn_del(hcon, bt_err(reason)); - - return 0; + l2cap_conn_del(hcon, bt_to_errno(reason)); } -static inline void l2cap_check_encryption(struct sock *sk, u8 encrypt) +static inline void l2cap_check_encryption(struct l2cap_chan *chan, u8 encrypt) { - if (sk->sk_type != SOCK_SEQPACKET && sk->sk_type != SOCK_STREAM) + if (chan->chan_type != L2CAP_CHAN_CONN_ORIENTED) return; if (encrypt == 0x00) { - if (l2cap_pi(sk)->sec_level == BT_SECURITY_MEDIUM) { - l2cap_sock_clear_timer(sk); - l2cap_sock_set_timer(sk, HZ * 5); - } else if (l2cap_pi(sk)->sec_level == BT_SECURITY_HIGH) - __l2cap_sock_close(sk, ECONNREFUSED); + if (chan->sec_level == BT_SECURITY_MEDIUM) { + __set_chan_timer(chan, L2CAP_ENC_TIMEOUT); + } else if (chan->sec_level == BT_SECURITY_HIGH || + chan->sec_level == BT_SECURITY_FIPS) + l2cap_chan_close(chan, ECONNREFUSED); } else { - if (l2cap_pi(sk)->sec_level == BT_SECURITY_MEDIUM) - l2cap_sock_clear_timer(sk); + if (chan->sec_level == BT_SECURITY_MEDIUM) + __clear_chan_timer(chan); } } -static int l2cap_security_cfm(struct hci_conn *hcon, u8 status, u8 encrypt) +int l2cap_security_cfm(struct hci_conn *hcon, u8 status, u8 encrypt) { - struct l2cap_chan_list *l; struct l2cap_conn *conn = hcon->l2cap_data; - struct sock *sk; + struct l2cap_chan *chan; if (!conn) return 0; - l = &conn->chan_list; + BT_DBG("conn %p status 0x%2.2x encrypt %u", conn, status, encrypt); - BT_DBG("conn %p", conn); + if (hcon->type == LE_LINK) { + if (!status && encrypt) + smp_distribute_keys(conn); + cancel_delayed_work(&conn->security_timer); + } + + mutex_lock(&conn->chan_lock); - read_lock(&l->lock); + list_for_each_entry(chan, &conn->chan_l, list) { + l2cap_chan_lock(chan); - for (sk = l->head; sk; sk = l2cap_pi(sk)->next_c) { - bh_lock_sock(sk); + BT_DBG("chan %p scid 0x%4.4x state %s", chan, chan->scid, + state_to_string(chan->state)); - if (l2cap_pi(sk)->conf_state & L2CAP_CONF_CONNECT_PEND) { - bh_unlock_sock(sk); + if (chan->scid == L2CAP_CID_A2MP) { + l2cap_chan_unlock(chan); continue; } - if (!status && (sk->sk_state == BT_CONNECTED || - sk->sk_state == BT_CONFIG)) { - l2cap_check_encryption(sk, encrypt); - bh_unlock_sock(sk); + if (chan->scid == L2CAP_CID_ATT) { + if (!status && encrypt) { + chan->sec_level = hcon->sec_level; + l2cap_chan_ready(chan); + } + + l2cap_chan_unlock(chan); continue; } - if (sk->sk_state == BT_CONNECT) { - if (!status) { - struct l2cap_conn_req req; - req.scid = cpu_to_le16(l2cap_pi(sk)->scid); - req.psm = l2cap_pi(sk)->psm; + if (!__l2cap_no_conn_pending(chan)) { + l2cap_chan_unlock(chan); + continue; + } - l2cap_pi(sk)->ident = l2cap_get_ident(conn); - l2cap_pi(sk)->conf_state |= L2CAP_CONF_CONNECT_PEND; + if (!status && (chan->state == BT_CONNECTED || + chan->state == BT_CONFIG)) { + chan->ops->resume(chan); + l2cap_check_encryption(chan, encrypt); + l2cap_chan_unlock(chan); + continue; + } - l2cap_send_cmd(conn, l2cap_pi(sk)->ident, - L2CAP_CONN_REQ, sizeof(req), &req); - } else { - l2cap_sock_clear_timer(sk); - l2cap_sock_set_timer(sk, HZ / 10); - } - } else if (sk->sk_state == BT_CONNECT2) { + if (chan->state == BT_CONNECT) { + if (!status) + l2cap_start_connection(chan); + else + __set_chan_timer(chan, L2CAP_DISC_TIMEOUT); + } else if (chan->state == BT_CONNECT2) { struct l2cap_conn_rsp rsp; - __u16 result; + __u16 res, stat; if (!status) { - sk->sk_state = BT_CONFIG; - result = L2CAP_CR_SUCCESS; + if (test_bit(FLAG_DEFER_SETUP, &chan->flags)) { + res = L2CAP_CR_PEND; + stat = L2CAP_CS_AUTHOR_PEND; + chan->ops->defer(chan); + } else { + l2cap_state_change(chan, BT_CONFIG); + res = L2CAP_CR_SUCCESS; + stat = L2CAP_CS_NO_INFO; + } } else { - sk->sk_state = BT_DISCONN; - l2cap_sock_set_timer(sk, HZ / 10); - result = L2CAP_CR_SEC_BLOCK; + l2cap_state_change(chan, BT_DISCONN); + __set_chan_timer(chan, L2CAP_DISC_TIMEOUT); + res = L2CAP_CR_SEC_BLOCK; + stat = L2CAP_CS_NO_INFO; } - rsp.scid = cpu_to_le16(l2cap_pi(sk)->dcid); - rsp.dcid = cpu_to_le16(l2cap_pi(sk)->scid); - rsp.result = cpu_to_le16(result); - rsp.status = cpu_to_le16(L2CAP_CS_NO_INFO); - l2cap_send_cmd(conn, l2cap_pi(sk)->ident, - L2CAP_CONN_RSP, sizeof(rsp), &rsp); + rsp.scid = cpu_to_le16(chan->dcid); + rsp.dcid = cpu_to_le16(chan->scid); + rsp.result = cpu_to_le16(res); + rsp.status = cpu_to_le16(stat); + l2cap_send_cmd(conn, chan->ident, L2CAP_CONN_RSP, + sizeof(rsp), &rsp); + + if (!test_bit(CONF_REQ_SENT, &chan->conf_state) && + res == L2CAP_CR_SUCCESS) { + char buf[128]; + set_bit(CONF_REQ_SENT, &chan->conf_state); + l2cap_send_cmd(conn, l2cap_get_ident(conn), + L2CAP_CONF_REQ, + l2cap_build_conf_req(chan, buf), + buf); + chan->num_conf_req++; + } } - bh_unlock_sock(sk); + l2cap_chan_unlock(chan); } - read_unlock(&l->lock); + mutex_unlock(&conn->chan_lock); return 0; } -static int l2cap_recv_acldata(struct hci_conn *hcon, struct sk_buff *skb, u16 flags) +int l2cap_recv_acldata(struct hci_conn *hcon, struct sk_buff *skb, u16 flags) { struct l2cap_conn *conn = hcon->l2cap_data; + struct l2cap_hdr *hdr; + int len; + + /* For AMP controller do not create l2cap conn */ + if (!conn && hcon->hdev->dev_type != HCI_BREDR) + goto drop; if (!conn) - conn = l2cap_conn_add(hcon, 0); + conn = l2cap_conn_add(hcon); if (!conn) goto drop; BT_DBG("conn %p len %d flags 0x%x", conn, skb->len, flags); - if (!(flags & ACL_CONT)) { - struct l2cap_hdr *hdr; - struct sock *sk; - u16 cid; - int len; - + switch (flags) { + case ACL_START: + case ACL_START_NO_FLUSH: + case ACL_COMPLETE: if (conn->rx_len) { BT_ERR("Unexpected start frame (len %d)", skb->len); kfree_skb(conn->rx_skb); @@ -3886,7 +7418,6 @@ static int l2cap_recv_acldata(struct hci_conn *hcon, struct sk_buff *skb, u16 fl hdr = (struct l2cap_hdr *) skb->data; len = __le16_to_cpu(hdr->len) + L2CAP_HDR_SIZE; - cid = __le16_to_cpu(hdr->cid); if (len == skb->len) { /* Complete frame received */ @@ -3898,33 +7429,22 @@ static int l2cap_recv_acldata(struct hci_conn *hcon, struct sk_buff *skb, u16 fl if (skb->len > len) { BT_ERR("Frame is too long (len %d, expected len %d)", - skb->len, len); + skb->len, len); l2cap_conn_unreliable(conn, ECOMM); goto drop; } - sk = l2cap_get_chan_by_scid(&conn->chan_list, cid); - - if (sk && l2cap_pi(sk)->imtu < len - L2CAP_HDR_SIZE) { - BT_ERR("Frame exceeding recv MTU (len %d, MTU %d)", - len, l2cap_pi(sk)->imtu); - bh_unlock_sock(sk); - l2cap_conn_unreliable(conn, ECOMM); - goto drop; - } - - if (sk) - bh_unlock_sock(sk); - /* Allocate skb for the complete frame (with header) */ - conn->rx_skb = bt_skb_alloc(len, GFP_ATOMIC); + conn->rx_skb = bt_skb_alloc(len, GFP_KERNEL); if (!conn->rx_skb) goto drop; skb_copy_from_linear_data(skb, skb_put(conn->rx_skb, skb->len), - skb->len); + skb->len); conn->rx_len = len - skb->len; - } else { + break; + + case ACL_CONT: BT_DBG("Cont: frag len %d (expecting %d)", skb->len, conn->rx_len); if (!conn->rx_len) { @@ -3935,7 +7455,7 @@ static int l2cap_recv_acldata(struct hci_conn *hcon, struct sk_buff *skb, u16 fl if (skb->len > conn->rx_len) { BT_ERR("Fragment is too long (len %d, expected %d)", - skb->len, conn->rx_len); + skb->len, conn->rx_len); kfree_skb(conn->rx_skb); conn->rx_skb = NULL; conn->rx_len = 0; @@ -3944,14 +7464,19 @@ static int l2cap_recv_acldata(struct hci_conn *hcon, struct sk_buff *skb, u16 fl } skb_copy_from_linear_data(skb, skb_put(conn->rx_skb, skb->len), - skb->len); + skb->len); conn->rx_len -= skb->len; if (!conn->rx_len) { - /* Complete frame received */ - l2cap_recv_frame(conn, conn->rx_skb); + /* Complete frame received. l2cap_recv_frame + * takes ownership of the skb so set the global + * rx_skb pointer to NULL first. + */ + struct sk_buff *rx_skb = conn->rx_skb; conn->rx_skb = NULL; + l2cap_recv_frame(conn, rx_skb); } + break; } drop: @@ -3961,24 +7486,19 @@ drop: static int l2cap_debugfs_show(struct seq_file *f, void *p) { - struct sock *sk; - struct hlist_node *node; - - read_lock_bh(&l2cap_sk_list.lock); + struct l2cap_chan *c; - sk_for_each(sk, node, &l2cap_sk_list.head) { - struct l2cap_pinfo *pi = l2cap_pi(sk); + read_lock(&chan_list_lock); - seq_printf(f, "%s %s %d %d 0x%4.4x 0x%4.4x %d %d %d %d\n", - batostr(&bt_sk(sk)->src), - batostr(&bt_sk(sk)->dst), - sk->sk_state, __le16_to_cpu(pi->psm), - pi->scid, pi->dcid, - pi->imtu, pi->omtu, pi->sec_level, - pi->mode); + list_for_each_entry(c, &chan_list, global_l) { + seq_printf(f, "%pMR %pMR %d %d 0x%4.4x 0x%4.4x %d %d %d %d\n", + &c->src, &c->dst, + c->state, __le16_to_cpu(c->psm), + c->scid, c->dcid, c->imtu, c->omtu, + c->sec_level, c->mode); } - read_unlock_bh(&l2cap_sk_list.lock); + read_unlock(&chan_list_lock); return 0; } @@ -3997,17 +7517,6 @@ static const struct file_operations l2cap_debugfs_fops = { static struct dentry *l2cap_debugfs; -static struct hci_proto l2cap_hci_proto = { - .name = "L2CAP", - .id = HCI_PROTO_L2CAP, - .connect_ind = l2cap_connect_ind, - .connect_cfm = l2cap_connect_cfm, - .disconn_ind = l2cap_disconn_ind, - .disconn_cfm = l2cap_disconn_cfm, - .security_cfm = l2cap_security_cfm, - .recv_acldata = l2cap_recv_acldata -}; - int __init l2cap_init(void) { int err; @@ -4016,44 +7525,26 @@ int __init l2cap_init(void) if (err < 0) return err; - _busy_wq = create_singlethread_workqueue("l2cap"); - if (!_busy_wq) { - err = -ENOMEM; - goto error; - } + if (IS_ERR_OR_NULL(bt_debugfs)) + return 0; - err = hci_register_proto(&l2cap_hci_proto); - if (err < 0) { - BT_ERR("L2CAP protocol registration failed"); - bt_sock_unregister(BTPROTO_L2CAP); - goto error; - } + l2cap_debugfs = debugfs_create_file("l2cap", 0444, bt_debugfs, + NULL, &l2cap_debugfs_fops); - if (bt_debugfs) { - l2cap_debugfs = debugfs_create_file("l2cap", 0444, - bt_debugfs, NULL, &l2cap_debugfs_fops); - if (!l2cap_debugfs) - BT_ERR("Failed to create L2CAP debug file"); - } + debugfs_create_u16("l2cap_le_max_credits", 0644, bt_debugfs, + &le_max_credits); + debugfs_create_u16("l2cap_le_default_mps", 0644, bt_debugfs, + &le_default_mps); - return 0; + bt_6lowpan_init(); -error: - destroy_workqueue(_busy_wq); - l2cap_cleanup_sockets(); - return err; + return 0; } void l2cap_exit(void) { + bt_6lowpan_cleanup(); debugfs_remove(l2cap_debugfs); - - flush_workqueue(_busy_wq); - destroy_workqueue(_busy_wq); - - if (hci_unregister_proto(&l2cap_hci_proto) < 0) - BT_ERR("L2CAP protocol unregistration failed"); - l2cap_cleanup_sockets(); } |