10a708f8fSGustavo F. Padovan /* 20a708f8fSGustavo F. Padovan BlueZ - Bluetooth protocol stack for Linux 30a708f8fSGustavo F. Padovan Copyright (C) 2000-2001 Qualcomm Incorporated 40a708f8fSGustavo F. Padovan Copyright (C) 2009-2010 Gustavo F. Padovan <gustavo@padovan.org> 50a708f8fSGustavo F. Padovan Copyright (C) 2010 Google Inc. 60a708f8fSGustavo F. Padovan 70a708f8fSGustavo F. Padovan Written 2000,2001 by Maxim Krasnyansky <maxk@qualcomm.com> 80a708f8fSGustavo F. Padovan 90a708f8fSGustavo F. Padovan This program is free software; you can redistribute it and/or modify 100a708f8fSGustavo F. Padovan it under the terms of the GNU General Public License version 2 as 110a708f8fSGustavo F. Padovan published by the Free Software Foundation; 120a708f8fSGustavo F. Padovan 130a708f8fSGustavo F. Padovan THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS 140a708f8fSGustavo F. Padovan OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 150a708f8fSGustavo F. Padovan FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT OF THIRD PARTY RIGHTS. 160a708f8fSGustavo F. Padovan IN NO EVENT SHALL THE COPYRIGHT HOLDER(S) AND AUTHOR(S) BE LIABLE FOR ANY 170a708f8fSGustavo F. Padovan CLAIM, OR ANY SPECIAL INDIRECT OR CONSEQUENTIAL DAMAGES, OR ANY DAMAGES 180a708f8fSGustavo F. Padovan WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN 190a708f8fSGustavo F. Padovan ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF 200a708f8fSGustavo F. Padovan OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. 210a708f8fSGustavo F. Padovan 220a708f8fSGustavo F. Padovan ALL LIABILITY, INCLUDING LIABILITY FOR INFRINGEMENT OF ANY PATENTS, 230a708f8fSGustavo F. Padovan COPYRIGHTS, TRADEMARKS OR OTHER RIGHTS, RELATING TO USE OF THIS 240a708f8fSGustavo F. Padovan SOFTWARE IS DISCLAIMED. 250a708f8fSGustavo F. Padovan */ 260a708f8fSGustavo F. Padovan 27bb58f747SGustavo F. Padovan /* Bluetooth L2CAP core. */ 280a708f8fSGustavo F. Padovan 290a708f8fSGustavo F. Padovan #include <linux/module.h> 300a708f8fSGustavo F. Padovan 310a708f8fSGustavo F. Padovan #include <linux/types.h> 320a708f8fSGustavo F. Padovan #include <linux/capability.h> 330a708f8fSGustavo F. Padovan #include <linux/errno.h> 340a708f8fSGustavo F. Padovan #include <linux/kernel.h> 350a708f8fSGustavo F. Padovan #include <linux/sched.h> 360a708f8fSGustavo F. Padovan #include <linux/slab.h> 370a708f8fSGustavo F. Padovan #include <linux/poll.h> 380a708f8fSGustavo F. Padovan #include <linux/fcntl.h> 390a708f8fSGustavo F. Padovan #include <linux/init.h> 400a708f8fSGustavo F. Padovan #include <linux/interrupt.h> 410a708f8fSGustavo F. Padovan #include <linux/socket.h> 420a708f8fSGustavo F. Padovan #include <linux/skbuff.h> 430a708f8fSGustavo F. Padovan #include <linux/list.h> 440a708f8fSGustavo F. Padovan #include <linux/device.h> 450a708f8fSGustavo F. Padovan #include <linux/debugfs.h> 460a708f8fSGustavo F. Padovan #include <linux/seq_file.h> 470a708f8fSGustavo F. Padovan #include <linux/uaccess.h> 480a708f8fSGustavo F. Padovan #include <linux/crc16.h> 490a708f8fSGustavo F. Padovan #include <net/sock.h> 500a708f8fSGustavo F. Padovan 510a708f8fSGustavo F. Padovan #include <asm/system.h> 520a708f8fSGustavo F. Padovan #include <asm/unaligned.h> 530a708f8fSGustavo F. Padovan 540a708f8fSGustavo F. Padovan #include <net/bluetooth/bluetooth.h> 550a708f8fSGustavo F. Padovan #include <net/bluetooth/hci_core.h> 560a708f8fSGustavo F. Padovan #include <net/bluetooth/l2cap.h> 570a708f8fSGustavo F. Padovan 58bb58f747SGustavo F. Padovan int disable_ertm; 590a708f8fSGustavo F. Padovan 600a708f8fSGustavo F. Padovan static u32 l2cap_feat_mask = L2CAP_FEAT_FIXED_CHAN; 610a708f8fSGustavo F. Padovan static u8 l2cap_fixed_chan[8] = { 0x02, }; 620a708f8fSGustavo F. Padovan 630a708f8fSGustavo F. Padovan static struct workqueue_struct *_busy_wq; 640a708f8fSGustavo F. Padovan 6523691d75SGustavo F. Padovan LIST_HEAD(chan_list); 6623691d75SGustavo F. Padovan DEFINE_RWLOCK(chan_list_lock); 670a708f8fSGustavo F. Padovan 680a708f8fSGustavo F. Padovan static void l2cap_busy_work(struct work_struct *work); 690a708f8fSGustavo F. Padovan 700a708f8fSGustavo F. Padovan static struct sk_buff *l2cap_build_cmd(struct l2cap_conn *conn, 710a708f8fSGustavo F. Padovan u8 code, u8 ident, u16 dlen, void *data); 72710f9b0aSGustavo F. Padovan static int l2cap_build_conf_req(struct l2cap_chan *chan, void *data); 730a708f8fSGustavo F. Padovan 740a708f8fSGustavo F. Padovan static int l2cap_ertm_data_rcv(struct sock *sk, struct sk_buff *skb); 750a708f8fSGustavo F. Padovan 760a708f8fSGustavo F. Padovan /* ---- L2CAP channels ---- */ 77baa7e1faSGustavo F. Padovan static struct l2cap_chan *__l2cap_get_chan_by_dcid(struct l2cap_conn *conn, u16 cid) 780a708f8fSGustavo F. Padovan { 7948454079SGustavo F. Padovan struct l2cap_chan *c; 80baa7e1faSGustavo F. Padovan 81baa7e1faSGustavo F. Padovan list_for_each_entry(c, &conn->chan_l, list) { 82fe4128e0SGustavo F. Padovan if (c->dcid == cid) 8348454079SGustavo F. Padovan return c; 840a708f8fSGustavo F. Padovan } 85baa7e1faSGustavo F. Padovan return NULL; 86baa7e1faSGustavo F. Padovan 87baa7e1faSGustavo F. Padovan } 880a708f8fSGustavo F. Padovan 89baa7e1faSGustavo F. Padovan static struct l2cap_chan *__l2cap_get_chan_by_scid(struct l2cap_conn *conn, u16 cid) 900a708f8fSGustavo F. Padovan { 9148454079SGustavo F. Padovan struct l2cap_chan *c; 92baa7e1faSGustavo F. Padovan 93baa7e1faSGustavo F. Padovan list_for_each_entry(c, &conn->chan_l, list) { 94fe4128e0SGustavo F. Padovan if (c->scid == cid) 9548454079SGustavo F. Padovan return c; 960a708f8fSGustavo F. Padovan } 97baa7e1faSGustavo F. Padovan return NULL; 98baa7e1faSGustavo F. Padovan } 990a708f8fSGustavo F. Padovan 1000a708f8fSGustavo F. Padovan /* Find channel with given SCID. 1010a708f8fSGustavo F. Padovan * Returns locked socket */ 102baa7e1faSGustavo F. Padovan static struct l2cap_chan *l2cap_get_chan_by_scid(struct l2cap_conn *conn, u16 cid) 1030a708f8fSGustavo F. Padovan { 10448454079SGustavo F. Padovan struct l2cap_chan *c; 105baa7e1faSGustavo F. Padovan 106baa7e1faSGustavo F. Padovan read_lock(&conn->chan_lock); 107baa7e1faSGustavo F. Padovan c = __l2cap_get_chan_by_scid(conn, cid); 10848454079SGustavo F. Padovan if (c) 10948454079SGustavo F. Padovan bh_lock_sock(c->sk); 110baa7e1faSGustavo F. Padovan read_unlock(&conn->chan_lock); 11148454079SGustavo F. Padovan return c; 1120a708f8fSGustavo F. Padovan } 1130a708f8fSGustavo F. Padovan 114baa7e1faSGustavo F. Padovan static struct l2cap_chan *__l2cap_get_chan_by_ident(struct l2cap_conn *conn, u8 ident) 1150a708f8fSGustavo F. Padovan { 11648454079SGustavo F. Padovan struct l2cap_chan *c; 117baa7e1faSGustavo F. Padovan 118baa7e1faSGustavo F. Padovan list_for_each_entry(c, &conn->chan_l, list) { 119fc7f8a7eSGustavo F. Padovan if (c->ident == ident) 12048454079SGustavo F. Padovan return c; 1210a708f8fSGustavo F. Padovan } 122baa7e1faSGustavo F. Padovan return NULL; 123baa7e1faSGustavo F. Padovan } 1240a708f8fSGustavo F. Padovan 125baa7e1faSGustavo F. Padovan static inline struct l2cap_chan *l2cap_get_chan_by_ident(struct l2cap_conn *conn, u8 ident) 1260a708f8fSGustavo F. Padovan { 12748454079SGustavo F. Padovan struct l2cap_chan *c; 128baa7e1faSGustavo F. Padovan 129baa7e1faSGustavo F. Padovan read_lock(&conn->chan_lock); 130baa7e1faSGustavo F. Padovan c = __l2cap_get_chan_by_ident(conn, ident); 13148454079SGustavo F. Padovan if (c) 13248454079SGustavo F. Padovan bh_lock_sock(c->sk); 133baa7e1faSGustavo F. Padovan read_unlock(&conn->chan_lock); 13448454079SGustavo F. Padovan return c; 1350a708f8fSGustavo F. Padovan } 1360a708f8fSGustavo F. Padovan 13723691d75SGustavo F. Padovan static struct l2cap_chan *__l2cap_global_chan_by_addr(__le16 psm, bdaddr_t *src) 1389e4425ffSGustavo F. Padovan { 13923691d75SGustavo F. Padovan struct l2cap_chan *c; 1409e4425ffSGustavo F. Padovan 14123691d75SGustavo F. Padovan list_for_each_entry(c, &chan_list, global_l) { 14223691d75SGustavo F. Padovan if (c->sport == psm && !bacmp(&bt_sk(c->sk)->src, src)) 1439e4425ffSGustavo F. Padovan goto found; 1449e4425ffSGustavo F. Padovan } 1459e4425ffSGustavo F. Padovan 14623691d75SGustavo F. Padovan c = NULL; 1479e4425ffSGustavo F. Padovan found: 14823691d75SGustavo F. Padovan return c; 1499e4425ffSGustavo F. Padovan } 1509e4425ffSGustavo F. Padovan 1519e4425ffSGustavo F. Padovan int l2cap_add_psm(struct l2cap_chan *chan, bdaddr_t *src, __le16 psm) 1529e4425ffSGustavo F. Padovan { 15373b2ec18SGustavo F. Padovan int err; 15473b2ec18SGustavo F. Padovan 15523691d75SGustavo F. Padovan write_lock_bh(&chan_list_lock); 1569e4425ffSGustavo F. Padovan 15723691d75SGustavo F. Padovan if (psm && __l2cap_global_chan_by_addr(psm, src)) { 15873b2ec18SGustavo F. Padovan err = -EADDRINUSE; 15973b2ec18SGustavo F. Padovan goto done; 1609e4425ffSGustavo F. Padovan } 1619e4425ffSGustavo F. Padovan 16273b2ec18SGustavo F. Padovan if (psm) { 1639e4425ffSGustavo F. Padovan chan->psm = psm; 1649e4425ffSGustavo F. Padovan chan->sport = psm; 16573b2ec18SGustavo F. Padovan err = 0; 16673b2ec18SGustavo F. Padovan } else { 16773b2ec18SGustavo F. Padovan u16 p; 1689e4425ffSGustavo F. Padovan 16973b2ec18SGustavo F. Padovan err = -EINVAL; 17073b2ec18SGustavo F. Padovan for (p = 0x1001; p < 0x1100; p += 2) 17123691d75SGustavo F. Padovan if (!__l2cap_global_chan_by_addr(cpu_to_le16(p), src)) { 17273b2ec18SGustavo F. Padovan chan->psm = cpu_to_le16(p); 17373b2ec18SGustavo F. Padovan chan->sport = cpu_to_le16(p); 17473b2ec18SGustavo F. Padovan err = 0; 17573b2ec18SGustavo F. Padovan break; 17673b2ec18SGustavo F. Padovan } 17773b2ec18SGustavo F. Padovan } 17873b2ec18SGustavo F. Padovan 17973b2ec18SGustavo F. Padovan done: 18023691d75SGustavo F. Padovan write_unlock_bh(&chan_list_lock); 18173b2ec18SGustavo F. Padovan return err; 1829e4425ffSGustavo F. Padovan } 1839e4425ffSGustavo F. Padovan 1849e4425ffSGustavo F. Padovan int l2cap_add_scid(struct l2cap_chan *chan, __u16 scid) 1859e4425ffSGustavo F. Padovan { 18623691d75SGustavo F. Padovan write_lock_bh(&chan_list_lock); 1879e4425ffSGustavo F. Padovan 1889e4425ffSGustavo F. Padovan chan->scid = scid; 1899e4425ffSGustavo F. Padovan 19023691d75SGustavo F. Padovan write_unlock_bh(&chan_list_lock); 1919e4425ffSGustavo F. Padovan 1929e4425ffSGustavo F. Padovan return 0; 1939e4425ffSGustavo F. Padovan } 1949e4425ffSGustavo F. Padovan 195baa7e1faSGustavo F. Padovan static u16 l2cap_alloc_cid(struct l2cap_conn *conn) 1960a708f8fSGustavo F. Padovan { 1970a708f8fSGustavo F. Padovan u16 cid = L2CAP_CID_DYN_START; 1980a708f8fSGustavo F. Padovan 1990a708f8fSGustavo F. Padovan for (; cid < L2CAP_CID_DYN_END; cid++) { 200baa7e1faSGustavo F. Padovan if (!__l2cap_get_chan_by_scid(conn, cid)) 2010a708f8fSGustavo F. Padovan return cid; 2020a708f8fSGustavo F. Padovan } 2030a708f8fSGustavo F. Padovan 2040a708f8fSGustavo F. Padovan return 0; 2050a708f8fSGustavo F. Padovan } 2060a708f8fSGustavo F. Padovan 20723691d75SGustavo F. Padovan struct l2cap_chan *l2cap_chan_create(struct sock *sk) 2080a708f8fSGustavo F. Padovan { 20948454079SGustavo F. Padovan struct l2cap_chan *chan; 2100a708f8fSGustavo F. Padovan 21148454079SGustavo F. Padovan chan = kzalloc(sizeof(*chan), GFP_ATOMIC); 21248454079SGustavo F. Padovan if (!chan) 21348454079SGustavo F. Padovan return NULL; 2140a708f8fSGustavo F. Padovan 21548454079SGustavo F. Padovan chan->sk = sk; 21648454079SGustavo F. Padovan 21723691d75SGustavo F. Padovan write_lock_bh(&chan_list_lock); 21823691d75SGustavo F. Padovan list_add(&chan->global_l, &chan_list); 21923691d75SGustavo F. Padovan write_unlock_bh(&chan_list_lock); 22023691d75SGustavo F. Padovan 22148454079SGustavo F. Padovan return chan; 2220a708f8fSGustavo F. Padovan } 2230a708f8fSGustavo F. Padovan 22423691d75SGustavo F. Padovan void l2cap_chan_destroy(struct l2cap_chan *chan) 2256ff5abbfSGustavo F. Padovan { 22623691d75SGustavo F. Padovan write_lock_bh(&chan_list_lock); 22723691d75SGustavo F. Padovan list_del(&chan->global_l); 22823691d75SGustavo F. Padovan write_unlock_bh(&chan_list_lock); 22923691d75SGustavo F. Padovan 2306ff5abbfSGustavo F. Padovan kfree(chan); 2316ff5abbfSGustavo F. Padovan } 2326ff5abbfSGustavo F. Padovan 23348454079SGustavo F. Padovan static void __l2cap_chan_add(struct l2cap_conn *conn, struct l2cap_chan *chan) 2340a708f8fSGustavo F. Padovan { 23548454079SGustavo F. Padovan struct sock *sk = chan->sk; 2360a708f8fSGustavo F. Padovan 2370a708f8fSGustavo F. Padovan BT_DBG("conn %p, psm 0x%2.2x, dcid 0x%4.4x", conn, 238fe4128e0SGustavo F. Padovan chan->psm, chan->dcid); 2390a708f8fSGustavo F. Padovan 2400a708f8fSGustavo F. Padovan conn->disc_reason = 0x13; 2410a708f8fSGustavo F. Padovan 2428c1d787bSGustavo F. Padovan chan->conn = conn; 2430a708f8fSGustavo F. Padovan 2440a708f8fSGustavo F. Padovan if (sk->sk_type == SOCK_SEQPACKET || sk->sk_type == SOCK_STREAM) { 245b62f328bSVille Tervo if (conn->hcon->type == LE_LINK) { 246b62f328bSVille Tervo /* LE connection */ 2470c1bc5c6SGustavo F. Padovan chan->omtu = L2CAP_LE_DEFAULT_MTU; 248fe4128e0SGustavo F. Padovan chan->scid = L2CAP_CID_LE_DATA; 249fe4128e0SGustavo F. Padovan chan->dcid = L2CAP_CID_LE_DATA; 250b62f328bSVille Tervo } else { 2510a708f8fSGustavo F. Padovan /* Alloc CID for connection-oriented socket */ 252fe4128e0SGustavo F. Padovan chan->scid = l2cap_alloc_cid(conn); 2530c1bc5c6SGustavo F. Padovan chan->omtu = L2CAP_DEFAULT_MTU; 254b62f328bSVille Tervo } 2550a708f8fSGustavo F. Padovan } else if (sk->sk_type == SOCK_DGRAM) { 2560a708f8fSGustavo F. Padovan /* Connectionless socket */ 257fe4128e0SGustavo F. Padovan chan->scid = L2CAP_CID_CONN_LESS; 258fe4128e0SGustavo F. Padovan chan->dcid = L2CAP_CID_CONN_LESS; 2590c1bc5c6SGustavo F. Padovan chan->omtu = L2CAP_DEFAULT_MTU; 2600a708f8fSGustavo F. Padovan } else { 2610a708f8fSGustavo F. Padovan /* Raw socket can send/recv signalling messages only */ 262fe4128e0SGustavo F. Padovan chan->scid = L2CAP_CID_SIGNALING; 263fe4128e0SGustavo F. Padovan chan->dcid = L2CAP_CID_SIGNALING; 2640c1bc5c6SGustavo F. Padovan chan->omtu = L2CAP_DEFAULT_MTU; 2650a708f8fSGustavo F. Padovan } 2660a708f8fSGustavo F. Padovan 267baa7e1faSGustavo F. Padovan sock_hold(sk); 268baa7e1faSGustavo F. Padovan 269baa7e1faSGustavo F. Padovan list_add(&chan->list, &conn->chan_l); 2700a708f8fSGustavo F. Padovan } 2710a708f8fSGustavo F. Padovan 2720a708f8fSGustavo F. Padovan /* Delete channel. 2730a708f8fSGustavo F. Padovan * Must be called on the locked socket. */ 27448454079SGustavo F. Padovan void l2cap_chan_del(struct l2cap_chan *chan, int err) 2750a708f8fSGustavo F. Padovan { 27648454079SGustavo F. Padovan struct sock *sk = chan->sk; 2778c1d787bSGustavo F. Padovan struct l2cap_conn *conn = chan->conn; 2780a708f8fSGustavo F. Padovan struct sock *parent = bt_sk(sk)->parent; 2790a708f8fSGustavo F. Padovan 2800a708f8fSGustavo F. Padovan l2cap_sock_clear_timer(sk); 2810a708f8fSGustavo F. Padovan 28249208c9cSGustavo F. Padovan BT_DBG("chan %p, conn %p, err %d", chan, conn, err); 2830a708f8fSGustavo F. Padovan 2840a708f8fSGustavo F. Padovan if (conn) { 285baa7e1faSGustavo F. Padovan /* Delete from channel list */ 286baa7e1faSGustavo F. Padovan write_lock_bh(&conn->chan_lock); 287baa7e1faSGustavo F. Padovan list_del(&chan->list); 288baa7e1faSGustavo F. Padovan write_unlock_bh(&conn->chan_lock); 289baa7e1faSGustavo F. Padovan __sock_put(sk); 290baa7e1faSGustavo F. Padovan 2918c1d787bSGustavo F. Padovan chan->conn = NULL; 2920a708f8fSGustavo F. Padovan hci_conn_put(conn->hcon); 2930a708f8fSGustavo F. Padovan } 2940a708f8fSGustavo F. Padovan 2950a708f8fSGustavo F. Padovan sk->sk_state = BT_CLOSED; 2960a708f8fSGustavo F. Padovan sock_set_flag(sk, SOCK_ZAPPED); 2970a708f8fSGustavo F. Padovan 2980a708f8fSGustavo F. Padovan if (err) 2990a708f8fSGustavo F. Padovan sk->sk_err = err; 3000a708f8fSGustavo F. Padovan 3010a708f8fSGustavo F. Padovan if (parent) { 3020a708f8fSGustavo F. Padovan bt_accept_unlink(sk); 3030a708f8fSGustavo F. Padovan parent->sk_data_ready(parent, 0); 3040a708f8fSGustavo F. Padovan } else 3050a708f8fSGustavo F. Padovan sk->sk_state_change(sk); 3060a708f8fSGustavo F. Padovan 307b4450035SGustavo F. Padovan if (!(chan->conf_state & L2CAP_CONF_OUTPUT_DONE && 308b4450035SGustavo F. Padovan chan->conf_state & L2CAP_CONF_INPUT_DONE)) 3096ff5abbfSGustavo F. Padovan return; 3102ead70b8SGustavo F. Padovan 31158d35f87SGustavo F. Padovan skb_queue_purge(&chan->tx_q); 3120a708f8fSGustavo F. Padovan 3130c1bc5c6SGustavo F. Padovan if (chan->mode == L2CAP_MODE_ERTM) { 3140a708f8fSGustavo F. Padovan struct srej_list *l, *tmp; 3150a708f8fSGustavo F. Padovan 316e92c8e70SGustavo F. Padovan del_timer(&chan->retrans_timer); 317e92c8e70SGustavo F. Padovan del_timer(&chan->monitor_timer); 318e92c8e70SGustavo F. Padovan del_timer(&chan->ack_timer); 3190a708f8fSGustavo F. Padovan 320f1c6775bSGustavo F. Padovan skb_queue_purge(&chan->srej_q); 321f1c6775bSGustavo F. Padovan skb_queue_purge(&chan->busy_q); 3220a708f8fSGustavo F. Padovan 32339d5a3eeSGustavo F. Padovan list_for_each_entry_safe(l, tmp, &chan->srej_l, list) { 3240a708f8fSGustavo F. Padovan list_del(&l->list); 3250a708f8fSGustavo F. Padovan kfree(l); 3260a708f8fSGustavo F. Padovan } 3270a708f8fSGustavo F. Padovan } 3280a708f8fSGustavo F. Padovan } 3290a708f8fSGustavo F. Padovan 3304343478fSGustavo F. Padovan static inline u8 l2cap_get_auth_type(struct l2cap_chan *chan) 3310a708f8fSGustavo F. Padovan { 3324343478fSGustavo F. Padovan struct sock *sk = chan->sk; 3334343478fSGustavo F. Padovan 3340a708f8fSGustavo F. Padovan if (sk->sk_type == SOCK_RAW) { 3354343478fSGustavo F. Padovan switch (chan->sec_level) { 3360a708f8fSGustavo F. Padovan case BT_SECURITY_HIGH: 3370a708f8fSGustavo F. Padovan return HCI_AT_DEDICATED_BONDING_MITM; 3380a708f8fSGustavo F. Padovan case BT_SECURITY_MEDIUM: 3390a708f8fSGustavo F. Padovan return HCI_AT_DEDICATED_BONDING; 3400a708f8fSGustavo F. Padovan default: 3410a708f8fSGustavo F. Padovan return HCI_AT_NO_BONDING; 3420a708f8fSGustavo F. Padovan } 343fe4128e0SGustavo F. Padovan } else if (chan->psm == cpu_to_le16(0x0001)) { 3444343478fSGustavo F. Padovan if (chan->sec_level == BT_SECURITY_LOW) 3454343478fSGustavo F. Padovan chan->sec_level = BT_SECURITY_SDP; 3460a708f8fSGustavo F. Padovan 3474343478fSGustavo F. Padovan if (chan->sec_level == BT_SECURITY_HIGH) 3480a708f8fSGustavo F. Padovan return HCI_AT_NO_BONDING_MITM; 3490a708f8fSGustavo F. Padovan else 3500a708f8fSGustavo F. Padovan return HCI_AT_NO_BONDING; 3510a708f8fSGustavo F. Padovan } else { 3524343478fSGustavo F. Padovan switch (chan->sec_level) { 3530a708f8fSGustavo F. Padovan case BT_SECURITY_HIGH: 3540a708f8fSGustavo F. Padovan return HCI_AT_GENERAL_BONDING_MITM; 3550a708f8fSGustavo F. Padovan case BT_SECURITY_MEDIUM: 3560a708f8fSGustavo F. Padovan return HCI_AT_GENERAL_BONDING; 3570a708f8fSGustavo F. Padovan default: 3580a708f8fSGustavo F. Padovan return HCI_AT_NO_BONDING; 3590a708f8fSGustavo F. Padovan } 3600a708f8fSGustavo F. Padovan } 3610a708f8fSGustavo F. Padovan } 3620a708f8fSGustavo F. Padovan 3630a708f8fSGustavo F. Padovan /* Service level security */ 3644343478fSGustavo F. Padovan static inline int l2cap_check_security(struct l2cap_chan *chan) 3650a708f8fSGustavo F. Padovan { 3668c1d787bSGustavo F. Padovan struct l2cap_conn *conn = chan->conn; 3670a708f8fSGustavo F. Padovan __u8 auth_type; 3680a708f8fSGustavo F. Padovan 3694343478fSGustavo F. Padovan auth_type = l2cap_get_auth_type(chan); 3700a708f8fSGustavo F. Padovan 3714343478fSGustavo F. Padovan return hci_conn_security(conn->hcon, chan->sec_level, auth_type); 3720a708f8fSGustavo F. Padovan } 3730a708f8fSGustavo F. Padovan 37468983259SGustavo F. Padovan u8 l2cap_get_ident(struct l2cap_conn *conn) 3750a708f8fSGustavo F. Padovan { 3760a708f8fSGustavo F. Padovan u8 id; 3770a708f8fSGustavo F. Padovan 3780a708f8fSGustavo F. Padovan /* Get next available identificator. 3790a708f8fSGustavo F. Padovan * 1 - 128 are used by kernel. 3800a708f8fSGustavo F. Padovan * 129 - 199 are reserved. 3810a708f8fSGustavo F. Padovan * 200 - 254 are used by utilities like l2ping, etc. 3820a708f8fSGustavo F. Padovan */ 3830a708f8fSGustavo F. Padovan 3840a708f8fSGustavo F. Padovan spin_lock_bh(&conn->lock); 3850a708f8fSGustavo F. Padovan 3860a708f8fSGustavo F. Padovan if (++conn->tx_ident > 128) 3870a708f8fSGustavo F. Padovan conn->tx_ident = 1; 3880a708f8fSGustavo F. Padovan 3890a708f8fSGustavo F. Padovan id = conn->tx_ident; 3900a708f8fSGustavo F. Padovan 3910a708f8fSGustavo F. Padovan spin_unlock_bh(&conn->lock); 3920a708f8fSGustavo F. Padovan 3930a708f8fSGustavo F. Padovan return id; 3940a708f8fSGustavo F. Padovan } 3950a708f8fSGustavo F. Padovan 39668983259SGustavo F. Padovan void l2cap_send_cmd(struct l2cap_conn *conn, u8 ident, u8 code, u16 len, void *data) 3970a708f8fSGustavo F. Padovan { 3980a708f8fSGustavo F. Padovan struct sk_buff *skb = l2cap_build_cmd(conn, code, ident, len, data); 3990a708f8fSGustavo F. Padovan u8 flags; 4000a708f8fSGustavo F. Padovan 4010a708f8fSGustavo F. Padovan BT_DBG("code 0x%2.2x", code); 4020a708f8fSGustavo F. Padovan 4030a708f8fSGustavo F. Padovan if (!skb) 4040a708f8fSGustavo F. Padovan return; 4050a708f8fSGustavo F. Padovan 4060a708f8fSGustavo F. Padovan if (lmp_no_flush_capable(conn->hcon->hdev)) 4070a708f8fSGustavo F. Padovan flags = ACL_START_NO_FLUSH; 4080a708f8fSGustavo F. Padovan else 4090a708f8fSGustavo F. Padovan flags = ACL_START; 4100a708f8fSGustavo F. Padovan 4110a708f8fSGustavo F. Padovan hci_send_acl(conn->hcon, skb, flags); 4120a708f8fSGustavo F. Padovan } 4130a708f8fSGustavo F. Padovan 414525cd185SGustavo F. Padovan static inline void l2cap_send_sframe(struct l2cap_chan *chan, u16 control) 4150a708f8fSGustavo F. Padovan { 4160a708f8fSGustavo F. Padovan struct sk_buff *skb; 4170a708f8fSGustavo F. Padovan struct l2cap_hdr *lh; 418525cd185SGustavo F. Padovan struct l2cap_pinfo *pi = l2cap_pi(chan->sk); 4198c1d787bSGustavo F. Padovan struct l2cap_conn *conn = chan->conn; 4200a708f8fSGustavo F. Padovan struct sock *sk = (struct sock *)pi; 4210a708f8fSGustavo F. Padovan int count, hlen = L2CAP_HDR_SIZE + 2; 4220a708f8fSGustavo F. Padovan u8 flags; 4230a708f8fSGustavo F. Padovan 4240a708f8fSGustavo F. Padovan if (sk->sk_state != BT_CONNECTED) 4250a708f8fSGustavo F. Padovan return; 4260a708f8fSGustavo F. Padovan 42747d1ec61SGustavo F. Padovan if (chan->fcs == L2CAP_FCS_CRC16) 4280a708f8fSGustavo F. Padovan hlen += 2; 4290a708f8fSGustavo F. Padovan 43049208c9cSGustavo F. Padovan BT_DBG("chan %p, control 0x%2.2x", chan, control); 4310a708f8fSGustavo F. Padovan 4320a708f8fSGustavo F. Padovan count = min_t(unsigned int, conn->mtu, hlen); 4330a708f8fSGustavo F. Padovan control |= L2CAP_CTRL_FRAME_TYPE; 4340a708f8fSGustavo F. Padovan 435525cd185SGustavo F. Padovan if (chan->conn_state & L2CAP_CONN_SEND_FBIT) { 4360a708f8fSGustavo F. Padovan control |= L2CAP_CTRL_FINAL; 437525cd185SGustavo F. Padovan chan->conn_state &= ~L2CAP_CONN_SEND_FBIT; 4380a708f8fSGustavo F. Padovan } 4390a708f8fSGustavo F. Padovan 440525cd185SGustavo F. Padovan if (chan->conn_state & L2CAP_CONN_SEND_PBIT) { 4410a708f8fSGustavo F. Padovan control |= L2CAP_CTRL_POLL; 442525cd185SGustavo F. Padovan chan->conn_state &= ~L2CAP_CONN_SEND_PBIT; 4430a708f8fSGustavo F. Padovan } 4440a708f8fSGustavo F. Padovan 4450a708f8fSGustavo F. Padovan skb = bt_skb_alloc(count, GFP_ATOMIC); 4460a708f8fSGustavo F. Padovan if (!skb) 4470a708f8fSGustavo F. Padovan return; 4480a708f8fSGustavo F. Padovan 4490a708f8fSGustavo F. Padovan lh = (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE); 4500a708f8fSGustavo F. Padovan lh->len = cpu_to_le16(hlen - L2CAP_HDR_SIZE); 451fe4128e0SGustavo F. Padovan lh->cid = cpu_to_le16(chan->dcid); 4520a708f8fSGustavo F. Padovan put_unaligned_le16(control, skb_put(skb, 2)); 4530a708f8fSGustavo F. Padovan 45447d1ec61SGustavo F. Padovan if (chan->fcs == L2CAP_FCS_CRC16) { 4550a708f8fSGustavo F. Padovan u16 fcs = crc16(0, (u8 *)lh, count - 2); 4560a708f8fSGustavo F. Padovan put_unaligned_le16(fcs, skb_put(skb, 2)); 4570a708f8fSGustavo F. Padovan } 4580a708f8fSGustavo F. Padovan 4590a708f8fSGustavo F. Padovan if (lmp_no_flush_capable(conn->hcon->hdev)) 4600a708f8fSGustavo F. Padovan flags = ACL_START_NO_FLUSH; 4610a708f8fSGustavo F. Padovan else 4620a708f8fSGustavo F. Padovan flags = ACL_START; 4630a708f8fSGustavo F. Padovan 4648c1d787bSGustavo F. Padovan hci_send_acl(chan->conn->hcon, skb, flags); 4650a708f8fSGustavo F. Padovan } 4660a708f8fSGustavo F. Padovan 467525cd185SGustavo F. Padovan static inline void l2cap_send_rr_or_rnr(struct l2cap_chan *chan, u16 control) 4680a708f8fSGustavo F. Padovan { 469525cd185SGustavo F. Padovan if (chan->conn_state & L2CAP_CONN_LOCAL_BUSY) { 4700a708f8fSGustavo F. Padovan control |= L2CAP_SUPER_RCV_NOT_READY; 471525cd185SGustavo F. Padovan chan->conn_state |= L2CAP_CONN_RNR_SENT; 4720a708f8fSGustavo F. Padovan } else 4730a708f8fSGustavo F. Padovan control |= L2CAP_SUPER_RCV_READY; 4740a708f8fSGustavo F. Padovan 47542e5c802SGustavo F. Padovan control |= chan->buffer_seq << L2CAP_CTRL_REQSEQ_SHIFT; 4760a708f8fSGustavo F. Padovan 477525cd185SGustavo F. Padovan l2cap_send_sframe(chan, control); 4780a708f8fSGustavo F. Padovan } 4790a708f8fSGustavo F. Padovan 480b4450035SGustavo F. Padovan static inline int __l2cap_no_conn_pending(struct l2cap_chan *chan) 4810a708f8fSGustavo F. Padovan { 482b4450035SGustavo F. Padovan return !(chan->conf_state & L2CAP_CONF_CONNECT_PEND); 4830a708f8fSGustavo F. Padovan } 4840a708f8fSGustavo F. Padovan 485fc7f8a7eSGustavo F. Padovan static void l2cap_do_start(struct l2cap_chan *chan) 4860a708f8fSGustavo F. Padovan { 4878c1d787bSGustavo F. Padovan struct l2cap_conn *conn = chan->conn; 4880a708f8fSGustavo F. Padovan 4890a708f8fSGustavo F. Padovan if (conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_SENT) { 4900a708f8fSGustavo F. Padovan if (!(conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_DONE)) 4910a708f8fSGustavo F. Padovan return; 4920a708f8fSGustavo F. Padovan 4934343478fSGustavo F. Padovan if (l2cap_check_security(chan) && 4944343478fSGustavo F. Padovan __l2cap_no_conn_pending(chan)) { 4950a708f8fSGustavo F. Padovan struct l2cap_conn_req req; 496fe4128e0SGustavo F. Padovan req.scid = cpu_to_le16(chan->scid); 497fe4128e0SGustavo F. Padovan req.psm = chan->psm; 4980a708f8fSGustavo F. Padovan 499fc7f8a7eSGustavo F. Padovan chan->ident = l2cap_get_ident(conn); 500b4450035SGustavo F. Padovan chan->conf_state |= L2CAP_CONF_CONNECT_PEND; 5010a708f8fSGustavo F. Padovan 502fc7f8a7eSGustavo F. Padovan l2cap_send_cmd(conn, chan->ident, L2CAP_CONN_REQ, 503fc7f8a7eSGustavo F. Padovan sizeof(req), &req); 5040a708f8fSGustavo F. Padovan } 5050a708f8fSGustavo F. Padovan } else { 5060a708f8fSGustavo F. Padovan struct l2cap_info_req req; 5070a708f8fSGustavo F. Padovan req.type = cpu_to_le16(L2CAP_IT_FEAT_MASK); 5080a708f8fSGustavo F. Padovan 5090a708f8fSGustavo F. Padovan conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_SENT; 5100a708f8fSGustavo F. Padovan conn->info_ident = l2cap_get_ident(conn); 5110a708f8fSGustavo F. Padovan 5120a708f8fSGustavo F. Padovan mod_timer(&conn->info_timer, jiffies + 5130a708f8fSGustavo F. Padovan msecs_to_jiffies(L2CAP_INFO_TIMEOUT)); 5140a708f8fSGustavo F. Padovan 5150a708f8fSGustavo F. Padovan l2cap_send_cmd(conn, conn->info_ident, 5160a708f8fSGustavo F. Padovan L2CAP_INFO_REQ, sizeof(req), &req); 5170a708f8fSGustavo F. Padovan } 5180a708f8fSGustavo F. Padovan } 5190a708f8fSGustavo F. Padovan 5200a708f8fSGustavo F. Padovan static inline int l2cap_mode_supported(__u8 mode, __u32 feat_mask) 5210a708f8fSGustavo F. Padovan { 5220a708f8fSGustavo F. Padovan u32 local_feat_mask = l2cap_feat_mask; 5230a708f8fSGustavo F. Padovan if (!disable_ertm) 5240a708f8fSGustavo F. Padovan local_feat_mask |= L2CAP_FEAT_ERTM | L2CAP_FEAT_STREAMING; 5250a708f8fSGustavo F. Padovan 5260a708f8fSGustavo F. Padovan switch (mode) { 5270a708f8fSGustavo F. Padovan case L2CAP_MODE_ERTM: 5280a708f8fSGustavo F. Padovan return L2CAP_FEAT_ERTM & feat_mask & local_feat_mask; 5290a708f8fSGustavo F. Padovan case L2CAP_MODE_STREAMING: 5300a708f8fSGustavo F. Padovan return L2CAP_FEAT_STREAMING & feat_mask & local_feat_mask; 5310a708f8fSGustavo F. Padovan default: 5320a708f8fSGustavo F. Padovan return 0x00; 5330a708f8fSGustavo F. Padovan } 5340a708f8fSGustavo F. Padovan } 5350a708f8fSGustavo F. Padovan 536e92c8e70SGustavo F. Padovan void l2cap_send_disconn_req(struct l2cap_conn *conn, struct l2cap_chan *chan, int err) 5370a708f8fSGustavo F. Padovan { 538e92c8e70SGustavo F. Padovan struct sock *sk; 5390a708f8fSGustavo F. Padovan struct l2cap_disconn_req req; 5400a708f8fSGustavo F. Padovan 5410a708f8fSGustavo F. Padovan if (!conn) 5420a708f8fSGustavo F. Padovan return; 5430a708f8fSGustavo F. Padovan 544e92c8e70SGustavo F. Padovan sk = chan->sk; 545e92c8e70SGustavo F. Padovan 5460c1bc5c6SGustavo F. Padovan if (chan->mode == L2CAP_MODE_ERTM) { 547e92c8e70SGustavo F. Padovan del_timer(&chan->retrans_timer); 548e92c8e70SGustavo F. Padovan del_timer(&chan->monitor_timer); 549e92c8e70SGustavo F. Padovan del_timer(&chan->ack_timer); 5500a708f8fSGustavo F. Padovan } 5510a708f8fSGustavo F. Padovan 552fe4128e0SGustavo F. Padovan req.dcid = cpu_to_le16(chan->dcid); 553fe4128e0SGustavo F. Padovan req.scid = cpu_to_le16(chan->scid); 5540a708f8fSGustavo F. Padovan l2cap_send_cmd(conn, l2cap_get_ident(conn), 5550a708f8fSGustavo F. Padovan L2CAP_DISCONN_REQ, sizeof(req), &req); 5560a708f8fSGustavo F. Padovan 5570a708f8fSGustavo F. Padovan sk->sk_state = BT_DISCONN; 5580a708f8fSGustavo F. Padovan sk->sk_err = err; 5590a708f8fSGustavo F. Padovan } 5600a708f8fSGustavo F. Padovan 5610a708f8fSGustavo F. Padovan /* ---- L2CAP connections ---- */ 5620a708f8fSGustavo F. Padovan static void l2cap_conn_start(struct l2cap_conn *conn) 5630a708f8fSGustavo F. Padovan { 564820ffdb3SGustavo F. Padovan struct l2cap_chan *chan, *tmp; 5650a708f8fSGustavo F. Padovan 5660a708f8fSGustavo F. Padovan BT_DBG("conn %p", conn); 5670a708f8fSGustavo F. Padovan 568baa7e1faSGustavo F. Padovan read_lock(&conn->chan_lock); 5690a708f8fSGustavo F. Padovan 570820ffdb3SGustavo F. Padovan list_for_each_entry_safe(chan, tmp, &conn->chan_l, list) { 57148454079SGustavo F. Padovan struct sock *sk = chan->sk; 572baa7e1faSGustavo F. Padovan 5730a708f8fSGustavo F. Padovan bh_lock_sock(sk); 5740a708f8fSGustavo F. Padovan 5750a708f8fSGustavo F. Padovan if (sk->sk_type != SOCK_SEQPACKET && 5760a708f8fSGustavo F. Padovan sk->sk_type != SOCK_STREAM) { 5770a708f8fSGustavo F. Padovan bh_unlock_sock(sk); 5780a708f8fSGustavo F. Padovan continue; 5790a708f8fSGustavo F. Padovan } 5800a708f8fSGustavo F. Padovan 5810a708f8fSGustavo F. Padovan if (sk->sk_state == BT_CONNECT) { 5820a708f8fSGustavo F. Padovan struct l2cap_conn_req req; 5830a708f8fSGustavo F. Padovan 5844343478fSGustavo F. Padovan if (!l2cap_check_security(chan) || 585b4450035SGustavo F. Padovan !__l2cap_no_conn_pending(chan)) { 5860a708f8fSGustavo F. Padovan bh_unlock_sock(sk); 5870a708f8fSGustavo F. Padovan continue; 5880a708f8fSGustavo F. Padovan } 5890a708f8fSGustavo F. Padovan 5900c1bc5c6SGustavo F. Padovan if (!l2cap_mode_supported(chan->mode, 5910a708f8fSGustavo F. Padovan conn->feat_mask) 592b4450035SGustavo F. Padovan && chan->conf_state & 5930a708f8fSGustavo F. Padovan L2CAP_CONF_STATE2_DEVICE) { 594820ffdb3SGustavo F. Padovan /* __l2cap_sock_close() calls list_del(chan) 595820ffdb3SGustavo F. Padovan * so release the lock */ 596820ffdb3SGustavo F. Padovan read_unlock_bh(&conn->chan_lock); 597820ffdb3SGustavo F. Padovan __l2cap_sock_close(sk, ECONNRESET); 598820ffdb3SGustavo F. Padovan read_lock_bh(&conn->chan_lock); 5990a708f8fSGustavo F. Padovan bh_unlock_sock(sk); 6000a708f8fSGustavo F. Padovan continue; 6010a708f8fSGustavo F. Padovan } 6020a708f8fSGustavo F. Padovan 603fe4128e0SGustavo F. Padovan req.scid = cpu_to_le16(chan->scid); 604fe4128e0SGustavo F. Padovan req.psm = chan->psm; 6050a708f8fSGustavo F. Padovan 606fc7f8a7eSGustavo F. Padovan chan->ident = l2cap_get_ident(conn); 607b4450035SGustavo F. Padovan chan->conf_state |= L2CAP_CONF_CONNECT_PEND; 6080a708f8fSGustavo F. Padovan 609fc7f8a7eSGustavo F. Padovan l2cap_send_cmd(conn, chan->ident, L2CAP_CONN_REQ, 610fc7f8a7eSGustavo F. Padovan sizeof(req), &req); 6110a708f8fSGustavo F. Padovan 6120a708f8fSGustavo F. Padovan } else if (sk->sk_state == BT_CONNECT2) { 6130a708f8fSGustavo F. Padovan struct l2cap_conn_rsp rsp; 6140a708f8fSGustavo F. Padovan char buf[128]; 615fe4128e0SGustavo F. Padovan rsp.scid = cpu_to_le16(chan->dcid); 616fe4128e0SGustavo F. Padovan rsp.dcid = cpu_to_le16(chan->scid); 6170a708f8fSGustavo F. Padovan 6184343478fSGustavo F. Padovan if (l2cap_check_security(chan)) { 6190a708f8fSGustavo F. Padovan if (bt_sk(sk)->defer_setup) { 6200a708f8fSGustavo F. Padovan struct sock *parent = bt_sk(sk)->parent; 6210a708f8fSGustavo F. Padovan rsp.result = cpu_to_le16(L2CAP_CR_PEND); 6220a708f8fSGustavo F. Padovan rsp.status = cpu_to_le16(L2CAP_CS_AUTHOR_PEND); 6230a708f8fSGustavo F. Padovan parent->sk_data_ready(parent, 0); 6240a708f8fSGustavo F. Padovan 6250a708f8fSGustavo F. Padovan } else { 6260a708f8fSGustavo F. Padovan sk->sk_state = BT_CONFIG; 6270a708f8fSGustavo F. Padovan rsp.result = cpu_to_le16(L2CAP_CR_SUCCESS); 6280a708f8fSGustavo F. Padovan rsp.status = cpu_to_le16(L2CAP_CS_NO_INFO); 6290a708f8fSGustavo F. Padovan } 6300a708f8fSGustavo F. Padovan } else { 6310a708f8fSGustavo F. Padovan rsp.result = cpu_to_le16(L2CAP_CR_PEND); 6320a708f8fSGustavo F. Padovan rsp.status = cpu_to_le16(L2CAP_CS_AUTHEN_PEND); 6330a708f8fSGustavo F. Padovan } 6340a708f8fSGustavo F. Padovan 635fc7f8a7eSGustavo F. Padovan l2cap_send_cmd(conn, chan->ident, L2CAP_CONN_RSP, 636fc7f8a7eSGustavo F. Padovan sizeof(rsp), &rsp); 6370a708f8fSGustavo F. Padovan 638b4450035SGustavo F. Padovan if (chan->conf_state & L2CAP_CONF_REQ_SENT || 6390a708f8fSGustavo F. Padovan rsp.result != L2CAP_CR_SUCCESS) { 6400a708f8fSGustavo F. Padovan bh_unlock_sock(sk); 6410a708f8fSGustavo F. Padovan continue; 6420a708f8fSGustavo F. Padovan } 6430a708f8fSGustavo F. Padovan 644b4450035SGustavo F. Padovan chan->conf_state |= L2CAP_CONF_REQ_SENT; 6450a708f8fSGustavo F. Padovan l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ, 64673ffa904SGustavo F. Padovan l2cap_build_conf_req(chan, buf), buf); 64773ffa904SGustavo F. Padovan chan->num_conf_req++; 6480a708f8fSGustavo F. Padovan } 6490a708f8fSGustavo F. Padovan 6500a708f8fSGustavo F. Padovan bh_unlock_sock(sk); 6510a708f8fSGustavo F. Padovan } 6520a708f8fSGustavo F. Padovan 653baa7e1faSGustavo F. Padovan read_unlock(&conn->chan_lock); 6540a708f8fSGustavo F. Padovan } 6550a708f8fSGustavo F. Padovan 656b62f328bSVille Tervo /* Find socket with cid and source bdaddr. 657b62f328bSVille Tervo * Returns closest match, locked. 658b62f328bSVille Tervo */ 65923691d75SGustavo F. Padovan static struct l2cap_chan *l2cap_global_chan_by_scid(int state, __le16 cid, bdaddr_t *src) 660b62f328bSVille Tervo { 66123691d75SGustavo F. Padovan struct l2cap_chan *c, *c1 = NULL; 662b62f328bSVille Tervo 66323691d75SGustavo F. Padovan read_lock(&chan_list_lock); 664b62f328bSVille Tervo 66523691d75SGustavo F. Padovan list_for_each_entry(c, &chan_list, global_l) { 66623691d75SGustavo F. Padovan struct sock *sk = c->sk; 667fe4128e0SGustavo F. Padovan 668b62f328bSVille Tervo if (state && sk->sk_state != state) 669b62f328bSVille Tervo continue; 670b62f328bSVille Tervo 67123691d75SGustavo F. Padovan if (c->scid == cid) { 672b62f328bSVille Tervo /* Exact match. */ 67323691d75SGustavo F. Padovan if (!bacmp(&bt_sk(sk)->src, src)) { 67423691d75SGustavo F. Padovan read_unlock(&chan_list_lock); 67523691d75SGustavo F. Padovan return c; 67623691d75SGustavo F. Padovan } 677b62f328bSVille Tervo 678b62f328bSVille Tervo /* Closest match */ 679b62f328bSVille Tervo if (!bacmp(&bt_sk(sk)->src, BDADDR_ANY)) 68023691d75SGustavo F. Padovan c1 = c; 681b62f328bSVille Tervo } 682b62f328bSVille Tervo } 683280f294fSGustavo F. Padovan 68423691d75SGustavo F. Padovan read_unlock(&chan_list_lock); 685b62f328bSVille Tervo 68623691d75SGustavo F. Padovan return c1; 687b62f328bSVille Tervo } 688b62f328bSVille Tervo 689b62f328bSVille Tervo static void l2cap_le_conn_ready(struct l2cap_conn *conn) 690b62f328bSVille Tervo { 691c916fbe4SGustavo F. Padovan struct sock *parent, *sk; 69223691d75SGustavo F. Padovan struct l2cap_chan *chan, *pchan; 693b62f328bSVille Tervo 694b62f328bSVille Tervo BT_DBG(""); 695b62f328bSVille Tervo 696b62f328bSVille Tervo /* Check if we have socket listening on cid */ 69723691d75SGustavo F. Padovan pchan = l2cap_global_chan_by_scid(BT_LISTEN, L2CAP_CID_LE_DATA, 698b62f328bSVille Tervo conn->src); 69923691d75SGustavo F. Padovan if (!pchan) 700b62f328bSVille Tervo return; 701b62f328bSVille Tervo 70223691d75SGustavo F. Padovan parent = pchan->sk; 70323691d75SGustavo F. Padovan 70462f3a2cfSGustavo F. Padovan bh_lock_sock(parent); 70562f3a2cfSGustavo F. Padovan 706b62f328bSVille Tervo /* Check for backlog size */ 707b62f328bSVille Tervo if (sk_acceptq_is_full(parent)) { 708b62f328bSVille Tervo BT_DBG("backlog full %d", parent->sk_ack_backlog); 709b62f328bSVille Tervo goto clean; 710b62f328bSVille Tervo } 711b62f328bSVille Tervo 712b62f328bSVille Tervo sk = l2cap_sock_alloc(sock_net(parent), NULL, BTPROTO_L2CAP, GFP_ATOMIC); 713b62f328bSVille Tervo if (!sk) 714b62f328bSVille Tervo goto clean; 715b62f328bSVille Tervo 71623691d75SGustavo F. Padovan chan = l2cap_chan_create(sk); 71748454079SGustavo F. Padovan if (!chan) { 71848454079SGustavo F. Padovan l2cap_sock_kill(sk); 71948454079SGustavo F. Padovan goto clean; 72048454079SGustavo F. Padovan } 72148454079SGustavo F. Padovan 7225d41ce1dSGustavo F. Padovan l2cap_pi(sk)->chan = chan; 7235d41ce1dSGustavo F. Padovan 724baa7e1faSGustavo F. Padovan write_lock_bh(&conn->chan_lock); 725b62f328bSVille Tervo 726b62f328bSVille Tervo hci_conn_hold(conn->hcon); 727b62f328bSVille Tervo 728b62f328bSVille Tervo l2cap_sock_init(sk, parent); 729baa7e1faSGustavo F. Padovan 730b62f328bSVille Tervo bacpy(&bt_sk(sk)->src, conn->src); 731b62f328bSVille Tervo bacpy(&bt_sk(sk)->dst, conn->dst); 732b62f328bSVille Tervo 733d1010240SGustavo F. Padovan bt_accept_enqueue(parent, sk); 734d1010240SGustavo F. Padovan 73548454079SGustavo F. Padovan __l2cap_chan_add(conn, chan); 73648454079SGustavo F. Padovan 737b62f328bSVille Tervo l2cap_sock_set_timer(sk, sk->sk_sndtimeo); 738b62f328bSVille Tervo 739b62f328bSVille Tervo sk->sk_state = BT_CONNECTED; 740b62f328bSVille Tervo parent->sk_data_ready(parent, 0); 741b62f328bSVille Tervo 742baa7e1faSGustavo F. Padovan write_unlock_bh(&conn->chan_lock); 743b62f328bSVille Tervo 744b62f328bSVille Tervo clean: 745b62f328bSVille Tervo bh_unlock_sock(parent); 746b62f328bSVille Tervo } 747b62f328bSVille Tervo 7480a708f8fSGustavo F. Padovan static void l2cap_conn_ready(struct l2cap_conn *conn) 7490a708f8fSGustavo F. Padovan { 75048454079SGustavo F. Padovan struct l2cap_chan *chan; 7510a708f8fSGustavo F. Padovan 7520a708f8fSGustavo F. Padovan BT_DBG("conn %p", conn); 7530a708f8fSGustavo F. Padovan 754b62f328bSVille Tervo if (!conn->hcon->out && conn->hcon->type == LE_LINK) 755b62f328bSVille Tervo l2cap_le_conn_ready(conn); 756b62f328bSVille Tervo 757baa7e1faSGustavo F. Padovan read_lock(&conn->chan_lock); 7580a708f8fSGustavo F. Padovan 759baa7e1faSGustavo F. Padovan list_for_each_entry(chan, &conn->chan_l, list) { 76048454079SGustavo F. Padovan struct sock *sk = chan->sk; 761baa7e1faSGustavo F. Padovan 7620a708f8fSGustavo F. Padovan bh_lock_sock(sk); 7630a708f8fSGustavo F. Padovan 764acd7d370SVille Tervo if (conn->hcon->type == LE_LINK) { 765acd7d370SVille Tervo l2cap_sock_clear_timer(sk); 766acd7d370SVille Tervo sk->sk_state = BT_CONNECTED; 767acd7d370SVille Tervo sk->sk_state_change(sk); 768acd7d370SVille Tervo } 769acd7d370SVille Tervo 7700a708f8fSGustavo F. Padovan if (sk->sk_type != SOCK_SEQPACKET && 7710a708f8fSGustavo F. Padovan sk->sk_type != SOCK_STREAM) { 7720a708f8fSGustavo F. Padovan l2cap_sock_clear_timer(sk); 7730a708f8fSGustavo F. Padovan sk->sk_state = BT_CONNECTED; 7740a708f8fSGustavo F. Padovan sk->sk_state_change(sk); 7750a708f8fSGustavo F. Padovan } else if (sk->sk_state == BT_CONNECT) 776fc7f8a7eSGustavo F. Padovan l2cap_do_start(chan); 7770a708f8fSGustavo F. Padovan 7780a708f8fSGustavo F. Padovan bh_unlock_sock(sk); 7790a708f8fSGustavo F. Padovan } 7800a708f8fSGustavo F. Padovan 781baa7e1faSGustavo F. Padovan read_unlock(&conn->chan_lock); 7820a708f8fSGustavo F. Padovan } 7830a708f8fSGustavo F. Padovan 7840a708f8fSGustavo F. Padovan /* Notify sockets that we cannot guaranty reliability anymore */ 7850a708f8fSGustavo F. Padovan static void l2cap_conn_unreliable(struct l2cap_conn *conn, int err) 7860a708f8fSGustavo F. Padovan { 78748454079SGustavo F. Padovan struct l2cap_chan *chan; 7880a708f8fSGustavo F. Padovan 7890a708f8fSGustavo F. Padovan BT_DBG("conn %p", conn); 7900a708f8fSGustavo F. Padovan 791baa7e1faSGustavo F. Padovan read_lock(&conn->chan_lock); 7920a708f8fSGustavo F. Padovan 793baa7e1faSGustavo F. Padovan list_for_each_entry(chan, &conn->chan_l, list) { 79448454079SGustavo F. Padovan struct sock *sk = chan->sk; 795baa7e1faSGustavo F. Padovan 7964343478fSGustavo F. Padovan if (chan->force_reliable) 7970a708f8fSGustavo F. Padovan sk->sk_err = err; 7980a708f8fSGustavo F. Padovan } 7990a708f8fSGustavo F. Padovan 800baa7e1faSGustavo F. Padovan read_unlock(&conn->chan_lock); 8010a708f8fSGustavo F. Padovan } 8020a708f8fSGustavo F. Padovan 8030a708f8fSGustavo F. Padovan static void l2cap_info_timeout(unsigned long arg) 8040a708f8fSGustavo F. Padovan { 8050a708f8fSGustavo F. Padovan struct l2cap_conn *conn = (void *) arg; 8060a708f8fSGustavo F. Padovan 8070a708f8fSGustavo F. Padovan conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_DONE; 8080a708f8fSGustavo F. Padovan conn->info_ident = 0; 8090a708f8fSGustavo F. Padovan 8100a708f8fSGustavo F. Padovan l2cap_conn_start(conn); 8110a708f8fSGustavo F. Padovan } 8120a708f8fSGustavo F. Padovan 8130a708f8fSGustavo F. Padovan static struct l2cap_conn *l2cap_conn_add(struct hci_conn *hcon, u8 status) 8140a708f8fSGustavo F. Padovan { 8150a708f8fSGustavo F. Padovan struct l2cap_conn *conn = hcon->l2cap_data; 8160a708f8fSGustavo F. Padovan 8170a708f8fSGustavo F. Padovan if (conn || status) 8180a708f8fSGustavo F. Padovan return conn; 8190a708f8fSGustavo F. Padovan 8200a708f8fSGustavo F. Padovan conn = kzalloc(sizeof(struct l2cap_conn), GFP_ATOMIC); 8210a708f8fSGustavo F. Padovan if (!conn) 8220a708f8fSGustavo F. Padovan return NULL; 8230a708f8fSGustavo F. Padovan 8240a708f8fSGustavo F. Padovan hcon->l2cap_data = conn; 8250a708f8fSGustavo F. Padovan conn->hcon = hcon; 8260a708f8fSGustavo F. Padovan 8270a708f8fSGustavo F. Padovan BT_DBG("hcon %p conn %p", hcon, conn); 8280a708f8fSGustavo F. Padovan 829acd7d370SVille Tervo if (hcon->hdev->le_mtu && hcon->type == LE_LINK) 830acd7d370SVille Tervo conn->mtu = hcon->hdev->le_mtu; 831acd7d370SVille Tervo else 8320a708f8fSGustavo F. Padovan conn->mtu = hcon->hdev->acl_mtu; 833acd7d370SVille Tervo 8340a708f8fSGustavo F. Padovan conn->src = &hcon->hdev->bdaddr; 8350a708f8fSGustavo F. Padovan conn->dst = &hcon->dst; 8360a708f8fSGustavo F. Padovan 8370a708f8fSGustavo F. Padovan conn->feat_mask = 0; 8380a708f8fSGustavo F. Padovan 8390a708f8fSGustavo F. Padovan spin_lock_init(&conn->lock); 840baa7e1faSGustavo F. Padovan rwlock_init(&conn->chan_lock); 841baa7e1faSGustavo F. Padovan 842baa7e1faSGustavo F. Padovan INIT_LIST_HEAD(&conn->chan_l); 8430a708f8fSGustavo F. Padovan 844b62f328bSVille Tervo if (hcon->type != LE_LINK) 8450a708f8fSGustavo F. Padovan setup_timer(&conn->info_timer, l2cap_info_timeout, 8460a708f8fSGustavo F. Padovan (unsigned long) conn); 8470a708f8fSGustavo F. Padovan 8480a708f8fSGustavo F. Padovan conn->disc_reason = 0x13; 8490a708f8fSGustavo F. Padovan 8500a708f8fSGustavo F. Padovan return conn; 8510a708f8fSGustavo F. Padovan } 8520a708f8fSGustavo F. Padovan 8530a708f8fSGustavo F. Padovan static void l2cap_conn_del(struct hci_conn *hcon, int err) 8540a708f8fSGustavo F. Padovan { 8550a708f8fSGustavo F. Padovan struct l2cap_conn *conn = hcon->l2cap_data; 856baa7e1faSGustavo F. Padovan struct l2cap_chan *chan, *l; 8570a708f8fSGustavo F. Padovan struct sock *sk; 8580a708f8fSGustavo F. Padovan 8590a708f8fSGustavo F. Padovan if (!conn) 8600a708f8fSGustavo F. Padovan return; 8610a708f8fSGustavo F. Padovan 8620a708f8fSGustavo F. Padovan BT_DBG("hcon %p conn %p, err %d", hcon, conn, err); 8630a708f8fSGustavo F. Padovan 8640a708f8fSGustavo F. Padovan kfree_skb(conn->rx_skb); 8650a708f8fSGustavo F. Padovan 8660a708f8fSGustavo F. Padovan /* Kill channels */ 867baa7e1faSGustavo F. Padovan list_for_each_entry_safe(chan, l, &conn->chan_l, list) { 86848454079SGustavo F. Padovan sk = chan->sk; 8690a708f8fSGustavo F. Padovan bh_lock_sock(sk); 87048454079SGustavo F. Padovan l2cap_chan_del(chan, err); 8710a708f8fSGustavo F. Padovan bh_unlock_sock(sk); 8720a708f8fSGustavo F. Padovan l2cap_sock_kill(sk); 8730a708f8fSGustavo F. Padovan } 8740a708f8fSGustavo F. Padovan 8750a708f8fSGustavo F. Padovan if (conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_SENT) 8760a708f8fSGustavo F. Padovan del_timer_sync(&conn->info_timer); 8770a708f8fSGustavo F. Padovan 8780a708f8fSGustavo F. Padovan hcon->l2cap_data = NULL; 8790a708f8fSGustavo F. Padovan kfree(conn); 8800a708f8fSGustavo F. Padovan } 8810a708f8fSGustavo F. Padovan 88248454079SGustavo F. Padovan static inline void l2cap_chan_add(struct l2cap_conn *conn, struct l2cap_chan *chan) 8830a708f8fSGustavo F. Padovan { 884baa7e1faSGustavo F. Padovan write_lock_bh(&conn->chan_lock); 88548454079SGustavo F. Padovan __l2cap_chan_add(conn, chan); 886baa7e1faSGustavo F. Padovan write_unlock_bh(&conn->chan_lock); 8870a708f8fSGustavo F. Padovan } 8880a708f8fSGustavo F. Padovan 8890a708f8fSGustavo F. Padovan /* ---- Socket interface ---- */ 8900a708f8fSGustavo F. Padovan 8910a708f8fSGustavo F. Padovan /* Find socket with psm and source bdaddr. 8920a708f8fSGustavo F. Padovan * Returns closest match. 8930a708f8fSGustavo F. Padovan */ 89423691d75SGustavo F. Padovan static struct l2cap_chan *l2cap_global_chan_by_psm(int state, __le16 psm, bdaddr_t *src) 8950a708f8fSGustavo F. Padovan { 89623691d75SGustavo F. Padovan struct l2cap_chan *c, *c1 = NULL; 8970a708f8fSGustavo F. Padovan 89823691d75SGustavo F. Padovan read_lock(&chan_list_lock); 8990a708f8fSGustavo F. Padovan 90023691d75SGustavo F. Padovan list_for_each_entry(c, &chan_list, global_l) { 90123691d75SGustavo F. Padovan struct sock *sk = c->sk; 902fe4128e0SGustavo F. Padovan 9030a708f8fSGustavo F. Padovan if (state && sk->sk_state != state) 9040a708f8fSGustavo F. Padovan continue; 9050a708f8fSGustavo F. Padovan 90623691d75SGustavo F. Padovan if (c->psm == psm) { 9070a708f8fSGustavo F. Padovan /* Exact match. */ 90823691d75SGustavo F. Padovan if (!bacmp(&bt_sk(sk)->src, src)) { 909a7567b20SJohannes Berg read_unlock(&chan_list_lock); 91023691d75SGustavo F. Padovan return c; 91123691d75SGustavo F. Padovan } 9120a708f8fSGustavo F. Padovan 9130a708f8fSGustavo F. Padovan /* Closest match */ 9140a708f8fSGustavo F. Padovan if (!bacmp(&bt_sk(sk)->src, BDADDR_ANY)) 91523691d75SGustavo F. Padovan c1 = c; 9160a708f8fSGustavo F. Padovan } 9170a708f8fSGustavo F. Padovan } 9180a708f8fSGustavo F. Padovan 91923691d75SGustavo F. Padovan read_unlock(&chan_list_lock); 9200a708f8fSGustavo F. Padovan 92123691d75SGustavo F. Padovan return c1; 9220a708f8fSGustavo F. Padovan } 9230a708f8fSGustavo F. Padovan 92477a74c7eSGustavo F. Padovan int l2cap_chan_connect(struct l2cap_chan *chan) 9250a708f8fSGustavo F. Padovan { 9265d41ce1dSGustavo F. Padovan struct sock *sk = chan->sk; 9270a708f8fSGustavo F. Padovan bdaddr_t *src = &bt_sk(sk)->src; 9280a708f8fSGustavo F. Padovan bdaddr_t *dst = &bt_sk(sk)->dst; 9290a708f8fSGustavo F. Padovan struct l2cap_conn *conn; 9300a708f8fSGustavo F. Padovan struct hci_conn *hcon; 9310a708f8fSGustavo F. Padovan struct hci_dev *hdev; 9320a708f8fSGustavo F. Padovan __u8 auth_type; 9330a708f8fSGustavo F. Padovan int err; 9340a708f8fSGustavo F. Padovan 9350a708f8fSGustavo F. Padovan BT_DBG("%s -> %s psm 0x%2.2x", batostr(src), batostr(dst), 936fe4128e0SGustavo F. Padovan chan->psm); 9370a708f8fSGustavo F. Padovan 9380a708f8fSGustavo F. Padovan hdev = hci_get_route(dst, src); 9390a708f8fSGustavo F. Padovan if (!hdev) 9400a708f8fSGustavo F. Padovan return -EHOSTUNREACH; 9410a708f8fSGustavo F. Padovan 9420a708f8fSGustavo F. Padovan hci_dev_lock_bh(hdev); 9430a708f8fSGustavo F. Padovan 9444343478fSGustavo F. Padovan auth_type = l2cap_get_auth_type(chan); 9450a708f8fSGustavo F. Padovan 946fe4128e0SGustavo F. Padovan if (chan->dcid == L2CAP_CID_LE_DATA) 947acd7d370SVille Tervo hcon = hci_connect(hdev, LE_LINK, dst, 9484343478fSGustavo F. Padovan chan->sec_level, auth_type); 949acd7d370SVille Tervo else 9500a708f8fSGustavo F. Padovan hcon = hci_connect(hdev, ACL_LINK, dst, 9514343478fSGustavo F. Padovan chan->sec_level, auth_type); 952acd7d370SVille Tervo 95330e76272SVille Tervo if (IS_ERR(hcon)) { 95430e76272SVille Tervo err = PTR_ERR(hcon); 9550a708f8fSGustavo F. Padovan goto done; 95630e76272SVille Tervo } 9570a708f8fSGustavo F. Padovan 9580a708f8fSGustavo F. Padovan conn = l2cap_conn_add(hcon, 0); 9590a708f8fSGustavo F. Padovan if (!conn) { 9600a708f8fSGustavo F. Padovan hci_conn_put(hcon); 96130e76272SVille Tervo err = -ENOMEM; 9620a708f8fSGustavo F. Padovan goto done; 9630a708f8fSGustavo F. Padovan } 9640a708f8fSGustavo F. Padovan 9650a708f8fSGustavo F. Padovan /* Update source addr of the socket */ 9660a708f8fSGustavo F. Padovan bacpy(src, conn->src); 9670a708f8fSGustavo F. Padovan 96848454079SGustavo F. Padovan l2cap_chan_add(conn, chan); 96948454079SGustavo F. Padovan 9700a708f8fSGustavo F. Padovan sk->sk_state = BT_CONNECT; 9710a708f8fSGustavo F. Padovan l2cap_sock_set_timer(sk, sk->sk_sndtimeo); 9720a708f8fSGustavo F. Padovan 9730a708f8fSGustavo F. Padovan if (hcon->state == BT_CONNECTED) { 9740a708f8fSGustavo F. Padovan if (sk->sk_type != SOCK_SEQPACKET && 9750a708f8fSGustavo F. Padovan sk->sk_type != SOCK_STREAM) { 9760a708f8fSGustavo F. Padovan l2cap_sock_clear_timer(sk); 9774343478fSGustavo F. Padovan if (l2cap_check_security(chan)) 9780a708f8fSGustavo F. Padovan sk->sk_state = BT_CONNECTED; 9790a708f8fSGustavo F. Padovan } else 980fc7f8a7eSGustavo F. Padovan l2cap_do_start(chan); 9810a708f8fSGustavo F. Padovan } 9820a708f8fSGustavo F. Padovan 98330e76272SVille Tervo err = 0; 98430e76272SVille Tervo 9850a708f8fSGustavo F. Padovan done: 9860a708f8fSGustavo F. Padovan hci_dev_unlock_bh(hdev); 9870a708f8fSGustavo F. Padovan hci_dev_put(hdev); 9880a708f8fSGustavo F. Padovan return err; 9890a708f8fSGustavo F. Padovan } 9900a708f8fSGustavo F. Padovan 991dcba0dbaSGustavo F. Padovan int __l2cap_wait_ack(struct sock *sk) 9920a708f8fSGustavo F. Padovan { 9938c1d787bSGustavo F. Padovan struct l2cap_chan *chan = l2cap_pi(sk)->chan; 9940a708f8fSGustavo F. Padovan DECLARE_WAITQUEUE(wait, current); 9950a708f8fSGustavo F. Padovan int err = 0; 9960a708f8fSGustavo F. Padovan int timeo = HZ/5; 9970a708f8fSGustavo F. Padovan 9980a708f8fSGustavo F. Padovan add_wait_queue(sk_sleep(sk), &wait); 9998c1d787bSGustavo F. Padovan while ((chan->unacked_frames > 0 && chan->conn)) { 10000a708f8fSGustavo F. Padovan set_current_state(TASK_INTERRUPTIBLE); 10010a708f8fSGustavo F. Padovan 10020a708f8fSGustavo F. Padovan if (!timeo) 10030a708f8fSGustavo F. Padovan timeo = HZ/5; 10040a708f8fSGustavo F. Padovan 10050a708f8fSGustavo F. Padovan if (signal_pending(current)) { 10060a708f8fSGustavo F. Padovan err = sock_intr_errno(timeo); 10070a708f8fSGustavo F. Padovan break; 10080a708f8fSGustavo F. Padovan } 10090a708f8fSGustavo F. Padovan 10100a708f8fSGustavo F. Padovan release_sock(sk); 10110a708f8fSGustavo F. Padovan timeo = schedule_timeout(timeo); 10120a708f8fSGustavo F. Padovan lock_sock(sk); 10130a708f8fSGustavo F. Padovan 10140a708f8fSGustavo F. Padovan err = sock_error(sk); 10150a708f8fSGustavo F. Padovan if (err) 10160a708f8fSGustavo F. Padovan break; 10170a708f8fSGustavo F. Padovan } 10180a708f8fSGustavo F. Padovan set_current_state(TASK_RUNNING); 10190a708f8fSGustavo F. Padovan remove_wait_queue(sk_sleep(sk), &wait); 10200a708f8fSGustavo F. Padovan return err; 10210a708f8fSGustavo F. Padovan } 10220a708f8fSGustavo F. Padovan 10230a708f8fSGustavo F. Padovan static void l2cap_monitor_timeout(unsigned long arg) 10240a708f8fSGustavo F. Padovan { 1025525cd185SGustavo F. Padovan struct l2cap_chan *chan = (void *) arg; 1026525cd185SGustavo F. Padovan struct sock *sk = chan->sk; 10270a708f8fSGustavo F. Padovan 1028525cd185SGustavo F. Padovan BT_DBG("chan %p", chan); 10290a708f8fSGustavo F. Padovan 10300a708f8fSGustavo F. Padovan bh_lock_sock(sk); 10312c03a7a4SGustavo F. Padovan if (chan->retry_count >= chan->remote_max_tx) { 10328c1d787bSGustavo F. Padovan l2cap_send_disconn_req(chan->conn, chan, ECONNABORTED); 10330a708f8fSGustavo F. Padovan bh_unlock_sock(sk); 10340a708f8fSGustavo F. Padovan return; 10350a708f8fSGustavo F. Padovan } 10360a708f8fSGustavo F. Padovan 10376a026610SGustavo F. Padovan chan->retry_count++; 10380a708f8fSGustavo F. Padovan __mod_monitor_timer(); 10390a708f8fSGustavo F. Padovan 1040525cd185SGustavo F. Padovan l2cap_send_rr_or_rnr(chan, L2CAP_CTRL_POLL); 10410a708f8fSGustavo F. Padovan bh_unlock_sock(sk); 10420a708f8fSGustavo F. Padovan } 10430a708f8fSGustavo F. Padovan 10440a708f8fSGustavo F. Padovan static void l2cap_retrans_timeout(unsigned long arg) 10450a708f8fSGustavo F. Padovan { 1046525cd185SGustavo F. Padovan struct l2cap_chan *chan = (void *) arg; 1047525cd185SGustavo F. Padovan struct sock *sk = chan->sk; 10480a708f8fSGustavo F. Padovan 104949208c9cSGustavo F. Padovan BT_DBG("chan %p", chan); 10500a708f8fSGustavo F. Padovan 10510a708f8fSGustavo F. Padovan bh_lock_sock(sk); 10526a026610SGustavo F. Padovan chan->retry_count = 1; 10530a708f8fSGustavo F. Padovan __mod_monitor_timer(); 10540a708f8fSGustavo F. Padovan 1055525cd185SGustavo F. Padovan chan->conn_state |= L2CAP_CONN_WAIT_F; 10560a708f8fSGustavo F. Padovan 1057525cd185SGustavo F. Padovan l2cap_send_rr_or_rnr(chan, L2CAP_CTRL_POLL); 10580a708f8fSGustavo F. Padovan bh_unlock_sock(sk); 10590a708f8fSGustavo F. Padovan } 10600a708f8fSGustavo F. Padovan 106142e5c802SGustavo F. Padovan static void l2cap_drop_acked_frames(struct l2cap_chan *chan) 10620a708f8fSGustavo F. Padovan { 10630a708f8fSGustavo F. Padovan struct sk_buff *skb; 10640a708f8fSGustavo F. Padovan 106558d35f87SGustavo F. Padovan while ((skb = skb_peek(&chan->tx_q)) && 10666a026610SGustavo F. Padovan chan->unacked_frames) { 106742e5c802SGustavo F. Padovan if (bt_cb(skb)->tx_seq == chan->expected_ack_seq) 10680a708f8fSGustavo F. Padovan break; 10690a708f8fSGustavo F. Padovan 107058d35f87SGustavo F. Padovan skb = skb_dequeue(&chan->tx_q); 10710a708f8fSGustavo F. Padovan kfree_skb(skb); 10720a708f8fSGustavo F. Padovan 10736a026610SGustavo F. Padovan chan->unacked_frames--; 10740a708f8fSGustavo F. Padovan } 10750a708f8fSGustavo F. Padovan 10766a026610SGustavo F. Padovan if (!chan->unacked_frames) 1077e92c8e70SGustavo F. Padovan del_timer(&chan->retrans_timer); 10780a708f8fSGustavo F. Padovan } 10790a708f8fSGustavo F. Padovan 10804343478fSGustavo F. Padovan void l2cap_do_send(struct l2cap_chan *chan, struct sk_buff *skb) 10810a708f8fSGustavo F. Padovan { 10828c1d787bSGustavo F. Padovan struct hci_conn *hcon = chan->conn->hcon; 10830a708f8fSGustavo F. Padovan u16 flags; 10840a708f8fSGustavo F. Padovan 10854343478fSGustavo F. Padovan BT_DBG("chan %p, skb %p len %d", chan, skb, skb->len); 10860a708f8fSGustavo F. Padovan 10874343478fSGustavo F. Padovan if (!chan->flushable && lmp_no_flush_capable(hcon->hdev)) 10880a708f8fSGustavo F. Padovan flags = ACL_START_NO_FLUSH; 10890a708f8fSGustavo F. Padovan else 10900a708f8fSGustavo F. Padovan flags = ACL_START; 10910a708f8fSGustavo F. Padovan 10920a708f8fSGustavo F. Padovan hci_send_acl(hcon, skb, flags); 10930a708f8fSGustavo F. Padovan } 10940a708f8fSGustavo F. Padovan 109542e5c802SGustavo F. Padovan void l2cap_streaming_send(struct l2cap_chan *chan) 10960a708f8fSGustavo F. Padovan { 10970a708f8fSGustavo F. Padovan struct sk_buff *skb; 10980a708f8fSGustavo F. Padovan u16 control, fcs; 10990a708f8fSGustavo F. Padovan 110058d35f87SGustavo F. Padovan while ((skb = skb_dequeue(&chan->tx_q))) { 11010a708f8fSGustavo F. Padovan control = get_unaligned_le16(skb->data + L2CAP_HDR_SIZE); 110242e5c802SGustavo F. Padovan control |= chan->next_tx_seq << L2CAP_CTRL_TXSEQ_SHIFT; 11030a708f8fSGustavo F. Padovan put_unaligned_le16(control, skb->data + L2CAP_HDR_SIZE); 11040a708f8fSGustavo F. Padovan 110547d1ec61SGustavo F. Padovan if (chan->fcs == L2CAP_FCS_CRC16) { 11060a708f8fSGustavo F. Padovan fcs = crc16(0, (u8 *)skb->data, skb->len - 2); 11070a708f8fSGustavo F. Padovan put_unaligned_le16(fcs, skb->data + skb->len - 2); 11080a708f8fSGustavo F. Padovan } 11090a708f8fSGustavo F. Padovan 11104343478fSGustavo F. Padovan l2cap_do_send(chan, skb); 11110a708f8fSGustavo F. Padovan 111242e5c802SGustavo F. Padovan chan->next_tx_seq = (chan->next_tx_seq + 1) % 64; 11130a708f8fSGustavo F. Padovan } 11140a708f8fSGustavo F. Padovan } 11150a708f8fSGustavo F. Padovan 1116525cd185SGustavo F. Padovan static void l2cap_retransmit_one_frame(struct l2cap_chan *chan, u8 tx_seq) 11170a708f8fSGustavo F. Padovan { 11180a708f8fSGustavo F. Padovan struct sk_buff *skb, *tx_skb; 11190a708f8fSGustavo F. Padovan u16 control, fcs; 11200a708f8fSGustavo F. Padovan 112158d35f87SGustavo F. Padovan skb = skb_peek(&chan->tx_q); 11220a708f8fSGustavo F. Padovan if (!skb) 11230a708f8fSGustavo F. Padovan return; 11240a708f8fSGustavo F. Padovan 11250a708f8fSGustavo F. Padovan do { 11260a708f8fSGustavo F. Padovan if (bt_cb(skb)->tx_seq == tx_seq) 11270a708f8fSGustavo F. Padovan break; 11280a708f8fSGustavo F. Padovan 112958d35f87SGustavo F. Padovan if (skb_queue_is_last(&chan->tx_q, skb)) 11300a708f8fSGustavo F. Padovan return; 11310a708f8fSGustavo F. Padovan 113258d35f87SGustavo F. Padovan } while ((skb = skb_queue_next(&chan->tx_q, skb))); 11330a708f8fSGustavo F. Padovan 11342c03a7a4SGustavo F. Padovan if (chan->remote_max_tx && 11352c03a7a4SGustavo F. Padovan bt_cb(skb)->retries == chan->remote_max_tx) { 11368c1d787bSGustavo F. Padovan l2cap_send_disconn_req(chan->conn, chan, ECONNABORTED); 11370a708f8fSGustavo F. Padovan return; 11380a708f8fSGustavo F. Padovan } 11390a708f8fSGustavo F. Padovan 11400a708f8fSGustavo F. Padovan tx_skb = skb_clone(skb, GFP_ATOMIC); 11410a708f8fSGustavo F. Padovan bt_cb(skb)->retries++; 11420a708f8fSGustavo F. Padovan control = get_unaligned_le16(tx_skb->data + L2CAP_HDR_SIZE); 1143a429b519SRuiyi Zhang control &= L2CAP_CTRL_SAR; 11440a708f8fSGustavo F. Padovan 1145525cd185SGustavo F. Padovan if (chan->conn_state & L2CAP_CONN_SEND_FBIT) { 11460a708f8fSGustavo F. Padovan control |= L2CAP_CTRL_FINAL; 1147525cd185SGustavo F. Padovan chan->conn_state &= ~L2CAP_CONN_SEND_FBIT; 11480a708f8fSGustavo F. Padovan } 11490a708f8fSGustavo F. Padovan 115042e5c802SGustavo F. Padovan control |= (chan->buffer_seq << L2CAP_CTRL_REQSEQ_SHIFT) 11510a708f8fSGustavo F. Padovan | (tx_seq << L2CAP_CTRL_TXSEQ_SHIFT); 11520a708f8fSGustavo F. Padovan 11530a708f8fSGustavo F. Padovan put_unaligned_le16(control, tx_skb->data + L2CAP_HDR_SIZE); 11540a708f8fSGustavo F. Padovan 115547d1ec61SGustavo F. Padovan if (chan->fcs == L2CAP_FCS_CRC16) { 11560a708f8fSGustavo F. Padovan fcs = crc16(0, (u8 *)tx_skb->data, tx_skb->len - 2); 11570a708f8fSGustavo F. Padovan put_unaligned_le16(fcs, tx_skb->data + tx_skb->len - 2); 11580a708f8fSGustavo F. Padovan } 11590a708f8fSGustavo F. Padovan 11604343478fSGustavo F. Padovan l2cap_do_send(chan, tx_skb); 11610a708f8fSGustavo F. Padovan } 11620a708f8fSGustavo F. Padovan 1163525cd185SGustavo F. Padovan int l2cap_ertm_send(struct l2cap_chan *chan) 11640a708f8fSGustavo F. Padovan { 11650a708f8fSGustavo F. Padovan struct sk_buff *skb, *tx_skb; 1166525cd185SGustavo F. Padovan struct sock *sk = chan->sk; 11670a708f8fSGustavo F. Padovan u16 control, fcs; 11680a708f8fSGustavo F. Padovan int nsent = 0; 11690a708f8fSGustavo F. Padovan 11700a708f8fSGustavo F. Padovan if (sk->sk_state != BT_CONNECTED) 11710a708f8fSGustavo F. Padovan return -ENOTCONN; 11720a708f8fSGustavo F. Padovan 117358d35f87SGustavo F. Padovan while ((skb = chan->tx_send_head) && (!l2cap_tx_window_full(chan))) { 11740a708f8fSGustavo F. Padovan 11752c03a7a4SGustavo F. Padovan if (chan->remote_max_tx && 11762c03a7a4SGustavo F. Padovan bt_cb(skb)->retries == chan->remote_max_tx) { 11778c1d787bSGustavo F. Padovan l2cap_send_disconn_req(chan->conn, chan, ECONNABORTED); 11780a708f8fSGustavo F. Padovan break; 11790a708f8fSGustavo F. Padovan } 11800a708f8fSGustavo F. Padovan 11810a708f8fSGustavo F. Padovan tx_skb = skb_clone(skb, GFP_ATOMIC); 11820a708f8fSGustavo F. Padovan 11830a708f8fSGustavo F. Padovan bt_cb(skb)->retries++; 11840a708f8fSGustavo F. Padovan 11850a708f8fSGustavo F. Padovan control = get_unaligned_le16(tx_skb->data + L2CAP_HDR_SIZE); 11860a708f8fSGustavo F. Padovan control &= L2CAP_CTRL_SAR; 11870a708f8fSGustavo F. Padovan 1188525cd185SGustavo F. Padovan if (chan->conn_state & L2CAP_CONN_SEND_FBIT) { 11890a708f8fSGustavo F. Padovan control |= L2CAP_CTRL_FINAL; 1190525cd185SGustavo F. Padovan chan->conn_state &= ~L2CAP_CONN_SEND_FBIT; 11910a708f8fSGustavo F. Padovan } 119242e5c802SGustavo F. Padovan control |= (chan->buffer_seq << L2CAP_CTRL_REQSEQ_SHIFT) 119342e5c802SGustavo F. Padovan | (chan->next_tx_seq << L2CAP_CTRL_TXSEQ_SHIFT); 11940a708f8fSGustavo F. Padovan put_unaligned_le16(control, tx_skb->data + L2CAP_HDR_SIZE); 11950a708f8fSGustavo F. Padovan 11960a708f8fSGustavo F. Padovan 119747d1ec61SGustavo F. Padovan if (chan->fcs == L2CAP_FCS_CRC16) { 11980a708f8fSGustavo F. Padovan fcs = crc16(0, (u8 *)skb->data, tx_skb->len - 2); 11990a708f8fSGustavo F. Padovan put_unaligned_le16(fcs, skb->data + tx_skb->len - 2); 12000a708f8fSGustavo F. Padovan } 12010a708f8fSGustavo F. Padovan 12024343478fSGustavo F. Padovan l2cap_do_send(chan, tx_skb); 12030a708f8fSGustavo F. Padovan 12040a708f8fSGustavo F. Padovan __mod_retrans_timer(); 12050a708f8fSGustavo F. Padovan 120642e5c802SGustavo F. Padovan bt_cb(skb)->tx_seq = chan->next_tx_seq; 120742e5c802SGustavo F. Padovan chan->next_tx_seq = (chan->next_tx_seq + 1) % 64; 12080a708f8fSGustavo F. Padovan 120923e9fde2SSuraj Sumangala if (bt_cb(skb)->retries == 1) 12106a026610SGustavo F. Padovan chan->unacked_frames++; 121123e9fde2SSuraj Sumangala 12126a026610SGustavo F. Padovan chan->frames_sent++; 12130a708f8fSGustavo F. Padovan 121458d35f87SGustavo F. Padovan if (skb_queue_is_last(&chan->tx_q, skb)) 121558d35f87SGustavo F. Padovan chan->tx_send_head = NULL; 12160a708f8fSGustavo F. Padovan else 121758d35f87SGustavo F. Padovan chan->tx_send_head = skb_queue_next(&chan->tx_q, skb); 12180a708f8fSGustavo F. Padovan 12190a708f8fSGustavo F. Padovan nsent++; 12200a708f8fSGustavo F. Padovan } 12210a708f8fSGustavo F. Padovan 12220a708f8fSGustavo F. Padovan return nsent; 12230a708f8fSGustavo F. Padovan } 12240a708f8fSGustavo F. Padovan 1225525cd185SGustavo F. Padovan static int l2cap_retransmit_frames(struct l2cap_chan *chan) 12260a708f8fSGustavo F. Padovan { 12270a708f8fSGustavo F. Padovan int ret; 12280a708f8fSGustavo F. Padovan 122958d35f87SGustavo F. Padovan if (!skb_queue_empty(&chan->tx_q)) 123058d35f87SGustavo F. Padovan chan->tx_send_head = chan->tx_q.next; 12310a708f8fSGustavo F. Padovan 123242e5c802SGustavo F. Padovan chan->next_tx_seq = chan->expected_ack_seq; 1233525cd185SGustavo F. Padovan ret = l2cap_ertm_send(chan); 12340a708f8fSGustavo F. Padovan return ret; 12350a708f8fSGustavo F. Padovan } 12360a708f8fSGustavo F. Padovan 1237525cd185SGustavo F. Padovan static void l2cap_send_ack(struct l2cap_chan *chan) 12380a708f8fSGustavo F. Padovan { 12390a708f8fSGustavo F. Padovan u16 control = 0; 12400a708f8fSGustavo F. Padovan 124142e5c802SGustavo F. Padovan control |= chan->buffer_seq << L2CAP_CTRL_REQSEQ_SHIFT; 12420a708f8fSGustavo F. Padovan 1243525cd185SGustavo F. Padovan if (chan->conn_state & L2CAP_CONN_LOCAL_BUSY) { 12440a708f8fSGustavo F. Padovan control |= L2CAP_SUPER_RCV_NOT_READY; 1245525cd185SGustavo F. Padovan chan->conn_state |= L2CAP_CONN_RNR_SENT; 1246525cd185SGustavo F. Padovan l2cap_send_sframe(chan, control); 12470a708f8fSGustavo F. Padovan return; 12480a708f8fSGustavo F. Padovan } 12490a708f8fSGustavo F. Padovan 1250525cd185SGustavo F. Padovan if (l2cap_ertm_send(chan) > 0) 12510a708f8fSGustavo F. Padovan return; 12520a708f8fSGustavo F. Padovan 12530a708f8fSGustavo F. Padovan control |= L2CAP_SUPER_RCV_READY; 1254525cd185SGustavo F. Padovan l2cap_send_sframe(chan, control); 12550a708f8fSGustavo F. Padovan } 12560a708f8fSGustavo F. Padovan 1257525cd185SGustavo F. Padovan static void l2cap_send_srejtail(struct l2cap_chan *chan) 12580a708f8fSGustavo F. Padovan { 12590a708f8fSGustavo F. Padovan struct srej_list *tail; 12600a708f8fSGustavo F. Padovan u16 control; 12610a708f8fSGustavo F. Padovan 12620a708f8fSGustavo F. Padovan control = L2CAP_SUPER_SELECT_REJECT; 12630a708f8fSGustavo F. Padovan control |= L2CAP_CTRL_FINAL; 12640a708f8fSGustavo F. Padovan 126539d5a3eeSGustavo F. Padovan tail = list_entry((&chan->srej_l)->prev, struct srej_list, list); 12660a708f8fSGustavo F. Padovan control |= tail->tx_seq << L2CAP_CTRL_REQSEQ_SHIFT; 12670a708f8fSGustavo F. Padovan 1268525cd185SGustavo F. Padovan l2cap_send_sframe(chan, control); 12690a708f8fSGustavo F. Padovan } 12700a708f8fSGustavo F. Padovan 12710a708f8fSGustavo F. Padovan static inline int l2cap_skbuff_fromiovec(struct sock *sk, struct msghdr *msg, int len, int count, struct sk_buff *skb) 12720a708f8fSGustavo F. Padovan { 12738c1d787bSGustavo F. Padovan struct l2cap_conn *conn = l2cap_pi(sk)->chan->conn; 12740a708f8fSGustavo F. Padovan struct sk_buff **frag; 12750a708f8fSGustavo F. Padovan int err, sent = 0; 12760a708f8fSGustavo F. Padovan 12770a708f8fSGustavo F. Padovan if (memcpy_fromiovec(skb_put(skb, count), msg->msg_iov, count)) 12780a708f8fSGustavo F. Padovan return -EFAULT; 12790a708f8fSGustavo F. Padovan 12800a708f8fSGustavo F. Padovan sent += count; 12810a708f8fSGustavo F. Padovan len -= count; 12820a708f8fSGustavo F. Padovan 12830a708f8fSGustavo F. Padovan /* Continuation fragments (no L2CAP header) */ 12840a708f8fSGustavo F. Padovan frag = &skb_shinfo(skb)->frag_list; 12850a708f8fSGustavo F. Padovan while (len) { 12860a708f8fSGustavo F. Padovan count = min_t(unsigned int, conn->mtu, len); 12870a708f8fSGustavo F. Padovan 12880a708f8fSGustavo F. Padovan *frag = bt_skb_send_alloc(sk, count, msg->msg_flags & MSG_DONTWAIT, &err); 12890a708f8fSGustavo F. Padovan if (!*frag) 12900a708f8fSGustavo F. Padovan return err; 12910a708f8fSGustavo F. Padovan if (memcpy_fromiovec(skb_put(*frag, count), msg->msg_iov, count)) 12920a708f8fSGustavo F. Padovan return -EFAULT; 12930a708f8fSGustavo F. Padovan 12940a708f8fSGustavo F. Padovan sent += count; 12950a708f8fSGustavo F. Padovan len -= count; 12960a708f8fSGustavo F. Padovan 12970a708f8fSGustavo F. Padovan frag = &(*frag)->next; 12980a708f8fSGustavo F. Padovan } 12990a708f8fSGustavo F. Padovan 13000a708f8fSGustavo F. Padovan return sent; 13010a708f8fSGustavo F. Padovan } 13020a708f8fSGustavo F. Padovan 1303fe4128e0SGustavo F. Padovan struct sk_buff *l2cap_create_connless_pdu(struct l2cap_chan *chan, struct msghdr *msg, size_t len) 13040a708f8fSGustavo F. Padovan { 1305fe4128e0SGustavo F. Padovan struct sock *sk = chan->sk; 13068c1d787bSGustavo F. Padovan struct l2cap_conn *conn = chan->conn; 13070a708f8fSGustavo F. Padovan struct sk_buff *skb; 13080a708f8fSGustavo F. Padovan int err, count, hlen = L2CAP_HDR_SIZE + 2; 13090a708f8fSGustavo F. Padovan struct l2cap_hdr *lh; 13100a708f8fSGustavo F. Padovan 13110a708f8fSGustavo F. Padovan BT_DBG("sk %p len %d", sk, (int)len); 13120a708f8fSGustavo F. Padovan 13130a708f8fSGustavo F. Padovan count = min_t(unsigned int, (conn->mtu - hlen), len); 13140a708f8fSGustavo F. Padovan skb = bt_skb_send_alloc(sk, count + hlen, 13150a708f8fSGustavo F. Padovan msg->msg_flags & MSG_DONTWAIT, &err); 13160a708f8fSGustavo F. Padovan if (!skb) 13170a708f8fSGustavo F. Padovan return ERR_PTR(err); 13180a708f8fSGustavo F. Padovan 13190a708f8fSGustavo F. Padovan /* Create L2CAP header */ 13200a708f8fSGustavo F. Padovan lh = (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE); 1321fe4128e0SGustavo F. Padovan lh->cid = cpu_to_le16(chan->dcid); 13220a708f8fSGustavo F. Padovan lh->len = cpu_to_le16(len + (hlen - L2CAP_HDR_SIZE)); 1323fe4128e0SGustavo F. Padovan put_unaligned_le16(chan->psm, skb_put(skb, 2)); 13240a708f8fSGustavo F. Padovan 13250a708f8fSGustavo F. Padovan err = l2cap_skbuff_fromiovec(sk, msg, len, count, skb); 13260a708f8fSGustavo F. Padovan if (unlikely(err < 0)) { 13270a708f8fSGustavo F. Padovan kfree_skb(skb); 13280a708f8fSGustavo F. Padovan return ERR_PTR(err); 13290a708f8fSGustavo F. Padovan } 13300a708f8fSGustavo F. Padovan return skb; 13310a708f8fSGustavo F. Padovan } 13320a708f8fSGustavo F. Padovan 1333fe4128e0SGustavo F. Padovan struct sk_buff *l2cap_create_basic_pdu(struct l2cap_chan *chan, struct msghdr *msg, size_t len) 13340a708f8fSGustavo F. Padovan { 1335fe4128e0SGustavo F. Padovan struct sock *sk = chan->sk; 13368c1d787bSGustavo F. Padovan struct l2cap_conn *conn = chan->conn; 13370a708f8fSGustavo F. Padovan struct sk_buff *skb; 13380a708f8fSGustavo F. Padovan int err, count, hlen = L2CAP_HDR_SIZE; 13390a708f8fSGustavo F. Padovan struct l2cap_hdr *lh; 13400a708f8fSGustavo F. Padovan 13410a708f8fSGustavo F. Padovan BT_DBG("sk %p len %d", sk, (int)len); 13420a708f8fSGustavo F. Padovan 13430a708f8fSGustavo F. Padovan count = min_t(unsigned int, (conn->mtu - hlen), len); 13440a708f8fSGustavo F. Padovan skb = bt_skb_send_alloc(sk, count + hlen, 13450a708f8fSGustavo F. Padovan msg->msg_flags & MSG_DONTWAIT, &err); 13460a708f8fSGustavo F. Padovan if (!skb) 13470a708f8fSGustavo F. Padovan return ERR_PTR(err); 13480a708f8fSGustavo F. Padovan 13490a708f8fSGustavo F. Padovan /* Create L2CAP header */ 13500a708f8fSGustavo F. Padovan lh = (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE); 1351fe4128e0SGustavo F. Padovan lh->cid = cpu_to_le16(chan->dcid); 13520a708f8fSGustavo F. Padovan lh->len = cpu_to_le16(len + (hlen - L2CAP_HDR_SIZE)); 13530a708f8fSGustavo F. Padovan 13540a708f8fSGustavo F. Padovan err = l2cap_skbuff_fromiovec(sk, msg, len, count, skb); 13550a708f8fSGustavo F. Padovan if (unlikely(err < 0)) { 13560a708f8fSGustavo F. Padovan kfree_skb(skb); 13570a708f8fSGustavo F. Padovan return ERR_PTR(err); 13580a708f8fSGustavo F. Padovan } 13590a708f8fSGustavo F. Padovan return skb; 13600a708f8fSGustavo F. Padovan } 13610a708f8fSGustavo F. Padovan 136247d1ec61SGustavo F. Padovan struct sk_buff *l2cap_create_iframe_pdu(struct l2cap_chan *chan, struct msghdr *msg, size_t len, u16 control, u16 sdulen) 13630a708f8fSGustavo F. Padovan { 136447d1ec61SGustavo F. Padovan struct sock *sk = chan->sk; 13658c1d787bSGustavo F. Padovan struct l2cap_conn *conn = chan->conn; 13660a708f8fSGustavo F. Padovan struct sk_buff *skb; 13670a708f8fSGustavo F. Padovan int err, count, hlen = L2CAP_HDR_SIZE + 2; 13680a708f8fSGustavo F. Padovan struct l2cap_hdr *lh; 13690a708f8fSGustavo F. Padovan 13700a708f8fSGustavo F. Padovan BT_DBG("sk %p len %d", sk, (int)len); 13710a708f8fSGustavo F. Padovan 13720a708f8fSGustavo F. Padovan if (!conn) 13730a708f8fSGustavo F. Padovan return ERR_PTR(-ENOTCONN); 13740a708f8fSGustavo F. Padovan 13750a708f8fSGustavo F. Padovan if (sdulen) 13760a708f8fSGustavo F. Padovan hlen += 2; 13770a708f8fSGustavo F. Padovan 137847d1ec61SGustavo F. Padovan if (chan->fcs == L2CAP_FCS_CRC16) 13790a708f8fSGustavo F. Padovan hlen += 2; 13800a708f8fSGustavo F. Padovan 13810a708f8fSGustavo F. Padovan count = min_t(unsigned int, (conn->mtu - hlen), len); 13820a708f8fSGustavo F. Padovan skb = bt_skb_send_alloc(sk, count + hlen, 13830a708f8fSGustavo F. Padovan msg->msg_flags & MSG_DONTWAIT, &err); 13840a708f8fSGustavo F. Padovan if (!skb) 13850a708f8fSGustavo F. Padovan return ERR_PTR(err); 13860a708f8fSGustavo F. Padovan 13870a708f8fSGustavo F. Padovan /* Create L2CAP header */ 13880a708f8fSGustavo F. Padovan lh = (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE); 1389fe4128e0SGustavo F. Padovan lh->cid = cpu_to_le16(chan->dcid); 13900a708f8fSGustavo F. Padovan lh->len = cpu_to_le16(len + (hlen - L2CAP_HDR_SIZE)); 13910a708f8fSGustavo F. Padovan put_unaligned_le16(control, skb_put(skb, 2)); 13920a708f8fSGustavo F. Padovan if (sdulen) 13930a708f8fSGustavo F. Padovan put_unaligned_le16(sdulen, skb_put(skb, 2)); 13940a708f8fSGustavo F. Padovan 13950a708f8fSGustavo F. Padovan err = l2cap_skbuff_fromiovec(sk, msg, len, count, skb); 13960a708f8fSGustavo F. Padovan if (unlikely(err < 0)) { 13970a708f8fSGustavo F. Padovan kfree_skb(skb); 13980a708f8fSGustavo F. Padovan return ERR_PTR(err); 13990a708f8fSGustavo F. Padovan } 14000a708f8fSGustavo F. Padovan 140147d1ec61SGustavo F. Padovan if (chan->fcs == L2CAP_FCS_CRC16) 14020a708f8fSGustavo F. Padovan put_unaligned_le16(0, skb_put(skb, 2)); 14030a708f8fSGustavo F. Padovan 14040a708f8fSGustavo F. Padovan bt_cb(skb)->retries = 0; 14050a708f8fSGustavo F. Padovan return skb; 14060a708f8fSGustavo F. Padovan } 14070a708f8fSGustavo F. Padovan 14082c03a7a4SGustavo F. Padovan int l2cap_sar_segment_sdu(struct l2cap_chan *chan, struct msghdr *msg, size_t len) 14090a708f8fSGustavo F. Padovan { 14100a708f8fSGustavo F. Padovan struct sk_buff *skb; 14110a708f8fSGustavo F. Padovan struct sk_buff_head sar_queue; 14120a708f8fSGustavo F. Padovan u16 control; 14130a708f8fSGustavo F. Padovan size_t size = 0; 14140a708f8fSGustavo F. Padovan 14150a708f8fSGustavo F. Padovan skb_queue_head_init(&sar_queue); 14160a708f8fSGustavo F. Padovan control = L2CAP_SDU_START; 141747d1ec61SGustavo F. Padovan skb = l2cap_create_iframe_pdu(chan, msg, chan->remote_mps, control, len); 14180a708f8fSGustavo F. Padovan if (IS_ERR(skb)) 14190a708f8fSGustavo F. Padovan return PTR_ERR(skb); 14200a708f8fSGustavo F. Padovan 14210a708f8fSGustavo F. Padovan __skb_queue_tail(&sar_queue, skb); 14222c03a7a4SGustavo F. Padovan len -= chan->remote_mps; 14232c03a7a4SGustavo F. Padovan size += chan->remote_mps; 14240a708f8fSGustavo F. Padovan 14250a708f8fSGustavo F. Padovan while (len > 0) { 14260a708f8fSGustavo F. Padovan size_t buflen; 14270a708f8fSGustavo F. Padovan 14282c03a7a4SGustavo F. Padovan if (len > chan->remote_mps) { 14290a708f8fSGustavo F. Padovan control = L2CAP_SDU_CONTINUE; 14302c03a7a4SGustavo F. Padovan buflen = chan->remote_mps; 14310a708f8fSGustavo F. Padovan } else { 14320a708f8fSGustavo F. Padovan control = L2CAP_SDU_END; 14330a708f8fSGustavo F. Padovan buflen = len; 14340a708f8fSGustavo F. Padovan } 14350a708f8fSGustavo F. Padovan 143647d1ec61SGustavo F. Padovan skb = l2cap_create_iframe_pdu(chan, msg, buflen, control, 0); 14370a708f8fSGustavo F. Padovan if (IS_ERR(skb)) { 14380a708f8fSGustavo F. Padovan skb_queue_purge(&sar_queue); 14390a708f8fSGustavo F. Padovan return PTR_ERR(skb); 14400a708f8fSGustavo F. Padovan } 14410a708f8fSGustavo F. Padovan 14420a708f8fSGustavo F. Padovan __skb_queue_tail(&sar_queue, skb); 14430a708f8fSGustavo F. Padovan len -= buflen; 14440a708f8fSGustavo F. Padovan size += buflen; 14450a708f8fSGustavo F. Padovan } 144658d35f87SGustavo F. Padovan skb_queue_splice_tail(&sar_queue, &chan->tx_q); 144758d35f87SGustavo F. Padovan if (chan->tx_send_head == NULL) 144858d35f87SGustavo F. Padovan chan->tx_send_head = sar_queue.next; 14490a708f8fSGustavo F. Padovan 14500a708f8fSGustavo F. Padovan return size; 14510a708f8fSGustavo F. Padovan } 14520a708f8fSGustavo F. Padovan 14530a708f8fSGustavo F. Padovan static void l2cap_chan_ready(struct sock *sk) 14540a708f8fSGustavo F. Padovan { 14550a708f8fSGustavo F. Padovan struct sock *parent = bt_sk(sk)->parent; 1456b4450035SGustavo F. Padovan struct l2cap_chan *chan = l2cap_pi(sk)->chan; 14570a708f8fSGustavo F. Padovan 14580a708f8fSGustavo F. Padovan BT_DBG("sk %p, parent %p", sk, parent); 14590a708f8fSGustavo F. Padovan 1460b4450035SGustavo F. Padovan chan->conf_state = 0; 14610a708f8fSGustavo F. Padovan l2cap_sock_clear_timer(sk); 14620a708f8fSGustavo F. Padovan 14630a708f8fSGustavo F. Padovan if (!parent) { 14640a708f8fSGustavo F. Padovan /* Outgoing channel. 14650a708f8fSGustavo F. Padovan * Wake up socket sleeping on connect. 14660a708f8fSGustavo F. Padovan */ 14670a708f8fSGustavo F. Padovan sk->sk_state = BT_CONNECTED; 14680a708f8fSGustavo F. Padovan sk->sk_state_change(sk); 14690a708f8fSGustavo F. Padovan } else { 14700a708f8fSGustavo F. Padovan /* Incoming channel. 14710a708f8fSGustavo F. Padovan * Wake up socket sleeping on accept. 14720a708f8fSGustavo F. Padovan */ 14730a708f8fSGustavo F. Padovan parent->sk_data_ready(parent, 0); 14740a708f8fSGustavo F. Padovan } 14750a708f8fSGustavo F. Padovan } 14760a708f8fSGustavo F. Padovan 14770a708f8fSGustavo F. Padovan /* Copy frame to all raw sockets on that connection */ 14780a708f8fSGustavo F. Padovan static void l2cap_raw_recv(struct l2cap_conn *conn, struct sk_buff *skb) 14790a708f8fSGustavo F. Padovan { 14800a708f8fSGustavo F. Padovan struct sk_buff *nskb; 148148454079SGustavo F. Padovan struct l2cap_chan *chan; 14820a708f8fSGustavo F. Padovan 14830a708f8fSGustavo F. Padovan BT_DBG("conn %p", conn); 14840a708f8fSGustavo F. Padovan 1485baa7e1faSGustavo F. Padovan read_lock(&conn->chan_lock); 1486baa7e1faSGustavo F. Padovan list_for_each_entry(chan, &conn->chan_l, list) { 148748454079SGustavo F. Padovan struct sock *sk = chan->sk; 14880a708f8fSGustavo F. Padovan if (sk->sk_type != SOCK_RAW) 14890a708f8fSGustavo F. Padovan continue; 14900a708f8fSGustavo F. Padovan 14910a708f8fSGustavo F. Padovan /* Don't send frame to the socket it came from */ 14920a708f8fSGustavo F. Padovan if (skb->sk == sk) 14930a708f8fSGustavo F. Padovan continue; 14940a708f8fSGustavo F. Padovan nskb = skb_clone(skb, GFP_ATOMIC); 14950a708f8fSGustavo F. Padovan if (!nskb) 14960a708f8fSGustavo F. Padovan continue; 14970a708f8fSGustavo F. Padovan 14980a708f8fSGustavo F. Padovan if (sock_queue_rcv_skb(sk, nskb)) 14990a708f8fSGustavo F. Padovan kfree_skb(nskb); 15000a708f8fSGustavo F. Padovan } 1501baa7e1faSGustavo F. Padovan read_unlock(&conn->chan_lock); 15020a708f8fSGustavo F. Padovan } 15030a708f8fSGustavo F. Padovan 15040a708f8fSGustavo F. Padovan /* ---- L2CAP signalling commands ---- */ 15050a708f8fSGustavo F. Padovan static struct sk_buff *l2cap_build_cmd(struct l2cap_conn *conn, 15060a708f8fSGustavo F. Padovan u8 code, u8 ident, u16 dlen, void *data) 15070a708f8fSGustavo F. Padovan { 15080a708f8fSGustavo F. Padovan struct sk_buff *skb, **frag; 15090a708f8fSGustavo F. Padovan struct l2cap_cmd_hdr *cmd; 15100a708f8fSGustavo F. Padovan struct l2cap_hdr *lh; 15110a708f8fSGustavo F. Padovan int len, count; 15120a708f8fSGustavo F. Padovan 15130a708f8fSGustavo F. Padovan BT_DBG("conn %p, code 0x%2.2x, ident 0x%2.2x, len %d", 15140a708f8fSGustavo F. Padovan conn, code, ident, dlen); 15150a708f8fSGustavo F. Padovan 15160a708f8fSGustavo F. Padovan len = L2CAP_HDR_SIZE + L2CAP_CMD_HDR_SIZE + dlen; 15170a708f8fSGustavo F. Padovan count = min_t(unsigned int, conn->mtu, len); 15180a708f8fSGustavo F. Padovan 15190a708f8fSGustavo F. Padovan skb = bt_skb_alloc(count, GFP_ATOMIC); 15200a708f8fSGustavo F. Padovan if (!skb) 15210a708f8fSGustavo F. Padovan return NULL; 15220a708f8fSGustavo F. Padovan 15230a708f8fSGustavo F. Padovan lh = (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE); 15240a708f8fSGustavo F. Padovan lh->len = cpu_to_le16(L2CAP_CMD_HDR_SIZE + dlen); 15253300d9a9SClaudio Takahasi 15263300d9a9SClaudio Takahasi if (conn->hcon->type == LE_LINK) 15273300d9a9SClaudio Takahasi lh->cid = cpu_to_le16(L2CAP_CID_LE_SIGNALING); 15283300d9a9SClaudio Takahasi else 15290a708f8fSGustavo F. Padovan lh->cid = cpu_to_le16(L2CAP_CID_SIGNALING); 15300a708f8fSGustavo F. Padovan 15310a708f8fSGustavo F. Padovan cmd = (struct l2cap_cmd_hdr *) skb_put(skb, L2CAP_CMD_HDR_SIZE); 15320a708f8fSGustavo F. Padovan cmd->code = code; 15330a708f8fSGustavo F. Padovan cmd->ident = ident; 15340a708f8fSGustavo F. Padovan cmd->len = cpu_to_le16(dlen); 15350a708f8fSGustavo F. Padovan 15360a708f8fSGustavo F. Padovan if (dlen) { 15370a708f8fSGustavo F. Padovan count -= L2CAP_HDR_SIZE + L2CAP_CMD_HDR_SIZE; 15380a708f8fSGustavo F. Padovan memcpy(skb_put(skb, count), data, count); 15390a708f8fSGustavo F. Padovan data += count; 15400a708f8fSGustavo F. Padovan } 15410a708f8fSGustavo F. Padovan 15420a708f8fSGustavo F. Padovan len -= skb->len; 15430a708f8fSGustavo F. Padovan 15440a708f8fSGustavo F. Padovan /* Continuation fragments (no L2CAP header) */ 15450a708f8fSGustavo F. Padovan frag = &skb_shinfo(skb)->frag_list; 15460a708f8fSGustavo F. Padovan while (len) { 15470a708f8fSGustavo F. Padovan count = min_t(unsigned int, conn->mtu, len); 15480a708f8fSGustavo F. Padovan 15490a708f8fSGustavo F. Padovan *frag = bt_skb_alloc(count, GFP_ATOMIC); 15500a708f8fSGustavo F. Padovan if (!*frag) 15510a708f8fSGustavo F. Padovan goto fail; 15520a708f8fSGustavo F. Padovan 15530a708f8fSGustavo F. Padovan memcpy(skb_put(*frag, count), data, count); 15540a708f8fSGustavo F. Padovan 15550a708f8fSGustavo F. Padovan len -= count; 15560a708f8fSGustavo F. Padovan data += count; 15570a708f8fSGustavo F. Padovan 15580a708f8fSGustavo F. Padovan frag = &(*frag)->next; 15590a708f8fSGustavo F. Padovan } 15600a708f8fSGustavo F. Padovan 15610a708f8fSGustavo F. Padovan return skb; 15620a708f8fSGustavo F. Padovan 15630a708f8fSGustavo F. Padovan fail: 15640a708f8fSGustavo F. Padovan kfree_skb(skb); 15650a708f8fSGustavo F. Padovan return NULL; 15660a708f8fSGustavo F. Padovan } 15670a708f8fSGustavo F. Padovan 15680a708f8fSGustavo F. Padovan static inline int l2cap_get_conf_opt(void **ptr, int *type, int *olen, unsigned long *val) 15690a708f8fSGustavo F. Padovan { 15700a708f8fSGustavo F. Padovan struct l2cap_conf_opt *opt = *ptr; 15710a708f8fSGustavo F. Padovan int len; 15720a708f8fSGustavo F. Padovan 15730a708f8fSGustavo F. Padovan len = L2CAP_CONF_OPT_SIZE + opt->len; 15740a708f8fSGustavo F. Padovan *ptr += len; 15750a708f8fSGustavo F. Padovan 15760a708f8fSGustavo F. Padovan *type = opt->type; 15770a708f8fSGustavo F. Padovan *olen = opt->len; 15780a708f8fSGustavo F. Padovan 15790a708f8fSGustavo F. Padovan switch (opt->len) { 15800a708f8fSGustavo F. Padovan case 1: 15810a708f8fSGustavo F. Padovan *val = *((u8 *) opt->val); 15820a708f8fSGustavo F. Padovan break; 15830a708f8fSGustavo F. Padovan 15840a708f8fSGustavo F. Padovan case 2: 15850a708f8fSGustavo F. Padovan *val = get_unaligned_le16(opt->val); 15860a708f8fSGustavo F. Padovan break; 15870a708f8fSGustavo F. Padovan 15880a708f8fSGustavo F. Padovan case 4: 15890a708f8fSGustavo F. Padovan *val = get_unaligned_le32(opt->val); 15900a708f8fSGustavo F. Padovan break; 15910a708f8fSGustavo F. Padovan 15920a708f8fSGustavo F. Padovan default: 15930a708f8fSGustavo F. Padovan *val = (unsigned long) opt->val; 15940a708f8fSGustavo F. Padovan break; 15950a708f8fSGustavo F. Padovan } 15960a708f8fSGustavo F. Padovan 15970a708f8fSGustavo F. Padovan BT_DBG("type 0x%2.2x len %d val 0x%lx", *type, opt->len, *val); 15980a708f8fSGustavo F. Padovan return len; 15990a708f8fSGustavo F. Padovan } 16000a708f8fSGustavo F. Padovan 16010a708f8fSGustavo F. Padovan static void l2cap_add_conf_opt(void **ptr, u8 type, u8 len, unsigned long val) 16020a708f8fSGustavo F. Padovan { 16030a708f8fSGustavo F. Padovan struct l2cap_conf_opt *opt = *ptr; 16040a708f8fSGustavo F. Padovan 16050a708f8fSGustavo F. Padovan BT_DBG("type 0x%2.2x len %d val 0x%lx", type, len, val); 16060a708f8fSGustavo F. Padovan 16070a708f8fSGustavo F. Padovan opt->type = type; 16080a708f8fSGustavo F. Padovan opt->len = len; 16090a708f8fSGustavo F. Padovan 16100a708f8fSGustavo F. Padovan switch (len) { 16110a708f8fSGustavo F. Padovan case 1: 16120a708f8fSGustavo F. Padovan *((u8 *) opt->val) = val; 16130a708f8fSGustavo F. Padovan break; 16140a708f8fSGustavo F. Padovan 16150a708f8fSGustavo F. Padovan case 2: 16160a708f8fSGustavo F. Padovan put_unaligned_le16(val, opt->val); 16170a708f8fSGustavo F. Padovan break; 16180a708f8fSGustavo F. Padovan 16190a708f8fSGustavo F. Padovan case 4: 16200a708f8fSGustavo F. Padovan put_unaligned_le32(val, opt->val); 16210a708f8fSGustavo F. Padovan break; 16220a708f8fSGustavo F. Padovan 16230a708f8fSGustavo F. Padovan default: 16240a708f8fSGustavo F. Padovan memcpy(opt->val, (void *) val, len); 16250a708f8fSGustavo F. Padovan break; 16260a708f8fSGustavo F. Padovan } 16270a708f8fSGustavo F. Padovan 16280a708f8fSGustavo F. Padovan *ptr += L2CAP_CONF_OPT_SIZE + len; 16290a708f8fSGustavo F. Padovan } 16300a708f8fSGustavo F. Padovan 16310a708f8fSGustavo F. Padovan static void l2cap_ack_timeout(unsigned long arg) 16320a708f8fSGustavo F. Padovan { 1633525cd185SGustavo F. Padovan struct l2cap_chan *chan = (void *) arg; 16340a708f8fSGustavo F. Padovan 1635525cd185SGustavo F. Padovan bh_lock_sock(chan->sk); 1636525cd185SGustavo F. Padovan l2cap_send_ack(chan); 1637525cd185SGustavo F. Padovan bh_unlock_sock(chan->sk); 16380a708f8fSGustavo F. Padovan } 16390a708f8fSGustavo F. Padovan 1640525cd185SGustavo F. Padovan static inline void l2cap_ertm_init(struct l2cap_chan *chan) 16410a708f8fSGustavo F. Padovan { 1642525cd185SGustavo F. Padovan struct sock *sk = chan->sk; 1643525cd185SGustavo F. Padovan 164442e5c802SGustavo F. Padovan chan->expected_ack_seq = 0; 16456a026610SGustavo F. Padovan chan->unacked_frames = 0; 164642e5c802SGustavo F. Padovan chan->buffer_seq = 0; 16476a026610SGustavo F. Padovan chan->num_acked = 0; 16486a026610SGustavo F. Padovan chan->frames_sent = 0; 16490a708f8fSGustavo F. Padovan 1650e92c8e70SGustavo F. Padovan setup_timer(&chan->retrans_timer, l2cap_retrans_timeout, 1651e92c8e70SGustavo F. Padovan (unsigned long) chan); 1652e92c8e70SGustavo F. Padovan setup_timer(&chan->monitor_timer, l2cap_monitor_timeout, 1653e92c8e70SGustavo F. Padovan (unsigned long) chan); 1654e92c8e70SGustavo F. Padovan setup_timer(&chan->ack_timer, l2cap_ack_timeout, (unsigned long) chan); 16550a708f8fSGustavo F. Padovan 1656f1c6775bSGustavo F. Padovan skb_queue_head_init(&chan->srej_q); 1657f1c6775bSGustavo F. Padovan skb_queue_head_init(&chan->busy_q); 16580a708f8fSGustavo F. Padovan 165939d5a3eeSGustavo F. Padovan INIT_LIST_HEAD(&chan->srej_l); 166039d5a3eeSGustavo F. Padovan 1661311bb895SGustavo F. Padovan INIT_WORK(&chan->busy_work, l2cap_busy_work); 16620a708f8fSGustavo F. Padovan 16630a708f8fSGustavo F. Padovan sk->sk_backlog_rcv = l2cap_ertm_data_rcv; 16640a708f8fSGustavo F. Padovan } 16650a708f8fSGustavo F. Padovan 16660a708f8fSGustavo F. Padovan static inline __u8 l2cap_select_mode(__u8 mode, __u16 remote_feat_mask) 16670a708f8fSGustavo F. Padovan { 16680a708f8fSGustavo F. Padovan switch (mode) { 16690a708f8fSGustavo F. Padovan case L2CAP_MODE_STREAMING: 16700a708f8fSGustavo F. Padovan case L2CAP_MODE_ERTM: 16710a708f8fSGustavo F. Padovan if (l2cap_mode_supported(mode, remote_feat_mask)) 16720a708f8fSGustavo F. Padovan return mode; 16730a708f8fSGustavo F. Padovan /* fall through */ 16740a708f8fSGustavo F. Padovan default: 16750a708f8fSGustavo F. Padovan return L2CAP_MODE_BASIC; 16760a708f8fSGustavo F. Padovan } 16770a708f8fSGustavo F. Padovan } 16780a708f8fSGustavo F. Padovan 1679710f9b0aSGustavo F. Padovan static int l2cap_build_conf_req(struct l2cap_chan *chan, void *data) 16800a708f8fSGustavo F. Padovan { 16810a708f8fSGustavo F. Padovan struct l2cap_conf_req *req = data; 16820c1bc5c6SGustavo F. Padovan struct l2cap_conf_rfc rfc = { .mode = chan->mode }; 16830a708f8fSGustavo F. Padovan void *ptr = req->data; 16840a708f8fSGustavo F. Padovan 168549208c9cSGustavo F. Padovan BT_DBG("chan %p", chan); 16860a708f8fSGustavo F. Padovan 168773ffa904SGustavo F. Padovan if (chan->num_conf_req || chan->num_conf_rsp) 16880a708f8fSGustavo F. Padovan goto done; 16890a708f8fSGustavo F. Padovan 16900c1bc5c6SGustavo F. Padovan switch (chan->mode) { 16910a708f8fSGustavo F. Padovan case L2CAP_MODE_STREAMING: 16920a708f8fSGustavo F. Padovan case L2CAP_MODE_ERTM: 1693b4450035SGustavo F. Padovan if (chan->conf_state & L2CAP_CONF_STATE2_DEVICE) 16940a708f8fSGustavo F. Padovan break; 16950a708f8fSGustavo F. Padovan 16960a708f8fSGustavo F. Padovan /* fall through */ 16970a708f8fSGustavo F. Padovan default: 16988c1d787bSGustavo F. Padovan chan->mode = l2cap_select_mode(rfc.mode, chan->conn->feat_mask); 16990a708f8fSGustavo F. Padovan break; 17000a708f8fSGustavo F. Padovan } 17010a708f8fSGustavo F. Padovan 17020a708f8fSGustavo F. Padovan done: 17030c1bc5c6SGustavo F. Padovan if (chan->imtu != L2CAP_DEFAULT_MTU) 17040c1bc5c6SGustavo F. Padovan l2cap_add_conf_opt(&ptr, L2CAP_CONF_MTU, 2, chan->imtu); 17050a708f8fSGustavo F. Padovan 17060c1bc5c6SGustavo F. Padovan switch (chan->mode) { 17070a708f8fSGustavo F. Padovan case L2CAP_MODE_BASIC: 17088c1d787bSGustavo F. Padovan if (!(chan->conn->feat_mask & L2CAP_FEAT_ERTM) && 17098c1d787bSGustavo F. Padovan !(chan->conn->feat_mask & L2CAP_FEAT_STREAMING)) 17100a708f8fSGustavo F. Padovan break; 17110a708f8fSGustavo F. Padovan 17120a708f8fSGustavo F. Padovan rfc.mode = L2CAP_MODE_BASIC; 17130a708f8fSGustavo F. Padovan rfc.txwin_size = 0; 17140a708f8fSGustavo F. Padovan rfc.max_transmit = 0; 17150a708f8fSGustavo F. Padovan rfc.retrans_timeout = 0; 17160a708f8fSGustavo F. Padovan rfc.monitor_timeout = 0; 17170a708f8fSGustavo F. Padovan rfc.max_pdu_size = 0; 17180a708f8fSGustavo F. Padovan 17190a708f8fSGustavo F. Padovan l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC, sizeof(rfc), 17200a708f8fSGustavo F. Padovan (unsigned long) &rfc); 17210a708f8fSGustavo F. Padovan break; 17220a708f8fSGustavo F. Padovan 17230a708f8fSGustavo F. Padovan case L2CAP_MODE_ERTM: 17240a708f8fSGustavo F. Padovan rfc.mode = L2CAP_MODE_ERTM; 172547d1ec61SGustavo F. Padovan rfc.txwin_size = chan->tx_win; 172647d1ec61SGustavo F. Padovan rfc.max_transmit = chan->max_tx; 17270a708f8fSGustavo F. Padovan rfc.retrans_timeout = 0; 17280a708f8fSGustavo F. Padovan rfc.monitor_timeout = 0; 17290a708f8fSGustavo F. Padovan rfc.max_pdu_size = cpu_to_le16(L2CAP_DEFAULT_MAX_PDU_SIZE); 17308c1d787bSGustavo F. Padovan if (L2CAP_DEFAULT_MAX_PDU_SIZE > chan->conn->mtu - 10) 17318c1d787bSGustavo F. Padovan rfc.max_pdu_size = cpu_to_le16(chan->conn->mtu - 10); 17320a708f8fSGustavo F. Padovan 17330a708f8fSGustavo F. Padovan l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC, sizeof(rfc), 17340a708f8fSGustavo F. Padovan (unsigned long) &rfc); 17350a708f8fSGustavo F. Padovan 17368c1d787bSGustavo F. Padovan if (!(chan->conn->feat_mask & L2CAP_FEAT_FCS)) 17370a708f8fSGustavo F. Padovan break; 17380a708f8fSGustavo F. Padovan 173947d1ec61SGustavo F. Padovan if (chan->fcs == L2CAP_FCS_NONE || 1740b4450035SGustavo F. Padovan chan->conf_state & L2CAP_CONF_NO_FCS_RECV) { 174147d1ec61SGustavo F. Padovan chan->fcs = L2CAP_FCS_NONE; 174247d1ec61SGustavo F. Padovan l2cap_add_conf_opt(&ptr, L2CAP_CONF_FCS, 1, chan->fcs); 17430a708f8fSGustavo F. Padovan } 17440a708f8fSGustavo F. Padovan break; 17450a708f8fSGustavo F. Padovan 17460a708f8fSGustavo F. Padovan case L2CAP_MODE_STREAMING: 17470a708f8fSGustavo F. Padovan rfc.mode = L2CAP_MODE_STREAMING; 17480a708f8fSGustavo F. Padovan rfc.txwin_size = 0; 17490a708f8fSGustavo F. Padovan rfc.max_transmit = 0; 17500a708f8fSGustavo F. Padovan rfc.retrans_timeout = 0; 17510a708f8fSGustavo F. Padovan rfc.monitor_timeout = 0; 17520a708f8fSGustavo F. Padovan rfc.max_pdu_size = cpu_to_le16(L2CAP_DEFAULT_MAX_PDU_SIZE); 17538c1d787bSGustavo F. Padovan if (L2CAP_DEFAULT_MAX_PDU_SIZE > chan->conn->mtu - 10) 17548c1d787bSGustavo F. Padovan rfc.max_pdu_size = cpu_to_le16(chan->conn->mtu - 10); 17550a708f8fSGustavo F. Padovan 17560a708f8fSGustavo F. Padovan l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC, sizeof(rfc), 17570a708f8fSGustavo F. Padovan (unsigned long) &rfc); 17580a708f8fSGustavo F. Padovan 17598c1d787bSGustavo F. Padovan if (!(chan->conn->feat_mask & L2CAP_FEAT_FCS)) 17600a708f8fSGustavo F. Padovan break; 17610a708f8fSGustavo F. Padovan 176247d1ec61SGustavo F. Padovan if (chan->fcs == L2CAP_FCS_NONE || 1763b4450035SGustavo F. Padovan chan->conf_state & L2CAP_CONF_NO_FCS_RECV) { 176447d1ec61SGustavo F. Padovan chan->fcs = L2CAP_FCS_NONE; 176547d1ec61SGustavo F. Padovan l2cap_add_conf_opt(&ptr, L2CAP_CONF_FCS, 1, chan->fcs); 17660a708f8fSGustavo F. Padovan } 17670a708f8fSGustavo F. Padovan break; 17680a708f8fSGustavo F. Padovan } 17690a708f8fSGustavo F. Padovan 1770fe4128e0SGustavo F. Padovan req->dcid = cpu_to_le16(chan->dcid); 17710a708f8fSGustavo F. Padovan req->flags = cpu_to_le16(0); 17720a708f8fSGustavo F. Padovan 17730a708f8fSGustavo F. Padovan return ptr - data; 17740a708f8fSGustavo F. Padovan } 17750a708f8fSGustavo F. Padovan 177673ffa904SGustavo F. Padovan static int l2cap_parse_conf_req(struct l2cap_chan *chan, void *data) 17770a708f8fSGustavo F. Padovan { 17780a708f8fSGustavo F. Padovan struct l2cap_conf_rsp *rsp = data; 17790a708f8fSGustavo F. Padovan void *ptr = rsp->data; 178073ffa904SGustavo F. Padovan void *req = chan->conf_req; 178173ffa904SGustavo F. Padovan int len = chan->conf_len; 17820a708f8fSGustavo F. Padovan int type, hint, olen; 17830a708f8fSGustavo F. Padovan unsigned long val; 17840a708f8fSGustavo F. Padovan struct l2cap_conf_rfc rfc = { .mode = L2CAP_MODE_BASIC }; 17850a708f8fSGustavo F. Padovan u16 mtu = L2CAP_DEFAULT_MTU; 17860a708f8fSGustavo F. Padovan u16 result = L2CAP_CONF_SUCCESS; 17870a708f8fSGustavo F. Padovan 178873ffa904SGustavo F. Padovan BT_DBG("chan %p", chan); 17890a708f8fSGustavo F. Padovan 17900a708f8fSGustavo F. Padovan while (len >= L2CAP_CONF_OPT_SIZE) { 17910a708f8fSGustavo F. Padovan len -= l2cap_get_conf_opt(&req, &type, &olen, &val); 17920a708f8fSGustavo F. Padovan 17930a708f8fSGustavo F. Padovan hint = type & L2CAP_CONF_HINT; 17940a708f8fSGustavo F. Padovan type &= L2CAP_CONF_MASK; 17950a708f8fSGustavo F. Padovan 17960a708f8fSGustavo F. Padovan switch (type) { 17970a708f8fSGustavo F. Padovan case L2CAP_CONF_MTU: 17980a708f8fSGustavo F. Padovan mtu = val; 17990a708f8fSGustavo F. Padovan break; 18000a708f8fSGustavo F. Padovan 18010a708f8fSGustavo F. Padovan case L2CAP_CONF_FLUSH_TO: 18020c1bc5c6SGustavo F. Padovan chan->flush_to = val; 18030a708f8fSGustavo F. Padovan break; 18040a708f8fSGustavo F. Padovan 18050a708f8fSGustavo F. Padovan case L2CAP_CONF_QOS: 18060a708f8fSGustavo F. Padovan break; 18070a708f8fSGustavo F. Padovan 18080a708f8fSGustavo F. Padovan case L2CAP_CONF_RFC: 18090a708f8fSGustavo F. Padovan if (olen == sizeof(rfc)) 18100a708f8fSGustavo F. Padovan memcpy(&rfc, (void *) val, olen); 18110a708f8fSGustavo F. Padovan break; 18120a708f8fSGustavo F. Padovan 18130a708f8fSGustavo F. Padovan case L2CAP_CONF_FCS: 18140a708f8fSGustavo F. Padovan if (val == L2CAP_FCS_NONE) 1815b4450035SGustavo F. Padovan chan->conf_state |= L2CAP_CONF_NO_FCS_RECV; 18160a708f8fSGustavo F. Padovan 18170a708f8fSGustavo F. Padovan break; 18180a708f8fSGustavo F. Padovan 18190a708f8fSGustavo F. Padovan default: 18200a708f8fSGustavo F. Padovan if (hint) 18210a708f8fSGustavo F. Padovan break; 18220a708f8fSGustavo F. Padovan 18230a708f8fSGustavo F. Padovan result = L2CAP_CONF_UNKNOWN; 18240a708f8fSGustavo F. Padovan *((u8 *) ptr++) = type; 18250a708f8fSGustavo F. Padovan break; 18260a708f8fSGustavo F. Padovan } 18270a708f8fSGustavo F. Padovan } 18280a708f8fSGustavo F. Padovan 182973ffa904SGustavo F. Padovan if (chan->num_conf_rsp || chan->num_conf_req > 1) 18300a708f8fSGustavo F. Padovan goto done; 18310a708f8fSGustavo F. Padovan 18320c1bc5c6SGustavo F. Padovan switch (chan->mode) { 18330a708f8fSGustavo F. Padovan case L2CAP_MODE_STREAMING: 18340a708f8fSGustavo F. Padovan case L2CAP_MODE_ERTM: 1835b4450035SGustavo F. Padovan if (!(chan->conf_state & L2CAP_CONF_STATE2_DEVICE)) { 18360c1bc5c6SGustavo F. Padovan chan->mode = l2cap_select_mode(rfc.mode, 18378c1d787bSGustavo F. Padovan chan->conn->feat_mask); 18380a708f8fSGustavo F. Padovan break; 18390a708f8fSGustavo F. Padovan } 18400a708f8fSGustavo F. Padovan 18410c1bc5c6SGustavo F. Padovan if (chan->mode != rfc.mode) 18420a708f8fSGustavo F. Padovan return -ECONNREFUSED; 18430a708f8fSGustavo F. Padovan 18440a708f8fSGustavo F. Padovan break; 18450a708f8fSGustavo F. Padovan } 18460a708f8fSGustavo F. Padovan 18470a708f8fSGustavo F. Padovan done: 18480c1bc5c6SGustavo F. Padovan if (chan->mode != rfc.mode) { 18490a708f8fSGustavo F. Padovan result = L2CAP_CONF_UNACCEPT; 18500c1bc5c6SGustavo F. Padovan rfc.mode = chan->mode; 18510a708f8fSGustavo F. Padovan 185273ffa904SGustavo F. Padovan if (chan->num_conf_rsp == 1) 18530a708f8fSGustavo F. Padovan return -ECONNREFUSED; 18540a708f8fSGustavo F. Padovan 18550a708f8fSGustavo F. Padovan l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC, 18560a708f8fSGustavo F. Padovan sizeof(rfc), (unsigned long) &rfc); 18570a708f8fSGustavo F. Padovan } 18580a708f8fSGustavo F. Padovan 18590a708f8fSGustavo F. Padovan 18600a708f8fSGustavo F. Padovan if (result == L2CAP_CONF_SUCCESS) { 18610a708f8fSGustavo F. Padovan /* Configure output options and let the other side know 18620a708f8fSGustavo F. Padovan * which ones we don't like. */ 18630a708f8fSGustavo F. Padovan 18640a708f8fSGustavo F. Padovan if (mtu < L2CAP_DEFAULT_MIN_MTU) 18650a708f8fSGustavo F. Padovan result = L2CAP_CONF_UNACCEPT; 18660a708f8fSGustavo F. Padovan else { 18670c1bc5c6SGustavo F. Padovan chan->omtu = mtu; 1868b4450035SGustavo F. Padovan chan->conf_state |= L2CAP_CONF_MTU_DONE; 18690a708f8fSGustavo F. Padovan } 18700c1bc5c6SGustavo F. Padovan l2cap_add_conf_opt(&ptr, L2CAP_CONF_MTU, 2, chan->omtu); 18710a708f8fSGustavo F. Padovan 18720a708f8fSGustavo F. Padovan switch (rfc.mode) { 18730a708f8fSGustavo F. Padovan case L2CAP_MODE_BASIC: 187447d1ec61SGustavo F. Padovan chan->fcs = L2CAP_FCS_NONE; 1875b4450035SGustavo F. Padovan chan->conf_state |= L2CAP_CONF_MODE_DONE; 18760a708f8fSGustavo F. Padovan break; 18770a708f8fSGustavo F. Padovan 18780a708f8fSGustavo F. Padovan case L2CAP_MODE_ERTM: 18792c03a7a4SGustavo F. Padovan chan->remote_tx_win = rfc.txwin_size; 18802c03a7a4SGustavo F. Padovan chan->remote_max_tx = rfc.max_transmit; 18810a708f8fSGustavo F. Padovan 18828c1d787bSGustavo F. Padovan if (le16_to_cpu(rfc.max_pdu_size) > chan->conn->mtu - 10) 18838c1d787bSGustavo F. Padovan rfc.max_pdu_size = cpu_to_le16(chan->conn->mtu - 10); 18840a708f8fSGustavo F. Padovan 18852c03a7a4SGustavo F. Padovan chan->remote_mps = le16_to_cpu(rfc.max_pdu_size); 18860a708f8fSGustavo F. Padovan 18870a708f8fSGustavo F. Padovan rfc.retrans_timeout = 18880a708f8fSGustavo F. Padovan le16_to_cpu(L2CAP_DEFAULT_RETRANS_TO); 18890a708f8fSGustavo F. Padovan rfc.monitor_timeout = 18900a708f8fSGustavo F. Padovan le16_to_cpu(L2CAP_DEFAULT_MONITOR_TO); 18910a708f8fSGustavo F. Padovan 1892b4450035SGustavo F. Padovan chan->conf_state |= L2CAP_CONF_MODE_DONE; 18930a708f8fSGustavo F. Padovan 18940a708f8fSGustavo F. Padovan l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC, 18950a708f8fSGustavo F. Padovan sizeof(rfc), (unsigned long) &rfc); 18960a708f8fSGustavo F. Padovan 18970a708f8fSGustavo F. Padovan break; 18980a708f8fSGustavo F. Padovan 18990a708f8fSGustavo F. Padovan case L2CAP_MODE_STREAMING: 19008c1d787bSGustavo F. Padovan if (le16_to_cpu(rfc.max_pdu_size) > chan->conn->mtu - 10) 19018c1d787bSGustavo F. Padovan rfc.max_pdu_size = cpu_to_le16(chan->conn->mtu - 10); 19020a708f8fSGustavo F. Padovan 19032c03a7a4SGustavo F. Padovan chan->remote_mps = le16_to_cpu(rfc.max_pdu_size); 19040a708f8fSGustavo F. Padovan 1905b4450035SGustavo F. Padovan chan->conf_state |= L2CAP_CONF_MODE_DONE; 19060a708f8fSGustavo F. Padovan 19070a708f8fSGustavo F. Padovan l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC, 19080a708f8fSGustavo F. Padovan sizeof(rfc), (unsigned long) &rfc); 19090a708f8fSGustavo F. Padovan 19100a708f8fSGustavo F. Padovan break; 19110a708f8fSGustavo F. Padovan 19120a708f8fSGustavo F. Padovan default: 19130a708f8fSGustavo F. Padovan result = L2CAP_CONF_UNACCEPT; 19140a708f8fSGustavo F. Padovan 19150a708f8fSGustavo F. Padovan memset(&rfc, 0, sizeof(rfc)); 19160c1bc5c6SGustavo F. Padovan rfc.mode = chan->mode; 19170a708f8fSGustavo F. Padovan } 19180a708f8fSGustavo F. Padovan 19190a708f8fSGustavo F. Padovan if (result == L2CAP_CONF_SUCCESS) 1920b4450035SGustavo F. Padovan chan->conf_state |= L2CAP_CONF_OUTPUT_DONE; 19210a708f8fSGustavo F. Padovan } 1922fe4128e0SGustavo F. Padovan rsp->scid = cpu_to_le16(chan->dcid); 19230a708f8fSGustavo F. Padovan rsp->result = cpu_to_le16(result); 19240a708f8fSGustavo F. Padovan rsp->flags = cpu_to_le16(0x0000); 19250a708f8fSGustavo F. Padovan 19260a708f8fSGustavo F. Padovan return ptr - data; 19270a708f8fSGustavo F. Padovan } 19280a708f8fSGustavo F. Padovan 1929b4450035SGustavo F. Padovan static int l2cap_parse_conf_rsp(struct l2cap_chan *chan, void *rsp, int len, void *data, u16 *result) 19300a708f8fSGustavo F. Padovan { 19310a708f8fSGustavo F. Padovan struct l2cap_conf_req *req = data; 19320a708f8fSGustavo F. Padovan void *ptr = req->data; 19330a708f8fSGustavo F. Padovan int type, olen; 19340a708f8fSGustavo F. Padovan unsigned long val; 19350a708f8fSGustavo F. Padovan struct l2cap_conf_rfc rfc; 19360a708f8fSGustavo F. Padovan 1937fe4128e0SGustavo F. Padovan BT_DBG("chan %p, rsp %p, len %d, req %p", chan, rsp, len, data); 19380a708f8fSGustavo F. Padovan 19390a708f8fSGustavo F. Padovan while (len >= L2CAP_CONF_OPT_SIZE) { 19400a708f8fSGustavo F. Padovan len -= l2cap_get_conf_opt(&rsp, &type, &olen, &val); 19410a708f8fSGustavo F. Padovan 19420a708f8fSGustavo F. Padovan switch (type) { 19430a708f8fSGustavo F. Padovan case L2CAP_CONF_MTU: 19440a708f8fSGustavo F. Padovan if (val < L2CAP_DEFAULT_MIN_MTU) { 19450a708f8fSGustavo F. Padovan *result = L2CAP_CONF_UNACCEPT; 19460c1bc5c6SGustavo F. Padovan chan->imtu = L2CAP_DEFAULT_MIN_MTU; 19470a708f8fSGustavo F. Padovan } else 19480c1bc5c6SGustavo F. Padovan chan->imtu = val; 19490c1bc5c6SGustavo F. Padovan l2cap_add_conf_opt(&ptr, L2CAP_CONF_MTU, 2, chan->imtu); 19500a708f8fSGustavo F. Padovan break; 19510a708f8fSGustavo F. Padovan 19520a708f8fSGustavo F. Padovan case L2CAP_CONF_FLUSH_TO: 19530c1bc5c6SGustavo F. Padovan chan->flush_to = val; 19540a708f8fSGustavo F. Padovan l2cap_add_conf_opt(&ptr, L2CAP_CONF_FLUSH_TO, 19550c1bc5c6SGustavo F. Padovan 2, chan->flush_to); 19560a708f8fSGustavo F. Padovan break; 19570a708f8fSGustavo F. Padovan 19580a708f8fSGustavo F. Padovan case L2CAP_CONF_RFC: 19590a708f8fSGustavo F. Padovan if (olen == sizeof(rfc)) 19600a708f8fSGustavo F. Padovan memcpy(&rfc, (void *)val, olen); 19610a708f8fSGustavo F. Padovan 1962b4450035SGustavo F. Padovan if ((chan->conf_state & L2CAP_CONF_STATE2_DEVICE) && 19630c1bc5c6SGustavo F. Padovan rfc.mode != chan->mode) 19640a708f8fSGustavo F. Padovan return -ECONNREFUSED; 19650a708f8fSGustavo F. Padovan 196647d1ec61SGustavo F. Padovan chan->fcs = 0; 19670a708f8fSGustavo F. Padovan 19680a708f8fSGustavo F. Padovan l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC, 19690a708f8fSGustavo F. Padovan sizeof(rfc), (unsigned long) &rfc); 19700a708f8fSGustavo F. Padovan break; 19710a708f8fSGustavo F. Padovan } 19720a708f8fSGustavo F. Padovan } 19730a708f8fSGustavo F. Padovan 19740c1bc5c6SGustavo F. Padovan if (chan->mode == L2CAP_MODE_BASIC && chan->mode != rfc.mode) 19750a708f8fSGustavo F. Padovan return -ECONNREFUSED; 19760a708f8fSGustavo F. Padovan 19770c1bc5c6SGustavo F. Padovan chan->mode = rfc.mode; 19780a708f8fSGustavo F. Padovan 19790a708f8fSGustavo F. Padovan if (*result == L2CAP_CONF_SUCCESS) { 19800a708f8fSGustavo F. Padovan switch (rfc.mode) { 19810a708f8fSGustavo F. Padovan case L2CAP_MODE_ERTM: 198247d1ec61SGustavo F. Padovan chan->retrans_timeout = le16_to_cpu(rfc.retrans_timeout); 198347d1ec61SGustavo F. Padovan chan->monitor_timeout = le16_to_cpu(rfc.monitor_timeout); 198447d1ec61SGustavo F. Padovan chan->mps = le16_to_cpu(rfc.max_pdu_size); 19850a708f8fSGustavo F. Padovan break; 19860a708f8fSGustavo F. Padovan case L2CAP_MODE_STREAMING: 198747d1ec61SGustavo F. Padovan chan->mps = le16_to_cpu(rfc.max_pdu_size); 19880a708f8fSGustavo F. Padovan } 19890a708f8fSGustavo F. Padovan } 19900a708f8fSGustavo F. Padovan 1991fe4128e0SGustavo F. Padovan req->dcid = cpu_to_le16(chan->dcid); 19920a708f8fSGustavo F. Padovan req->flags = cpu_to_le16(0x0000); 19930a708f8fSGustavo F. Padovan 19940a708f8fSGustavo F. Padovan return ptr - data; 19950a708f8fSGustavo F. Padovan } 19960a708f8fSGustavo F. Padovan 1997fe4128e0SGustavo F. Padovan static int l2cap_build_conf_rsp(struct l2cap_chan *chan, void *data, u16 result, u16 flags) 19980a708f8fSGustavo F. Padovan { 19990a708f8fSGustavo F. Padovan struct l2cap_conf_rsp *rsp = data; 20000a708f8fSGustavo F. Padovan void *ptr = rsp->data; 20010a708f8fSGustavo F. Padovan 2002fe4128e0SGustavo F. Padovan BT_DBG("chan %p", chan); 20030a708f8fSGustavo F. Padovan 2004fe4128e0SGustavo F. Padovan rsp->scid = cpu_to_le16(chan->dcid); 20050a708f8fSGustavo F. Padovan rsp->result = cpu_to_le16(result); 20060a708f8fSGustavo F. Padovan rsp->flags = cpu_to_le16(flags); 20070a708f8fSGustavo F. Padovan 20080a708f8fSGustavo F. Padovan return ptr - data; 20090a708f8fSGustavo F. Padovan } 20100a708f8fSGustavo F. Padovan 20118c1d787bSGustavo F. Padovan void __l2cap_connect_rsp_defer(struct l2cap_chan *chan) 2012710f9b0aSGustavo F. Padovan { 2013710f9b0aSGustavo F. Padovan struct l2cap_conn_rsp rsp; 20148c1d787bSGustavo F. Padovan struct l2cap_conn *conn = chan->conn; 2015710f9b0aSGustavo F. Padovan u8 buf[128]; 2016710f9b0aSGustavo F. Padovan 2017fe4128e0SGustavo F. Padovan rsp.scid = cpu_to_le16(chan->dcid); 2018fe4128e0SGustavo F. Padovan rsp.dcid = cpu_to_le16(chan->scid); 2019710f9b0aSGustavo F. Padovan rsp.result = cpu_to_le16(L2CAP_CR_SUCCESS); 2020710f9b0aSGustavo F. Padovan rsp.status = cpu_to_le16(L2CAP_CS_NO_INFO); 2021710f9b0aSGustavo F. Padovan l2cap_send_cmd(conn, chan->ident, 2022710f9b0aSGustavo F. Padovan L2CAP_CONN_RSP, sizeof(rsp), &rsp); 2023710f9b0aSGustavo F. Padovan 2024b4450035SGustavo F. Padovan if (chan->conf_state & L2CAP_CONF_REQ_SENT) 2025710f9b0aSGustavo F. Padovan return; 2026710f9b0aSGustavo F. Padovan 2027b4450035SGustavo F. Padovan chan->conf_state |= L2CAP_CONF_REQ_SENT; 2028710f9b0aSGustavo F. Padovan l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ, 2029710f9b0aSGustavo F. Padovan l2cap_build_conf_req(chan, buf), buf); 2030710f9b0aSGustavo F. Padovan chan->num_conf_req++; 2031710f9b0aSGustavo F. Padovan } 2032710f9b0aSGustavo F. Padovan 203347d1ec61SGustavo F. Padovan static void l2cap_conf_rfc_get(struct l2cap_chan *chan, void *rsp, int len) 20340a708f8fSGustavo F. Padovan { 20350a708f8fSGustavo F. Padovan int type, olen; 20360a708f8fSGustavo F. Padovan unsigned long val; 20370a708f8fSGustavo F. Padovan struct l2cap_conf_rfc rfc; 20380a708f8fSGustavo F. Padovan 203947d1ec61SGustavo F. Padovan BT_DBG("chan %p, rsp %p, len %d", chan, rsp, len); 20400a708f8fSGustavo F. Padovan 20410c1bc5c6SGustavo F. Padovan if ((chan->mode != L2CAP_MODE_ERTM) && (chan->mode != L2CAP_MODE_STREAMING)) 20420a708f8fSGustavo F. Padovan return; 20430a708f8fSGustavo F. Padovan 20440a708f8fSGustavo F. Padovan while (len >= L2CAP_CONF_OPT_SIZE) { 20450a708f8fSGustavo F. Padovan len -= l2cap_get_conf_opt(&rsp, &type, &olen, &val); 20460a708f8fSGustavo F. Padovan 20470a708f8fSGustavo F. Padovan switch (type) { 20480a708f8fSGustavo F. Padovan case L2CAP_CONF_RFC: 20490a708f8fSGustavo F. Padovan if (olen == sizeof(rfc)) 20500a708f8fSGustavo F. Padovan memcpy(&rfc, (void *)val, olen); 20510a708f8fSGustavo F. Padovan goto done; 20520a708f8fSGustavo F. Padovan } 20530a708f8fSGustavo F. Padovan } 20540a708f8fSGustavo F. Padovan 20550a708f8fSGustavo F. Padovan done: 20560a708f8fSGustavo F. Padovan switch (rfc.mode) { 20570a708f8fSGustavo F. Padovan case L2CAP_MODE_ERTM: 205847d1ec61SGustavo F. Padovan chan->retrans_timeout = le16_to_cpu(rfc.retrans_timeout); 205947d1ec61SGustavo F. Padovan chan->monitor_timeout = le16_to_cpu(rfc.monitor_timeout); 206047d1ec61SGustavo F. Padovan chan->mps = le16_to_cpu(rfc.max_pdu_size); 20610a708f8fSGustavo F. Padovan break; 20620a708f8fSGustavo F. Padovan case L2CAP_MODE_STREAMING: 206347d1ec61SGustavo F. Padovan chan->mps = le16_to_cpu(rfc.max_pdu_size); 20640a708f8fSGustavo F. Padovan } 20650a708f8fSGustavo F. Padovan } 20660a708f8fSGustavo F. Padovan 20670a708f8fSGustavo F. Padovan static inline int l2cap_command_rej(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data) 20680a708f8fSGustavo F. Padovan { 20690a708f8fSGustavo F. Padovan struct l2cap_cmd_rej *rej = (struct l2cap_cmd_rej *) data; 20700a708f8fSGustavo F. Padovan 20710a708f8fSGustavo F. Padovan if (rej->reason != 0x0000) 20720a708f8fSGustavo F. Padovan return 0; 20730a708f8fSGustavo F. Padovan 20740a708f8fSGustavo F. Padovan if ((conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_SENT) && 20750a708f8fSGustavo F. Padovan cmd->ident == conn->info_ident) { 20760a708f8fSGustavo F. Padovan del_timer(&conn->info_timer); 20770a708f8fSGustavo F. Padovan 20780a708f8fSGustavo F. Padovan conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_DONE; 20790a708f8fSGustavo F. Padovan conn->info_ident = 0; 20800a708f8fSGustavo F. Padovan 20810a708f8fSGustavo F. Padovan l2cap_conn_start(conn); 20820a708f8fSGustavo F. Padovan } 20830a708f8fSGustavo F. Padovan 20840a708f8fSGustavo F. Padovan return 0; 20850a708f8fSGustavo F. Padovan } 20860a708f8fSGustavo F. Padovan 20870a708f8fSGustavo F. Padovan static inline int l2cap_connect_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data) 20880a708f8fSGustavo F. Padovan { 20890a708f8fSGustavo F. Padovan struct l2cap_conn_req *req = (struct l2cap_conn_req *) data; 20900a708f8fSGustavo F. Padovan struct l2cap_conn_rsp rsp; 209123691d75SGustavo F. Padovan struct l2cap_chan *chan = NULL, *pchan; 20920a708f8fSGustavo F. Padovan struct sock *parent, *sk = NULL; 20930a708f8fSGustavo F. Padovan int result, status = L2CAP_CS_NO_INFO; 20940a708f8fSGustavo F. Padovan 20950a708f8fSGustavo F. Padovan u16 dcid = 0, scid = __le16_to_cpu(req->scid); 20960a708f8fSGustavo F. Padovan __le16 psm = req->psm; 20970a708f8fSGustavo F. Padovan 20980a708f8fSGustavo F. Padovan BT_DBG("psm 0x%2.2x scid 0x%4.4x", psm, scid); 20990a708f8fSGustavo F. Padovan 21000a708f8fSGustavo F. Padovan /* Check if we have socket listening on psm */ 210123691d75SGustavo F. Padovan pchan = l2cap_global_chan_by_psm(BT_LISTEN, psm, conn->src); 210223691d75SGustavo F. Padovan if (!pchan) { 21030a708f8fSGustavo F. Padovan result = L2CAP_CR_BAD_PSM; 21040a708f8fSGustavo F. Padovan goto sendresp; 21050a708f8fSGustavo F. Padovan } 21060a708f8fSGustavo F. Padovan 210723691d75SGustavo F. Padovan parent = pchan->sk; 210823691d75SGustavo F. Padovan 21090a708f8fSGustavo F. Padovan bh_lock_sock(parent); 21100a708f8fSGustavo F. Padovan 21110a708f8fSGustavo F. Padovan /* Check if the ACL is secure enough (if not SDP) */ 21120a708f8fSGustavo F. Padovan if (psm != cpu_to_le16(0x0001) && 21130a708f8fSGustavo F. Padovan !hci_conn_check_link_mode(conn->hcon)) { 21140a708f8fSGustavo F. Padovan conn->disc_reason = 0x05; 21150a708f8fSGustavo F. Padovan result = L2CAP_CR_SEC_BLOCK; 21160a708f8fSGustavo F. Padovan goto response; 21170a708f8fSGustavo F. Padovan } 21180a708f8fSGustavo F. Padovan 21190a708f8fSGustavo F. Padovan result = L2CAP_CR_NO_MEM; 21200a708f8fSGustavo F. Padovan 21210a708f8fSGustavo F. Padovan /* Check for backlog size */ 21220a708f8fSGustavo F. Padovan if (sk_acceptq_is_full(parent)) { 21230a708f8fSGustavo F. Padovan BT_DBG("backlog full %d", parent->sk_ack_backlog); 21240a708f8fSGustavo F. Padovan goto response; 21250a708f8fSGustavo F. Padovan } 21260a708f8fSGustavo F. Padovan 21270a708f8fSGustavo F. Padovan sk = l2cap_sock_alloc(sock_net(parent), NULL, BTPROTO_L2CAP, GFP_ATOMIC); 21280a708f8fSGustavo F. Padovan if (!sk) 21290a708f8fSGustavo F. Padovan goto response; 21300a708f8fSGustavo F. Padovan 213123691d75SGustavo F. Padovan chan = l2cap_chan_create(sk); 213248454079SGustavo F. Padovan if (!chan) { 213348454079SGustavo F. Padovan l2cap_sock_kill(sk); 213448454079SGustavo F. Padovan goto response; 213548454079SGustavo F. Padovan } 213648454079SGustavo F. Padovan 21375d41ce1dSGustavo F. Padovan l2cap_pi(sk)->chan = chan; 21385d41ce1dSGustavo F. Padovan 2139baa7e1faSGustavo F. Padovan write_lock_bh(&conn->chan_lock); 21400a708f8fSGustavo F. Padovan 21410a708f8fSGustavo F. Padovan /* Check if we already have channel with that dcid */ 2142baa7e1faSGustavo F. Padovan if (__l2cap_get_chan_by_dcid(conn, scid)) { 2143baa7e1faSGustavo F. Padovan write_unlock_bh(&conn->chan_lock); 21440a708f8fSGustavo F. Padovan sock_set_flag(sk, SOCK_ZAPPED); 21450a708f8fSGustavo F. Padovan l2cap_sock_kill(sk); 21460a708f8fSGustavo F. Padovan goto response; 21470a708f8fSGustavo F. Padovan } 21480a708f8fSGustavo F. Padovan 21490a708f8fSGustavo F. Padovan hci_conn_hold(conn->hcon); 21500a708f8fSGustavo F. Padovan 21510a708f8fSGustavo F. Padovan l2cap_sock_init(sk, parent); 21520a708f8fSGustavo F. Padovan bacpy(&bt_sk(sk)->src, conn->src); 21530a708f8fSGustavo F. Padovan bacpy(&bt_sk(sk)->dst, conn->dst); 2154fe4128e0SGustavo F. Padovan chan->psm = psm; 2155fe4128e0SGustavo F. Padovan chan->dcid = scid; 21560a708f8fSGustavo F. Padovan 2157d1010240SGustavo F. Padovan bt_accept_enqueue(parent, sk); 2158d1010240SGustavo F. Padovan 215948454079SGustavo F. Padovan __l2cap_chan_add(conn, chan); 216048454079SGustavo F. Padovan 2161fe4128e0SGustavo F. Padovan dcid = chan->scid; 21620a708f8fSGustavo F. Padovan 21630a708f8fSGustavo F. Padovan l2cap_sock_set_timer(sk, sk->sk_sndtimeo); 21640a708f8fSGustavo F. Padovan 2165fc7f8a7eSGustavo F. Padovan chan->ident = cmd->ident; 21660a708f8fSGustavo F. Padovan 21670a708f8fSGustavo F. Padovan if (conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_DONE) { 21684343478fSGustavo F. Padovan if (l2cap_check_security(chan)) { 21690a708f8fSGustavo F. Padovan if (bt_sk(sk)->defer_setup) { 21700a708f8fSGustavo F. Padovan sk->sk_state = BT_CONNECT2; 21710a708f8fSGustavo F. Padovan result = L2CAP_CR_PEND; 21720a708f8fSGustavo F. Padovan status = L2CAP_CS_AUTHOR_PEND; 21730a708f8fSGustavo F. Padovan parent->sk_data_ready(parent, 0); 21740a708f8fSGustavo F. Padovan } else { 21750a708f8fSGustavo F. Padovan sk->sk_state = BT_CONFIG; 21760a708f8fSGustavo F. Padovan result = L2CAP_CR_SUCCESS; 21770a708f8fSGustavo F. Padovan status = L2CAP_CS_NO_INFO; 21780a708f8fSGustavo F. Padovan } 21790a708f8fSGustavo F. Padovan } else { 21800a708f8fSGustavo F. Padovan sk->sk_state = BT_CONNECT2; 21810a708f8fSGustavo F. Padovan result = L2CAP_CR_PEND; 21820a708f8fSGustavo F. Padovan status = L2CAP_CS_AUTHEN_PEND; 21830a708f8fSGustavo F. Padovan } 21840a708f8fSGustavo F. Padovan } else { 21850a708f8fSGustavo F. Padovan sk->sk_state = BT_CONNECT2; 21860a708f8fSGustavo F. Padovan result = L2CAP_CR_PEND; 21870a708f8fSGustavo F. Padovan status = L2CAP_CS_NO_INFO; 21880a708f8fSGustavo F. Padovan } 21890a708f8fSGustavo F. Padovan 2190baa7e1faSGustavo F. Padovan write_unlock_bh(&conn->chan_lock); 21910a708f8fSGustavo F. Padovan 21920a708f8fSGustavo F. Padovan response: 21930a708f8fSGustavo F. Padovan bh_unlock_sock(parent); 21940a708f8fSGustavo F. Padovan 21950a708f8fSGustavo F. Padovan sendresp: 21960a708f8fSGustavo F. Padovan rsp.scid = cpu_to_le16(scid); 21970a708f8fSGustavo F. Padovan rsp.dcid = cpu_to_le16(dcid); 21980a708f8fSGustavo F. Padovan rsp.result = cpu_to_le16(result); 21990a708f8fSGustavo F. Padovan rsp.status = cpu_to_le16(status); 22000a708f8fSGustavo F. Padovan l2cap_send_cmd(conn, cmd->ident, L2CAP_CONN_RSP, sizeof(rsp), &rsp); 22010a708f8fSGustavo F. Padovan 22020a708f8fSGustavo F. Padovan if (result == L2CAP_CR_PEND && status == L2CAP_CS_NO_INFO) { 22030a708f8fSGustavo F. Padovan struct l2cap_info_req info; 22040a708f8fSGustavo F. Padovan info.type = cpu_to_le16(L2CAP_IT_FEAT_MASK); 22050a708f8fSGustavo F. Padovan 22060a708f8fSGustavo F. Padovan conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_SENT; 22070a708f8fSGustavo F. Padovan conn->info_ident = l2cap_get_ident(conn); 22080a708f8fSGustavo F. Padovan 22090a708f8fSGustavo F. Padovan mod_timer(&conn->info_timer, jiffies + 22100a708f8fSGustavo F. Padovan msecs_to_jiffies(L2CAP_INFO_TIMEOUT)); 22110a708f8fSGustavo F. Padovan 22120a708f8fSGustavo F. Padovan l2cap_send_cmd(conn, conn->info_ident, 22130a708f8fSGustavo F. Padovan L2CAP_INFO_REQ, sizeof(info), &info); 22140a708f8fSGustavo F. Padovan } 22150a708f8fSGustavo F. Padovan 2216b4450035SGustavo F. Padovan if (chan && !(chan->conf_state & L2CAP_CONF_REQ_SENT) && 22170a708f8fSGustavo F. Padovan result == L2CAP_CR_SUCCESS) { 22180a708f8fSGustavo F. Padovan u8 buf[128]; 2219b4450035SGustavo F. Padovan chan->conf_state |= L2CAP_CONF_REQ_SENT; 22200a708f8fSGustavo F. Padovan l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ, 222173ffa904SGustavo F. Padovan l2cap_build_conf_req(chan, buf), buf); 222273ffa904SGustavo F. Padovan chan->num_conf_req++; 22230a708f8fSGustavo F. Padovan } 22240a708f8fSGustavo F. Padovan 22250a708f8fSGustavo F. Padovan return 0; 22260a708f8fSGustavo F. Padovan } 22270a708f8fSGustavo F. Padovan 22280a708f8fSGustavo F. Padovan static inline int l2cap_connect_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data) 22290a708f8fSGustavo F. Padovan { 22300a708f8fSGustavo F. Padovan struct l2cap_conn_rsp *rsp = (struct l2cap_conn_rsp *) data; 22310a708f8fSGustavo F. Padovan u16 scid, dcid, result, status; 223248454079SGustavo F. Padovan struct l2cap_chan *chan; 22330a708f8fSGustavo F. Padovan struct sock *sk; 22340a708f8fSGustavo F. Padovan u8 req[128]; 22350a708f8fSGustavo F. Padovan 22360a708f8fSGustavo F. Padovan scid = __le16_to_cpu(rsp->scid); 22370a708f8fSGustavo F. Padovan dcid = __le16_to_cpu(rsp->dcid); 22380a708f8fSGustavo F. Padovan result = __le16_to_cpu(rsp->result); 22390a708f8fSGustavo F. Padovan status = __le16_to_cpu(rsp->status); 22400a708f8fSGustavo F. Padovan 22410a708f8fSGustavo F. Padovan BT_DBG("dcid 0x%4.4x scid 0x%4.4x result 0x%2.2x status 0x%2.2x", dcid, scid, result, status); 22420a708f8fSGustavo F. Padovan 22430a708f8fSGustavo F. Padovan if (scid) { 2244baa7e1faSGustavo F. Padovan chan = l2cap_get_chan_by_scid(conn, scid); 224548454079SGustavo F. Padovan if (!chan) 22460a708f8fSGustavo F. Padovan return -EFAULT; 22470a708f8fSGustavo F. Padovan } else { 2248baa7e1faSGustavo F. Padovan chan = l2cap_get_chan_by_ident(conn, cmd->ident); 224948454079SGustavo F. Padovan if (!chan) 22500a708f8fSGustavo F. Padovan return -EFAULT; 22510a708f8fSGustavo F. Padovan } 22520a708f8fSGustavo F. Padovan 225348454079SGustavo F. Padovan sk = chan->sk; 225448454079SGustavo F. Padovan 22550a708f8fSGustavo F. Padovan switch (result) { 22560a708f8fSGustavo F. Padovan case L2CAP_CR_SUCCESS: 22570a708f8fSGustavo F. Padovan sk->sk_state = BT_CONFIG; 2258fc7f8a7eSGustavo F. Padovan chan->ident = 0; 2259fe4128e0SGustavo F. Padovan chan->dcid = dcid; 2260b4450035SGustavo F. Padovan chan->conf_state &= ~L2CAP_CONF_CONNECT_PEND; 22610a708f8fSGustavo F. Padovan 2262b4450035SGustavo F. Padovan if (chan->conf_state & L2CAP_CONF_REQ_SENT) 22630a708f8fSGustavo F. Padovan break; 22640a708f8fSGustavo F. Padovan 2265b4450035SGustavo F. Padovan chan->conf_state |= L2CAP_CONF_REQ_SENT; 22660a708f8fSGustavo F. Padovan 22670a708f8fSGustavo F. Padovan l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ, 226873ffa904SGustavo F. Padovan l2cap_build_conf_req(chan, req), req); 226973ffa904SGustavo F. Padovan chan->num_conf_req++; 22700a708f8fSGustavo F. Padovan break; 22710a708f8fSGustavo F. Padovan 22720a708f8fSGustavo F. Padovan case L2CAP_CR_PEND: 2273b4450035SGustavo F. Padovan chan->conf_state |= L2CAP_CONF_CONNECT_PEND; 22740a708f8fSGustavo F. Padovan break; 22750a708f8fSGustavo F. Padovan 22760a708f8fSGustavo F. Padovan default: 22770a708f8fSGustavo F. Padovan /* don't delete l2cap channel if sk is owned by user */ 22780a708f8fSGustavo F. Padovan if (sock_owned_by_user(sk)) { 22790a708f8fSGustavo F. Padovan sk->sk_state = BT_DISCONN; 22800a708f8fSGustavo F. Padovan l2cap_sock_clear_timer(sk); 22810a708f8fSGustavo F. Padovan l2cap_sock_set_timer(sk, HZ / 5); 22820a708f8fSGustavo F. Padovan break; 22830a708f8fSGustavo F. Padovan } 22840a708f8fSGustavo F. Padovan 228548454079SGustavo F. Padovan l2cap_chan_del(chan, ECONNREFUSED); 22860a708f8fSGustavo F. Padovan break; 22870a708f8fSGustavo F. Padovan } 22880a708f8fSGustavo F. Padovan 22890a708f8fSGustavo F. Padovan bh_unlock_sock(sk); 22900a708f8fSGustavo F. Padovan return 0; 22910a708f8fSGustavo F. Padovan } 22920a708f8fSGustavo F. Padovan 229347d1ec61SGustavo F. Padovan static inline void set_default_fcs(struct l2cap_chan *chan) 22940a708f8fSGustavo F. Padovan { 229547d1ec61SGustavo F. Padovan struct l2cap_pinfo *pi = l2cap_pi(chan->sk); 229647d1ec61SGustavo F. Padovan 22970a708f8fSGustavo F. Padovan /* FCS is enabled only in ERTM or streaming mode, if one or both 22980a708f8fSGustavo F. Padovan * sides request it. 22990a708f8fSGustavo F. Padovan */ 23000c1bc5c6SGustavo F. Padovan if (chan->mode != L2CAP_MODE_ERTM && chan->mode != L2CAP_MODE_STREAMING) 230147d1ec61SGustavo F. Padovan chan->fcs = L2CAP_FCS_NONE; 2302b4450035SGustavo F. Padovan else if (!(pi->chan->conf_state & L2CAP_CONF_NO_FCS_RECV)) 230347d1ec61SGustavo F. Padovan chan->fcs = L2CAP_FCS_CRC16; 23040a708f8fSGustavo F. Padovan } 23050a708f8fSGustavo F. Padovan 23060a708f8fSGustavo F. Padovan static inline int l2cap_config_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u16 cmd_len, u8 *data) 23070a708f8fSGustavo F. Padovan { 23080a708f8fSGustavo F. Padovan struct l2cap_conf_req *req = (struct l2cap_conf_req *) data; 23090a708f8fSGustavo F. Padovan u16 dcid, flags; 23100a708f8fSGustavo F. Padovan u8 rsp[64]; 231148454079SGustavo F. Padovan struct l2cap_chan *chan; 23120a708f8fSGustavo F. Padovan struct sock *sk; 23130a708f8fSGustavo F. Padovan int len; 23140a708f8fSGustavo F. Padovan 23150a708f8fSGustavo F. Padovan dcid = __le16_to_cpu(req->dcid); 23160a708f8fSGustavo F. Padovan flags = __le16_to_cpu(req->flags); 23170a708f8fSGustavo F. Padovan 23180a708f8fSGustavo F. Padovan BT_DBG("dcid 0x%4.4x flags 0x%2.2x", dcid, flags); 23190a708f8fSGustavo F. Padovan 2320baa7e1faSGustavo F. Padovan chan = l2cap_get_chan_by_scid(conn, dcid); 232148454079SGustavo F. Padovan if (!chan) 23220a708f8fSGustavo F. Padovan return -ENOENT; 23230a708f8fSGustavo F. Padovan 232448454079SGustavo F. Padovan sk = chan->sk; 232548454079SGustavo F. Padovan 23260a708f8fSGustavo F. Padovan if (sk->sk_state != BT_CONFIG) { 23270a708f8fSGustavo F. Padovan struct l2cap_cmd_rej rej; 23280a708f8fSGustavo F. Padovan 23290a708f8fSGustavo F. Padovan rej.reason = cpu_to_le16(0x0002); 23300a708f8fSGustavo F. Padovan l2cap_send_cmd(conn, cmd->ident, L2CAP_COMMAND_REJ, 23310a708f8fSGustavo F. Padovan sizeof(rej), &rej); 23320a708f8fSGustavo F. Padovan goto unlock; 23330a708f8fSGustavo F. Padovan } 23340a708f8fSGustavo F. Padovan 23350a708f8fSGustavo F. Padovan /* Reject if config buffer is too small. */ 23360a708f8fSGustavo F. Padovan len = cmd_len - sizeof(*req); 233773ffa904SGustavo F. Padovan if (chan->conf_len + len > sizeof(chan->conf_req)) { 23380a708f8fSGustavo F. Padovan l2cap_send_cmd(conn, cmd->ident, L2CAP_CONF_RSP, 2339fe4128e0SGustavo F. Padovan l2cap_build_conf_rsp(chan, rsp, 23400a708f8fSGustavo F. Padovan L2CAP_CONF_REJECT, flags), rsp); 23410a708f8fSGustavo F. Padovan goto unlock; 23420a708f8fSGustavo F. Padovan } 23430a708f8fSGustavo F. Padovan 23440a708f8fSGustavo F. Padovan /* Store config. */ 234573ffa904SGustavo F. Padovan memcpy(chan->conf_req + chan->conf_len, req->data, len); 234673ffa904SGustavo F. Padovan chan->conf_len += len; 23470a708f8fSGustavo F. Padovan 23480a708f8fSGustavo F. Padovan if (flags & 0x0001) { 23490a708f8fSGustavo F. Padovan /* Incomplete config. Send empty response. */ 23500a708f8fSGustavo F. Padovan l2cap_send_cmd(conn, cmd->ident, L2CAP_CONF_RSP, 2351fe4128e0SGustavo F. Padovan l2cap_build_conf_rsp(chan, rsp, 23520a708f8fSGustavo F. Padovan L2CAP_CONF_SUCCESS, 0x0001), rsp); 23530a708f8fSGustavo F. Padovan goto unlock; 23540a708f8fSGustavo F. Padovan } 23550a708f8fSGustavo F. Padovan 23560a708f8fSGustavo F. Padovan /* Complete config. */ 235773ffa904SGustavo F. Padovan len = l2cap_parse_conf_req(chan, rsp); 23580a708f8fSGustavo F. Padovan if (len < 0) { 2359e92c8e70SGustavo F. Padovan l2cap_send_disconn_req(conn, chan, ECONNRESET); 23600a708f8fSGustavo F. Padovan goto unlock; 23610a708f8fSGustavo F. Padovan } 23620a708f8fSGustavo F. Padovan 23630a708f8fSGustavo F. Padovan l2cap_send_cmd(conn, cmd->ident, L2CAP_CONF_RSP, len, rsp); 236473ffa904SGustavo F. Padovan chan->num_conf_rsp++; 23650a708f8fSGustavo F. Padovan 23660a708f8fSGustavo F. Padovan /* Reset config buffer. */ 236773ffa904SGustavo F. Padovan chan->conf_len = 0; 23680a708f8fSGustavo F. Padovan 2369b4450035SGustavo F. Padovan if (!(chan->conf_state & L2CAP_CONF_OUTPUT_DONE)) 23700a708f8fSGustavo F. Padovan goto unlock; 23710a708f8fSGustavo F. Padovan 2372b4450035SGustavo F. Padovan if (chan->conf_state & L2CAP_CONF_INPUT_DONE) { 237347d1ec61SGustavo F. Padovan set_default_fcs(chan); 23740a708f8fSGustavo F. Padovan 23750a708f8fSGustavo F. Padovan sk->sk_state = BT_CONNECTED; 23760a708f8fSGustavo F. Padovan 237742e5c802SGustavo F. Padovan chan->next_tx_seq = 0; 237842e5c802SGustavo F. Padovan chan->expected_tx_seq = 0; 237958d35f87SGustavo F. Padovan skb_queue_head_init(&chan->tx_q); 23800c1bc5c6SGustavo F. Padovan if (chan->mode == L2CAP_MODE_ERTM) 2381525cd185SGustavo F. Padovan l2cap_ertm_init(chan); 23820a708f8fSGustavo F. Padovan 23830a708f8fSGustavo F. Padovan l2cap_chan_ready(sk); 23840a708f8fSGustavo F. Padovan goto unlock; 23850a708f8fSGustavo F. Padovan } 23860a708f8fSGustavo F. Padovan 2387b4450035SGustavo F. Padovan if (!(chan->conf_state & L2CAP_CONF_REQ_SENT)) { 23880a708f8fSGustavo F. Padovan u8 buf[64]; 2389b4450035SGustavo F. Padovan chan->conf_state |= L2CAP_CONF_REQ_SENT; 23900a708f8fSGustavo F. Padovan l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ, 239173ffa904SGustavo F. Padovan l2cap_build_conf_req(chan, buf), buf); 239273ffa904SGustavo F. Padovan chan->num_conf_req++; 23930a708f8fSGustavo F. Padovan } 23940a708f8fSGustavo F. Padovan 23950a708f8fSGustavo F. Padovan unlock: 23960a708f8fSGustavo F. Padovan bh_unlock_sock(sk); 23970a708f8fSGustavo F. Padovan return 0; 23980a708f8fSGustavo F. Padovan } 23990a708f8fSGustavo F. Padovan 24000a708f8fSGustavo F. Padovan static inline int l2cap_config_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data) 24010a708f8fSGustavo F. Padovan { 24020a708f8fSGustavo F. Padovan struct l2cap_conf_rsp *rsp = (struct l2cap_conf_rsp *)data; 24030a708f8fSGustavo F. Padovan u16 scid, flags, result; 240448454079SGustavo F. Padovan struct l2cap_chan *chan; 24050a708f8fSGustavo F. Padovan struct sock *sk; 24060a708f8fSGustavo F. Padovan int len = cmd->len - sizeof(*rsp); 24070a708f8fSGustavo F. Padovan 24080a708f8fSGustavo F. Padovan scid = __le16_to_cpu(rsp->scid); 24090a708f8fSGustavo F. Padovan flags = __le16_to_cpu(rsp->flags); 24100a708f8fSGustavo F. Padovan result = __le16_to_cpu(rsp->result); 24110a708f8fSGustavo F. Padovan 24120a708f8fSGustavo F. Padovan BT_DBG("scid 0x%4.4x flags 0x%2.2x result 0x%2.2x", 24130a708f8fSGustavo F. Padovan scid, flags, result); 24140a708f8fSGustavo F. Padovan 2415baa7e1faSGustavo F. Padovan chan = l2cap_get_chan_by_scid(conn, scid); 241648454079SGustavo F. Padovan if (!chan) 24170a708f8fSGustavo F. Padovan return 0; 24180a708f8fSGustavo F. Padovan 241948454079SGustavo F. Padovan sk = chan->sk; 242048454079SGustavo F. Padovan 24210a708f8fSGustavo F. Padovan switch (result) { 24220a708f8fSGustavo F. Padovan case L2CAP_CONF_SUCCESS: 242347d1ec61SGustavo F. Padovan l2cap_conf_rfc_get(chan, rsp->data, len); 24240a708f8fSGustavo F. Padovan break; 24250a708f8fSGustavo F. Padovan 24260a708f8fSGustavo F. Padovan case L2CAP_CONF_UNACCEPT: 242773ffa904SGustavo F. Padovan if (chan->num_conf_rsp <= L2CAP_CONF_MAX_CONF_RSP) { 24280a708f8fSGustavo F. Padovan char req[64]; 24290a708f8fSGustavo F. Padovan 24300a708f8fSGustavo F. Padovan if (len > sizeof(req) - sizeof(struct l2cap_conf_req)) { 2431e92c8e70SGustavo F. Padovan l2cap_send_disconn_req(conn, chan, ECONNRESET); 24320a708f8fSGustavo F. Padovan goto done; 24330a708f8fSGustavo F. Padovan } 24340a708f8fSGustavo F. Padovan 24350a708f8fSGustavo F. Padovan /* throw out any old stored conf requests */ 24360a708f8fSGustavo F. Padovan result = L2CAP_CONF_SUCCESS; 2437b4450035SGustavo F. Padovan len = l2cap_parse_conf_rsp(chan, rsp->data, len, 2438b4450035SGustavo F. Padovan req, &result); 24390a708f8fSGustavo F. Padovan if (len < 0) { 2440e92c8e70SGustavo F. Padovan l2cap_send_disconn_req(conn, chan, ECONNRESET); 24410a708f8fSGustavo F. Padovan goto done; 24420a708f8fSGustavo F. Padovan } 24430a708f8fSGustavo F. Padovan 24440a708f8fSGustavo F. Padovan l2cap_send_cmd(conn, l2cap_get_ident(conn), 24450a708f8fSGustavo F. Padovan L2CAP_CONF_REQ, len, req); 244673ffa904SGustavo F. Padovan chan->num_conf_req++; 24470a708f8fSGustavo F. Padovan if (result != L2CAP_CONF_SUCCESS) 24480a708f8fSGustavo F. Padovan goto done; 24490a708f8fSGustavo F. Padovan break; 24500a708f8fSGustavo F. Padovan } 24510a708f8fSGustavo F. Padovan 24520a708f8fSGustavo F. Padovan default: 24530a708f8fSGustavo F. Padovan sk->sk_err = ECONNRESET; 24540a708f8fSGustavo F. Padovan l2cap_sock_set_timer(sk, HZ * 5); 2455e92c8e70SGustavo F. Padovan l2cap_send_disconn_req(conn, chan, ECONNRESET); 24560a708f8fSGustavo F. Padovan goto done; 24570a708f8fSGustavo F. Padovan } 24580a708f8fSGustavo F. Padovan 24590a708f8fSGustavo F. Padovan if (flags & 0x01) 24600a708f8fSGustavo F. Padovan goto done; 24610a708f8fSGustavo F. Padovan 2462b4450035SGustavo F. Padovan chan->conf_state |= L2CAP_CONF_INPUT_DONE; 24630a708f8fSGustavo F. Padovan 2464b4450035SGustavo F. Padovan if (chan->conf_state & L2CAP_CONF_OUTPUT_DONE) { 246547d1ec61SGustavo F. Padovan set_default_fcs(chan); 24660a708f8fSGustavo F. Padovan 24670a708f8fSGustavo F. Padovan sk->sk_state = BT_CONNECTED; 246842e5c802SGustavo F. Padovan chan->next_tx_seq = 0; 246942e5c802SGustavo F. Padovan chan->expected_tx_seq = 0; 247058d35f87SGustavo F. Padovan skb_queue_head_init(&chan->tx_q); 24710c1bc5c6SGustavo F. Padovan if (chan->mode == L2CAP_MODE_ERTM) 2472525cd185SGustavo F. Padovan l2cap_ertm_init(chan); 24730a708f8fSGustavo F. Padovan 24740a708f8fSGustavo F. Padovan l2cap_chan_ready(sk); 24750a708f8fSGustavo F. Padovan } 24760a708f8fSGustavo F. Padovan 24770a708f8fSGustavo F. Padovan done: 24780a708f8fSGustavo F. Padovan bh_unlock_sock(sk); 24790a708f8fSGustavo F. Padovan return 0; 24800a708f8fSGustavo F. Padovan } 24810a708f8fSGustavo F. Padovan 24820a708f8fSGustavo F. Padovan static inline int l2cap_disconnect_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data) 24830a708f8fSGustavo F. Padovan { 24840a708f8fSGustavo F. Padovan struct l2cap_disconn_req *req = (struct l2cap_disconn_req *) data; 24850a708f8fSGustavo F. Padovan struct l2cap_disconn_rsp rsp; 24860a708f8fSGustavo F. Padovan u16 dcid, scid; 248748454079SGustavo F. Padovan struct l2cap_chan *chan; 24880a708f8fSGustavo F. Padovan struct sock *sk; 24890a708f8fSGustavo F. Padovan 24900a708f8fSGustavo F. Padovan scid = __le16_to_cpu(req->scid); 24910a708f8fSGustavo F. Padovan dcid = __le16_to_cpu(req->dcid); 24920a708f8fSGustavo F. Padovan 24930a708f8fSGustavo F. Padovan BT_DBG("scid 0x%4.4x dcid 0x%4.4x", scid, dcid); 24940a708f8fSGustavo F. Padovan 2495baa7e1faSGustavo F. Padovan chan = l2cap_get_chan_by_scid(conn, dcid); 249648454079SGustavo F. Padovan if (!chan) 24970a708f8fSGustavo F. Padovan return 0; 24980a708f8fSGustavo F. Padovan 249948454079SGustavo F. Padovan sk = chan->sk; 250048454079SGustavo F. Padovan 2501fe4128e0SGustavo F. Padovan rsp.dcid = cpu_to_le16(chan->scid); 2502fe4128e0SGustavo F. Padovan rsp.scid = cpu_to_le16(chan->dcid); 25030a708f8fSGustavo F. Padovan l2cap_send_cmd(conn, cmd->ident, L2CAP_DISCONN_RSP, sizeof(rsp), &rsp); 25040a708f8fSGustavo F. Padovan 25050a708f8fSGustavo F. Padovan sk->sk_shutdown = SHUTDOWN_MASK; 25060a708f8fSGustavo F. Padovan 25070a708f8fSGustavo F. Padovan /* don't delete l2cap channel if sk is owned by user */ 25080a708f8fSGustavo F. Padovan if (sock_owned_by_user(sk)) { 25090a708f8fSGustavo F. Padovan sk->sk_state = BT_DISCONN; 25100a708f8fSGustavo F. Padovan l2cap_sock_clear_timer(sk); 25110a708f8fSGustavo F. Padovan l2cap_sock_set_timer(sk, HZ / 5); 25120a708f8fSGustavo F. Padovan bh_unlock_sock(sk); 25130a708f8fSGustavo F. Padovan return 0; 25140a708f8fSGustavo F. Padovan } 25150a708f8fSGustavo F. Padovan 251648454079SGustavo F. Padovan l2cap_chan_del(chan, ECONNRESET); 25170a708f8fSGustavo F. Padovan bh_unlock_sock(sk); 25180a708f8fSGustavo F. Padovan 25190a708f8fSGustavo F. Padovan l2cap_sock_kill(sk); 25200a708f8fSGustavo F. Padovan return 0; 25210a708f8fSGustavo F. Padovan } 25220a708f8fSGustavo F. Padovan 25230a708f8fSGustavo F. Padovan static inline int l2cap_disconnect_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data) 25240a708f8fSGustavo F. Padovan { 25250a708f8fSGustavo F. Padovan struct l2cap_disconn_rsp *rsp = (struct l2cap_disconn_rsp *) data; 25260a708f8fSGustavo F. Padovan u16 dcid, scid; 252748454079SGustavo F. Padovan struct l2cap_chan *chan; 25280a708f8fSGustavo F. Padovan struct sock *sk; 25290a708f8fSGustavo F. Padovan 25300a708f8fSGustavo F. Padovan scid = __le16_to_cpu(rsp->scid); 25310a708f8fSGustavo F. Padovan dcid = __le16_to_cpu(rsp->dcid); 25320a708f8fSGustavo F. Padovan 25330a708f8fSGustavo F. Padovan BT_DBG("dcid 0x%4.4x scid 0x%4.4x", dcid, scid); 25340a708f8fSGustavo F. Padovan 2535baa7e1faSGustavo F. Padovan chan = l2cap_get_chan_by_scid(conn, scid); 253648454079SGustavo F. Padovan if (!chan) 25370a708f8fSGustavo F. Padovan return 0; 25380a708f8fSGustavo F. Padovan 253948454079SGustavo F. Padovan sk = chan->sk; 254048454079SGustavo F. Padovan 25410a708f8fSGustavo F. Padovan /* don't delete l2cap channel if sk is owned by user */ 25420a708f8fSGustavo F. Padovan if (sock_owned_by_user(sk)) { 25430a708f8fSGustavo F. Padovan sk->sk_state = BT_DISCONN; 25440a708f8fSGustavo F. Padovan l2cap_sock_clear_timer(sk); 25450a708f8fSGustavo F. Padovan l2cap_sock_set_timer(sk, HZ / 5); 25460a708f8fSGustavo F. Padovan bh_unlock_sock(sk); 25470a708f8fSGustavo F. Padovan return 0; 25480a708f8fSGustavo F. Padovan } 25490a708f8fSGustavo F. Padovan 255048454079SGustavo F. Padovan l2cap_chan_del(chan, 0); 25510a708f8fSGustavo F. Padovan bh_unlock_sock(sk); 25520a708f8fSGustavo F. Padovan 25530a708f8fSGustavo F. Padovan l2cap_sock_kill(sk); 25540a708f8fSGustavo F. Padovan return 0; 25550a708f8fSGustavo F. Padovan } 25560a708f8fSGustavo F. Padovan 25570a708f8fSGustavo F. Padovan static inline int l2cap_information_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data) 25580a708f8fSGustavo F. Padovan { 25590a708f8fSGustavo F. Padovan struct l2cap_info_req *req = (struct l2cap_info_req *) data; 25600a708f8fSGustavo F. Padovan u16 type; 25610a708f8fSGustavo F. Padovan 25620a708f8fSGustavo F. Padovan type = __le16_to_cpu(req->type); 25630a708f8fSGustavo F. Padovan 25640a708f8fSGustavo F. Padovan BT_DBG("type 0x%4.4x", type); 25650a708f8fSGustavo F. Padovan 25660a708f8fSGustavo F. Padovan if (type == L2CAP_IT_FEAT_MASK) { 25670a708f8fSGustavo F. Padovan u8 buf[8]; 25680a708f8fSGustavo F. Padovan u32 feat_mask = l2cap_feat_mask; 25690a708f8fSGustavo F. Padovan struct l2cap_info_rsp *rsp = (struct l2cap_info_rsp *) buf; 25700a708f8fSGustavo F. Padovan rsp->type = cpu_to_le16(L2CAP_IT_FEAT_MASK); 25710a708f8fSGustavo F. Padovan rsp->result = cpu_to_le16(L2CAP_IR_SUCCESS); 25720a708f8fSGustavo F. Padovan if (!disable_ertm) 25730a708f8fSGustavo F. Padovan feat_mask |= L2CAP_FEAT_ERTM | L2CAP_FEAT_STREAMING 25740a708f8fSGustavo F. Padovan | L2CAP_FEAT_FCS; 25750a708f8fSGustavo F. Padovan put_unaligned_le32(feat_mask, rsp->data); 25760a708f8fSGustavo F. Padovan l2cap_send_cmd(conn, cmd->ident, 25770a708f8fSGustavo F. Padovan L2CAP_INFO_RSP, sizeof(buf), buf); 25780a708f8fSGustavo F. Padovan } else if (type == L2CAP_IT_FIXED_CHAN) { 25790a708f8fSGustavo F. Padovan u8 buf[12]; 25800a708f8fSGustavo F. Padovan struct l2cap_info_rsp *rsp = (struct l2cap_info_rsp *) buf; 25810a708f8fSGustavo F. Padovan rsp->type = cpu_to_le16(L2CAP_IT_FIXED_CHAN); 25820a708f8fSGustavo F. Padovan rsp->result = cpu_to_le16(L2CAP_IR_SUCCESS); 25830a708f8fSGustavo F. Padovan memcpy(buf + 4, l2cap_fixed_chan, 8); 25840a708f8fSGustavo F. Padovan l2cap_send_cmd(conn, cmd->ident, 25850a708f8fSGustavo F. Padovan L2CAP_INFO_RSP, sizeof(buf), buf); 25860a708f8fSGustavo F. Padovan } else { 25870a708f8fSGustavo F. Padovan struct l2cap_info_rsp rsp; 25880a708f8fSGustavo F. Padovan rsp.type = cpu_to_le16(type); 25890a708f8fSGustavo F. Padovan rsp.result = cpu_to_le16(L2CAP_IR_NOTSUPP); 25900a708f8fSGustavo F. Padovan l2cap_send_cmd(conn, cmd->ident, 25910a708f8fSGustavo F. Padovan L2CAP_INFO_RSP, sizeof(rsp), &rsp); 25920a708f8fSGustavo F. Padovan } 25930a708f8fSGustavo F. Padovan 25940a708f8fSGustavo F. Padovan return 0; 25950a708f8fSGustavo F. Padovan } 25960a708f8fSGustavo F. Padovan 25970a708f8fSGustavo F. Padovan static inline int l2cap_information_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data) 25980a708f8fSGustavo F. Padovan { 25990a708f8fSGustavo F. Padovan struct l2cap_info_rsp *rsp = (struct l2cap_info_rsp *) data; 26000a708f8fSGustavo F. Padovan u16 type, result; 26010a708f8fSGustavo F. Padovan 26020a708f8fSGustavo F. Padovan type = __le16_to_cpu(rsp->type); 26030a708f8fSGustavo F. Padovan result = __le16_to_cpu(rsp->result); 26040a708f8fSGustavo F. Padovan 26050a708f8fSGustavo F. Padovan BT_DBG("type 0x%4.4x result 0x%2.2x", type, result); 26060a708f8fSGustavo F. Padovan 2607e90165beSAndrei Emeltchenko /* L2CAP Info req/rsp are unbound to channels, add extra checks */ 2608e90165beSAndrei Emeltchenko if (cmd->ident != conn->info_ident || 2609e90165beSAndrei Emeltchenko conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_DONE) 2610e90165beSAndrei Emeltchenko return 0; 2611e90165beSAndrei Emeltchenko 26120a708f8fSGustavo F. Padovan del_timer(&conn->info_timer); 26130a708f8fSGustavo F. Padovan 26140a708f8fSGustavo F. Padovan if (result != L2CAP_IR_SUCCESS) { 26150a708f8fSGustavo F. Padovan conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_DONE; 26160a708f8fSGustavo F. Padovan conn->info_ident = 0; 26170a708f8fSGustavo F. Padovan 26180a708f8fSGustavo F. Padovan l2cap_conn_start(conn); 26190a708f8fSGustavo F. Padovan 26200a708f8fSGustavo F. Padovan return 0; 26210a708f8fSGustavo F. Padovan } 26220a708f8fSGustavo F. Padovan 26230a708f8fSGustavo F. Padovan if (type == L2CAP_IT_FEAT_MASK) { 26240a708f8fSGustavo F. Padovan conn->feat_mask = get_unaligned_le32(rsp->data); 26250a708f8fSGustavo F. Padovan 26260a708f8fSGustavo F. Padovan if (conn->feat_mask & L2CAP_FEAT_FIXED_CHAN) { 26270a708f8fSGustavo F. Padovan struct l2cap_info_req req; 26280a708f8fSGustavo F. Padovan req.type = cpu_to_le16(L2CAP_IT_FIXED_CHAN); 26290a708f8fSGustavo F. Padovan 26300a708f8fSGustavo F. Padovan conn->info_ident = l2cap_get_ident(conn); 26310a708f8fSGustavo F. Padovan 26320a708f8fSGustavo F. Padovan l2cap_send_cmd(conn, conn->info_ident, 26330a708f8fSGustavo F. Padovan L2CAP_INFO_REQ, sizeof(req), &req); 26340a708f8fSGustavo F. Padovan } else { 26350a708f8fSGustavo F. Padovan conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_DONE; 26360a708f8fSGustavo F. Padovan conn->info_ident = 0; 26370a708f8fSGustavo F. Padovan 26380a708f8fSGustavo F. Padovan l2cap_conn_start(conn); 26390a708f8fSGustavo F. Padovan } 26400a708f8fSGustavo F. Padovan } else if (type == L2CAP_IT_FIXED_CHAN) { 26410a708f8fSGustavo F. Padovan conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_DONE; 26420a708f8fSGustavo F. Padovan conn->info_ident = 0; 26430a708f8fSGustavo F. Padovan 26440a708f8fSGustavo F. Padovan l2cap_conn_start(conn); 26450a708f8fSGustavo F. Padovan } 26460a708f8fSGustavo F. Padovan 26470a708f8fSGustavo F. Padovan return 0; 26480a708f8fSGustavo F. Padovan } 26490a708f8fSGustavo F. Padovan 2650e2174ca4SGustavo F. Padovan static inline int l2cap_check_conn_param(u16 min, u16 max, u16 latency, 2651de73115aSClaudio Takahasi u16 to_multiplier) 2652de73115aSClaudio Takahasi { 2653de73115aSClaudio Takahasi u16 max_latency; 2654de73115aSClaudio Takahasi 2655de73115aSClaudio Takahasi if (min > max || min < 6 || max > 3200) 2656de73115aSClaudio Takahasi return -EINVAL; 2657de73115aSClaudio Takahasi 2658de73115aSClaudio Takahasi if (to_multiplier < 10 || to_multiplier > 3200) 2659de73115aSClaudio Takahasi return -EINVAL; 2660de73115aSClaudio Takahasi 2661de73115aSClaudio Takahasi if (max >= to_multiplier * 8) 2662de73115aSClaudio Takahasi return -EINVAL; 2663de73115aSClaudio Takahasi 2664de73115aSClaudio Takahasi max_latency = (to_multiplier * 8 / max) - 1; 2665de73115aSClaudio Takahasi if (latency > 499 || latency > max_latency) 2666de73115aSClaudio Takahasi return -EINVAL; 2667de73115aSClaudio Takahasi 2668de73115aSClaudio Takahasi return 0; 2669de73115aSClaudio Takahasi } 2670de73115aSClaudio Takahasi 2671de73115aSClaudio Takahasi static inline int l2cap_conn_param_update_req(struct l2cap_conn *conn, 2672de73115aSClaudio Takahasi struct l2cap_cmd_hdr *cmd, u8 *data) 2673de73115aSClaudio Takahasi { 2674de73115aSClaudio Takahasi struct hci_conn *hcon = conn->hcon; 2675de73115aSClaudio Takahasi struct l2cap_conn_param_update_req *req; 2676de73115aSClaudio Takahasi struct l2cap_conn_param_update_rsp rsp; 2677de73115aSClaudio Takahasi u16 min, max, latency, to_multiplier, cmd_len; 26782ce603ebSClaudio Takahasi int err; 2679de73115aSClaudio Takahasi 2680de73115aSClaudio Takahasi if (!(hcon->link_mode & HCI_LM_MASTER)) 2681de73115aSClaudio Takahasi return -EINVAL; 2682de73115aSClaudio Takahasi 2683de73115aSClaudio Takahasi cmd_len = __le16_to_cpu(cmd->len); 2684de73115aSClaudio Takahasi if (cmd_len != sizeof(struct l2cap_conn_param_update_req)) 2685de73115aSClaudio Takahasi return -EPROTO; 2686de73115aSClaudio Takahasi 2687de73115aSClaudio Takahasi req = (struct l2cap_conn_param_update_req *) data; 2688de73115aSClaudio Takahasi min = __le16_to_cpu(req->min); 2689de73115aSClaudio Takahasi max = __le16_to_cpu(req->max); 2690de73115aSClaudio Takahasi latency = __le16_to_cpu(req->latency); 2691de73115aSClaudio Takahasi to_multiplier = __le16_to_cpu(req->to_multiplier); 2692de73115aSClaudio Takahasi 2693de73115aSClaudio Takahasi BT_DBG("min 0x%4.4x max 0x%4.4x latency: 0x%4.4x Timeout: 0x%4.4x", 2694de73115aSClaudio Takahasi min, max, latency, to_multiplier); 2695de73115aSClaudio Takahasi 2696de73115aSClaudio Takahasi memset(&rsp, 0, sizeof(rsp)); 26972ce603ebSClaudio Takahasi 26982ce603ebSClaudio Takahasi err = l2cap_check_conn_param(min, max, latency, to_multiplier); 26992ce603ebSClaudio Takahasi if (err) 2700de73115aSClaudio Takahasi rsp.result = cpu_to_le16(L2CAP_CONN_PARAM_REJECTED); 2701de73115aSClaudio Takahasi else 2702de73115aSClaudio Takahasi rsp.result = cpu_to_le16(L2CAP_CONN_PARAM_ACCEPTED); 2703de73115aSClaudio Takahasi 2704de73115aSClaudio Takahasi l2cap_send_cmd(conn, cmd->ident, L2CAP_CONN_PARAM_UPDATE_RSP, 2705de73115aSClaudio Takahasi sizeof(rsp), &rsp); 2706de73115aSClaudio Takahasi 27072ce603ebSClaudio Takahasi if (!err) 27082ce603ebSClaudio Takahasi hci_le_conn_update(hcon, min, max, latency, to_multiplier); 27092ce603ebSClaudio Takahasi 2710de73115aSClaudio Takahasi return 0; 2711de73115aSClaudio Takahasi } 2712de73115aSClaudio Takahasi 27133300d9a9SClaudio Takahasi static inline int l2cap_bredr_sig_cmd(struct l2cap_conn *conn, 27143300d9a9SClaudio Takahasi struct l2cap_cmd_hdr *cmd, u16 cmd_len, u8 *data) 27153300d9a9SClaudio Takahasi { 27163300d9a9SClaudio Takahasi int err = 0; 27173300d9a9SClaudio Takahasi 27183300d9a9SClaudio Takahasi switch (cmd->code) { 27193300d9a9SClaudio Takahasi case L2CAP_COMMAND_REJ: 27203300d9a9SClaudio Takahasi l2cap_command_rej(conn, cmd, data); 27213300d9a9SClaudio Takahasi break; 27223300d9a9SClaudio Takahasi 27233300d9a9SClaudio Takahasi case L2CAP_CONN_REQ: 27243300d9a9SClaudio Takahasi err = l2cap_connect_req(conn, cmd, data); 27253300d9a9SClaudio Takahasi break; 27263300d9a9SClaudio Takahasi 27273300d9a9SClaudio Takahasi case L2CAP_CONN_RSP: 27283300d9a9SClaudio Takahasi err = l2cap_connect_rsp(conn, cmd, data); 27293300d9a9SClaudio Takahasi break; 27303300d9a9SClaudio Takahasi 27313300d9a9SClaudio Takahasi case L2CAP_CONF_REQ: 27323300d9a9SClaudio Takahasi err = l2cap_config_req(conn, cmd, cmd_len, data); 27333300d9a9SClaudio Takahasi break; 27343300d9a9SClaudio Takahasi 27353300d9a9SClaudio Takahasi case L2CAP_CONF_RSP: 27363300d9a9SClaudio Takahasi err = l2cap_config_rsp(conn, cmd, data); 27373300d9a9SClaudio Takahasi break; 27383300d9a9SClaudio Takahasi 27393300d9a9SClaudio Takahasi case L2CAP_DISCONN_REQ: 27403300d9a9SClaudio Takahasi err = l2cap_disconnect_req(conn, cmd, data); 27413300d9a9SClaudio Takahasi break; 27423300d9a9SClaudio Takahasi 27433300d9a9SClaudio Takahasi case L2CAP_DISCONN_RSP: 27443300d9a9SClaudio Takahasi err = l2cap_disconnect_rsp(conn, cmd, data); 27453300d9a9SClaudio Takahasi break; 27463300d9a9SClaudio Takahasi 27473300d9a9SClaudio Takahasi case L2CAP_ECHO_REQ: 27483300d9a9SClaudio Takahasi l2cap_send_cmd(conn, cmd->ident, L2CAP_ECHO_RSP, cmd_len, data); 27493300d9a9SClaudio Takahasi break; 27503300d9a9SClaudio Takahasi 27513300d9a9SClaudio Takahasi case L2CAP_ECHO_RSP: 27523300d9a9SClaudio Takahasi break; 27533300d9a9SClaudio Takahasi 27543300d9a9SClaudio Takahasi case L2CAP_INFO_REQ: 27553300d9a9SClaudio Takahasi err = l2cap_information_req(conn, cmd, data); 27563300d9a9SClaudio Takahasi break; 27573300d9a9SClaudio Takahasi 27583300d9a9SClaudio Takahasi case L2CAP_INFO_RSP: 27593300d9a9SClaudio Takahasi err = l2cap_information_rsp(conn, cmd, data); 27603300d9a9SClaudio Takahasi break; 27613300d9a9SClaudio Takahasi 27623300d9a9SClaudio Takahasi default: 27633300d9a9SClaudio Takahasi BT_ERR("Unknown BR/EDR signaling command 0x%2.2x", cmd->code); 27643300d9a9SClaudio Takahasi err = -EINVAL; 27653300d9a9SClaudio Takahasi break; 27663300d9a9SClaudio Takahasi } 27673300d9a9SClaudio Takahasi 27683300d9a9SClaudio Takahasi return err; 27693300d9a9SClaudio Takahasi } 27703300d9a9SClaudio Takahasi 27713300d9a9SClaudio Takahasi static inline int l2cap_le_sig_cmd(struct l2cap_conn *conn, 27723300d9a9SClaudio Takahasi struct l2cap_cmd_hdr *cmd, u8 *data) 27733300d9a9SClaudio Takahasi { 27743300d9a9SClaudio Takahasi switch (cmd->code) { 27753300d9a9SClaudio Takahasi case L2CAP_COMMAND_REJ: 27763300d9a9SClaudio Takahasi return 0; 27773300d9a9SClaudio Takahasi 27783300d9a9SClaudio Takahasi case L2CAP_CONN_PARAM_UPDATE_REQ: 2779de73115aSClaudio Takahasi return l2cap_conn_param_update_req(conn, cmd, data); 27803300d9a9SClaudio Takahasi 27813300d9a9SClaudio Takahasi case L2CAP_CONN_PARAM_UPDATE_RSP: 27823300d9a9SClaudio Takahasi return 0; 27833300d9a9SClaudio Takahasi 27843300d9a9SClaudio Takahasi default: 27853300d9a9SClaudio Takahasi BT_ERR("Unknown LE signaling command 0x%2.2x", cmd->code); 27863300d9a9SClaudio Takahasi return -EINVAL; 27873300d9a9SClaudio Takahasi } 27883300d9a9SClaudio Takahasi } 27893300d9a9SClaudio Takahasi 27903300d9a9SClaudio Takahasi static inline void l2cap_sig_channel(struct l2cap_conn *conn, 27913300d9a9SClaudio Takahasi struct sk_buff *skb) 27920a708f8fSGustavo F. Padovan { 27930a708f8fSGustavo F. Padovan u8 *data = skb->data; 27940a708f8fSGustavo F. Padovan int len = skb->len; 27950a708f8fSGustavo F. Padovan struct l2cap_cmd_hdr cmd; 27963300d9a9SClaudio Takahasi int err; 27970a708f8fSGustavo F. Padovan 27980a708f8fSGustavo F. Padovan l2cap_raw_recv(conn, skb); 27990a708f8fSGustavo F. Padovan 28000a708f8fSGustavo F. Padovan while (len >= L2CAP_CMD_HDR_SIZE) { 28010a708f8fSGustavo F. Padovan u16 cmd_len; 28020a708f8fSGustavo F. Padovan memcpy(&cmd, data, L2CAP_CMD_HDR_SIZE); 28030a708f8fSGustavo F. Padovan data += L2CAP_CMD_HDR_SIZE; 28040a708f8fSGustavo F. Padovan len -= L2CAP_CMD_HDR_SIZE; 28050a708f8fSGustavo F. Padovan 28060a708f8fSGustavo F. Padovan cmd_len = le16_to_cpu(cmd.len); 28070a708f8fSGustavo F. Padovan 28080a708f8fSGustavo F. Padovan BT_DBG("code 0x%2.2x len %d id 0x%2.2x", cmd.code, cmd_len, cmd.ident); 28090a708f8fSGustavo F. Padovan 28100a708f8fSGustavo F. Padovan if (cmd_len > len || !cmd.ident) { 28110a708f8fSGustavo F. Padovan BT_DBG("corrupted command"); 28120a708f8fSGustavo F. Padovan break; 28130a708f8fSGustavo F. Padovan } 28140a708f8fSGustavo F. Padovan 28153300d9a9SClaudio Takahasi if (conn->hcon->type == LE_LINK) 28163300d9a9SClaudio Takahasi err = l2cap_le_sig_cmd(conn, &cmd, data); 28173300d9a9SClaudio Takahasi else 28183300d9a9SClaudio Takahasi err = l2cap_bredr_sig_cmd(conn, &cmd, cmd_len, data); 28190a708f8fSGustavo F. Padovan 28200a708f8fSGustavo F. Padovan if (err) { 28210a708f8fSGustavo F. Padovan struct l2cap_cmd_rej rej; 28222c6d1a2eSGustavo F. Padovan 28232c6d1a2eSGustavo F. Padovan BT_ERR("Wrong link type (%d)", err); 28240a708f8fSGustavo F. Padovan 28250a708f8fSGustavo F. Padovan /* FIXME: Map err to a valid reason */ 28260a708f8fSGustavo F. Padovan rej.reason = cpu_to_le16(0); 28270a708f8fSGustavo F. Padovan l2cap_send_cmd(conn, cmd.ident, L2CAP_COMMAND_REJ, sizeof(rej), &rej); 28280a708f8fSGustavo F. Padovan } 28290a708f8fSGustavo F. Padovan 28300a708f8fSGustavo F. Padovan data += cmd_len; 28310a708f8fSGustavo F. Padovan len -= cmd_len; 28320a708f8fSGustavo F. Padovan } 28330a708f8fSGustavo F. Padovan 28340a708f8fSGustavo F. Padovan kfree_skb(skb); 28350a708f8fSGustavo F. Padovan } 28360a708f8fSGustavo F. Padovan 283747d1ec61SGustavo F. Padovan static int l2cap_check_fcs(struct l2cap_chan *chan, struct sk_buff *skb) 28380a708f8fSGustavo F. Padovan { 28390a708f8fSGustavo F. Padovan u16 our_fcs, rcv_fcs; 28400a708f8fSGustavo F. Padovan int hdr_size = L2CAP_HDR_SIZE + 2; 28410a708f8fSGustavo F. Padovan 284247d1ec61SGustavo F. Padovan if (chan->fcs == L2CAP_FCS_CRC16) { 28430a708f8fSGustavo F. Padovan skb_trim(skb, skb->len - 2); 28440a708f8fSGustavo F. Padovan rcv_fcs = get_unaligned_le16(skb->data + skb->len); 28450a708f8fSGustavo F. Padovan our_fcs = crc16(0, skb->data - hdr_size, skb->len + hdr_size); 28460a708f8fSGustavo F. Padovan 28470a708f8fSGustavo F. Padovan if (our_fcs != rcv_fcs) 28480a708f8fSGustavo F. Padovan return -EBADMSG; 28490a708f8fSGustavo F. Padovan } 28500a708f8fSGustavo F. Padovan return 0; 28510a708f8fSGustavo F. Padovan } 28520a708f8fSGustavo F. Padovan 2853525cd185SGustavo F. Padovan static inline void l2cap_send_i_or_rr_or_rnr(struct l2cap_chan *chan) 28540a708f8fSGustavo F. Padovan { 28550a708f8fSGustavo F. Padovan u16 control = 0; 28560a708f8fSGustavo F. Padovan 28576a026610SGustavo F. Padovan chan->frames_sent = 0; 28580a708f8fSGustavo F. Padovan 285942e5c802SGustavo F. Padovan control |= chan->buffer_seq << L2CAP_CTRL_REQSEQ_SHIFT; 28600a708f8fSGustavo F. Padovan 2861525cd185SGustavo F. Padovan if (chan->conn_state & L2CAP_CONN_LOCAL_BUSY) { 28620a708f8fSGustavo F. Padovan control |= L2CAP_SUPER_RCV_NOT_READY; 2863525cd185SGustavo F. Padovan l2cap_send_sframe(chan, control); 2864525cd185SGustavo F. Padovan chan->conn_state |= L2CAP_CONN_RNR_SENT; 28650a708f8fSGustavo F. Padovan } 28660a708f8fSGustavo F. Padovan 2867525cd185SGustavo F. Padovan if (chan->conn_state & L2CAP_CONN_REMOTE_BUSY) 2868525cd185SGustavo F. Padovan l2cap_retransmit_frames(chan); 28690a708f8fSGustavo F. Padovan 2870525cd185SGustavo F. Padovan l2cap_ertm_send(chan); 28710a708f8fSGustavo F. Padovan 2872525cd185SGustavo F. Padovan if (!(chan->conn_state & L2CAP_CONN_LOCAL_BUSY) && 28736a026610SGustavo F. Padovan chan->frames_sent == 0) { 28740a708f8fSGustavo F. Padovan control |= L2CAP_SUPER_RCV_READY; 2875525cd185SGustavo F. Padovan l2cap_send_sframe(chan, control); 28760a708f8fSGustavo F. Padovan } 28770a708f8fSGustavo F. Padovan } 28780a708f8fSGustavo F. Padovan 287942e5c802SGustavo F. Padovan static int l2cap_add_to_srej_queue(struct l2cap_chan *chan, struct sk_buff *skb, u8 tx_seq, u8 sar) 28800a708f8fSGustavo F. Padovan { 28810a708f8fSGustavo F. Padovan struct sk_buff *next_skb; 28820a708f8fSGustavo F. Padovan int tx_seq_offset, next_tx_seq_offset; 28830a708f8fSGustavo F. Padovan 28840a708f8fSGustavo F. Padovan bt_cb(skb)->tx_seq = tx_seq; 28850a708f8fSGustavo F. Padovan bt_cb(skb)->sar = sar; 28860a708f8fSGustavo F. Padovan 2887f1c6775bSGustavo F. Padovan next_skb = skb_peek(&chan->srej_q); 28880a708f8fSGustavo F. Padovan if (!next_skb) { 2889f1c6775bSGustavo F. Padovan __skb_queue_tail(&chan->srej_q, skb); 28900a708f8fSGustavo F. Padovan return 0; 28910a708f8fSGustavo F. Padovan } 28920a708f8fSGustavo F. Padovan 289342e5c802SGustavo F. Padovan tx_seq_offset = (tx_seq - chan->buffer_seq) % 64; 28940a708f8fSGustavo F. Padovan if (tx_seq_offset < 0) 28950a708f8fSGustavo F. Padovan tx_seq_offset += 64; 28960a708f8fSGustavo F. Padovan 28970a708f8fSGustavo F. Padovan do { 28980a708f8fSGustavo F. Padovan if (bt_cb(next_skb)->tx_seq == tx_seq) 28990a708f8fSGustavo F. Padovan return -EINVAL; 29000a708f8fSGustavo F. Padovan 29010a708f8fSGustavo F. Padovan next_tx_seq_offset = (bt_cb(next_skb)->tx_seq - 290242e5c802SGustavo F. Padovan chan->buffer_seq) % 64; 29030a708f8fSGustavo F. Padovan if (next_tx_seq_offset < 0) 29040a708f8fSGustavo F. Padovan next_tx_seq_offset += 64; 29050a708f8fSGustavo F. Padovan 29060a708f8fSGustavo F. Padovan if (next_tx_seq_offset > tx_seq_offset) { 2907f1c6775bSGustavo F. Padovan __skb_queue_before(&chan->srej_q, next_skb, skb); 29080a708f8fSGustavo F. Padovan return 0; 29090a708f8fSGustavo F. Padovan } 29100a708f8fSGustavo F. Padovan 2911f1c6775bSGustavo F. Padovan if (skb_queue_is_last(&chan->srej_q, next_skb)) 29120a708f8fSGustavo F. Padovan break; 29130a708f8fSGustavo F. Padovan 2914f1c6775bSGustavo F. Padovan } while ((next_skb = skb_queue_next(&chan->srej_q, next_skb))); 29150a708f8fSGustavo F. Padovan 2916f1c6775bSGustavo F. Padovan __skb_queue_tail(&chan->srej_q, skb); 29170a708f8fSGustavo F. Padovan 29180a708f8fSGustavo F. Padovan return 0; 29190a708f8fSGustavo F. Padovan } 29200a708f8fSGustavo F. Padovan 2921525cd185SGustavo F. Padovan static int l2cap_ertm_reassembly_sdu(struct l2cap_chan *chan, struct sk_buff *skb, u16 control) 29220a708f8fSGustavo F. Padovan { 29230a708f8fSGustavo F. Padovan struct sk_buff *_skb; 29240a708f8fSGustavo F. Padovan int err; 29250a708f8fSGustavo F. Padovan 29260a708f8fSGustavo F. Padovan switch (control & L2CAP_CTRL_SAR) { 29270a708f8fSGustavo F. Padovan case L2CAP_SDU_UNSEGMENTED: 2928525cd185SGustavo F. Padovan if (chan->conn_state & L2CAP_CONN_SAR_SDU) 29290a708f8fSGustavo F. Padovan goto drop; 29300a708f8fSGustavo F. Padovan 2931525cd185SGustavo F. Padovan err = sock_queue_rcv_skb(chan->sk, skb); 29320a708f8fSGustavo F. Padovan if (!err) 29330a708f8fSGustavo F. Padovan return err; 29340a708f8fSGustavo F. Padovan 29350a708f8fSGustavo F. Padovan break; 29360a708f8fSGustavo F. Padovan 29370a708f8fSGustavo F. Padovan case L2CAP_SDU_START: 2938525cd185SGustavo F. Padovan if (chan->conn_state & L2CAP_CONN_SAR_SDU) 29390a708f8fSGustavo F. Padovan goto drop; 29400a708f8fSGustavo F. Padovan 29416f61fd47SGustavo F. Padovan chan->sdu_len = get_unaligned_le16(skb->data); 29420a708f8fSGustavo F. Padovan 29430c1bc5c6SGustavo F. Padovan if (chan->sdu_len > chan->imtu) 29440a708f8fSGustavo F. Padovan goto disconnect; 29450a708f8fSGustavo F. Padovan 29466f61fd47SGustavo F. Padovan chan->sdu = bt_skb_alloc(chan->sdu_len, GFP_ATOMIC); 29476f61fd47SGustavo F. Padovan if (!chan->sdu) 29480a708f8fSGustavo F. Padovan return -ENOMEM; 29490a708f8fSGustavo F. Padovan 29500a708f8fSGustavo F. Padovan /* pull sdu_len bytes only after alloc, because of Local Busy 29510a708f8fSGustavo F. Padovan * condition we have to be sure that this will be executed 29520a708f8fSGustavo F. Padovan * only once, i.e., when alloc does not fail */ 29530a708f8fSGustavo F. Padovan skb_pull(skb, 2); 29540a708f8fSGustavo F. Padovan 29556f61fd47SGustavo F. Padovan memcpy(skb_put(chan->sdu, skb->len), skb->data, skb->len); 29560a708f8fSGustavo F. Padovan 2957525cd185SGustavo F. Padovan chan->conn_state |= L2CAP_CONN_SAR_SDU; 29586f61fd47SGustavo F. Padovan chan->partial_sdu_len = skb->len; 29590a708f8fSGustavo F. Padovan break; 29600a708f8fSGustavo F. Padovan 29610a708f8fSGustavo F. Padovan case L2CAP_SDU_CONTINUE: 2962525cd185SGustavo F. Padovan if (!(chan->conn_state & L2CAP_CONN_SAR_SDU)) 29630a708f8fSGustavo F. Padovan goto disconnect; 29640a708f8fSGustavo F. Padovan 29656f61fd47SGustavo F. Padovan if (!chan->sdu) 29660a708f8fSGustavo F. Padovan goto disconnect; 29670a708f8fSGustavo F. Padovan 29686f61fd47SGustavo F. Padovan chan->partial_sdu_len += skb->len; 29696f61fd47SGustavo F. Padovan if (chan->partial_sdu_len > chan->sdu_len) 29700a708f8fSGustavo F. Padovan goto drop; 29710a708f8fSGustavo F. Padovan 29726f61fd47SGustavo F. Padovan memcpy(skb_put(chan->sdu, skb->len), skb->data, skb->len); 29730a708f8fSGustavo F. Padovan 29740a708f8fSGustavo F. Padovan break; 29750a708f8fSGustavo F. Padovan 29760a708f8fSGustavo F. Padovan case L2CAP_SDU_END: 2977525cd185SGustavo F. Padovan if (!(chan->conn_state & L2CAP_CONN_SAR_SDU)) 29780a708f8fSGustavo F. Padovan goto disconnect; 29790a708f8fSGustavo F. Padovan 29806f61fd47SGustavo F. Padovan if (!chan->sdu) 29810a708f8fSGustavo F. Padovan goto disconnect; 29820a708f8fSGustavo F. Padovan 2983525cd185SGustavo F. Padovan if (!(chan->conn_state & L2CAP_CONN_SAR_RETRY)) { 29846f61fd47SGustavo F. Padovan chan->partial_sdu_len += skb->len; 29850a708f8fSGustavo F. Padovan 29860c1bc5c6SGustavo F. Padovan if (chan->partial_sdu_len > chan->imtu) 29870a708f8fSGustavo F. Padovan goto drop; 29880a708f8fSGustavo F. Padovan 29896f61fd47SGustavo F. Padovan if (chan->partial_sdu_len != chan->sdu_len) 29900a708f8fSGustavo F. Padovan goto drop; 29910a708f8fSGustavo F. Padovan 29926f61fd47SGustavo F. Padovan memcpy(skb_put(chan->sdu, skb->len), skb->data, skb->len); 29930a708f8fSGustavo F. Padovan } 29940a708f8fSGustavo F. Padovan 29956f61fd47SGustavo F. Padovan _skb = skb_clone(chan->sdu, GFP_ATOMIC); 29960a708f8fSGustavo F. Padovan if (!_skb) { 2997525cd185SGustavo F. Padovan chan->conn_state |= L2CAP_CONN_SAR_RETRY; 29980a708f8fSGustavo F. Padovan return -ENOMEM; 29990a708f8fSGustavo F. Padovan } 30000a708f8fSGustavo F. Padovan 3001525cd185SGustavo F. Padovan err = sock_queue_rcv_skb(chan->sk, _skb); 30020a708f8fSGustavo F. Padovan if (err < 0) { 30030a708f8fSGustavo F. Padovan kfree_skb(_skb); 3004525cd185SGustavo F. Padovan chan->conn_state |= L2CAP_CONN_SAR_RETRY; 30050a708f8fSGustavo F. Padovan return err; 30060a708f8fSGustavo F. Padovan } 30070a708f8fSGustavo F. Padovan 3008525cd185SGustavo F. Padovan chan->conn_state &= ~L2CAP_CONN_SAR_RETRY; 3009525cd185SGustavo F. Padovan chan->conn_state &= ~L2CAP_CONN_SAR_SDU; 30100a708f8fSGustavo F. Padovan 30116f61fd47SGustavo F. Padovan kfree_skb(chan->sdu); 30120a708f8fSGustavo F. Padovan break; 30130a708f8fSGustavo F. Padovan } 30140a708f8fSGustavo F. Padovan 30150a708f8fSGustavo F. Padovan kfree_skb(skb); 30160a708f8fSGustavo F. Padovan return 0; 30170a708f8fSGustavo F. Padovan 30180a708f8fSGustavo F. Padovan drop: 30196f61fd47SGustavo F. Padovan kfree_skb(chan->sdu); 30206f61fd47SGustavo F. Padovan chan->sdu = NULL; 30210a708f8fSGustavo F. Padovan 30220a708f8fSGustavo F. Padovan disconnect: 30238c1d787bSGustavo F. Padovan l2cap_send_disconn_req(chan->conn, chan, ECONNRESET); 30240a708f8fSGustavo F. Padovan kfree_skb(skb); 30250a708f8fSGustavo F. Padovan return 0; 30260a708f8fSGustavo F. Padovan } 30270a708f8fSGustavo F. Padovan 3028525cd185SGustavo F. Padovan static int l2cap_try_push_rx_skb(struct l2cap_chan *chan) 30290a708f8fSGustavo F. Padovan { 30300a708f8fSGustavo F. Padovan struct sk_buff *skb; 30310a708f8fSGustavo F. Padovan u16 control; 30320a708f8fSGustavo F. Padovan int err; 30330a708f8fSGustavo F. Padovan 3034f1c6775bSGustavo F. Padovan while ((skb = skb_dequeue(&chan->busy_q))) { 30350a708f8fSGustavo F. Padovan control = bt_cb(skb)->sar << L2CAP_CTRL_SAR_SHIFT; 3036525cd185SGustavo F. Padovan err = l2cap_ertm_reassembly_sdu(chan, skb, control); 30370a708f8fSGustavo F. Padovan if (err < 0) { 3038f1c6775bSGustavo F. Padovan skb_queue_head(&chan->busy_q, skb); 30390a708f8fSGustavo F. Padovan return -EBUSY; 30400a708f8fSGustavo F. Padovan } 30410a708f8fSGustavo F. Padovan 304242e5c802SGustavo F. Padovan chan->buffer_seq = (chan->buffer_seq + 1) % 64; 30430a708f8fSGustavo F. Padovan } 30440a708f8fSGustavo F. Padovan 3045525cd185SGustavo F. Padovan if (!(chan->conn_state & L2CAP_CONN_RNR_SENT)) 30460a708f8fSGustavo F. Padovan goto done; 30470a708f8fSGustavo F. Padovan 304842e5c802SGustavo F. Padovan control = chan->buffer_seq << L2CAP_CTRL_REQSEQ_SHIFT; 30490a708f8fSGustavo F. Padovan control |= L2CAP_SUPER_RCV_READY | L2CAP_CTRL_POLL; 3050525cd185SGustavo F. Padovan l2cap_send_sframe(chan, control); 30516a026610SGustavo F. Padovan chan->retry_count = 1; 30520a708f8fSGustavo F. Padovan 3053e92c8e70SGustavo F. Padovan del_timer(&chan->retrans_timer); 30540a708f8fSGustavo F. Padovan __mod_monitor_timer(); 30550a708f8fSGustavo F. Padovan 3056525cd185SGustavo F. Padovan chan->conn_state |= L2CAP_CONN_WAIT_F; 30570a708f8fSGustavo F. Padovan 30580a708f8fSGustavo F. Padovan done: 3059525cd185SGustavo F. Padovan chan->conn_state &= ~L2CAP_CONN_LOCAL_BUSY; 3060525cd185SGustavo F. Padovan chan->conn_state &= ~L2CAP_CONN_RNR_SENT; 30610a708f8fSGustavo F. Padovan 306249208c9cSGustavo F. Padovan BT_DBG("chan %p, Exit local busy", chan); 30630a708f8fSGustavo F. Padovan 30640a708f8fSGustavo F. Padovan return 0; 30650a708f8fSGustavo F. Padovan } 30660a708f8fSGustavo F. Padovan 30670a708f8fSGustavo F. Padovan static void l2cap_busy_work(struct work_struct *work) 30680a708f8fSGustavo F. Padovan { 30690a708f8fSGustavo F. Padovan DECLARE_WAITQUEUE(wait, current); 3070311bb895SGustavo F. Padovan struct l2cap_chan *chan = 3071311bb895SGustavo F. Padovan container_of(work, struct l2cap_chan, busy_work); 3072311bb895SGustavo F. Padovan struct sock *sk = chan->sk; 30730a708f8fSGustavo F. Padovan int n_tries = 0, timeo = HZ/5, err; 30740a708f8fSGustavo F. Padovan struct sk_buff *skb; 30750a708f8fSGustavo F. Padovan 30760a708f8fSGustavo F. Padovan lock_sock(sk); 30770a708f8fSGustavo F. Padovan 30780a708f8fSGustavo F. Padovan add_wait_queue(sk_sleep(sk), &wait); 3079311bb895SGustavo F. Padovan while ((skb = skb_peek(&chan->busy_q))) { 30800a708f8fSGustavo F. Padovan set_current_state(TASK_INTERRUPTIBLE); 30810a708f8fSGustavo F. Padovan 30820a708f8fSGustavo F. Padovan if (n_tries++ > L2CAP_LOCAL_BUSY_TRIES) { 30830a708f8fSGustavo F. Padovan err = -EBUSY; 30848c1d787bSGustavo F. Padovan l2cap_send_disconn_req(chan->conn, chan, EBUSY); 30850a708f8fSGustavo F. Padovan break; 30860a708f8fSGustavo F. Padovan } 30870a708f8fSGustavo F. Padovan 30880a708f8fSGustavo F. Padovan if (!timeo) 30890a708f8fSGustavo F. Padovan timeo = HZ/5; 30900a708f8fSGustavo F. Padovan 30910a708f8fSGustavo F. Padovan if (signal_pending(current)) { 30920a708f8fSGustavo F. Padovan err = sock_intr_errno(timeo); 30930a708f8fSGustavo F. Padovan break; 30940a708f8fSGustavo F. Padovan } 30950a708f8fSGustavo F. Padovan 30960a708f8fSGustavo F. Padovan release_sock(sk); 30970a708f8fSGustavo F. Padovan timeo = schedule_timeout(timeo); 30980a708f8fSGustavo F. Padovan lock_sock(sk); 30990a708f8fSGustavo F. Padovan 31000a708f8fSGustavo F. Padovan err = sock_error(sk); 31010a708f8fSGustavo F. Padovan if (err) 31020a708f8fSGustavo F. Padovan break; 31030a708f8fSGustavo F. Padovan 3104311bb895SGustavo F. Padovan if (l2cap_try_push_rx_skb(chan) == 0) 31050a708f8fSGustavo F. Padovan break; 31060a708f8fSGustavo F. Padovan } 31070a708f8fSGustavo F. Padovan 31080a708f8fSGustavo F. Padovan set_current_state(TASK_RUNNING); 31090a708f8fSGustavo F. Padovan remove_wait_queue(sk_sleep(sk), &wait); 31100a708f8fSGustavo F. Padovan 31110a708f8fSGustavo F. Padovan release_sock(sk); 31120a708f8fSGustavo F. Padovan } 31130a708f8fSGustavo F. Padovan 3114525cd185SGustavo F. Padovan static int l2cap_push_rx_skb(struct l2cap_chan *chan, struct sk_buff *skb, u16 control) 31150a708f8fSGustavo F. Padovan { 31160a708f8fSGustavo F. Padovan int sctrl, err; 31170a708f8fSGustavo F. Padovan 3118525cd185SGustavo F. Padovan if (chan->conn_state & L2CAP_CONN_LOCAL_BUSY) { 31190a708f8fSGustavo F. Padovan bt_cb(skb)->sar = control >> L2CAP_CTRL_SAR_SHIFT; 3120f1c6775bSGustavo F. Padovan __skb_queue_tail(&chan->busy_q, skb); 3121525cd185SGustavo F. Padovan return l2cap_try_push_rx_skb(chan); 31220a708f8fSGustavo F. Padovan 31230a708f8fSGustavo F. Padovan 31240a708f8fSGustavo F. Padovan } 31250a708f8fSGustavo F. Padovan 3126525cd185SGustavo F. Padovan err = l2cap_ertm_reassembly_sdu(chan, skb, control); 31270a708f8fSGustavo F. Padovan if (err >= 0) { 312842e5c802SGustavo F. Padovan chan->buffer_seq = (chan->buffer_seq + 1) % 64; 31290a708f8fSGustavo F. Padovan return err; 31300a708f8fSGustavo F. Padovan } 31310a708f8fSGustavo F. Padovan 31320a708f8fSGustavo F. Padovan /* Busy Condition */ 3133311bb895SGustavo F. Padovan BT_DBG("chan %p, Enter local busy", chan); 31340a708f8fSGustavo F. Padovan 3135525cd185SGustavo F. Padovan chan->conn_state |= L2CAP_CONN_LOCAL_BUSY; 31360a708f8fSGustavo F. Padovan bt_cb(skb)->sar = control >> L2CAP_CTRL_SAR_SHIFT; 3137f1c6775bSGustavo F. Padovan __skb_queue_tail(&chan->busy_q, skb); 31380a708f8fSGustavo F. Padovan 313942e5c802SGustavo F. Padovan sctrl = chan->buffer_seq << L2CAP_CTRL_REQSEQ_SHIFT; 31400a708f8fSGustavo F. Padovan sctrl |= L2CAP_SUPER_RCV_NOT_READY; 3141525cd185SGustavo F. Padovan l2cap_send_sframe(chan, sctrl); 31420a708f8fSGustavo F. Padovan 3143525cd185SGustavo F. Padovan chan->conn_state |= L2CAP_CONN_RNR_SENT; 31440a708f8fSGustavo F. Padovan 3145e92c8e70SGustavo F. Padovan del_timer(&chan->ack_timer); 31460a708f8fSGustavo F. Padovan 3147311bb895SGustavo F. Padovan queue_work(_busy_wq, &chan->busy_work); 31480a708f8fSGustavo F. Padovan 31490a708f8fSGustavo F. Padovan return err; 31500a708f8fSGustavo F. Padovan } 31510a708f8fSGustavo F. Padovan 3152525cd185SGustavo F. Padovan static int l2cap_streaming_reassembly_sdu(struct l2cap_chan *chan, struct sk_buff *skb, u16 control) 31530a708f8fSGustavo F. Padovan { 31540a708f8fSGustavo F. Padovan struct sk_buff *_skb; 31550a708f8fSGustavo F. Padovan int err = -EINVAL; 31560a708f8fSGustavo F. Padovan 31570a708f8fSGustavo F. Padovan /* 31580a708f8fSGustavo F. Padovan * TODO: We have to notify the userland if some data is lost with the 31590a708f8fSGustavo F. Padovan * Streaming Mode. 31600a708f8fSGustavo F. Padovan */ 31610a708f8fSGustavo F. Padovan 31620a708f8fSGustavo F. Padovan switch (control & L2CAP_CTRL_SAR) { 31630a708f8fSGustavo F. Padovan case L2CAP_SDU_UNSEGMENTED: 3164525cd185SGustavo F. Padovan if (chan->conn_state & L2CAP_CONN_SAR_SDU) { 31656f61fd47SGustavo F. Padovan kfree_skb(chan->sdu); 31660a708f8fSGustavo F. Padovan break; 31670a708f8fSGustavo F. Padovan } 31680a708f8fSGustavo F. Padovan 3169525cd185SGustavo F. Padovan err = sock_queue_rcv_skb(chan->sk, skb); 31700a708f8fSGustavo F. Padovan if (!err) 31710a708f8fSGustavo F. Padovan return 0; 31720a708f8fSGustavo F. Padovan 31730a708f8fSGustavo F. Padovan break; 31740a708f8fSGustavo F. Padovan 31750a708f8fSGustavo F. Padovan case L2CAP_SDU_START: 3176525cd185SGustavo F. Padovan if (chan->conn_state & L2CAP_CONN_SAR_SDU) { 31776f61fd47SGustavo F. Padovan kfree_skb(chan->sdu); 31780a708f8fSGustavo F. Padovan break; 31790a708f8fSGustavo F. Padovan } 31800a708f8fSGustavo F. Padovan 31816f61fd47SGustavo F. Padovan chan->sdu_len = get_unaligned_le16(skb->data); 31820a708f8fSGustavo F. Padovan skb_pull(skb, 2); 31830a708f8fSGustavo F. Padovan 31840c1bc5c6SGustavo F. Padovan if (chan->sdu_len > chan->imtu) { 31850a708f8fSGustavo F. Padovan err = -EMSGSIZE; 31860a708f8fSGustavo F. Padovan break; 31870a708f8fSGustavo F. Padovan } 31880a708f8fSGustavo F. Padovan 31896f61fd47SGustavo F. Padovan chan->sdu = bt_skb_alloc(chan->sdu_len, GFP_ATOMIC); 31906f61fd47SGustavo F. Padovan if (!chan->sdu) { 31910a708f8fSGustavo F. Padovan err = -ENOMEM; 31920a708f8fSGustavo F. Padovan break; 31930a708f8fSGustavo F. Padovan } 31940a708f8fSGustavo F. Padovan 31956f61fd47SGustavo F. Padovan memcpy(skb_put(chan->sdu, skb->len), skb->data, skb->len); 31960a708f8fSGustavo F. Padovan 3197525cd185SGustavo F. Padovan chan->conn_state |= L2CAP_CONN_SAR_SDU; 31986f61fd47SGustavo F. Padovan chan->partial_sdu_len = skb->len; 31990a708f8fSGustavo F. Padovan err = 0; 32000a708f8fSGustavo F. Padovan break; 32010a708f8fSGustavo F. Padovan 32020a708f8fSGustavo F. Padovan case L2CAP_SDU_CONTINUE: 3203525cd185SGustavo F. Padovan if (!(chan->conn_state & L2CAP_CONN_SAR_SDU)) 32040a708f8fSGustavo F. Padovan break; 32050a708f8fSGustavo F. Padovan 32066f61fd47SGustavo F. Padovan memcpy(skb_put(chan->sdu, skb->len), skb->data, skb->len); 32070a708f8fSGustavo F. Padovan 32086f61fd47SGustavo F. Padovan chan->partial_sdu_len += skb->len; 32096f61fd47SGustavo F. Padovan if (chan->partial_sdu_len > chan->sdu_len) 32106f61fd47SGustavo F. Padovan kfree_skb(chan->sdu); 32110a708f8fSGustavo F. Padovan else 32120a708f8fSGustavo F. Padovan err = 0; 32130a708f8fSGustavo F. Padovan 32140a708f8fSGustavo F. Padovan break; 32150a708f8fSGustavo F. Padovan 32160a708f8fSGustavo F. Padovan case L2CAP_SDU_END: 3217525cd185SGustavo F. Padovan if (!(chan->conn_state & L2CAP_CONN_SAR_SDU)) 32180a708f8fSGustavo F. Padovan break; 32190a708f8fSGustavo F. Padovan 32206f61fd47SGustavo F. Padovan memcpy(skb_put(chan->sdu, skb->len), skb->data, skb->len); 32210a708f8fSGustavo F. Padovan 3222525cd185SGustavo F. Padovan chan->conn_state &= ~L2CAP_CONN_SAR_SDU; 32236f61fd47SGustavo F. Padovan chan->partial_sdu_len += skb->len; 32240a708f8fSGustavo F. Padovan 32250c1bc5c6SGustavo F. Padovan if (chan->partial_sdu_len > chan->imtu) 32260a708f8fSGustavo F. Padovan goto drop; 32270a708f8fSGustavo F. Padovan 32286f61fd47SGustavo F. Padovan if (chan->partial_sdu_len == chan->sdu_len) { 32296f61fd47SGustavo F. Padovan _skb = skb_clone(chan->sdu, GFP_ATOMIC); 3230525cd185SGustavo F. Padovan err = sock_queue_rcv_skb(chan->sk, _skb); 32310a708f8fSGustavo F. Padovan if (err < 0) 32320a708f8fSGustavo F. Padovan kfree_skb(_skb); 32330a708f8fSGustavo F. Padovan } 32340a708f8fSGustavo F. Padovan err = 0; 32350a708f8fSGustavo F. Padovan 32360a708f8fSGustavo F. Padovan drop: 32376f61fd47SGustavo F. Padovan kfree_skb(chan->sdu); 32380a708f8fSGustavo F. Padovan break; 32390a708f8fSGustavo F. Padovan } 32400a708f8fSGustavo F. Padovan 32410a708f8fSGustavo F. Padovan kfree_skb(skb); 32420a708f8fSGustavo F. Padovan return err; 32430a708f8fSGustavo F. Padovan } 32440a708f8fSGustavo F. Padovan 3245525cd185SGustavo F. Padovan static void l2cap_check_srej_gap(struct l2cap_chan *chan, u8 tx_seq) 32460a708f8fSGustavo F. Padovan { 32470a708f8fSGustavo F. Padovan struct sk_buff *skb; 32480a708f8fSGustavo F. Padovan u16 control; 32490a708f8fSGustavo F. Padovan 3250f1c6775bSGustavo F. Padovan while ((skb = skb_peek(&chan->srej_q))) { 32510a708f8fSGustavo F. Padovan if (bt_cb(skb)->tx_seq != tx_seq) 32520a708f8fSGustavo F. Padovan break; 32530a708f8fSGustavo F. Padovan 3254f1c6775bSGustavo F. Padovan skb = skb_dequeue(&chan->srej_q); 32550a708f8fSGustavo F. Padovan control = bt_cb(skb)->sar << L2CAP_CTRL_SAR_SHIFT; 3256525cd185SGustavo F. Padovan l2cap_ertm_reassembly_sdu(chan, skb, control); 325742e5c802SGustavo F. Padovan chan->buffer_seq_srej = 325842e5c802SGustavo F. Padovan (chan->buffer_seq_srej + 1) % 64; 32590a708f8fSGustavo F. Padovan tx_seq = (tx_seq + 1) % 64; 32600a708f8fSGustavo F. Padovan } 32610a708f8fSGustavo F. Padovan } 32620a708f8fSGustavo F. Padovan 3263525cd185SGustavo F. Padovan static void l2cap_resend_srejframe(struct l2cap_chan *chan, u8 tx_seq) 32640a708f8fSGustavo F. Padovan { 32650a708f8fSGustavo F. Padovan struct srej_list *l, *tmp; 32660a708f8fSGustavo F. Padovan u16 control; 32670a708f8fSGustavo F. Padovan 326839d5a3eeSGustavo F. Padovan list_for_each_entry_safe(l, tmp, &chan->srej_l, list) { 32690a708f8fSGustavo F. Padovan if (l->tx_seq == tx_seq) { 32700a708f8fSGustavo F. Padovan list_del(&l->list); 32710a708f8fSGustavo F. Padovan kfree(l); 32720a708f8fSGustavo F. Padovan return; 32730a708f8fSGustavo F. Padovan } 32740a708f8fSGustavo F. Padovan control = L2CAP_SUPER_SELECT_REJECT; 32750a708f8fSGustavo F. Padovan control |= l->tx_seq << L2CAP_CTRL_REQSEQ_SHIFT; 3276525cd185SGustavo F. Padovan l2cap_send_sframe(chan, control); 32770a708f8fSGustavo F. Padovan list_del(&l->list); 327839d5a3eeSGustavo F. Padovan list_add_tail(&l->list, &chan->srej_l); 32790a708f8fSGustavo F. Padovan } 32800a708f8fSGustavo F. Padovan } 32810a708f8fSGustavo F. Padovan 3282525cd185SGustavo F. Padovan static void l2cap_send_srejframe(struct l2cap_chan *chan, u8 tx_seq) 32830a708f8fSGustavo F. Padovan { 32840a708f8fSGustavo F. Padovan struct srej_list *new; 32850a708f8fSGustavo F. Padovan u16 control; 32860a708f8fSGustavo F. Padovan 328742e5c802SGustavo F. Padovan while (tx_seq != chan->expected_tx_seq) { 32880a708f8fSGustavo F. Padovan control = L2CAP_SUPER_SELECT_REJECT; 328942e5c802SGustavo F. Padovan control |= chan->expected_tx_seq << L2CAP_CTRL_REQSEQ_SHIFT; 3290525cd185SGustavo F. Padovan l2cap_send_sframe(chan, control); 32910a708f8fSGustavo F. Padovan 32920a708f8fSGustavo F. Padovan new = kzalloc(sizeof(struct srej_list), GFP_ATOMIC); 329342e5c802SGustavo F. Padovan new->tx_seq = chan->expected_tx_seq; 329442e5c802SGustavo F. Padovan chan->expected_tx_seq = (chan->expected_tx_seq + 1) % 64; 329539d5a3eeSGustavo F. Padovan list_add_tail(&new->list, &chan->srej_l); 32960a708f8fSGustavo F. Padovan } 329742e5c802SGustavo F. Padovan chan->expected_tx_seq = (chan->expected_tx_seq + 1) % 64; 32980a708f8fSGustavo F. Padovan } 32990a708f8fSGustavo F. Padovan 3300525cd185SGustavo F. Padovan static inline int l2cap_data_channel_iframe(struct l2cap_chan *chan, u16 rx_control, struct sk_buff *skb) 33010a708f8fSGustavo F. Padovan { 33020a708f8fSGustavo F. Padovan u8 tx_seq = __get_txseq(rx_control); 33030a708f8fSGustavo F. Padovan u8 req_seq = __get_reqseq(rx_control); 33040a708f8fSGustavo F. Padovan u8 sar = rx_control >> L2CAP_CTRL_SAR_SHIFT; 33050a708f8fSGustavo F. Padovan int tx_seq_offset, expected_tx_seq_offset; 330647d1ec61SGustavo F. Padovan int num_to_ack = (chan->tx_win/6) + 1; 33070a708f8fSGustavo F. Padovan int err = 0; 33080a708f8fSGustavo F. Padovan 3309525cd185SGustavo F. Padovan BT_DBG("chan %p len %d tx_seq %d rx_control 0x%4.4x", chan, skb->len, 3310525cd185SGustavo F. Padovan tx_seq, rx_control); 33110a708f8fSGustavo F. Padovan 33120a708f8fSGustavo F. Padovan if (L2CAP_CTRL_FINAL & rx_control && 3313525cd185SGustavo F. Padovan chan->conn_state & L2CAP_CONN_WAIT_F) { 3314e92c8e70SGustavo F. Padovan del_timer(&chan->monitor_timer); 33156a026610SGustavo F. Padovan if (chan->unacked_frames > 0) 33160a708f8fSGustavo F. Padovan __mod_retrans_timer(); 3317525cd185SGustavo F. Padovan chan->conn_state &= ~L2CAP_CONN_WAIT_F; 33180a708f8fSGustavo F. Padovan } 33190a708f8fSGustavo F. Padovan 332042e5c802SGustavo F. Padovan chan->expected_ack_seq = req_seq; 332142e5c802SGustavo F. Padovan l2cap_drop_acked_frames(chan); 33220a708f8fSGustavo F. Padovan 332342e5c802SGustavo F. Padovan if (tx_seq == chan->expected_tx_seq) 33240a708f8fSGustavo F. Padovan goto expected; 33250a708f8fSGustavo F. Padovan 332642e5c802SGustavo F. Padovan tx_seq_offset = (tx_seq - chan->buffer_seq) % 64; 33270a708f8fSGustavo F. Padovan if (tx_seq_offset < 0) 33280a708f8fSGustavo F. Padovan tx_seq_offset += 64; 33290a708f8fSGustavo F. Padovan 33300a708f8fSGustavo F. Padovan /* invalid tx_seq */ 333147d1ec61SGustavo F. Padovan if (tx_seq_offset >= chan->tx_win) { 33328c1d787bSGustavo F. Padovan l2cap_send_disconn_req(chan->conn, chan, ECONNRESET); 33330a708f8fSGustavo F. Padovan goto drop; 33340a708f8fSGustavo F. Padovan } 33350a708f8fSGustavo F. Padovan 3336525cd185SGustavo F. Padovan if (chan->conn_state == L2CAP_CONN_LOCAL_BUSY) 33370a708f8fSGustavo F. Padovan goto drop; 33380a708f8fSGustavo F. Padovan 3339525cd185SGustavo F. Padovan if (chan->conn_state & L2CAP_CONN_SREJ_SENT) { 33400a708f8fSGustavo F. Padovan struct srej_list *first; 33410a708f8fSGustavo F. Padovan 334239d5a3eeSGustavo F. Padovan first = list_first_entry(&chan->srej_l, 33430a708f8fSGustavo F. Padovan struct srej_list, list); 33440a708f8fSGustavo F. Padovan if (tx_seq == first->tx_seq) { 334542e5c802SGustavo F. Padovan l2cap_add_to_srej_queue(chan, skb, tx_seq, sar); 3346525cd185SGustavo F. Padovan l2cap_check_srej_gap(chan, tx_seq); 33470a708f8fSGustavo F. Padovan 33480a708f8fSGustavo F. Padovan list_del(&first->list); 33490a708f8fSGustavo F. Padovan kfree(first); 33500a708f8fSGustavo F. Padovan 335139d5a3eeSGustavo F. Padovan if (list_empty(&chan->srej_l)) { 335242e5c802SGustavo F. Padovan chan->buffer_seq = chan->buffer_seq_srej; 3353525cd185SGustavo F. Padovan chan->conn_state &= ~L2CAP_CONN_SREJ_SENT; 3354525cd185SGustavo F. Padovan l2cap_send_ack(chan); 335549208c9cSGustavo F. Padovan BT_DBG("chan %p, Exit SREJ_SENT", chan); 33560a708f8fSGustavo F. Padovan } 33570a708f8fSGustavo F. Padovan } else { 33580a708f8fSGustavo F. Padovan struct srej_list *l; 33590a708f8fSGustavo F. Padovan 33600a708f8fSGustavo F. Padovan /* duplicated tx_seq */ 336142e5c802SGustavo F. Padovan if (l2cap_add_to_srej_queue(chan, skb, tx_seq, sar) < 0) 33620a708f8fSGustavo F. Padovan goto drop; 33630a708f8fSGustavo F. Padovan 336439d5a3eeSGustavo F. Padovan list_for_each_entry(l, &chan->srej_l, list) { 33650a708f8fSGustavo F. Padovan if (l->tx_seq == tx_seq) { 3366525cd185SGustavo F. Padovan l2cap_resend_srejframe(chan, tx_seq); 33670a708f8fSGustavo F. Padovan return 0; 33680a708f8fSGustavo F. Padovan } 33690a708f8fSGustavo F. Padovan } 3370525cd185SGustavo F. Padovan l2cap_send_srejframe(chan, tx_seq); 33710a708f8fSGustavo F. Padovan } 33720a708f8fSGustavo F. Padovan } else { 33730a708f8fSGustavo F. Padovan expected_tx_seq_offset = 337442e5c802SGustavo F. Padovan (chan->expected_tx_seq - chan->buffer_seq) % 64; 33750a708f8fSGustavo F. Padovan if (expected_tx_seq_offset < 0) 33760a708f8fSGustavo F. Padovan expected_tx_seq_offset += 64; 33770a708f8fSGustavo F. Padovan 33780a708f8fSGustavo F. Padovan /* duplicated tx_seq */ 33790a708f8fSGustavo F. Padovan if (tx_seq_offset < expected_tx_seq_offset) 33800a708f8fSGustavo F. Padovan goto drop; 33810a708f8fSGustavo F. Padovan 3382525cd185SGustavo F. Padovan chan->conn_state |= L2CAP_CONN_SREJ_SENT; 33830a708f8fSGustavo F. Padovan 338449208c9cSGustavo F. Padovan BT_DBG("chan %p, Enter SREJ", chan); 33850a708f8fSGustavo F. Padovan 338639d5a3eeSGustavo F. Padovan INIT_LIST_HEAD(&chan->srej_l); 338742e5c802SGustavo F. Padovan chan->buffer_seq_srej = chan->buffer_seq; 33880a708f8fSGustavo F. Padovan 3389f1c6775bSGustavo F. Padovan __skb_queue_head_init(&chan->srej_q); 3390f1c6775bSGustavo F. Padovan __skb_queue_head_init(&chan->busy_q); 339142e5c802SGustavo F. Padovan l2cap_add_to_srej_queue(chan, skb, tx_seq, sar); 33920a708f8fSGustavo F. Padovan 3393525cd185SGustavo F. Padovan chan->conn_state |= L2CAP_CONN_SEND_PBIT; 33940a708f8fSGustavo F. Padovan 3395525cd185SGustavo F. Padovan l2cap_send_srejframe(chan, tx_seq); 33960a708f8fSGustavo F. Padovan 3397e92c8e70SGustavo F. Padovan del_timer(&chan->ack_timer); 33980a708f8fSGustavo F. Padovan } 33990a708f8fSGustavo F. Padovan return 0; 34000a708f8fSGustavo F. Padovan 34010a708f8fSGustavo F. Padovan expected: 340242e5c802SGustavo F. Padovan chan->expected_tx_seq = (chan->expected_tx_seq + 1) % 64; 34030a708f8fSGustavo F. Padovan 3404525cd185SGustavo F. Padovan if (chan->conn_state & L2CAP_CONN_SREJ_SENT) { 34050a708f8fSGustavo F. Padovan bt_cb(skb)->tx_seq = tx_seq; 34060a708f8fSGustavo F. Padovan bt_cb(skb)->sar = sar; 3407f1c6775bSGustavo F. Padovan __skb_queue_tail(&chan->srej_q, skb); 34080a708f8fSGustavo F. Padovan return 0; 34090a708f8fSGustavo F. Padovan } 34100a708f8fSGustavo F. Padovan 3411525cd185SGustavo F. Padovan err = l2cap_push_rx_skb(chan, skb, rx_control); 34120a708f8fSGustavo F. Padovan if (err < 0) 34130a708f8fSGustavo F. Padovan return 0; 34140a708f8fSGustavo F. Padovan 34150a708f8fSGustavo F. Padovan if (rx_control & L2CAP_CTRL_FINAL) { 3416525cd185SGustavo F. Padovan if (chan->conn_state & L2CAP_CONN_REJ_ACT) 3417525cd185SGustavo F. Padovan chan->conn_state &= ~L2CAP_CONN_REJ_ACT; 34180a708f8fSGustavo F. Padovan else 3419525cd185SGustavo F. Padovan l2cap_retransmit_frames(chan); 34200a708f8fSGustavo F. Padovan } 34210a708f8fSGustavo F. Padovan 34220a708f8fSGustavo F. Padovan __mod_ack_timer(); 34230a708f8fSGustavo F. Padovan 34246a026610SGustavo F. Padovan chan->num_acked = (chan->num_acked + 1) % num_to_ack; 34256a026610SGustavo F. Padovan if (chan->num_acked == num_to_ack - 1) 3426525cd185SGustavo F. Padovan l2cap_send_ack(chan); 34270a708f8fSGustavo F. Padovan 34280a708f8fSGustavo F. Padovan return 0; 34290a708f8fSGustavo F. Padovan 34300a708f8fSGustavo F. Padovan drop: 34310a708f8fSGustavo F. Padovan kfree_skb(skb); 34320a708f8fSGustavo F. Padovan return 0; 34330a708f8fSGustavo F. Padovan } 34340a708f8fSGustavo F. Padovan 3435525cd185SGustavo F. Padovan static inline void l2cap_data_channel_rrframe(struct l2cap_chan *chan, u16 rx_control) 34360a708f8fSGustavo F. Padovan { 343749208c9cSGustavo F. Padovan BT_DBG("chan %p, req_seq %d ctrl 0x%4.4x", chan, __get_reqseq(rx_control), 34380a708f8fSGustavo F. Padovan rx_control); 34390a708f8fSGustavo F. Padovan 344042e5c802SGustavo F. Padovan chan->expected_ack_seq = __get_reqseq(rx_control); 344142e5c802SGustavo F. Padovan l2cap_drop_acked_frames(chan); 34420a708f8fSGustavo F. Padovan 34430a708f8fSGustavo F. Padovan if (rx_control & L2CAP_CTRL_POLL) { 3444525cd185SGustavo F. Padovan chan->conn_state |= L2CAP_CONN_SEND_FBIT; 3445525cd185SGustavo F. Padovan if (chan->conn_state & L2CAP_CONN_SREJ_SENT) { 3446525cd185SGustavo F. Padovan if ((chan->conn_state & L2CAP_CONN_REMOTE_BUSY) && 34476a026610SGustavo F. Padovan (chan->unacked_frames > 0)) 34480a708f8fSGustavo F. Padovan __mod_retrans_timer(); 34490a708f8fSGustavo F. Padovan 3450525cd185SGustavo F. Padovan chan->conn_state &= ~L2CAP_CONN_REMOTE_BUSY; 3451525cd185SGustavo F. Padovan l2cap_send_srejtail(chan); 34520a708f8fSGustavo F. Padovan } else { 3453525cd185SGustavo F. Padovan l2cap_send_i_or_rr_or_rnr(chan); 34540a708f8fSGustavo F. Padovan } 34550a708f8fSGustavo F. Padovan 34560a708f8fSGustavo F. Padovan } else if (rx_control & L2CAP_CTRL_FINAL) { 3457525cd185SGustavo F. Padovan chan->conn_state &= ~L2CAP_CONN_REMOTE_BUSY; 34580a708f8fSGustavo F. Padovan 3459525cd185SGustavo F. Padovan if (chan->conn_state & L2CAP_CONN_REJ_ACT) 3460525cd185SGustavo F. Padovan chan->conn_state &= ~L2CAP_CONN_REJ_ACT; 34610a708f8fSGustavo F. Padovan else 3462525cd185SGustavo F. Padovan l2cap_retransmit_frames(chan); 34630a708f8fSGustavo F. Padovan 34640a708f8fSGustavo F. Padovan } else { 3465525cd185SGustavo F. Padovan if ((chan->conn_state & L2CAP_CONN_REMOTE_BUSY) && 34666a026610SGustavo F. Padovan (chan->unacked_frames > 0)) 34670a708f8fSGustavo F. Padovan __mod_retrans_timer(); 34680a708f8fSGustavo F. Padovan 3469525cd185SGustavo F. Padovan chan->conn_state &= ~L2CAP_CONN_REMOTE_BUSY; 3470525cd185SGustavo F. Padovan if (chan->conn_state & L2CAP_CONN_SREJ_SENT) 3471525cd185SGustavo F. Padovan l2cap_send_ack(chan); 34720a708f8fSGustavo F. Padovan else 3473525cd185SGustavo F. Padovan l2cap_ertm_send(chan); 34740a708f8fSGustavo F. Padovan } 34750a708f8fSGustavo F. Padovan } 34760a708f8fSGustavo F. Padovan 3477525cd185SGustavo F. Padovan static inline void l2cap_data_channel_rejframe(struct l2cap_chan *chan, u16 rx_control) 34780a708f8fSGustavo F. Padovan { 34790a708f8fSGustavo F. Padovan u8 tx_seq = __get_reqseq(rx_control); 34800a708f8fSGustavo F. Padovan 3481525cd185SGustavo F. Padovan BT_DBG("chan %p, req_seq %d ctrl 0x%4.4x", chan, tx_seq, rx_control); 34820a708f8fSGustavo F. Padovan 3483525cd185SGustavo F. Padovan chan->conn_state &= ~L2CAP_CONN_REMOTE_BUSY; 34840a708f8fSGustavo F. Padovan 348542e5c802SGustavo F. Padovan chan->expected_ack_seq = tx_seq; 348642e5c802SGustavo F. Padovan l2cap_drop_acked_frames(chan); 34870a708f8fSGustavo F. Padovan 34880a708f8fSGustavo F. Padovan if (rx_control & L2CAP_CTRL_FINAL) { 3489525cd185SGustavo F. Padovan if (chan->conn_state & L2CAP_CONN_REJ_ACT) 3490525cd185SGustavo F. Padovan chan->conn_state &= ~L2CAP_CONN_REJ_ACT; 34910a708f8fSGustavo F. Padovan else 3492525cd185SGustavo F. Padovan l2cap_retransmit_frames(chan); 34930a708f8fSGustavo F. Padovan } else { 3494525cd185SGustavo F. Padovan l2cap_retransmit_frames(chan); 34950a708f8fSGustavo F. Padovan 3496525cd185SGustavo F. Padovan if (chan->conn_state & L2CAP_CONN_WAIT_F) 3497525cd185SGustavo F. Padovan chan->conn_state |= L2CAP_CONN_REJ_ACT; 34980a708f8fSGustavo F. Padovan } 34990a708f8fSGustavo F. Padovan } 3500525cd185SGustavo F. Padovan static inline void l2cap_data_channel_srejframe(struct l2cap_chan *chan, u16 rx_control) 35010a708f8fSGustavo F. Padovan { 35020a708f8fSGustavo F. Padovan u8 tx_seq = __get_reqseq(rx_control); 35030a708f8fSGustavo F. Padovan 3504525cd185SGustavo F. Padovan BT_DBG("chan %p, req_seq %d ctrl 0x%4.4x", chan, tx_seq, rx_control); 35050a708f8fSGustavo F. Padovan 3506525cd185SGustavo F. Padovan chan->conn_state &= ~L2CAP_CONN_REMOTE_BUSY; 35070a708f8fSGustavo F. Padovan 35080a708f8fSGustavo F. Padovan if (rx_control & L2CAP_CTRL_POLL) { 350942e5c802SGustavo F. Padovan chan->expected_ack_seq = tx_seq; 351042e5c802SGustavo F. Padovan l2cap_drop_acked_frames(chan); 35110a708f8fSGustavo F. Padovan 3512525cd185SGustavo F. Padovan chan->conn_state |= L2CAP_CONN_SEND_FBIT; 3513525cd185SGustavo F. Padovan l2cap_retransmit_one_frame(chan, tx_seq); 35140a708f8fSGustavo F. Padovan 3515525cd185SGustavo F. Padovan l2cap_ertm_send(chan); 35160a708f8fSGustavo F. Padovan 3517525cd185SGustavo F. Padovan if (chan->conn_state & L2CAP_CONN_WAIT_F) { 35186a026610SGustavo F. Padovan chan->srej_save_reqseq = tx_seq; 3519525cd185SGustavo F. Padovan chan->conn_state |= L2CAP_CONN_SREJ_ACT; 35200a708f8fSGustavo F. Padovan } 35210a708f8fSGustavo F. Padovan } else if (rx_control & L2CAP_CTRL_FINAL) { 3522525cd185SGustavo F. Padovan if ((chan->conn_state & L2CAP_CONN_SREJ_ACT) && 35236a026610SGustavo F. Padovan chan->srej_save_reqseq == tx_seq) 3524525cd185SGustavo F. Padovan chan->conn_state &= ~L2CAP_CONN_SREJ_ACT; 35250a708f8fSGustavo F. Padovan else 3526525cd185SGustavo F. Padovan l2cap_retransmit_one_frame(chan, tx_seq); 35270a708f8fSGustavo F. Padovan } else { 3528525cd185SGustavo F. Padovan l2cap_retransmit_one_frame(chan, tx_seq); 3529525cd185SGustavo F. Padovan if (chan->conn_state & L2CAP_CONN_WAIT_F) { 35306a026610SGustavo F. Padovan chan->srej_save_reqseq = tx_seq; 3531525cd185SGustavo F. Padovan chan->conn_state |= L2CAP_CONN_SREJ_ACT; 35320a708f8fSGustavo F. Padovan } 35330a708f8fSGustavo F. Padovan } 35340a708f8fSGustavo F. Padovan } 35350a708f8fSGustavo F. Padovan 3536525cd185SGustavo F. Padovan static inline void l2cap_data_channel_rnrframe(struct l2cap_chan *chan, u16 rx_control) 35370a708f8fSGustavo F. Padovan { 35380a708f8fSGustavo F. Padovan u8 tx_seq = __get_reqseq(rx_control); 35390a708f8fSGustavo F. Padovan 3540525cd185SGustavo F. Padovan BT_DBG("chan %p, req_seq %d ctrl 0x%4.4x", chan, tx_seq, rx_control); 35410a708f8fSGustavo F. Padovan 3542525cd185SGustavo F. Padovan chan->conn_state |= L2CAP_CONN_REMOTE_BUSY; 354342e5c802SGustavo F. Padovan chan->expected_ack_seq = tx_seq; 354442e5c802SGustavo F. Padovan l2cap_drop_acked_frames(chan); 35450a708f8fSGustavo F. Padovan 35460a708f8fSGustavo F. Padovan if (rx_control & L2CAP_CTRL_POLL) 3547525cd185SGustavo F. Padovan chan->conn_state |= L2CAP_CONN_SEND_FBIT; 35480a708f8fSGustavo F. Padovan 3549525cd185SGustavo F. Padovan if (!(chan->conn_state & L2CAP_CONN_SREJ_SENT)) { 3550e92c8e70SGustavo F. Padovan del_timer(&chan->retrans_timer); 35510a708f8fSGustavo F. Padovan if (rx_control & L2CAP_CTRL_POLL) 3552525cd185SGustavo F. Padovan l2cap_send_rr_or_rnr(chan, L2CAP_CTRL_FINAL); 35530a708f8fSGustavo F. Padovan return; 35540a708f8fSGustavo F. Padovan } 35550a708f8fSGustavo F. Padovan 35560a708f8fSGustavo F. Padovan if (rx_control & L2CAP_CTRL_POLL) 3557525cd185SGustavo F. Padovan l2cap_send_srejtail(chan); 35580a708f8fSGustavo F. Padovan else 3559525cd185SGustavo F. Padovan l2cap_send_sframe(chan, L2CAP_SUPER_RCV_READY); 35600a708f8fSGustavo F. Padovan } 35610a708f8fSGustavo F. Padovan 3562525cd185SGustavo F. Padovan static inline int l2cap_data_channel_sframe(struct l2cap_chan *chan, u16 rx_control, struct sk_buff *skb) 35630a708f8fSGustavo F. Padovan { 3564525cd185SGustavo F. Padovan BT_DBG("chan %p rx_control 0x%4.4x len %d", chan, rx_control, skb->len); 35650a708f8fSGustavo F. Padovan 35660a708f8fSGustavo F. Padovan if (L2CAP_CTRL_FINAL & rx_control && 3567525cd185SGustavo F. Padovan chan->conn_state & L2CAP_CONN_WAIT_F) { 3568e92c8e70SGustavo F. Padovan del_timer(&chan->monitor_timer); 35696a026610SGustavo F. Padovan if (chan->unacked_frames > 0) 35700a708f8fSGustavo F. Padovan __mod_retrans_timer(); 3571525cd185SGustavo F. Padovan chan->conn_state &= ~L2CAP_CONN_WAIT_F; 35720a708f8fSGustavo F. Padovan } 35730a708f8fSGustavo F. Padovan 35740a708f8fSGustavo F. Padovan switch (rx_control & L2CAP_CTRL_SUPERVISE) { 35750a708f8fSGustavo F. Padovan case L2CAP_SUPER_RCV_READY: 3576525cd185SGustavo F. Padovan l2cap_data_channel_rrframe(chan, rx_control); 35770a708f8fSGustavo F. Padovan break; 35780a708f8fSGustavo F. Padovan 35790a708f8fSGustavo F. Padovan case L2CAP_SUPER_REJECT: 3580525cd185SGustavo F. Padovan l2cap_data_channel_rejframe(chan, rx_control); 35810a708f8fSGustavo F. Padovan break; 35820a708f8fSGustavo F. Padovan 35830a708f8fSGustavo F. Padovan case L2CAP_SUPER_SELECT_REJECT: 3584525cd185SGustavo F. Padovan l2cap_data_channel_srejframe(chan, rx_control); 35850a708f8fSGustavo F. Padovan break; 35860a708f8fSGustavo F. Padovan 35870a708f8fSGustavo F. Padovan case L2CAP_SUPER_RCV_NOT_READY: 3588525cd185SGustavo F. Padovan l2cap_data_channel_rnrframe(chan, rx_control); 35890a708f8fSGustavo F. Padovan break; 35900a708f8fSGustavo F. Padovan } 35910a708f8fSGustavo F. Padovan 35920a708f8fSGustavo F. Padovan kfree_skb(skb); 35930a708f8fSGustavo F. Padovan return 0; 35940a708f8fSGustavo F. Padovan } 35950a708f8fSGustavo F. Padovan 35960a708f8fSGustavo F. Padovan static int l2cap_ertm_data_rcv(struct sock *sk, struct sk_buff *skb) 35970a708f8fSGustavo F. Padovan { 3598525cd185SGustavo F. Padovan struct l2cap_chan *chan = l2cap_pi(sk)->chan; 35990a708f8fSGustavo F. Padovan u16 control; 36000a708f8fSGustavo F. Padovan u8 req_seq; 36010a708f8fSGustavo F. Padovan int len, next_tx_seq_offset, req_seq_offset; 36020a708f8fSGustavo F. Padovan 36030a708f8fSGustavo F. Padovan control = get_unaligned_le16(skb->data); 36040a708f8fSGustavo F. Padovan skb_pull(skb, 2); 36050a708f8fSGustavo F. Padovan len = skb->len; 36060a708f8fSGustavo F. Padovan 36070a708f8fSGustavo F. Padovan /* 36080a708f8fSGustavo F. Padovan * We can just drop the corrupted I-frame here. 36090a708f8fSGustavo F. Padovan * Receiver will miss it and start proper recovery 36100a708f8fSGustavo F. Padovan * procedures and ask retransmission. 36110a708f8fSGustavo F. Padovan */ 361247d1ec61SGustavo F. Padovan if (l2cap_check_fcs(chan, skb)) 36130a708f8fSGustavo F. Padovan goto drop; 36140a708f8fSGustavo F. Padovan 36150a708f8fSGustavo F. Padovan if (__is_sar_start(control) && __is_iframe(control)) 36160a708f8fSGustavo F. Padovan len -= 2; 36170a708f8fSGustavo F. Padovan 361847d1ec61SGustavo F. Padovan if (chan->fcs == L2CAP_FCS_CRC16) 36190a708f8fSGustavo F. Padovan len -= 2; 36200a708f8fSGustavo F. Padovan 362147d1ec61SGustavo F. Padovan if (len > chan->mps) { 36228c1d787bSGustavo F. Padovan l2cap_send_disconn_req(chan->conn, chan, ECONNRESET); 36230a708f8fSGustavo F. Padovan goto drop; 36240a708f8fSGustavo F. Padovan } 36250a708f8fSGustavo F. Padovan 36260a708f8fSGustavo F. Padovan req_seq = __get_reqseq(control); 362742e5c802SGustavo F. Padovan req_seq_offset = (req_seq - chan->expected_ack_seq) % 64; 36280a708f8fSGustavo F. Padovan if (req_seq_offset < 0) 36290a708f8fSGustavo F. Padovan req_seq_offset += 64; 36300a708f8fSGustavo F. Padovan 36310a708f8fSGustavo F. Padovan next_tx_seq_offset = 363242e5c802SGustavo F. Padovan (chan->next_tx_seq - chan->expected_ack_seq) % 64; 36330a708f8fSGustavo F. Padovan if (next_tx_seq_offset < 0) 36340a708f8fSGustavo F. Padovan next_tx_seq_offset += 64; 36350a708f8fSGustavo F. Padovan 36360a708f8fSGustavo F. Padovan /* check for invalid req-seq */ 36370a708f8fSGustavo F. Padovan if (req_seq_offset > next_tx_seq_offset) { 36388c1d787bSGustavo F. Padovan l2cap_send_disconn_req(chan->conn, chan, ECONNRESET); 36390a708f8fSGustavo F. Padovan goto drop; 36400a708f8fSGustavo F. Padovan } 36410a708f8fSGustavo F. Padovan 36420a708f8fSGustavo F. Padovan if (__is_iframe(control)) { 36430a708f8fSGustavo F. Padovan if (len < 0) { 36448c1d787bSGustavo F. Padovan l2cap_send_disconn_req(chan->conn, chan, ECONNRESET); 36450a708f8fSGustavo F. Padovan goto drop; 36460a708f8fSGustavo F. Padovan } 36470a708f8fSGustavo F. Padovan 3648525cd185SGustavo F. Padovan l2cap_data_channel_iframe(chan, control, skb); 36490a708f8fSGustavo F. Padovan } else { 36500a708f8fSGustavo F. Padovan if (len != 0) { 36510a708f8fSGustavo F. Padovan BT_ERR("%d", len); 36528c1d787bSGustavo F. Padovan l2cap_send_disconn_req(chan->conn, chan, ECONNRESET); 36530a708f8fSGustavo F. Padovan goto drop; 36540a708f8fSGustavo F. Padovan } 36550a708f8fSGustavo F. Padovan 3656525cd185SGustavo F. Padovan l2cap_data_channel_sframe(chan, control, skb); 36570a708f8fSGustavo F. Padovan } 36580a708f8fSGustavo F. Padovan 36590a708f8fSGustavo F. Padovan return 0; 36600a708f8fSGustavo F. Padovan 36610a708f8fSGustavo F. Padovan drop: 36620a708f8fSGustavo F. Padovan kfree_skb(skb); 36630a708f8fSGustavo F. Padovan return 0; 36640a708f8fSGustavo F. Padovan } 36650a708f8fSGustavo F. Padovan 36660a708f8fSGustavo F. Padovan static inline int l2cap_data_channel(struct l2cap_conn *conn, u16 cid, struct sk_buff *skb) 36670a708f8fSGustavo F. Padovan { 366848454079SGustavo F. Padovan struct l2cap_chan *chan; 3669bf734843SDavid S. Miller struct sock *sk = NULL; 36700a708f8fSGustavo F. Padovan struct l2cap_pinfo *pi; 36710a708f8fSGustavo F. Padovan u16 control; 36720a708f8fSGustavo F. Padovan u8 tx_seq; 36730a708f8fSGustavo F. Padovan int len; 36740a708f8fSGustavo F. Padovan 3675baa7e1faSGustavo F. Padovan chan = l2cap_get_chan_by_scid(conn, cid); 367648454079SGustavo F. Padovan if (!chan) { 36770a708f8fSGustavo F. Padovan BT_DBG("unknown cid 0x%4.4x", cid); 36780a708f8fSGustavo F. Padovan goto drop; 36790a708f8fSGustavo F. Padovan } 36800a708f8fSGustavo F. Padovan 368148454079SGustavo F. Padovan sk = chan->sk; 36820a708f8fSGustavo F. Padovan pi = l2cap_pi(sk); 36830a708f8fSGustavo F. Padovan 368449208c9cSGustavo F. Padovan BT_DBG("chan %p, len %d", chan, skb->len); 36850a708f8fSGustavo F. Padovan 36860a708f8fSGustavo F. Padovan if (sk->sk_state != BT_CONNECTED) 36870a708f8fSGustavo F. Padovan goto drop; 36880a708f8fSGustavo F. Padovan 36890c1bc5c6SGustavo F. Padovan switch (chan->mode) { 36900a708f8fSGustavo F. Padovan case L2CAP_MODE_BASIC: 36910a708f8fSGustavo F. Padovan /* If socket recv buffers overflows we drop data here 36920a708f8fSGustavo F. Padovan * which is *bad* because L2CAP has to be reliable. 36930a708f8fSGustavo F. Padovan * But we don't have any other choice. L2CAP doesn't 36940a708f8fSGustavo F. Padovan * provide flow control mechanism. */ 36950a708f8fSGustavo F. Padovan 36960c1bc5c6SGustavo F. Padovan if (chan->imtu < skb->len) 36970a708f8fSGustavo F. Padovan goto drop; 36980a708f8fSGustavo F. Padovan 36990a708f8fSGustavo F. Padovan if (!sock_queue_rcv_skb(sk, skb)) 37000a708f8fSGustavo F. Padovan goto done; 37010a708f8fSGustavo F. Padovan break; 37020a708f8fSGustavo F. Padovan 37030a708f8fSGustavo F. Padovan case L2CAP_MODE_ERTM: 37040a708f8fSGustavo F. Padovan if (!sock_owned_by_user(sk)) { 37050a708f8fSGustavo F. Padovan l2cap_ertm_data_rcv(sk, skb); 37060a708f8fSGustavo F. Padovan } else { 37070a708f8fSGustavo F. Padovan if (sk_add_backlog(sk, skb)) 37080a708f8fSGustavo F. Padovan goto drop; 37090a708f8fSGustavo F. Padovan } 37100a708f8fSGustavo F. Padovan 37110a708f8fSGustavo F. Padovan goto done; 37120a708f8fSGustavo F. Padovan 37130a708f8fSGustavo F. Padovan case L2CAP_MODE_STREAMING: 37140a708f8fSGustavo F. Padovan control = get_unaligned_le16(skb->data); 37150a708f8fSGustavo F. Padovan skb_pull(skb, 2); 37160a708f8fSGustavo F. Padovan len = skb->len; 37170a708f8fSGustavo F. Padovan 371847d1ec61SGustavo F. Padovan if (l2cap_check_fcs(chan, skb)) 37190a708f8fSGustavo F. Padovan goto drop; 37200a708f8fSGustavo F. Padovan 37210a708f8fSGustavo F. Padovan if (__is_sar_start(control)) 37220a708f8fSGustavo F. Padovan len -= 2; 37230a708f8fSGustavo F. Padovan 372447d1ec61SGustavo F. Padovan if (chan->fcs == L2CAP_FCS_CRC16) 37250a708f8fSGustavo F. Padovan len -= 2; 37260a708f8fSGustavo F. Padovan 372747d1ec61SGustavo F. Padovan if (len > chan->mps || len < 0 || __is_sframe(control)) 37280a708f8fSGustavo F. Padovan goto drop; 37290a708f8fSGustavo F. Padovan 37300a708f8fSGustavo F. Padovan tx_seq = __get_txseq(control); 37310a708f8fSGustavo F. Padovan 373242e5c802SGustavo F. Padovan if (chan->expected_tx_seq == tx_seq) 373342e5c802SGustavo F. Padovan chan->expected_tx_seq = (chan->expected_tx_seq + 1) % 64; 37340a708f8fSGustavo F. Padovan else 373542e5c802SGustavo F. Padovan chan->expected_tx_seq = (tx_seq + 1) % 64; 37360a708f8fSGustavo F. Padovan 3737525cd185SGustavo F. Padovan l2cap_streaming_reassembly_sdu(chan, skb, control); 37380a708f8fSGustavo F. Padovan 37390a708f8fSGustavo F. Padovan goto done; 37400a708f8fSGustavo F. Padovan 37410a708f8fSGustavo F. Padovan default: 37420c1bc5c6SGustavo F. Padovan BT_DBG("chan %p: bad mode 0x%2.2x", chan, chan->mode); 37430a708f8fSGustavo F. Padovan break; 37440a708f8fSGustavo F. Padovan } 37450a708f8fSGustavo F. Padovan 37460a708f8fSGustavo F. Padovan drop: 37470a708f8fSGustavo F. Padovan kfree_skb(skb); 37480a708f8fSGustavo F. Padovan 37490a708f8fSGustavo F. Padovan done: 37500a708f8fSGustavo F. Padovan if (sk) 37510a708f8fSGustavo F. Padovan bh_unlock_sock(sk); 37520a708f8fSGustavo F. Padovan 37530a708f8fSGustavo F. Padovan return 0; 37540a708f8fSGustavo F. Padovan } 37550a708f8fSGustavo F. Padovan 37560a708f8fSGustavo F. Padovan static inline int l2cap_conless_channel(struct l2cap_conn *conn, __le16 psm, struct sk_buff *skb) 37570a708f8fSGustavo F. Padovan { 37586dcae1eaSDavid S. Miller struct sock *sk = NULL; 375923691d75SGustavo F. Padovan struct l2cap_chan *chan; 37600a708f8fSGustavo F. Padovan 376123691d75SGustavo F. Padovan chan = l2cap_global_chan_by_psm(0, psm, conn->src); 376223691d75SGustavo F. Padovan if (!chan) 37630a708f8fSGustavo F. Padovan goto drop; 37640a708f8fSGustavo F. Padovan 376523691d75SGustavo F. Padovan sk = chan->sk; 376623691d75SGustavo F. Padovan 37670a708f8fSGustavo F. Padovan bh_lock_sock(sk); 37680a708f8fSGustavo F. Padovan 37690a708f8fSGustavo F. Padovan BT_DBG("sk %p, len %d", sk, skb->len); 37700a708f8fSGustavo F. Padovan 37710a708f8fSGustavo F. Padovan if (sk->sk_state != BT_BOUND && sk->sk_state != BT_CONNECTED) 37720a708f8fSGustavo F. Padovan goto drop; 37730a708f8fSGustavo F. Padovan 37740c1bc5c6SGustavo F. Padovan if (l2cap_pi(sk)->chan->imtu < skb->len) 37750a708f8fSGustavo F. Padovan goto drop; 37760a708f8fSGustavo F. Padovan 37770a708f8fSGustavo F. Padovan if (!sock_queue_rcv_skb(sk, skb)) 37780a708f8fSGustavo F. Padovan goto done; 37790a708f8fSGustavo F. Padovan 37800a708f8fSGustavo F. Padovan drop: 37810a708f8fSGustavo F. Padovan kfree_skb(skb); 37820a708f8fSGustavo F. Padovan 37830a708f8fSGustavo F. Padovan done: 37840a708f8fSGustavo F. Padovan if (sk) 37850a708f8fSGustavo F. Padovan bh_unlock_sock(sk); 37860a708f8fSGustavo F. Padovan return 0; 37870a708f8fSGustavo F. Padovan } 37880a708f8fSGustavo F. Padovan 37899f69bda6SGustavo F. Padovan static inline int l2cap_att_channel(struct l2cap_conn *conn, __le16 cid, struct sk_buff *skb) 37909f69bda6SGustavo F. Padovan { 37916dcae1eaSDavid S. Miller struct sock *sk = NULL; 379223691d75SGustavo F. Padovan struct l2cap_chan *chan; 37939f69bda6SGustavo F. Padovan 379423691d75SGustavo F. Padovan chan = l2cap_global_chan_by_scid(0, cid, conn->src); 379523691d75SGustavo F. Padovan if (!chan) 37969f69bda6SGustavo F. Padovan goto drop; 37979f69bda6SGustavo F. Padovan 379823691d75SGustavo F. Padovan sk = chan->sk; 379923691d75SGustavo F. Padovan 38009f69bda6SGustavo F. Padovan bh_lock_sock(sk); 38019f69bda6SGustavo F. Padovan 38029f69bda6SGustavo F. Padovan BT_DBG("sk %p, len %d", sk, skb->len); 38039f69bda6SGustavo F. Padovan 38049f69bda6SGustavo F. Padovan if (sk->sk_state != BT_BOUND && sk->sk_state != BT_CONNECTED) 38059f69bda6SGustavo F. Padovan goto drop; 38069f69bda6SGustavo F. Padovan 38070c1bc5c6SGustavo F. Padovan if (l2cap_pi(sk)->chan->imtu < skb->len) 38089f69bda6SGustavo F. Padovan goto drop; 38099f69bda6SGustavo F. Padovan 38109f69bda6SGustavo F. Padovan if (!sock_queue_rcv_skb(sk, skb)) 38119f69bda6SGustavo F. Padovan goto done; 38129f69bda6SGustavo F. Padovan 38139f69bda6SGustavo F. Padovan drop: 38149f69bda6SGustavo F. Padovan kfree_skb(skb); 38159f69bda6SGustavo F. Padovan 38169f69bda6SGustavo F. Padovan done: 38179f69bda6SGustavo F. Padovan if (sk) 38189f69bda6SGustavo F. Padovan bh_unlock_sock(sk); 38199f69bda6SGustavo F. Padovan return 0; 38209f69bda6SGustavo F. Padovan } 38219f69bda6SGustavo F. Padovan 38220a708f8fSGustavo F. Padovan static void l2cap_recv_frame(struct l2cap_conn *conn, struct sk_buff *skb) 38230a708f8fSGustavo F. Padovan { 38240a708f8fSGustavo F. Padovan struct l2cap_hdr *lh = (void *) skb->data; 38250a708f8fSGustavo F. Padovan u16 cid, len; 38260a708f8fSGustavo F. Padovan __le16 psm; 38270a708f8fSGustavo F. Padovan 38280a708f8fSGustavo F. Padovan skb_pull(skb, L2CAP_HDR_SIZE); 38290a708f8fSGustavo F. Padovan cid = __le16_to_cpu(lh->cid); 38300a708f8fSGustavo F. Padovan len = __le16_to_cpu(lh->len); 38310a708f8fSGustavo F. Padovan 38320a708f8fSGustavo F. Padovan if (len != skb->len) { 38330a708f8fSGustavo F. Padovan kfree_skb(skb); 38340a708f8fSGustavo F. Padovan return; 38350a708f8fSGustavo F. Padovan } 38360a708f8fSGustavo F. Padovan 38370a708f8fSGustavo F. Padovan BT_DBG("len %d, cid 0x%4.4x", len, cid); 38380a708f8fSGustavo F. Padovan 38390a708f8fSGustavo F. Padovan switch (cid) { 38403300d9a9SClaudio Takahasi case L2CAP_CID_LE_SIGNALING: 38410a708f8fSGustavo F. Padovan case L2CAP_CID_SIGNALING: 38420a708f8fSGustavo F. Padovan l2cap_sig_channel(conn, skb); 38430a708f8fSGustavo F. Padovan break; 38440a708f8fSGustavo F. Padovan 38450a708f8fSGustavo F. Padovan case L2CAP_CID_CONN_LESS: 38460a708f8fSGustavo F. Padovan psm = get_unaligned_le16(skb->data); 38470a708f8fSGustavo F. Padovan skb_pull(skb, 2); 38480a708f8fSGustavo F. Padovan l2cap_conless_channel(conn, psm, skb); 38490a708f8fSGustavo F. Padovan break; 38500a708f8fSGustavo F. Padovan 38519f69bda6SGustavo F. Padovan case L2CAP_CID_LE_DATA: 38529f69bda6SGustavo F. Padovan l2cap_att_channel(conn, cid, skb); 38539f69bda6SGustavo F. Padovan break; 38549f69bda6SGustavo F. Padovan 38550a708f8fSGustavo F. Padovan default: 38560a708f8fSGustavo F. Padovan l2cap_data_channel(conn, cid, skb); 38570a708f8fSGustavo F. Padovan break; 38580a708f8fSGustavo F. Padovan } 38590a708f8fSGustavo F. Padovan } 38600a708f8fSGustavo F. Padovan 38610a708f8fSGustavo F. Padovan /* ---- L2CAP interface with lower layer (HCI) ---- */ 38620a708f8fSGustavo F. Padovan 38630a708f8fSGustavo F. Padovan static int l2cap_connect_ind(struct hci_dev *hdev, bdaddr_t *bdaddr, u8 type) 38640a708f8fSGustavo F. Padovan { 38650a708f8fSGustavo F. Padovan int exact = 0, lm1 = 0, lm2 = 0; 386623691d75SGustavo F. Padovan struct l2cap_chan *c; 38670a708f8fSGustavo F. Padovan 38680a708f8fSGustavo F. Padovan if (type != ACL_LINK) 38690a708f8fSGustavo F. Padovan return -EINVAL; 38700a708f8fSGustavo F. Padovan 38710a708f8fSGustavo F. Padovan BT_DBG("hdev %s, bdaddr %s", hdev->name, batostr(bdaddr)); 38720a708f8fSGustavo F. Padovan 38730a708f8fSGustavo F. Padovan /* Find listening sockets and check their link_mode */ 387423691d75SGustavo F. Padovan read_lock(&chan_list_lock); 387523691d75SGustavo F. Padovan list_for_each_entry(c, &chan_list, global_l) { 387623691d75SGustavo F. Padovan struct sock *sk = c->sk; 38774343478fSGustavo F. Padovan 38780a708f8fSGustavo F. Padovan if (sk->sk_state != BT_LISTEN) 38790a708f8fSGustavo F. Padovan continue; 38800a708f8fSGustavo F. Padovan 38810a708f8fSGustavo F. Padovan if (!bacmp(&bt_sk(sk)->src, &hdev->bdaddr)) { 38820a708f8fSGustavo F. Padovan lm1 |= HCI_LM_ACCEPT; 388323691d75SGustavo F. Padovan if (c->role_switch) 38840a708f8fSGustavo F. Padovan lm1 |= HCI_LM_MASTER; 38850a708f8fSGustavo F. Padovan exact++; 38860a708f8fSGustavo F. Padovan } else if (!bacmp(&bt_sk(sk)->src, BDADDR_ANY)) { 38870a708f8fSGustavo F. Padovan lm2 |= HCI_LM_ACCEPT; 388823691d75SGustavo F. Padovan if (c->role_switch) 38890a708f8fSGustavo F. Padovan lm2 |= HCI_LM_MASTER; 38900a708f8fSGustavo F. Padovan } 38910a708f8fSGustavo F. Padovan } 389223691d75SGustavo F. Padovan read_unlock(&chan_list_lock); 38930a708f8fSGustavo F. Padovan 38940a708f8fSGustavo F. Padovan return exact ? lm1 : lm2; 38950a708f8fSGustavo F. Padovan } 38960a708f8fSGustavo F. Padovan 38970a708f8fSGustavo F. Padovan static int l2cap_connect_cfm(struct hci_conn *hcon, u8 status) 38980a708f8fSGustavo F. Padovan { 38990a708f8fSGustavo F. Padovan struct l2cap_conn *conn; 39000a708f8fSGustavo F. Padovan 39010a708f8fSGustavo F. Padovan BT_DBG("hcon %p bdaddr %s status %d", hcon, batostr(&hcon->dst), status); 39020a708f8fSGustavo F. Padovan 3903acd7d370SVille Tervo if (!(hcon->type == ACL_LINK || hcon->type == LE_LINK)) 39040a708f8fSGustavo F. Padovan return -EINVAL; 39050a708f8fSGustavo F. Padovan 39060a708f8fSGustavo F. Padovan if (!status) { 39070a708f8fSGustavo F. Padovan conn = l2cap_conn_add(hcon, status); 39080a708f8fSGustavo F. Padovan if (conn) 39090a708f8fSGustavo F. Padovan l2cap_conn_ready(conn); 39100a708f8fSGustavo F. Padovan } else 39110a708f8fSGustavo F. Padovan l2cap_conn_del(hcon, bt_err(status)); 39120a708f8fSGustavo F. Padovan 39130a708f8fSGustavo F. Padovan return 0; 39140a708f8fSGustavo F. Padovan } 39150a708f8fSGustavo F. Padovan 39160a708f8fSGustavo F. Padovan static int l2cap_disconn_ind(struct hci_conn *hcon) 39170a708f8fSGustavo F. Padovan { 39180a708f8fSGustavo F. Padovan struct l2cap_conn *conn = hcon->l2cap_data; 39190a708f8fSGustavo F. Padovan 39200a708f8fSGustavo F. Padovan BT_DBG("hcon %p", hcon); 39210a708f8fSGustavo F. Padovan 39220a708f8fSGustavo F. Padovan if (hcon->type != ACL_LINK || !conn) 39230a708f8fSGustavo F. Padovan return 0x13; 39240a708f8fSGustavo F. Padovan 39250a708f8fSGustavo F. Padovan return conn->disc_reason; 39260a708f8fSGustavo F. Padovan } 39270a708f8fSGustavo F. Padovan 39280a708f8fSGustavo F. Padovan static int l2cap_disconn_cfm(struct hci_conn *hcon, u8 reason) 39290a708f8fSGustavo F. Padovan { 39300a708f8fSGustavo F. Padovan BT_DBG("hcon %p reason %d", hcon, reason); 39310a708f8fSGustavo F. Padovan 3932acd7d370SVille Tervo if (!(hcon->type == ACL_LINK || hcon->type == LE_LINK)) 39330a708f8fSGustavo F. Padovan return -EINVAL; 39340a708f8fSGustavo F. Padovan 39350a708f8fSGustavo F. Padovan l2cap_conn_del(hcon, bt_err(reason)); 39360a708f8fSGustavo F. Padovan 39370a708f8fSGustavo F. Padovan return 0; 39380a708f8fSGustavo F. Padovan } 39390a708f8fSGustavo F. Padovan 39404343478fSGustavo F. Padovan static inline void l2cap_check_encryption(struct l2cap_chan *chan, u8 encrypt) 39410a708f8fSGustavo F. Padovan { 39424343478fSGustavo F. Padovan struct sock *sk = chan->sk; 39434343478fSGustavo F. Padovan 39440a708f8fSGustavo F. Padovan if (sk->sk_type != SOCK_SEQPACKET && sk->sk_type != SOCK_STREAM) 39450a708f8fSGustavo F. Padovan return; 39460a708f8fSGustavo F. Padovan 39470a708f8fSGustavo F. Padovan if (encrypt == 0x00) { 39484343478fSGustavo F. Padovan if (chan->sec_level == BT_SECURITY_MEDIUM) { 39490a708f8fSGustavo F. Padovan l2cap_sock_clear_timer(sk); 39500a708f8fSGustavo F. Padovan l2cap_sock_set_timer(sk, HZ * 5); 39514343478fSGustavo F. Padovan } else if (chan->sec_level == BT_SECURITY_HIGH) 39520a708f8fSGustavo F. Padovan __l2cap_sock_close(sk, ECONNREFUSED); 39530a708f8fSGustavo F. Padovan } else { 39544343478fSGustavo F. Padovan if (chan->sec_level == BT_SECURITY_MEDIUM) 39550a708f8fSGustavo F. Padovan l2cap_sock_clear_timer(sk); 39560a708f8fSGustavo F. Padovan } 39570a708f8fSGustavo F. Padovan } 39580a708f8fSGustavo F. Padovan 39590a708f8fSGustavo F. Padovan static int l2cap_security_cfm(struct hci_conn *hcon, u8 status, u8 encrypt) 39600a708f8fSGustavo F. Padovan { 39610a708f8fSGustavo F. Padovan struct l2cap_conn *conn = hcon->l2cap_data; 396248454079SGustavo F. Padovan struct l2cap_chan *chan; 39630a708f8fSGustavo F. Padovan 39640a708f8fSGustavo F. Padovan if (!conn) 39650a708f8fSGustavo F. Padovan return 0; 39660a708f8fSGustavo F. Padovan 39670a708f8fSGustavo F. Padovan BT_DBG("conn %p", conn); 39680a708f8fSGustavo F. Padovan 3969baa7e1faSGustavo F. Padovan read_lock(&conn->chan_lock); 39700a708f8fSGustavo F. Padovan 3971baa7e1faSGustavo F. Padovan list_for_each_entry(chan, &conn->chan_l, list) { 397248454079SGustavo F. Padovan struct sock *sk = chan->sk; 3973baa7e1faSGustavo F. Padovan 39740a708f8fSGustavo F. Padovan bh_lock_sock(sk); 39750a708f8fSGustavo F. Padovan 3976b4450035SGustavo F. Padovan if (chan->conf_state & L2CAP_CONF_CONNECT_PEND) { 39770a708f8fSGustavo F. Padovan bh_unlock_sock(sk); 39780a708f8fSGustavo F. Padovan continue; 39790a708f8fSGustavo F. Padovan } 39800a708f8fSGustavo F. Padovan 39810a708f8fSGustavo F. Padovan if (!status && (sk->sk_state == BT_CONNECTED || 39820a708f8fSGustavo F. Padovan sk->sk_state == BT_CONFIG)) { 39834343478fSGustavo F. Padovan l2cap_check_encryption(chan, encrypt); 39840a708f8fSGustavo F. Padovan bh_unlock_sock(sk); 39850a708f8fSGustavo F. Padovan continue; 39860a708f8fSGustavo F. Padovan } 39870a708f8fSGustavo F. Padovan 39880a708f8fSGustavo F. Padovan if (sk->sk_state == BT_CONNECT) { 39890a708f8fSGustavo F. Padovan if (!status) { 39900a708f8fSGustavo F. Padovan struct l2cap_conn_req req; 3991fe4128e0SGustavo F. Padovan req.scid = cpu_to_le16(chan->scid); 3992fe4128e0SGustavo F. Padovan req.psm = chan->psm; 39930a708f8fSGustavo F. Padovan 3994fc7f8a7eSGustavo F. Padovan chan->ident = l2cap_get_ident(conn); 3995b4450035SGustavo F. Padovan chan->conf_state |= L2CAP_CONF_CONNECT_PEND; 39960a708f8fSGustavo F. Padovan 3997fc7f8a7eSGustavo F. Padovan l2cap_send_cmd(conn, chan->ident, 39980a708f8fSGustavo F. Padovan L2CAP_CONN_REQ, sizeof(req), &req); 39990a708f8fSGustavo F. Padovan } else { 40000a708f8fSGustavo F. Padovan l2cap_sock_clear_timer(sk); 40010a708f8fSGustavo F. Padovan l2cap_sock_set_timer(sk, HZ / 10); 40020a708f8fSGustavo F. Padovan } 40030a708f8fSGustavo F. Padovan } else if (sk->sk_state == BT_CONNECT2) { 40040a708f8fSGustavo F. Padovan struct l2cap_conn_rsp rsp; 40050a708f8fSGustavo F. Padovan __u16 result; 40060a708f8fSGustavo F. Padovan 40070a708f8fSGustavo F. Padovan if (!status) { 40080a708f8fSGustavo F. Padovan sk->sk_state = BT_CONFIG; 40090a708f8fSGustavo F. Padovan result = L2CAP_CR_SUCCESS; 40100a708f8fSGustavo F. Padovan } else { 40110a708f8fSGustavo F. Padovan sk->sk_state = BT_DISCONN; 40120a708f8fSGustavo F. Padovan l2cap_sock_set_timer(sk, HZ / 10); 40130a708f8fSGustavo F. Padovan result = L2CAP_CR_SEC_BLOCK; 40140a708f8fSGustavo F. Padovan } 40150a708f8fSGustavo F. Padovan 4016fe4128e0SGustavo F. Padovan rsp.scid = cpu_to_le16(chan->dcid); 4017fe4128e0SGustavo F. Padovan rsp.dcid = cpu_to_le16(chan->scid); 40180a708f8fSGustavo F. Padovan rsp.result = cpu_to_le16(result); 40190a708f8fSGustavo F. Padovan rsp.status = cpu_to_le16(L2CAP_CS_NO_INFO); 4020fc7f8a7eSGustavo F. Padovan l2cap_send_cmd(conn, chan->ident, L2CAP_CONN_RSP, 4021fc7f8a7eSGustavo F. Padovan sizeof(rsp), &rsp); 40220a708f8fSGustavo F. Padovan } 40230a708f8fSGustavo F. Padovan 40240a708f8fSGustavo F. Padovan bh_unlock_sock(sk); 40250a708f8fSGustavo F. Padovan } 40260a708f8fSGustavo F. Padovan 4027baa7e1faSGustavo F. Padovan read_unlock(&conn->chan_lock); 40280a708f8fSGustavo F. Padovan 40290a708f8fSGustavo F. Padovan return 0; 40300a708f8fSGustavo F. Padovan } 40310a708f8fSGustavo F. Padovan 40320a708f8fSGustavo F. Padovan static int l2cap_recv_acldata(struct hci_conn *hcon, struct sk_buff *skb, u16 flags) 40330a708f8fSGustavo F. Padovan { 40340a708f8fSGustavo F. Padovan struct l2cap_conn *conn = hcon->l2cap_data; 40350a708f8fSGustavo F. Padovan 40360a708f8fSGustavo F. Padovan if (!conn) 40370a708f8fSGustavo F. Padovan conn = l2cap_conn_add(hcon, 0); 40380a708f8fSGustavo F. Padovan 40390a708f8fSGustavo F. Padovan if (!conn) 40400a708f8fSGustavo F. Padovan goto drop; 40410a708f8fSGustavo F. Padovan 40420a708f8fSGustavo F. Padovan BT_DBG("conn %p len %d flags 0x%x", conn, skb->len, flags); 40430a708f8fSGustavo F. Padovan 40440a708f8fSGustavo F. Padovan if (!(flags & ACL_CONT)) { 40450a708f8fSGustavo F. Padovan struct l2cap_hdr *hdr; 404648454079SGustavo F. Padovan struct l2cap_chan *chan; 40470a708f8fSGustavo F. Padovan u16 cid; 40480a708f8fSGustavo F. Padovan int len; 40490a708f8fSGustavo F. Padovan 40500a708f8fSGustavo F. Padovan if (conn->rx_len) { 40510a708f8fSGustavo F. Padovan BT_ERR("Unexpected start frame (len %d)", skb->len); 40520a708f8fSGustavo F. Padovan kfree_skb(conn->rx_skb); 40530a708f8fSGustavo F. Padovan conn->rx_skb = NULL; 40540a708f8fSGustavo F. Padovan conn->rx_len = 0; 40550a708f8fSGustavo F. Padovan l2cap_conn_unreliable(conn, ECOMM); 40560a708f8fSGustavo F. Padovan } 40570a708f8fSGustavo F. Padovan 40580a708f8fSGustavo F. Padovan /* Start fragment always begin with Basic L2CAP header */ 40590a708f8fSGustavo F. Padovan if (skb->len < L2CAP_HDR_SIZE) { 40600a708f8fSGustavo F. Padovan BT_ERR("Frame is too short (len %d)", skb->len); 40610a708f8fSGustavo F. Padovan l2cap_conn_unreliable(conn, ECOMM); 40620a708f8fSGustavo F. Padovan goto drop; 40630a708f8fSGustavo F. Padovan } 40640a708f8fSGustavo F. Padovan 40650a708f8fSGustavo F. Padovan hdr = (struct l2cap_hdr *) skb->data; 40660a708f8fSGustavo F. Padovan len = __le16_to_cpu(hdr->len) + L2CAP_HDR_SIZE; 40670a708f8fSGustavo F. Padovan cid = __le16_to_cpu(hdr->cid); 40680a708f8fSGustavo F. Padovan 40690a708f8fSGustavo F. Padovan if (len == skb->len) { 40700a708f8fSGustavo F. Padovan /* Complete frame received */ 40710a708f8fSGustavo F. Padovan l2cap_recv_frame(conn, skb); 40720a708f8fSGustavo F. Padovan return 0; 40730a708f8fSGustavo F. Padovan } 40740a708f8fSGustavo F. Padovan 40750a708f8fSGustavo F. Padovan BT_DBG("Start: total len %d, frag len %d", len, skb->len); 40760a708f8fSGustavo F. Padovan 40770a708f8fSGustavo F. Padovan if (skb->len > len) { 40780a708f8fSGustavo F. Padovan BT_ERR("Frame is too long (len %d, expected len %d)", 40790a708f8fSGustavo F. Padovan skb->len, len); 40800a708f8fSGustavo F. Padovan l2cap_conn_unreliable(conn, ECOMM); 40810a708f8fSGustavo F. Padovan goto drop; 40820a708f8fSGustavo F. Padovan } 40830a708f8fSGustavo F. Padovan 4084baa7e1faSGustavo F. Padovan chan = l2cap_get_chan_by_scid(conn, cid); 40850a708f8fSGustavo F. Padovan 408648454079SGustavo F. Padovan if (chan && chan->sk) { 408748454079SGustavo F. Padovan struct sock *sk = chan->sk; 408848454079SGustavo F. Padovan 40890c1bc5c6SGustavo F. Padovan if (chan->imtu < len - L2CAP_HDR_SIZE) { 409048454079SGustavo F. Padovan BT_ERR("Frame exceeding recv MTU (len %d, " 409148454079SGustavo F. Padovan "MTU %d)", len, 40920c1bc5c6SGustavo F. Padovan chan->imtu); 40930a708f8fSGustavo F. Padovan bh_unlock_sock(sk); 40940a708f8fSGustavo F. Padovan l2cap_conn_unreliable(conn, ECOMM); 40950a708f8fSGustavo F. Padovan goto drop; 40960a708f8fSGustavo F. Padovan } 40970a708f8fSGustavo F. Padovan bh_unlock_sock(sk); 409848454079SGustavo F. Padovan } 40990a708f8fSGustavo F. Padovan 41000a708f8fSGustavo F. Padovan /* Allocate skb for the complete frame (with header) */ 41010a708f8fSGustavo F. Padovan conn->rx_skb = bt_skb_alloc(len, GFP_ATOMIC); 41020a708f8fSGustavo F. Padovan if (!conn->rx_skb) 41030a708f8fSGustavo F. Padovan goto drop; 41040a708f8fSGustavo F. Padovan 41050a708f8fSGustavo F. Padovan skb_copy_from_linear_data(skb, skb_put(conn->rx_skb, skb->len), 41060a708f8fSGustavo F. Padovan skb->len); 41070a708f8fSGustavo F. Padovan conn->rx_len = len - skb->len; 41080a708f8fSGustavo F. Padovan } else { 41090a708f8fSGustavo F. Padovan BT_DBG("Cont: frag len %d (expecting %d)", skb->len, conn->rx_len); 41100a708f8fSGustavo F. Padovan 41110a708f8fSGustavo F. Padovan if (!conn->rx_len) { 41120a708f8fSGustavo F. Padovan BT_ERR("Unexpected continuation frame (len %d)", skb->len); 41130a708f8fSGustavo F. Padovan l2cap_conn_unreliable(conn, ECOMM); 41140a708f8fSGustavo F. Padovan goto drop; 41150a708f8fSGustavo F. Padovan } 41160a708f8fSGustavo F. Padovan 41170a708f8fSGustavo F. Padovan if (skb->len > conn->rx_len) { 41180a708f8fSGustavo F. Padovan BT_ERR("Fragment is too long (len %d, expected %d)", 41190a708f8fSGustavo F. Padovan skb->len, conn->rx_len); 41200a708f8fSGustavo F. Padovan kfree_skb(conn->rx_skb); 41210a708f8fSGustavo F. Padovan conn->rx_skb = NULL; 41220a708f8fSGustavo F. Padovan conn->rx_len = 0; 41230a708f8fSGustavo F. Padovan l2cap_conn_unreliable(conn, ECOMM); 41240a708f8fSGustavo F. Padovan goto drop; 41250a708f8fSGustavo F. Padovan } 41260a708f8fSGustavo F. Padovan 41270a708f8fSGustavo F. Padovan skb_copy_from_linear_data(skb, skb_put(conn->rx_skb, skb->len), 41280a708f8fSGustavo F. Padovan skb->len); 41290a708f8fSGustavo F. Padovan conn->rx_len -= skb->len; 41300a708f8fSGustavo F. Padovan 41310a708f8fSGustavo F. Padovan if (!conn->rx_len) { 41320a708f8fSGustavo F. Padovan /* Complete frame received */ 41330a708f8fSGustavo F. Padovan l2cap_recv_frame(conn, conn->rx_skb); 41340a708f8fSGustavo F. Padovan conn->rx_skb = NULL; 41350a708f8fSGustavo F. Padovan } 41360a708f8fSGustavo F. Padovan } 41370a708f8fSGustavo F. Padovan 41380a708f8fSGustavo F. Padovan drop: 41390a708f8fSGustavo F. Padovan kfree_skb(skb); 41400a708f8fSGustavo F. Padovan return 0; 41410a708f8fSGustavo F. Padovan } 41420a708f8fSGustavo F. Padovan 41430a708f8fSGustavo F. Padovan static int l2cap_debugfs_show(struct seq_file *f, void *p) 41440a708f8fSGustavo F. Padovan { 414523691d75SGustavo F. Padovan struct l2cap_chan *c; 41460a708f8fSGustavo F. Padovan 414723691d75SGustavo F. Padovan read_lock_bh(&chan_list_lock); 41480a708f8fSGustavo F. Padovan 414923691d75SGustavo F. Padovan list_for_each_entry(c, &chan_list, global_l) { 415023691d75SGustavo F. Padovan struct sock *sk = c->sk; 41510a708f8fSGustavo F. Padovan 4152903d343eSGustavo F. Padovan seq_printf(f, "%s %s %d %d 0x%4.4x 0x%4.4x %d %d %d %d\n", 41530a708f8fSGustavo F. Padovan batostr(&bt_sk(sk)->src), 41540a708f8fSGustavo F. Padovan batostr(&bt_sk(sk)->dst), 415523691d75SGustavo F. Padovan sk->sk_state, __le16_to_cpu(c->psm), 415623691d75SGustavo F. Padovan c->scid, c->dcid, c->imtu, c->omtu, 415723691d75SGustavo F. Padovan c->sec_level, c->mode); 41580a708f8fSGustavo F. Padovan } 41590a708f8fSGustavo F. Padovan 416023691d75SGustavo F. Padovan read_unlock_bh(&chan_list_lock); 41610a708f8fSGustavo F. Padovan 41620a708f8fSGustavo F. Padovan return 0; 41630a708f8fSGustavo F. Padovan } 41640a708f8fSGustavo F. Padovan 41650a708f8fSGustavo F. Padovan static int l2cap_debugfs_open(struct inode *inode, struct file *file) 41660a708f8fSGustavo F. Padovan { 41670a708f8fSGustavo F. Padovan return single_open(file, l2cap_debugfs_show, inode->i_private); 41680a708f8fSGustavo F. Padovan } 41690a708f8fSGustavo F. Padovan 41700a708f8fSGustavo F. Padovan static const struct file_operations l2cap_debugfs_fops = { 41710a708f8fSGustavo F. Padovan .open = l2cap_debugfs_open, 41720a708f8fSGustavo F. Padovan .read = seq_read, 41730a708f8fSGustavo F. Padovan .llseek = seq_lseek, 41740a708f8fSGustavo F. Padovan .release = single_release, 41750a708f8fSGustavo F. Padovan }; 41760a708f8fSGustavo F. Padovan 41770a708f8fSGustavo F. Padovan static struct dentry *l2cap_debugfs; 41780a708f8fSGustavo F. Padovan 41790a708f8fSGustavo F. Padovan static struct hci_proto l2cap_hci_proto = { 41800a708f8fSGustavo F. Padovan .name = "L2CAP", 41810a708f8fSGustavo F. Padovan .id = HCI_PROTO_L2CAP, 41820a708f8fSGustavo F. Padovan .connect_ind = l2cap_connect_ind, 41830a708f8fSGustavo F. Padovan .connect_cfm = l2cap_connect_cfm, 41840a708f8fSGustavo F. Padovan .disconn_ind = l2cap_disconn_ind, 41850a708f8fSGustavo F. Padovan .disconn_cfm = l2cap_disconn_cfm, 41860a708f8fSGustavo F. Padovan .security_cfm = l2cap_security_cfm, 41870a708f8fSGustavo F. Padovan .recv_acldata = l2cap_recv_acldata 41880a708f8fSGustavo F. Padovan }; 41890a708f8fSGustavo F. Padovan 419064274518SGustavo F. Padovan int __init l2cap_init(void) 41910a708f8fSGustavo F. Padovan { 41920a708f8fSGustavo F. Padovan int err; 41930a708f8fSGustavo F. Padovan 4194bb58f747SGustavo F. Padovan err = l2cap_init_sockets(); 41950a708f8fSGustavo F. Padovan if (err < 0) 41960a708f8fSGustavo F. Padovan return err; 41970a708f8fSGustavo F. Padovan 41980a708f8fSGustavo F. Padovan _busy_wq = create_singlethread_workqueue("l2cap"); 41990a708f8fSGustavo F. Padovan if (!_busy_wq) { 4200bb58f747SGustavo F. Padovan err = -ENOMEM; 42010a708f8fSGustavo F. Padovan goto error; 42020a708f8fSGustavo F. Padovan } 42030a708f8fSGustavo F. Padovan 42040a708f8fSGustavo F. Padovan err = hci_register_proto(&l2cap_hci_proto); 42050a708f8fSGustavo F. Padovan if (err < 0) { 42060a708f8fSGustavo F. Padovan BT_ERR("L2CAP protocol registration failed"); 42070a708f8fSGustavo F. Padovan bt_sock_unregister(BTPROTO_L2CAP); 42080a708f8fSGustavo F. Padovan goto error; 42090a708f8fSGustavo F. Padovan } 42100a708f8fSGustavo F. Padovan 42110a708f8fSGustavo F. Padovan if (bt_debugfs) { 42120a708f8fSGustavo F. Padovan l2cap_debugfs = debugfs_create_file("l2cap", 0444, 42130a708f8fSGustavo F. Padovan bt_debugfs, NULL, &l2cap_debugfs_fops); 42140a708f8fSGustavo F. Padovan if (!l2cap_debugfs) 42150a708f8fSGustavo F. Padovan BT_ERR("Failed to create L2CAP debug file"); 42160a708f8fSGustavo F. Padovan } 42170a708f8fSGustavo F. Padovan 42180a708f8fSGustavo F. Padovan return 0; 42190a708f8fSGustavo F. Padovan 42200a708f8fSGustavo F. Padovan error: 42210a708f8fSGustavo F. Padovan destroy_workqueue(_busy_wq); 4222bb58f747SGustavo F. Padovan l2cap_cleanup_sockets(); 42230a708f8fSGustavo F. Padovan return err; 42240a708f8fSGustavo F. Padovan } 42250a708f8fSGustavo F. Padovan 422664274518SGustavo F. Padovan void l2cap_exit(void) 42270a708f8fSGustavo F. Padovan { 42280a708f8fSGustavo F. Padovan debugfs_remove(l2cap_debugfs); 42290a708f8fSGustavo F. Padovan 42300a708f8fSGustavo F. Padovan flush_workqueue(_busy_wq); 42310a708f8fSGustavo F. Padovan destroy_workqueue(_busy_wq); 42320a708f8fSGustavo F. Padovan 42330a708f8fSGustavo F. Padovan if (hci_unregister_proto(&l2cap_hci_proto) < 0) 42340a708f8fSGustavo F. Padovan BT_ERR("L2CAP protocol unregistration failed"); 42350a708f8fSGustavo F. Padovan 4236bb58f747SGustavo F. Padovan l2cap_cleanup_sockets(); 42370a708f8fSGustavo F. Padovan } 42380a708f8fSGustavo F. Padovan 42390a708f8fSGustavo F. Padovan module_param(disable_ertm, bool, 0644); 42400a708f8fSGustavo F. Padovan MODULE_PARM_DESC(disable_ertm, "Disable enhanced retransmission mode"); 4241