diff options
Diffstat (limited to 'net/bluetooth/l2cap_core.c')
| -rw-r--r-- | net/bluetooth/l2cap_core.c | 5513 |
1 files changed, 3959 insertions, 1554 deletions
diff --git a/net/bluetooth/l2cap_core.c b/net/bluetooth/l2cap_core.c index 4554e80d16a..323f23cd2c3 100644 --- a/net/bluetooth/l2cap_core.c +++ b/net/bluetooth/l2cap_core.c @@ -30,52 +30,57 @@ #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/unaligned.h> #include <net/bluetooth/bluetooth.h> #include <net/bluetooth/hci_core.h> #include <net/bluetooth/l2cap.h> -#include <net/bluetooth/smp.h> + +#include "smp.h" +#include "a2mp.h" +#include "amp.h" +#include "6lowpan.h" + +#define LE_FLOWCTL_MAX_CREDITS 65535 bool disable_ertm; -static u32 l2cap_feat_mask = L2CAP_FEAT_FIXED_CHAN; -static u8 l2cap_fixed_chan[8] = { L2CAP_FC_L2CAP, }; +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 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); + void *data); static int l2cap_build_conf_req(struct l2cap_chan *chan, void *data); -static void l2cap_send_disconn_req(struct l2cap_conn *conn, - struct l2cap_chan *chan, int err); +static void l2cap_send_disconn_req(struct l2cap_chan *chan, int err); + +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 l2cap_chan *__l2cap_get_chan_by_dcid(struct l2cap_conn *conn, u16 cid) +static struct l2cap_chan *__l2cap_get_chan_by_dcid(struct l2cap_conn *conn, + u16 cid) { struct l2cap_chan *c; @@ -86,7 +91,8 @@ static struct l2cap_chan *__l2cap_get_chan_by_dcid(struct l2cap_conn *conn, u16 return NULL; } -static struct l2cap_chan *__l2cap_get_chan_by_scid(struct l2cap_conn *conn, u16 cid) +static struct l2cap_chan *__l2cap_get_chan_by_scid(struct l2cap_conn *conn, + u16 cid) { struct l2cap_chan *c; @@ -99,7 +105,8 @@ static struct l2cap_chan *__l2cap_get_chan_by_scid(struct l2cap_conn *conn, u16 /* Find channel with given SCID. * Returns locked channel. */ -static struct l2cap_chan *l2cap_get_chan_by_scid(struct l2cap_conn *conn, u16 cid) +static struct l2cap_chan *l2cap_get_chan_by_scid(struct l2cap_conn *conn, + u16 cid) { struct l2cap_chan *c; @@ -112,7 +119,25 @@ static struct l2cap_chan *l2cap_get_chan_by_scid(struct l2cap_conn *conn, u16 ci return c; } -static struct l2cap_chan *__l2cap_get_chan_by_ident(struct l2cap_conn *conn, 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 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; @@ -123,12 +148,26 @@ static struct l2cap_chan *__l2cap_get_chan_by_ident(struct l2cap_conn *conn, u8 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(&bt_sk(c->sk)->src, src)) + if (c->sport == psm && !bacmp(&c->src, src)) return c; } return NULL; @@ -180,9 +219,14 @@ int l2cap_add_scid(struct l2cap_chan *chan, __u16 scid) static u16 l2cap_alloc_cid(struct l2cap_conn *conn) { - u16 cid = L2CAP_CID_DYN_START; + u16 cid, dyn_end; - for (; cid < L2CAP_CID_DYN_END; cid++) { + 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; } @@ -190,38 +234,56 @@ static u16 l2cap_alloc_cid(struct l2cap_conn *conn) return 0; } -static void __l2cap_state_change(struct l2cap_chan *chan, int state) +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)); + state_to_string(state)); chan->state = state; - chan->ops->state_change(chan->data, state); + chan->ops->state_change(chan, state, 0); } -static void l2cap_state_change(struct l2cap_chan *chan, int state) +static inline void l2cap_state_change_and_error(struct l2cap_chan *chan, + int state, int err) { - struct sock *sk = chan->sk; + chan->state = state; + chan->ops->state_change(chan, chan->state, err); +} - lock_sock(sk); - __l2cap_state_change(chan, state); - release_sock(sk); +static inline void l2cap_chan_set_err(struct l2cap_chan *chan, int err) +{ + chan->ops->state_change(chan, chan->state, err); } -static inline void __l2cap_chan_set_err(struct l2cap_chan *chan, int err) +static void __set_retrans_timer(struct l2cap_chan *chan) { - struct sock *sk = chan->sk; + if (!delayed_work_pending(&chan->monitor_timer) && + chan->retrans_timeout) { + l2cap_set_timer(chan, &chan->retrans_timer, + msecs_to_jiffies(chan->retrans_timeout)); + } +} - sk->sk_err = err; +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 inline void l2cap_chan_set_err(struct l2cap_chan *chan, int err) +static struct sk_buff *l2cap_ertm_seq_in_queue(struct sk_buff_head *head, + u16 seq) { - struct sock *sk = chan->sk; + struct sk_buff *skb; + + skb_queue_walk(head, skb) { + if (bt_cb(skb)->control.txseq == seq) + return skb; + } - lock_sock(sk); - __l2cap_chan_set_err(chan, err); - release_sock(sk); + return NULL; } /* ---- L2CAP sequence number lists ---- */ @@ -270,44 +332,20 @@ static inline bool l2cap_seq_list_contains(struct l2cap_seq_list *seq_list, return seq_list->list[seq & seq_list->mask] != L2CAP_SEQ_LIST_CLEAR; } -static u16 l2cap_seq_list_remove(struct l2cap_seq_list *seq_list, u16 seq) +static inline u16 l2cap_seq_list_pop(struct l2cap_seq_list *seq_list) { + u16 seq = seq_list->head; u16 mask = seq_list->mask; - if (seq_list->head == L2CAP_SEQ_LIST_CLEAR) { - /* In case someone tries to pop the head of an empty list */ - return L2CAP_SEQ_LIST_CLEAR; - } else if (seq_list->head == seq) { - /* Head can be removed in constant time */ - 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; - } - } else { - /* Walk the list to find the sequence number */ - u16 prev = seq_list->head; - while (seq_list->list[prev & mask] != seq) { - prev = seq_list->list[prev & mask]; - if (prev == L2CAP_SEQ_LIST_TAIL) - return L2CAP_SEQ_LIST_CLEAR; - } + seq_list->head = seq_list->list[seq & mask]; + seq_list->list[seq & mask] = L2CAP_SEQ_LIST_CLEAR; - /* Unlink the number from the list and clear it */ - seq_list->list[prev & mask] = seq_list->list[seq & mask]; - seq_list->list[seq & mask] = L2CAP_SEQ_LIST_CLEAR; - if (seq_list->tail == seq) - seq_list->tail = prev; + 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 inline u16 l2cap_seq_list_pop(struct l2cap_seq_list *seq_list) -{ - /* Remove the head in constant time */ - return l2cap_seq_list_remove(seq_list, seq_list->head); + return seq; } static void l2cap_seq_list_clear(struct l2cap_seq_list *seq_list) @@ -345,7 +383,7 @@ static void l2cap_seq_list_append(struct l2cap_seq_list *seq_list, u16 seq) static void l2cap_chan_timeout(struct work_struct *work) { struct l2cap_chan *chan = container_of(work, struct l2cap_chan, - chan_timer.work); + chan_timer.work); struct l2cap_conn *conn = chan->conn; int reason; @@ -357,7 +395,7 @@ static void l2cap_chan_timeout(struct work_struct *work) if (chan->state == BT_CONNECTED || chan->state == BT_CONFIG) reason = ECONNREFUSED; else if (chan->state == BT_CONNECT && - chan->sec_level != BT_SECURITY_SDP) + chan->sec_level != BT_SECURITY_SDP) reason = ECONNREFUSED; else reason = ETIMEDOUT; @@ -366,7 +404,7 @@ static void l2cap_chan_timeout(struct work_struct *work) l2cap_chan_unlock(chan); - chan->ops->close(chan->data); + chan->ops->close(chan); mutex_unlock(&conn->chan_lock); l2cap_chan_put(chan); @@ -390,20 +428,41 @@ struct l2cap_chan *l2cap_chan_create(void) chan->state = BT_OPEN; - atomic_set(&chan->refcnt, 1); + kref_init(&chan->kref); + + /* This flag is cleared in l2cap_chan_ready() */ + set_bit(CONF_NOT_COMPLETE, &chan->conf_state); BT_DBG("chan %p", chan); return chan; } -void l2cap_chan_destroy(struct l2cap_chan *chan) +static void l2cap_chan_destroy(struct kref *kref) { + struct l2cap_chan *chan = container_of(kref, struct l2cap_chan, kref); + + BT_DBG("chan %p", chan); + write_lock(&chan_list_lock); list_del(&chan->global_l); write_unlock(&chan_list_lock); - l2cap_chan_put(chan); + kfree(chan); +} + +void l2cap_chan_hold(struct l2cap_chan *c) +{ + BT_DBG("chan %p orig refcnt %d", c, atomic_read(&c->kref.refcount)); + + kref_get(&c->kref); +} + +void l2cap_chan_put(struct l2cap_chan *c) +{ + 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) @@ -412,12 +471,31 @@ void l2cap_chan_set_defaults(struct l2cap_chan *chan) 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_chan_add(struct l2cap_conn *conn, struct l2cap_chan *chan) +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, __le16_to_cpu(chan->psm), chan->dcid); @@ -428,16 +506,10 @@ static void __l2cap_chan_add(struct l2cap_conn *conn, struct l2cap_chan *chan) switch (chan->chan_type) { case L2CAP_CHAN_CONN_ORIENTED: - if (conn->hcon->type == LE_LINK) { - /* LE connection */ - chan->omtu = L2CAP_LE_DEFAULT_MTU; - chan->scid = L2CAP_CID_LE_DATA; - chan->dcid = L2CAP_CID_LE_DATA; - } else { - /* Alloc CID for connection-oriented socket */ - chan->scid = l2cap_alloc_cid(conn); + /* 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: @@ -447,6 +519,10 @@ static void __l2cap_chan_add(struct l2cap_conn *conn, struct l2cap_chan *chan) 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 */ chan->scid = L2CAP_CID_SIGNALING; @@ -459,65 +535,67 @@ static void __l2cap_chan_add(struct l2cap_conn *conn, struct l2cap_chan *chan) 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_DEFAULT_FLUSH_TO; + chan->local_flush_to = L2CAP_EFS_DEFAULT_FLUSH_TO; l2cap_chan_hold(chan); + hci_conn_hold(conn->hcon); + list_add(&chan->list, &conn->chan_l); } -static void l2cap_chan_add(struct l2cap_conn *conn, struct l2cap_chan *chan) +void l2cap_chan_add(struct l2cap_conn *conn, struct l2cap_chan *chan) { mutex_lock(&conn->chan_lock); __l2cap_chan_add(conn, chan); mutex_unlock(&conn->chan_lock); } -static void l2cap_chan_del(struct l2cap_chan *chan, int err) +void l2cap_chan_del(struct l2cap_chan *chan, int err) { - struct sock *sk = chan->sk; struct l2cap_conn *conn = chan->conn; - struct sock *parent = bt_sk(sk)->parent; __clear_chan_timer(chan); BT_DBG("chan %p, conn %p, err %d", chan, conn, err); if (conn) { + struct amp_mgr *mgr = conn->hcon->amp_mgr; /* Delete from channel list */ list_del(&chan->list); l2cap_chan_put(chan); chan->conn = NULL; - hci_conn_put(conn->hcon); - } - lock_sock(sk); + if (chan->scid != L2CAP_CID_A2MP) + hci_conn_drop(conn->hcon); - __l2cap_state_change(chan, BT_CLOSED); - sock_set_flag(sk, SOCK_ZAPPED); + if (mgr && mgr->bredr_chan == chan) + mgr->bredr_chan = NULL; + } - if (err) - __l2cap_chan_set_err(chan, err); + if (chan->hs_hchan) { + struct hci_chan *hs_hchan = chan->hs_hchan; - if (parent) { - bt_accept_unlink(sk); - parent->sk_data_ready(parent, 0); - } else - sk->sk_state_change(sk); + BT_DBG("chan %p disconnect hs_hchan %p", chan, hs_hchan); + amp_disconnect_logical_link(hs_hchan); + } - release_sock(sk); + chan->ops->teardown(chan, err); - if (!(test_bit(CONF_OUTPUT_DONE, &chan->conf_state) && - test_bit(CONF_INPUT_DONE, &chan->conf_state))) + if (test_bit(CONF_NOT_COMPLETE, &chan->conf_state)) return; - skb_queue_purge(&chan->tx_q); + switch(chan->mode) { + case L2CAP_MODE_BASIC: + break; - if (chan->mode == L2CAP_MODE_ERTM) { - struct srej_list *l, *tmp; + 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); @@ -526,78 +604,104 @@ static void l2cap_chan_del(struct l2cap_chan *chan, int err) l2cap_seq_list_free(&chan->srej_list); l2cap_seq_list_free(&chan->retrans_list); - list_for_each_entry_safe(l, tmp, &chan->srej_l, list) { - list_del(&l->list); - kfree(l); - } + + /* fall through */ + + case L2CAP_MODE_STREAMING: + skb_queue_purge(&chan->tx_q); + break; } + + return; } -static void l2cap_chan_cleanup_listen(struct sock *parent) +void l2cap_conn_update_id_addr(struct hci_conn *hcon) { - struct sock *sk; - - BT_DBG("parent %p", parent); + struct l2cap_conn *conn = hcon->l2cap_data; + struct l2cap_chan *chan; - /* Close not yet accepted channels */ - while ((sk = bt_accept_dequeue(parent, NULL))) { - struct l2cap_chan *chan = l2cap_pi(sk)->chan; + mutex_lock(&conn->chan_lock); + list_for_each_entry(chan, &conn->chan_l, list) { l2cap_chan_lock(chan); - __clear_chan_timer(chan); - l2cap_chan_close(chan, ECONNRESET); + bacpy(&chan->dst, &hcon->dst); + chan->dst_type = bdaddr_type(hcon, hcon->dst_type); l2cap_chan_unlock(chan); - - chan->ops->close(chan->data); } + + 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); + + 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); + + l2cap_send_cmd(conn, chan->ident, L2CAP_LE_CONN_RSP, sizeof(rsp), + &rsp); +} + +static void l2cap_chan_connect_reject(struct l2cap_chan *chan) +{ + struct l2cap_conn *conn = chan->conn; + struct l2cap_conn_rsp rsp; + u16 result; + + 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); + + 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); + + 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; - struct sock *sk = chan->sk; - BT_DBG("chan %p state %s sk %p", chan, - state_to_string(chan->state), sk); + BT_DBG("chan %p state %s", chan, state_to_string(chan->state)); switch (chan->state) { case BT_LISTEN: - lock_sock(sk); - l2cap_chan_cleanup_listen(sk); - - __l2cap_state_change(chan, BT_CLOSED); - sock_set_flag(sk, SOCK_ZAPPED); - release_sock(sk); + chan->ops->teardown(chan, 0); break; case BT_CONNECTED: case BT_CONFIG: - if (chan->chan_type == L2CAP_CHAN_CONN_ORIENTED && - conn->hcon->type == ACL_LINK) { - __set_chan_timer(chan, sk->sk_sndtimeo); - l2cap_send_disconn_req(conn, chan, reason); + 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 && - conn->hcon->type == ACL_LINK) { - struct l2cap_conn_rsp rsp; - __u16 result; - - if (test_bit(BT_SK_DEFER_SETUP, &bt_sk(sk)->flags)) - result = L2CAP_CR_SEC_BLOCK; - else - result = L2CAP_CR_BAD_PSM; - l2cap_state_change(chan, BT_DISCONN); - - 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); - l2cap_send_cmd(conn, chan->ident, L2CAP_CONN_RSP, - sizeof(rsp), &rsp); + 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); @@ -609,41 +713,59 @@ void l2cap_chan_close(struct l2cap_chan *chan, int reason) break; default: - lock_sock(sk); - sock_set_flag(sk, SOCK_ZAPPED); - release_sock(sk); + chan->ops->teardown(chan, 0); break; } } static inline u8 l2cap_get_auth_type(struct l2cap_chan *chan) { - if (chan->chan_type == L2CAP_CHAN_RAW) { + 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 (chan->psm == cpu_to_le16(0x0001)) { - if (chan->sec_level == BT_SECURITY_LOW) - chan->sec_level = BT_SECURITY_SDP; - - if (chan->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 { + 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; } } @@ -653,6 +775,9 @@ int l2cap_chan_check_security(struct l2cap_chan *chan) struct l2cap_conn *conn = chan->conn; __u8 auth_type; + 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, chan->sec_level, auth_type); @@ -680,7 +805,8 @@ static u8 l2cap_get_ident(struct l2cap_conn *conn) return id; } -static 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; @@ -701,16 +827,31 @@ static void l2cap_send_cmd(struct l2cap_conn *conn, u8 ident, u8 code, u16 len, hci_send_acl(conn->hchan, skb, flags); } +static bool __chan_is_moving(struct l2cap_chan *chan) +{ + 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); + 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); + + return; + } if (!test_bit(FLAG_FLUSHABLE, &chan->flags) && - lmp_no_flush_capable(hcon->hdev)) + lmp_no_flush_capable(hcon->hdev)) flags = ACL_START_NO_FLUSH; else flags = ACL_START; @@ -773,9 +914,11 @@ static inline void __unpack_control(struct l2cap_chan *chan, 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); } } @@ -830,66 +973,105 @@ static inline void __pack_control(struct l2cap_chan *chan, } } -static inline void l2cap_send_sframe(struct l2cap_chan *chan, u32 control) +static inline unsigned int __ertm_hdr_size(struct l2cap_chan *chan) { - struct sk_buff *skb; - struct l2cap_hdr *lh; - struct l2cap_conn *conn = chan->conn; - int count, hlen; - - if (chan->state != BT_CONNECTED) - return; - if (test_bit(FLAG_EXT_CTRL, &chan->flags)) - hlen = L2CAP_EXT_HDR_SIZE; + return L2CAP_EXT_HDR_SIZE; else - hlen = L2CAP_ENH_HDR_SIZE; + 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; - BT_DBG("chan %p, control 0x%8.8x", chan, control); - - count = min_t(unsigned int, conn->mtu, hlen); - - control |= __set_sframe(chan); + skb = bt_skb_alloc(hlen, GFP_KERNEL); - if (test_and_clear_bit(CONN_SEND_FBIT, &chan->conn_state)) - control |= __set_ctrl_final(chan); - - if (test_and_clear_bit(CONN_SEND_PBIT, &chan->conn_state)) - control |= __set_ctrl_poll(chan); - - skb = bt_skb_alloc(count, GFP_ATOMIC); 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(chan->dcid); - __put_control(chan, control, skb_put(skb, __ctrl_size(chan))); + 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 *)lh, count - L2CAP_FCS_SIZE); + u16 fcs = crc16(0, (u8 *)skb->data, skb->len); put_unaligned_le16(fcs, skb_put(skb, L2CAP_FCS_SIZE)); } skb->priority = HCI_PRIO_MAX; - l2cap_do_send(chan, skb); + return skb; } -static inline void l2cap_send_rr_or_rnr(struct l2cap_chan *chan, u32 control) +static void l2cap_send_sframe(struct l2cap_chan *chan, + struct l2cap_ctrl *control) { - if (test_bit(CONN_LOCAL_BUSY, &chan->conn_state)) { - control |= __set_ctrl_super(chan, L2CAP_SUPER_RNR); + 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); - } else - control |= __set_ctrl_super(chan, L2CAP_SUPER_RR); - control |= __set_reqseq(chan, chan->buffer_seq); + if (control->super != L2CAP_SUPER_SREJ) { + chan->last_acked_seq = control->reqseq; + __clear_ack_timer(chan); + } - l2cap_send_sframe(chan, control); + 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 + 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); } static inline int __l2cap_no_conn_pending(struct l2cap_chan *chan) @@ -897,7 +1079,41 @@ static inline int __l2cap_no_conn_pending(struct l2cap_chan *chan) return !test_bit(CONF_CONNECT_PEND, &chan->conf_state); } -static void l2cap_send_conn_req(struct l2cap_chan *chan) +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; @@ -912,27 +1128,136 @@ static void l2cap_send_conn_req(struct l2cap_chan *chan) l2cap_send_cmd(conn, chan->ident, L2CAP_CONN_REQ, sizeof(req), &req); } -static void l2cap_chan_ready(struct l2cap_chan *chan) +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 void l2cap_move_done(struct l2cap_chan *chan) { - struct sock *sk = chan->sk; - struct sock *parent; + u8 move_role = chan->move_role; + BT_DBG("chan %p", chan); - lock_sock(sk); + chan->move_state = L2CAP_MOVE_STABLE; + chan->move_role = L2CAP_MOVE_ROLE_NONE; - parent = bt_sk(sk)->parent; + if (chan->mode != L2CAP_MODE_ERTM) + return; - BT_DBG("sk %p, parent %p", sk, parent); + 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 void l2cap_chan_ready(struct l2cap_chan *chan) +{ + /* This clears all conf flags, including CONF_NOT_COMPLETE */ chan->conf_state = 0; __clear_chan_timer(chan); - __l2cap_state_change(chan, BT_CONNECTED); - sk->sk_state_change(sk); + 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_le_connect(struct l2cap_chan *chan) +{ + 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); - if (parent) - parent->sk_data_ready(parent, 0); + chan->ident = l2cap_get_ident(conn); - release_sock(sk); + 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) @@ -940,7 +1265,7 @@ static void l2cap_do_start(struct l2cap_chan *chan) struct l2cap_conn *conn = chan->conn; if (conn->hcon->type == LE_LINK) { - l2cap_chan_ready(chan); + l2cap_le_start(chan); return; } @@ -949,8 +1274,9 @@ static void l2cap_do_start(struct l2cap_chan *chan) return; if (l2cap_chan_check_security(chan) && - __l2cap_no_conn_pending(chan)) - l2cap_send_conn_req(chan); + __l2cap_no_conn_pending(chan)) { + l2cap_start_connection(chan); + } } else { struct l2cap_info_req req; req.type = cpu_to_le16(L2CAP_IT_FEAT_MASK); @@ -960,8 +1286,8 @@ static void l2cap_do_start(struct l2cap_chan *chan) 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); } } @@ -981,29 +1307,31 @@ static inline int l2cap_mode_supported(__u8 mode, __u32 feat_mask) } } -static void l2cap_send_disconn_req(struct l2cap_conn *conn, struct l2cap_chan *chan, int err) +static void l2cap_send_disconn_req(struct l2cap_chan *chan, int err) { - struct sock *sk = chan->sk; + struct l2cap_conn *conn = chan->conn; struct l2cap_disconn_req req; if (!conn) return; - if (chan->mode == L2CAP_MODE_ERTM) { + if (chan->mode == L2CAP_MODE_ERTM && chan->state == BT_CONNECTED) { __clear_retrans_timer(chan); __clear_monitor_timer(chan); __clear_ack_timer(chan); } + if (chan->scid == L2CAP_CID_A2MP) { + l2cap_state_change(chan, BT_DISCONN); + return; + } + 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); + l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_DISCONN_REQ, + sizeof(req), &req); - lock_sock(sk); - __l2cap_state_change(chan, BT_DISCONN); - __l2cap_chan_set_err(chan, err); - release_sock(sk); + l2cap_state_change_and_error(chan, BT_DISCONN, err); } /* ---- L2CAP connections ---- */ @@ -1016,8 +1344,6 @@ static void l2cap_conn_start(struct l2cap_conn *conn) mutex_lock(&conn->chan_lock); list_for_each_entry_safe(chan, tmp, &conn->chan_l, list) { - struct sock *sk = chan->sk; - l2cap_chan_lock(chan); if (chan->chan_type != L2CAP_CHAN_CONN_ORIENTED) { @@ -1027,20 +1353,20 @@ static void l2cap_conn_start(struct l2cap_conn *conn) if (chan->state == BT_CONNECT) { if (!l2cap_chan_check_security(chan) || - !__l2cap_no_conn_pending(chan)) { + !__l2cap_no_conn_pending(chan)) { l2cap_chan_unlock(chan); continue; } if (!l2cap_mode_supported(chan->mode, conn->feat_mask) - && test_bit(CONF_STATE2_DEVICE, + && test_bit(CONF_STATE2_DEVICE, &chan->conf_state)) { l2cap_chan_close(chan, ECONNRESET); l2cap_chan_unlock(chan); continue; } - l2cap_send_conn_req(chan); + l2cap_start_connection(chan); } else if (chan->state == BT_CONNECT2) { struct l2cap_conn_rsp rsp; @@ -1049,38 +1375,33 @@ static void l2cap_conn_start(struct l2cap_conn *conn) rsp.dcid = cpu_to_le16(chan->scid); if (l2cap_chan_check_security(chan)) { - lock_sock(sk); - if (test_bit(BT_SK_DEFER_SETUP, - &bt_sk(sk)->flags)) { - struct sock *parent = bt_sk(sk)->parent; + 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); - if (parent) - parent->sk_data_ready(parent, 0); + chan->ops->defer(chan); } else { - __l2cap_state_change(chan, 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); } - release_sock(sk); } else { rsp.result = cpu_to_le16(L2CAP_CR_PEND); rsp.status = cpu_to_le16(L2CAP_CS_AUTHEN_PEND); } l2cap_send_cmd(conn, chan->ident, L2CAP_CONN_RSP, - sizeof(rsp), &rsp); + sizeof(rsp), &rsp); if (test_bit(CONF_REQ_SENT, &chan->conf_state) || - rsp.result != L2CAP_CR_SUCCESS) { + rsp.result != L2CAP_CR_SUCCESS) { l2cap_chan_unlock(chan); continue; } 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); + l2cap_build_conf_req(chan, buf), buf); chan->num_conf_req++; } @@ -1102,8 +1423,6 @@ static struct l2cap_chan *l2cap_global_chan_by_scid(int state, u16 cid, read_lock(&chan_list_lock); list_for_each_entry(c, &chan_list, global_l) { - struct sock *sk = c->sk; - if (state && c->state != state) continue; @@ -1112,16 +1431,16 @@ static struct l2cap_chan *l2cap_global_chan_by_scid(int state, u16 cid, int src_any, dst_any; /* Exact match. */ - src_match = !bacmp(&bt_sk(sk)->src, src); - dst_match = !bacmp(&bt_sk(sk)->dst, dst); + 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 */ - src_any = !bacmp(&bt_sk(sk)->src, BDADDR_ANY); - dst_any = !bacmp(&bt_sk(sk)->dst, BDADDR_ANY); + 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; @@ -1135,88 +1454,89 @@ static struct l2cap_chan *l2cap_global_chan_by_scid(int state, u16 cid, static void l2cap_le_conn_ready(struct l2cap_conn *conn) { - struct sock *parent, *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 */ - pchan = l2cap_global_chan_by_scid(BT_LISTEN, L2CAP_CID_LE_DATA, - conn->src, conn->dst); + pchan = l2cap_global_chan_by_scid(BT_LISTEN, L2CAP_CID_ATT, + &hcon->src, &hcon->dst); if (!pchan) return; - parent = pchan->sk; + /* Client ATT sockets should override the server one */ + if (__l2cap_get_chan_by_dcid(conn, L2CAP_CID_ATT)) + return; - lock_sock(parent); + dst_type = bdaddr_type(hcon, hcon->dst_type); - /* Check for backlog size */ - if (sk_acceptq_is_full(parent)) { - BT_DBG("backlog full %d", parent->sk_ack_backlog); - goto clean; - } + /* If device is blocked, do not create a channel for it */ + if (hci_blacklist_lookup(hcon->hdev, &hcon->dst, dst_type)) + return; + + l2cap_chan_lock(pchan); - chan = pchan->ops->new_connection(pchan->data); + chan = pchan->ops->new_connection(pchan); if (!chan) goto clean; - sk = chan->sk; + bacpy(&chan->src, &hcon->src); + bacpy(&chan->dst, &hcon->dst); + chan->src_type = bdaddr_type(hcon, hcon->src_type); + chan->dst_type = dst_type; - hci_conn_hold(conn->hcon); - - bacpy(&bt_sk(sk)->src, conn->src); - bacpy(&bt_sk(sk)->dst, conn->dst); - - bt_accept_enqueue(parent, sk); - - l2cap_chan_add(conn, chan); - - __set_chan_timer(chan, sk->sk_sndtimeo); - - __l2cap_state_change(chan, BT_CONNECTED); - parent->sk_data_ready(parent, 0); + __l2cap_chan_add(conn, chan); clean: - release_sock(parent); + l2cap_chan_unlock(pchan); } static void l2cap_conn_ready(struct l2cap_conn *conn) { struct l2cap_chan *chan; + struct hci_conn *hcon = conn->hcon; BT_DBG("conn %p", conn); - if (!conn->hcon->out && conn->hcon->type == LE_LINK) - l2cap_le_conn_ready(conn); - - if (conn->hcon->out && conn->hcon->type == LE_LINK) - smp_conn_security(conn, conn->hcon->pending_sec_level); + /* 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); + list_for_each_entry(chan, &conn->chan_l, list) { l2cap_chan_lock(chan); - if (conn->hcon->type == LE_LINK) { - if (smp_conn_security(conn, chan->sec_level)) - l2cap_chan_ready(chan); + if (chan->scid == L2CAP_CID_A2MP) { + l2cap_chan_unlock(chan); + continue; + } + if (hcon->type == LE_LINK) { + l2cap_le_start(chan); } else if (chan->chan_type != L2CAP_CHAN_CONN_ORIENTED) { - struct sock *sk = chan->sk; - __clear_chan_timer(chan); - lock_sock(sk); - __l2cap_state_change(chan, BT_CONNECTED); - sk->sk_state_change(sk); - release_sock(sk); + l2cap_chan_ready(chan); - } else if (chan->state == BT_CONNECT) + } else if (chan->state == BT_CONNECT) { l2cap_do_start(chan); + } l2cap_chan_unlock(chan); } mutex_unlock(&conn->chan_lock); + + queue_work(hcon->hdev->workqueue, &conn->pending_rx_work); } /* Notify sockets that we cannot guaranty reliability anymore */ @@ -1230,7 +1550,7 @@ static void l2cap_conn_unreliable(struct l2cap_conn *conn, int err) list_for_each_entry(chan, &conn->chan_l, list) { if (test_bit(FLAG_FORCE_RELIABLE, &chan->flags)) - __l2cap_chan_set_err(chan, err); + l2cap_chan_set_err(chan, err); } mutex_unlock(&conn->chan_lock); @@ -1239,7 +1559,7 @@ static void l2cap_conn_unreliable(struct l2cap_conn *conn, int err) static void l2cap_info_timeout(struct work_struct *work) { struct l2cap_conn *conn = container_of(work, struct l2cap_conn, - info_timer.work); + info_timer.work); conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_DONE; conn->info_ident = 0; @@ -1247,6 +1567,89 @@ static void l2cap_info_timeout(struct work_struct *work) l2cap_conn_start(conn); } +/* + * 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 hci_dev *hdev = conn->hcon->hdev; + int ret; + + /* 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. */ + + hci_dev_lock(hdev); + + if (user->list.next || user->list.prev) { + ret = -EINVAL; + goto out_unlock; + } + + /* conn->hchan is NULL after l2cap_conn_del() was called */ + if (!conn->hchan) { + ret = -ENODEV; + goto out_unlock; + } + + ret = user->probe(conn, user); + if (ret) + goto out_unlock; + + list_add(&user->list, &conn->users); + ret = 0; + +out_unlock: + hci_dev_unlock(hdev); + return ret; +} +EXPORT_SYMBOL(l2cap_register_user); + +void l2cap_unregister_user(struct l2cap_conn *conn, struct l2cap_user *user) +{ + struct hci_dev *hdev = conn->hcon->hdev; + + hci_dev_lock(hdev); + + if (!user->list.next || !user->list.prev) + goto out_unlock; + + 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; @@ -1259,6 +1662,17 @@ 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 */ @@ -1270,7 +1684,7 @@ static void l2cap_conn_del(struct hci_conn *hcon, int err) l2cap_chan_unlock(chan); - chan->ops->close(chan->data); + chan->ops->close(chan); l2cap_chan_put(chan); } @@ -1287,13 +1701,14 @@ static void l2cap_conn_del(struct hci_conn *hcon, int err) } hcon->l2cap_data = NULL; - kfree(conn); + 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); + security_timer.work); BT_DBG("conn %p", conn); @@ -1303,54 +1718,25 @@ static void security_timeout(struct work_struct *work) } } -static struct l2cap_conn *l2cap_conn_add(struct hci_conn *hcon, u8 status) +static void l2cap_conn_free(struct kref *ref) { - struct l2cap_conn *conn = hcon->l2cap_data; - struct hci_chan *hchan; - - if (conn || status) - return conn; - - hchan = hci_chan_create(hcon); - if (!hchan) - return NULL; - - conn = kzalloc(sizeof(struct l2cap_conn), GFP_ATOMIC); - if (!conn) { - hci_chan_del(hchan); - return NULL; - } - - hcon->l2cap_data = conn; - conn->hcon = hcon; - conn->hchan = hchan; + struct l2cap_conn *conn = container_of(ref, struct l2cap_conn, ref); - BT_DBG("hcon %p conn %p hchan %p", hcon, conn, hchan); - - if (hcon->hdev->le_mtu && hcon->type == LE_LINK) - conn->mtu = hcon->hdev->le_mtu; - else - conn->mtu = hcon->hdev->acl_mtu; - - conn->src = &hcon->hdev->bdaddr; - conn->dst = &hcon->dst; - - conn->feat_mask = 0; - - spin_lock_init(&conn->lock); - mutex_init(&conn->chan_lock); - - INIT_LIST_HEAD(&conn->chan_l); - - if (hcon->type == LE_LINK) - INIT_DELAYED_WORK(&conn->security_timer, security_timeout); - else - INIT_DELAYED_WORK(&conn->info_timer, l2cap_info_timeout); + hci_conn_put(conn->hcon); + kfree(conn); +} - conn->disc_reason = HCI_ERROR_REMOTE_USER_TERM; +void l2cap_conn_get(struct l2cap_conn *conn) +{ + kref_get(&conn->ref); +} +EXPORT_SYMBOL(l2cap_conn_get); - return conn; +void l2cap_conn_put(struct l2cap_conn *conn) +{ + kref_put(&conn->ref, l2cap_conn_free); } +EXPORT_SYMBOL(l2cap_conn_put); /* ---- Socket interface ---- */ @@ -1359,33 +1745,38 @@ static struct l2cap_conn *l2cap_conn_add(struct hci_conn *hcon, u8 status) */ static struct l2cap_chan *l2cap_global_chan_by_psm(int state, __le16 psm, bdaddr_t *src, - bdaddr_t *dst) + bdaddr_t *dst, + u8 link_type) { struct l2cap_chan *c, *c1 = NULL; read_lock(&chan_list_lock); list_for_each_entry(c, &chan_list, global_l) { - struct sock *sk = c->sk; - if (state && c->state != state) continue; + 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. */ - src_match = !bacmp(&bt_sk(sk)->src, src); - dst_match = !bacmp(&bt_sk(sk)->dst, dst); + 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 */ - src_any = !bacmp(&bt_sk(sk)->src, BDADDR_ANY); - dst_any = !bacmp(&bt_sk(sk)->dst, BDADDR_ANY); + 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; @@ -1397,202 +1788,23 @@ static struct l2cap_chan *l2cap_global_chan_by_psm(int state, __le16 psm, return c1; } -int l2cap_chan_connect(struct l2cap_chan *chan, __le16 psm, u16 cid, - bdaddr_t *dst, u8 dst_type) -{ - struct sock *sk = chan->sk; - bdaddr_t *src = &bt_sk(sk)->src; - struct l2cap_conn *conn; - struct hci_conn *hcon; - struct hci_dev *hdev; - __u8 auth_type; - int err; - - BT_DBG("%s -> %s (type %u) psm 0x%2.2x", batostr(src), batostr(dst), - dst_type, __le16_to_cpu(chan->psm)); - - hdev = hci_get_route(dst, src); - if (!hdev) - return -EHOSTUNREACH; - - hci_dev_lock(hdev); - - l2cap_chan_lock(chan); - - /* PSM must be odd and lsb of upper byte must be 0 */ - if ((__le16_to_cpu(psm) & 0x0101) != 0x0001 && !cid && - chan->chan_type != L2CAP_CHAN_RAW) { - err = -EINVAL; - goto done; - } - - if (chan->chan_type == L2CAP_CHAN_CONN_ORIENTED && !(psm || cid)) { - err = -EINVAL; - goto done; - } - - switch (chan->mode) { - case L2CAP_MODE_BASIC: - break; - case L2CAP_MODE_ERTM: - case L2CAP_MODE_STREAMING: - if (!disable_ertm) - break; - /* fall through */ - default: - err = -ENOTSUPP; - goto done; - } - - lock_sock(sk); - - switch (sk->sk_state) { - case BT_CONNECT: - case BT_CONNECT2: - case BT_CONFIG: - /* Already connecting */ - err = 0; - release_sock(sk); - goto done; - - case BT_CONNECTED: - /* Already connected */ - err = -EISCONN; - release_sock(sk); - goto done; - - case BT_OPEN: - case BT_BOUND: - /* Can connect */ - break; - - default: - err = -EBADFD; - release_sock(sk); - goto done; - } - - /* Set destination address and psm */ - bacpy(&bt_sk(sk)->dst, dst); - - release_sock(sk); - - chan->psm = psm; - chan->dcid = cid; - - auth_type = l2cap_get_auth_type(chan); - - if (chan->dcid == L2CAP_CID_LE_DATA) - hcon = hci_connect(hdev, LE_LINK, dst, dst_type, - chan->sec_level, auth_type); - else - hcon = hci_connect(hdev, ACL_LINK, dst, dst_type, - chan->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; - } - - if (hcon->type == LE_LINK) { - err = 0; - - if (!list_empty(&conn->chan_l)) { - err = -EBUSY; - hci_conn_put(hcon); - } - - if (err) - goto done; - } - - /* Update source addr of the socket */ - bacpy(src, conn->src); - - l2cap_chan_unlock(chan); - l2cap_chan_add(conn, chan); - l2cap_chan_lock(chan); - - l2cap_state_change(chan, BT_CONNECT); - __set_chan_timer(chan, sk->sk_sndtimeo); - - 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; -} - -int __l2cap_wait_ack(struct sock *sk) -{ - struct l2cap_chan *chan = l2cap_pi(sk)->chan; - DECLARE_WAITQUEUE(wait, current); - int err = 0; - int timeo = HZ/5; - - add_wait_queue(sk_sleep(sk), &wait); - set_current_state(TASK_INTERRUPTIBLE); - while (chan->unacked_frames > 0 && chan->conn) { - if (!timeo) - timeo = HZ/5; - - if (signal_pending(current)) { - err = sock_intr_errno(timeo); - break; - } - - release_sock(sk); - timeo = schedule_timeout(timeo); - lock_sock(sk); - set_current_state(TASK_INTERRUPTIBLE); - - err = sock_error(sk); - if (err) - break; - } - set_current_state(TASK_RUNNING); - remove_wait_queue(sk_sleep(sk), &wait); - return err; -} - static void l2cap_monitor_timeout(struct work_struct *work) { struct l2cap_chan *chan = container_of(work, struct l2cap_chan, - monitor_timer.work); + monitor_timer.work); BT_DBG("chan %p", chan); l2cap_chan_lock(chan); - if (chan->retry_count >= chan->remote_max_tx) { - l2cap_send_disconn_req(chan->conn, chan, ECONNABORTED); + if (!chan->conn) { l2cap_chan_unlock(chan); l2cap_chan_put(chan); return; } - chan->retry_count++; - __set_monitor_timer(chan); + l2cap_tx(chan, NULL, NULL, L2CAP_EV_MONITOR_TO); - l2cap_send_rr_or_rnr(chan, L2CAP_CTRL_POLL); l2cap_chan_unlock(chan); l2cap_chan_put(chan); } @@ -1600,234 +1812,302 @@ static void l2cap_monitor_timeout(struct work_struct *work) static void l2cap_retrans_timeout(struct work_struct *work) { struct l2cap_chan *chan = container_of(work, struct l2cap_chan, - retrans_timer.work); + retrans_timer.work); BT_DBG("chan %p", chan); l2cap_chan_lock(chan); - chan->retry_count = 1; - __set_monitor_timer(chan); - - set_bit(CONN_WAIT_F, &chan->conn_state); - - l2cap_send_rr_or_rnr(chan, L2CAP_CTRL_POLL); + if (!chan->conn) { + l2cap_chan_unlock(chan); + l2cap_chan_put(chan); + return; + } + l2cap_tx(chan, NULL, NULL, L2CAP_EV_RETRANS_TO); l2cap_chan_unlock(chan); l2cap_chan_put(chan); } -static void l2cap_drop_acked_frames(struct l2cap_chan *chan) +static void l2cap_streaming_send(struct l2cap_chan *chan, + struct sk_buff_head *skbs) { struct sk_buff *skb; + struct l2cap_ctrl *control; - while ((skb = skb_peek(&chan->tx_q)) && - chan->unacked_frames) { - if (bt_cb(skb)->control.txseq == chan->expected_ack_seq) - break; + BT_DBG("chan %p, skbs %p", chan, skbs); - skb = skb_dequeue(&chan->tx_q); - kfree_skb(skb); + if (__chan_is_moving(chan)) + return; - chan->unacked_frames--; - } + skb_queue_splice_tail_init(skbs, &chan->tx_q); - if (!chan->unacked_frames) - __clear_retrans_timer(chan); -} + while (!skb_queue_empty(&chan->tx_q)) { -static void l2cap_streaming_send(struct l2cap_chan *chan) -{ - struct sk_buff *skb; - u32 control; - u16 fcs; + skb = skb_dequeue(&chan->tx_q); + + bt_cb(skb)->control.retries = 1; + control = &bt_cb(skb)->control; + + control->reqseq = 0; + control->txseq = chan->next_tx_seq; - while ((skb = skb_dequeue(&chan->tx_q))) { - control = __get_control(chan, skb->data + L2CAP_HDR_SIZE); - control |= __set_txseq(chan, chan->next_tx_seq); - control |= __set_ctrl_sar(chan, bt_cb(skb)->control.sar); - __put_control(chan, control, skb->data + L2CAP_HDR_SIZE); + __pack_control(chan, control, skb); if (chan->fcs == L2CAP_FCS_CRC16) { - fcs = crc16(0, (u8 *)skb->data, - skb->len - L2CAP_FCS_SIZE); - put_unaligned_le16(fcs, - skb->data + skb->len - L2CAP_FCS_SIZE); + u16 fcs = crc16(0, (u8 *) skb->data, skb->len); + put_unaligned_le16(fcs, skb_put(skb, L2CAP_FCS_SIZE)); } l2cap_do_send(chan, skb); + BT_DBG("Sent txseq %u", control->txseq); + chan->next_tx_seq = __next_seq(chan, chan->next_tx_seq); + chan->frames_sent++; } } -static void l2cap_retransmit_one_frame(struct l2cap_chan *chan, u16 tx_seq) +static int l2cap_ertm_send(struct l2cap_chan *chan) { struct sk_buff *skb, *tx_skb; - u16 fcs; - u32 control; + struct l2cap_ctrl *control; + int sent = 0; - skb = skb_peek(&chan->tx_q); - if (!skb) - return; + BT_DBG("chan %p", chan); - while (bt_cb(skb)->control.txseq != tx_seq) { - if (skb_queue_is_last(&chan->tx_q, skb)) - return; + if (chan->state != BT_CONNECTED) + return -ENOTCONN; - skb = skb_queue_next(&chan->tx_q, skb); - } + if (test_bit(CONN_REMOTE_BUSY, &chan->conn_state)) + return 0; - if (bt_cb(skb)->control.retries == chan->remote_max_tx && - chan->remote_max_tx) { - l2cap_send_disconn_req(chan->conn, chan, ECONNABORTED); - return; - } + if (__chan_is_moving(chan)) + return 0; - tx_skb = skb_clone(skb, GFP_ATOMIC); - bt_cb(skb)->control.retries++; + while (chan->tx_send_head && + chan->unacked_frames < chan->remote_tx_win && + chan->tx_state == L2CAP_TX_STATE_XMIT) { - control = __get_control(chan, tx_skb->data + L2CAP_HDR_SIZE); - control &= __get_sar_mask(chan); + skb = chan->tx_send_head; - if (test_and_clear_bit(CONN_SEND_FBIT, &chan->conn_state)) - control |= __set_ctrl_final(chan); + bt_cb(skb)->control.retries = 1; + control = &bt_cb(skb)->control; - control |= __set_reqseq(chan, chan->buffer_seq); - control |= __set_txseq(chan, tx_seq); + if (test_and_clear_bit(CONN_SEND_FBIT, &chan->conn_state)) + control->final = 1; - __put_control(chan, control, tx_skb->data + L2CAP_HDR_SIZE); + control->reqseq = chan->buffer_seq; + chan->last_acked_seq = chan->buffer_seq; + control->txseq = chan->next_tx_seq; - if (chan->fcs == L2CAP_FCS_CRC16) { - fcs = crc16(0, (u8 *)tx_skb->data, - tx_skb->len - L2CAP_FCS_SIZE); - put_unaligned_le16(fcs, - tx_skb->data + tx_skb->len - L2CAP_FCS_SIZE); + __pack_control(chan, control, 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)); + } + + /* 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); + + if (!tx_skb) + break; + + __set_retrans_timer(chan); + + chan->next_tx_seq = __next_seq(chan, chan->next_tx_seq); + chan->unacked_frames++; + chan->frames_sent++; + sent++; + + 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); + + l2cap_do_send(chan, tx_skb); + BT_DBG("Sent txseq %u", control->txseq); } - l2cap_do_send(chan, tx_skb); + BT_DBG("Sent %d, %u unacked, %u in ERTM queue", sent, + chan->unacked_frames, skb_queue_len(&chan->tx_q)); + + return sent; } -static int l2cap_ertm_send(struct l2cap_chan *chan) +static void l2cap_ertm_resend(struct l2cap_chan *chan) { - struct sk_buff *skb, *tx_skb; - u16 fcs; - u32 control; - int nsent = 0; + struct l2cap_ctrl control; + struct sk_buff *skb; + struct sk_buff *tx_skb; + u16 seq; - if (chan->state != BT_CONNECTED) - return -ENOTCONN; + BT_DBG("chan %p", chan); if (test_bit(CONN_REMOTE_BUSY, &chan->conn_state)) - return 0; + return; - while ((skb = chan->tx_send_head) && (!l2cap_tx_window_full(chan))) { + if (__chan_is_moving(chan)) + return; - if (bt_cb(skb)->control.retries == chan->remote_max_tx && - chan->remote_max_tx) { - l2cap_send_disconn_req(chan->conn, chan, ECONNABORTED); - break; - } + while (chan->retrans_list.head != L2CAP_SEQ_LIST_CLEAR) { + seq = l2cap_seq_list_pop(&chan->retrans_list); - tx_skb = skb_clone(skb, GFP_ATOMIC); + skb = l2cap_ertm_seq_in_queue(&chan->tx_q, seq); + if (!skb) { + BT_DBG("Error: Can't retransmit seq %d, frame missing", + seq); + continue; + } bt_cb(skb)->control.retries++; + control = bt_cb(skb)->control; - control = __get_control(chan, tx_skb->data + L2CAP_HDR_SIZE); - control &= __get_sar_mask(chan); + 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.reqseq = chan->buffer_seq; if (test_and_clear_bit(CONN_SEND_FBIT, &chan->conn_state)) - control |= __set_ctrl_final(chan); + control.final = 1; + else + control.final = 0; + + 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); + } - control |= __set_reqseq(chan, chan->buffer_seq); - control |= __set_txseq(chan, chan->next_tx_seq); - control |= __set_ctrl_sar(chan, bt_cb(skb)->control.sar); + if (!tx_skb) { + l2cap_seq_list_clear(&chan->retrans_list); + break; + } - __put_control(chan, control, tx_skb->data + L2CAP_HDR_SIZE); + /* 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 (chan->fcs == L2CAP_FCS_CRC16) { - fcs = crc16(0, (u8 *)skb->data, - tx_skb->len - L2CAP_FCS_SIZE); - put_unaligned_le16(fcs, skb->data + - tx_skb->len - L2CAP_FCS_SIZE); + u16 fcs = crc16(0, (u8 *) tx_skb->data, tx_skb->len); + put_unaligned_le16(fcs, skb_put(tx_skb, + L2CAP_FCS_SIZE)); } l2cap_do_send(chan, tx_skb); - __set_retrans_timer(chan); - - bt_cb(skb)->control.txseq = chan->next_tx_seq; - - chan->next_tx_seq = __next_seq(chan, chan->next_tx_seq); - - if (bt_cb(skb)->control.retries == 1) { - chan->unacked_frames++; + BT_DBG("Resent txseq %d", control.txseq); - if (!nsent++) - __clear_ack_timer(chan); - } - - chan->frames_sent++; - - 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); + chan->last_acked_seq = chan->buffer_seq; } - - return nsent; } -static int l2cap_retransmit_frames(struct l2cap_chan *chan) +static void l2cap_retransmit(struct l2cap_chan *chan, + struct l2cap_ctrl *control) { - int ret; + BT_DBG("chan %p, control %p", chan, control); - if (!skb_queue_empty(&chan->tx_q)) - chan->tx_send_head = chan->tx_q.next; - - chan->next_tx_seq = chan->expected_ack_seq; - ret = l2cap_ertm_send(chan); - return ret; + l2cap_seq_list_append(&chan->retrans_list, control->reqseq); + l2cap_ertm_resend(chan); } -static void __l2cap_send_ack(struct l2cap_chan *chan) +static void l2cap_retransmit_all(struct l2cap_chan *chan, + struct l2cap_ctrl *control) { - u32 control = 0; + struct sk_buff *skb; - control |= __set_reqseq(chan, chan->buffer_seq); + BT_DBG("chan %p, control %p", chan, control); - if (test_bit(CONN_LOCAL_BUSY, &chan->conn_state)) { - control |= __set_ctrl_super(chan, L2CAP_SUPER_RNR); - set_bit(CONN_RNR_SENT, &chan->conn_state); - l2cap_send_sframe(chan, control); - return; - } + if (control->poll) + set_bit(CONN_SEND_FBIT, &chan->conn_state); - if (l2cap_ertm_send(chan) > 0) + l2cap_seq_list_clear(&chan->retrans_list); + + if (test_bit(CONN_REMOTE_BUSY, &chan->conn_state)) return; - control |= __set_ctrl_super(chan, L2CAP_SUPER_RR); - l2cap_send_sframe(chan, 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_ack(struct l2cap_chan *chan) { - __clear_ack_timer(chan); - __l2cap_send_ack(chan); -} + struct l2cap_ctrl control; + u16 frames_to_ack = __seq_offset(chan, chan->buffer_seq, + chan->last_acked_seq); + int threshold; -static void l2cap_send_srejtail(struct l2cap_chan *chan) -{ - struct srej_list *tail; - u32 control; + BT_DBG("chan %p last_acked_seq %d buffer_seq %d", + chan, chan->last_acked_seq, chan->buffer_seq); - control = __set_ctrl_super(chan, L2CAP_SUPER_SREJ); - control |= __set_ctrl_final(chan); + memset(&control, 0, sizeof(control)); + control.sframe = 1; - tail = list_entry((&chan->srej_l)->prev, struct srej_list, list); - control |= __set_reqseq(chan, tail->tx_seq); + 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; + } + + /* 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(chan, control); + if (frames_to_ack) + __set_ack_timer(chan); + } } static inline int l2cap_skbuff_fromiovec(struct l2cap_chan *chan, @@ -1876,15 +2156,16 @@ static inline int l2cap_skbuff_fromiovec(struct l2cap_chan *chan, } static struct sk_buff *l2cap_create_connless_pdu(struct l2cap_chan *chan, - struct msghdr *msg, size_t len, - u32 priority) + struct msghdr *msg, size_t len, + u32 priority) { struct l2cap_conn *conn = chan->conn; struct sk_buff *skb; int err, count, hlen = L2CAP_HDR_SIZE + L2CAP_PSMLEN_SIZE; struct l2cap_hdr *lh; - BT_DBG("chan %p len %d priority %u", chan, (int)len, priority); + 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); @@ -1899,7 +2180,7 @@ static struct sk_buff *l2cap_create_connless_pdu(struct l2cap_chan *chan, lh = (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE); lh->cid = cpu_to_le16(chan->dcid); lh->len = cpu_to_le16(len + L2CAP_PSMLEN_SIZE); - put_unaligned(chan->psm, skb_put(skb, L2CAP_PSMLEN_SIZE)); + put_unaligned(chan->psm, (__le16 *) skb_put(skb, L2CAP_PSMLEN_SIZE)); err = l2cap_skbuff_fromiovec(chan, msg, len, count, skb); if (unlikely(err < 0)) { @@ -1910,15 +2191,15 @@ static struct sk_buff *l2cap_create_connless_pdu(struct l2cap_chan *chan, } static struct sk_buff *l2cap_create_basic_pdu(struct l2cap_chan *chan, - struct msghdr *msg, size_t len, - u32 priority) + struct msghdr *msg, size_t len, + u32 priority) { struct l2cap_conn *conn = chan->conn; struct sk_buff *skb; int err, count; struct l2cap_hdr *lh; - BT_DBG("chan %p len %d", chan, (int)len); + BT_DBG("chan %p len %zu", chan, len); count = min_t(unsigned int, (conn->mtu - L2CAP_HDR_SIZE), len); @@ -1943,23 +2224,20 @@ static struct sk_buff *l2cap_create_basic_pdu(struct l2cap_chan *chan, } static struct sk_buff *l2cap_create_iframe_pdu(struct l2cap_chan *chan, - struct msghdr *msg, size_t len, - u16 sdulen) + 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 %d", chan, (int)len); + BT_DBG("chan %p len %zu", chan, len); if (!conn) return ERR_PTR(-ENOTCONN); - if (test_bit(FLAG_EXT_CTRL, &chan->flags)) - hlen = L2CAP_EXT_HDR_SIZE; - else - hlen = L2CAP_ENH_HDR_SIZE; + hlen = __ertm_hdr_size(chan); if (sdulen) hlen += L2CAP_SDULEN_SIZE; @@ -1979,7 +2257,11 @@ static struct sk_buff *l2cap_create_iframe_pdu(struct l2cap_chan *chan, lh->cid = cpu_to_le16(chan->dcid); lh->len = cpu_to_le16(len + (hlen - L2CAP_HDR_SIZE)); - __put_control(chan, 0, skb_put(skb, __ctrl_size(chan))); + /* 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, L2CAP_SDULEN_SIZE)); @@ -1990,9 +2272,7 @@ static struct sk_buff *l2cap_create_iframe_pdu(struct l2cap_chan *chan, return ERR_PTR(err); } - if (chan->fcs == L2CAP_FCS_CRC16) - put_unaligned_le16(0, skb_put(skb, L2CAP_FCS_SIZE)); - + bt_cb(skb)->control.fcs = chan->fcs; bt_cb(skb)->control.retries = 0; return skb; } @@ -2004,10 +2284,9 @@ static int l2cap_segment_sdu(struct l2cap_chan *chan, struct sk_buff *skb; u16 sdu_len; size_t pdu_len; - int err = 0; u8 sar; - BT_DBG("chan %p, msg %p, len %d", chan, msg, (int)len); + 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 @@ -2017,10 +2296,15 @@ static int l2cap_segment_sdu(struct l2cap_chan *chan, /* PDU size is derived from the HCI MTU */ pdu_len = chan->conn->mtu; - pdu_len = min_t(size_t, pdu_len, L2CAP_BREDR_MAX_PAYLOAD); + /* 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. */ - pdu_len -= L2CAP_EXT_HDR_SIZE + L2CAP_FCS_SIZE; + if (chan->fcs) + pdu_len -= L2CAP_FCS_SIZE; + + pdu_len -= __ertm_hdr_size(chan); /* Remote device may have requested smaller PDUs */ pdu_len = min_t(size_t, pdu_len, chan->remote_mps); @@ -2060,27 +2344,155 @@ static int l2cap_segment_sdu(struct l2cap_chan *chan, } } - return err; + return 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(seg_queue); + return PTR_ERR(skb); + } + + __skb_queue_tail(seg_queue, skb); + + len -= pdu_len; + + if (sdu_len) { + sdu_len = 0; + pdu_len += L2CAP_SDULEN_SIZE; + } + } + + return 0; } int l2cap_chan_send(struct l2cap_chan *chan, struct msghdr *msg, size_t len, - u32 priority) + u32 priority) { struct sk_buff *skb; int err; struct sk_buff_head seg_queue; + if (!chan->conn) + return -ENOTCONN; + /* 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); + /* Channel lock is released before requesting new skb and then + * reacquired thus we need to recheck channel state. + */ + 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) @@ -2091,6 +2503,14 @@ int l2cap_chan_send(struct l2cap_chan *chan, struct msghdr *msg, size_t len, 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. + */ + if (chan->state != BT_CONNECTED) { + kfree_skb(skb); + return -ENOTCONN; + } + l2cap_do_send(chan, skb); err = len; break; @@ -2122,17 +2542,12 @@ int l2cap_chan_send(struct l2cap_chan *chan, struct msghdr *msg, size_t len, if (err) break; - if (chan->mode == L2CAP_MODE_ERTM && chan->tx_send_head == NULL) - chan->tx_send_head = seg_queue.next; - skb_queue_splice_tail_init(&seg_queue, &chan->tx_q); - if (chan->mode == L2CAP_MODE_ERTM) - err = l2cap_ertm_send(chan); + l2cap_tx(chan, NULL, &seg_queue, L2CAP_EV_DATA_REQUEST); else - l2cap_streaming_send(chan); + l2cap_streaming_send(chan, &seg_queue); - if (err >= 0) - err = len; + err = len; /* If the skbs were not queued for sending, they'll still be in * seg_queue and need to be purged. @@ -2148,6 +2563,296 @@ int l2cap_chan_send(struct l2cap_chan *chan, struct msghdr *msg, size_t len, 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) { @@ -2159,18 +2864,17 @@ static void l2cap_raw_recv(struct l2cap_conn *conn, struct sk_buff *skb) mutex_lock(&conn->chan_lock); list_for_each_entry(chan, &conn->chan_l, list) { - struct sock *sk = chan->sk; 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 (chan->ops->recv(chan->data, nskb)) + if (chan->ops->recv(chan, nskb)) kfree_skb(nskb); } @@ -2178,21 +2882,24 @@ static void l2cap_raw_recv(struct l2cap_conn *conn, struct sk_buff *skb) } /* ---- 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; @@ -2222,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; @@ -2241,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; @@ -2270,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; } @@ -2278,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; @@ -2315,7 +3023,7 @@ static void l2cap_add_opt_efs(void **ptr, struct l2cap_chan *chan) 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_DEFAULT_FLUSH_TO); + efs.flush_to = cpu_to_le32(L2CAP_EFS_DEFAULT_FLUSH_TO); break; case L2CAP_MODE_STREAMING: @@ -2332,26 +3040,30 @@ static void l2cap_add_opt_efs(void **ptr, struct l2cap_chan *chan) } l2cap_add_conf_opt(ptr, L2CAP_CONF_EFS, sizeof(efs), - (unsigned long) &efs); + (unsigned long) &efs); } static void l2cap_ack_timeout(struct work_struct *work) { struct l2cap_chan *chan = container_of(work, struct l2cap_chan, - ack_timer.work); + ack_timer.work); + u16 frames_to_ack; BT_DBG("chan %p", chan); l2cap_chan_lock(chan); - __l2cap_send_ack(chan); + frames_to_ack = __seq_offset(chan, chan->buffer_seq, + chan->last_acked_seq); - l2cap_chan_unlock(chan); + if (frames_to_ack) + l2cap_send_rr_or_rnr(chan, 0); + l2cap_chan_unlock(chan); l2cap_chan_put(chan); } -static inline int l2cap_ertm_init(struct l2cap_chan *chan) +int l2cap_ertm_init(struct l2cap_chan *chan) { int err; @@ -2360,7 +3072,6 @@ static inline int l2cap_ertm_init(struct l2cap_chan *chan) chan->expected_ack_seq = 0; chan->unacked_frames = 0; chan->buffer_seq = 0; - chan->num_acked = 0; chan->frames_sent = 0; chan->last_acked_seq = 0; chan->sdu = NULL; @@ -2369,6 +3080,11 @@ static inline int l2cap_ertm_init(struct l2cap_chan *chan) 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; @@ -2381,12 +3097,15 @@ static inline int l2cap_ertm_init(struct l2cap_chan *chan) skb_queue_head_init(&chan->srej_q); - INIT_LIST_HEAD(&chan->srej_l); err = l2cap_seq_list_init(&chan->srej_list, chan->tx_win); if (err < 0) return err; - return l2cap_seq_list_init(&chan->retrans_list, chan->remote_tx_win); + 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) @@ -2402,28 +3121,67 @@ static inline __u8 l2cap_select_mode(__u8 mode, __u16 remote_feat_mask) } } -static inline bool __l2cap_ews_supported(struct l2cap_chan *chan) +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 enable_hs && chan->conn->feat_mask & L2CAP_FEAT_EXT_WINDOW; + return conn->hs_enabled && conn->feat_mask & L2CAP_FEAT_EXT_FLOW; } -static inline bool __l2cap_efs_supported(struct l2cap_chan *chan) +static void __l2cap_set_ertm_timeouts(struct l2cap_chan *chan, + struct l2cap_conf_rfc *rfc) { - return enable_hs && chan->conn->feat_mask & L2CAP_FEAT_EXT_FLOW; + 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)) { + __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); + 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) @@ -2444,7 +3202,7 @@ static int l2cap_build_conf_req(struct l2cap_chan *chan, void *data) if (test_bit(CONF_STATE2_DEVICE, &chan->conf_state)) break; - if (__l2cap_efs_supported(chan)) + if (__l2cap_efs_supported(chan->conn)) set_bit(FLAG_EFS_ENABLE, &chan->flags); /* fall through */ @@ -2460,7 +3218,7 @@ done: switch (chan->mode) { case L2CAP_MODE_BASIC: if (!(chan->conn->feat_mask & L2CAP_FEAT_ERTM) && - !(chan->conn->feat_mask & L2CAP_FEAT_STREAMING)) + !(chan->conn->feat_mask & L2CAP_FEAT_STREAMING)) break; rfc.mode = L2CAP_MODE_BASIC; @@ -2471,47 +3229,46 @@ 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.max_transmit = chan->max_tx; - rfc.retrans_timeout = 0; - rfc.monitor_timeout = 0; + + __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); + 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_DEFAULT_TX_WINDOW); l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC, sizeof(rfc), - (unsigned long) &rfc); + (unsigned long) &rfc); if (test_bit(FLAG_EFS_ENABLE, &chan->flags)) l2cap_add_opt_efs(&ptr, chan); - if (!(chan->conn->feat_mask & L2CAP_FEAT_FCS)) - break; - - if (chan->fcs == L2CAP_FCS_NONE || - test_bit(CONF_NO_FCS_RECV, &chan->conf_state)) { - chan->fcs = L2CAP_FCS_NONE; - l2cap_add_conf_opt(&ptr, L2CAP_CONF_FCS, 1, chan->fcs); - } - if (test_bit(FLAG_EXT_CTRL, &chan->flags)) l2cap_add_conf_opt(&ptr, L2CAP_CONF_EWS, 2, - chan->tx_win); + 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; @@ -2519,25 +3276,23 @@ done: rfc.monitor_timeout = 0; size = min_t(u16, L2CAP_DEFAULT_MAX_PDU_SIZE, chan->conn->mtu - - L2CAP_EXT_HDR_SIZE - - L2CAP_SDULEN_SIZE - - L2CAP_FCS_SIZE); + 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 (test_bit(FLAG_EFS_ENABLE, &chan->flags)) l2cap_add_opt_efs(&ptr, chan); - if (!(chan->conn->feat_mask & L2CAP_FEAT_FCS)) - break; - - if (chan->fcs == L2CAP_FCS_NONE || - test_bit(CONF_NO_FCS_RECV, &chan->conf_state)) { - chan->fcs = L2CAP_FCS_NONE; - l2cap_add_conf_opt(&ptr, L2CAP_CONF_FCS, 1, chan->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; } @@ -2589,7 +3344,7 @@ static int l2cap_parse_conf_req(struct l2cap_chan *chan, void *data) case L2CAP_CONF_FCS: if (val == L2CAP_FCS_NONE) - set_bit(CONF_NO_FCS_RECV, &chan->conf_state); + set_bit(CONF_RECV_NO_FCS, &chan->conf_state); break; case L2CAP_CONF_EFS: @@ -2599,7 +3354,7 @@ static int l2cap_parse_conf_req(struct l2cap_chan *chan, void *data) break; case L2CAP_CONF_EWS: - if (!enable_hs) + if (!chan->conn->hs_enabled) return -ECONNREFUSED; set_bit(FLAG_EXT_CTRL, &chan->flags); @@ -2626,12 +3381,12 @@ static int l2cap_parse_conf_req(struct l2cap_chan *chan, void *data) case L2CAP_MODE_ERTM: if (!test_bit(CONF_STATE2_DEVICE, &chan->conf_state)) { chan->mode = l2cap_select_mode(rfc.mode, - chan->conn->feat_mask); + chan->conn->feat_mask); break; } if (remote_efs) { - if (__l2cap_efs_supported(chan)) + if (__l2cap_efs_supported(chan->conn)) set_bit(FLAG_EFS_ENABLE, &chan->flags); else return -ECONNREFUSED; @@ -2651,8 +3406,8 @@ done: 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) { @@ -2669,8 +3424,8 @@ done: if (remote_efs) { if (chan->local_stype != L2CAP_SERV_NOTRAFIC && - efs.stype != L2CAP_SERV_NOTRAFIC && - efs.stype != chan->local_stype) { + efs.stype != L2CAP_SERV_NOTRAFIC && + efs.stype != chan->local_stype) { result = L2CAP_CONF_UNACCEPT; @@ -2678,8 +3433,8 @@ done: return -ECONNREFUSED; l2cap_add_conf_opt(&ptr, L2CAP_CONF_EFS, - sizeof(efs), - (unsigned long) &efs); + sizeof(efs), + (unsigned long) &efs); } else { /* Send PENDING Conf Rsp */ result = L2CAP_CONF_PENDING; @@ -2702,51 +3457,45 @@ done: chan->remote_max_tx = rfc.max_transmit; size = min_t(u16, le16_to_cpu(rfc.max_pdu_size), - chan->conn->mtu - - L2CAP_EXT_HDR_SIZE - - L2CAP_SDULEN_SIZE - - L2CAP_FCS_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 = - __constant_cpu_to_le16(L2CAP_DEFAULT_RETRANS_TO); - rfc.monitor_timeout = - __constant_cpu_to_le16(L2CAP_DEFAULT_MONITOR_TO); + __l2cap_set_ertm_timeouts(chan, &rfc); 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); + le32_to_cpu(efs.flush_to); chan->remote_acc_lat = - le32_to_cpu(efs.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); + sizeof(efs), + (unsigned long) &efs); } break; case L2CAP_MODE_STREAMING: size = min_t(u16, le16_to_cpu(rfc.max_pdu_size), - chan->conn->mtu - - L2CAP_EXT_HDR_SIZE - - L2CAP_SDULEN_SIZE - - L2CAP_FCS_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; set_bit(CONF_MODE_DONE, &chan->conf_state); - 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; @@ -2762,12 +3511,13 @@ done: } 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 l2cap_chan *chan, 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_conf_req *req = data; void *ptr = req->data; @@ -2794,7 +3544,7 @@ static int l2cap_parse_conf_rsp(struct l2cap_chan *chan, void *rsp, int len, voi case L2CAP_CONF_FLUSH_TO: chan->flush_to = val; l2cap_add_conf_opt(&ptr, L2CAP_CONF_FLUSH_TO, - 2, chan->flush_to); + 2, chan->flush_to); break; case L2CAP_CONF_RFC: @@ -2802,20 +3552,19 @@ static int l2cap_parse_conf_rsp(struct l2cap_chan *chan, void *rsp, int len, voi memcpy(&rfc, (void *)val, olen); if (test_bit(CONF_STATE2_DEVICE, &chan->conf_state) && - rfc.mode != chan->mode) + rfc.mode != chan->mode) return -ECONNREFUSED; 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->tx_win = min_t(u16, val, - L2CAP_DEFAULT_EXT_WINDOW); + chan->ack_win = min_t(u16, val, chan->ack_win); l2cap_add_conf_opt(&ptr, L2CAP_CONF_EWS, 2, - chan->tx_win); + chan->tx_win); break; case L2CAP_CONF_EFS: @@ -2823,12 +3572,19 @@ static int l2cap_parse_conf_rsp(struct l2cap_chan *chan, void *rsp, int len, voi memcpy(&efs, (void *)val, olen); if (chan->local_stype != L2CAP_SERV_NOTRAFIC && - efs.stype != L2CAP_SERV_NOTRAFIC && - efs.stype != chan->local_stype) + 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); + 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; } } @@ -2844,14 +3600,17 @@ static int l2cap_parse_conf_rsp(struct l2cap_chan *chan, void *rsp, int len, voi 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); + 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); + le32_to_cpu(efs.flush_to); } break; @@ -2861,12 +3620,13 @@ static int l2cap_parse_conf_rsp(struct l2cap_chan *chan, void *rsp, int len, voi } req->dcid = cpu_to_le16(chan->dcid); - req->flags = cpu_to_le16(0x0000); + req->flags = cpu_to_le16(0); return ptr - data; } -static int l2cap_build_conf_rsp(struct l2cap_chan *chan, 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; @@ -2880,24 +3640,49 @@ static int l2cap_build_conf_rsp(struct l2cap_chan *chan, void *data, u16 result, return ptr - data; } +void __l2cap_le_connect_rsp_defer(struct l2cap_chan *chan) +{ + 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); - l2cap_send_cmd(conn, chan->ident, - L2CAP_CONN_RSP, sizeof(rsp), &rsp); + + 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; l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ, - l2cap_build_conf_req(chan, buf), buf); + l2cap_build_conf_req(chan, buf), buf); chan->num_conf_req++; } @@ -2905,7 +3690,17 @@ static void l2cap_conf_rfc_get(struct l2cap_chan *chan, void *rsp, int len) { int type, olen; unsigned long val; - struct l2cap_conf_rfc rfc; + /* 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); @@ -2915,47 +3710,47 @@ static void l2cap_conf_rfc_get(struct l2cap_chan *chan, void *rsp, int len) while (len >= L2CAP_CONF_OPT_SIZE) { len -= l2cap_get_conf_opt(&rsp, &type, &olen, &val); - if (type != L2CAP_CONF_RFC) - continue; - - if (olen != sizeof(rfc)) + switch (type) { + case L2CAP_CONF_RFC: + if (olen == sizeof(rfc)) + memcpy(&rfc, (void *)val, olen); break; - - memcpy(&rfc, (void *)val, olen); - goto done; + case L2CAP_CONF_EWS: + txwin_ext = val; + break; + } } - /* Use sane default values in case a misbehaving remote device - * did not send an RFC option. - */ - rfc.mode = chan->mode; - rfc.retrans_timeout = cpu_to_le16(L2CAP_DEFAULT_RETRANS_TO); - rfc.monitor_timeout = cpu_to_le16(L2CAP_DEFAULT_MONITOR_TO); - rfc.max_pdu_size = cpu_to_le16(chan->imtu); - - BT_ERR("Expected RFC option was not found, using defaults"); - -done: switch (rfc.mode) { case L2CAP_MODE_ERTM: 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); + 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: 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_unk *rej = (struct l2cap_cmd_rej_unk *) data; + if (cmd_len < sizeof(*rej)) + return -EPROTO; + if (rej->reason != L2CAP_REJ_NOT_UNDERSTOOD) return 0; if ((conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_SENT) && - cmd->ident == conn->info_ident) { + cmd->ident == conn->info_ident) { cancel_delayed_work(&conn->info_timer); conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_DONE; @@ -2967,12 +3762,13 @@ 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_conn_req *req = (struct l2cap_conn_req *) data; struct l2cap_conn_rsp rsp; struct l2cap_chan *chan = NULL, *pchan; - struct sock *parent, *sk = NULL; int result, status = L2CAP_CS_NO_INFO; u16 dcid = 0, scid = __le16_to_cpu(req->scid); @@ -2981,20 +3777,19 @@ static inline int l2cap_connect_req(struct l2cap_conn *conn, struct l2cap_cmd_hd BT_DBG("psm 0x%2.2x scid 0x%4.4x", __le16_to_cpu(psm), scid); /* Check if we have socket listening on psm */ - pchan = l2cap_global_chan_by_psm(BT_LISTEN, psm, conn->src, conn->dst); + 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; } - parent = pchan->sk; - mutex_lock(&conn->chan_lock); - lock_sock(parent); + 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)) { + 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; @@ -3002,67 +3797,71 @@ static inline int l2cap_connect_req(struct l2cap_conn *conn, struct l2cap_cmd_hd 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; - } - chan = pchan->ops->new_connection(pchan->data); + chan = pchan->ops->new_connection(pchan); if (!chan) goto response; - sk = chan->sk; - - /* Check if we already have channel with that dcid */ - if (__l2cap_get_chan_by_dcid(conn, scid)) { - sock_set_flag(sk, SOCK_ZAPPED); - chan->ops->close(chan->data); - goto response; - } - - hci_conn_hold(conn->hcon); + /* 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; - bacpy(&bt_sk(sk)->src, conn->src); - bacpy(&bt_sk(sk)->dst, conn->dst); + 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; - - bt_accept_enqueue(parent, sk); + chan->local_amp_id = amp_id; __l2cap_chan_add(conn, chan); dcid = chan->scid; - __set_chan_timer(chan, sk->sk_sndtimeo); + __set_chan_timer(chan, chan->ops->get_sndtimeo(chan)); chan->ident = cmd->ident; if (conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_DONE) { if (l2cap_chan_check_security(chan)) { - if (test_bit(BT_SK_DEFER_SETUP, &bt_sk(sk)->flags)) { - __l2cap_state_change(chan, BT_CONNECT2); + 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 { - __l2cap_state_change(chan, 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 { - __l2cap_state_change(chan, BT_CONNECT2); + l2cap_state_change(chan, BT_CONNECT2); result = L2CAP_CR_PEND; status = L2CAP_CS_AUTHEN_PEND; } } else { - __l2cap_state_change(chan, BT_CONNECT2); + l2cap_state_change(chan, BT_CONNECT2); result = L2CAP_CR_PEND; status = L2CAP_CS_NO_INFO; } response: - release_sock(parent); + l2cap_chan_unlock(pchan); mutex_unlock(&conn->chan_lock); sendresp: @@ -3070,7 +3869,7 @@ sendresp: 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; @@ -3081,23 +3880,46 @@ sendresp: 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 (chan && !test_bit(CONF_REQ_SENT, &chan->conf_state) && - result == L2CAP_CR_SUCCESS) { + result == L2CAP_CR_SUCCESS) { u8 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); + 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; @@ -3105,26 +3927,29 @@ static inline int l2cap_connect_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hd 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); + dcid, scid, result, status); mutex_lock(&conn->chan_lock); if (scid) { chan = __l2cap_get_chan_by_scid(conn, scid); if (!chan) { - err = -EFAULT; + err = -EBADSLT; goto unlock; } } else { chan = __l2cap_get_chan_by_ident(conn, cmd->ident); if (!chan) { - err = -EFAULT; + err = -EBADSLT; goto unlock; } } @@ -3144,7 +3969,7 @@ static inline int l2cap_connect_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hd break; l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ, - l2cap_build_conf_req(chan, req), req); + l2cap_build_conf_req(chan, req), req); chan->num_conf_req++; break; @@ -3172,11 +3997,41 @@ static inline void set_default_fcs(struct l2cap_chan *chan) */ if (chan->mode != L2CAP_MODE_ERTM && chan->mode != L2CAP_MODE_STREAMING) chan->fcs = L2CAP_FCS_NONE; - else if (!test_bit(CONF_NO_FCS_RECV, &chan->conf_state)) + 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; @@ -3184,33 +4039,32 @@ static inline int l2cap_config_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr 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); chan = l2cap_get_chan_by_scid(conn, dcid); - if (!chan) - return -ENOENT; + if (!chan) { + cmd_reject_invalid_cid(conn, cmd->ident, dcid, 0); + return 0; + } if (chan->state != BT_CONFIG && chan->state != BT_CONNECT2) { - struct l2cap_cmd_rej_cid rej; - - rej.reason = cpu_to_le16(L2CAP_REJ_INVALID_CID); - rej.scid = cpu_to_le16(chan->scid); - rej.dcid = cpu_to_le16(chan->dcid); - - l2cap_send_cmd(conn, cmd->ident, L2CAP_COMMAND_REJ, - sizeof(rej), &rej); + 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 (len < 0 || chan->conf_len + len > sizeof(chan->conf_req)) { + if (chan->conf_len + len > sizeof(chan->conf_req)) { l2cap_send_cmd(conn, cmd->ident, L2CAP_CONF_RSP, - l2cap_build_conf_rsp(chan, rsp, - L2CAP_CONF_REJECT, flags), rsp); + l2cap_build_conf_rsp(chan, rsp, + L2CAP_CONF_REJECT, flags), rsp); goto unlock; } @@ -3218,21 +4072,22 @@ static inline int l2cap_config_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr 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(chan, 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(chan, rsp); if (len < 0) { - l2cap_send_disconn_req(conn, chan, ECONNRESET); + l2cap_send_disconn_req(chan, ECONNRESET); goto unlock; } + chan->ident = cmd->ident; l2cap_send_cmd(conn, cmd->ident, L2CAP_CONF_RSP, len, rsp); chan->num_conf_rsp++; @@ -3245,14 +4100,12 @@ static inline int l2cap_config_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr if (test_bit(CONF_INPUT_DONE, &chan->conf_state)) { set_default_fcs(chan); - l2cap_state_change(chan, BT_CONNECTED); - if (chan->mode == L2CAP_MODE_ERTM || chan->mode == L2CAP_MODE_STREAMING) err = l2cap_ertm_init(chan); if (err < 0) - l2cap_send_disconn_req(chan->conn, chan, -err); + l2cap_send_disconn_req(chan, -err); else l2cap_chan_ready(chan); @@ -3262,23 +4115,22 @@ static inline int l2cap_config_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr if (!test_and_set_bit(CONF_REQ_SENT, &chan->conf_state)) { u8 buf[64]; l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ, - l2cap_build_conf_req(chan, buf), buf); + 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)) { + test_bit(CONF_LOC_CONF_PEND, &chan->conf_state)) { /* check compatibility */ - clear_bit(CONF_LOC_CONF_PEND, &chan->conf_state); - set_bit(CONF_OUTPUT_DONE, &chan->conf_state); - - l2cap_send_cmd(conn, cmd->ident, L2CAP_CONF_RSP, - l2cap_build_conf_rsp(chan, rsp, - L2CAP_CONF_SUCCESS, 0x0000), rsp); + /* 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: @@ -3286,14 +4138,19 @@ unlock: 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 l2cap_chan *chan; - int len = le16_to_cpu(cmd->len) - sizeof(*rsp); + 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); @@ -3318,20 +4175,21 @@ static inline int l2cap_config_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hdr char buf[64]; len = l2cap_parse_conf_rsp(chan, rsp->data, len, - buf, &result); + buf, &result); if (len < 0) { - l2cap_send_disconn_req(conn, chan, ECONNRESET); + l2cap_send_disconn_req(chan, ECONNRESET); goto done; } - /* check compatibility */ - - clear_bit(CONF_LOC_CONF_PEND, &chan->conf_state); - set_bit(CONF_OUTPUT_DONE, &chan->conf_state); - - l2cap_send_cmd(conn, cmd->ident, L2CAP_CONF_RSP, - l2cap_build_conf_rsp(chan, buf, - L2CAP_CONF_SUCCESS, 0x0000), buf); + 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; @@ -3340,21 +4198,21 @@ static inline int l2cap_config_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hdr char req[64]; if (len > sizeof(req) - sizeof(struct l2cap_conf_req)) { - l2cap_send_disconn_req(conn, chan, 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(chan, rsp->data, len, - req, &result); + req, &result); if (len < 0) { - l2cap_send_disconn_req(conn, chan, ECONNRESET); + l2cap_send_disconn_req(chan, ECONNRESET); goto done; } l2cap_send_cmd(conn, l2cap_get_ident(conn), - L2CAP_CONF_REQ, len, req); + L2CAP_CONF_REQ, len, req); chan->num_conf_req++; if (result != L2CAP_CONF_SUCCESS) goto done; @@ -3365,11 +4223,11 @@ static inline int l2cap_config_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hdr l2cap_chan_set_err(chan, ECONNRESET); __set_chan_timer(chan, L2CAP_DISC_REJ_TIMEOUT); - l2cap_send_disconn_req(conn, chan, ECONNRESET); + l2cap_send_disconn_req(chan, ECONNRESET); goto done; } - if (flags & 0x01) + if (flags & L2CAP_CONF_FLAG_CONTINUATION) goto done; set_bit(CONF_INPUT_DONE, &chan->conf_state); @@ -3377,13 +4235,12 @@ static inline int l2cap_config_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hdr if (test_bit(CONF_OUTPUT_DONE, &chan->conf_state)) { set_default_fcs(chan); - l2cap_state_change(chan, BT_CONNECTED); if (chan->mode == L2CAP_MODE_ERTM || chan->mode == L2CAP_MODE_STREAMING) err = l2cap_ertm_init(chan); if (err < 0) - l2cap_send_disconn_req(chan->conn, chan, -err); + l2cap_send_disconn_req(chan, -err); else l2cap_chan_ready(chan); } @@ -3393,13 +4250,17 @@ done: 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 l2cap_chan *chan; - struct sock *sk; + + if (cmd_len != sizeof(*req)) + return -EPROTO; scid = __le16_to_cpu(req->scid); dcid = __le16_to_cpu(req->dcid); @@ -3411,27 +4272,24 @@ static inline int l2cap_disconnect_req(struct l2cap_conn *conn, struct l2cap_cmd 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; } l2cap_chan_lock(chan); - sk = chan->sk; - 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); - lock_sock(sk); - sk->sk_shutdown = SHUTDOWN_MASK; - release_sock(sk); + chan->ops->set_shutdown(chan); l2cap_chan_hold(chan); l2cap_chan_del(chan, ECONNRESET); l2cap_chan_unlock(chan); - chan->ops->close(chan->data); + chan->ops->close(chan); l2cap_chan_put(chan); mutex_unlock(&conn->chan_lock); @@ -3439,12 +4297,17 @@ static inline int l2cap_disconnect_req(struct l2cap_conn *conn, struct l2cap_cmd 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 l2cap_chan *chan; + if (cmd_len != sizeof(*rsp)) + return -EPROTO; + scid = __le16_to_cpu(rsp->scid); dcid = __le16_to_cpu(rsp->dcid); @@ -3465,7 +4328,7 @@ static inline int l2cap_disconnect_rsp(struct l2cap_conn *conn, struct l2cap_cmd l2cap_chan_unlock(chan); - chan->ops->close(chan->data); + chan->ops->close(chan); l2cap_chan_put(chan); mutex_unlock(&conn->chan_lock); @@ -3473,11 +4336,16 @@ static inline int l2cap_disconnect_rsp(struct l2cap_conn *conn, struct l2cap_cmd 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); @@ -3490,19 +4358,19 @@ 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; - if (enable_hs) + | L2CAP_FEAT_FCS; + if (conn->hs_enabled) feat_mask |= L2CAP_FEAT_EXT_FLOW - | L2CAP_FEAT_EXT_WINDOW; + | 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 (enable_hs) + if (conn->hs_enabled) l2cap_fixed_chan[0] |= L2CAP_FC_A2MP; else l2cap_fixed_chan[0] &= ~L2CAP_FC_A2MP; @@ -3510,24 +4378,29 @@ static inline int l2cap_information_req(struct l2cap_conn *conn, struct l2cap_cm rsp->type = cpu_to_le16(L2CAP_IT_FIXED_CHAN); rsp->result = cpu_to_le16(L2CAP_IR_SUCCESS); memcpy(rsp->data, l2cap_fixed_chan, sizeof(l2cap_fixed_chan)); - 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 { 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); @@ -3535,7 +4408,7 @@ static inline int l2cap_information_rsp(struct l2cap_conn *conn, struct l2cap_cm /* 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) + conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_DONE) return 0; cancel_delayed_work(&conn->info_timer); @@ -3560,7 +4433,7 @@ 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; @@ -3581,30 +4454,76 @@ static inline int l2cap_information_rsp(struct l2cap_conn *conn, struct l2cap_cm return 0; } -static inline int l2cap_create_channel_req(struct l2cap_conn *conn, - struct l2cap_cmd_hdr *cmd, u16 cmd_len, - void *data) +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 (!enable_hs) + if (!conn->hs_enabled) return -EINVAL; psm = le16_to_cpu(req->psm); scid = le16_to_cpu(req->scid); - BT_DBG("psm %d, scid %d, amp_id %d", psm, scid, req->amp_id); + 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); - /* Placeholder: Always reject */ + 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 = __constant_cpu_to_le16(L2CAP_CR_NO_MEM); - rsp.status = __constant_cpu_to_le16(L2CAP_CS_NO_INFO); + 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); @@ -3612,60 +4531,362 @@ static inline int l2cap_create_channel_req(struct l2cap_conn *conn, return 0; } -static inline int l2cap_create_channel_rsp(struct l2cap_conn *conn, - struct l2cap_cmd_hdr *cmd, void *data) +static void l2cap_send_move_chan_req(struct l2cap_chan *chan, u8 dest_amp_id) { - BT_DBG("conn %p", conn); + 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); - return l2cap_connect_rsp(conn, cmd, data); + __set_chan_timer(chan, L2CAP_MOVE_TIMEOUT); } -static void l2cap_send_move_chan_rsp(struct l2cap_conn *conn, u8 ident, - u16 icid, u16 result) +static void l2cap_send_move_chan_rsp(struct l2cap_chan *chan, u16 result) { struct l2cap_move_chan_rsp rsp; - BT_DBG("icid %d, result %d", icid, result); + BT_DBG("chan %p, result 0x%4.4x", chan, result); - rsp.icid = cpu_to_le16(icid); + rsp.icid = cpu_to_le16(chan->dcid); rsp.result = cpu_to_le16(result); - l2cap_send_cmd(conn, ident, L2CAP_MOVE_CHAN_RSP, sizeof(rsp), &rsp); + l2cap_send_cmd(chan->conn, chan->ident, L2CAP_MOVE_CHAN_RSP, + sizeof(rsp), &rsp); } -static void l2cap_send_move_chan_cfm(struct l2cap_conn *conn, - struct l2cap_chan *chan, u16 icid, u16 result) +static void l2cap_send_move_chan_cfm(struct l2cap_chan *chan, u16 result) { struct l2cap_move_chan_cfm cfm; - u8 ident; - BT_DBG("icid %d, result %d", icid, result); + BT_DBG("chan %p, result 0x%4.4x", chan, result); - ident = l2cap_get_ident(conn); - if (chan) - chan->ident = ident; + chan->ident = l2cap_get_ident(chan->conn); - cfm.icid = cpu_to_le16(icid); + cfm.icid = cpu_to_le16(chan->scid); cfm.result = cpu_to_le16(result); - l2cap_send_cmd(conn, ident, L2CAP_MOVE_CHAN_CFM, sizeof(cfm), &cfm); + 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) + u16 icid) { struct l2cap_move_chan_cfm_rsp rsp; - BT_DBG("icid %d", icid); + 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_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; @@ -3674,19 +4895,211 @@ static inline int l2cap_move_channel_req(struct l2cap_conn *conn, icid = le16_to_cpu(req->icid); - BT_DBG("icid %d, dest_amp_id %d", icid, req->dest_amp_id); + BT_DBG("icid 0x%4.4x, dest_amp_id %d", icid, req->dest_amp_id); - if (!enable_hs) + if (!conn->hs_enabled) return -EINVAL; - /* Placeholder: Always refuse */ - l2cap_send_move_chan_rsp(conn, cmd->ident, icid, result); + 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 inline int l2cap_move_channel_rsp(struct l2cap_conn *conn, - struct l2cap_cmd_hdr *cmd, u16 cmd_len, void *data) +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; @@ -3697,18 +5110,22 @@ static inline int l2cap_move_channel_rsp(struct l2cap_conn *conn, icid = le16_to_cpu(rsp->icid); result = le16_to_cpu(rsp->result); - BT_DBG("icid %d, result %d", icid, result); + BT_DBG("icid 0x%4.4x, result 0x%4.4x", icid, result); - /* Placeholder: Always unconfirmed */ - l2cap_send_move_chan_cfm(conn, NULL, icid, L2CAP_MC_UNCONFIRMED); + 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 inline int l2cap_move_channel_confirm(struct l2cap_conn *conn, - struct l2cap_cmd_hdr *cmd, u16 cmd_len, void *data) +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)) @@ -3717,17 +5134,40 @@ static inline int l2cap_move_channel_confirm(struct l2cap_conn *conn, icid = le16_to_cpu(cfm->icid); result = le16_to_cpu(cfm->result); - BT_DBG("icid %d, result %d", icid, 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_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)) @@ -3735,13 +5175,30 @@ static inline int l2cap_move_channel_confirm_rsp(struct l2cap_conn *conn, icid = le16_to_cpu(rsp->icid); - BT_DBG("icid %d", 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; @@ -3762,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; @@ -3784,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)); @@ -3795,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); @@ -3803,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: @@ -3826,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: @@ -3845,27 +5363,23 @@ 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_CREATE_CHAN_RSP: - err = l2cap_create_channel_rsp(conn, cmd, data); - break; - case L2CAP_MOVE_CHAN_REQ: err = l2cap_move_channel_req(conn, cmd, cmd_len, data); break; case L2CAP_MOVE_CHAN_RSP: - err = l2cap_move_channel_rsp(conn, cmd, cmd_len, data); + l2cap_move_channel_rsp(conn, cmd, cmd_len, data); break; case L2CAP_MOVE_CHAN_CFM: @@ -3873,7 +5387,7 @@ static inline int l2cap_bredr_sig_cmd(struct l2cap_conn *conn, break; case L2CAP_MOVE_CHAN_CFM_RSP: - err = l2cap_move_channel_confirm_rsp(conn, cmd, cmd_len, data); + l2cap_move_channel_confirm_rsp(conn, cmd, cmd_len, data); break; default: @@ -3885,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; @@ -3914,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); @@ -3922,32 +5693,30 @@ 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_unk rej; BT_ERR("Wrong link type (%d)", err); - /* FIXME: Map err to a valid reason */ rej.reason = cpu_to_le16(L2CAP_REJ_NOT_UNDERSTOOD); - l2cap_send_cmd(conn, cmd.ident, L2CAP_COMMAND_REJ, sizeof(rej), &rej); + l2cap_send_cmd(conn, cmd.ident, L2CAP_COMMAND_REJ, + sizeof(rej), &rej); } data += cmd_len; len -= cmd_len; } +drop: kfree_skb(skb); } @@ -3972,69 +5741,42 @@ static int l2cap_check_fcs(struct l2cap_chan *chan, struct sk_buff *skb) return 0; } -static inline void l2cap_send_i_or_rr_or_rnr(struct l2cap_chan *chan) +static void l2cap_send_i_or_rr_or_rnr(struct l2cap_chan *chan) { - u32 control = 0; + struct l2cap_ctrl control; - chan->frames_sent = 0; + BT_DBG("chan %p", chan); - control |= __set_reqseq(chan, chan->buffer_seq); + 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 (test_bit(CONN_LOCAL_BUSY, &chan->conn_state)) { - control |= __set_ctrl_super(chan, L2CAP_SUPER_RNR); - l2cap_send_sframe(chan, control); - set_bit(CONN_RNR_SENT, &chan->conn_state); + control.super = L2CAP_SUPER_RNR; + l2cap_send_sframe(chan, &control); } - if (test_bit(CONN_REMOTE_BUSY, &chan->conn_state)) - l2cap_retransmit_frames(chan); + if (test_and_clear_bit(CONN_REMOTE_BUSY, &chan->conn_state) && + chan->unacked_frames > 0) + __set_retrans_timer(chan); + /* Send pending iframes */ l2cap_ertm_send(chan); if (!test_bit(CONN_LOCAL_BUSY, &chan->conn_state) && - chan->frames_sent == 0) { - control |= __set_ctrl_super(chan, L2CAP_SUPER_RR); - l2cap_send_sframe(chan, control); - } -} - -static int l2cap_add_to_srej_queue(struct l2cap_chan *chan, struct sk_buff *skb, u16 tx_seq, u8 sar) -{ - struct sk_buff *next_skb; - int tx_seq_offset, next_tx_seq_offset; - - bt_cb(skb)->control.txseq = tx_seq; - bt_cb(skb)->control.sar = sar; - - next_skb = skb_peek(&chan->srej_q); - - tx_seq_offset = __seq_offset(chan, tx_seq, chan->buffer_seq); - - while (next_skb) { - if (bt_cb(next_skb)->control.txseq == tx_seq) - return -EINVAL; - - next_tx_seq_offset = __seq_offset(chan, - bt_cb(next_skb)->control.txseq, chan->buffer_seq); - - if (next_tx_seq_offset > tx_seq_offset) { - __skb_queue_before(&chan->srej_q, next_skb, skb); - return 0; - } - - if (skb_queue_is_last(&chan->srej_q, next_skb)) - next_skb = NULL; - else - next_skb = skb_queue_next(&chan->srej_q, next_skb); + 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); } - - __skb_queue_tail(&chan->srej_q, skb); - - return 0; } -static void append_skb_frag(struct sk_buff *skb, - struct sk_buff *new_frag, struct sk_buff **last_frag) +static void append_skb_frag(struct sk_buff *skb, struct sk_buff *new_frag, + struct sk_buff **last_frag) { /* skb->len reflects data in skb as well as all fragments * skb->data_len reflects only data in fragments @@ -4052,16 +5794,17 @@ static void append_skb_frag(struct sk_buff *skb, skb->truesize += new_frag->truesize; } -static int l2cap_reassemble_sdu(struct l2cap_chan *chan, struct sk_buff *skb, u32 control) +static int l2cap_reassemble_sdu(struct l2cap_chan *chan, struct sk_buff *skb, + struct l2cap_ctrl *control) { int err = -EINVAL; - switch (__get_ctrl_sar(chan, control)) { + switch (control->sar) { case L2CAP_SAR_UNSEGMENTED: if (chan->sdu) break; - err = chan->ops->recv(chan->data, skb); + err = chan->ops->recv(chan, skb); break; case L2CAP_SAR_START: @@ -4111,7 +5854,7 @@ static int l2cap_reassemble_sdu(struct l2cap_chan *chan, struct sk_buff *skb, u3 if (chan->sdu->len != chan->sdu_len) break; - err = chan->ops->recv(chan->data, chan->sdu); + err = chan->ops->recv(chan, chan->sdu); if (!err) { /* Reassembly complete */ @@ -4133,486 +5876,768 @@ static int l2cap_reassemble_sdu(struct l2cap_chan *chan, struct sk_buff *skb, u3 return err; } -static void l2cap_ertm_enter_local_busy(struct l2cap_chan *chan) +static int l2cap_resegment(struct l2cap_chan *chan) +{ + /* Placeholder */ + return 0; +} + +void l2cap_chan_busy(struct l2cap_chan *chan, int busy) { - BT_DBG("chan %p, Enter local busy", chan); + u8 event; - set_bit(CONN_LOCAL_BUSY, &chan->conn_state); - l2cap_seq_list_clear(&chan->srej_list); + if (chan->mode != L2CAP_MODE_ERTM) + return; - __set_ack_timer(chan); + event = busy ? L2CAP_EV_LOCAL_BUSY_DETECTED : L2CAP_EV_LOCAL_BUSY_CLEAR; + l2cap_tx(chan, NULL, NULL, event); } -static void l2cap_ertm_exit_local_busy(struct l2cap_chan *chan) +static int l2cap_rx_queued_iframes(struct l2cap_chan *chan) { - u32 control; - - if (!test_bit(CONN_RNR_SENT, &chan->conn_state)) - goto done; + int err = 0; + /* Pass sequential frames to l2cap_reassemble_sdu() + * until a gap is encountered. + */ - control = __set_reqseq(chan, chan->buffer_seq); - control |= __set_ctrl_poll(chan); - control |= __set_ctrl_super(chan, L2CAP_SUPER_RR); - l2cap_send_sframe(chan, control); - chan->retry_count = 1; + BT_DBG("chan %p", chan); - __clear_retrans_timer(chan); - __set_monitor_timer(chan); + 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)); - set_bit(CONN_WAIT_F, &chan->conn_state); + skb = l2cap_ertm_seq_in_queue(&chan->srej_q, chan->buffer_seq); -done: - clear_bit(CONN_LOCAL_BUSY, &chan->conn_state); - clear_bit(CONN_RNR_SENT, &chan->conn_state); + if (!skb) + break; - BT_DBG("chan %p, Exit local busy", chan); -} + 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; + } -void l2cap_chan_busy(struct l2cap_chan *chan, int busy) -{ - if (chan->mode == L2CAP_MODE_ERTM) { - if (busy) - l2cap_ertm_enter_local_busy(chan); - else - l2cap_ertm_exit_local_busy(chan); + if (skb_queue_empty(&chan->srej_q)) { + chan->rx_state = L2CAP_RX_STATE_RECV; + l2cap_send_ack(chan); } + + return err; } -static void l2cap_check_srej_gap(struct l2cap_chan *chan, u16 tx_seq) +static void l2cap_handle_srej(struct l2cap_chan *chan, + struct l2cap_ctrl *control) { struct sk_buff *skb; - u32 control; - while ((skb = skb_peek(&chan->srej_q)) && - !test_bit(CONN_LOCAL_BUSY, &chan->conn_state)) { - int err; + BT_DBG("chan %p, control %p", chan, control); - if (bt_cb(skb)->control.txseq != tx_seq) - break; + if (control->reqseq == chan->next_tx_seq) { + BT_DBG("Invalid reqseq %d, disconnecting", control->reqseq); + l2cap_send_disconn_req(chan, ECONNRESET); + return; + } - skb = skb_dequeue(&chan->srej_q); - control = __set_ctrl_sar(chan, bt_cb(skb)->control.sar); - err = l2cap_reassemble_sdu(chan, skb, control); + skb = l2cap_ertm_seq_in_queue(&chan->tx_q, control->reqseq); - if (err < 0) { - l2cap_send_disconn_req(chan->conn, chan, ECONNRESET); - break; - } + if (skb == NULL) { + BT_DBG("Seq %d not available for retransmission", + control->reqseq); + return; + } - chan->buffer_seq_srej = __next_seq(chan, chan->buffer_seq_srej); - tx_seq = __next_seq(chan, tx_seq); + 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; } -} -static void l2cap_resend_srejframe(struct l2cap_chan *chan, u16 tx_seq) -{ - struct srej_list *l, *tmp; - u32 control; + clear_bit(CONN_REMOTE_BUSY, &chan->conn_state); - list_for_each_entry_safe(l, tmp, &chan->srej_l, list) { - if (l->tx_seq == tx_seq) { - list_del(&l->list); - kfree(l); - return; + if (control->poll) { + l2cap_pass_to_tx(chan, control); + + set_bit(CONN_SEND_FBIT, &chan->conn_state); + l2cap_retransmit(chan, control); + l2cap_ertm_send(chan); + + 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; + } } - control = __set_ctrl_super(chan, L2CAP_SUPER_SREJ); - control |= __set_reqseq(chan, l->tx_seq); - l2cap_send_sframe(chan, control); - list_del(&l->list); - list_add_tail(&l->list, &chan->srej_l); } } -static int l2cap_send_srejframe(struct l2cap_chan *chan, u16 tx_seq) +static void l2cap_handle_rej(struct l2cap_chan *chan, + struct l2cap_ctrl *control) { - struct srej_list *new; - u32 control; - - while (tx_seq != chan->expected_tx_seq) { - control = __set_ctrl_super(chan, L2CAP_SUPER_SREJ); - control |= __set_reqseq(chan, chan->expected_tx_seq); - l2cap_seq_list_append(&chan->srej_list, chan->expected_tx_seq); - l2cap_send_sframe(chan, control); + struct sk_buff *skb; - new = kzalloc(sizeof(struct srej_list), GFP_ATOMIC); - if (!new) - return -ENOMEM; + BT_DBG("chan %p, control %p", chan, control); - new->tx_seq = chan->expected_tx_seq; + if (control->reqseq == chan->next_tx_seq) { + BT_DBG("Invalid reqseq %d, disconnecting", control->reqseq); + l2cap_send_disconn_req(chan, ECONNRESET); + return; + } - chan->expected_tx_seq = __next_seq(chan, chan->expected_tx_seq); + skb = l2cap_ertm_seq_in_queue(&chan->tx_q, control->reqseq); - list_add_tail(&new->list, &chan->srej_l); + 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; } - chan->expected_tx_seq = __next_seq(chan, chan->expected_tx_seq); + clear_bit(CONN_REMOTE_BUSY, &chan->conn_state); - return 0; + l2cap_pass_to_tx(chan, control); + + 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); + } } -static inline int l2cap_data_channel_iframe(struct l2cap_chan *chan, u32 rx_control, struct sk_buff *skb) +static u8 l2cap_classify_txseq(struct l2cap_chan *chan, u16 txseq) { - u16 tx_seq = __get_txseq(chan, rx_control); - u16 req_seq = __get_reqseq(chan, rx_control); - u8 sar = __get_ctrl_sar(chan, rx_control); - int tx_seq_offset, expected_tx_seq_offset; - int num_to_ack = (chan->tx_win/6) + 1; - int err = 0; + BT_DBG("chan %p, txseq %d", chan, txseq); - BT_DBG("chan %p len %d tx_seq %d rx_control 0x%8.8x", chan, skb->len, - tx_seq, rx_control); + BT_DBG("last_acked_seq %d, expected_tx_seq %d", chan->last_acked_seq, + chan->expected_tx_seq); - if (__is_ctrl_final(chan, rx_control) && - test_bit(CONN_WAIT_F, &chan->conn_state)) { - __clear_monitor_timer(chan); - if (chan->unacked_frames > 0) - __set_retrans_timer(chan); - clear_bit(CONN_WAIT_F, &chan->conn_state); - } + 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; + } + } - chan->expected_ack_seq = req_seq; - l2cap_drop_acked_frames(chan); + if (chan->srej_list.head == txseq) { + BT_DBG("Expected SREJ"); + return L2CAP_TXSEQ_EXPECTED_SREJ; + } - tx_seq_offset = __seq_offset(chan, tx_seq, chan->buffer_seq); + if (l2cap_ertm_seq_in_queue(&chan->srej_q, txseq)) { + BT_DBG("Duplicate SREJ - txseq already stored"); + return L2CAP_TXSEQ_DUPLICATE_SREJ; + } - /* invalid tx_seq */ - if (tx_seq_offset >= chan->tx_win) { - l2cap_send_disconn_req(chan->conn, chan, ECONNRESET); - goto drop; + if (l2cap_seq_list_contains(&chan->srej_list, txseq)) { + BT_DBG("Unexpected SREJ - not requested"); + return L2CAP_TXSEQ_UNEXPECTED_SREJ; + } } - if (test_bit(CONN_LOCAL_BUSY, &chan->conn_state)) { - if (!test_bit(CONN_RNR_SENT, &chan->conn_state)) - l2cap_send_ack(chan); - goto drop; + 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; + } } - if (tx_seq == chan->expected_tx_seq) - goto expected; + 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; + } + + 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. + */ - if (test_bit(CONN_SREJ_SENT, &chan->conn_state)) { - struct srej_list *first; + 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; + } +} - first = list_first_entry(&chan->srej_l, - struct srej_list, list); - if (tx_seq == first->tx_seq) { - l2cap_add_to_srej_queue(chan, skb, tx_seq, sar); - l2cap_check_srej_gap(chan, tx_seq); +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; - list_del(&first->list); - kfree(first); + BT_DBG("chan %p, control %p, skb %p, event %d", chan, control, skb, + event); - if (list_empty(&chan->srej_l)) { - chan->buffer_seq = chan->buffer_seq_srej; - clear_bit(CONN_SREJ_SENT, &chan->conn_state); - l2cap_send_ack(chan); - BT_DBG("chan %p, Exit SREJ_SENT", chan); + switch (event) { + case L2CAP_EV_RECV_IFRAME: + switch (l2cap_classify_txseq(chan, control->txseq)) { + case L2CAP_TXSEQ_EXPECTED: + l2cap_pass_to_tx(chan, control); + + if (test_bit(CONN_LOCAL_BUSY, &chan->conn_state)) { + BT_DBG("Busy, discarding expected seq %d", + control->txseq); + break; } - } else { - struct srej_list *l; - /* duplicated tx_seq */ - if (l2cap_add_to_srej_queue(chan, skb, tx_seq, sar) < 0) - goto drop; + chan->expected_tx_seq = __next_seq(chan, + control->txseq); + + chan->buffer_seq = chan->expected_tx_seq; + skb_in_use = true; + + err = l2cap_reassemble_sdu(chan, skb, control); + if (err) + break; - list_for_each_entry(l, &chan->srej_l, list) { - if (l->tx_seq == tx_seq) { - l2cap_resend_srejframe(chan, tx_seq); - return 0; + 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); } } - err = l2cap_send_srejframe(chan, tx_seq); - if (err < 0) { - l2cap_send_disconn_req(chan->conn, chan, -err); - return err; + 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; } - } - } else { - expected_tx_seq_offset = __seq_offset(chan, - chan->expected_tx_seq, chan->buffer_seq); - - /* duplicated tx_seq */ - if (tx_seq_offset < expected_tx_seq_offset) - goto drop; - set_bit(CONN_SREJ_SENT, &chan->conn_state); + /* 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)); - BT_DBG("chan %p, Enter SREJ", chan); + clear_bit(CONN_SREJ_ACT, &chan->conn_state); + l2cap_seq_list_clear(&chan->srej_list); + l2cap_send_srej(chan, control->txseq); - INIT_LIST_HEAD(&chan->srej_l); - chan->buffer_seq_srej = chan->buffer_seq; + 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); - __skb_queue_head_init(&chan->srej_q); - l2cap_add_to_srej_queue(chan, skb, tx_seq, sar); + if (!test_and_clear_bit(CONN_REJ_ACT, &chan->conn_state) && + !__chan_is_moving(chan)) { + control->final = 0; + l2cap_retransmit_all(chan, control); + } - /* Set P-bit only if there are some I-frames to ack. */ - if (__clear_ack_timer(chan)) - set_bit(CONN_SEND_PBIT, &chan->conn_state); + 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); - err = l2cap_send_srejframe(chan, tx_seq); - if (err < 0) { - l2cap_send_disconn_req(chan->conn, chan, -err); - return err; + l2cap_ertm_send(chan); } - } - return 0; - -expected: - chan->expected_tx_seq = __next_seq(chan, chan->expected_tx_seq); - - if (test_bit(CONN_SREJ_SENT, &chan->conn_state)) { - bt_cb(skb)->control.txseq = tx_seq; - bt_cb(skb)->control.sar = sar; - __skb_queue_tail(&chan->srej_q, skb); - return 0; + 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; } - err = l2cap_reassemble_sdu(chan, skb, rx_control); - chan->buffer_seq = __next_seq(chan, chan->buffer_seq); - - if (err < 0) { - l2cap_send_disconn_req(chan->conn, chan, ECONNRESET); - return err; + if (skb && !skb_in_use) { + BT_DBG("Freeing %p", skb); + kfree_skb(skb); } - if (__is_ctrl_final(chan, rx_control)) { - if (!test_and_clear_bit(CONN_REJ_ACT, &chan->conn_state)) - l2cap_retransmit_frames(chan); - } + return err; +} +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); - chan->num_acked = (chan->num_acked + 1) % num_to_ack; - if (chan->num_acked == num_to_ack - 1) - l2cap_send_ack(chan); - else - __set_ack_timer(chan); + 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)); - return 0; + err = l2cap_rx_queued_iframes(chan); + if (err) + break; -drop: - kfree_skb(skb); - return 0; -} + 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); -static inline void l2cap_data_channel_rrframe(struct l2cap_chan *chan, u32 rx_control) -{ - BT_DBG("chan %p, req_seq %d ctrl 0x%8.8x", chan, - __get_reqseq(chan, rx_control), rx_control); + if (!test_and_clear_bit(CONN_REJ_ACT, + &chan->conn_state)) { + control->final = 0; + l2cap_retransmit_all(chan, control); + } - chan->expected_ack_seq = __get_reqseq(chan, rx_control); - l2cap_drop_acked_frames(chan); + 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 (__is_ctrl_poll(chan, rx_control)) { - set_bit(CONN_SEND_FBIT, &chan->conn_state); - if (test_bit(CONN_SREJ_SENT, &chan->conn_state)) { - if (test_bit(CONN_REMOTE_BUSY, &chan->conn_state) && - (chan->unacked_frames > 0)) + 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); - clear_bit(CONN_REMOTE_BUSY, &chan->conn_state); - l2cap_send_srejtail(chan); + 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 { - l2cap_send_i_or_rr_or_rnr(chan); + 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); } - } else if (__is_ctrl_final(chan, rx_control)) { - clear_bit(CONN_REMOTE_BUSY, &chan->conn_state); + break; + case L2CAP_EV_RECV_REJ: + l2cap_handle_rej(chan, control); + break; + case L2CAP_EV_RECV_SREJ: + l2cap_handle_srej(chan, control); + break; + } + + if (skb && !skb_in_use) { + BT_DBG("Freeing %p", skb); + kfree_skb(skb); + } - if (!test_and_clear_bit(CONN_REJ_ACT, &chan->conn_state)) - l2cap_retransmit_frames(chan); + return err; +} - } else { - if (test_bit(CONN_REMOTE_BUSY, &chan->conn_state) && - (chan->unacked_frames > 0)) - __set_retrans_timer(chan); +static int l2cap_finish_move(struct l2cap_chan *chan) +{ + BT_DBG("chan %p", chan); - clear_bit(CONN_REMOTE_BUSY, &chan->conn_state); - if (test_bit(CONN_SREJ_SENT, &chan->conn_state)) - l2cap_send_ack(chan); - else - l2cap_ertm_send(chan); - } + chan->rx_state = L2CAP_RX_STATE_RECV; + + if (chan->hs_hcon) + chan->conn->mtu = chan->hs_hcon->hdev->block_mtu; + else + chan->conn->mtu = chan->conn->hcon->hdev->acl_mtu; + + return l2cap_resegment(chan); } -static inline void l2cap_data_channel_rejframe(struct l2cap_chan *chan, u32 rx_control) +static int l2cap_rx_state_wait_p(struct l2cap_chan *chan, + struct l2cap_ctrl *control, + struct sk_buff *skb, u8 event) { - u16 tx_seq = __get_reqseq(chan, rx_control); + int err; - BT_DBG("chan %p, req_seq %d ctrl 0x%8.8x", chan, tx_seq, rx_control); + BT_DBG("chan %p, control %p, skb %p, event %d", chan, control, skb, + event); - clear_bit(CONN_REMOTE_BUSY, &chan->conn_state); + if (!control->poll) + return -EPROTO; - chan->expected_ack_seq = tx_seq; - l2cap_drop_acked_frames(chan); + l2cap_process_reqseq(chan, control->reqseq); - if (__is_ctrl_final(chan, rx_control)) { - if (!test_and_clear_bit(CONN_REJ_ACT, &chan->conn_state)) - l2cap_retransmit_frames(chan); - } else { - l2cap_retransmit_frames(chan); + if (!skb_queue_empty(&chan->tx_q)) + chan->tx_send_head = skb_peek(&chan->tx_q); + else + chan->tx_send_head = NULL; - if (test_bit(CONN_WAIT_F, &chan->conn_state)) - set_bit(CONN_REJ_ACT, &chan->conn_state); - } + /* Rewind next_tx_seq to the point expected + * by the receiver. + */ + chan->next_tx_seq = control->reqseq; + chan->unacked_frames = 0; + + err = l2cap_finish_move(chan); + if (err) + return err; + + set_bit(CONN_SEND_FBIT, &chan->conn_state); + l2cap_send_i_or_rr_or_rnr(chan); + + if (event == L2CAP_EV_RECV_IFRAME) + return -EPROTO; + + return l2cap_rx_state_recv(chan, control, NULL, event); } -static inline void l2cap_data_channel_srejframe(struct l2cap_chan *chan, u32 rx_control) + +static int l2cap_rx_state_wait_f(struct l2cap_chan *chan, + struct l2cap_ctrl *control, + struct sk_buff *skb, u8 event) { - u16 tx_seq = __get_reqseq(chan, rx_control); + int err; - BT_DBG("chan %p, req_seq %d ctrl 0x%8.8x", chan, tx_seq, rx_control); + if (!control->final) + return -EPROTO; clear_bit(CONN_REMOTE_BUSY, &chan->conn_state); - if (__is_ctrl_poll(chan, rx_control)) { - chan->expected_ack_seq = tx_seq; - l2cap_drop_acked_frames(chan); + chan->rx_state = L2CAP_RX_STATE_RECV; + l2cap_process_reqseq(chan, control->reqseq); - set_bit(CONN_SEND_FBIT, &chan->conn_state); - l2cap_retransmit_one_frame(chan, tx_seq); + if (!skb_queue_empty(&chan->tx_q)) + chan->tx_send_head = skb_peek(&chan->tx_q); + else + chan->tx_send_head = NULL; - l2cap_ertm_send(chan); + /* Rewind next_tx_seq to the point expected + * by the receiver. + */ + chan->next_tx_seq = control->reqseq; + chan->unacked_frames = 0; - if (test_bit(CONN_WAIT_F, &chan->conn_state)) { - chan->srej_save_reqseq = tx_seq; - set_bit(CONN_SREJ_ACT, &chan->conn_state); - } - } else if (__is_ctrl_final(chan, rx_control)) { - if (test_bit(CONN_SREJ_ACT, &chan->conn_state) && - chan->srej_save_reqseq == tx_seq) - clear_bit(CONN_SREJ_ACT, &chan->conn_state); - else - l2cap_retransmit_one_frame(chan, tx_seq); - } else { - l2cap_retransmit_one_frame(chan, tx_seq); - if (test_bit(CONN_WAIT_F, &chan->conn_state)) { - chan->srej_save_reqseq = tx_seq; - set_bit(CONN_SREJ_ACT, &chan->conn_state); - } - } + if (chan->hs_hcon) + chan->conn->mtu = chan->hs_hcon->hdev->block_mtu; + else + chan->conn->mtu = chan->conn->hcon->hdev->acl_mtu; + + err = l2cap_resegment(chan); + + if (!err) + err = l2cap_rx_state_recv(chan, control, skb, event); + + return err; } -static inline void l2cap_data_channel_rnrframe(struct l2cap_chan *chan, u32 rx_control) +static bool __valid_reqseq(struct l2cap_chan *chan, u16 reqseq) { - u16 tx_seq = __get_reqseq(chan, rx_control); + /* Make sure reqseq is for a packet that has been sent but not acked */ + u16 unacked; - BT_DBG("chan %p, req_seq %d ctrl 0x%8.8x", chan, tx_seq, rx_control); - - set_bit(CONN_REMOTE_BUSY, &chan->conn_state); - chan->expected_ack_seq = tx_seq; - l2cap_drop_acked_frames(chan); + unacked = __seq_offset(chan, chan->next_tx_seq, chan->expected_ack_seq); + return __seq_offset(chan, chan->next_tx_seq, reqseq) <= unacked; +} - if (__is_ctrl_poll(chan, rx_control)) - set_bit(CONN_SEND_FBIT, &chan->conn_state); +static int l2cap_rx(struct l2cap_chan *chan, struct l2cap_ctrl *control, + struct sk_buff *skb, u8 event) +{ + int err = 0; - if (!test_bit(CONN_SREJ_SENT, &chan->conn_state)) { - __clear_retrans_timer(chan); - if (__is_ctrl_poll(chan, rx_control)) - l2cap_send_rr_or_rnr(chan, L2CAP_CTRL_FINAL); - return; - } + BT_DBG("chan %p, control %p, skb %p, event %d, state %d", chan, + control, skb, event, chan->rx_state); - if (__is_ctrl_poll(chan, rx_control)) { - l2cap_send_srejtail(chan); + 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 { - rx_control = __set_ctrl_super(chan, L2CAP_SUPER_RR); - l2cap_send_sframe(chan, rx_control); + 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 err; } -static inline int l2cap_data_channel_sframe(struct l2cap_chan *chan, u32 rx_control, struct sk_buff *skb) +static int l2cap_stream_rx(struct l2cap_chan *chan, struct l2cap_ctrl *control, + struct sk_buff *skb) { - BT_DBG("chan %p rx_control 0x%8.8x len %d", chan, rx_control, skb->len); + int err = 0; - if (__is_ctrl_final(chan, rx_control) && - test_bit(CONN_WAIT_F, &chan->conn_state)) { - __clear_monitor_timer(chan); - if (chan->unacked_frames > 0) - __set_retrans_timer(chan); - clear_bit(CONN_WAIT_F, &chan->conn_state); - } + BT_DBG("chan %p, control %p, skb %p, state %d", chan, control, skb, + chan->rx_state); - switch (__get_ctrl_super(chan, rx_control)) { - case L2CAP_SUPER_RR: - l2cap_data_channel_rrframe(chan, rx_control); - break; + if (l2cap_classify_txseq(chan, control->txseq) == + L2CAP_TXSEQ_EXPECTED) { + l2cap_pass_to_tx(chan, control); - case L2CAP_SUPER_REJ: - l2cap_data_channel_rejframe(chan, rx_control); - break; + BT_DBG("buffer_seq %d->%d", chan->buffer_seq, + __next_seq(chan, chan->buffer_seq)); - case L2CAP_SUPER_SREJ: - l2cap_data_channel_srejframe(chan, rx_control); - break; + chan->buffer_seq = __next_seq(chan, chan->buffer_seq); - case L2CAP_SUPER_RNR: - l2cap_data_channel_rnrframe(chan, rx_control); - break; + 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; + + if (skb) { + BT_DBG("Freeing %p", skb); + kfree_skb(skb); + } } - kfree_skb(skb); - return 0; + chan->last_acked_seq = control->txseq; + chan->expected_tx_seq = __next_seq(chan, control->txseq); + + return err; } -static int l2cap_ertm_data_rcv(struct l2cap_chan *chan, struct sk_buff *skb) +static int l2cap_data_rcv(struct l2cap_chan *chan, struct sk_buff *skb) { - u32 control; - u16 req_seq; - int len, next_tx_seq_offset, req_seq_offset; + struct l2cap_ctrl *control = &bt_cb(skb)->control; + u16 len; + u8 event; __unpack_control(chan, skb); - control = __get_control(chan, skb->data); - skb_pull(skb, __ctrl_size(chan)); len = skb->len; /* * We can just drop the corrupted I-frame here. * Receiver will miss it and start proper recovery - * procedures and ask retransmission. + * procedures and ask for retransmission. */ if (l2cap_check_fcs(chan, skb)) goto drop; - if (__is_sar_start(chan, control) && !__is_sframe(chan, control)) + if (!control->sframe && control->sar == L2CAP_SAR_START) len -= L2CAP_SDULEN_SIZE; if (chan->fcs == L2CAP_FCS_CRC16) len -= L2CAP_FCS_SIZE; if (len > chan->mps) { - l2cap_send_disconn_req(chan->conn, chan, ECONNRESET); + l2cap_send_disconn_req(chan, ECONNRESET); goto drop; } - req_seq = __get_reqseq(chan, control); - - req_seq_offset = __seq_offset(chan, req_seq, chan->expected_ack_seq); - - next_tx_seq_offset = __seq_offset(chan, chan->next_tx_seq, - chan->expected_ack_seq); + if (!control->sframe) { + int err; - /* check for invalid req-seq */ - if (req_seq_offset > next_tx_seq_offset) { - l2cap_send_disconn_req(chan->conn, chan, ECONNRESET); - goto drop; - } + BT_DBG("iframe sar %d, reqseq %d, final %d, txseq %d", + control->sar, control->reqseq, control->final, + control->txseq); - if (!__is_sframe(chan, control)) { - if (len < 0) { - l2cap_send_disconn_req(chan->conn, chan, ECONNRESET); + /* 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; + + 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); } - l2cap_data_channel_iframe(chan, control, skb); + if (err) + l2cap_send_disconn_req(chan, ECONNRESET); } else { + const u8 rx_func_to_event[4] = { + L2CAP_EV_RECV_RR, L2CAP_EV_RECV_REJ, + L2CAP_EV_RECV_RNR, L2CAP_EV_RECV_SREJ + }; + + /* Only I-frames are expected in streaming mode */ + if (chan->mode == L2CAP_MODE_STREAMING) + goto drop; + + BT_DBG("sframe reqseq %d, final %d, poll %d, super %d", + control->reqseq, control->final, control->poll, + control->super); + if (len != 0) { - BT_ERR("%d", len); - l2cap_send_disconn_req(chan->conn, chan, ECONNRESET); + BT_ERR("Trailing bytes: %d in sframe", len); + l2cap_send_disconn_req(chan, ECONNRESET); goto drop; } - l2cap_data_channel_sframe(chan, control, skb); + /* Validate F and P bits */ + if (control->final && (control->poll || + chan->tx_state != L2CAP_TX_STATE_WAIT_F)) + goto drop; + + event = rx_func_to_event[control->super]; + if (l2cap_rx(chan, control, skb, event)) + l2cap_send_disconn_req(chan, ECONNRESET); } return 0; @@ -4622,19 +6647,143 @@ drop: return 0; } -static inline int l2cap_data_channel(struct l2cap_conn *conn, u16 cid, struct sk_buff *skb) +static void l2cap_chan_le_send_credits(struct l2cap_chan *chan) +{ + struct l2cap_conn *conn = chan->conn; + struct l2cap_le_credits pkt; + u16 return_credits; + + /* 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; + + return_credits = le_max_credits - chan->rx_credits; + + BT_DBG("chan %p returning %u credits to sender", chan, return_credits); + + chan->rx_credits += return_credits; + + 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 int l2cap_le_data_rcv(struct l2cap_chan *chan, struct sk_buff *skb) +{ + int err; + + if (!chan->rx_credits) { + BT_ERR("No credits to receive LE L2CAP data"); + l2cap_send_disconn_req(chan, ECONNRESET); + return -ENOBUFS; + } + + if (chan->imtu < skb->len) { + BT_ERR("Too big LE L2CAP PDU"); + return -ENOBUFS; + } + + chan->rx_credits--; + BT_DBG("rx_credits %u -> %u", chan->rx_credits + 1, chan->rx_credits); + + l2cap_chan_le_send_credits(chan); + + err = 0; + + if (!chan->sdu) { + u16 sdu_len; + + sdu_len = get_unaligned_le16(skb->data); + skb_pull(skb, L2CAP_SDULEN_SIZE); + + BT_DBG("Start of new SDU. sdu_len %u skb->len %u imtu %u", + sdu_len, skb->len, chan->imtu); + + if (sdu_len > chan->imtu) { + BT_ERR("Too big LE L2CAP SDU length received"); + err = -EMSGSIZE; + goto failed; + } + + if (skb->len > sdu_len) { + BT_ERR("Too much LE L2CAP data received"); + err = -EINVAL; + goto failed; + } + + if (skb->len == sdu_len) + return chan->ops->recv(chan, skb); + + chan->sdu = skb; + chan->sdu_len = sdu_len; + chan->sdu_last_frag = skb; + + return 0; + } + + BT_DBG("SDU fragment. chan->sdu->len %u skb->len %u chan->sdu_len %u", + chan->sdu->len, skb->len, chan->sdu_len); + + if (chan->sdu->len + skb->len > chan->sdu_len) { + BT_ERR("Too much LE L2CAP data received"); + err = -EINVAL; + goto failed; + } + + append_skb_frag(chan->sdu, skb, &chan->sdu_last_frag); + skb = NULL; + + 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; + } + } + +failed: + if (err) { + kfree_skb(skb); + kfree_skb(chan->sdu); + chan->sdu = NULL; + chan->sdu_last_frag = NULL; + chan->sdu_len = 0; + } + + /* 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 void l2cap_data_channel(struct l2cap_conn *conn, u16 cid, + struct sk_buff *skb) { struct l2cap_chan *chan; - u32 control; - u16 tx_seq; - int len; chan = l2cap_get_chan_by_scid(conn, cid); if (!chan) { - BT_DBG("unknown cid 0x%4.4x", cid); - /* Drop packet and return */ - kfree_skb(skb); - return 0; + if (cid == L2CAP_CID_A2MP) { + chan = a2mp_channel_create(conn, skb); + if (!chan) { + kfree_skb(skb); + return; + } + + l2cap_chan_lock(chan); + } else { + BT_DBG("unknown cid 0x%4.4x", cid); + /* Drop packet and return */ + kfree_skb(skb); + return; + } } BT_DBG("chan %p, len %d", chan, skb->len); @@ -4643,58 +6792,30 @@ static inline int l2cap_data_channel(struct l2cap_conn *conn, u16 cid, struct sk goto drop; 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 (chan->imtu < skb->len) + if (chan->imtu < skb->len) { + BT_ERR("Dropping L2CAP data: receive buffer overflow"); goto drop; + } - if (!chan->ops->recv(chan->data, skb)) + if (!chan->ops->recv(chan, skb)) goto done; break; case L2CAP_MODE_ERTM: - l2cap_ertm_data_rcv(chan, skb); - - goto done; - case L2CAP_MODE_STREAMING: - control = __get_control(chan, skb->data); - skb_pull(skb, __ctrl_size(chan)); - len = skb->len; - - if (l2cap_check_fcs(chan, skb)) - goto drop; - - if (__is_sar_start(chan, control)) - len -= L2CAP_SDULEN_SIZE; - - if (chan->fcs == L2CAP_FCS_CRC16) - len -= L2CAP_FCS_SIZE; - - if (len > chan->mps || len < 0 || __is_sframe(chan, control)) - goto drop; - - tx_seq = __get_txseq(chan, control); - - if (chan->expected_tx_seq != tx_seq) { - /* Frame(s) missing - must discard partial SDU */ - kfree_skb(chan->sdu); - chan->sdu = NULL; - chan->sdu_last_frag = NULL; - chan->sdu_len = 0; - - /* TODO: Notify userland of missing data */ - } - - chan->expected_tx_seq = __next_seq(chan, tx_seq); - - if (l2cap_reassemble_sdu(chan, skb, control) == -EMSGSIZE) - l2cap_send_disconn_req(chan->conn, chan, ECONNRESET); - + l2cap_data_rcv(chan, skb); goto done; default: @@ -4707,15 +6828,19 @@ drop: done: l2cap_chan_unlock(chan); - - return 0; } -static inline int l2cap_conless_channel(struct l2cap_conn *conn, __le16 psm, struct sk_buff *skb) +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; - chan = l2cap_global_chan_by_psm(0, psm, conn->src, conn->dst); + if (hcon->type != ACL_LINK) + goto drop; + + chan = l2cap_global_chan_by_psm(0, psm, &hcon->src, &hcon->dst, + ACL_LINK); if (!chan) goto drop; @@ -4727,47 +6852,59 @@ static inline int l2cap_conless_channel(struct l2cap_conn *conn, __le16 psm, str if (chan->imtu < skb->len) goto drop; - if (!chan->ops->recv(chan->data, skb)) - return 0; + /* 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); - - return 0; } -static inline int l2cap_att_channel(struct l2cap_conn *conn, u16 cid, - struct sk_buff *skb) +static void l2cap_att_channel(struct l2cap_conn *conn, + struct sk_buff *skb) { + struct hci_conn *hcon = conn->hcon; struct l2cap_chan *chan; - chan = l2cap_global_chan_by_scid(0, cid, conn->src, conn->dst); + if (hcon->type != LE_LINK) + goto drop; + + chan = l2cap_global_chan_by_scid(BT_CONNECTED, L2CAP_CID_ATT, + &hcon->src, &hcon->dst); if (!chan) goto drop; BT_DBG("chan %p, len %d", chan, skb->len); - if (chan->state != BT_BOUND && chan->state != BT_CONNECTED) + if (hci_blacklist_lookup(hcon->hdev, &hcon->dst, hcon->dst_type)) goto drop; if (chan->imtu < skb->len) goto drop; - if (!chan->ops->recv(chan->data, skb)) - return 0; + if (!chan->ops->recv(chan, skb)) + return; drop: kfree_skb(skb); - - 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); @@ -4780,19 +6917,22 @@ 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); + skb_pull(skb, L2CAP_PSMLEN_SIZE); l2cap_conless_channel(conn, psm, skb); break; - case L2CAP_CID_LE_DATA: - l2cap_att_channel(conn, cid, skb); + 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: @@ -4800,12 +6940,257 @@ static void l2cap_recv_frame(struct l2cap_conn *conn, struct sk_buff *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) ---- */ int l2cap_connect_ind(struct hci_dev *hdev, bdaddr_t *bdaddr) @@ -4813,22 +7198,20 @@ int l2cap_connect_ind(struct hci_dev *hdev, bdaddr_t *bdaddr) int exact = 0, lm1 = 0, lm2 = 0; 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(&chan_list_lock); list_for_each_entry(c, &chan_list, global_l) { - struct sock *sk = c->sk; - 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 (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 (test_bit(FLAG_ROLE_SWITCH, &c->flags)) lm2 |= HCI_LM_MASTER; @@ -4839,20 +7222,19 @@ int l2cap_connect_ind(struct hci_dev *hdev, bdaddr_t *bdaddr) return exact ? lm1 : lm2; } -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); + 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 + } else { l2cap_conn_del(hcon, bt_to_errno(status)); - - return 0; + } } int l2cap_disconn_ind(struct hci_conn *hcon) @@ -4866,12 +7248,13 @@ int l2cap_disconn_ind(struct hci_conn *hcon) return conn->disc_reason; } -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); + bt_6lowpan_del_conn(hcon->l2cap_data); + l2cap_conn_del(hcon, bt_to_errno(reason)); - return 0; } static inline void l2cap_check_encryption(struct l2cap_chan *chan, u8 encrypt) @@ -4882,7 +7265,8 @@ static inline void l2cap_check_encryption(struct l2cap_chan *chan, u8 encrypt) if (encrypt == 0x00) { if (chan->sec_level == BT_SECURITY_MEDIUM) { __set_chan_timer(chan, L2CAP_ENC_TIMEOUT); - } else if (chan->sec_level == BT_SECURITY_HIGH) + } else if (chan->sec_level == BT_SECURITY_HIGH || + chan->sec_level == BT_SECURITY_FIPS) l2cap_chan_close(chan, ECONNREFUSED); } else { if (chan->sec_level == BT_SECURITY_MEDIUM) @@ -4898,11 +7282,11 @@ int l2cap_security_cfm(struct hci_conn *hcon, u8 status, u8 encrypt) if (!conn) return 0; - BT_DBG("conn %p", conn); + BT_DBG("conn %p status 0x%2.2x encrypt %u", conn, status, encrypt); if (hcon->type == LE_LINK) { if (!status && encrypt) - smp_distribute_keys(conn, 0); + smp_distribute_keys(conn); cancel_delayed_work(&conn->security_timer); } @@ -4911,9 +7295,15 @@ int l2cap_security_cfm(struct hci_conn *hcon, u8 status, u8 encrypt) list_for_each_entry(chan, &conn->chan_l, list) { l2cap_chan_lock(chan); - BT_DBG("chan->scid %d", chan->scid); + BT_DBG("chan %p scid 0x%4.4x state %s", chan, chan->scid, + state_to_string(chan->state)); - if (chan->scid == L2CAP_CID_LE_DATA) { + if (chan->scid == L2CAP_CID_A2MP) { + l2cap_chan_unlock(chan); + continue; + } + + if (chan->scid == L2CAP_CID_ATT) { if (!status && encrypt) { chan->sec_level = hcon->sec_level; l2cap_chan_ready(chan); @@ -4923,64 +7313,62 @@ int l2cap_security_cfm(struct hci_conn *hcon, u8 status, u8 encrypt) continue; } - if (test_bit(CONF_CONNECT_PEND, &chan->conf_state)) { + if (!__l2cap_no_conn_pending(chan)) { l2cap_chan_unlock(chan); continue; } if (!status && (chan->state == BT_CONNECTED || - chan->state == BT_CONFIG)) { - struct sock *sk = chan->sk; - - clear_bit(BT_SK_SUSPEND, &bt_sk(sk)->flags); - sk->sk_state_change(sk); - + chan->state == BT_CONFIG)) { + chan->ops->resume(chan); l2cap_check_encryption(chan, encrypt); l2cap_chan_unlock(chan); continue; } if (chan->state == BT_CONNECT) { - if (!status) { - l2cap_send_conn_req(chan); - } else { + if (!status) + l2cap_start_connection(chan); + else __set_chan_timer(chan, L2CAP_DISC_TIMEOUT); - } } else if (chan->state == BT_CONNECT2) { - struct sock *sk = chan->sk; struct l2cap_conn_rsp rsp; __u16 res, stat; - lock_sock(sk); - if (!status) { - if (test_bit(BT_SK_DEFER_SETUP, - &bt_sk(sk)->flags)) { - struct sock *parent = bt_sk(sk)->parent; + if (test_bit(FLAG_DEFER_SETUP, &chan->flags)) { res = L2CAP_CR_PEND; stat = L2CAP_CS_AUTHOR_PEND; - if (parent) - parent->sk_data_ready(parent, 0); + chan->ops->defer(chan); } else { - __l2cap_state_change(chan, BT_CONFIG); + l2cap_state_change(chan, BT_CONFIG); res = L2CAP_CR_SUCCESS; stat = L2CAP_CS_NO_INFO; } } else { - __l2cap_state_change(chan, BT_DISCONN); + l2cap_state_change(chan, BT_DISCONN); __set_chan_timer(chan, L2CAP_DISC_TIMEOUT); res = L2CAP_CR_SEC_BLOCK; stat = L2CAP_CS_NO_INFO; } - release_sock(sk); - 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); + 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++; + } } l2cap_chan_unlock(chan); @@ -4994,19 +7382,25 @@ int l2cap_security_cfm(struct hci_conn *hcon, u8 status, u8 encrypt) 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; - 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); @@ -5035,20 +7429,22 @@ int l2cap_recv_acldata(struct hci_conn *hcon, struct sk_buff *skb, u16 flags) 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; } /* 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) { @@ -5059,7 +7455,7 @@ int l2cap_recv_acldata(struct hci_conn *hcon, struct sk_buff *skb, u16 flags) 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; @@ -5068,14 +7464,19 @@ int l2cap_recv_acldata(struct hci_conn *hcon, struct sk_buff *skb, u16 flags) } 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: @@ -5090,14 +7491,11 @@ static int l2cap_debugfs_show(struct seq_file *f, void *p) read_lock(&chan_list_lock); list_for_each_entry(c, &chan_list, global_l) { - struct sock *sk = c->sk; - - 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), - c->state, __le16_to_cpu(c->psm), - c->scid, c->dcid, c->imtu, c->omtu, - c->sec_level, c->mode); + 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(&chan_list_lock); @@ -5127,18 +7525,25 @@ int __init l2cap_init(void) if (err < 0) return err; - 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"); - } + if (IS_ERR_OR_NULL(bt_debugfs)) + return 0; + + l2cap_debugfs = debugfs_create_file("l2cap", 0444, bt_debugfs, + NULL, &l2cap_debugfs_fops); + + 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); + + bt_6lowpan_init(); return 0; } void l2cap_exit(void) { + bt_6lowpan_cleanup(); debugfs_remove(l2cap_debugfs); l2cap_cleanup_sockets(); } |