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); 724519de9aSGustavo F. Padovan static void l2cap_send_cmd(struct l2cap_conn *conn, u8 ident, u8 code, u16 len, 734519de9aSGustavo F. Padovan void *data); 74710f9b0aSGustavo F. Padovan static int l2cap_build_conf_req(struct l2cap_chan *chan, void *data); 754519de9aSGustavo F. Padovan static void l2cap_send_disconn_req(struct l2cap_conn *conn, 764519de9aSGustavo F. Padovan struct l2cap_chan *chan, int err); 770a708f8fSGustavo F. Padovan 780a708f8fSGustavo F. Padovan static int l2cap_ertm_data_rcv(struct sock *sk, struct sk_buff *skb); 790a708f8fSGustavo F. Padovan 800a708f8fSGustavo F. Padovan /* ---- L2CAP channels ---- */ 81baa7e1faSGustavo F. Padovan static struct l2cap_chan *__l2cap_get_chan_by_dcid(struct l2cap_conn *conn, u16 cid) 820a708f8fSGustavo F. Padovan { 8348454079SGustavo F. Padovan struct l2cap_chan *c; 84baa7e1faSGustavo F. Padovan 85baa7e1faSGustavo F. Padovan list_for_each_entry(c, &conn->chan_l, list) { 86fe4128e0SGustavo F. Padovan if (c->dcid == cid) 8748454079SGustavo F. Padovan return c; 880a708f8fSGustavo F. Padovan } 89baa7e1faSGustavo F. Padovan return NULL; 90baa7e1faSGustavo F. Padovan 91baa7e1faSGustavo F. Padovan } 920a708f8fSGustavo F. Padovan 93baa7e1faSGustavo F. Padovan static struct l2cap_chan *__l2cap_get_chan_by_scid(struct l2cap_conn *conn, u16 cid) 940a708f8fSGustavo F. Padovan { 9548454079SGustavo F. Padovan struct l2cap_chan *c; 96baa7e1faSGustavo F. Padovan 97baa7e1faSGustavo F. Padovan list_for_each_entry(c, &conn->chan_l, list) { 98fe4128e0SGustavo F. Padovan if (c->scid == cid) 9948454079SGustavo F. Padovan return c; 1000a708f8fSGustavo F. Padovan } 101baa7e1faSGustavo F. Padovan return NULL; 102baa7e1faSGustavo F. Padovan } 1030a708f8fSGustavo F. Padovan 1040a708f8fSGustavo F. Padovan /* Find channel with given SCID. 1050a708f8fSGustavo F. Padovan * Returns locked socket */ 106baa7e1faSGustavo F. Padovan static struct l2cap_chan *l2cap_get_chan_by_scid(struct l2cap_conn *conn, u16 cid) 1070a708f8fSGustavo F. Padovan { 10848454079SGustavo F. Padovan struct l2cap_chan *c; 109baa7e1faSGustavo F. Padovan 110baa7e1faSGustavo F. Padovan read_lock(&conn->chan_lock); 111baa7e1faSGustavo F. Padovan c = __l2cap_get_chan_by_scid(conn, cid); 11248454079SGustavo F. Padovan if (c) 11348454079SGustavo F. Padovan bh_lock_sock(c->sk); 114baa7e1faSGustavo F. Padovan read_unlock(&conn->chan_lock); 11548454079SGustavo F. Padovan return c; 1160a708f8fSGustavo F. Padovan } 1170a708f8fSGustavo F. Padovan 118baa7e1faSGustavo F. Padovan static struct l2cap_chan *__l2cap_get_chan_by_ident(struct l2cap_conn *conn, u8 ident) 1190a708f8fSGustavo F. Padovan { 12048454079SGustavo F. Padovan struct l2cap_chan *c; 121baa7e1faSGustavo F. Padovan 122baa7e1faSGustavo F. Padovan list_for_each_entry(c, &conn->chan_l, list) { 123fc7f8a7eSGustavo F. Padovan if (c->ident == ident) 12448454079SGustavo F. Padovan return c; 1250a708f8fSGustavo F. Padovan } 126baa7e1faSGustavo F. Padovan return NULL; 127baa7e1faSGustavo F. Padovan } 1280a708f8fSGustavo F. Padovan 129baa7e1faSGustavo F. Padovan static inline struct l2cap_chan *l2cap_get_chan_by_ident(struct l2cap_conn *conn, u8 ident) 1300a708f8fSGustavo F. Padovan { 13148454079SGustavo F. Padovan struct l2cap_chan *c; 132baa7e1faSGustavo F. Padovan 133baa7e1faSGustavo F. Padovan read_lock(&conn->chan_lock); 134baa7e1faSGustavo F. Padovan c = __l2cap_get_chan_by_ident(conn, ident); 13548454079SGustavo F. Padovan if (c) 13648454079SGustavo F. Padovan bh_lock_sock(c->sk); 137baa7e1faSGustavo F. Padovan read_unlock(&conn->chan_lock); 13848454079SGustavo F. Padovan return c; 1390a708f8fSGustavo F. Padovan } 1400a708f8fSGustavo F. Padovan 14123691d75SGustavo F. Padovan static struct l2cap_chan *__l2cap_global_chan_by_addr(__le16 psm, bdaddr_t *src) 1429e4425ffSGustavo F. Padovan { 14323691d75SGustavo F. Padovan struct l2cap_chan *c; 1449e4425ffSGustavo F. Padovan 14523691d75SGustavo F. Padovan list_for_each_entry(c, &chan_list, global_l) { 14623691d75SGustavo F. Padovan if (c->sport == psm && !bacmp(&bt_sk(c->sk)->src, src)) 1479e4425ffSGustavo F. Padovan goto found; 1489e4425ffSGustavo F. Padovan } 1499e4425ffSGustavo F. Padovan 15023691d75SGustavo F. Padovan c = NULL; 1519e4425ffSGustavo F. Padovan found: 15223691d75SGustavo F. Padovan return c; 1539e4425ffSGustavo F. Padovan } 1549e4425ffSGustavo F. Padovan 1559e4425ffSGustavo F. Padovan int l2cap_add_psm(struct l2cap_chan *chan, bdaddr_t *src, __le16 psm) 1569e4425ffSGustavo F. Padovan { 15773b2ec18SGustavo F. Padovan int err; 15873b2ec18SGustavo F. Padovan 15923691d75SGustavo F. Padovan write_lock_bh(&chan_list_lock); 1609e4425ffSGustavo F. Padovan 16123691d75SGustavo F. Padovan if (psm && __l2cap_global_chan_by_addr(psm, src)) { 16273b2ec18SGustavo F. Padovan err = -EADDRINUSE; 16373b2ec18SGustavo F. Padovan goto done; 1649e4425ffSGustavo F. Padovan } 1659e4425ffSGustavo F. Padovan 16673b2ec18SGustavo F. Padovan if (psm) { 1679e4425ffSGustavo F. Padovan chan->psm = psm; 1689e4425ffSGustavo F. Padovan chan->sport = psm; 16973b2ec18SGustavo F. Padovan err = 0; 17073b2ec18SGustavo F. Padovan } else { 17173b2ec18SGustavo F. Padovan u16 p; 1729e4425ffSGustavo F. Padovan 17373b2ec18SGustavo F. Padovan err = -EINVAL; 17473b2ec18SGustavo F. Padovan for (p = 0x1001; p < 0x1100; p += 2) 17523691d75SGustavo F. Padovan if (!__l2cap_global_chan_by_addr(cpu_to_le16(p), src)) { 17673b2ec18SGustavo F. Padovan chan->psm = cpu_to_le16(p); 17773b2ec18SGustavo F. Padovan chan->sport = cpu_to_le16(p); 17873b2ec18SGustavo F. Padovan err = 0; 17973b2ec18SGustavo F. Padovan break; 18073b2ec18SGustavo F. Padovan } 18173b2ec18SGustavo F. Padovan } 18273b2ec18SGustavo F. Padovan 18373b2ec18SGustavo F. Padovan done: 18423691d75SGustavo F. Padovan write_unlock_bh(&chan_list_lock); 18573b2ec18SGustavo F. Padovan return err; 1869e4425ffSGustavo F. Padovan } 1879e4425ffSGustavo F. Padovan 1889e4425ffSGustavo F. Padovan int l2cap_add_scid(struct l2cap_chan *chan, __u16 scid) 1899e4425ffSGustavo F. Padovan { 19023691d75SGustavo F. Padovan write_lock_bh(&chan_list_lock); 1919e4425ffSGustavo F. Padovan 1929e4425ffSGustavo F. Padovan chan->scid = scid; 1939e4425ffSGustavo F. Padovan 19423691d75SGustavo F. Padovan write_unlock_bh(&chan_list_lock); 1959e4425ffSGustavo F. Padovan 1969e4425ffSGustavo F. Padovan return 0; 1979e4425ffSGustavo F. Padovan } 1989e4425ffSGustavo F. Padovan 199baa7e1faSGustavo F. Padovan static u16 l2cap_alloc_cid(struct l2cap_conn *conn) 2000a708f8fSGustavo F. Padovan { 2010a708f8fSGustavo F. Padovan u16 cid = L2CAP_CID_DYN_START; 2020a708f8fSGustavo F. Padovan 2030a708f8fSGustavo F. Padovan for (; cid < L2CAP_CID_DYN_END; cid++) { 204baa7e1faSGustavo F. Padovan if (!__l2cap_get_chan_by_scid(conn, cid)) 2050a708f8fSGustavo F. Padovan return cid; 2060a708f8fSGustavo F. Padovan } 2070a708f8fSGustavo F. Padovan 2080a708f8fSGustavo F. Padovan return 0; 2090a708f8fSGustavo F. Padovan } 2100a708f8fSGustavo F. Padovan 21123691d75SGustavo F. Padovan struct l2cap_chan *l2cap_chan_create(struct sock *sk) 2120a708f8fSGustavo F. Padovan { 21348454079SGustavo F. Padovan struct l2cap_chan *chan; 2140a708f8fSGustavo F. Padovan 21548454079SGustavo F. Padovan chan = kzalloc(sizeof(*chan), GFP_ATOMIC); 21648454079SGustavo F. Padovan if (!chan) 21748454079SGustavo F. Padovan return NULL; 2180a708f8fSGustavo F. Padovan 21948454079SGustavo F. Padovan chan->sk = sk; 22048454079SGustavo F. Padovan 22123691d75SGustavo F. Padovan write_lock_bh(&chan_list_lock); 22223691d75SGustavo F. Padovan list_add(&chan->global_l, &chan_list); 22323691d75SGustavo F. Padovan write_unlock_bh(&chan_list_lock); 22423691d75SGustavo F. Padovan 22548454079SGustavo F. Padovan return chan; 2260a708f8fSGustavo F. Padovan } 2270a708f8fSGustavo F. Padovan 22823691d75SGustavo F. Padovan void l2cap_chan_destroy(struct l2cap_chan *chan) 2296ff5abbfSGustavo F. Padovan { 23023691d75SGustavo F. Padovan write_lock_bh(&chan_list_lock); 23123691d75SGustavo F. Padovan list_del(&chan->global_l); 23223691d75SGustavo F. Padovan write_unlock_bh(&chan_list_lock); 23323691d75SGustavo F. Padovan 2346ff5abbfSGustavo F. Padovan kfree(chan); 2356ff5abbfSGustavo F. Padovan } 2366ff5abbfSGustavo F. Padovan 23748454079SGustavo F. Padovan static void __l2cap_chan_add(struct l2cap_conn *conn, struct l2cap_chan *chan) 2380a708f8fSGustavo F. Padovan { 23948454079SGustavo F. Padovan struct sock *sk = chan->sk; 2400a708f8fSGustavo F. Padovan 2410a708f8fSGustavo F. Padovan BT_DBG("conn %p, psm 0x%2.2x, dcid 0x%4.4x", conn, 242fe4128e0SGustavo F. Padovan chan->psm, chan->dcid); 2430a708f8fSGustavo F. Padovan 2440a708f8fSGustavo F. Padovan conn->disc_reason = 0x13; 2450a708f8fSGustavo F. Padovan 2468c1d787bSGustavo F. Padovan chan->conn = conn; 2470a708f8fSGustavo F. Padovan 2480a708f8fSGustavo F. Padovan if (sk->sk_type == SOCK_SEQPACKET || sk->sk_type == SOCK_STREAM) { 249b62f328bSVille Tervo if (conn->hcon->type == LE_LINK) { 250b62f328bSVille Tervo /* LE connection */ 2510c1bc5c6SGustavo F. Padovan chan->omtu = L2CAP_LE_DEFAULT_MTU; 252fe4128e0SGustavo F. Padovan chan->scid = L2CAP_CID_LE_DATA; 253fe4128e0SGustavo F. Padovan chan->dcid = L2CAP_CID_LE_DATA; 254b62f328bSVille Tervo } else { 2550a708f8fSGustavo F. Padovan /* Alloc CID for connection-oriented socket */ 256fe4128e0SGustavo F. Padovan chan->scid = l2cap_alloc_cid(conn); 2570c1bc5c6SGustavo F. Padovan chan->omtu = L2CAP_DEFAULT_MTU; 258b62f328bSVille Tervo } 2590a708f8fSGustavo F. Padovan } else if (sk->sk_type == SOCK_DGRAM) { 2600a708f8fSGustavo F. Padovan /* Connectionless socket */ 261fe4128e0SGustavo F. Padovan chan->scid = L2CAP_CID_CONN_LESS; 262fe4128e0SGustavo F. Padovan chan->dcid = L2CAP_CID_CONN_LESS; 2630c1bc5c6SGustavo F. Padovan chan->omtu = L2CAP_DEFAULT_MTU; 2640a708f8fSGustavo F. Padovan } else { 2650a708f8fSGustavo F. Padovan /* Raw socket can send/recv signalling messages only */ 266fe4128e0SGustavo F. Padovan chan->scid = L2CAP_CID_SIGNALING; 267fe4128e0SGustavo F. Padovan chan->dcid = L2CAP_CID_SIGNALING; 2680c1bc5c6SGustavo F. Padovan chan->omtu = L2CAP_DEFAULT_MTU; 2690a708f8fSGustavo F. Padovan } 2700a708f8fSGustavo F. Padovan 271baa7e1faSGustavo F. Padovan sock_hold(sk); 272baa7e1faSGustavo F. Padovan 273baa7e1faSGustavo F. Padovan list_add(&chan->list, &conn->chan_l); 2740a708f8fSGustavo F. Padovan } 2750a708f8fSGustavo F. Padovan 2760a708f8fSGustavo F. Padovan /* Delete channel. 2770a708f8fSGustavo F. Padovan * Must be called on the locked socket. */ 2784519de9aSGustavo F. Padovan static void l2cap_chan_del(struct l2cap_chan *chan, int err) 2790a708f8fSGustavo F. Padovan { 28048454079SGustavo F. Padovan struct sock *sk = chan->sk; 2818c1d787bSGustavo F. Padovan struct l2cap_conn *conn = chan->conn; 2820a708f8fSGustavo F. Padovan struct sock *parent = bt_sk(sk)->parent; 2830a708f8fSGustavo F. Padovan 2840a708f8fSGustavo F. Padovan l2cap_sock_clear_timer(sk); 2850a708f8fSGustavo F. Padovan 28649208c9cSGustavo F. Padovan BT_DBG("chan %p, conn %p, err %d", chan, conn, err); 2870a708f8fSGustavo F. Padovan 2880a708f8fSGustavo F. Padovan if (conn) { 289baa7e1faSGustavo F. Padovan /* Delete from channel list */ 290baa7e1faSGustavo F. Padovan write_lock_bh(&conn->chan_lock); 291baa7e1faSGustavo F. Padovan list_del(&chan->list); 292baa7e1faSGustavo F. Padovan write_unlock_bh(&conn->chan_lock); 293baa7e1faSGustavo F. Padovan __sock_put(sk); 294baa7e1faSGustavo F. Padovan 2958c1d787bSGustavo F. Padovan chan->conn = NULL; 2960a708f8fSGustavo F. Padovan hci_conn_put(conn->hcon); 2970a708f8fSGustavo F. Padovan } 2980a708f8fSGustavo F. Padovan 2990a708f8fSGustavo F. Padovan sk->sk_state = BT_CLOSED; 3000a708f8fSGustavo F. Padovan sock_set_flag(sk, SOCK_ZAPPED); 3010a708f8fSGustavo F. Padovan 3020a708f8fSGustavo F. Padovan if (err) 3030a708f8fSGustavo F. Padovan sk->sk_err = err; 3040a708f8fSGustavo F. Padovan 3050a708f8fSGustavo F. Padovan if (parent) { 3060a708f8fSGustavo F. Padovan bt_accept_unlink(sk); 3070a708f8fSGustavo F. Padovan parent->sk_data_ready(parent, 0); 3080a708f8fSGustavo F. Padovan } else 3090a708f8fSGustavo F. Padovan sk->sk_state_change(sk); 3100a708f8fSGustavo F. Padovan 311b4450035SGustavo F. Padovan if (!(chan->conf_state & L2CAP_CONF_OUTPUT_DONE && 312b4450035SGustavo F. Padovan chan->conf_state & L2CAP_CONF_INPUT_DONE)) 3136ff5abbfSGustavo F. Padovan return; 3142ead70b8SGustavo F. Padovan 31558d35f87SGustavo F. Padovan skb_queue_purge(&chan->tx_q); 3160a708f8fSGustavo F. Padovan 3170c1bc5c6SGustavo F. Padovan if (chan->mode == L2CAP_MODE_ERTM) { 3180a708f8fSGustavo F. Padovan struct srej_list *l, *tmp; 3190a708f8fSGustavo F. Padovan 320e92c8e70SGustavo F. Padovan del_timer(&chan->retrans_timer); 321e92c8e70SGustavo F. Padovan del_timer(&chan->monitor_timer); 322e92c8e70SGustavo F. Padovan del_timer(&chan->ack_timer); 3230a708f8fSGustavo F. Padovan 324f1c6775bSGustavo F. Padovan skb_queue_purge(&chan->srej_q); 325f1c6775bSGustavo F. Padovan skb_queue_purge(&chan->busy_q); 3260a708f8fSGustavo F. Padovan 32739d5a3eeSGustavo F. Padovan list_for_each_entry_safe(l, tmp, &chan->srej_l, list) { 3280a708f8fSGustavo F. Padovan list_del(&l->list); 3290a708f8fSGustavo F. Padovan kfree(l); 3300a708f8fSGustavo F. Padovan } 3310a708f8fSGustavo F. Padovan } 3320a708f8fSGustavo F. Padovan } 3330a708f8fSGustavo F. Padovan 3344519de9aSGustavo F. Padovan /* Must be called on unlocked socket. */ 3354519de9aSGustavo F. Padovan static void l2cap_chan_close(struct sock *sk) 3364519de9aSGustavo F. Padovan { 3374519de9aSGustavo F. Padovan l2cap_sock_clear_timer(sk); 3384519de9aSGustavo F. Padovan lock_sock(sk); 3394519de9aSGustavo F. Padovan __l2cap_chan_close(l2cap_pi(sk)->chan, ECONNRESET); 3404519de9aSGustavo F. Padovan release_sock(sk); 3414519de9aSGustavo F. Padovan l2cap_sock_kill(sk); 3424519de9aSGustavo F. Padovan } 3434519de9aSGustavo F. Padovan 3444519de9aSGustavo F. Padovan static void l2cap_chan_cleanup_listen(struct sock *parent) 3454519de9aSGustavo F. Padovan { 3464519de9aSGustavo F. Padovan struct sock *sk; 3474519de9aSGustavo F. Padovan 3484519de9aSGustavo F. Padovan BT_DBG("parent %p", parent); 3494519de9aSGustavo F. Padovan 3504519de9aSGustavo F. Padovan /* Close not yet accepted channels */ 3514519de9aSGustavo F. Padovan while ((sk = bt_accept_dequeue(parent, NULL))) 3524519de9aSGustavo F. Padovan l2cap_chan_close(sk); 3534519de9aSGustavo F. Padovan 3544519de9aSGustavo F. Padovan parent->sk_state = BT_CLOSED; 3554519de9aSGustavo F. Padovan sock_set_flag(parent, SOCK_ZAPPED); 3564519de9aSGustavo F. Padovan } 3574519de9aSGustavo F. Padovan 3584519de9aSGustavo F. Padovan void __l2cap_chan_close(struct l2cap_chan *chan, int reason) 3594519de9aSGustavo F. Padovan { 3604519de9aSGustavo F. Padovan struct l2cap_conn *conn = chan->conn; 3614519de9aSGustavo F. Padovan struct sock *sk = chan->sk; 3624519de9aSGustavo F. Padovan 3634519de9aSGustavo F. Padovan BT_DBG("chan %p state %d socket %p", chan, sk->sk_state, sk->sk_socket); 3644519de9aSGustavo F. Padovan 3654519de9aSGustavo F. Padovan switch (sk->sk_state) { 3664519de9aSGustavo F. Padovan case BT_LISTEN: 3674519de9aSGustavo F. Padovan l2cap_chan_cleanup_listen(sk); 3684519de9aSGustavo F. Padovan break; 3694519de9aSGustavo F. Padovan 3704519de9aSGustavo F. Padovan case BT_CONNECTED: 3714519de9aSGustavo F. Padovan case BT_CONFIG: 3724519de9aSGustavo F. Padovan if ((sk->sk_type == SOCK_SEQPACKET || 3734519de9aSGustavo F. Padovan sk->sk_type == SOCK_STREAM) && 3744519de9aSGustavo F. Padovan conn->hcon->type == ACL_LINK) { 3754519de9aSGustavo F. Padovan l2cap_sock_set_timer(sk, sk->sk_sndtimeo); 3764519de9aSGustavo F. Padovan l2cap_send_disconn_req(conn, chan, reason); 3774519de9aSGustavo F. Padovan } else 3784519de9aSGustavo F. Padovan l2cap_chan_del(chan, reason); 3794519de9aSGustavo F. Padovan break; 3804519de9aSGustavo F. Padovan 3814519de9aSGustavo F. Padovan case BT_CONNECT2: 3824519de9aSGustavo F. Padovan if ((sk->sk_type == SOCK_SEQPACKET || 3834519de9aSGustavo F. Padovan sk->sk_type == SOCK_STREAM) && 3844519de9aSGustavo F. Padovan conn->hcon->type == ACL_LINK) { 3854519de9aSGustavo F. Padovan struct l2cap_conn_rsp rsp; 3864519de9aSGustavo F. Padovan __u16 result; 3874519de9aSGustavo F. Padovan 3884519de9aSGustavo F. Padovan if (bt_sk(sk)->defer_setup) 3894519de9aSGustavo F. Padovan result = L2CAP_CR_SEC_BLOCK; 3904519de9aSGustavo F. Padovan else 3914519de9aSGustavo F. Padovan result = L2CAP_CR_BAD_PSM; 3924519de9aSGustavo F. Padovan 3934519de9aSGustavo F. Padovan rsp.scid = cpu_to_le16(chan->dcid); 3944519de9aSGustavo F. Padovan rsp.dcid = cpu_to_le16(chan->scid); 3954519de9aSGustavo F. Padovan rsp.result = cpu_to_le16(result); 3964519de9aSGustavo F. Padovan rsp.status = cpu_to_le16(L2CAP_CS_NO_INFO); 3974519de9aSGustavo F. Padovan l2cap_send_cmd(conn, chan->ident, L2CAP_CONN_RSP, 3984519de9aSGustavo F. Padovan sizeof(rsp), &rsp); 3994519de9aSGustavo F. Padovan } 4004519de9aSGustavo F. Padovan 4014519de9aSGustavo F. Padovan l2cap_chan_del(chan, reason); 4024519de9aSGustavo F. Padovan break; 4034519de9aSGustavo F. Padovan 4044519de9aSGustavo F. Padovan case BT_CONNECT: 4054519de9aSGustavo F. Padovan case BT_DISCONN: 4064519de9aSGustavo F. Padovan l2cap_chan_del(chan, reason); 4074519de9aSGustavo F. Padovan break; 4084519de9aSGustavo F. Padovan 4094519de9aSGustavo F. Padovan default: 4104519de9aSGustavo F. Padovan sock_set_flag(sk, SOCK_ZAPPED); 4114519de9aSGustavo F. Padovan break; 4124519de9aSGustavo F. Padovan } 4134519de9aSGustavo F. Padovan } 4144519de9aSGustavo F. Padovan 4154343478fSGustavo F. Padovan static inline u8 l2cap_get_auth_type(struct l2cap_chan *chan) 4160a708f8fSGustavo F. Padovan { 4174343478fSGustavo F. Padovan struct sock *sk = chan->sk; 4184343478fSGustavo F. Padovan 4190a708f8fSGustavo F. Padovan if (sk->sk_type == SOCK_RAW) { 4204343478fSGustavo F. Padovan switch (chan->sec_level) { 4210a708f8fSGustavo F. Padovan case BT_SECURITY_HIGH: 4220a708f8fSGustavo F. Padovan return HCI_AT_DEDICATED_BONDING_MITM; 4230a708f8fSGustavo F. Padovan case BT_SECURITY_MEDIUM: 4240a708f8fSGustavo F. Padovan return HCI_AT_DEDICATED_BONDING; 4250a708f8fSGustavo F. Padovan default: 4260a708f8fSGustavo F. Padovan return HCI_AT_NO_BONDING; 4270a708f8fSGustavo F. Padovan } 428fe4128e0SGustavo F. Padovan } else if (chan->psm == cpu_to_le16(0x0001)) { 4294343478fSGustavo F. Padovan if (chan->sec_level == BT_SECURITY_LOW) 4304343478fSGustavo F. Padovan chan->sec_level = BT_SECURITY_SDP; 4310a708f8fSGustavo F. Padovan 4324343478fSGustavo F. Padovan if (chan->sec_level == BT_SECURITY_HIGH) 4330a708f8fSGustavo F. Padovan return HCI_AT_NO_BONDING_MITM; 4340a708f8fSGustavo F. Padovan else 4350a708f8fSGustavo F. Padovan return HCI_AT_NO_BONDING; 4360a708f8fSGustavo F. Padovan } else { 4374343478fSGustavo F. Padovan switch (chan->sec_level) { 4380a708f8fSGustavo F. Padovan case BT_SECURITY_HIGH: 4390a708f8fSGustavo F. Padovan return HCI_AT_GENERAL_BONDING_MITM; 4400a708f8fSGustavo F. Padovan case BT_SECURITY_MEDIUM: 4410a708f8fSGustavo F. Padovan return HCI_AT_GENERAL_BONDING; 4420a708f8fSGustavo F. Padovan default: 4430a708f8fSGustavo F. Padovan return HCI_AT_NO_BONDING; 4440a708f8fSGustavo F. Padovan } 4450a708f8fSGustavo F. Padovan } 4460a708f8fSGustavo F. Padovan } 4470a708f8fSGustavo F. Padovan 4480a708f8fSGustavo F. Padovan /* Service level security */ 4494343478fSGustavo F. Padovan static inline int l2cap_check_security(struct l2cap_chan *chan) 4500a708f8fSGustavo F. Padovan { 4518c1d787bSGustavo F. Padovan struct l2cap_conn *conn = chan->conn; 4520a708f8fSGustavo F. Padovan __u8 auth_type; 4530a708f8fSGustavo F. Padovan 4544343478fSGustavo F. Padovan auth_type = l2cap_get_auth_type(chan); 4550a708f8fSGustavo F. Padovan 4564343478fSGustavo F. Padovan return hci_conn_security(conn->hcon, chan->sec_level, auth_type); 4570a708f8fSGustavo F. Padovan } 4580a708f8fSGustavo F. Padovan 45968983259SGustavo F. Padovan u8 l2cap_get_ident(struct l2cap_conn *conn) 4600a708f8fSGustavo F. Padovan { 4610a708f8fSGustavo F. Padovan u8 id; 4620a708f8fSGustavo F. Padovan 4630a708f8fSGustavo F. Padovan /* Get next available identificator. 4640a708f8fSGustavo F. Padovan * 1 - 128 are used by kernel. 4650a708f8fSGustavo F. Padovan * 129 - 199 are reserved. 4660a708f8fSGustavo F. Padovan * 200 - 254 are used by utilities like l2ping, etc. 4670a708f8fSGustavo F. Padovan */ 4680a708f8fSGustavo F. Padovan 4690a708f8fSGustavo F. Padovan spin_lock_bh(&conn->lock); 4700a708f8fSGustavo F. Padovan 4710a708f8fSGustavo F. Padovan if (++conn->tx_ident > 128) 4720a708f8fSGustavo F. Padovan conn->tx_ident = 1; 4730a708f8fSGustavo F. Padovan 4740a708f8fSGustavo F. Padovan id = conn->tx_ident; 4750a708f8fSGustavo F. Padovan 4760a708f8fSGustavo F. Padovan spin_unlock_bh(&conn->lock); 4770a708f8fSGustavo F. Padovan 4780a708f8fSGustavo F. Padovan return id; 4790a708f8fSGustavo F. Padovan } 4800a708f8fSGustavo F. Padovan 4814519de9aSGustavo F. Padovan static void l2cap_send_cmd(struct l2cap_conn *conn, u8 ident, u8 code, u16 len, void *data) 4820a708f8fSGustavo F. Padovan { 4830a708f8fSGustavo F. Padovan struct sk_buff *skb = l2cap_build_cmd(conn, code, ident, len, data); 4840a708f8fSGustavo F. Padovan u8 flags; 4850a708f8fSGustavo F. Padovan 4860a708f8fSGustavo F. Padovan BT_DBG("code 0x%2.2x", code); 4870a708f8fSGustavo F. Padovan 4880a708f8fSGustavo F. Padovan if (!skb) 4890a708f8fSGustavo F. Padovan return; 4900a708f8fSGustavo F. Padovan 4910a708f8fSGustavo F. Padovan if (lmp_no_flush_capable(conn->hcon->hdev)) 4920a708f8fSGustavo F. Padovan flags = ACL_START_NO_FLUSH; 4930a708f8fSGustavo F. Padovan else 4940a708f8fSGustavo F. Padovan flags = ACL_START; 4950a708f8fSGustavo F. Padovan 4960a708f8fSGustavo F. Padovan hci_send_acl(conn->hcon, skb, flags); 4970a708f8fSGustavo F. Padovan } 4980a708f8fSGustavo F. Padovan 499525cd185SGustavo F. Padovan static inline void l2cap_send_sframe(struct l2cap_chan *chan, u16 control) 5000a708f8fSGustavo F. Padovan { 5010a708f8fSGustavo F. Padovan struct sk_buff *skb; 5020a708f8fSGustavo F. Padovan struct l2cap_hdr *lh; 503525cd185SGustavo F. Padovan struct l2cap_pinfo *pi = l2cap_pi(chan->sk); 5048c1d787bSGustavo F. Padovan struct l2cap_conn *conn = chan->conn; 5050a708f8fSGustavo F. Padovan struct sock *sk = (struct sock *)pi; 5060a708f8fSGustavo F. Padovan int count, hlen = L2CAP_HDR_SIZE + 2; 5070a708f8fSGustavo F. Padovan u8 flags; 5080a708f8fSGustavo F. Padovan 5090a708f8fSGustavo F. Padovan if (sk->sk_state != BT_CONNECTED) 5100a708f8fSGustavo F. Padovan return; 5110a708f8fSGustavo F. Padovan 51247d1ec61SGustavo F. Padovan if (chan->fcs == L2CAP_FCS_CRC16) 5130a708f8fSGustavo F. Padovan hlen += 2; 5140a708f8fSGustavo F. Padovan 51549208c9cSGustavo F. Padovan BT_DBG("chan %p, control 0x%2.2x", chan, control); 5160a708f8fSGustavo F. Padovan 5170a708f8fSGustavo F. Padovan count = min_t(unsigned int, conn->mtu, hlen); 5180a708f8fSGustavo F. Padovan control |= L2CAP_CTRL_FRAME_TYPE; 5190a708f8fSGustavo F. Padovan 520525cd185SGustavo F. Padovan if (chan->conn_state & L2CAP_CONN_SEND_FBIT) { 5210a708f8fSGustavo F. Padovan control |= L2CAP_CTRL_FINAL; 522525cd185SGustavo F. Padovan chan->conn_state &= ~L2CAP_CONN_SEND_FBIT; 5230a708f8fSGustavo F. Padovan } 5240a708f8fSGustavo F. Padovan 525525cd185SGustavo F. Padovan if (chan->conn_state & L2CAP_CONN_SEND_PBIT) { 5260a708f8fSGustavo F. Padovan control |= L2CAP_CTRL_POLL; 527525cd185SGustavo F. Padovan chan->conn_state &= ~L2CAP_CONN_SEND_PBIT; 5280a708f8fSGustavo F. Padovan } 5290a708f8fSGustavo F. Padovan 5300a708f8fSGustavo F. Padovan skb = bt_skb_alloc(count, GFP_ATOMIC); 5310a708f8fSGustavo F. Padovan if (!skb) 5320a708f8fSGustavo F. Padovan return; 5330a708f8fSGustavo F. Padovan 5340a708f8fSGustavo F. Padovan lh = (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE); 5350a708f8fSGustavo F. Padovan lh->len = cpu_to_le16(hlen - L2CAP_HDR_SIZE); 536fe4128e0SGustavo F. Padovan lh->cid = cpu_to_le16(chan->dcid); 5370a708f8fSGustavo F. Padovan put_unaligned_le16(control, skb_put(skb, 2)); 5380a708f8fSGustavo F. Padovan 53947d1ec61SGustavo F. Padovan if (chan->fcs == L2CAP_FCS_CRC16) { 5400a708f8fSGustavo F. Padovan u16 fcs = crc16(0, (u8 *)lh, count - 2); 5410a708f8fSGustavo F. Padovan put_unaligned_le16(fcs, skb_put(skb, 2)); 5420a708f8fSGustavo F. Padovan } 5430a708f8fSGustavo F. Padovan 5440a708f8fSGustavo F. Padovan if (lmp_no_flush_capable(conn->hcon->hdev)) 5450a708f8fSGustavo F. Padovan flags = ACL_START_NO_FLUSH; 5460a708f8fSGustavo F. Padovan else 5470a708f8fSGustavo F. Padovan flags = ACL_START; 5480a708f8fSGustavo F. Padovan 5498c1d787bSGustavo F. Padovan hci_send_acl(chan->conn->hcon, skb, flags); 5500a708f8fSGustavo F. Padovan } 5510a708f8fSGustavo F. Padovan 552525cd185SGustavo F. Padovan static inline void l2cap_send_rr_or_rnr(struct l2cap_chan *chan, u16 control) 5530a708f8fSGustavo F. Padovan { 554525cd185SGustavo F. Padovan if (chan->conn_state & L2CAP_CONN_LOCAL_BUSY) { 5550a708f8fSGustavo F. Padovan control |= L2CAP_SUPER_RCV_NOT_READY; 556525cd185SGustavo F. Padovan chan->conn_state |= L2CAP_CONN_RNR_SENT; 5570a708f8fSGustavo F. Padovan } else 5580a708f8fSGustavo F. Padovan control |= L2CAP_SUPER_RCV_READY; 5590a708f8fSGustavo F. Padovan 56042e5c802SGustavo F. Padovan control |= chan->buffer_seq << L2CAP_CTRL_REQSEQ_SHIFT; 5610a708f8fSGustavo F. Padovan 562525cd185SGustavo F. Padovan l2cap_send_sframe(chan, control); 5630a708f8fSGustavo F. Padovan } 5640a708f8fSGustavo F. Padovan 565b4450035SGustavo F. Padovan static inline int __l2cap_no_conn_pending(struct l2cap_chan *chan) 5660a708f8fSGustavo F. Padovan { 567b4450035SGustavo F. Padovan return !(chan->conf_state & L2CAP_CONF_CONNECT_PEND); 5680a708f8fSGustavo F. Padovan } 5690a708f8fSGustavo F. Padovan 570fc7f8a7eSGustavo F. Padovan static void l2cap_do_start(struct l2cap_chan *chan) 5710a708f8fSGustavo F. Padovan { 5728c1d787bSGustavo F. Padovan struct l2cap_conn *conn = chan->conn; 5730a708f8fSGustavo F. Padovan 5740a708f8fSGustavo F. Padovan if (conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_SENT) { 5750a708f8fSGustavo F. Padovan if (!(conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_DONE)) 5760a708f8fSGustavo F. Padovan return; 5770a708f8fSGustavo F. Padovan 5784343478fSGustavo F. Padovan if (l2cap_check_security(chan) && 5794343478fSGustavo F. Padovan __l2cap_no_conn_pending(chan)) { 5800a708f8fSGustavo F. Padovan struct l2cap_conn_req req; 581fe4128e0SGustavo F. Padovan req.scid = cpu_to_le16(chan->scid); 582fe4128e0SGustavo F. Padovan req.psm = chan->psm; 5830a708f8fSGustavo F. Padovan 584fc7f8a7eSGustavo F. Padovan chan->ident = l2cap_get_ident(conn); 585b4450035SGustavo F. Padovan chan->conf_state |= L2CAP_CONF_CONNECT_PEND; 5860a708f8fSGustavo F. Padovan 587fc7f8a7eSGustavo F. Padovan l2cap_send_cmd(conn, chan->ident, L2CAP_CONN_REQ, 588fc7f8a7eSGustavo F. Padovan sizeof(req), &req); 5890a708f8fSGustavo F. Padovan } 5900a708f8fSGustavo F. Padovan } else { 5910a708f8fSGustavo F. Padovan struct l2cap_info_req req; 5920a708f8fSGustavo F. Padovan req.type = cpu_to_le16(L2CAP_IT_FEAT_MASK); 5930a708f8fSGustavo F. Padovan 5940a708f8fSGustavo F. Padovan conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_SENT; 5950a708f8fSGustavo F. Padovan conn->info_ident = l2cap_get_ident(conn); 5960a708f8fSGustavo F. Padovan 5970a708f8fSGustavo F. Padovan mod_timer(&conn->info_timer, jiffies + 5980a708f8fSGustavo F. Padovan msecs_to_jiffies(L2CAP_INFO_TIMEOUT)); 5990a708f8fSGustavo F. Padovan 6000a708f8fSGustavo F. Padovan l2cap_send_cmd(conn, conn->info_ident, 6010a708f8fSGustavo F. Padovan L2CAP_INFO_REQ, sizeof(req), &req); 6020a708f8fSGustavo F. Padovan } 6030a708f8fSGustavo F. Padovan } 6040a708f8fSGustavo F. Padovan 6050a708f8fSGustavo F. Padovan static inline int l2cap_mode_supported(__u8 mode, __u32 feat_mask) 6060a708f8fSGustavo F. Padovan { 6070a708f8fSGustavo F. Padovan u32 local_feat_mask = l2cap_feat_mask; 6080a708f8fSGustavo F. Padovan if (!disable_ertm) 6090a708f8fSGustavo F. Padovan local_feat_mask |= L2CAP_FEAT_ERTM | L2CAP_FEAT_STREAMING; 6100a708f8fSGustavo F. Padovan 6110a708f8fSGustavo F. Padovan switch (mode) { 6120a708f8fSGustavo F. Padovan case L2CAP_MODE_ERTM: 6130a708f8fSGustavo F. Padovan return L2CAP_FEAT_ERTM & feat_mask & local_feat_mask; 6140a708f8fSGustavo F. Padovan case L2CAP_MODE_STREAMING: 6150a708f8fSGustavo F. Padovan return L2CAP_FEAT_STREAMING & feat_mask & local_feat_mask; 6160a708f8fSGustavo F. Padovan default: 6170a708f8fSGustavo F. Padovan return 0x00; 6180a708f8fSGustavo F. Padovan } 6190a708f8fSGustavo F. Padovan } 6200a708f8fSGustavo F. Padovan 6214519de9aSGustavo F. Padovan static void l2cap_send_disconn_req(struct l2cap_conn *conn, struct l2cap_chan *chan, int err) 6220a708f8fSGustavo F. Padovan { 623e92c8e70SGustavo F. Padovan struct sock *sk; 6240a708f8fSGustavo F. Padovan struct l2cap_disconn_req req; 6250a708f8fSGustavo F. Padovan 6260a708f8fSGustavo F. Padovan if (!conn) 6270a708f8fSGustavo F. Padovan return; 6280a708f8fSGustavo F. Padovan 629e92c8e70SGustavo F. Padovan sk = chan->sk; 630e92c8e70SGustavo F. Padovan 6310c1bc5c6SGustavo F. Padovan if (chan->mode == L2CAP_MODE_ERTM) { 632e92c8e70SGustavo F. Padovan del_timer(&chan->retrans_timer); 633e92c8e70SGustavo F. Padovan del_timer(&chan->monitor_timer); 634e92c8e70SGustavo F. Padovan del_timer(&chan->ack_timer); 6350a708f8fSGustavo F. Padovan } 6360a708f8fSGustavo F. Padovan 637fe4128e0SGustavo F. Padovan req.dcid = cpu_to_le16(chan->dcid); 638fe4128e0SGustavo F. Padovan req.scid = cpu_to_le16(chan->scid); 6390a708f8fSGustavo F. Padovan l2cap_send_cmd(conn, l2cap_get_ident(conn), 6400a708f8fSGustavo F. Padovan L2CAP_DISCONN_REQ, sizeof(req), &req); 6410a708f8fSGustavo F. Padovan 6420a708f8fSGustavo F. Padovan sk->sk_state = BT_DISCONN; 6430a708f8fSGustavo F. Padovan sk->sk_err = err; 6440a708f8fSGustavo F. Padovan } 6450a708f8fSGustavo F. Padovan 6460a708f8fSGustavo F. Padovan /* ---- L2CAP connections ---- */ 6470a708f8fSGustavo F. Padovan static void l2cap_conn_start(struct l2cap_conn *conn) 6480a708f8fSGustavo F. Padovan { 649820ffdb3SGustavo F. Padovan struct l2cap_chan *chan, *tmp; 6500a708f8fSGustavo F. Padovan 6510a708f8fSGustavo F. Padovan BT_DBG("conn %p", conn); 6520a708f8fSGustavo F. Padovan 653baa7e1faSGustavo F. Padovan read_lock(&conn->chan_lock); 6540a708f8fSGustavo F. Padovan 655820ffdb3SGustavo F. Padovan list_for_each_entry_safe(chan, tmp, &conn->chan_l, list) { 65648454079SGustavo F. Padovan struct sock *sk = chan->sk; 657baa7e1faSGustavo F. Padovan 6580a708f8fSGustavo F. Padovan bh_lock_sock(sk); 6590a708f8fSGustavo F. Padovan 6600a708f8fSGustavo F. Padovan if (sk->sk_type != SOCK_SEQPACKET && 6610a708f8fSGustavo F. Padovan sk->sk_type != SOCK_STREAM) { 6620a708f8fSGustavo F. Padovan bh_unlock_sock(sk); 6630a708f8fSGustavo F. Padovan continue; 6640a708f8fSGustavo F. Padovan } 6650a708f8fSGustavo F. Padovan 6660a708f8fSGustavo F. Padovan if (sk->sk_state == BT_CONNECT) { 6670a708f8fSGustavo F. Padovan struct l2cap_conn_req req; 6680a708f8fSGustavo F. Padovan 6694343478fSGustavo F. Padovan if (!l2cap_check_security(chan) || 670b4450035SGustavo F. Padovan !__l2cap_no_conn_pending(chan)) { 6710a708f8fSGustavo F. Padovan bh_unlock_sock(sk); 6720a708f8fSGustavo F. Padovan continue; 6730a708f8fSGustavo F. Padovan } 6740a708f8fSGustavo F. Padovan 6750c1bc5c6SGustavo F. Padovan if (!l2cap_mode_supported(chan->mode, 6760a708f8fSGustavo F. Padovan conn->feat_mask) 677b4450035SGustavo F. Padovan && chan->conf_state & 6780a708f8fSGustavo F. Padovan L2CAP_CONF_STATE2_DEVICE) { 6794519de9aSGustavo F. Padovan /* __l2cap_chan_close() calls list_del(chan) 680820ffdb3SGustavo F. Padovan * so release the lock */ 681820ffdb3SGustavo F. Padovan read_unlock_bh(&conn->chan_lock); 6824519de9aSGustavo F. Padovan __l2cap_chan_close(chan, ECONNRESET); 683820ffdb3SGustavo F. Padovan read_lock_bh(&conn->chan_lock); 6840a708f8fSGustavo F. Padovan bh_unlock_sock(sk); 6850a708f8fSGustavo F. Padovan continue; 6860a708f8fSGustavo F. Padovan } 6870a708f8fSGustavo F. Padovan 688fe4128e0SGustavo F. Padovan req.scid = cpu_to_le16(chan->scid); 689fe4128e0SGustavo F. Padovan req.psm = chan->psm; 6900a708f8fSGustavo F. Padovan 691fc7f8a7eSGustavo F. Padovan chan->ident = l2cap_get_ident(conn); 692b4450035SGustavo F. Padovan chan->conf_state |= L2CAP_CONF_CONNECT_PEND; 6930a708f8fSGustavo F. Padovan 694fc7f8a7eSGustavo F. Padovan l2cap_send_cmd(conn, chan->ident, L2CAP_CONN_REQ, 695fc7f8a7eSGustavo F. Padovan sizeof(req), &req); 6960a708f8fSGustavo F. Padovan 6970a708f8fSGustavo F. Padovan } else if (sk->sk_state == BT_CONNECT2) { 6980a708f8fSGustavo F. Padovan struct l2cap_conn_rsp rsp; 6990a708f8fSGustavo F. Padovan char buf[128]; 700fe4128e0SGustavo F. Padovan rsp.scid = cpu_to_le16(chan->dcid); 701fe4128e0SGustavo F. Padovan rsp.dcid = cpu_to_le16(chan->scid); 7020a708f8fSGustavo F. Padovan 7034343478fSGustavo F. Padovan if (l2cap_check_security(chan)) { 7040a708f8fSGustavo F. Padovan if (bt_sk(sk)->defer_setup) { 7050a708f8fSGustavo F. Padovan struct sock *parent = bt_sk(sk)->parent; 7060a708f8fSGustavo F. Padovan rsp.result = cpu_to_le16(L2CAP_CR_PEND); 7070a708f8fSGustavo F. Padovan rsp.status = cpu_to_le16(L2CAP_CS_AUTHOR_PEND); 7080a708f8fSGustavo F. Padovan parent->sk_data_ready(parent, 0); 7090a708f8fSGustavo F. Padovan 7100a708f8fSGustavo F. Padovan } else { 7110a708f8fSGustavo F. Padovan sk->sk_state = BT_CONFIG; 7120a708f8fSGustavo F. Padovan rsp.result = cpu_to_le16(L2CAP_CR_SUCCESS); 7130a708f8fSGustavo F. Padovan rsp.status = cpu_to_le16(L2CAP_CS_NO_INFO); 7140a708f8fSGustavo F. Padovan } 7150a708f8fSGustavo F. Padovan } else { 7160a708f8fSGustavo F. Padovan rsp.result = cpu_to_le16(L2CAP_CR_PEND); 7170a708f8fSGustavo F. Padovan rsp.status = cpu_to_le16(L2CAP_CS_AUTHEN_PEND); 7180a708f8fSGustavo F. Padovan } 7190a708f8fSGustavo F. Padovan 720fc7f8a7eSGustavo F. Padovan l2cap_send_cmd(conn, chan->ident, L2CAP_CONN_RSP, 721fc7f8a7eSGustavo F. Padovan sizeof(rsp), &rsp); 7220a708f8fSGustavo F. Padovan 723b4450035SGustavo F. Padovan if (chan->conf_state & L2CAP_CONF_REQ_SENT || 7240a708f8fSGustavo F. Padovan rsp.result != L2CAP_CR_SUCCESS) { 7250a708f8fSGustavo F. Padovan bh_unlock_sock(sk); 7260a708f8fSGustavo F. Padovan continue; 7270a708f8fSGustavo F. Padovan } 7280a708f8fSGustavo F. Padovan 729b4450035SGustavo F. Padovan chan->conf_state |= L2CAP_CONF_REQ_SENT; 7300a708f8fSGustavo F. Padovan l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ, 73173ffa904SGustavo F. Padovan l2cap_build_conf_req(chan, buf), buf); 73273ffa904SGustavo F. Padovan chan->num_conf_req++; 7330a708f8fSGustavo F. Padovan } 7340a708f8fSGustavo F. Padovan 7350a708f8fSGustavo F. Padovan bh_unlock_sock(sk); 7360a708f8fSGustavo F. Padovan } 7370a708f8fSGustavo F. Padovan 738baa7e1faSGustavo F. Padovan read_unlock(&conn->chan_lock); 7390a708f8fSGustavo F. Padovan } 7400a708f8fSGustavo F. Padovan 741b62f328bSVille Tervo /* Find socket with cid and source bdaddr. 742b62f328bSVille Tervo * Returns closest match, locked. 743b62f328bSVille Tervo */ 74423691d75SGustavo F. Padovan static struct l2cap_chan *l2cap_global_chan_by_scid(int state, __le16 cid, bdaddr_t *src) 745b62f328bSVille Tervo { 74623691d75SGustavo F. Padovan struct l2cap_chan *c, *c1 = NULL; 747b62f328bSVille Tervo 74823691d75SGustavo F. Padovan read_lock(&chan_list_lock); 749b62f328bSVille Tervo 75023691d75SGustavo F. Padovan list_for_each_entry(c, &chan_list, global_l) { 75123691d75SGustavo F. Padovan struct sock *sk = c->sk; 752fe4128e0SGustavo F. Padovan 753b62f328bSVille Tervo if (state && sk->sk_state != state) 754b62f328bSVille Tervo continue; 755b62f328bSVille Tervo 75623691d75SGustavo F. Padovan if (c->scid == cid) { 757b62f328bSVille Tervo /* Exact match. */ 75823691d75SGustavo F. Padovan if (!bacmp(&bt_sk(sk)->src, src)) { 75923691d75SGustavo F. Padovan read_unlock(&chan_list_lock); 76023691d75SGustavo F. Padovan return c; 76123691d75SGustavo F. Padovan } 762b62f328bSVille Tervo 763b62f328bSVille Tervo /* Closest match */ 764b62f328bSVille Tervo if (!bacmp(&bt_sk(sk)->src, BDADDR_ANY)) 76523691d75SGustavo F. Padovan c1 = c; 766b62f328bSVille Tervo } 767b62f328bSVille Tervo } 768280f294fSGustavo F. Padovan 76923691d75SGustavo F. Padovan read_unlock(&chan_list_lock); 770b62f328bSVille Tervo 77123691d75SGustavo F. Padovan return c1; 772b62f328bSVille Tervo } 773b62f328bSVille Tervo 774b62f328bSVille Tervo static void l2cap_le_conn_ready(struct l2cap_conn *conn) 775b62f328bSVille Tervo { 776c916fbe4SGustavo F. Padovan struct sock *parent, *sk; 77723691d75SGustavo F. Padovan struct l2cap_chan *chan, *pchan; 778b62f328bSVille Tervo 779b62f328bSVille Tervo BT_DBG(""); 780b62f328bSVille Tervo 781b62f328bSVille Tervo /* Check if we have socket listening on cid */ 78223691d75SGustavo F. Padovan pchan = l2cap_global_chan_by_scid(BT_LISTEN, L2CAP_CID_LE_DATA, 783b62f328bSVille Tervo conn->src); 78423691d75SGustavo F. Padovan if (!pchan) 785b62f328bSVille Tervo return; 786b62f328bSVille Tervo 78723691d75SGustavo F. Padovan parent = pchan->sk; 78823691d75SGustavo F. Padovan 78962f3a2cfSGustavo F. Padovan bh_lock_sock(parent); 79062f3a2cfSGustavo F. Padovan 791b62f328bSVille Tervo /* Check for backlog size */ 792b62f328bSVille Tervo if (sk_acceptq_is_full(parent)) { 793b62f328bSVille Tervo BT_DBG("backlog full %d", parent->sk_ack_backlog); 794b62f328bSVille Tervo goto clean; 795b62f328bSVille Tervo } 796b62f328bSVille Tervo 797b62f328bSVille Tervo sk = l2cap_sock_alloc(sock_net(parent), NULL, BTPROTO_L2CAP, GFP_ATOMIC); 798b62f328bSVille Tervo if (!sk) 799b62f328bSVille Tervo goto clean; 800b62f328bSVille Tervo 80123691d75SGustavo F. Padovan chan = l2cap_chan_create(sk); 80248454079SGustavo F. Padovan if (!chan) { 80348454079SGustavo F. Padovan l2cap_sock_kill(sk); 80448454079SGustavo F. Padovan goto clean; 80548454079SGustavo F. Padovan } 80648454079SGustavo F. Padovan 8075d41ce1dSGustavo F. Padovan l2cap_pi(sk)->chan = chan; 8085d41ce1dSGustavo F. Padovan 809baa7e1faSGustavo F. Padovan write_lock_bh(&conn->chan_lock); 810b62f328bSVille Tervo 811b62f328bSVille Tervo hci_conn_hold(conn->hcon); 812b62f328bSVille Tervo 813b62f328bSVille Tervo l2cap_sock_init(sk, parent); 814baa7e1faSGustavo F. Padovan 815b62f328bSVille Tervo bacpy(&bt_sk(sk)->src, conn->src); 816b62f328bSVille Tervo bacpy(&bt_sk(sk)->dst, conn->dst); 817b62f328bSVille Tervo 818d1010240SGustavo F. Padovan bt_accept_enqueue(parent, sk); 819d1010240SGustavo F. Padovan 82048454079SGustavo F. Padovan __l2cap_chan_add(conn, chan); 82148454079SGustavo F. Padovan 822b62f328bSVille Tervo l2cap_sock_set_timer(sk, sk->sk_sndtimeo); 823b62f328bSVille Tervo 824b62f328bSVille Tervo sk->sk_state = BT_CONNECTED; 825b62f328bSVille Tervo parent->sk_data_ready(parent, 0); 826b62f328bSVille Tervo 827baa7e1faSGustavo F. Padovan write_unlock_bh(&conn->chan_lock); 828b62f328bSVille Tervo 829b62f328bSVille Tervo clean: 830b62f328bSVille Tervo bh_unlock_sock(parent); 831b62f328bSVille Tervo } 832b62f328bSVille Tervo 8330a708f8fSGustavo F. Padovan static void l2cap_conn_ready(struct l2cap_conn *conn) 8340a708f8fSGustavo F. Padovan { 83548454079SGustavo F. Padovan struct l2cap_chan *chan; 8360a708f8fSGustavo F. Padovan 8370a708f8fSGustavo F. Padovan BT_DBG("conn %p", conn); 8380a708f8fSGustavo F. Padovan 839b62f328bSVille Tervo if (!conn->hcon->out && conn->hcon->type == LE_LINK) 840b62f328bSVille Tervo l2cap_le_conn_ready(conn); 841b62f328bSVille Tervo 842baa7e1faSGustavo F. Padovan read_lock(&conn->chan_lock); 8430a708f8fSGustavo F. Padovan 844baa7e1faSGustavo F. Padovan list_for_each_entry(chan, &conn->chan_l, list) { 84548454079SGustavo F. Padovan struct sock *sk = chan->sk; 846baa7e1faSGustavo F. Padovan 8470a708f8fSGustavo F. Padovan bh_lock_sock(sk); 8480a708f8fSGustavo F. Padovan 849acd7d370SVille Tervo if (conn->hcon->type == LE_LINK) { 850acd7d370SVille Tervo l2cap_sock_clear_timer(sk); 851acd7d370SVille Tervo sk->sk_state = BT_CONNECTED; 852acd7d370SVille Tervo sk->sk_state_change(sk); 853acd7d370SVille Tervo } 854acd7d370SVille Tervo 8550a708f8fSGustavo F. Padovan if (sk->sk_type != SOCK_SEQPACKET && 8560a708f8fSGustavo F. Padovan sk->sk_type != SOCK_STREAM) { 8570a708f8fSGustavo F. Padovan l2cap_sock_clear_timer(sk); 8580a708f8fSGustavo F. Padovan sk->sk_state = BT_CONNECTED; 8590a708f8fSGustavo F. Padovan sk->sk_state_change(sk); 8600a708f8fSGustavo F. Padovan } else if (sk->sk_state == BT_CONNECT) 861fc7f8a7eSGustavo F. Padovan l2cap_do_start(chan); 8620a708f8fSGustavo F. Padovan 8630a708f8fSGustavo F. Padovan bh_unlock_sock(sk); 8640a708f8fSGustavo F. Padovan } 8650a708f8fSGustavo F. Padovan 866baa7e1faSGustavo F. Padovan read_unlock(&conn->chan_lock); 8670a708f8fSGustavo F. Padovan } 8680a708f8fSGustavo F. Padovan 8690a708f8fSGustavo F. Padovan /* Notify sockets that we cannot guaranty reliability anymore */ 8700a708f8fSGustavo F. Padovan static void l2cap_conn_unreliable(struct l2cap_conn *conn, int err) 8710a708f8fSGustavo F. Padovan { 87248454079SGustavo F. Padovan struct l2cap_chan *chan; 8730a708f8fSGustavo F. Padovan 8740a708f8fSGustavo F. Padovan BT_DBG("conn %p", conn); 8750a708f8fSGustavo F. Padovan 876baa7e1faSGustavo F. Padovan read_lock(&conn->chan_lock); 8770a708f8fSGustavo F. Padovan 878baa7e1faSGustavo F. Padovan list_for_each_entry(chan, &conn->chan_l, list) { 87948454079SGustavo F. Padovan struct sock *sk = chan->sk; 880baa7e1faSGustavo F. Padovan 8814343478fSGustavo F. Padovan if (chan->force_reliable) 8820a708f8fSGustavo F. Padovan sk->sk_err = err; 8830a708f8fSGustavo F. Padovan } 8840a708f8fSGustavo F. Padovan 885baa7e1faSGustavo F. Padovan read_unlock(&conn->chan_lock); 8860a708f8fSGustavo F. Padovan } 8870a708f8fSGustavo F. Padovan 8880a708f8fSGustavo F. Padovan static void l2cap_info_timeout(unsigned long arg) 8890a708f8fSGustavo F. Padovan { 8900a708f8fSGustavo F. Padovan struct l2cap_conn *conn = (void *) arg; 8910a708f8fSGustavo F. Padovan 8920a708f8fSGustavo F. Padovan conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_DONE; 8930a708f8fSGustavo F. Padovan conn->info_ident = 0; 8940a708f8fSGustavo F. Padovan 8950a708f8fSGustavo F. Padovan l2cap_conn_start(conn); 8960a708f8fSGustavo F. Padovan } 8970a708f8fSGustavo F. Padovan 8980a708f8fSGustavo F. Padovan static struct l2cap_conn *l2cap_conn_add(struct hci_conn *hcon, u8 status) 8990a708f8fSGustavo F. Padovan { 9000a708f8fSGustavo F. Padovan struct l2cap_conn *conn = hcon->l2cap_data; 9010a708f8fSGustavo F. Padovan 9020a708f8fSGustavo F. Padovan if (conn || status) 9030a708f8fSGustavo F. Padovan return conn; 9040a708f8fSGustavo F. Padovan 9050a708f8fSGustavo F. Padovan conn = kzalloc(sizeof(struct l2cap_conn), GFP_ATOMIC); 9060a708f8fSGustavo F. Padovan if (!conn) 9070a708f8fSGustavo F. Padovan return NULL; 9080a708f8fSGustavo F. Padovan 9090a708f8fSGustavo F. Padovan hcon->l2cap_data = conn; 9100a708f8fSGustavo F. Padovan conn->hcon = hcon; 9110a708f8fSGustavo F. Padovan 9120a708f8fSGustavo F. Padovan BT_DBG("hcon %p conn %p", hcon, conn); 9130a708f8fSGustavo F. Padovan 914acd7d370SVille Tervo if (hcon->hdev->le_mtu && hcon->type == LE_LINK) 915acd7d370SVille Tervo conn->mtu = hcon->hdev->le_mtu; 916acd7d370SVille Tervo else 9170a708f8fSGustavo F. Padovan conn->mtu = hcon->hdev->acl_mtu; 918acd7d370SVille Tervo 9190a708f8fSGustavo F. Padovan conn->src = &hcon->hdev->bdaddr; 9200a708f8fSGustavo F. Padovan conn->dst = &hcon->dst; 9210a708f8fSGustavo F. Padovan 9220a708f8fSGustavo F. Padovan conn->feat_mask = 0; 9230a708f8fSGustavo F. Padovan 9240a708f8fSGustavo F. Padovan spin_lock_init(&conn->lock); 925baa7e1faSGustavo F. Padovan rwlock_init(&conn->chan_lock); 926baa7e1faSGustavo F. Padovan 927baa7e1faSGustavo F. Padovan INIT_LIST_HEAD(&conn->chan_l); 9280a708f8fSGustavo F. Padovan 929b62f328bSVille Tervo if (hcon->type != LE_LINK) 9300a708f8fSGustavo F. Padovan setup_timer(&conn->info_timer, l2cap_info_timeout, 9310a708f8fSGustavo F. Padovan (unsigned long) conn); 9320a708f8fSGustavo F. Padovan 9330a708f8fSGustavo F. Padovan conn->disc_reason = 0x13; 9340a708f8fSGustavo F. Padovan 9350a708f8fSGustavo F. Padovan return conn; 9360a708f8fSGustavo F. Padovan } 9370a708f8fSGustavo F. Padovan 9380a708f8fSGustavo F. Padovan static void l2cap_conn_del(struct hci_conn *hcon, int err) 9390a708f8fSGustavo F. Padovan { 9400a708f8fSGustavo F. Padovan struct l2cap_conn *conn = hcon->l2cap_data; 941baa7e1faSGustavo F. Padovan struct l2cap_chan *chan, *l; 9420a708f8fSGustavo F. Padovan struct sock *sk; 9430a708f8fSGustavo F. Padovan 9440a708f8fSGustavo F. Padovan if (!conn) 9450a708f8fSGustavo F. Padovan return; 9460a708f8fSGustavo F. Padovan 9470a708f8fSGustavo F. Padovan BT_DBG("hcon %p conn %p, err %d", hcon, conn, err); 9480a708f8fSGustavo F. Padovan 9490a708f8fSGustavo F. Padovan kfree_skb(conn->rx_skb); 9500a708f8fSGustavo F. Padovan 9510a708f8fSGustavo F. Padovan /* Kill channels */ 952baa7e1faSGustavo F. Padovan list_for_each_entry_safe(chan, l, &conn->chan_l, list) { 95348454079SGustavo F. Padovan sk = chan->sk; 9540a708f8fSGustavo F. Padovan bh_lock_sock(sk); 95548454079SGustavo F. Padovan l2cap_chan_del(chan, err); 9560a708f8fSGustavo F. Padovan bh_unlock_sock(sk); 9570a708f8fSGustavo F. Padovan l2cap_sock_kill(sk); 9580a708f8fSGustavo F. Padovan } 9590a708f8fSGustavo F. Padovan 9600a708f8fSGustavo F. Padovan if (conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_SENT) 9610a708f8fSGustavo F. Padovan del_timer_sync(&conn->info_timer); 9620a708f8fSGustavo F. Padovan 9630a708f8fSGustavo F. Padovan hcon->l2cap_data = NULL; 9640a708f8fSGustavo F. Padovan kfree(conn); 9650a708f8fSGustavo F. Padovan } 9660a708f8fSGustavo F. Padovan 96748454079SGustavo F. Padovan static inline void l2cap_chan_add(struct l2cap_conn *conn, struct l2cap_chan *chan) 9680a708f8fSGustavo F. Padovan { 969baa7e1faSGustavo F. Padovan write_lock_bh(&conn->chan_lock); 97048454079SGustavo F. Padovan __l2cap_chan_add(conn, chan); 971baa7e1faSGustavo F. Padovan write_unlock_bh(&conn->chan_lock); 9720a708f8fSGustavo F. Padovan } 9730a708f8fSGustavo F. Padovan 9740a708f8fSGustavo F. Padovan /* ---- Socket interface ---- */ 9750a708f8fSGustavo F. Padovan 9760a708f8fSGustavo F. Padovan /* Find socket with psm and source bdaddr. 9770a708f8fSGustavo F. Padovan * Returns closest match. 9780a708f8fSGustavo F. Padovan */ 97923691d75SGustavo F. Padovan static struct l2cap_chan *l2cap_global_chan_by_psm(int state, __le16 psm, bdaddr_t *src) 9800a708f8fSGustavo F. Padovan { 98123691d75SGustavo F. Padovan struct l2cap_chan *c, *c1 = NULL; 9820a708f8fSGustavo F. Padovan 98323691d75SGustavo F. Padovan read_lock(&chan_list_lock); 9840a708f8fSGustavo F. Padovan 98523691d75SGustavo F. Padovan list_for_each_entry(c, &chan_list, global_l) { 98623691d75SGustavo F. Padovan struct sock *sk = c->sk; 987fe4128e0SGustavo F. Padovan 9880a708f8fSGustavo F. Padovan if (state && sk->sk_state != state) 9890a708f8fSGustavo F. Padovan continue; 9900a708f8fSGustavo F. Padovan 99123691d75SGustavo F. Padovan if (c->psm == psm) { 9920a708f8fSGustavo F. Padovan /* Exact match. */ 99323691d75SGustavo F. Padovan if (!bacmp(&bt_sk(sk)->src, src)) { 994a7567b20SJohannes Berg read_unlock(&chan_list_lock); 99523691d75SGustavo F. Padovan return c; 99623691d75SGustavo F. Padovan } 9970a708f8fSGustavo F. Padovan 9980a708f8fSGustavo F. Padovan /* Closest match */ 9990a708f8fSGustavo F. Padovan if (!bacmp(&bt_sk(sk)->src, BDADDR_ANY)) 100023691d75SGustavo F. Padovan c1 = c; 10010a708f8fSGustavo F. Padovan } 10020a708f8fSGustavo F. Padovan } 10030a708f8fSGustavo F. Padovan 100423691d75SGustavo F. Padovan read_unlock(&chan_list_lock); 10050a708f8fSGustavo F. Padovan 100623691d75SGustavo F. Padovan return c1; 10070a708f8fSGustavo F. Padovan } 10080a708f8fSGustavo F. Padovan 100977a74c7eSGustavo F. Padovan int l2cap_chan_connect(struct l2cap_chan *chan) 10100a708f8fSGustavo F. Padovan { 10115d41ce1dSGustavo F. Padovan struct sock *sk = chan->sk; 10120a708f8fSGustavo F. Padovan bdaddr_t *src = &bt_sk(sk)->src; 10130a708f8fSGustavo F. Padovan bdaddr_t *dst = &bt_sk(sk)->dst; 10140a708f8fSGustavo F. Padovan struct l2cap_conn *conn; 10150a708f8fSGustavo F. Padovan struct hci_conn *hcon; 10160a708f8fSGustavo F. Padovan struct hci_dev *hdev; 10170a708f8fSGustavo F. Padovan __u8 auth_type; 10180a708f8fSGustavo F. Padovan int err; 10190a708f8fSGustavo F. Padovan 10200a708f8fSGustavo F. Padovan BT_DBG("%s -> %s psm 0x%2.2x", batostr(src), batostr(dst), 1021fe4128e0SGustavo F. Padovan chan->psm); 10220a708f8fSGustavo F. Padovan 10230a708f8fSGustavo F. Padovan hdev = hci_get_route(dst, src); 10240a708f8fSGustavo F. Padovan if (!hdev) 10250a708f8fSGustavo F. Padovan return -EHOSTUNREACH; 10260a708f8fSGustavo F. Padovan 10270a708f8fSGustavo F. Padovan hci_dev_lock_bh(hdev); 10280a708f8fSGustavo F. Padovan 10294343478fSGustavo F. Padovan auth_type = l2cap_get_auth_type(chan); 10300a708f8fSGustavo F. Padovan 1031fe4128e0SGustavo F. Padovan if (chan->dcid == L2CAP_CID_LE_DATA) 1032acd7d370SVille Tervo hcon = hci_connect(hdev, LE_LINK, dst, 10334343478fSGustavo F. Padovan chan->sec_level, auth_type); 1034acd7d370SVille Tervo else 10350a708f8fSGustavo F. Padovan hcon = hci_connect(hdev, ACL_LINK, dst, 10364343478fSGustavo F. Padovan chan->sec_level, auth_type); 1037acd7d370SVille Tervo 103830e76272SVille Tervo if (IS_ERR(hcon)) { 103930e76272SVille Tervo err = PTR_ERR(hcon); 10400a708f8fSGustavo F. Padovan goto done; 104130e76272SVille Tervo } 10420a708f8fSGustavo F. Padovan 10430a708f8fSGustavo F. Padovan conn = l2cap_conn_add(hcon, 0); 10440a708f8fSGustavo F. Padovan if (!conn) { 10450a708f8fSGustavo F. Padovan hci_conn_put(hcon); 104630e76272SVille Tervo err = -ENOMEM; 10470a708f8fSGustavo F. Padovan goto done; 10480a708f8fSGustavo F. Padovan } 10490a708f8fSGustavo F. Padovan 10500a708f8fSGustavo F. Padovan /* Update source addr of the socket */ 10510a708f8fSGustavo F. Padovan bacpy(src, conn->src); 10520a708f8fSGustavo F. Padovan 105348454079SGustavo F. Padovan l2cap_chan_add(conn, chan); 105448454079SGustavo F. Padovan 10550a708f8fSGustavo F. Padovan sk->sk_state = BT_CONNECT; 10560a708f8fSGustavo F. Padovan l2cap_sock_set_timer(sk, sk->sk_sndtimeo); 10570a708f8fSGustavo F. Padovan 10580a708f8fSGustavo F. Padovan if (hcon->state == BT_CONNECTED) { 10590a708f8fSGustavo F. Padovan if (sk->sk_type != SOCK_SEQPACKET && 10600a708f8fSGustavo F. Padovan sk->sk_type != SOCK_STREAM) { 10610a708f8fSGustavo F. Padovan l2cap_sock_clear_timer(sk); 10624343478fSGustavo F. Padovan if (l2cap_check_security(chan)) 10630a708f8fSGustavo F. Padovan sk->sk_state = BT_CONNECTED; 10640a708f8fSGustavo F. Padovan } else 1065fc7f8a7eSGustavo F. Padovan l2cap_do_start(chan); 10660a708f8fSGustavo F. Padovan } 10670a708f8fSGustavo F. Padovan 106830e76272SVille Tervo err = 0; 106930e76272SVille Tervo 10700a708f8fSGustavo F. Padovan done: 10710a708f8fSGustavo F. Padovan hci_dev_unlock_bh(hdev); 10720a708f8fSGustavo F. Padovan hci_dev_put(hdev); 10730a708f8fSGustavo F. Padovan return err; 10740a708f8fSGustavo F. Padovan } 10750a708f8fSGustavo F. Padovan 1076dcba0dbaSGustavo F. Padovan int __l2cap_wait_ack(struct sock *sk) 10770a708f8fSGustavo F. Padovan { 10788c1d787bSGustavo F. Padovan struct l2cap_chan *chan = l2cap_pi(sk)->chan; 10790a708f8fSGustavo F. Padovan DECLARE_WAITQUEUE(wait, current); 10800a708f8fSGustavo F. Padovan int err = 0; 10810a708f8fSGustavo F. Padovan int timeo = HZ/5; 10820a708f8fSGustavo F. Padovan 10830a708f8fSGustavo F. Padovan add_wait_queue(sk_sleep(sk), &wait); 10848c1d787bSGustavo F. Padovan while ((chan->unacked_frames > 0 && chan->conn)) { 10850a708f8fSGustavo F. Padovan set_current_state(TASK_INTERRUPTIBLE); 10860a708f8fSGustavo F. Padovan 10870a708f8fSGustavo F. Padovan if (!timeo) 10880a708f8fSGustavo F. Padovan timeo = HZ/5; 10890a708f8fSGustavo F. Padovan 10900a708f8fSGustavo F. Padovan if (signal_pending(current)) { 10910a708f8fSGustavo F. Padovan err = sock_intr_errno(timeo); 10920a708f8fSGustavo F. Padovan break; 10930a708f8fSGustavo F. Padovan } 10940a708f8fSGustavo F. Padovan 10950a708f8fSGustavo F. Padovan release_sock(sk); 10960a708f8fSGustavo F. Padovan timeo = schedule_timeout(timeo); 10970a708f8fSGustavo F. Padovan lock_sock(sk); 10980a708f8fSGustavo F. Padovan 10990a708f8fSGustavo F. Padovan err = sock_error(sk); 11000a708f8fSGustavo F. Padovan if (err) 11010a708f8fSGustavo F. Padovan break; 11020a708f8fSGustavo F. Padovan } 11030a708f8fSGustavo F. Padovan set_current_state(TASK_RUNNING); 11040a708f8fSGustavo F. Padovan remove_wait_queue(sk_sleep(sk), &wait); 11050a708f8fSGustavo F. Padovan return err; 11060a708f8fSGustavo F. Padovan } 11070a708f8fSGustavo F. Padovan 11080a708f8fSGustavo F. Padovan static void l2cap_monitor_timeout(unsigned long arg) 11090a708f8fSGustavo F. Padovan { 1110525cd185SGustavo F. Padovan struct l2cap_chan *chan = (void *) arg; 1111525cd185SGustavo F. Padovan struct sock *sk = chan->sk; 11120a708f8fSGustavo F. Padovan 1113525cd185SGustavo F. Padovan BT_DBG("chan %p", chan); 11140a708f8fSGustavo F. Padovan 11150a708f8fSGustavo F. Padovan bh_lock_sock(sk); 11162c03a7a4SGustavo F. Padovan if (chan->retry_count >= chan->remote_max_tx) { 11178c1d787bSGustavo F. Padovan l2cap_send_disconn_req(chan->conn, chan, ECONNABORTED); 11180a708f8fSGustavo F. Padovan bh_unlock_sock(sk); 11190a708f8fSGustavo F. Padovan return; 11200a708f8fSGustavo F. Padovan } 11210a708f8fSGustavo F. Padovan 11226a026610SGustavo F. Padovan chan->retry_count++; 11230a708f8fSGustavo F. Padovan __mod_monitor_timer(); 11240a708f8fSGustavo F. Padovan 1125525cd185SGustavo F. Padovan l2cap_send_rr_or_rnr(chan, L2CAP_CTRL_POLL); 11260a708f8fSGustavo F. Padovan bh_unlock_sock(sk); 11270a708f8fSGustavo F. Padovan } 11280a708f8fSGustavo F. Padovan 11290a708f8fSGustavo F. Padovan static void l2cap_retrans_timeout(unsigned long arg) 11300a708f8fSGustavo F. Padovan { 1131525cd185SGustavo F. Padovan struct l2cap_chan *chan = (void *) arg; 1132525cd185SGustavo F. Padovan struct sock *sk = chan->sk; 11330a708f8fSGustavo F. Padovan 113449208c9cSGustavo F. Padovan BT_DBG("chan %p", chan); 11350a708f8fSGustavo F. Padovan 11360a708f8fSGustavo F. Padovan bh_lock_sock(sk); 11376a026610SGustavo F. Padovan chan->retry_count = 1; 11380a708f8fSGustavo F. Padovan __mod_monitor_timer(); 11390a708f8fSGustavo F. Padovan 1140525cd185SGustavo F. Padovan chan->conn_state |= L2CAP_CONN_WAIT_F; 11410a708f8fSGustavo F. Padovan 1142525cd185SGustavo F. Padovan l2cap_send_rr_or_rnr(chan, L2CAP_CTRL_POLL); 11430a708f8fSGustavo F. Padovan bh_unlock_sock(sk); 11440a708f8fSGustavo F. Padovan } 11450a708f8fSGustavo F. Padovan 114642e5c802SGustavo F. Padovan static void l2cap_drop_acked_frames(struct l2cap_chan *chan) 11470a708f8fSGustavo F. Padovan { 11480a708f8fSGustavo F. Padovan struct sk_buff *skb; 11490a708f8fSGustavo F. Padovan 115058d35f87SGustavo F. Padovan while ((skb = skb_peek(&chan->tx_q)) && 11516a026610SGustavo F. Padovan chan->unacked_frames) { 115242e5c802SGustavo F. Padovan if (bt_cb(skb)->tx_seq == chan->expected_ack_seq) 11530a708f8fSGustavo F. Padovan break; 11540a708f8fSGustavo F. Padovan 115558d35f87SGustavo F. Padovan skb = skb_dequeue(&chan->tx_q); 11560a708f8fSGustavo F. Padovan kfree_skb(skb); 11570a708f8fSGustavo F. Padovan 11586a026610SGustavo F. Padovan chan->unacked_frames--; 11590a708f8fSGustavo F. Padovan } 11600a708f8fSGustavo F. Padovan 11616a026610SGustavo F. Padovan if (!chan->unacked_frames) 1162e92c8e70SGustavo F. Padovan del_timer(&chan->retrans_timer); 11630a708f8fSGustavo F. Padovan } 11640a708f8fSGustavo F. Padovan 11654343478fSGustavo F. Padovan void l2cap_do_send(struct l2cap_chan *chan, struct sk_buff *skb) 11660a708f8fSGustavo F. Padovan { 11678c1d787bSGustavo F. Padovan struct hci_conn *hcon = chan->conn->hcon; 11680a708f8fSGustavo F. Padovan u16 flags; 11690a708f8fSGustavo F. Padovan 11704343478fSGustavo F. Padovan BT_DBG("chan %p, skb %p len %d", chan, skb, skb->len); 11710a708f8fSGustavo F. Padovan 11724343478fSGustavo F. Padovan if (!chan->flushable && lmp_no_flush_capable(hcon->hdev)) 11730a708f8fSGustavo F. Padovan flags = ACL_START_NO_FLUSH; 11740a708f8fSGustavo F. Padovan else 11750a708f8fSGustavo F. Padovan flags = ACL_START; 11760a708f8fSGustavo F. Padovan 11770a708f8fSGustavo F. Padovan hci_send_acl(hcon, skb, flags); 11780a708f8fSGustavo F. Padovan } 11790a708f8fSGustavo F. Padovan 118042e5c802SGustavo F. Padovan void l2cap_streaming_send(struct l2cap_chan *chan) 11810a708f8fSGustavo F. Padovan { 11820a708f8fSGustavo F. Padovan struct sk_buff *skb; 11830a708f8fSGustavo F. Padovan u16 control, fcs; 11840a708f8fSGustavo F. Padovan 118558d35f87SGustavo F. Padovan while ((skb = skb_dequeue(&chan->tx_q))) { 11860a708f8fSGustavo F. Padovan control = get_unaligned_le16(skb->data + L2CAP_HDR_SIZE); 118742e5c802SGustavo F. Padovan control |= chan->next_tx_seq << L2CAP_CTRL_TXSEQ_SHIFT; 11880a708f8fSGustavo F. Padovan put_unaligned_le16(control, skb->data + L2CAP_HDR_SIZE); 11890a708f8fSGustavo F. Padovan 119047d1ec61SGustavo F. Padovan if (chan->fcs == L2CAP_FCS_CRC16) { 11910a708f8fSGustavo F. Padovan fcs = crc16(0, (u8 *)skb->data, skb->len - 2); 11920a708f8fSGustavo F. Padovan put_unaligned_le16(fcs, skb->data + skb->len - 2); 11930a708f8fSGustavo F. Padovan } 11940a708f8fSGustavo F. Padovan 11954343478fSGustavo F. Padovan l2cap_do_send(chan, skb); 11960a708f8fSGustavo F. Padovan 119742e5c802SGustavo F. Padovan chan->next_tx_seq = (chan->next_tx_seq + 1) % 64; 11980a708f8fSGustavo F. Padovan } 11990a708f8fSGustavo F. Padovan } 12000a708f8fSGustavo F. Padovan 1201525cd185SGustavo F. Padovan static void l2cap_retransmit_one_frame(struct l2cap_chan *chan, u8 tx_seq) 12020a708f8fSGustavo F. Padovan { 12030a708f8fSGustavo F. Padovan struct sk_buff *skb, *tx_skb; 12040a708f8fSGustavo F. Padovan u16 control, fcs; 12050a708f8fSGustavo F. Padovan 120658d35f87SGustavo F. Padovan skb = skb_peek(&chan->tx_q); 12070a708f8fSGustavo F. Padovan if (!skb) 12080a708f8fSGustavo F. Padovan return; 12090a708f8fSGustavo F. Padovan 12100a708f8fSGustavo F. Padovan do { 12110a708f8fSGustavo F. Padovan if (bt_cb(skb)->tx_seq == tx_seq) 12120a708f8fSGustavo F. Padovan break; 12130a708f8fSGustavo F. Padovan 121458d35f87SGustavo F. Padovan if (skb_queue_is_last(&chan->tx_q, skb)) 12150a708f8fSGustavo F. Padovan return; 12160a708f8fSGustavo F. Padovan 121758d35f87SGustavo F. Padovan } while ((skb = skb_queue_next(&chan->tx_q, skb))); 12180a708f8fSGustavo F. Padovan 12192c03a7a4SGustavo F. Padovan if (chan->remote_max_tx && 12202c03a7a4SGustavo F. Padovan bt_cb(skb)->retries == chan->remote_max_tx) { 12218c1d787bSGustavo F. Padovan l2cap_send_disconn_req(chan->conn, chan, ECONNABORTED); 12220a708f8fSGustavo F. Padovan return; 12230a708f8fSGustavo F. Padovan } 12240a708f8fSGustavo F. Padovan 12250a708f8fSGustavo F. Padovan tx_skb = skb_clone(skb, GFP_ATOMIC); 12260a708f8fSGustavo F. Padovan bt_cb(skb)->retries++; 12270a708f8fSGustavo F. Padovan control = get_unaligned_le16(tx_skb->data + L2CAP_HDR_SIZE); 1228a429b519SRuiyi Zhang control &= L2CAP_CTRL_SAR; 12290a708f8fSGustavo F. Padovan 1230525cd185SGustavo F. Padovan if (chan->conn_state & L2CAP_CONN_SEND_FBIT) { 12310a708f8fSGustavo F. Padovan control |= L2CAP_CTRL_FINAL; 1232525cd185SGustavo F. Padovan chan->conn_state &= ~L2CAP_CONN_SEND_FBIT; 12330a708f8fSGustavo F. Padovan } 12340a708f8fSGustavo F. Padovan 123542e5c802SGustavo F. Padovan control |= (chan->buffer_seq << L2CAP_CTRL_REQSEQ_SHIFT) 12360a708f8fSGustavo F. Padovan | (tx_seq << L2CAP_CTRL_TXSEQ_SHIFT); 12370a708f8fSGustavo F. Padovan 12380a708f8fSGustavo F. Padovan put_unaligned_le16(control, tx_skb->data + L2CAP_HDR_SIZE); 12390a708f8fSGustavo F. Padovan 124047d1ec61SGustavo F. Padovan if (chan->fcs == L2CAP_FCS_CRC16) { 12410a708f8fSGustavo F. Padovan fcs = crc16(0, (u8 *)tx_skb->data, tx_skb->len - 2); 12420a708f8fSGustavo F. Padovan put_unaligned_le16(fcs, tx_skb->data + tx_skb->len - 2); 12430a708f8fSGustavo F. Padovan } 12440a708f8fSGustavo F. Padovan 12454343478fSGustavo F. Padovan l2cap_do_send(chan, tx_skb); 12460a708f8fSGustavo F. Padovan } 12470a708f8fSGustavo F. Padovan 1248525cd185SGustavo F. Padovan int l2cap_ertm_send(struct l2cap_chan *chan) 12490a708f8fSGustavo F. Padovan { 12500a708f8fSGustavo F. Padovan struct sk_buff *skb, *tx_skb; 1251525cd185SGustavo F. Padovan struct sock *sk = chan->sk; 12520a708f8fSGustavo F. Padovan u16 control, fcs; 12530a708f8fSGustavo F. Padovan int nsent = 0; 12540a708f8fSGustavo F. Padovan 12550a708f8fSGustavo F. Padovan if (sk->sk_state != BT_CONNECTED) 12560a708f8fSGustavo F. Padovan return -ENOTCONN; 12570a708f8fSGustavo F. Padovan 125858d35f87SGustavo F. Padovan while ((skb = chan->tx_send_head) && (!l2cap_tx_window_full(chan))) { 12590a708f8fSGustavo F. Padovan 12602c03a7a4SGustavo F. Padovan if (chan->remote_max_tx && 12612c03a7a4SGustavo F. Padovan bt_cb(skb)->retries == chan->remote_max_tx) { 12628c1d787bSGustavo F. Padovan l2cap_send_disconn_req(chan->conn, chan, ECONNABORTED); 12630a708f8fSGustavo F. Padovan break; 12640a708f8fSGustavo F. Padovan } 12650a708f8fSGustavo F. Padovan 12660a708f8fSGustavo F. Padovan tx_skb = skb_clone(skb, GFP_ATOMIC); 12670a708f8fSGustavo F. Padovan 12680a708f8fSGustavo F. Padovan bt_cb(skb)->retries++; 12690a708f8fSGustavo F. Padovan 12700a708f8fSGustavo F. Padovan control = get_unaligned_le16(tx_skb->data + L2CAP_HDR_SIZE); 12710a708f8fSGustavo F. Padovan control &= L2CAP_CTRL_SAR; 12720a708f8fSGustavo F. Padovan 1273525cd185SGustavo F. Padovan if (chan->conn_state & L2CAP_CONN_SEND_FBIT) { 12740a708f8fSGustavo F. Padovan control |= L2CAP_CTRL_FINAL; 1275525cd185SGustavo F. Padovan chan->conn_state &= ~L2CAP_CONN_SEND_FBIT; 12760a708f8fSGustavo F. Padovan } 127742e5c802SGustavo F. Padovan control |= (chan->buffer_seq << L2CAP_CTRL_REQSEQ_SHIFT) 127842e5c802SGustavo F. Padovan | (chan->next_tx_seq << L2CAP_CTRL_TXSEQ_SHIFT); 12790a708f8fSGustavo F. Padovan put_unaligned_le16(control, tx_skb->data + L2CAP_HDR_SIZE); 12800a708f8fSGustavo F. Padovan 12810a708f8fSGustavo F. Padovan 128247d1ec61SGustavo F. Padovan if (chan->fcs == L2CAP_FCS_CRC16) { 12830a708f8fSGustavo F. Padovan fcs = crc16(0, (u8 *)skb->data, tx_skb->len - 2); 12840a708f8fSGustavo F. Padovan put_unaligned_le16(fcs, skb->data + tx_skb->len - 2); 12850a708f8fSGustavo F. Padovan } 12860a708f8fSGustavo F. Padovan 12874343478fSGustavo F. Padovan l2cap_do_send(chan, tx_skb); 12880a708f8fSGustavo F. Padovan 12890a708f8fSGustavo F. Padovan __mod_retrans_timer(); 12900a708f8fSGustavo F. Padovan 129142e5c802SGustavo F. Padovan bt_cb(skb)->tx_seq = chan->next_tx_seq; 129242e5c802SGustavo F. Padovan chan->next_tx_seq = (chan->next_tx_seq + 1) % 64; 12930a708f8fSGustavo F. Padovan 129423e9fde2SSuraj Sumangala if (bt_cb(skb)->retries == 1) 12956a026610SGustavo F. Padovan chan->unacked_frames++; 129623e9fde2SSuraj Sumangala 12976a026610SGustavo F. Padovan chan->frames_sent++; 12980a708f8fSGustavo F. Padovan 129958d35f87SGustavo F. Padovan if (skb_queue_is_last(&chan->tx_q, skb)) 130058d35f87SGustavo F. Padovan chan->tx_send_head = NULL; 13010a708f8fSGustavo F. Padovan else 130258d35f87SGustavo F. Padovan chan->tx_send_head = skb_queue_next(&chan->tx_q, skb); 13030a708f8fSGustavo F. Padovan 13040a708f8fSGustavo F. Padovan nsent++; 13050a708f8fSGustavo F. Padovan } 13060a708f8fSGustavo F. Padovan 13070a708f8fSGustavo F. Padovan return nsent; 13080a708f8fSGustavo F. Padovan } 13090a708f8fSGustavo F. Padovan 1310525cd185SGustavo F. Padovan static int l2cap_retransmit_frames(struct l2cap_chan *chan) 13110a708f8fSGustavo F. Padovan { 13120a708f8fSGustavo F. Padovan int ret; 13130a708f8fSGustavo F. Padovan 131458d35f87SGustavo F. Padovan if (!skb_queue_empty(&chan->tx_q)) 131558d35f87SGustavo F. Padovan chan->tx_send_head = chan->tx_q.next; 13160a708f8fSGustavo F. Padovan 131742e5c802SGustavo F. Padovan chan->next_tx_seq = chan->expected_ack_seq; 1318525cd185SGustavo F. Padovan ret = l2cap_ertm_send(chan); 13190a708f8fSGustavo F. Padovan return ret; 13200a708f8fSGustavo F. Padovan } 13210a708f8fSGustavo F. Padovan 1322525cd185SGustavo F. Padovan static void l2cap_send_ack(struct l2cap_chan *chan) 13230a708f8fSGustavo F. Padovan { 13240a708f8fSGustavo F. Padovan u16 control = 0; 13250a708f8fSGustavo F. Padovan 132642e5c802SGustavo F. Padovan control |= chan->buffer_seq << L2CAP_CTRL_REQSEQ_SHIFT; 13270a708f8fSGustavo F. Padovan 1328525cd185SGustavo F. Padovan if (chan->conn_state & L2CAP_CONN_LOCAL_BUSY) { 13290a708f8fSGustavo F. Padovan control |= L2CAP_SUPER_RCV_NOT_READY; 1330525cd185SGustavo F. Padovan chan->conn_state |= L2CAP_CONN_RNR_SENT; 1331525cd185SGustavo F. Padovan l2cap_send_sframe(chan, control); 13320a708f8fSGustavo F. Padovan return; 13330a708f8fSGustavo F. Padovan } 13340a708f8fSGustavo F. Padovan 1335525cd185SGustavo F. Padovan if (l2cap_ertm_send(chan) > 0) 13360a708f8fSGustavo F. Padovan return; 13370a708f8fSGustavo F. Padovan 13380a708f8fSGustavo F. Padovan control |= L2CAP_SUPER_RCV_READY; 1339525cd185SGustavo F. Padovan l2cap_send_sframe(chan, control); 13400a708f8fSGustavo F. Padovan } 13410a708f8fSGustavo F. Padovan 1342525cd185SGustavo F. Padovan static void l2cap_send_srejtail(struct l2cap_chan *chan) 13430a708f8fSGustavo F. Padovan { 13440a708f8fSGustavo F. Padovan struct srej_list *tail; 13450a708f8fSGustavo F. Padovan u16 control; 13460a708f8fSGustavo F. Padovan 13470a708f8fSGustavo F. Padovan control = L2CAP_SUPER_SELECT_REJECT; 13480a708f8fSGustavo F. Padovan control |= L2CAP_CTRL_FINAL; 13490a708f8fSGustavo F. Padovan 135039d5a3eeSGustavo F. Padovan tail = list_entry((&chan->srej_l)->prev, struct srej_list, list); 13510a708f8fSGustavo F. Padovan control |= tail->tx_seq << L2CAP_CTRL_REQSEQ_SHIFT; 13520a708f8fSGustavo F. Padovan 1353525cd185SGustavo F. Padovan l2cap_send_sframe(chan, control); 13540a708f8fSGustavo F. Padovan } 13550a708f8fSGustavo F. Padovan 13560a708f8fSGustavo F. Padovan static inline int l2cap_skbuff_fromiovec(struct sock *sk, struct msghdr *msg, int len, int count, struct sk_buff *skb) 13570a708f8fSGustavo F. Padovan { 13588c1d787bSGustavo F. Padovan struct l2cap_conn *conn = l2cap_pi(sk)->chan->conn; 13590a708f8fSGustavo F. Padovan struct sk_buff **frag; 13600a708f8fSGustavo F. Padovan int err, sent = 0; 13610a708f8fSGustavo F. Padovan 13620a708f8fSGustavo F. Padovan if (memcpy_fromiovec(skb_put(skb, count), msg->msg_iov, count)) 13630a708f8fSGustavo F. Padovan return -EFAULT; 13640a708f8fSGustavo F. Padovan 13650a708f8fSGustavo F. Padovan sent += count; 13660a708f8fSGustavo F. Padovan len -= count; 13670a708f8fSGustavo F. Padovan 13680a708f8fSGustavo F. Padovan /* Continuation fragments (no L2CAP header) */ 13690a708f8fSGustavo F. Padovan frag = &skb_shinfo(skb)->frag_list; 13700a708f8fSGustavo F. Padovan while (len) { 13710a708f8fSGustavo F. Padovan count = min_t(unsigned int, conn->mtu, len); 13720a708f8fSGustavo F. Padovan 13730a708f8fSGustavo F. Padovan *frag = bt_skb_send_alloc(sk, count, msg->msg_flags & MSG_DONTWAIT, &err); 13740a708f8fSGustavo F. Padovan if (!*frag) 13750a708f8fSGustavo F. Padovan return err; 13760a708f8fSGustavo F. Padovan if (memcpy_fromiovec(skb_put(*frag, count), msg->msg_iov, count)) 13770a708f8fSGustavo F. Padovan return -EFAULT; 13780a708f8fSGustavo F. Padovan 13790a708f8fSGustavo F. Padovan sent += count; 13800a708f8fSGustavo F. Padovan len -= count; 13810a708f8fSGustavo F. Padovan 13820a708f8fSGustavo F. Padovan frag = &(*frag)->next; 13830a708f8fSGustavo F. Padovan } 13840a708f8fSGustavo F. Padovan 13850a708f8fSGustavo F. Padovan return sent; 13860a708f8fSGustavo F. Padovan } 13870a708f8fSGustavo F. Padovan 1388fe4128e0SGustavo F. Padovan struct sk_buff *l2cap_create_connless_pdu(struct l2cap_chan *chan, struct msghdr *msg, size_t len) 13890a708f8fSGustavo F. Padovan { 1390fe4128e0SGustavo F. Padovan struct sock *sk = chan->sk; 13918c1d787bSGustavo F. Padovan struct l2cap_conn *conn = chan->conn; 13920a708f8fSGustavo F. Padovan struct sk_buff *skb; 13930a708f8fSGustavo F. Padovan int err, count, hlen = L2CAP_HDR_SIZE + 2; 13940a708f8fSGustavo F. Padovan struct l2cap_hdr *lh; 13950a708f8fSGustavo F. Padovan 13960a708f8fSGustavo F. Padovan BT_DBG("sk %p len %d", sk, (int)len); 13970a708f8fSGustavo F. Padovan 13980a708f8fSGustavo F. Padovan count = min_t(unsigned int, (conn->mtu - hlen), len); 13990a708f8fSGustavo F. Padovan skb = bt_skb_send_alloc(sk, count + hlen, 14000a708f8fSGustavo F. Padovan msg->msg_flags & MSG_DONTWAIT, &err); 14010a708f8fSGustavo F. Padovan if (!skb) 14020a708f8fSGustavo F. Padovan return ERR_PTR(err); 14030a708f8fSGustavo F. Padovan 14040a708f8fSGustavo F. Padovan /* Create L2CAP header */ 14050a708f8fSGustavo F. Padovan lh = (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE); 1406fe4128e0SGustavo F. Padovan lh->cid = cpu_to_le16(chan->dcid); 14070a708f8fSGustavo F. Padovan lh->len = cpu_to_le16(len + (hlen - L2CAP_HDR_SIZE)); 1408fe4128e0SGustavo F. Padovan put_unaligned_le16(chan->psm, skb_put(skb, 2)); 14090a708f8fSGustavo F. Padovan 14100a708f8fSGustavo F. Padovan err = l2cap_skbuff_fromiovec(sk, msg, len, count, skb); 14110a708f8fSGustavo F. Padovan if (unlikely(err < 0)) { 14120a708f8fSGustavo F. Padovan kfree_skb(skb); 14130a708f8fSGustavo F. Padovan return ERR_PTR(err); 14140a708f8fSGustavo F. Padovan } 14150a708f8fSGustavo F. Padovan return skb; 14160a708f8fSGustavo F. Padovan } 14170a708f8fSGustavo F. Padovan 1418fe4128e0SGustavo F. Padovan struct sk_buff *l2cap_create_basic_pdu(struct l2cap_chan *chan, struct msghdr *msg, size_t len) 14190a708f8fSGustavo F. Padovan { 1420fe4128e0SGustavo F. Padovan struct sock *sk = chan->sk; 14218c1d787bSGustavo F. Padovan struct l2cap_conn *conn = chan->conn; 14220a708f8fSGustavo F. Padovan struct sk_buff *skb; 14230a708f8fSGustavo F. Padovan int err, count, hlen = L2CAP_HDR_SIZE; 14240a708f8fSGustavo F. Padovan struct l2cap_hdr *lh; 14250a708f8fSGustavo F. Padovan 14260a708f8fSGustavo F. Padovan BT_DBG("sk %p len %d", sk, (int)len); 14270a708f8fSGustavo F. Padovan 14280a708f8fSGustavo F. Padovan count = min_t(unsigned int, (conn->mtu - hlen), len); 14290a708f8fSGustavo F. Padovan skb = bt_skb_send_alloc(sk, count + hlen, 14300a708f8fSGustavo F. Padovan msg->msg_flags & MSG_DONTWAIT, &err); 14310a708f8fSGustavo F. Padovan if (!skb) 14320a708f8fSGustavo F. Padovan return ERR_PTR(err); 14330a708f8fSGustavo F. Padovan 14340a708f8fSGustavo F. Padovan /* Create L2CAP header */ 14350a708f8fSGustavo F. Padovan lh = (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE); 1436fe4128e0SGustavo F. Padovan lh->cid = cpu_to_le16(chan->dcid); 14370a708f8fSGustavo F. Padovan lh->len = cpu_to_le16(len + (hlen - L2CAP_HDR_SIZE)); 14380a708f8fSGustavo F. Padovan 14390a708f8fSGustavo F. Padovan err = l2cap_skbuff_fromiovec(sk, msg, len, count, skb); 14400a708f8fSGustavo F. Padovan if (unlikely(err < 0)) { 14410a708f8fSGustavo F. Padovan kfree_skb(skb); 14420a708f8fSGustavo F. Padovan return ERR_PTR(err); 14430a708f8fSGustavo F. Padovan } 14440a708f8fSGustavo F. Padovan return skb; 14450a708f8fSGustavo F. Padovan } 14460a708f8fSGustavo F. Padovan 144747d1ec61SGustavo F. Padovan struct sk_buff *l2cap_create_iframe_pdu(struct l2cap_chan *chan, struct msghdr *msg, size_t len, u16 control, u16 sdulen) 14480a708f8fSGustavo F. Padovan { 144947d1ec61SGustavo F. Padovan struct sock *sk = chan->sk; 14508c1d787bSGustavo F. Padovan struct l2cap_conn *conn = chan->conn; 14510a708f8fSGustavo F. Padovan struct sk_buff *skb; 14520a708f8fSGustavo F. Padovan int err, count, hlen = L2CAP_HDR_SIZE + 2; 14530a708f8fSGustavo F. Padovan struct l2cap_hdr *lh; 14540a708f8fSGustavo F. Padovan 14550a708f8fSGustavo F. Padovan BT_DBG("sk %p len %d", sk, (int)len); 14560a708f8fSGustavo F. Padovan 14570a708f8fSGustavo F. Padovan if (!conn) 14580a708f8fSGustavo F. Padovan return ERR_PTR(-ENOTCONN); 14590a708f8fSGustavo F. Padovan 14600a708f8fSGustavo F. Padovan if (sdulen) 14610a708f8fSGustavo F. Padovan hlen += 2; 14620a708f8fSGustavo F. Padovan 146347d1ec61SGustavo F. Padovan if (chan->fcs == L2CAP_FCS_CRC16) 14640a708f8fSGustavo F. Padovan hlen += 2; 14650a708f8fSGustavo F. Padovan 14660a708f8fSGustavo F. Padovan count = min_t(unsigned int, (conn->mtu - hlen), len); 14670a708f8fSGustavo F. Padovan skb = bt_skb_send_alloc(sk, count + hlen, 14680a708f8fSGustavo F. Padovan msg->msg_flags & MSG_DONTWAIT, &err); 14690a708f8fSGustavo F. Padovan if (!skb) 14700a708f8fSGustavo F. Padovan return ERR_PTR(err); 14710a708f8fSGustavo F. Padovan 14720a708f8fSGustavo F. Padovan /* Create L2CAP header */ 14730a708f8fSGustavo F. Padovan lh = (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE); 1474fe4128e0SGustavo F. Padovan lh->cid = cpu_to_le16(chan->dcid); 14750a708f8fSGustavo F. Padovan lh->len = cpu_to_le16(len + (hlen - L2CAP_HDR_SIZE)); 14760a708f8fSGustavo F. Padovan put_unaligned_le16(control, skb_put(skb, 2)); 14770a708f8fSGustavo F. Padovan if (sdulen) 14780a708f8fSGustavo F. Padovan put_unaligned_le16(sdulen, skb_put(skb, 2)); 14790a708f8fSGustavo F. Padovan 14800a708f8fSGustavo F. Padovan err = l2cap_skbuff_fromiovec(sk, msg, len, count, skb); 14810a708f8fSGustavo F. Padovan if (unlikely(err < 0)) { 14820a708f8fSGustavo F. Padovan kfree_skb(skb); 14830a708f8fSGustavo F. Padovan return ERR_PTR(err); 14840a708f8fSGustavo F. Padovan } 14850a708f8fSGustavo F. Padovan 148647d1ec61SGustavo F. Padovan if (chan->fcs == L2CAP_FCS_CRC16) 14870a708f8fSGustavo F. Padovan put_unaligned_le16(0, skb_put(skb, 2)); 14880a708f8fSGustavo F. Padovan 14890a708f8fSGustavo F. Padovan bt_cb(skb)->retries = 0; 14900a708f8fSGustavo F. Padovan return skb; 14910a708f8fSGustavo F. Padovan } 14920a708f8fSGustavo F. Padovan 14932c03a7a4SGustavo F. Padovan int l2cap_sar_segment_sdu(struct l2cap_chan *chan, struct msghdr *msg, size_t len) 14940a708f8fSGustavo F. Padovan { 14950a708f8fSGustavo F. Padovan struct sk_buff *skb; 14960a708f8fSGustavo F. Padovan struct sk_buff_head sar_queue; 14970a708f8fSGustavo F. Padovan u16 control; 14980a708f8fSGustavo F. Padovan size_t size = 0; 14990a708f8fSGustavo F. Padovan 15000a708f8fSGustavo F. Padovan skb_queue_head_init(&sar_queue); 15010a708f8fSGustavo F. Padovan control = L2CAP_SDU_START; 150247d1ec61SGustavo F. Padovan skb = l2cap_create_iframe_pdu(chan, msg, chan->remote_mps, control, len); 15030a708f8fSGustavo F. Padovan if (IS_ERR(skb)) 15040a708f8fSGustavo F. Padovan return PTR_ERR(skb); 15050a708f8fSGustavo F. Padovan 15060a708f8fSGustavo F. Padovan __skb_queue_tail(&sar_queue, skb); 15072c03a7a4SGustavo F. Padovan len -= chan->remote_mps; 15082c03a7a4SGustavo F. Padovan size += chan->remote_mps; 15090a708f8fSGustavo F. Padovan 15100a708f8fSGustavo F. Padovan while (len > 0) { 15110a708f8fSGustavo F. Padovan size_t buflen; 15120a708f8fSGustavo F. Padovan 15132c03a7a4SGustavo F. Padovan if (len > chan->remote_mps) { 15140a708f8fSGustavo F. Padovan control = L2CAP_SDU_CONTINUE; 15152c03a7a4SGustavo F. Padovan buflen = chan->remote_mps; 15160a708f8fSGustavo F. Padovan } else { 15170a708f8fSGustavo F. Padovan control = L2CAP_SDU_END; 15180a708f8fSGustavo F. Padovan buflen = len; 15190a708f8fSGustavo F. Padovan } 15200a708f8fSGustavo F. Padovan 152147d1ec61SGustavo F. Padovan skb = l2cap_create_iframe_pdu(chan, msg, buflen, control, 0); 15220a708f8fSGustavo F. Padovan if (IS_ERR(skb)) { 15230a708f8fSGustavo F. Padovan skb_queue_purge(&sar_queue); 15240a708f8fSGustavo F. Padovan return PTR_ERR(skb); 15250a708f8fSGustavo F. Padovan } 15260a708f8fSGustavo F. Padovan 15270a708f8fSGustavo F. Padovan __skb_queue_tail(&sar_queue, skb); 15280a708f8fSGustavo F. Padovan len -= buflen; 15290a708f8fSGustavo F. Padovan size += buflen; 15300a708f8fSGustavo F. Padovan } 153158d35f87SGustavo F. Padovan skb_queue_splice_tail(&sar_queue, &chan->tx_q); 153258d35f87SGustavo F. Padovan if (chan->tx_send_head == NULL) 153358d35f87SGustavo F. Padovan chan->tx_send_head = sar_queue.next; 15340a708f8fSGustavo F. Padovan 15350a708f8fSGustavo F. Padovan return size; 15360a708f8fSGustavo F. Padovan } 15370a708f8fSGustavo F. Padovan 15389a91a04aSGustavo F. Padovan int l2cap_chan_send(struct l2cap_chan *chan, struct msghdr *msg, size_t len) 15399a91a04aSGustavo F. Padovan { 15409a91a04aSGustavo F. Padovan struct sock *sk = chan->sk; 15419a91a04aSGustavo F. Padovan struct sk_buff *skb; 15429a91a04aSGustavo F. Padovan u16 control; 15439a91a04aSGustavo F. Padovan int err; 15449a91a04aSGustavo F. Padovan 15459a91a04aSGustavo F. Padovan /* Connectionless channel */ 15469a91a04aSGustavo F. Padovan if (sk->sk_type == SOCK_DGRAM) { 15479a91a04aSGustavo F. Padovan skb = l2cap_create_connless_pdu(chan, msg, len); 15489a91a04aSGustavo F. Padovan if (IS_ERR(skb)) 15499a91a04aSGustavo F. Padovan return PTR_ERR(skb); 15509a91a04aSGustavo F. Padovan 15519a91a04aSGustavo F. Padovan l2cap_do_send(chan, skb); 15529a91a04aSGustavo F. Padovan return len; 15539a91a04aSGustavo F. Padovan } 15549a91a04aSGustavo F. Padovan 15559a91a04aSGustavo F. Padovan switch (chan->mode) { 15569a91a04aSGustavo F. Padovan case L2CAP_MODE_BASIC: 15579a91a04aSGustavo F. Padovan /* Check outgoing MTU */ 15589a91a04aSGustavo F. Padovan if (len > chan->omtu) 15599a91a04aSGustavo F. Padovan return -EMSGSIZE; 15609a91a04aSGustavo F. Padovan 15619a91a04aSGustavo F. Padovan /* Create a basic PDU */ 15629a91a04aSGustavo F. Padovan skb = l2cap_create_basic_pdu(chan, msg, len); 15639a91a04aSGustavo F. Padovan if (IS_ERR(skb)) 15649a91a04aSGustavo F. Padovan return PTR_ERR(skb); 15659a91a04aSGustavo F. Padovan 15669a91a04aSGustavo F. Padovan l2cap_do_send(chan, skb); 15679a91a04aSGustavo F. Padovan err = len; 15689a91a04aSGustavo F. Padovan break; 15699a91a04aSGustavo F. Padovan 15709a91a04aSGustavo F. Padovan case L2CAP_MODE_ERTM: 15719a91a04aSGustavo F. Padovan case L2CAP_MODE_STREAMING: 15729a91a04aSGustavo F. Padovan /* Entire SDU fits into one PDU */ 15739a91a04aSGustavo F. Padovan if (len <= chan->remote_mps) { 15749a91a04aSGustavo F. Padovan control = L2CAP_SDU_UNSEGMENTED; 15759a91a04aSGustavo F. Padovan skb = l2cap_create_iframe_pdu(chan, msg, len, control, 15769a91a04aSGustavo F. Padovan 0); 15779a91a04aSGustavo F. Padovan if (IS_ERR(skb)) 15789a91a04aSGustavo F. Padovan return PTR_ERR(skb); 15799a91a04aSGustavo F. Padovan 15809a91a04aSGustavo F. Padovan __skb_queue_tail(&chan->tx_q, skb); 15819a91a04aSGustavo F. Padovan 15829a91a04aSGustavo F. Padovan if (chan->tx_send_head == NULL) 15839a91a04aSGustavo F. Padovan chan->tx_send_head = skb; 15849a91a04aSGustavo F. Padovan 15859a91a04aSGustavo F. Padovan } else { 15869a91a04aSGustavo F. Padovan /* Segment SDU into multiples PDUs */ 15879a91a04aSGustavo F. Padovan err = l2cap_sar_segment_sdu(chan, msg, len); 15889a91a04aSGustavo F. Padovan if (err < 0) 15899a91a04aSGustavo F. Padovan return err; 15909a91a04aSGustavo F. Padovan } 15919a91a04aSGustavo F. Padovan 15929a91a04aSGustavo F. Padovan if (chan->mode == L2CAP_MODE_STREAMING) { 15939a91a04aSGustavo F. Padovan l2cap_streaming_send(chan); 15949a91a04aSGustavo F. Padovan err = len; 15959a91a04aSGustavo F. Padovan break; 15969a91a04aSGustavo F. Padovan } 15979a91a04aSGustavo F. Padovan 15989a91a04aSGustavo F. Padovan if ((chan->conn_state & L2CAP_CONN_REMOTE_BUSY) && 15999a91a04aSGustavo F. Padovan (chan->conn_state & L2CAP_CONN_WAIT_F)) { 16009a91a04aSGustavo F. Padovan err = len; 16019a91a04aSGustavo F. Padovan break; 16029a91a04aSGustavo F. Padovan } 16039a91a04aSGustavo F. Padovan 16049a91a04aSGustavo F. Padovan err = l2cap_ertm_send(chan); 16059a91a04aSGustavo F. Padovan if (err >= 0) 16069a91a04aSGustavo F. Padovan err = len; 16079a91a04aSGustavo F. Padovan 16089a91a04aSGustavo F. Padovan break; 16099a91a04aSGustavo F. Padovan 16109a91a04aSGustavo F. Padovan default: 16119a91a04aSGustavo F. Padovan BT_DBG("bad state %1.1x", chan->mode); 16129a91a04aSGustavo F. Padovan err = -EBADFD; 16139a91a04aSGustavo F. Padovan } 16149a91a04aSGustavo F. Padovan 16159a91a04aSGustavo F. Padovan return err; 16169a91a04aSGustavo F. Padovan } 16179a91a04aSGustavo F. Padovan 16180a708f8fSGustavo F. Padovan static void l2cap_chan_ready(struct sock *sk) 16190a708f8fSGustavo F. Padovan { 16200a708f8fSGustavo F. Padovan struct sock *parent = bt_sk(sk)->parent; 1621b4450035SGustavo F. Padovan struct l2cap_chan *chan = l2cap_pi(sk)->chan; 16220a708f8fSGustavo F. Padovan 16230a708f8fSGustavo F. Padovan BT_DBG("sk %p, parent %p", sk, parent); 16240a708f8fSGustavo F. Padovan 1625b4450035SGustavo F. Padovan chan->conf_state = 0; 16260a708f8fSGustavo F. Padovan l2cap_sock_clear_timer(sk); 16270a708f8fSGustavo F. Padovan 16280a708f8fSGustavo F. Padovan if (!parent) { 16290a708f8fSGustavo F. Padovan /* Outgoing channel. 16300a708f8fSGustavo F. Padovan * Wake up socket sleeping on connect. 16310a708f8fSGustavo F. Padovan */ 16320a708f8fSGustavo F. Padovan sk->sk_state = BT_CONNECTED; 16330a708f8fSGustavo F. Padovan sk->sk_state_change(sk); 16340a708f8fSGustavo F. Padovan } else { 16350a708f8fSGustavo F. Padovan /* Incoming channel. 16360a708f8fSGustavo F. Padovan * Wake up socket sleeping on accept. 16370a708f8fSGustavo F. Padovan */ 16380a708f8fSGustavo F. Padovan parent->sk_data_ready(parent, 0); 16390a708f8fSGustavo F. Padovan } 16400a708f8fSGustavo F. Padovan } 16410a708f8fSGustavo F. Padovan 16420a708f8fSGustavo F. Padovan /* Copy frame to all raw sockets on that connection */ 16430a708f8fSGustavo F. Padovan static void l2cap_raw_recv(struct l2cap_conn *conn, struct sk_buff *skb) 16440a708f8fSGustavo F. Padovan { 16450a708f8fSGustavo F. Padovan struct sk_buff *nskb; 164648454079SGustavo F. Padovan struct l2cap_chan *chan; 16470a708f8fSGustavo F. Padovan 16480a708f8fSGustavo F. Padovan BT_DBG("conn %p", conn); 16490a708f8fSGustavo F. Padovan 1650baa7e1faSGustavo F. Padovan read_lock(&conn->chan_lock); 1651baa7e1faSGustavo F. Padovan list_for_each_entry(chan, &conn->chan_l, list) { 165248454079SGustavo F. Padovan struct sock *sk = chan->sk; 16530a708f8fSGustavo F. Padovan if (sk->sk_type != SOCK_RAW) 16540a708f8fSGustavo F. Padovan continue; 16550a708f8fSGustavo F. Padovan 16560a708f8fSGustavo F. Padovan /* Don't send frame to the socket it came from */ 16570a708f8fSGustavo F. Padovan if (skb->sk == sk) 16580a708f8fSGustavo F. Padovan continue; 16590a708f8fSGustavo F. Padovan nskb = skb_clone(skb, GFP_ATOMIC); 16600a708f8fSGustavo F. Padovan if (!nskb) 16610a708f8fSGustavo F. Padovan continue; 16620a708f8fSGustavo F. Padovan 16630a708f8fSGustavo F. Padovan if (sock_queue_rcv_skb(sk, nskb)) 16640a708f8fSGustavo F. Padovan kfree_skb(nskb); 16650a708f8fSGustavo F. Padovan } 1666baa7e1faSGustavo F. Padovan read_unlock(&conn->chan_lock); 16670a708f8fSGustavo F. Padovan } 16680a708f8fSGustavo F. Padovan 16690a708f8fSGustavo F. Padovan /* ---- L2CAP signalling commands ---- */ 16700a708f8fSGustavo F. Padovan static struct sk_buff *l2cap_build_cmd(struct l2cap_conn *conn, 16710a708f8fSGustavo F. Padovan u8 code, u8 ident, u16 dlen, void *data) 16720a708f8fSGustavo F. Padovan { 16730a708f8fSGustavo F. Padovan struct sk_buff *skb, **frag; 16740a708f8fSGustavo F. Padovan struct l2cap_cmd_hdr *cmd; 16750a708f8fSGustavo F. Padovan struct l2cap_hdr *lh; 16760a708f8fSGustavo F. Padovan int len, count; 16770a708f8fSGustavo F. Padovan 16780a708f8fSGustavo F. Padovan BT_DBG("conn %p, code 0x%2.2x, ident 0x%2.2x, len %d", 16790a708f8fSGustavo F. Padovan conn, code, ident, dlen); 16800a708f8fSGustavo F. Padovan 16810a708f8fSGustavo F. Padovan len = L2CAP_HDR_SIZE + L2CAP_CMD_HDR_SIZE + dlen; 16820a708f8fSGustavo F. Padovan count = min_t(unsigned int, conn->mtu, len); 16830a708f8fSGustavo F. Padovan 16840a708f8fSGustavo F. Padovan skb = bt_skb_alloc(count, GFP_ATOMIC); 16850a708f8fSGustavo F. Padovan if (!skb) 16860a708f8fSGustavo F. Padovan return NULL; 16870a708f8fSGustavo F. Padovan 16880a708f8fSGustavo F. Padovan lh = (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE); 16890a708f8fSGustavo F. Padovan lh->len = cpu_to_le16(L2CAP_CMD_HDR_SIZE + dlen); 16903300d9a9SClaudio Takahasi 16913300d9a9SClaudio Takahasi if (conn->hcon->type == LE_LINK) 16923300d9a9SClaudio Takahasi lh->cid = cpu_to_le16(L2CAP_CID_LE_SIGNALING); 16933300d9a9SClaudio Takahasi else 16940a708f8fSGustavo F. Padovan lh->cid = cpu_to_le16(L2CAP_CID_SIGNALING); 16950a708f8fSGustavo F. Padovan 16960a708f8fSGustavo F. Padovan cmd = (struct l2cap_cmd_hdr *) skb_put(skb, L2CAP_CMD_HDR_SIZE); 16970a708f8fSGustavo F. Padovan cmd->code = code; 16980a708f8fSGustavo F. Padovan cmd->ident = ident; 16990a708f8fSGustavo F. Padovan cmd->len = cpu_to_le16(dlen); 17000a708f8fSGustavo F. Padovan 17010a708f8fSGustavo F. Padovan if (dlen) { 17020a708f8fSGustavo F. Padovan count -= L2CAP_HDR_SIZE + L2CAP_CMD_HDR_SIZE; 17030a708f8fSGustavo F. Padovan memcpy(skb_put(skb, count), data, count); 17040a708f8fSGustavo F. Padovan data += count; 17050a708f8fSGustavo F. Padovan } 17060a708f8fSGustavo F. Padovan 17070a708f8fSGustavo F. Padovan len -= skb->len; 17080a708f8fSGustavo F. Padovan 17090a708f8fSGustavo F. Padovan /* Continuation fragments (no L2CAP header) */ 17100a708f8fSGustavo F. Padovan frag = &skb_shinfo(skb)->frag_list; 17110a708f8fSGustavo F. Padovan while (len) { 17120a708f8fSGustavo F. Padovan count = min_t(unsigned int, conn->mtu, len); 17130a708f8fSGustavo F. Padovan 17140a708f8fSGustavo F. Padovan *frag = bt_skb_alloc(count, GFP_ATOMIC); 17150a708f8fSGustavo F. Padovan if (!*frag) 17160a708f8fSGustavo F. Padovan goto fail; 17170a708f8fSGustavo F. Padovan 17180a708f8fSGustavo F. Padovan memcpy(skb_put(*frag, count), data, count); 17190a708f8fSGustavo F. Padovan 17200a708f8fSGustavo F. Padovan len -= count; 17210a708f8fSGustavo F. Padovan data += count; 17220a708f8fSGustavo F. Padovan 17230a708f8fSGustavo F. Padovan frag = &(*frag)->next; 17240a708f8fSGustavo F. Padovan } 17250a708f8fSGustavo F. Padovan 17260a708f8fSGustavo F. Padovan return skb; 17270a708f8fSGustavo F. Padovan 17280a708f8fSGustavo F. Padovan fail: 17290a708f8fSGustavo F. Padovan kfree_skb(skb); 17300a708f8fSGustavo F. Padovan return NULL; 17310a708f8fSGustavo F. Padovan } 17320a708f8fSGustavo F. Padovan 17330a708f8fSGustavo F. Padovan static inline int l2cap_get_conf_opt(void **ptr, int *type, int *olen, unsigned long *val) 17340a708f8fSGustavo F. Padovan { 17350a708f8fSGustavo F. Padovan struct l2cap_conf_opt *opt = *ptr; 17360a708f8fSGustavo F. Padovan int len; 17370a708f8fSGustavo F. Padovan 17380a708f8fSGustavo F. Padovan len = L2CAP_CONF_OPT_SIZE + opt->len; 17390a708f8fSGustavo F. Padovan *ptr += len; 17400a708f8fSGustavo F. Padovan 17410a708f8fSGustavo F. Padovan *type = opt->type; 17420a708f8fSGustavo F. Padovan *olen = opt->len; 17430a708f8fSGustavo F. Padovan 17440a708f8fSGustavo F. Padovan switch (opt->len) { 17450a708f8fSGustavo F. Padovan case 1: 17460a708f8fSGustavo F. Padovan *val = *((u8 *) opt->val); 17470a708f8fSGustavo F. Padovan break; 17480a708f8fSGustavo F. Padovan 17490a708f8fSGustavo F. Padovan case 2: 17500a708f8fSGustavo F. Padovan *val = get_unaligned_le16(opt->val); 17510a708f8fSGustavo F. Padovan break; 17520a708f8fSGustavo F. Padovan 17530a708f8fSGustavo F. Padovan case 4: 17540a708f8fSGustavo F. Padovan *val = get_unaligned_le32(opt->val); 17550a708f8fSGustavo F. Padovan break; 17560a708f8fSGustavo F. Padovan 17570a708f8fSGustavo F. Padovan default: 17580a708f8fSGustavo F. Padovan *val = (unsigned long) opt->val; 17590a708f8fSGustavo F. Padovan break; 17600a708f8fSGustavo F. Padovan } 17610a708f8fSGustavo F. Padovan 17620a708f8fSGustavo F. Padovan BT_DBG("type 0x%2.2x len %d val 0x%lx", *type, opt->len, *val); 17630a708f8fSGustavo F. Padovan return len; 17640a708f8fSGustavo F. Padovan } 17650a708f8fSGustavo F. Padovan 17660a708f8fSGustavo F. Padovan static void l2cap_add_conf_opt(void **ptr, u8 type, u8 len, unsigned long val) 17670a708f8fSGustavo F. Padovan { 17680a708f8fSGustavo F. Padovan struct l2cap_conf_opt *opt = *ptr; 17690a708f8fSGustavo F. Padovan 17700a708f8fSGustavo F. Padovan BT_DBG("type 0x%2.2x len %d val 0x%lx", type, len, val); 17710a708f8fSGustavo F. Padovan 17720a708f8fSGustavo F. Padovan opt->type = type; 17730a708f8fSGustavo F. Padovan opt->len = len; 17740a708f8fSGustavo F. Padovan 17750a708f8fSGustavo F. Padovan switch (len) { 17760a708f8fSGustavo F. Padovan case 1: 17770a708f8fSGustavo F. Padovan *((u8 *) opt->val) = val; 17780a708f8fSGustavo F. Padovan break; 17790a708f8fSGustavo F. Padovan 17800a708f8fSGustavo F. Padovan case 2: 17810a708f8fSGustavo F. Padovan put_unaligned_le16(val, opt->val); 17820a708f8fSGustavo F. Padovan break; 17830a708f8fSGustavo F. Padovan 17840a708f8fSGustavo F. Padovan case 4: 17850a708f8fSGustavo F. Padovan put_unaligned_le32(val, opt->val); 17860a708f8fSGustavo F. Padovan break; 17870a708f8fSGustavo F. Padovan 17880a708f8fSGustavo F. Padovan default: 17890a708f8fSGustavo F. Padovan memcpy(opt->val, (void *) val, len); 17900a708f8fSGustavo F. Padovan break; 17910a708f8fSGustavo F. Padovan } 17920a708f8fSGustavo F. Padovan 17930a708f8fSGustavo F. Padovan *ptr += L2CAP_CONF_OPT_SIZE + len; 17940a708f8fSGustavo F. Padovan } 17950a708f8fSGustavo F. Padovan 17960a708f8fSGustavo F. Padovan static void l2cap_ack_timeout(unsigned long arg) 17970a708f8fSGustavo F. Padovan { 1798525cd185SGustavo F. Padovan struct l2cap_chan *chan = (void *) arg; 17990a708f8fSGustavo F. Padovan 1800525cd185SGustavo F. Padovan bh_lock_sock(chan->sk); 1801525cd185SGustavo F. Padovan l2cap_send_ack(chan); 1802525cd185SGustavo F. Padovan bh_unlock_sock(chan->sk); 18030a708f8fSGustavo F. Padovan } 18040a708f8fSGustavo F. Padovan 1805525cd185SGustavo F. Padovan static inline void l2cap_ertm_init(struct l2cap_chan *chan) 18060a708f8fSGustavo F. Padovan { 1807525cd185SGustavo F. Padovan struct sock *sk = chan->sk; 1808525cd185SGustavo F. Padovan 180942e5c802SGustavo F. Padovan chan->expected_ack_seq = 0; 18106a026610SGustavo F. Padovan chan->unacked_frames = 0; 181142e5c802SGustavo F. Padovan chan->buffer_seq = 0; 18126a026610SGustavo F. Padovan chan->num_acked = 0; 18136a026610SGustavo F. Padovan chan->frames_sent = 0; 18140a708f8fSGustavo F. Padovan 1815e92c8e70SGustavo F. Padovan setup_timer(&chan->retrans_timer, l2cap_retrans_timeout, 1816e92c8e70SGustavo F. Padovan (unsigned long) chan); 1817e92c8e70SGustavo F. Padovan setup_timer(&chan->monitor_timer, l2cap_monitor_timeout, 1818e92c8e70SGustavo F. Padovan (unsigned long) chan); 1819e92c8e70SGustavo F. Padovan setup_timer(&chan->ack_timer, l2cap_ack_timeout, (unsigned long) chan); 18200a708f8fSGustavo F. Padovan 1821f1c6775bSGustavo F. Padovan skb_queue_head_init(&chan->srej_q); 1822f1c6775bSGustavo F. Padovan skb_queue_head_init(&chan->busy_q); 18230a708f8fSGustavo F. Padovan 182439d5a3eeSGustavo F. Padovan INIT_LIST_HEAD(&chan->srej_l); 182539d5a3eeSGustavo F. Padovan 1826311bb895SGustavo F. Padovan INIT_WORK(&chan->busy_work, l2cap_busy_work); 18270a708f8fSGustavo F. Padovan 18280a708f8fSGustavo F. Padovan sk->sk_backlog_rcv = l2cap_ertm_data_rcv; 18290a708f8fSGustavo F. Padovan } 18300a708f8fSGustavo F. Padovan 18310a708f8fSGustavo F. Padovan static inline __u8 l2cap_select_mode(__u8 mode, __u16 remote_feat_mask) 18320a708f8fSGustavo F. Padovan { 18330a708f8fSGustavo F. Padovan switch (mode) { 18340a708f8fSGustavo F. Padovan case L2CAP_MODE_STREAMING: 18350a708f8fSGustavo F. Padovan case L2CAP_MODE_ERTM: 18360a708f8fSGustavo F. Padovan if (l2cap_mode_supported(mode, remote_feat_mask)) 18370a708f8fSGustavo F. Padovan return mode; 18380a708f8fSGustavo F. Padovan /* fall through */ 18390a708f8fSGustavo F. Padovan default: 18400a708f8fSGustavo F. Padovan return L2CAP_MODE_BASIC; 18410a708f8fSGustavo F. Padovan } 18420a708f8fSGustavo F. Padovan } 18430a708f8fSGustavo F. Padovan 1844710f9b0aSGustavo F. Padovan static int l2cap_build_conf_req(struct l2cap_chan *chan, void *data) 18450a708f8fSGustavo F. Padovan { 18460a708f8fSGustavo F. Padovan struct l2cap_conf_req *req = data; 18470c1bc5c6SGustavo F. Padovan struct l2cap_conf_rfc rfc = { .mode = chan->mode }; 18480a708f8fSGustavo F. Padovan void *ptr = req->data; 18490a708f8fSGustavo F. Padovan 185049208c9cSGustavo F. Padovan BT_DBG("chan %p", chan); 18510a708f8fSGustavo F. Padovan 185273ffa904SGustavo F. Padovan if (chan->num_conf_req || chan->num_conf_rsp) 18530a708f8fSGustavo F. Padovan goto done; 18540a708f8fSGustavo F. Padovan 18550c1bc5c6SGustavo F. Padovan switch (chan->mode) { 18560a708f8fSGustavo F. Padovan case L2CAP_MODE_STREAMING: 18570a708f8fSGustavo F. Padovan case L2CAP_MODE_ERTM: 1858b4450035SGustavo F. Padovan if (chan->conf_state & L2CAP_CONF_STATE2_DEVICE) 18590a708f8fSGustavo F. Padovan break; 18600a708f8fSGustavo F. Padovan 18610a708f8fSGustavo F. Padovan /* fall through */ 18620a708f8fSGustavo F. Padovan default: 18638c1d787bSGustavo F. Padovan chan->mode = l2cap_select_mode(rfc.mode, chan->conn->feat_mask); 18640a708f8fSGustavo F. Padovan break; 18650a708f8fSGustavo F. Padovan } 18660a708f8fSGustavo F. Padovan 18670a708f8fSGustavo F. Padovan done: 18680c1bc5c6SGustavo F. Padovan if (chan->imtu != L2CAP_DEFAULT_MTU) 18690c1bc5c6SGustavo F. Padovan l2cap_add_conf_opt(&ptr, L2CAP_CONF_MTU, 2, chan->imtu); 18700a708f8fSGustavo F. Padovan 18710c1bc5c6SGustavo F. Padovan switch (chan->mode) { 18720a708f8fSGustavo F. Padovan case L2CAP_MODE_BASIC: 18738c1d787bSGustavo F. Padovan if (!(chan->conn->feat_mask & L2CAP_FEAT_ERTM) && 18748c1d787bSGustavo F. Padovan !(chan->conn->feat_mask & L2CAP_FEAT_STREAMING)) 18750a708f8fSGustavo F. Padovan break; 18760a708f8fSGustavo F. Padovan 18770a708f8fSGustavo F. Padovan rfc.mode = L2CAP_MODE_BASIC; 18780a708f8fSGustavo F. Padovan rfc.txwin_size = 0; 18790a708f8fSGustavo F. Padovan rfc.max_transmit = 0; 18800a708f8fSGustavo F. Padovan rfc.retrans_timeout = 0; 18810a708f8fSGustavo F. Padovan rfc.monitor_timeout = 0; 18820a708f8fSGustavo F. Padovan rfc.max_pdu_size = 0; 18830a708f8fSGustavo F. Padovan 18840a708f8fSGustavo F. Padovan l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC, sizeof(rfc), 18850a708f8fSGustavo F. Padovan (unsigned long) &rfc); 18860a708f8fSGustavo F. Padovan break; 18870a708f8fSGustavo F. Padovan 18880a708f8fSGustavo F. Padovan case L2CAP_MODE_ERTM: 18890a708f8fSGustavo F. Padovan rfc.mode = L2CAP_MODE_ERTM; 189047d1ec61SGustavo F. Padovan rfc.txwin_size = chan->tx_win; 189147d1ec61SGustavo F. Padovan rfc.max_transmit = chan->max_tx; 18920a708f8fSGustavo F. Padovan rfc.retrans_timeout = 0; 18930a708f8fSGustavo F. Padovan rfc.monitor_timeout = 0; 18940a708f8fSGustavo F. Padovan rfc.max_pdu_size = cpu_to_le16(L2CAP_DEFAULT_MAX_PDU_SIZE); 18958c1d787bSGustavo F. Padovan if (L2CAP_DEFAULT_MAX_PDU_SIZE > chan->conn->mtu - 10) 18968c1d787bSGustavo F. Padovan rfc.max_pdu_size = cpu_to_le16(chan->conn->mtu - 10); 18970a708f8fSGustavo F. Padovan 18980a708f8fSGustavo F. Padovan l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC, sizeof(rfc), 18990a708f8fSGustavo F. Padovan (unsigned long) &rfc); 19000a708f8fSGustavo F. Padovan 19018c1d787bSGustavo F. Padovan if (!(chan->conn->feat_mask & L2CAP_FEAT_FCS)) 19020a708f8fSGustavo F. Padovan break; 19030a708f8fSGustavo F. Padovan 190447d1ec61SGustavo F. Padovan if (chan->fcs == L2CAP_FCS_NONE || 1905b4450035SGustavo F. Padovan chan->conf_state & L2CAP_CONF_NO_FCS_RECV) { 190647d1ec61SGustavo F. Padovan chan->fcs = L2CAP_FCS_NONE; 190747d1ec61SGustavo F. Padovan l2cap_add_conf_opt(&ptr, L2CAP_CONF_FCS, 1, chan->fcs); 19080a708f8fSGustavo F. Padovan } 19090a708f8fSGustavo F. Padovan break; 19100a708f8fSGustavo F. Padovan 19110a708f8fSGustavo F. Padovan case L2CAP_MODE_STREAMING: 19120a708f8fSGustavo F. Padovan rfc.mode = L2CAP_MODE_STREAMING; 19130a708f8fSGustavo F. Padovan rfc.txwin_size = 0; 19140a708f8fSGustavo F. Padovan rfc.max_transmit = 0; 19150a708f8fSGustavo F. Padovan rfc.retrans_timeout = 0; 19160a708f8fSGustavo F. Padovan rfc.monitor_timeout = 0; 19170a708f8fSGustavo F. Padovan rfc.max_pdu_size = cpu_to_le16(L2CAP_DEFAULT_MAX_PDU_SIZE); 19188c1d787bSGustavo F. Padovan if (L2CAP_DEFAULT_MAX_PDU_SIZE > chan->conn->mtu - 10) 19198c1d787bSGustavo F. Padovan rfc.max_pdu_size = cpu_to_le16(chan->conn->mtu - 10); 19200a708f8fSGustavo F. Padovan 19210a708f8fSGustavo F. Padovan l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC, sizeof(rfc), 19220a708f8fSGustavo F. Padovan (unsigned long) &rfc); 19230a708f8fSGustavo F. Padovan 19248c1d787bSGustavo F. Padovan if (!(chan->conn->feat_mask & L2CAP_FEAT_FCS)) 19250a708f8fSGustavo F. Padovan break; 19260a708f8fSGustavo F. Padovan 192747d1ec61SGustavo F. Padovan if (chan->fcs == L2CAP_FCS_NONE || 1928b4450035SGustavo F. Padovan chan->conf_state & L2CAP_CONF_NO_FCS_RECV) { 192947d1ec61SGustavo F. Padovan chan->fcs = L2CAP_FCS_NONE; 193047d1ec61SGustavo F. Padovan l2cap_add_conf_opt(&ptr, L2CAP_CONF_FCS, 1, chan->fcs); 19310a708f8fSGustavo F. Padovan } 19320a708f8fSGustavo F. Padovan break; 19330a708f8fSGustavo F. Padovan } 19340a708f8fSGustavo F. Padovan 1935fe4128e0SGustavo F. Padovan req->dcid = cpu_to_le16(chan->dcid); 19360a708f8fSGustavo F. Padovan req->flags = cpu_to_le16(0); 19370a708f8fSGustavo F. Padovan 19380a708f8fSGustavo F. Padovan return ptr - data; 19390a708f8fSGustavo F. Padovan } 19400a708f8fSGustavo F. Padovan 194173ffa904SGustavo F. Padovan static int l2cap_parse_conf_req(struct l2cap_chan *chan, void *data) 19420a708f8fSGustavo F. Padovan { 19430a708f8fSGustavo F. Padovan struct l2cap_conf_rsp *rsp = data; 19440a708f8fSGustavo F. Padovan void *ptr = rsp->data; 194573ffa904SGustavo F. Padovan void *req = chan->conf_req; 194673ffa904SGustavo F. Padovan int len = chan->conf_len; 19470a708f8fSGustavo F. Padovan int type, hint, olen; 19480a708f8fSGustavo F. Padovan unsigned long val; 19490a708f8fSGustavo F. Padovan struct l2cap_conf_rfc rfc = { .mode = L2CAP_MODE_BASIC }; 19500a708f8fSGustavo F. Padovan u16 mtu = L2CAP_DEFAULT_MTU; 19510a708f8fSGustavo F. Padovan u16 result = L2CAP_CONF_SUCCESS; 19520a708f8fSGustavo F. Padovan 195373ffa904SGustavo F. Padovan BT_DBG("chan %p", chan); 19540a708f8fSGustavo F. Padovan 19550a708f8fSGustavo F. Padovan while (len >= L2CAP_CONF_OPT_SIZE) { 19560a708f8fSGustavo F. Padovan len -= l2cap_get_conf_opt(&req, &type, &olen, &val); 19570a708f8fSGustavo F. Padovan 19580a708f8fSGustavo F. Padovan hint = type & L2CAP_CONF_HINT; 19590a708f8fSGustavo F. Padovan type &= L2CAP_CONF_MASK; 19600a708f8fSGustavo F. Padovan 19610a708f8fSGustavo F. Padovan switch (type) { 19620a708f8fSGustavo F. Padovan case L2CAP_CONF_MTU: 19630a708f8fSGustavo F. Padovan mtu = val; 19640a708f8fSGustavo F. Padovan break; 19650a708f8fSGustavo F. Padovan 19660a708f8fSGustavo F. Padovan case L2CAP_CONF_FLUSH_TO: 19670c1bc5c6SGustavo F. Padovan chan->flush_to = val; 19680a708f8fSGustavo F. Padovan break; 19690a708f8fSGustavo F. Padovan 19700a708f8fSGustavo F. Padovan case L2CAP_CONF_QOS: 19710a708f8fSGustavo F. Padovan break; 19720a708f8fSGustavo F. Padovan 19730a708f8fSGustavo F. Padovan case L2CAP_CONF_RFC: 19740a708f8fSGustavo F. Padovan if (olen == sizeof(rfc)) 19750a708f8fSGustavo F. Padovan memcpy(&rfc, (void *) val, olen); 19760a708f8fSGustavo F. Padovan break; 19770a708f8fSGustavo F. Padovan 19780a708f8fSGustavo F. Padovan case L2CAP_CONF_FCS: 19790a708f8fSGustavo F. Padovan if (val == L2CAP_FCS_NONE) 1980b4450035SGustavo F. Padovan chan->conf_state |= L2CAP_CONF_NO_FCS_RECV; 19810a708f8fSGustavo F. Padovan 19820a708f8fSGustavo F. Padovan break; 19830a708f8fSGustavo F. Padovan 19840a708f8fSGustavo F. Padovan default: 19850a708f8fSGustavo F. Padovan if (hint) 19860a708f8fSGustavo F. Padovan break; 19870a708f8fSGustavo F. Padovan 19880a708f8fSGustavo F. Padovan result = L2CAP_CONF_UNKNOWN; 19890a708f8fSGustavo F. Padovan *((u8 *) ptr++) = type; 19900a708f8fSGustavo F. Padovan break; 19910a708f8fSGustavo F. Padovan } 19920a708f8fSGustavo F. Padovan } 19930a708f8fSGustavo F. Padovan 199473ffa904SGustavo F. Padovan if (chan->num_conf_rsp || chan->num_conf_req > 1) 19950a708f8fSGustavo F. Padovan goto done; 19960a708f8fSGustavo F. Padovan 19970c1bc5c6SGustavo F. Padovan switch (chan->mode) { 19980a708f8fSGustavo F. Padovan case L2CAP_MODE_STREAMING: 19990a708f8fSGustavo F. Padovan case L2CAP_MODE_ERTM: 2000b4450035SGustavo F. Padovan if (!(chan->conf_state & L2CAP_CONF_STATE2_DEVICE)) { 20010c1bc5c6SGustavo F. Padovan chan->mode = l2cap_select_mode(rfc.mode, 20028c1d787bSGustavo F. Padovan chan->conn->feat_mask); 20030a708f8fSGustavo F. Padovan break; 20040a708f8fSGustavo F. Padovan } 20050a708f8fSGustavo F. Padovan 20060c1bc5c6SGustavo F. Padovan if (chan->mode != rfc.mode) 20070a708f8fSGustavo F. Padovan return -ECONNREFUSED; 20080a708f8fSGustavo F. Padovan 20090a708f8fSGustavo F. Padovan break; 20100a708f8fSGustavo F. Padovan } 20110a708f8fSGustavo F. Padovan 20120a708f8fSGustavo F. Padovan done: 20130c1bc5c6SGustavo F. Padovan if (chan->mode != rfc.mode) { 20140a708f8fSGustavo F. Padovan result = L2CAP_CONF_UNACCEPT; 20150c1bc5c6SGustavo F. Padovan rfc.mode = chan->mode; 20160a708f8fSGustavo F. Padovan 201773ffa904SGustavo F. Padovan if (chan->num_conf_rsp == 1) 20180a708f8fSGustavo F. Padovan return -ECONNREFUSED; 20190a708f8fSGustavo F. Padovan 20200a708f8fSGustavo F. Padovan l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC, 20210a708f8fSGustavo F. Padovan sizeof(rfc), (unsigned long) &rfc); 20220a708f8fSGustavo F. Padovan } 20230a708f8fSGustavo F. Padovan 20240a708f8fSGustavo F. Padovan 20250a708f8fSGustavo F. Padovan if (result == L2CAP_CONF_SUCCESS) { 20260a708f8fSGustavo F. Padovan /* Configure output options and let the other side know 20270a708f8fSGustavo F. Padovan * which ones we don't like. */ 20280a708f8fSGustavo F. Padovan 20290a708f8fSGustavo F. Padovan if (mtu < L2CAP_DEFAULT_MIN_MTU) 20300a708f8fSGustavo F. Padovan result = L2CAP_CONF_UNACCEPT; 20310a708f8fSGustavo F. Padovan else { 20320c1bc5c6SGustavo F. Padovan chan->omtu = mtu; 2033b4450035SGustavo F. Padovan chan->conf_state |= L2CAP_CONF_MTU_DONE; 20340a708f8fSGustavo F. Padovan } 20350c1bc5c6SGustavo F. Padovan l2cap_add_conf_opt(&ptr, L2CAP_CONF_MTU, 2, chan->omtu); 20360a708f8fSGustavo F. Padovan 20370a708f8fSGustavo F. Padovan switch (rfc.mode) { 20380a708f8fSGustavo F. Padovan case L2CAP_MODE_BASIC: 203947d1ec61SGustavo F. Padovan chan->fcs = L2CAP_FCS_NONE; 2040b4450035SGustavo F. Padovan chan->conf_state |= L2CAP_CONF_MODE_DONE; 20410a708f8fSGustavo F. Padovan break; 20420a708f8fSGustavo F. Padovan 20430a708f8fSGustavo F. Padovan case L2CAP_MODE_ERTM: 20442c03a7a4SGustavo F. Padovan chan->remote_tx_win = rfc.txwin_size; 20452c03a7a4SGustavo F. Padovan chan->remote_max_tx = rfc.max_transmit; 20460a708f8fSGustavo F. Padovan 20478c1d787bSGustavo F. Padovan if (le16_to_cpu(rfc.max_pdu_size) > chan->conn->mtu - 10) 20488c1d787bSGustavo F. Padovan rfc.max_pdu_size = cpu_to_le16(chan->conn->mtu - 10); 20490a708f8fSGustavo F. Padovan 20502c03a7a4SGustavo F. Padovan chan->remote_mps = le16_to_cpu(rfc.max_pdu_size); 20510a708f8fSGustavo F. Padovan 20520a708f8fSGustavo F. Padovan rfc.retrans_timeout = 20530a708f8fSGustavo F. Padovan le16_to_cpu(L2CAP_DEFAULT_RETRANS_TO); 20540a708f8fSGustavo F. Padovan rfc.monitor_timeout = 20550a708f8fSGustavo F. Padovan le16_to_cpu(L2CAP_DEFAULT_MONITOR_TO); 20560a708f8fSGustavo F. Padovan 2057b4450035SGustavo F. Padovan chan->conf_state |= L2CAP_CONF_MODE_DONE; 20580a708f8fSGustavo F. Padovan 20590a708f8fSGustavo F. Padovan l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC, 20600a708f8fSGustavo F. Padovan sizeof(rfc), (unsigned long) &rfc); 20610a708f8fSGustavo F. Padovan 20620a708f8fSGustavo F. Padovan break; 20630a708f8fSGustavo F. Padovan 20640a708f8fSGustavo F. Padovan case L2CAP_MODE_STREAMING: 20658c1d787bSGustavo F. Padovan if (le16_to_cpu(rfc.max_pdu_size) > chan->conn->mtu - 10) 20668c1d787bSGustavo F. Padovan rfc.max_pdu_size = cpu_to_le16(chan->conn->mtu - 10); 20670a708f8fSGustavo F. Padovan 20682c03a7a4SGustavo F. Padovan chan->remote_mps = le16_to_cpu(rfc.max_pdu_size); 20690a708f8fSGustavo F. Padovan 2070b4450035SGustavo F. Padovan chan->conf_state |= L2CAP_CONF_MODE_DONE; 20710a708f8fSGustavo F. Padovan 20720a708f8fSGustavo F. Padovan l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC, 20730a708f8fSGustavo F. Padovan sizeof(rfc), (unsigned long) &rfc); 20740a708f8fSGustavo F. Padovan 20750a708f8fSGustavo F. Padovan break; 20760a708f8fSGustavo F. Padovan 20770a708f8fSGustavo F. Padovan default: 20780a708f8fSGustavo F. Padovan result = L2CAP_CONF_UNACCEPT; 20790a708f8fSGustavo F. Padovan 20800a708f8fSGustavo F. Padovan memset(&rfc, 0, sizeof(rfc)); 20810c1bc5c6SGustavo F. Padovan rfc.mode = chan->mode; 20820a708f8fSGustavo F. Padovan } 20830a708f8fSGustavo F. Padovan 20840a708f8fSGustavo F. Padovan if (result == L2CAP_CONF_SUCCESS) 2085b4450035SGustavo F. Padovan chan->conf_state |= L2CAP_CONF_OUTPUT_DONE; 20860a708f8fSGustavo F. Padovan } 2087fe4128e0SGustavo F. Padovan rsp->scid = cpu_to_le16(chan->dcid); 20880a708f8fSGustavo F. Padovan rsp->result = cpu_to_le16(result); 20890a708f8fSGustavo F. Padovan rsp->flags = cpu_to_le16(0x0000); 20900a708f8fSGustavo F. Padovan 20910a708f8fSGustavo F. Padovan return ptr - data; 20920a708f8fSGustavo F. Padovan } 20930a708f8fSGustavo F. Padovan 2094b4450035SGustavo F. Padovan static int l2cap_parse_conf_rsp(struct l2cap_chan *chan, void *rsp, int len, void *data, u16 *result) 20950a708f8fSGustavo F. Padovan { 20960a708f8fSGustavo F. Padovan struct l2cap_conf_req *req = data; 20970a708f8fSGustavo F. Padovan void *ptr = req->data; 20980a708f8fSGustavo F. Padovan int type, olen; 20990a708f8fSGustavo F. Padovan unsigned long val; 21000a708f8fSGustavo F. Padovan struct l2cap_conf_rfc rfc; 21010a708f8fSGustavo F. Padovan 2102fe4128e0SGustavo F. Padovan BT_DBG("chan %p, rsp %p, len %d, req %p", chan, rsp, len, data); 21030a708f8fSGustavo F. Padovan 21040a708f8fSGustavo F. Padovan while (len >= L2CAP_CONF_OPT_SIZE) { 21050a708f8fSGustavo F. Padovan len -= l2cap_get_conf_opt(&rsp, &type, &olen, &val); 21060a708f8fSGustavo F. Padovan 21070a708f8fSGustavo F. Padovan switch (type) { 21080a708f8fSGustavo F. Padovan case L2CAP_CONF_MTU: 21090a708f8fSGustavo F. Padovan if (val < L2CAP_DEFAULT_MIN_MTU) { 21100a708f8fSGustavo F. Padovan *result = L2CAP_CONF_UNACCEPT; 21110c1bc5c6SGustavo F. Padovan chan->imtu = L2CAP_DEFAULT_MIN_MTU; 21120a708f8fSGustavo F. Padovan } else 21130c1bc5c6SGustavo F. Padovan chan->imtu = val; 21140c1bc5c6SGustavo F. Padovan l2cap_add_conf_opt(&ptr, L2CAP_CONF_MTU, 2, chan->imtu); 21150a708f8fSGustavo F. Padovan break; 21160a708f8fSGustavo F. Padovan 21170a708f8fSGustavo F. Padovan case L2CAP_CONF_FLUSH_TO: 21180c1bc5c6SGustavo F. Padovan chan->flush_to = val; 21190a708f8fSGustavo F. Padovan l2cap_add_conf_opt(&ptr, L2CAP_CONF_FLUSH_TO, 21200c1bc5c6SGustavo F. Padovan 2, chan->flush_to); 21210a708f8fSGustavo F. Padovan break; 21220a708f8fSGustavo F. Padovan 21230a708f8fSGustavo F. Padovan case L2CAP_CONF_RFC: 21240a708f8fSGustavo F. Padovan if (olen == sizeof(rfc)) 21250a708f8fSGustavo F. Padovan memcpy(&rfc, (void *)val, olen); 21260a708f8fSGustavo F. Padovan 2127b4450035SGustavo F. Padovan if ((chan->conf_state & L2CAP_CONF_STATE2_DEVICE) && 21280c1bc5c6SGustavo F. Padovan rfc.mode != chan->mode) 21290a708f8fSGustavo F. Padovan return -ECONNREFUSED; 21300a708f8fSGustavo F. Padovan 213147d1ec61SGustavo F. Padovan chan->fcs = 0; 21320a708f8fSGustavo F. Padovan 21330a708f8fSGustavo F. Padovan l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC, 21340a708f8fSGustavo F. Padovan sizeof(rfc), (unsigned long) &rfc); 21350a708f8fSGustavo F. Padovan break; 21360a708f8fSGustavo F. Padovan } 21370a708f8fSGustavo F. Padovan } 21380a708f8fSGustavo F. Padovan 21390c1bc5c6SGustavo F. Padovan if (chan->mode == L2CAP_MODE_BASIC && chan->mode != rfc.mode) 21400a708f8fSGustavo F. Padovan return -ECONNREFUSED; 21410a708f8fSGustavo F. Padovan 21420c1bc5c6SGustavo F. Padovan chan->mode = rfc.mode; 21430a708f8fSGustavo F. Padovan 21440a708f8fSGustavo F. Padovan if (*result == L2CAP_CONF_SUCCESS) { 21450a708f8fSGustavo F. Padovan switch (rfc.mode) { 21460a708f8fSGustavo F. Padovan case L2CAP_MODE_ERTM: 214747d1ec61SGustavo F. Padovan chan->retrans_timeout = le16_to_cpu(rfc.retrans_timeout); 214847d1ec61SGustavo F. Padovan chan->monitor_timeout = le16_to_cpu(rfc.monitor_timeout); 214947d1ec61SGustavo F. Padovan chan->mps = le16_to_cpu(rfc.max_pdu_size); 21500a708f8fSGustavo F. Padovan break; 21510a708f8fSGustavo F. Padovan case L2CAP_MODE_STREAMING: 215247d1ec61SGustavo F. Padovan chan->mps = le16_to_cpu(rfc.max_pdu_size); 21530a708f8fSGustavo F. Padovan } 21540a708f8fSGustavo F. Padovan } 21550a708f8fSGustavo F. Padovan 2156fe4128e0SGustavo F. Padovan req->dcid = cpu_to_le16(chan->dcid); 21570a708f8fSGustavo F. Padovan req->flags = cpu_to_le16(0x0000); 21580a708f8fSGustavo F. Padovan 21590a708f8fSGustavo F. Padovan return ptr - data; 21600a708f8fSGustavo F. Padovan } 21610a708f8fSGustavo F. Padovan 2162fe4128e0SGustavo F. Padovan static int l2cap_build_conf_rsp(struct l2cap_chan *chan, void *data, u16 result, u16 flags) 21630a708f8fSGustavo F. Padovan { 21640a708f8fSGustavo F. Padovan struct l2cap_conf_rsp *rsp = data; 21650a708f8fSGustavo F. Padovan void *ptr = rsp->data; 21660a708f8fSGustavo F. Padovan 2167fe4128e0SGustavo F. Padovan BT_DBG("chan %p", chan); 21680a708f8fSGustavo F. Padovan 2169fe4128e0SGustavo F. Padovan rsp->scid = cpu_to_le16(chan->dcid); 21700a708f8fSGustavo F. Padovan rsp->result = cpu_to_le16(result); 21710a708f8fSGustavo F. Padovan rsp->flags = cpu_to_le16(flags); 21720a708f8fSGustavo F. Padovan 21730a708f8fSGustavo F. Padovan return ptr - data; 21740a708f8fSGustavo F. Padovan } 21750a708f8fSGustavo F. Padovan 21768c1d787bSGustavo F. Padovan void __l2cap_connect_rsp_defer(struct l2cap_chan *chan) 2177710f9b0aSGustavo F. Padovan { 2178710f9b0aSGustavo F. Padovan struct l2cap_conn_rsp rsp; 21798c1d787bSGustavo F. Padovan struct l2cap_conn *conn = chan->conn; 2180710f9b0aSGustavo F. Padovan u8 buf[128]; 2181710f9b0aSGustavo F. Padovan 2182fe4128e0SGustavo F. Padovan rsp.scid = cpu_to_le16(chan->dcid); 2183fe4128e0SGustavo F. Padovan rsp.dcid = cpu_to_le16(chan->scid); 2184710f9b0aSGustavo F. Padovan rsp.result = cpu_to_le16(L2CAP_CR_SUCCESS); 2185710f9b0aSGustavo F. Padovan rsp.status = cpu_to_le16(L2CAP_CS_NO_INFO); 2186710f9b0aSGustavo F. Padovan l2cap_send_cmd(conn, chan->ident, 2187710f9b0aSGustavo F. Padovan L2CAP_CONN_RSP, sizeof(rsp), &rsp); 2188710f9b0aSGustavo F. Padovan 2189b4450035SGustavo F. Padovan if (chan->conf_state & L2CAP_CONF_REQ_SENT) 2190710f9b0aSGustavo F. Padovan return; 2191710f9b0aSGustavo F. Padovan 2192b4450035SGustavo F. Padovan chan->conf_state |= L2CAP_CONF_REQ_SENT; 2193710f9b0aSGustavo F. Padovan l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ, 2194710f9b0aSGustavo F. Padovan l2cap_build_conf_req(chan, buf), buf); 2195710f9b0aSGustavo F. Padovan chan->num_conf_req++; 2196710f9b0aSGustavo F. Padovan } 2197710f9b0aSGustavo F. Padovan 219847d1ec61SGustavo F. Padovan static void l2cap_conf_rfc_get(struct l2cap_chan *chan, void *rsp, int len) 21990a708f8fSGustavo F. Padovan { 22000a708f8fSGustavo F. Padovan int type, olen; 22010a708f8fSGustavo F. Padovan unsigned long val; 22020a708f8fSGustavo F. Padovan struct l2cap_conf_rfc rfc; 22030a708f8fSGustavo F. Padovan 220447d1ec61SGustavo F. Padovan BT_DBG("chan %p, rsp %p, len %d", chan, rsp, len); 22050a708f8fSGustavo F. Padovan 22060c1bc5c6SGustavo F. Padovan if ((chan->mode != L2CAP_MODE_ERTM) && (chan->mode != L2CAP_MODE_STREAMING)) 22070a708f8fSGustavo F. Padovan return; 22080a708f8fSGustavo F. Padovan 22090a708f8fSGustavo F. Padovan while (len >= L2CAP_CONF_OPT_SIZE) { 22100a708f8fSGustavo F. Padovan len -= l2cap_get_conf_opt(&rsp, &type, &olen, &val); 22110a708f8fSGustavo F. Padovan 22120a708f8fSGustavo F. Padovan switch (type) { 22130a708f8fSGustavo F. Padovan case L2CAP_CONF_RFC: 22140a708f8fSGustavo F. Padovan if (olen == sizeof(rfc)) 22150a708f8fSGustavo F. Padovan memcpy(&rfc, (void *)val, olen); 22160a708f8fSGustavo F. Padovan goto done; 22170a708f8fSGustavo F. Padovan } 22180a708f8fSGustavo F. Padovan } 22190a708f8fSGustavo F. Padovan 22200a708f8fSGustavo F. Padovan done: 22210a708f8fSGustavo F. Padovan switch (rfc.mode) { 22220a708f8fSGustavo F. Padovan case L2CAP_MODE_ERTM: 222347d1ec61SGustavo F. Padovan chan->retrans_timeout = le16_to_cpu(rfc.retrans_timeout); 222447d1ec61SGustavo F. Padovan chan->monitor_timeout = le16_to_cpu(rfc.monitor_timeout); 222547d1ec61SGustavo F. Padovan chan->mps = le16_to_cpu(rfc.max_pdu_size); 22260a708f8fSGustavo F. Padovan break; 22270a708f8fSGustavo F. Padovan case L2CAP_MODE_STREAMING: 222847d1ec61SGustavo F. Padovan chan->mps = le16_to_cpu(rfc.max_pdu_size); 22290a708f8fSGustavo F. Padovan } 22300a708f8fSGustavo F. Padovan } 22310a708f8fSGustavo F. Padovan 22320a708f8fSGustavo F. Padovan static inline int l2cap_command_rej(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data) 22330a708f8fSGustavo F. Padovan { 22340a708f8fSGustavo F. Padovan struct l2cap_cmd_rej *rej = (struct l2cap_cmd_rej *) data; 22350a708f8fSGustavo F. Padovan 22360a708f8fSGustavo F. Padovan if (rej->reason != 0x0000) 22370a708f8fSGustavo F. Padovan return 0; 22380a708f8fSGustavo F. Padovan 22390a708f8fSGustavo F. Padovan if ((conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_SENT) && 22400a708f8fSGustavo F. Padovan cmd->ident == conn->info_ident) { 22410a708f8fSGustavo F. Padovan del_timer(&conn->info_timer); 22420a708f8fSGustavo F. Padovan 22430a708f8fSGustavo F. Padovan conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_DONE; 22440a708f8fSGustavo F. Padovan conn->info_ident = 0; 22450a708f8fSGustavo F. Padovan 22460a708f8fSGustavo F. Padovan l2cap_conn_start(conn); 22470a708f8fSGustavo F. Padovan } 22480a708f8fSGustavo F. Padovan 22490a708f8fSGustavo F. Padovan return 0; 22500a708f8fSGustavo F. Padovan } 22510a708f8fSGustavo F. Padovan 22520a708f8fSGustavo F. Padovan static inline int l2cap_connect_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data) 22530a708f8fSGustavo F. Padovan { 22540a708f8fSGustavo F. Padovan struct l2cap_conn_req *req = (struct l2cap_conn_req *) data; 22550a708f8fSGustavo F. Padovan struct l2cap_conn_rsp rsp; 225623691d75SGustavo F. Padovan struct l2cap_chan *chan = NULL, *pchan; 22570a708f8fSGustavo F. Padovan struct sock *parent, *sk = NULL; 22580a708f8fSGustavo F. Padovan int result, status = L2CAP_CS_NO_INFO; 22590a708f8fSGustavo F. Padovan 22600a708f8fSGustavo F. Padovan u16 dcid = 0, scid = __le16_to_cpu(req->scid); 22610a708f8fSGustavo F. Padovan __le16 psm = req->psm; 22620a708f8fSGustavo F. Padovan 22630a708f8fSGustavo F. Padovan BT_DBG("psm 0x%2.2x scid 0x%4.4x", psm, scid); 22640a708f8fSGustavo F. Padovan 22650a708f8fSGustavo F. Padovan /* Check if we have socket listening on psm */ 226623691d75SGustavo F. Padovan pchan = l2cap_global_chan_by_psm(BT_LISTEN, psm, conn->src); 226723691d75SGustavo F. Padovan if (!pchan) { 22680a708f8fSGustavo F. Padovan result = L2CAP_CR_BAD_PSM; 22690a708f8fSGustavo F. Padovan goto sendresp; 22700a708f8fSGustavo F. Padovan } 22710a708f8fSGustavo F. Padovan 227223691d75SGustavo F. Padovan parent = pchan->sk; 227323691d75SGustavo F. Padovan 22740a708f8fSGustavo F. Padovan bh_lock_sock(parent); 22750a708f8fSGustavo F. Padovan 22760a708f8fSGustavo F. Padovan /* Check if the ACL is secure enough (if not SDP) */ 22770a708f8fSGustavo F. Padovan if (psm != cpu_to_le16(0x0001) && 22780a708f8fSGustavo F. Padovan !hci_conn_check_link_mode(conn->hcon)) { 22790a708f8fSGustavo F. Padovan conn->disc_reason = 0x05; 22800a708f8fSGustavo F. Padovan result = L2CAP_CR_SEC_BLOCK; 22810a708f8fSGustavo F. Padovan goto response; 22820a708f8fSGustavo F. Padovan } 22830a708f8fSGustavo F. Padovan 22840a708f8fSGustavo F. Padovan result = L2CAP_CR_NO_MEM; 22850a708f8fSGustavo F. Padovan 22860a708f8fSGustavo F. Padovan /* Check for backlog size */ 22870a708f8fSGustavo F. Padovan if (sk_acceptq_is_full(parent)) { 22880a708f8fSGustavo F. Padovan BT_DBG("backlog full %d", parent->sk_ack_backlog); 22890a708f8fSGustavo F. Padovan goto response; 22900a708f8fSGustavo F. Padovan } 22910a708f8fSGustavo F. Padovan 22920a708f8fSGustavo F. Padovan sk = l2cap_sock_alloc(sock_net(parent), NULL, BTPROTO_L2CAP, GFP_ATOMIC); 22930a708f8fSGustavo F. Padovan if (!sk) 22940a708f8fSGustavo F. Padovan goto response; 22950a708f8fSGustavo F. Padovan 229623691d75SGustavo F. Padovan chan = l2cap_chan_create(sk); 229748454079SGustavo F. Padovan if (!chan) { 229848454079SGustavo F. Padovan l2cap_sock_kill(sk); 229948454079SGustavo F. Padovan goto response; 230048454079SGustavo F. Padovan } 230148454079SGustavo F. Padovan 23025d41ce1dSGustavo F. Padovan l2cap_pi(sk)->chan = chan; 23035d41ce1dSGustavo F. Padovan 2304baa7e1faSGustavo F. Padovan write_lock_bh(&conn->chan_lock); 23050a708f8fSGustavo F. Padovan 23060a708f8fSGustavo F. Padovan /* Check if we already have channel with that dcid */ 2307baa7e1faSGustavo F. Padovan if (__l2cap_get_chan_by_dcid(conn, scid)) { 2308baa7e1faSGustavo F. Padovan write_unlock_bh(&conn->chan_lock); 23090a708f8fSGustavo F. Padovan sock_set_flag(sk, SOCK_ZAPPED); 23100a708f8fSGustavo F. Padovan l2cap_sock_kill(sk); 23110a708f8fSGustavo F. Padovan goto response; 23120a708f8fSGustavo F. Padovan } 23130a708f8fSGustavo F. Padovan 23140a708f8fSGustavo F. Padovan hci_conn_hold(conn->hcon); 23150a708f8fSGustavo F. Padovan 23160a708f8fSGustavo F. Padovan l2cap_sock_init(sk, parent); 23170a708f8fSGustavo F. Padovan bacpy(&bt_sk(sk)->src, conn->src); 23180a708f8fSGustavo F. Padovan bacpy(&bt_sk(sk)->dst, conn->dst); 2319fe4128e0SGustavo F. Padovan chan->psm = psm; 2320fe4128e0SGustavo F. Padovan chan->dcid = scid; 23210a708f8fSGustavo F. Padovan 2322d1010240SGustavo F. Padovan bt_accept_enqueue(parent, sk); 2323d1010240SGustavo F. Padovan 232448454079SGustavo F. Padovan __l2cap_chan_add(conn, chan); 232548454079SGustavo F. Padovan 2326fe4128e0SGustavo F. Padovan dcid = chan->scid; 23270a708f8fSGustavo F. Padovan 23280a708f8fSGustavo F. Padovan l2cap_sock_set_timer(sk, sk->sk_sndtimeo); 23290a708f8fSGustavo F. Padovan 2330fc7f8a7eSGustavo F. Padovan chan->ident = cmd->ident; 23310a708f8fSGustavo F. Padovan 23320a708f8fSGustavo F. Padovan if (conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_DONE) { 23334343478fSGustavo F. Padovan if (l2cap_check_security(chan)) { 23340a708f8fSGustavo F. Padovan if (bt_sk(sk)->defer_setup) { 23350a708f8fSGustavo F. Padovan sk->sk_state = BT_CONNECT2; 23360a708f8fSGustavo F. Padovan result = L2CAP_CR_PEND; 23370a708f8fSGustavo F. Padovan status = L2CAP_CS_AUTHOR_PEND; 23380a708f8fSGustavo F. Padovan parent->sk_data_ready(parent, 0); 23390a708f8fSGustavo F. Padovan } else { 23400a708f8fSGustavo F. Padovan sk->sk_state = BT_CONFIG; 23410a708f8fSGustavo F. Padovan result = L2CAP_CR_SUCCESS; 23420a708f8fSGustavo F. Padovan status = L2CAP_CS_NO_INFO; 23430a708f8fSGustavo F. Padovan } 23440a708f8fSGustavo F. Padovan } else { 23450a708f8fSGustavo F. Padovan sk->sk_state = BT_CONNECT2; 23460a708f8fSGustavo F. Padovan result = L2CAP_CR_PEND; 23470a708f8fSGustavo F. Padovan status = L2CAP_CS_AUTHEN_PEND; 23480a708f8fSGustavo F. Padovan } 23490a708f8fSGustavo F. Padovan } else { 23500a708f8fSGustavo F. Padovan sk->sk_state = BT_CONNECT2; 23510a708f8fSGustavo F. Padovan result = L2CAP_CR_PEND; 23520a708f8fSGustavo F. Padovan status = L2CAP_CS_NO_INFO; 23530a708f8fSGustavo F. Padovan } 23540a708f8fSGustavo F. Padovan 2355baa7e1faSGustavo F. Padovan write_unlock_bh(&conn->chan_lock); 23560a708f8fSGustavo F. Padovan 23570a708f8fSGustavo F. Padovan response: 23580a708f8fSGustavo F. Padovan bh_unlock_sock(parent); 23590a708f8fSGustavo F. Padovan 23600a708f8fSGustavo F. Padovan sendresp: 23610a708f8fSGustavo F. Padovan rsp.scid = cpu_to_le16(scid); 23620a708f8fSGustavo F. Padovan rsp.dcid = cpu_to_le16(dcid); 23630a708f8fSGustavo F. Padovan rsp.result = cpu_to_le16(result); 23640a708f8fSGustavo F. Padovan rsp.status = cpu_to_le16(status); 23650a708f8fSGustavo F. Padovan l2cap_send_cmd(conn, cmd->ident, L2CAP_CONN_RSP, sizeof(rsp), &rsp); 23660a708f8fSGustavo F. Padovan 23670a708f8fSGustavo F. Padovan if (result == L2CAP_CR_PEND && status == L2CAP_CS_NO_INFO) { 23680a708f8fSGustavo F. Padovan struct l2cap_info_req info; 23690a708f8fSGustavo F. Padovan info.type = cpu_to_le16(L2CAP_IT_FEAT_MASK); 23700a708f8fSGustavo F. Padovan 23710a708f8fSGustavo F. Padovan conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_SENT; 23720a708f8fSGustavo F. Padovan conn->info_ident = l2cap_get_ident(conn); 23730a708f8fSGustavo F. Padovan 23740a708f8fSGustavo F. Padovan mod_timer(&conn->info_timer, jiffies + 23750a708f8fSGustavo F. Padovan msecs_to_jiffies(L2CAP_INFO_TIMEOUT)); 23760a708f8fSGustavo F. Padovan 23770a708f8fSGustavo F. Padovan l2cap_send_cmd(conn, conn->info_ident, 23780a708f8fSGustavo F. Padovan L2CAP_INFO_REQ, sizeof(info), &info); 23790a708f8fSGustavo F. Padovan } 23800a708f8fSGustavo F. Padovan 2381b4450035SGustavo F. Padovan if (chan && !(chan->conf_state & L2CAP_CONF_REQ_SENT) && 23820a708f8fSGustavo F. Padovan result == L2CAP_CR_SUCCESS) { 23830a708f8fSGustavo F. Padovan u8 buf[128]; 2384b4450035SGustavo F. Padovan chan->conf_state |= L2CAP_CONF_REQ_SENT; 23850a708f8fSGustavo F. Padovan l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ, 238673ffa904SGustavo F. Padovan l2cap_build_conf_req(chan, buf), buf); 238773ffa904SGustavo F. Padovan chan->num_conf_req++; 23880a708f8fSGustavo F. Padovan } 23890a708f8fSGustavo F. Padovan 23900a708f8fSGustavo F. Padovan return 0; 23910a708f8fSGustavo F. Padovan } 23920a708f8fSGustavo F. Padovan 23930a708f8fSGustavo F. Padovan static inline int l2cap_connect_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data) 23940a708f8fSGustavo F. Padovan { 23950a708f8fSGustavo F. Padovan struct l2cap_conn_rsp *rsp = (struct l2cap_conn_rsp *) data; 23960a708f8fSGustavo F. Padovan u16 scid, dcid, result, status; 239748454079SGustavo F. Padovan struct l2cap_chan *chan; 23980a708f8fSGustavo F. Padovan struct sock *sk; 23990a708f8fSGustavo F. Padovan u8 req[128]; 24000a708f8fSGustavo F. Padovan 24010a708f8fSGustavo F. Padovan scid = __le16_to_cpu(rsp->scid); 24020a708f8fSGustavo F. Padovan dcid = __le16_to_cpu(rsp->dcid); 24030a708f8fSGustavo F. Padovan result = __le16_to_cpu(rsp->result); 24040a708f8fSGustavo F. Padovan status = __le16_to_cpu(rsp->status); 24050a708f8fSGustavo F. Padovan 24060a708f8fSGustavo F. Padovan BT_DBG("dcid 0x%4.4x scid 0x%4.4x result 0x%2.2x status 0x%2.2x", dcid, scid, result, status); 24070a708f8fSGustavo F. Padovan 24080a708f8fSGustavo F. Padovan if (scid) { 2409baa7e1faSGustavo F. Padovan chan = l2cap_get_chan_by_scid(conn, scid); 241048454079SGustavo F. Padovan if (!chan) 24110a708f8fSGustavo F. Padovan return -EFAULT; 24120a708f8fSGustavo F. Padovan } else { 2413baa7e1faSGustavo F. Padovan chan = l2cap_get_chan_by_ident(conn, cmd->ident); 241448454079SGustavo F. Padovan if (!chan) 24150a708f8fSGustavo F. Padovan return -EFAULT; 24160a708f8fSGustavo F. Padovan } 24170a708f8fSGustavo F. Padovan 241848454079SGustavo F. Padovan sk = chan->sk; 241948454079SGustavo F. Padovan 24200a708f8fSGustavo F. Padovan switch (result) { 24210a708f8fSGustavo F. Padovan case L2CAP_CR_SUCCESS: 24220a708f8fSGustavo F. Padovan sk->sk_state = BT_CONFIG; 2423fc7f8a7eSGustavo F. Padovan chan->ident = 0; 2424fe4128e0SGustavo F. Padovan chan->dcid = dcid; 2425b4450035SGustavo F. Padovan chan->conf_state &= ~L2CAP_CONF_CONNECT_PEND; 24260a708f8fSGustavo F. Padovan 2427b4450035SGustavo F. Padovan if (chan->conf_state & L2CAP_CONF_REQ_SENT) 24280a708f8fSGustavo F. Padovan break; 24290a708f8fSGustavo F. Padovan 2430b4450035SGustavo F. Padovan chan->conf_state |= L2CAP_CONF_REQ_SENT; 24310a708f8fSGustavo F. Padovan 24320a708f8fSGustavo F. Padovan l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ, 243373ffa904SGustavo F. Padovan l2cap_build_conf_req(chan, req), req); 243473ffa904SGustavo F. Padovan chan->num_conf_req++; 24350a708f8fSGustavo F. Padovan break; 24360a708f8fSGustavo F. Padovan 24370a708f8fSGustavo F. Padovan case L2CAP_CR_PEND: 2438b4450035SGustavo F. Padovan chan->conf_state |= L2CAP_CONF_CONNECT_PEND; 24390a708f8fSGustavo F. Padovan break; 24400a708f8fSGustavo F. Padovan 24410a708f8fSGustavo F. Padovan default: 24420a708f8fSGustavo F. Padovan /* don't delete l2cap channel if sk is owned by user */ 24430a708f8fSGustavo F. Padovan if (sock_owned_by_user(sk)) { 24440a708f8fSGustavo F. Padovan sk->sk_state = BT_DISCONN; 24450a708f8fSGustavo F. Padovan l2cap_sock_clear_timer(sk); 24460a708f8fSGustavo F. Padovan l2cap_sock_set_timer(sk, HZ / 5); 24470a708f8fSGustavo F. Padovan break; 24480a708f8fSGustavo F. Padovan } 24490a708f8fSGustavo F. Padovan 245048454079SGustavo F. Padovan l2cap_chan_del(chan, ECONNREFUSED); 24510a708f8fSGustavo F. Padovan break; 24520a708f8fSGustavo F. Padovan } 24530a708f8fSGustavo F. Padovan 24540a708f8fSGustavo F. Padovan bh_unlock_sock(sk); 24550a708f8fSGustavo F. Padovan return 0; 24560a708f8fSGustavo F. Padovan } 24570a708f8fSGustavo F. Padovan 245847d1ec61SGustavo F. Padovan static inline void set_default_fcs(struct l2cap_chan *chan) 24590a708f8fSGustavo F. Padovan { 246047d1ec61SGustavo F. Padovan struct l2cap_pinfo *pi = l2cap_pi(chan->sk); 246147d1ec61SGustavo F. Padovan 24620a708f8fSGustavo F. Padovan /* FCS is enabled only in ERTM or streaming mode, if one or both 24630a708f8fSGustavo F. Padovan * sides request it. 24640a708f8fSGustavo F. Padovan */ 24650c1bc5c6SGustavo F. Padovan if (chan->mode != L2CAP_MODE_ERTM && chan->mode != L2CAP_MODE_STREAMING) 246647d1ec61SGustavo F. Padovan chan->fcs = L2CAP_FCS_NONE; 2467b4450035SGustavo F. Padovan else if (!(pi->chan->conf_state & L2CAP_CONF_NO_FCS_RECV)) 246847d1ec61SGustavo F. Padovan chan->fcs = L2CAP_FCS_CRC16; 24690a708f8fSGustavo F. Padovan } 24700a708f8fSGustavo F. Padovan 24710a708f8fSGustavo F. Padovan static inline int l2cap_config_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u16 cmd_len, u8 *data) 24720a708f8fSGustavo F. Padovan { 24730a708f8fSGustavo F. Padovan struct l2cap_conf_req *req = (struct l2cap_conf_req *) data; 24740a708f8fSGustavo F. Padovan u16 dcid, flags; 24750a708f8fSGustavo F. Padovan u8 rsp[64]; 247648454079SGustavo F. Padovan struct l2cap_chan *chan; 24770a708f8fSGustavo F. Padovan struct sock *sk; 24780a708f8fSGustavo F. Padovan int len; 24790a708f8fSGustavo F. Padovan 24800a708f8fSGustavo F. Padovan dcid = __le16_to_cpu(req->dcid); 24810a708f8fSGustavo F. Padovan flags = __le16_to_cpu(req->flags); 24820a708f8fSGustavo F. Padovan 24830a708f8fSGustavo F. Padovan BT_DBG("dcid 0x%4.4x flags 0x%2.2x", dcid, flags); 24840a708f8fSGustavo F. Padovan 2485baa7e1faSGustavo F. Padovan chan = l2cap_get_chan_by_scid(conn, dcid); 248648454079SGustavo F. Padovan if (!chan) 24870a708f8fSGustavo F. Padovan return -ENOENT; 24880a708f8fSGustavo F. Padovan 248948454079SGustavo F. Padovan sk = chan->sk; 249048454079SGustavo F. Padovan 24910a708f8fSGustavo F. Padovan if (sk->sk_state != BT_CONFIG) { 24920a708f8fSGustavo F. Padovan struct l2cap_cmd_rej rej; 24930a708f8fSGustavo F. Padovan 24940a708f8fSGustavo F. Padovan rej.reason = cpu_to_le16(0x0002); 24950a708f8fSGustavo F. Padovan l2cap_send_cmd(conn, cmd->ident, L2CAP_COMMAND_REJ, 24960a708f8fSGustavo F. Padovan sizeof(rej), &rej); 24970a708f8fSGustavo F. Padovan goto unlock; 24980a708f8fSGustavo F. Padovan } 24990a708f8fSGustavo F. Padovan 25000a708f8fSGustavo F. Padovan /* Reject if config buffer is too small. */ 25010a708f8fSGustavo F. Padovan len = cmd_len - sizeof(*req); 250273ffa904SGustavo F. Padovan if (chan->conf_len + len > sizeof(chan->conf_req)) { 25030a708f8fSGustavo F. Padovan l2cap_send_cmd(conn, cmd->ident, L2CAP_CONF_RSP, 2504fe4128e0SGustavo F. Padovan l2cap_build_conf_rsp(chan, rsp, 25050a708f8fSGustavo F. Padovan L2CAP_CONF_REJECT, flags), rsp); 25060a708f8fSGustavo F. Padovan goto unlock; 25070a708f8fSGustavo F. Padovan } 25080a708f8fSGustavo F. Padovan 25090a708f8fSGustavo F. Padovan /* Store config. */ 251073ffa904SGustavo F. Padovan memcpy(chan->conf_req + chan->conf_len, req->data, len); 251173ffa904SGustavo F. Padovan chan->conf_len += len; 25120a708f8fSGustavo F. Padovan 25130a708f8fSGustavo F. Padovan if (flags & 0x0001) { 25140a708f8fSGustavo F. Padovan /* Incomplete config. Send empty response. */ 25150a708f8fSGustavo F. Padovan l2cap_send_cmd(conn, cmd->ident, L2CAP_CONF_RSP, 2516fe4128e0SGustavo F. Padovan l2cap_build_conf_rsp(chan, rsp, 25170a708f8fSGustavo F. Padovan L2CAP_CONF_SUCCESS, 0x0001), rsp); 25180a708f8fSGustavo F. Padovan goto unlock; 25190a708f8fSGustavo F. Padovan } 25200a708f8fSGustavo F. Padovan 25210a708f8fSGustavo F. Padovan /* Complete config. */ 252273ffa904SGustavo F. Padovan len = l2cap_parse_conf_req(chan, rsp); 25230a708f8fSGustavo F. Padovan if (len < 0) { 2524e92c8e70SGustavo F. Padovan l2cap_send_disconn_req(conn, chan, ECONNRESET); 25250a708f8fSGustavo F. Padovan goto unlock; 25260a708f8fSGustavo F. Padovan } 25270a708f8fSGustavo F. Padovan 25280a708f8fSGustavo F. Padovan l2cap_send_cmd(conn, cmd->ident, L2CAP_CONF_RSP, len, rsp); 252973ffa904SGustavo F. Padovan chan->num_conf_rsp++; 25300a708f8fSGustavo F. Padovan 25310a708f8fSGustavo F. Padovan /* Reset config buffer. */ 253273ffa904SGustavo F. Padovan chan->conf_len = 0; 25330a708f8fSGustavo F. Padovan 2534b4450035SGustavo F. Padovan if (!(chan->conf_state & L2CAP_CONF_OUTPUT_DONE)) 25350a708f8fSGustavo F. Padovan goto unlock; 25360a708f8fSGustavo F. Padovan 2537b4450035SGustavo F. Padovan if (chan->conf_state & L2CAP_CONF_INPUT_DONE) { 253847d1ec61SGustavo F. Padovan set_default_fcs(chan); 25390a708f8fSGustavo F. Padovan 25400a708f8fSGustavo F. Padovan sk->sk_state = BT_CONNECTED; 25410a708f8fSGustavo F. Padovan 254242e5c802SGustavo F. Padovan chan->next_tx_seq = 0; 254342e5c802SGustavo F. Padovan chan->expected_tx_seq = 0; 254458d35f87SGustavo F. Padovan skb_queue_head_init(&chan->tx_q); 25450c1bc5c6SGustavo F. Padovan if (chan->mode == L2CAP_MODE_ERTM) 2546525cd185SGustavo F. Padovan l2cap_ertm_init(chan); 25470a708f8fSGustavo F. Padovan 25480a708f8fSGustavo F. Padovan l2cap_chan_ready(sk); 25490a708f8fSGustavo F. Padovan goto unlock; 25500a708f8fSGustavo F. Padovan } 25510a708f8fSGustavo F. Padovan 2552b4450035SGustavo F. Padovan if (!(chan->conf_state & L2CAP_CONF_REQ_SENT)) { 25530a708f8fSGustavo F. Padovan u8 buf[64]; 2554b4450035SGustavo F. Padovan chan->conf_state |= L2CAP_CONF_REQ_SENT; 25550a708f8fSGustavo F. Padovan l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ, 255673ffa904SGustavo F. Padovan l2cap_build_conf_req(chan, buf), buf); 255773ffa904SGustavo F. Padovan chan->num_conf_req++; 25580a708f8fSGustavo F. Padovan } 25590a708f8fSGustavo F. Padovan 25600a708f8fSGustavo F. Padovan unlock: 25610a708f8fSGustavo F. Padovan bh_unlock_sock(sk); 25620a708f8fSGustavo F. Padovan return 0; 25630a708f8fSGustavo F. Padovan } 25640a708f8fSGustavo F. Padovan 25650a708f8fSGustavo F. Padovan static inline int l2cap_config_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data) 25660a708f8fSGustavo F. Padovan { 25670a708f8fSGustavo F. Padovan struct l2cap_conf_rsp *rsp = (struct l2cap_conf_rsp *)data; 25680a708f8fSGustavo F. Padovan u16 scid, flags, result; 256948454079SGustavo F. Padovan struct l2cap_chan *chan; 25700a708f8fSGustavo F. Padovan struct sock *sk; 25710a708f8fSGustavo F. Padovan int len = cmd->len - sizeof(*rsp); 25720a708f8fSGustavo F. Padovan 25730a708f8fSGustavo F. Padovan scid = __le16_to_cpu(rsp->scid); 25740a708f8fSGustavo F. Padovan flags = __le16_to_cpu(rsp->flags); 25750a708f8fSGustavo F. Padovan result = __le16_to_cpu(rsp->result); 25760a708f8fSGustavo F. Padovan 25770a708f8fSGustavo F. Padovan BT_DBG("scid 0x%4.4x flags 0x%2.2x result 0x%2.2x", 25780a708f8fSGustavo F. Padovan scid, flags, result); 25790a708f8fSGustavo F. Padovan 2580baa7e1faSGustavo F. Padovan chan = l2cap_get_chan_by_scid(conn, scid); 258148454079SGustavo F. Padovan if (!chan) 25820a708f8fSGustavo F. Padovan return 0; 25830a708f8fSGustavo F. Padovan 258448454079SGustavo F. Padovan sk = chan->sk; 258548454079SGustavo F. Padovan 25860a708f8fSGustavo F. Padovan switch (result) { 25870a708f8fSGustavo F. Padovan case L2CAP_CONF_SUCCESS: 258847d1ec61SGustavo F. Padovan l2cap_conf_rfc_get(chan, rsp->data, len); 25890a708f8fSGustavo F. Padovan break; 25900a708f8fSGustavo F. Padovan 25910a708f8fSGustavo F. Padovan case L2CAP_CONF_UNACCEPT: 259273ffa904SGustavo F. Padovan if (chan->num_conf_rsp <= L2CAP_CONF_MAX_CONF_RSP) { 25930a708f8fSGustavo F. Padovan char req[64]; 25940a708f8fSGustavo F. Padovan 25950a708f8fSGustavo F. Padovan if (len > sizeof(req) - sizeof(struct l2cap_conf_req)) { 2596e92c8e70SGustavo F. Padovan l2cap_send_disconn_req(conn, chan, ECONNRESET); 25970a708f8fSGustavo F. Padovan goto done; 25980a708f8fSGustavo F. Padovan } 25990a708f8fSGustavo F. Padovan 26000a708f8fSGustavo F. Padovan /* throw out any old stored conf requests */ 26010a708f8fSGustavo F. Padovan result = L2CAP_CONF_SUCCESS; 2602b4450035SGustavo F. Padovan len = l2cap_parse_conf_rsp(chan, rsp->data, len, 2603b4450035SGustavo F. Padovan req, &result); 26040a708f8fSGustavo F. Padovan if (len < 0) { 2605e92c8e70SGustavo F. Padovan l2cap_send_disconn_req(conn, chan, ECONNRESET); 26060a708f8fSGustavo F. Padovan goto done; 26070a708f8fSGustavo F. Padovan } 26080a708f8fSGustavo F. Padovan 26090a708f8fSGustavo F. Padovan l2cap_send_cmd(conn, l2cap_get_ident(conn), 26100a708f8fSGustavo F. Padovan L2CAP_CONF_REQ, len, req); 261173ffa904SGustavo F. Padovan chan->num_conf_req++; 26120a708f8fSGustavo F. Padovan if (result != L2CAP_CONF_SUCCESS) 26130a708f8fSGustavo F. Padovan goto done; 26140a708f8fSGustavo F. Padovan break; 26150a708f8fSGustavo F. Padovan } 26160a708f8fSGustavo F. Padovan 26170a708f8fSGustavo F. Padovan default: 26180a708f8fSGustavo F. Padovan sk->sk_err = ECONNRESET; 26190a708f8fSGustavo F. Padovan l2cap_sock_set_timer(sk, HZ * 5); 2620e92c8e70SGustavo F. Padovan l2cap_send_disconn_req(conn, chan, ECONNRESET); 26210a708f8fSGustavo F. Padovan goto done; 26220a708f8fSGustavo F. Padovan } 26230a708f8fSGustavo F. Padovan 26240a708f8fSGustavo F. Padovan if (flags & 0x01) 26250a708f8fSGustavo F. Padovan goto done; 26260a708f8fSGustavo F. Padovan 2627b4450035SGustavo F. Padovan chan->conf_state |= L2CAP_CONF_INPUT_DONE; 26280a708f8fSGustavo F. Padovan 2629b4450035SGustavo F. Padovan if (chan->conf_state & L2CAP_CONF_OUTPUT_DONE) { 263047d1ec61SGustavo F. Padovan set_default_fcs(chan); 26310a708f8fSGustavo F. Padovan 26320a708f8fSGustavo F. Padovan sk->sk_state = BT_CONNECTED; 263342e5c802SGustavo F. Padovan chan->next_tx_seq = 0; 263442e5c802SGustavo F. Padovan chan->expected_tx_seq = 0; 263558d35f87SGustavo F. Padovan skb_queue_head_init(&chan->tx_q); 26360c1bc5c6SGustavo F. Padovan if (chan->mode == L2CAP_MODE_ERTM) 2637525cd185SGustavo F. Padovan l2cap_ertm_init(chan); 26380a708f8fSGustavo F. Padovan 26390a708f8fSGustavo F. Padovan l2cap_chan_ready(sk); 26400a708f8fSGustavo F. Padovan } 26410a708f8fSGustavo F. Padovan 26420a708f8fSGustavo F. Padovan done: 26430a708f8fSGustavo F. Padovan bh_unlock_sock(sk); 26440a708f8fSGustavo F. Padovan return 0; 26450a708f8fSGustavo F. Padovan } 26460a708f8fSGustavo F. Padovan 26470a708f8fSGustavo F. Padovan static inline int l2cap_disconnect_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data) 26480a708f8fSGustavo F. Padovan { 26490a708f8fSGustavo F. Padovan struct l2cap_disconn_req *req = (struct l2cap_disconn_req *) data; 26500a708f8fSGustavo F. Padovan struct l2cap_disconn_rsp rsp; 26510a708f8fSGustavo F. Padovan u16 dcid, scid; 265248454079SGustavo F. Padovan struct l2cap_chan *chan; 26530a708f8fSGustavo F. Padovan struct sock *sk; 26540a708f8fSGustavo F. Padovan 26550a708f8fSGustavo F. Padovan scid = __le16_to_cpu(req->scid); 26560a708f8fSGustavo F. Padovan dcid = __le16_to_cpu(req->dcid); 26570a708f8fSGustavo F. Padovan 26580a708f8fSGustavo F. Padovan BT_DBG("scid 0x%4.4x dcid 0x%4.4x", scid, dcid); 26590a708f8fSGustavo F. Padovan 2660baa7e1faSGustavo F. Padovan chan = l2cap_get_chan_by_scid(conn, dcid); 266148454079SGustavo F. Padovan if (!chan) 26620a708f8fSGustavo F. Padovan return 0; 26630a708f8fSGustavo F. Padovan 266448454079SGustavo F. Padovan sk = chan->sk; 266548454079SGustavo F. Padovan 2666fe4128e0SGustavo F. Padovan rsp.dcid = cpu_to_le16(chan->scid); 2667fe4128e0SGustavo F. Padovan rsp.scid = cpu_to_le16(chan->dcid); 26680a708f8fSGustavo F. Padovan l2cap_send_cmd(conn, cmd->ident, L2CAP_DISCONN_RSP, sizeof(rsp), &rsp); 26690a708f8fSGustavo F. Padovan 26700a708f8fSGustavo F. Padovan sk->sk_shutdown = SHUTDOWN_MASK; 26710a708f8fSGustavo F. Padovan 26720a708f8fSGustavo F. Padovan /* don't delete l2cap channel if sk is owned by user */ 26730a708f8fSGustavo F. Padovan if (sock_owned_by_user(sk)) { 26740a708f8fSGustavo F. Padovan sk->sk_state = BT_DISCONN; 26750a708f8fSGustavo F. Padovan l2cap_sock_clear_timer(sk); 26760a708f8fSGustavo F. Padovan l2cap_sock_set_timer(sk, HZ / 5); 26770a708f8fSGustavo F. Padovan bh_unlock_sock(sk); 26780a708f8fSGustavo F. Padovan return 0; 26790a708f8fSGustavo F. Padovan } 26800a708f8fSGustavo F. Padovan 268148454079SGustavo F. Padovan l2cap_chan_del(chan, ECONNRESET); 26820a708f8fSGustavo F. Padovan bh_unlock_sock(sk); 26830a708f8fSGustavo F. Padovan 26840a708f8fSGustavo F. Padovan l2cap_sock_kill(sk); 26850a708f8fSGustavo F. Padovan return 0; 26860a708f8fSGustavo F. Padovan } 26870a708f8fSGustavo F. Padovan 26880a708f8fSGustavo F. Padovan static inline int l2cap_disconnect_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data) 26890a708f8fSGustavo F. Padovan { 26900a708f8fSGustavo F. Padovan struct l2cap_disconn_rsp *rsp = (struct l2cap_disconn_rsp *) data; 26910a708f8fSGustavo F. Padovan u16 dcid, scid; 269248454079SGustavo F. Padovan struct l2cap_chan *chan; 26930a708f8fSGustavo F. Padovan struct sock *sk; 26940a708f8fSGustavo F. Padovan 26950a708f8fSGustavo F. Padovan scid = __le16_to_cpu(rsp->scid); 26960a708f8fSGustavo F. Padovan dcid = __le16_to_cpu(rsp->dcid); 26970a708f8fSGustavo F. Padovan 26980a708f8fSGustavo F. Padovan BT_DBG("dcid 0x%4.4x scid 0x%4.4x", dcid, scid); 26990a708f8fSGustavo F. Padovan 2700baa7e1faSGustavo F. Padovan chan = l2cap_get_chan_by_scid(conn, scid); 270148454079SGustavo F. Padovan if (!chan) 27020a708f8fSGustavo F. Padovan return 0; 27030a708f8fSGustavo F. Padovan 270448454079SGustavo F. Padovan sk = chan->sk; 270548454079SGustavo F. Padovan 27060a708f8fSGustavo F. Padovan /* don't delete l2cap channel if sk is owned by user */ 27070a708f8fSGustavo F. Padovan if (sock_owned_by_user(sk)) { 27080a708f8fSGustavo F. Padovan sk->sk_state = BT_DISCONN; 27090a708f8fSGustavo F. Padovan l2cap_sock_clear_timer(sk); 27100a708f8fSGustavo F. Padovan l2cap_sock_set_timer(sk, HZ / 5); 27110a708f8fSGustavo F. Padovan bh_unlock_sock(sk); 27120a708f8fSGustavo F. Padovan return 0; 27130a708f8fSGustavo F. Padovan } 27140a708f8fSGustavo F. Padovan 271548454079SGustavo F. Padovan l2cap_chan_del(chan, 0); 27160a708f8fSGustavo F. Padovan bh_unlock_sock(sk); 27170a708f8fSGustavo F. Padovan 27180a708f8fSGustavo F. Padovan l2cap_sock_kill(sk); 27190a708f8fSGustavo F. Padovan return 0; 27200a708f8fSGustavo F. Padovan } 27210a708f8fSGustavo F. Padovan 27220a708f8fSGustavo F. Padovan static inline int l2cap_information_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data) 27230a708f8fSGustavo F. Padovan { 27240a708f8fSGustavo F. Padovan struct l2cap_info_req *req = (struct l2cap_info_req *) data; 27250a708f8fSGustavo F. Padovan u16 type; 27260a708f8fSGustavo F. Padovan 27270a708f8fSGustavo F. Padovan type = __le16_to_cpu(req->type); 27280a708f8fSGustavo F. Padovan 27290a708f8fSGustavo F. Padovan BT_DBG("type 0x%4.4x", type); 27300a708f8fSGustavo F. Padovan 27310a708f8fSGustavo F. Padovan if (type == L2CAP_IT_FEAT_MASK) { 27320a708f8fSGustavo F. Padovan u8 buf[8]; 27330a708f8fSGustavo F. Padovan u32 feat_mask = l2cap_feat_mask; 27340a708f8fSGustavo F. Padovan struct l2cap_info_rsp *rsp = (struct l2cap_info_rsp *) buf; 27350a708f8fSGustavo F. Padovan rsp->type = cpu_to_le16(L2CAP_IT_FEAT_MASK); 27360a708f8fSGustavo F. Padovan rsp->result = cpu_to_le16(L2CAP_IR_SUCCESS); 27370a708f8fSGustavo F. Padovan if (!disable_ertm) 27380a708f8fSGustavo F. Padovan feat_mask |= L2CAP_FEAT_ERTM | L2CAP_FEAT_STREAMING 27390a708f8fSGustavo F. Padovan | L2CAP_FEAT_FCS; 27400a708f8fSGustavo F. Padovan put_unaligned_le32(feat_mask, rsp->data); 27410a708f8fSGustavo F. Padovan l2cap_send_cmd(conn, cmd->ident, 27420a708f8fSGustavo F. Padovan L2CAP_INFO_RSP, sizeof(buf), buf); 27430a708f8fSGustavo F. Padovan } else if (type == L2CAP_IT_FIXED_CHAN) { 27440a708f8fSGustavo F. Padovan u8 buf[12]; 27450a708f8fSGustavo F. Padovan struct l2cap_info_rsp *rsp = (struct l2cap_info_rsp *) buf; 27460a708f8fSGustavo F. Padovan rsp->type = cpu_to_le16(L2CAP_IT_FIXED_CHAN); 27470a708f8fSGustavo F. Padovan rsp->result = cpu_to_le16(L2CAP_IR_SUCCESS); 27480a708f8fSGustavo F. Padovan memcpy(buf + 4, l2cap_fixed_chan, 8); 27490a708f8fSGustavo F. Padovan l2cap_send_cmd(conn, cmd->ident, 27500a708f8fSGustavo F. Padovan L2CAP_INFO_RSP, sizeof(buf), buf); 27510a708f8fSGustavo F. Padovan } else { 27520a708f8fSGustavo F. Padovan struct l2cap_info_rsp rsp; 27530a708f8fSGustavo F. Padovan rsp.type = cpu_to_le16(type); 27540a708f8fSGustavo F. Padovan rsp.result = cpu_to_le16(L2CAP_IR_NOTSUPP); 27550a708f8fSGustavo F. Padovan l2cap_send_cmd(conn, cmd->ident, 27560a708f8fSGustavo F. Padovan L2CAP_INFO_RSP, sizeof(rsp), &rsp); 27570a708f8fSGustavo F. Padovan } 27580a708f8fSGustavo F. Padovan 27590a708f8fSGustavo F. Padovan return 0; 27600a708f8fSGustavo F. Padovan } 27610a708f8fSGustavo F. Padovan 27620a708f8fSGustavo F. Padovan static inline int l2cap_information_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data) 27630a708f8fSGustavo F. Padovan { 27640a708f8fSGustavo F. Padovan struct l2cap_info_rsp *rsp = (struct l2cap_info_rsp *) data; 27650a708f8fSGustavo F. Padovan u16 type, result; 27660a708f8fSGustavo F. Padovan 27670a708f8fSGustavo F. Padovan type = __le16_to_cpu(rsp->type); 27680a708f8fSGustavo F. Padovan result = __le16_to_cpu(rsp->result); 27690a708f8fSGustavo F. Padovan 27700a708f8fSGustavo F. Padovan BT_DBG("type 0x%4.4x result 0x%2.2x", type, result); 27710a708f8fSGustavo F. Padovan 2772e90165beSAndrei Emeltchenko /* L2CAP Info req/rsp are unbound to channels, add extra checks */ 2773e90165beSAndrei Emeltchenko if (cmd->ident != conn->info_ident || 2774e90165beSAndrei Emeltchenko conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_DONE) 2775e90165beSAndrei Emeltchenko return 0; 2776e90165beSAndrei Emeltchenko 27770a708f8fSGustavo F. Padovan del_timer(&conn->info_timer); 27780a708f8fSGustavo F. Padovan 27790a708f8fSGustavo F. Padovan if (result != L2CAP_IR_SUCCESS) { 27800a708f8fSGustavo F. Padovan conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_DONE; 27810a708f8fSGustavo F. Padovan conn->info_ident = 0; 27820a708f8fSGustavo F. Padovan 27830a708f8fSGustavo F. Padovan l2cap_conn_start(conn); 27840a708f8fSGustavo F. Padovan 27850a708f8fSGustavo F. Padovan return 0; 27860a708f8fSGustavo F. Padovan } 27870a708f8fSGustavo F. Padovan 27880a708f8fSGustavo F. Padovan if (type == L2CAP_IT_FEAT_MASK) { 27890a708f8fSGustavo F. Padovan conn->feat_mask = get_unaligned_le32(rsp->data); 27900a708f8fSGustavo F. Padovan 27910a708f8fSGustavo F. Padovan if (conn->feat_mask & L2CAP_FEAT_FIXED_CHAN) { 27920a708f8fSGustavo F. Padovan struct l2cap_info_req req; 27930a708f8fSGustavo F. Padovan req.type = cpu_to_le16(L2CAP_IT_FIXED_CHAN); 27940a708f8fSGustavo F. Padovan 27950a708f8fSGustavo F. Padovan conn->info_ident = l2cap_get_ident(conn); 27960a708f8fSGustavo F. Padovan 27970a708f8fSGustavo F. Padovan l2cap_send_cmd(conn, conn->info_ident, 27980a708f8fSGustavo F. Padovan L2CAP_INFO_REQ, sizeof(req), &req); 27990a708f8fSGustavo F. Padovan } else { 28000a708f8fSGustavo F. Padovan conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_DONE; 28010a708f8fSGustavo F. Padovan conn->info_ident = 0; 28020a708f8fSGustavo F. Padovan 28030a708f8fSGustavo F. Padovan l2cap_conn_start(conn); 28040a708f8fSGustavo F. Padovan } 28050a708f8fSGustavo F. Padovan } else if (type == L2CAP_IT_FIXED_CHAN) { 28060a708f8fSGustavo F. Padovan conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_DONE; 28070a708f8fSGustavo F. Padovan conn->info_ident = 0; 28080a708f8fSGustavo F. Padovan 28090a708f8fSGustavo F. Padovan l2cap_conn_start(conn); 28100a708f8fSGustavo F. Padovan } 28110a708f8fSGustavo F. Padovan 28120a708f8fSGustavo F. Padovan return 0; 28130a708f8fSGustavo F. Padovan } 28140a708f8fSGustavo F. Padovan 2815e2174ca4SGustavo F. Padovan static inline int l2cap_check_conn_param(u16 min, u16 max, u16 latency, 2816de73115aSClaudio Takahasi u16 to_multiplier) 2817de73115aSClaudio Takahasi { 2818de73115aSClaudio Takahasi u16 max_latency; 2819de73115aSClaudio Takahasi 2820de73115aSClaudio Takahasi if (min > max || min < 6 || max > 3200) 2821de73115aSClaudio Takahasi return -EINVAL; 2822de73115aSClaudio Takahasi 2823de73115aSClaudio Takahasi if (to_multiplier < 10 || to_multiplier > 3200) 2824de73115aSClaudio Takahasi return -EINVAL; 2825de73115aSClaudio Takahasi 2826de73115aSClaudio Takahasi if (max >= to_multiplier * 8) 2827de73115aSClaudio Takahasi return -EINVAL; 2828de73115aSClaudio Takahasi 2829de73115aSClaudio Takahasi max_latency = (to_multiplier * 8 / max) - 1; 2830de73115aSClaudio Takahasi if (latency > 499 || latency > max_latency) 2831de73115aSClaudio Takahasi return -EINVAL; 2832de73115aSClaudio Takahasi 2833de73115aSClaudio Takahasi return 0; 2834de73115aSClaudio Takahasi } 2835de73115aSClaudio Takahasi 2836de73115aSClaudio Takahasi static inline int l2cap_conn_param_update_req(struct l2cap_conn *conn, 2837de73115aSClaudio Takahasi struct l2cap_cmd_hdr *cmd, u8 *data) 2838de73115aSClaudio Takahasi { 2839de73115aSClaudio Takahasi struct hci_conn *hcon = conn->hcon; 2840de73115aSClaudio Takahasi struct l2cap_conn_param_update_req *req; 2841de73115aSClaudio Takahasi struct l2cap_conn_param_update_rsp rsp; 2842de73115aSClaudio Takahasi u16 min, max, latency, to_multiplier, cmd_len; 28432ce603ebSClaudio Takahasi int err; 2844de73115aSClaudio Takahasi 2845de73115aSClaudio Takahasi if (!(hcon->link_mode & HCI_LM_MASTER)) 2846de73115aSClaudio Takahasi return -EINVAL; 2847de73115aSClaudio Takahasi 2848de73115aSClaudio Takahasi cmd_len = __le16_to_cpu(cmd->len); 2849de73115aSClaudio Takahasi if (cmd_len != sizeof(struct l2cap_conn_param_update_req)) 2850de73115aSClaudio Takahasi return -EPROTO; 2851de73115aSClaudio Takahasi 2852de73115aSClaudio Takahasi req = (struct l2cap_conn_param_update_req *) data; 2853de73115aSClaudio Takahasi min = __le16_to_cpu(req->min); 2854de73115aSClaudio Takahasi max = __le16_to_cpu(req->max); 2855de73115aSClaudio Takahasi latency = __le16_to_cpu(req->latency); 2856de73115aSClaudio Takahasi to_multiplier = __le16_to_cpu(req->to_multiplier); 2857de73115aSClaudio Takahasi 2858de73115aSClaudio Takahasi BT_DBG("min 0x%4.4x max 0x%4.4x latency: 0x%4.4x Timeout: 0x%4.4x", 2859de73115aSClaudio Takahasi min, max, latency, to_multiplier); 2860de73115aSClaudio Takahasi 2861de73115aSClaudio Takahasi memset(&rsp, 0, sizeof(rsp)); 28622ce603ebSClaudio Takahasi 28632ce603ebSClaudio Takahasi err = l2cap_check_conn_param(min, max, latency, to_multiplier); 28642ce603ebSClaudio Takahasi if (err) 2865de73115aSClaudio Takahasi rsp.result = cpu_to_le16(L2CAP_CONN_PARAM_REJECTED); 2866de73115aSClaudio Takahasi else 2867de73115aSClaudio Takahasi rsp.result = cpu_to_le16(L2CAP_CONN_PARAM_ACCEPTED); 2868de73115aSClaudio Takahasi 2869de73115aSClaudio Takahasi l2cap_send_cmd(conn, cmd->ident, L2CAP_CONN_PARAM_UPDATE_RSP, 2870de73115aSClaudio Takahasi sizeof(rsp), &rsp); 2871de73115aSClaudio Takahasi 28722ce603ebSClaudio Takahasi if (!err) 28732ce603ebSClaudio Takahasi hci_le_conn_update(hcon, min, max, latency, to_multiplier); 28742ce603ebSClaudio Takahasi 2875de73115aSClaudio Takahasi return 0; 2876de73115aSClaudio Takahasi } 2877de73115aSClaudio Takahasi 28783300d9a9SClaudio Takahasi static inline int l2cap_bredr_sig_cmd(struct l2cap_conn *conn, 28793300d9a9SClaudio Takahasi struct l2cap_cmd_hdr *cmd, u16 cmd_len, u8 *data) 28803300d9a9SClaudio Takahasi { 28813300d9a9SClaudio Takahasi int err = 0; 28823300d9a9SClaudio Takahasi 28833300d9a9SClaudio Takahasi switch (cmd->code) { 28843300d9a9SClaudio Takahasi case L2CAP_COMMAND_REJ: 28853300d9a9SClaudio Takahasi l2cap_command_rej(conn, cmd, data); 28863300d9a9SClaudio Takahasi break; 28873300d9a9SClaudio Takahasi 28883300d9a9SClaudio Takahasi case L2CAP_CONN_REQ: 28893300d9a9SClaudio Takahasi err = l2cap_connect_req(conn, cmd, data); 28903300d9a9SClaudio Takahasi break; 28913300d9a9SClaudio Takahasi 28923300d9a9SClaudio Takahasi case L2CAP_CONN_RSP: 28933300d9a9SClaudio Takahasi err = l2cap_connect_rsp(conn, cmd, data); 28943300d9a9SClaudio Takahasi break; 28953300d9a9SClaudio Takahasi 28963300d9a9SClaudio Takahasi case L2CAP_CONF_REQ: 28973300d9a9SClaudio Takahasi err = l2cap_config_req(conn, cmd, cmd_len, data); 28983300d9a9SClaudio Takahasi break; 28993300d9a9SClaudio Takahasi 29003300d9a9SClaudio Takahasi case L2CAP_CONF_RSP: 29013300d9a9SClaudio Takahasi err = l2cap_config_rsp(conn, cmd, data); 29023300d9a9SClaudio Takahasi break; 29033300d9a9SClaudio Takahasi 29043300d9a9SClaudio Takahasi case L2CAP_DISCONN_REQ: 29053300d9a9SClaudio Takahasi err = l2cap_disconnect_req(conn, cmd, data); 29063300d9a9SClaudio Takahasi break; 29073300d9a9SClaudio Takahasi 29083300d9a9SClaudio Takahasi case L2CAP_DISCONN_RSP: 29093300d9a9SClaudio Takahasi err = l2cap_disconnect_rsp(conn, cmd, data); 29103300d9a9SClaudio Takahasi break; 29113300d9a9SClaudio Takahasi 29123300d9a9SClaudio Takahasi case L2CAP_ECHO_REQ: 29133300d9a9SClaudio Takahasi l2cap_send_cmd(conn, cmd->ident, L2CAP_ECHO_RSP, cmd_len, data); 29143300d9a9SClaudio Takahasi break; 29153300d9a9SClaudio Takahasi 29163300d9a9SClaudio Takahasi case L2CAP_ECHO_RSP: 29173300d9a9SClaudio Takahasi break; 29183300d9a9SClaudio Takahasi 29193300d9a9SClaudio Takahasi case L2CAP_INFO_REQ: 29203300d9a9SClaudio Takahasi err = l2cap_information_req(conn, cmd, data); 29213300d9a9SClaudio Takahasi break; 29223300d9a9SClaudio Takahasi 29233300d9a9SClaudio Takahasi case L2CAP_INFO_RSP: 29243300d9a9SClaudio Takahasi err = l2cap_information_rsp(conn, cmd, data); 29253300d9a9SClaudio Takahasi break; 29263300d9a9SClaudio Takahasi 29273300d9a9SClaudio Takahasi default: 29283300d9a9SClaudio Takahasi BT_ERR("Unknown BR/EDR signaling command 0x%2.2x", cmd->code); 29293300d9a9SClaudio Takahasi err = -EINVAL; 29303300d9a9SClaudio Takahasi break; 29313300d9a9SClaudio Takahasi } 29323300d9a9SClaudio Takahasi 29333300d9a9SClaudio Takahasi return err; 29343300d9a9SClaudio Takahasi } 29353300d9a9SClaudio Takahasi 29363300d9a9SClaudio Takahasi static inline int l2cap_le_sig_cmd(struct l2cap_conn *conn, 29373300d9a9SClaudio Takahasi struct l2cap_cmd_hdr *cmd, u8 *data) 29383300d9a9SClaudio Takahasi { 29393300d9a9SClaudio Takahasi switch (cmd->code) { 29403300d9a9SClaudio Takahasi case L2CAP_COMMAND_REJ: 29413300d9a9SClaudio Takahasi return 0; 29423300d9a9SClaudio Takahasi 29433300d9a9SClaudio Takahasi case L2CAP_CONN_PARAM_UPDATE_REQ: 2944de73115aSClaudio Takahasi return l2cap_conn_param_update_req(conn, cmd, data); 29453300d9a9SClaudio Takahasi 29463300d9a9SClaudio Takahasi case L2CAP_CONN_PARAM_UPDATE_RSP: 29473300d9a9SClaudio Takahasi return 0; 29483300d9a9SClaudio Takahasi 29493300d9a9SClaudio Takahasi default: 29503300d9a9SClaudio Takahasi BT_ERR("Unknown LE signaling command 0x%2.2x", cmd->code); 29513300d9a9SClaudio Takahasi return -EINVAL; 29523300d9a9SClaudio Takahasi } 29533300d9a9SClaudio Takahasi } 29543300d9a9SClaudio Takahasi 29553300d9a9SClaudio Takahasi static inline void l2cap_sig_channel(struct l2cap_conn *conn, 29563300d9a9SClaudio Takahasi struct sk_buff *skb) 29570a708f8fSGustavo F. Padovan { 29580a708f8fSGustavo F. Padovan u8 *data = skb->data; 29590a708f8fSGustavo F. Padovan int len = skb->len; 29600a708f8fSGustavo F. Padovan struct l2cap_cmd_hdr cmd; 29613300d9a9SClaudio Takahasi int err; 29620a708f8fSGustavo F. Padovan 29630a708f8fSGustavo F. Padovan l2cap_raw_recv(conn, skb); 29640a708f8fSGustavo F. Padovan 29650a708f8fSGustavo F. Padovan while (len >= L2CAP_CMD_HDR_SIZE) { 29660a708f8fSGustavo F. Padovan u16 cmd_len; 29670a708f8fSGustavo F. Padovan memcpy(&cmd, data, L2CAP_CMD_HDR_SIZE); 29680a708f8fSGustavo F. Padovan data += L2CAP_CMD_HDR_SIZE; 29690a708f8fSGustavo F. Padovan len -= L2CAP_CMD_HDR_SIZE; 29700a708f8fSGustavo F. Padovan 29710a708f8fSGustavo F. Padovan cmd_len = le16_to_cpu(cmd.len); 29720a708f8fSGustavo F. Padovan 29730a708f8fSGustavo F. Padovan BT_DBG("code 0x%2.2x len %d id 0x%2.2x", cmd.code, cmd_len, cmd.ident); 29740a708f8fSGustavo F. Padovan 29750a708f8fSGustavo F. Padovan if (cmd_len > len || !cmd.ident) { 29760a708f8fSGustavo F. Padovan BT_DBG("corrupted command"); 29770a708f8fSGustavo F. Padovan break; 29780a708f8fSGustavo F. Padovan } 29790a708f8fSGustavo F. Padovan 29803300d9a9SClaudio Takahasi if (conn->hcon->type == LE_LINK) 29813300d9a9SClaudio Takahasi err = l2cap_le_sig_cmd(conn, &cmd, data); 29823300d9a9SClaudio Takahasi else 29833300d9a9SClaudio Takahasi err = l2cap_bredr_sig_cmd(conn, &cmd, cmd_len, data); 29840a708f8fSGustavo F. Padovan 29850a708f8fSGustavo F. Padovan if (err) { 29860a708f8fSGustavo F. Padovan struct l2cap_cmd_rej rej; 29872c6d1a2eSGustavo F. Padovan 29882c6d1a2eSGustavo F. Padovan BT_ERR("Wrong link type (%d)", err); 29890a708f8fSGustavo F. Padovan 29900a708f8fSGustavo F. Padovan /* FIXME: Map err to a valid reason */ 29910a708f8fSGustavo F. Padovan rej.reason = cpu_to_le16(0); 29920a708f8fSGustavo F. Padovan l2cap_send_cmd(conn, cmd.ident, L2CAP_COMMAND_REJ, sizeof(rej), &rej); 29930a708f8fSGustavo F. Padovan } 29940a708f8fSGustavo F. Padovan 29950a708f8fSGustavo F. Padovan data += cmd_len; 29960a708f8fSGustavo F. Padovan len -= cmd_len; 29970a708f8fSGustavo F. Padovan } 29980a708f8fSGustavo F. Padovan 29990a708f8fSGustavo F. Padovan kfree_skb(skb); 30000a708f8fSGustavo F. Padovan } 30010a708f8fSGustavo F. Padovan 300247d1ec61SGustavo F. Padovan static int l2cap_check_fcs(struct l2cap_chan *chan, struct sk_buff *skb) 30030a708f8fSGustavo F. Padovan { 30040a708f8fSGustavo F. Padovan u16 our_fcs, rcv_fcs; 30050a708f8fSGustavo F. Padovan int hdr_size = L2CAP_HDR_SIZE + 2; 30060a708f8fSGustavo F. Padovan 300747d1ec61SGustavo F. Padovan if (chan->fcs == L2CAP_FCS_CRC16) { 30080a708f8fSGustavo F. Padovan skb_trim(skb, skb->len - 2); 30090a708f8fSGustavo F. Padovan rcv_fcs = get_unaligned_le16(skb->data + skb->len); 30100a708f8fSGustavo F. Padovan our_fcs = crc16(0, skb->data - hdr_size, skb->len + hdr_size); 30110a708f8fSGustavo F. Padovan 30120a708f8fSGustavo F. Padovan if (our_fcs != rcv_fcs) 30130a708f8fSGustavo F. Padovan return -EBADMSG; 30140a708f8fSGustavo F. Padovan } 30150a708f8fSGustavo F. Padovan return 0; 30160a708f8fSGustavo F. Padovan } 30170a708f8fSGustavo F. Padovan 3018525cd185SGustavo F. Padovan static inline void l2cap_send_i_or_rr_or_rnr(struct l2cap_chan *chan) 30190a708f8fSGustavo F. Padovan { 30200a708f8fSGustavo F. Padovan u16 control = 0; 30210a708f8fSGustavo F. Padovan 30226a026610SGustavo F. Padovan chan->frames_sent = 0; 30230a708f8fSGustavo F. Padovan 302442e5c802SGustavo F. Padovan control |= chan->buffer_seq << L2CAP_CTRL_REQSEQ_SHIFT; 30250a708f8fSGustavo F. Padovan 3026525cd185SGustavo F. Padovan if (chan->conn_state & L2CAP_CONN_LOCAL_BUSY) { 30270a708f8fSGustavo F. Padovan control |= L2CAP_SUPER_RCV_NOT_READY; 3028525cd185SGustavo F. Padovan l2cap_send_sframe(chan, control); 3029525cd185SGustavo F. Padovan chan->conn_state |= L2CAP_CONN_RNR_SENT; 30300a708f8fSGustavo F. Padovan } 30310a708f8fSGustavo F. Padovan 3032525cd185SGustavo F. Padovan if (chan->conn_state & L2CAP_CONN_REMOTE_BUSY) 3033525cd185SGustavo F. Padovan l2cap_retransmit_frames(chan); 30340a708f8fSGustavo F. Padovan 3035525cd185SGustavo F. Padovan l2cap_ertm_send(chan); 30360a708f8fSGustavo F. Padovan 3037525cd185SGustavo F. Padovan if (!(chan->conn_state & L2CAP_CONN_LOCAL_BUSY) && 30386a026610SGustavo F. Padovan chan->frames_sent == 0) { 30390a708f8fSGustavo F. Padovan control |= L2CAP_SUPER_RCV_READY; 3040525cd185SGustavo F. Padovan l2cap_send_sframe(chan, control); 30410a708f8fSGustavo F. Padovan } 30420a708f8fSGustavo F. Padovan } 30430a708f8fSGustavo F. Padovan 304442e5c802SGustavo F. Padovan static int l2cap_add_to_srej_queue(struct l2cap_chan *chan, struct sk_buff *skb, u8 tx_seq, u8 sar) 30450a708f8fSGustavo F. Padovan { 30460a708f8fSGustavo F. Padovan struct sk_buff *next_skb; 30470a708f8fSGustavo F. Padovan int tx_seq_offset, next_tx_seq_offset; 30480a708f8fSGustavo F. Padovan 30490a708f8fSGustavo F. Padovan bt_cb(skb)->tx_seq = tx_seq; 30500a708f8fSGustavo F. Padovan bt_cb(skb)->sar = sar; 30510a708f8fSGustavo F. Padovan 3052f1c6775bSGustavo F. Padovan next_skb = skb_peek(&chan->srej_q); 30530a708f8fSGustavo F. Padovan if (!next_skb) { 3054f1c6775bSGustavo F. Padovan __skb_queue_tail(&chan->srej_q, skb); 30550a708f8fSGustavo F. Padovan return 0; 30560a708f8fSGustavo F. Padovan } 30570a708f8fSGustavo F. Padovan 305842e5c802SGustavo F. Padovan tx_seq_offset = (tx_seq - chan->buffer_seq) % 64; 30590a708f8fSGustavo F. Padovan if (tx_seq_offset < 0) 30600a708f8fSGustavo F. Padovan tx_seq_offset += 64; 30610a708f8fSGustavo F. Padovan 30620a708f8fSGustavo F. Padovan do { 30630a708f8fSGustavo F. Padovan if (bt_cb(next_skb)->tx_seq == tx_seq) 30640a708f8fSGustavo F. Padovan return -EINVAL; 30650a708f8fSGustavo F. Padovan 30660a708f8fSGustavo F. Padovan next_tx_seq_offset = (bt_cb(next_skb)->tx_seq - 306742e5c802SGustavo F. Padovan chan->buffer_seq) % 64; 30680a708f8fSGustavo F. Padovan if (next_tx_seq_offset < 0) 30690a708f8fSGustavo F. Padovan next_tx_seq_offset += 64; 30700a708f8fSGustavo F. Padovan 30710a708f8fSGustavo F. Padovan if (next_tx_seq_offset > tx_seq_offset) { 3072f1c6775bSGustavo F. Padovan __skb_queue_before(&chan->srej_q, next_skb, skb); 30730a708f8fSGustavo F. Padovan return 0; 30740a708f8fSGustavo F. Padovan } 30750a708f8fSGustavo F. Padovan 3076f1c6775bSGustavo F. Padovan if (skb_queue_is_last(&chan->srej_q, next_skb)) 30770a708f8fSGustavo F. Padovan break; 30780a708f8fSGustavo F. Padovan 3079f1c6775bSGustavo F. Padovan } while ((next_skb = skb_queue_next(&chan->srej_q, next_skb))); 30800a708f8fSGustavo F. Padovan 3081f1c6775bSGustavo F. Padovan __skb_queue_tail(&chan->srej_q, skb); 30820a708f8fSGustavo F. Padovan 30830a708f8fSGustavo F. Padovan return 0; 30840a708f8fSGustavo F. Padovan } 30850a708f8fSGustavo F. Padovan 3086525cd185SGustavo F. Padovan static int l2cap_ertm_reassembly_sdu(struct l2cap_chan *chan, struct sk_buff *skb, u16 control) 30870a708f8fSGustavo F. Padovan { 30880a708f8fSGustavo F. Padovan struct sk_buff *_skb; 30890a708f8fSGustavo F. Padovan int err; 30900a708f8fSGustavo F. Padovan 30910a708f8fSGustavo F. Padovan switch (control & L2CAP_CTRL_SAR) { 30920a708f8fSGustavo F. Padovan case L2CAP_SDU_UNSEGMENTED: 3093525cd185SGustavo F. Padovan if (chan->conn_state & L2CAP_CONN_SAR_SDU) 30940a708f8fSGustavo F. Padovan goto drop; 30950a708f8fSGustavo F. Padovan 3096224f8af0SRuiyi Zhang return sock_queue_rcv_skb(chan->sk, skb); 30970a708f8fSGustavo F. Padovan 30980a708f8fSGustavo F. Padovan case L2CAP_SDU_START: 3099525cd185SGustavo F. Padovan if (chan->conn_state & L2CAP_CONN_SAR_SDU) 31000a708f8fSGustavo F. Padovan goto drop; 31010a708f8fSGustavo F. Padovan 31026f61fd47SGustavo F. Padovan chan->sdu_len = get_unaligned_le16(skb->data); 31030a708f8fSGustavo F. Padovan 31040c1bc5c6SGustavo F. Padovan if (chan->sdu_len > chan->imtu) 31050a708f8fSGustavo F. Padovan goto disconnect; 31060a708f8fSGustavo F. Padovan 31076f61fd47SGustavo F. Padovan chan->sdu = bt_skb_alloc(chan->sdu_len, GFP_ATOMIC); 31086f61fd47SGustavo F. Padovan if (!chan->sdu) 31090a708f8fSGustavo F. Padovan return -ENOMEM; 31100a708f8fSGustavo F. Padovan 31110a708f8fSGustavo F. Padovan /* pull sdu_len bytes only after alloc, because of Local Busy 31120a708f8fSGustavo F. Padovan * condition we have to be sure that this will be executed 31130a708f8fSGustavo F. Padovan * only once, i.e., when alloc does not fail */ 31140a708f8fSGustavo F. Padovan skb_pull(skb, 2); 31150a708f8fSGustavo F. Padovan 31166f61fd47SGustavo F. Padovan memcpy(skb_put(chan->sdu, skb->len), skb->data, skb->len); 31170a708f8fSGustavo F. Padovan 3118525cd185SGustavo F. Padovan chan->conn_state |= L2CAP_CONN_SAR_SDU; 31196f61fd47SGustavo F. Padovan chan->partial_sdu_len = skb->len; 31200a708f8fSGustavo F. Padovan break; 31210a708f8fSGustavo F. Padovan 31220a708f8fSGustavo F. Padovan case L2CAP_SDU_CONTINUE: 3123525cd185SGustavo F. Padovan if (!(chan->conn_state & L2CAP_CONN_SAR_SDU)) 31240a708f8fSGustavo F. Padovan goto disconnect; 31250a708f8fSGustavo F. Padovan 31266f61fd47SGustavo F. Padovan if (!chan->sdu) 31270a708f8fSGustavo F. Padovan goto disconnect; 31280a708f8fSGustavo F. Padovan 31296f61fd47SGustavo F. Padovan chan->partial_sdu_len += skb->len; 31306f61fd47SGustavo F. Padovan if (chan->partial_sdu_len > chan->sdu_len) 31310a708f8fSGustavo F. Padovan goto drop; 31320a708f8fSGustavo F. Padovan 31336f61fd47SGustavo F. Padovan memcpy(skb_put(chan->sdu, skb->len), skb->data, skb->len); 31340a708f8fSGustavo F. Padovan 31350a708f8fSGustavo F. Padovan break; 31360a708f8fSGustavo F. Padovan 31370a708f8fSGustavo F. Padovan case L2CAP_SDU_END: 3138525cd185SGustavo F. Padovan if (!(chan->conn_state & L2CAP_CONN_SAR_SDU)) 31390a708f8fSGustavo F. Padovan goto disconnect; 31400a708f8fSGustavo F. Padovan 31416f61fd47SGustavo F. Padovan if (!chan->sdu) 31420a708f8fSGustavo F. Padovan goto disconnect; 31430a708f8fSGustavo F. Padovan 3144525cd185SGustavo F. Padovan if (!(chan->conn_state & L2CAP_CONN_SAR_RETRY)) { 31456f61fd47SGustavo F. Padovan chan->partial_sdu_len += skb->len; 31460a708f8fSGustavo F. Padovan 31470c1bc5c6SGustavo F. Padovan if (chan->partial_sdu_len > chan->imtu) 31480a708f8fSGustavo F. Padovan goto drop; 31490a708f8fSGustavo F. Padovan 31506f61fd47SGustavo F. Padovan if (chan->partial_sdu_len != chan->sdu_len) 31510a708f8fSGustavo F. Padovan goto drop; 31520a708f8fSGustavo F. Padovan 31536f61fd47SGustavo F. Padovan memcpy(skb_put(chan->sdu, skb->len), skb->data, skb->len); 31540a708f8fSGustavo F. Padovan } 31550a708f8fSGustavo F. Padovan 31566f61fd47SGustavo F. Padovan _skb = skb_clone(chan->sdu, GFP_ATOMIC); 31570a708f8fSGustavo F. Padovan if (!_skb) { 3158525cd185SGustavo F. Padovan chan->conn_state |= L2CAP_CONN_SAR_RETRY; 31590a708f8fSGustavo F. Padovan return -ENOMEM; 31600a708f8fSGustavo F. Padovan } 31610a708f8fSGustavo F. Padovan 3162525cd185SGustavo F. Padovan err = sock_queue_rcv_skb(chan->sk, _skb); 31630a708f8fSGustavo F. Padovan if (err < 0) { 31640a708f8fSGustavo F. Padovan kfree_skb(_skb); 3165525cd185SGustavo F. Padovan chan->conn_state |= L2CAP_CONN_SAR_RETRY; 31660a708f8fSGustavo F. Padovan return err; 31670a708f8fSGustavo F. Padovan } 31680a708f8fSGustavo F. Padovan 3169525cd185SGustavo F. Padovan chan->conn_state &= ~L2CAP_CONN_SAR_RETRY; 3170525cd185SGustavo F. Padovan chan->conn_state &= ~L2CAP_CONN_SAR_SDU; 31710a708f8fSGustavo F. Padovan 31726f61fd47SGustavo F. Padovan kfree_skb(chan->sdu); 31730a708f8fSGustavo F. Padovan break; 31740a708f8fSGustavo F. Padovan } 31750a708f8fSGustavo F. Padovan 31760a708f8fSGustavo F. Padovan kfree_skb(skb); 31770a708f8fSGustavo F. Padovan return 0; 31780a708f8fSGustavo F. Padovan 31790a708f8fSGustavo F. Padovan drop: 31806f61fd47SGustavo F. Padovan kfree_skb(chan->sdu); 31816f61fd47SGustavo F. Padovan chan->sdu = NULL; 31820a708f8fSGustavo F. Padovan 31830a708f8fSGustavo F. Padovan disconnect: 31848c1d787bSGustavo F. Padovan l2cap_send_disconn_req(chan->conn, chan, ECONNRESET); 31850a708f8fSGustavo F. Padovan kfree_skb(skb); 31860a708f8fSGustavo F. Padovan return 0; 31870a708f8fSGustavo F. Padovan } 31880a708f8fSGustavo F. Padovan 3189525cd185SGustavo F. Padovan static int l2cap_try_push_rx_skb(struct l2cap_chan *chan) 31900a708f8fSGustavo F. Padovan { 31910a708f8fSGustavo F. Padovan struct sk_buff *skb; 31920a708f8fSGustavo F. Padovan u16 control; 31930a708f8fSGustavo F. Padovan int err; 31940a708f8fSGustavo F. Padovan 3195f1c6775bSGustavo F. Padovan while ((skb = skb_dequeue(&chan->busy_q))) { 31960a708f8fSGustavo F. Padovan control = bt_cb(skb)->sar << L2CAP_CTRL_SAR_SHIFT; 3197525cd185SGustavo F. Padovan err = l2cap_ertm_reassembly_sdu(chan, skb, control); 31980a708f8fSGustavo F. Padovan if (err < 0) { 3199f1c6775bSGustavo F. Padovan skb_queue_head(&chan->busy_q, skb); 32000a708f8fSGustavo F. Padovan return -EBUSY; 32010a708f8fSGustavo F. Padovan } 32020a708f8fSGustavo F. Padovan 320342e5c802SGustavo F. Padovan chan->buffer_seq = (chan->buffer_seq + 1) % 64; 32040a708f8fSGustavo F. Padovan } 32050a708f8fSGustavo F. Padovan 3206525cd185SGustavo F. Padovan if (!(chan->conn_state & L2CAP_CONN_RNR_SENT)) 32070a708f8fSGustavo F. Padovan goto done; 32080a708f8fSGustavo F. Padovan 320942e5c802SGustavo F. Padovan control = chan->buffer_seq << L2CAP_CTRL_REQSEQ_SHIFT; 32100a708f8fSGustavo F. Padovan control |= L2CAP_SUPER_RCV_READY | L2CAP_CTRL_POLL; 3211525cd185SGustavo F. Padovan l2cap_send_sframe(chan, control); 32126a026610SGustavo F. Padovan chan->retry_count = 1; 32130a708f8fSGustavo F. Padovan 3214e92c8e70SGustavo F. Padovan del_timer(&chan->retrans_timer); 32150a708f8fSGustavo F. Padovan __mod_monitor_timer(); 32160a708f8fSGustavo F. Padovan 3217525cd185SGustavo F. Padovan chan->conn_state |= L2CAP_CONN_WAIT_F; 32180a708f8fSGustavo F. Padovan 32190a708f8fSGustavo F. Padovan done: 3220525cd185SGustavo F. Padovan chan->conn_state &= ~L2CAP_CONN_LOCAL_BUSY; 3221525cd185SGustavo F. Padovan chan->conn_state &= ~L2CAP_CONN_RNR_SENT; 32220a708f8fSGustavo F. Padovan 322349208c9cSGustavo F. Padovan BT_DBG("chan %p, Exit local busy", chan); 32240a708f8fSGustavo F. Padovan 32250a708f8fSGustavo F. Padovan return 0; 32260a708f8fSGustavo F. Padovan } 32270a708f8fSGustavo F. Padovan 32280a708f8fSGustavo F. Padovan static void l2cap_busy_work(struct work_struct *work) 32290a708f8fSGustavo F. Padovan { 32300a708f8fSGustavo F. Padovan DECLARE_WAITQUEUE(wait, current); 3231311bb895SGustavo F. Padovan struct l2cap_chan *chan = 3232311bb895SGustavo F. Padovan container_of(work, struct l2cap_chan, busy_work); 3233311bb895SGustavo F. Padovan struct sock *sk = chan->sk; 32340a708f8fSGustavo F. Padovan int n_tries = 0, timeo = HZ/5, err; 32350a708f8fSGustavo F. Padovan struct sk_buff *skb; 32360a708f8fSGustavo F. Padovan 32370a708f8fSGustavo F. Padovan lock_sock(sk); 32380a708f8fSGustavo F. Padovan 32390a708f8fSGustavo F. Padovan add_wait_queue(sk_sleep(sk), &wait); 3240311bb895SGustavo F. Padovan while ((skb = skb_peek(&chan->busy_q))) { 32410a708f8fSGustavo F. Padovan set_current_state(TASK_INTERRUPTIBLE); 32420a708f8fSGustavo F. Padovan 32430a708f8fSGustavo F. Padovan if (n_tries++ > L2CAP_LOCAL_BUSY_TRIES) { 32440a708f8fSGustavo F. Padovan err = -EBUSY; 32458c1d787bSGustavo F. Padovan l2cap_send_disconn_req(chan->conn, chan, EBUSY); 32460a708f8fSGustavo F. Padovan break; 32470a708f8fSGustavo F. Padovan } 32480a708f8fSGustavo F. Padovan 32490a708f8fSGustavo F. Padovan if (!timeo) 32500a708f8fSGustavo F. Padovan timeo = HZ/5; 32510a708f8fSGustavo F. Padovan 32520a708f8fSGustavo F. Padovan if (signal_pending(current)) { 32530a708f8fSGustavo F. Padovan err = sock_intr_errno(timeo); 32540a708f8fSGustavo F. Padovan break; 32550a708f8fSGustavo F. Padovan } 32560a708f8fSGustavo F. Padovan 32570a708f8fSGustavo F. Padovan release_sock(sk); 32580a708f8fSGustavo F. Padovan timeo = schedule_timeout(timeo); 32590a708f8fSGustavo F. Padovan lock_sock(sk); 32600a708f8fSGustavo F. Padovan 32610a708f8fSGustavo F. Padovan err = sock_error(sk); 32620a708f8fSGustavo F. Padovan if (err) 32630a708f8fSGustavo F. Padovan break; 32640a708f8fSGustavo F. Padovan 3265311bb895SGustavo F. Padovan if (l2cap_try_push_rx_skb(chan) == 0) 32660a708f8fSGustavo F. Padovan break; 32670a708f8fSGustavo F. Padovan } 32680a708f8fSGustavo F. Padovan 32690a708f8fSGustavo F. Padovan set_current_state(TASK_RUNNING); 32700a708f8fSGustavo F. Padovan remove_wait_queue(sk_sleep(sk), &wait); 32710a708f8fSGustavo F. Padovan 32720a708f8fSGustavo F. Padovan release_sock(sk); 32730a708f8fSGustavo F. Padovan } 32740a708f8fSGustavo F. Padovan 3275525cd185SGustavo F. Padovan static int l2cap_push_rx_skb(struct l2cap_chan *chan, struct sk_buff *skb, u16 control) 32760a708f8fSGustavo F. Padovan { 32770a708f8fSGustavo F. Padovan int sctrl, err; 32780a708f8fSGustavo F. Padovan 3279525cd185SGustavo F. Padovan if (chan->conn_state & L2CAP_CONN_LOCAL_BUSY) { 32800a708f8fSGustavo F. Padovan bt_cb(skb)->sar = control >> L2CAP_CTRL_SAR_SHIFT; 3281f1c6775bSGustavo F. Padovan __skb_queue_tail(&chan->busy_q, skb); 3282525cd185SGustavo F. Padovan return l2cap_try_push_rx_skb(chan); 32830a708f8fSGustavo F. Padovan 32840a708f8fSGustavo F. Padovan 32850a708f8fSGustavo F. Padovan } 32860a708f8fSGustavo F. Padovan 3287525cd185SGustavo F. Padovan err = l2cap_ertm_reassembly_sdu(chan, skb, control); 32880a708f8fSGustavo F. Padovan if (err >= 0) { 328942e5c802SGustavo F. Padovan chan->buffer_seq = (chan->buffer_seq + 1) % 64; 32900a708f8fSGustavo F. Padovan return err; 32910a708f8fSGustavo F. Padovan } 32920a708f8fSGustavo F. Padovan 32930a708f8fSGustavo F. Padovan /* Busy Condition */ 3294311bb895SGustavo F. Padovan BT_DBG("chan %p, Enter local busy", chan); 32950a708f8fSGustavo F. Padovan 3296525cd185SGustavo F. Padovan chan->conn_state |= L2CAP_CONN_LOCAL_BUSY; 32970a708f8fSGustavo F. Padovan bt_cb(skb)->sar = control >> L2CAP_CTRL_SAR_SHIFT; 3298f1c6775bSGustavo F. Padovan __skb_queue_tail(&chan->busy_q, skb); 32990a708f8fSGustavo F. Padovan 330042e5c802SGustavo F. Padovan sctrl = chan->buffer_seq << L2CAP_CTRL_REQSEQ_SHIFT; 33010a708f8fSGustavo F. Padovan sctrl |= L2CAP_SUPER_RCV_NOT_READY; 3302525cd185SGustavo F. Padovan l2cap_send_sframe(chan, sctrl); 33030a708f8fSGustavo F. Padovan 3304525cd185SGustavo F. Padovan chan->conn_state |= L2CAP_CONN_RNR_SENT; 33050a708f8fSGustavo F. Padovan 3306e92c8e70SGustavo F. Padovan del_timer(&chan->ack_timer); 33070a708f8fSGustavo F. Padovan 3308311bb895SGustavo F. Padovan queue_work(_busy_wq, &chan->busy_work); 33090a708f8fSGustavo F. Padovan 33100a708f8fSGustavo F. Padovan return err; 33110a708f8fSGustavo F. Padovan } 33120a708f8fSGustavo F. Padovan 3313525cd185SGustavo F. Padovan static int l2cap_streaming_reassembly_sdu(struct l2cap_chan *chan, struct sk_buff *skb, u16 control) 33140a708f8fSGustavo F. Padovan { 33150a708f8fSGustavo F. Padovan struct sk_buff *_skb; 33160a708f8fSGustavo F. Padovan int err = -EINVAL; 33170a708f8fSGustavo F. Padovan 33180a708f8fSGustavo F. Padovan /* 33190a708f8fSGustavo F. Padovan * TODO: We have to notify the userland if some data is lost with the 33200a708f8fSGustavo F. Padovan * Streaming Mode. 33210a708f8fSGustavo F. Padovan */ 33220a708f8fSGustavo F. Padovan 33230a708f8fSGustavo F. Padovan switch (control & L2CAP_CTRL_SAR) { 33240a708f8fSGustavo F. Padovan case L2CAP_SDU_UNSEGMENTED: 3325525cd185SGustavo F. Padovan if (chan->conn_state & L2CAP_CONN_SAR_SDU) { 33266f61fd47SGustavo F. Padovan kfree_skb(chan->sdu); 33270a708f8fSGustavo F. Padovan break; 33280a708f8fSGustavo F. Padovan } 33290a708f8fSGustavo F. Padovan 3330525cd185SGustavo F. Padovan err = sock_queue_rcv_skb(chan->sk, skb); 33310a708f8fSGustavo F. Padovan if (!err) 33320a708f8fSGustavo F. Padovan return 0; 33330a708f8fSGustavo F. Padovan 33340a708f8fSGustavo F. Padovan break; 33350a708f8fSGustavo F. Padovan 33360a708f8fSGustavo F. Padovan case L2CAP_SDU_START: 3337525cd185SGustavo F. Padovan if (chan->conn_state & L2CAP_CONN_SAR_SDU) { 33386f61fd47SGustavo F. Padovan kfree_skb(chan->sdu); 33390a708f8fSGustavo F. Padovan break; 33400a708f8fSGustavo F. Padovan } 33410a708f8fSGustavo F. Padovan 33426f61fd47SGustavo F. Padovan chan->sdu_len = get_unaligned_le16(skb->data); 33430a708f8fSGustavo F. Padovan skb_pull(skb, 2); 33440a708f8fSGustavo F. Padovan 33450c1bc5c6SGustavo F. Padovan if (chan->sdu_len > chan->imtu) { 33460a708f8fSGustavo F. Padovan err = -EMSGSIZE; 33470a708f8fSGustavo F. Padovan break; 33480a708f8fSGustavo F. Padovan } 33490a708f8fSGustavo F. Padovan 33506f61fd47SGustavo F. Padovan chan->sdu = bt_skb_alloc(chan->sdu_len, GFP_ATOMIC); 33516f61fd47SGustavo F. Padovan if (!chan->sdu) { 33520a708f8fSGustavo F. Padovan err = -ENOMEM; 33530a708f8fSGustavo F. Padovan break; 33540a708f8fSGustavo F. Padovan } 33550a708f8fSGustavo F. Padovan 33566f61fd47SGustavo F. Padovan memcpy(skb_put(chan->sdu, skb->len), skb->data, skb->len); 33570a708f8fSGustavo F. Padovan 3358525cd185SGustavo F. Padovan chan->conn_state |= L2CAP_CONN_SAR_SDU; 33596f61fd47SGustavo F. Padovan chan->partial_sdu_len = skb->len; 33600a708f8fSGustavo F. Padovan err = 0; 33610a708f8fSGustavo F. Padovan break; 33620a708f8fSGustavo F. Padovan 33630a708f8fSGustavo F. Padovan case L2CAP_SDU_CONTINUE: 3364525cd185SGustavo F. Padovan if (!(chan->conn_state & L2CAP_CONN_SAR_SDU)) 33650a708f8fSGustavo F. Padovan break; 33660a708f8fSGustavo F. Padovan 33676f61fd47SGustavo F. Padovan memcpy(skb_put(chan->sdu, skb->len), skb->data, skb->len); 33680a708f8fSGustavo F. Padovan 33696f61fd47SGustavo F. Padovan chan->partial_sdu_len += skb->len; 33706f61fd47SGustavo F. Padovan if (chan->partial_sdu_len > chan->sdu_len) 33716f61fd47SGustavo F. Padovan kfree_skb(chan->sdu); 33720a708f8fSGustavo F. Padovan else 33730a708f8fSGustavo F. Padovan err = 0; 33740a708f8fSGustavo F. Padovan 33750a708f8fSGustavo F. Padovan break; 33760a708f8fSGustavo F. Padovan 33770a708f8fSGustavo F. Padovan case L2CAP_SDU_END: 3378525cd185SGustavo F. Padovan if (!(chan->conn_state & L2CAP_CONN_SAR_SDU)) 33790a708f8fSGustavo F. Padovan break; 33800a708f8fSGustavo F. Padovan 33816f61fd47SGustavo F. Padovan memcpy(skb_put(chan->sdu, skb->len), skb->data, skb->len); 33820a708f8fSGustavo F. Padovan 3383525cd185SGustavo F. Padovan chan->conn_state &= ~L2CAP_CONN_SAR_SDU; 33846f61fd47SGustavo F. Padovan chan->partial_sdu_len += skb->len; 33850a708f8fSGustavo F. Padovan 33860c1bc5c6SGustavo F. Padovan if (chan->partial_sdu_len > chan->imtu) 33870a708f8fSGustavo F. Padovan goto drop; 33880a708f8fSGustavo F. Padovan 33896f61fd47SGustavo F. Padovan if (chan->partial_sdu_len == chan->sdu_len) { 33906f61fd47SGustavo F. Padovan _skb = skb_clone(chan->sdu, GFP_ATOMIC); 3391525cd185SGustavo F. Padovan err = sock_queue_rcv_skb(chan->sk, _skb); 33920a708f8fSGustavo F. Padovan if (err < 0) 33930a708f8fSGustavo F. Padovan kfree_skb(_skb); 33940a708f8fSGustavo F. Padovan } 33950a708f8fSGustavo F. Padovan err = 0; 33960a708f8fSGustavo F. Padovan 33970a708f8fSGustavo F. Padovan drop: 33986f61fd47SGustavo F. Padovan kfree_skb(chan->sdu); 33990a708f8fSGustavo F. Padovan break; 34000a708f8fSGustavo F. Padovan } 34010a708f8fSGustavo F. Padovan 34020a708f8fSGustavo F. Padovan kfree_skb(skb); 34030a708f8fSGustavo F. Padovan return err; 34040a708f8fSGustavo F. Padovan } 34050a708f8fSGustavo F. Padovan 3406525cd185SGustavo F. Padovan static void l2cap_check_srej_gap(struct l2cap_chan *chan, u8 tx_seq) 34070a708f8fSGustavo F. Padovan { 34080a708f8fSGustavo F. Padovan struct sk_buff *skb; 34090a708f8fSGustavo F. Padovan u16 control; 34100a708f8fSGustavo F. Padovan 3411f1c6775bSGustavo F. Padovan while ((skb = skb_peek(&chan->srej_q))) { 34120a708f8fSGustavo F. Padovan if (bt_cb(skb)->tx_seq != tx_seq) 34130a708f8fSGustavo F. Padovan break; 34140a708f8fSGustavo F. Padovan 3415f1c6775bSGustavo F. Padovan skb = skb_dequeue(&chan->srej_q); 34160a708f8fSGustavo F. Padovan control = bt_cb(skb)->sar << L2CAP_CTRL_SAR_SHIFT; 3417525cd185SGustavo F. Padovan l2cap_ertm_reassembly_sdu(chan, skb, control); 341842e5c802SGustavo F. Padovan chan->buffer_seq_srej = 341942e5c802SGustavo F. Padovan (chan->buffer_seq_srej + 1) % 64; 34200a708f8fSGustavo F. Padovan tx_seq = (tx_seq + 1) % 64; 34210a708f8fSGustavo F. Padovan } 34220a708f8fSGustavo F. Padovan } 34230a708f8fSGustavo F. Padovan 3424525cd185SGustavo F. Padovan static void l2cap_resend_srejframe(struct l2cap_chan *chan, u8 tx_seq) 34250a708f8fSGustavo F. Padovan { 34260a708f8fSGustavo F. Padovan struct srej_list *l, *tmp; 34270a708f8fSGustavo F. Padovan u16 control; 34280a708f8fSGustavo F. Padovan 342939d5a3eeSGustavo F. Padovan list_for_each_entry_safe(l, tmp, &chan->srej_l, list) { 34300a708f8fSGustavo F. Padovan if (l->tx_seq == tx_seq) { 34310a708f8fSGustavo F. Padovan list_del(&l->list); 34320a708f8fSGustavo F. Padovan kfree(l); 34330a708f8fSGustavo F. Padovan return; 34340a708f8fSGustavo F. Padovan } 34350a708f8fSGustavo F. Padovan control = L2CAP_SUPER_SELECT_REJECT; 34360a708f8fSGustavo F. Padovan control |= l->tx_seq << L2CAP_CTRL_REQSEQ_SHIFT; 3437525cd185SGustavo F. Padovan l2cap_send_sframe(chan, control); 34380a708f8fSGustavo F. Padovan list_del(&l->list); 343939d5a3eeSGustavo F. Padovan list_add_tail(&l->list, &chan->srej_l); 34400a708f8fSGustavo F. Padovan } 34410a708f8fSGustavo F. Padovan } 34420a708f8fSGustavo F. Padovan 3443525cd185SGustavo F. Padovan static void l2cap_send_srejframe(struct l2cap_chan *chan, u8 tx_seq) 34440a708f8fSGustavo F. Padovan { 34450a708f8fSGustavo F. Padovan struct srej_list *new; 34460a708f8fSGustavo F. Padovan u16 control; 34470a708f8fSGustavo F. Padovan 344842e5c802SGustavo F. Padovan while (tx_seq != chan->expected_tx_seq) { 34490a708f8fSGustavo F. Padovan control = L2CAP_SUPER_SELECT_REJECT; 345042e5c802SGustavo F. Padovan control |= chan->expected_tx_seq << L2CAP_CTRL_REQSEQ_SHIFT; 3451525cd185SGustavo F. Padovan l2cap_send_sframe(chan, control); 34520a708f8fSGustavo F. Padovan 34530a708f8fSGustavo F. Padovan new = kzalloc(sizeof(struct srej_list), GFP_ATOMIC); 345442e5c802SGustavo F. Padovan new->tx_seq = chan->expected_tx_seq; 345542e5c802SGustavo F. Padovan chan->expected_tx_seq = (chan->expected_tx_seq + 1) % 64; 345639d5a3eeSGustavo F. Padovan list_add_tail(&new->list, &chan->srej_l); 34570a708f8fSGustavo F. Padovan } 345842e5c802SGustavo F. Padovan chan->expected_tx_seq = (chan->expected_tx_seq + 1) % 64; 34590a708f8fSGustavo F. Padovan } 34600a708f8fSGustavo F. Padovan 3461525cd185SGustavo F. Padovan static inline int l2cap_data_channel_iframe(struct l2cap_chan *chan, u16 rx_control, struct sk_buff *skb) 34620a708f8fSGustavo F. Padovan { 34630a708f8fSGustavo F. Padovan u8 tx_seq = __get_txseq(rx_control); 34640a708f8fSGustavo F. Padovan u8 req_seq = __get_reqseq(rx_control); 34650a708f8fSGustavo F. Padovan u8 sar = rx_control >> L2CAP_CTRL_SAR_SHIFT; 34660a708f8fSGustavo F. Padovan int tx_seq_offset, expected_tx_seq_offset; 346747d1ec61SGustavo F. Padovan int num_to_ack = (chan->tx_win/6) + 1; 34680a708f8fSGustavo F. Padovan int err = 0; 34690a708f8fSGustavo F. Padovan 3470525cd185SGustavo F. Padovan BT_DBG("chan %p len %d tx_seq %d rx_control 0x%4.4x", chan, skb->len, 3471525cd185SGustavo F. Padovan tx_seq, rx_control); 34720a708f8fSGustavo F. Padovan 34730a708f8fSGustavo F. Padovan if (L2CAP_CTRL_FINAL & rx_control && 3474525cd185SGustavo F. Padovan chan->conn_state & L2CAP_CONN_WAIT_F) { 3475e92c8e70SGustavo F. Padovan del_timer(&chan->monitor_timer); 34766a026610SGustavo F. Padovan if (chan->unacked_frames > 0) 34770a708f8fSGustavo F. Padovan __mod_retrans_timer(); 3478525cd185SGustavo F. Padovan chan->conn_state &= ~L2CAP_CONN_WAIT_F; 34790a708f8fSGustavo F. Padovan } 34800a708f8fSGustavo F. Padovan 348142e5c802SGustavo F. Padovan chan->expected_ack_seq = req_seq; 348242e5c802SGustavo F. Padovan l2cap_drop_acked_frames(chan); 34830a708f8fSGustavo F. Padovan 348442e5c802SGustavo F. Padovan if (tx_seq == chan->expected_tx_seq) 34850a708f8fSGustavo F. Padovan goto expected; 34860a708f8fSGustavo F. Padovan 348742e5c802SGustavo F. Padovan tx_seq_offset = (tx_seq - chan->buffer_seq) % 64; 34880a708f8fSGustavo F. Padovan if (tx_seq_offset < 0) 34890a708f8fSGustavo F. Padovan tx_seq_offset += 64; 34900a708f8fSGustavo F. Padovan 34910a708f8fSGustavo F. Padovan /* invalid tx_seq */ 349247d1ec61SGustavo F. Padovan if (tx_seq_offset >= chan->tx_win) { 34938c1d787bSGustavo F. Padovan l2cap_send_disconn_req(chan->conn, chan, ECONNRESET); 34940a708f8fSGustavo F. Padovan goto drop; 34950a708f8fSGustavo F. Padovan } 34960a708f8fSGustavo F. Padovan 3497525cd185SGustavo F. Padovan if (chan->conn_state == L2CAP_CONN_LOCAL_BUSY) 34980a708f8fSGustavo F. Padovan goto drop; 34990a708f8fSGustavo F. Padovan 3500525cd185SGustavo F. Padovan if (chan->conn_state & L2CAP_CONN_SREJ_SENT) { 35010a708f8fSGustavo F. Padovan struct srej_list *first; 35020a708f8fSGustavo F. Padovan 350339d5a3eeSGustavo F. Padovan first = list_first_entry(&chan->srej_l, 35040a708f8fSGustavo F. Padovan struct srej_list, list); 35050a708f8fSGustavo F. Padovan if (tx_seq == first->tx_seq) { 350642e5c802SGustavo F. Padovan l2cap_add_to_srej_queue(chan, skb, tx_seq, sar); 3507525cd185SGustavo F. Padovan l2cap_check_srej_gap(chan, tx_seq); 35080a708f8fSGustavo F. Padovan 35090a708f8fSGustavo F. Padovan list_del(&first->list); 35100a708f8fSGustavo F. Padovan kfree(first); 35110a708f8fSGustavo F. Padovan 351239d5a3eeSGustavo F. Padovan if (list_empty(&chan->srej_l)) { 351342e5c802SGustavo F. Padovan chan->buffer_seq = chan->buffer_seq_srej; 3514525cd185SGustavo F. Padovan chan->conn_state &= ~L2CAP_CONN_SREJ_SENT; 3515525cd185SGustavo F. Padovan l2cap_send_ack(chan); 351649208c9cSGustavo F. Padovan BT_DBG("chan %p, Exit SREJ_SENT", chan); 35170a708f8fSGustavo F. Padovan } 35180a708f8fSGustavo F. Padovan } else { 35190a708f8fSGustavo F. Padovan struct srej_list *l; 35200a708f8fSGustavo F. Padovan 35210a708f8fSGustavo F. Padovan /* duplicated tx_seq */ 352242e5c802SGustavo F. Padovan if (l2cap_add_to_srej_queue(chan, skb, tx_seq, sar) < 0) 35230a708f8fSGustavo F. Padovan goto drop; 35240a708f8fSGustavo F. Padovan 352539d5a3eeSGustavo F. Padovan list_for_each_entry(l, &chan->srej_l, list) { 35260a708f8fSGustavo F. Padovan if (l->tx_seq == tx_seq) { 3527525cd185SGustavo F. Padovan l2cap_resend_srejframe(chan, tx_seq); 35280a708f8fSGustavo F. Padovan return 0; 35290a708f8fSGustavo F. Padovan } 35300a708f8fSGustavo F. Padovan } 3531525cd185SGustavo F. Padovan l2cap_send_srejframe(chan, tx_seq); 35320a708f8fSGustavo F. Padovan } 35330a708f8fSGustavo F. Padovan } else { 35340a708f8fSGustavo F. Padovan expected_tx_seq_offset = 353542e5c802SGustavo F. Padovan (chan->expected_tx_seq - chan->buffer_seq) % 64; 35360a708f8fSGustavo F. Padovan if (expected_tx_seq_offset < 0) 35370a708f8fSGustavo F. Padovan expected_tx_seq_offset += 64; 35380a708f8fSGustavo F. Padovan 35390a708f8fSGustavo F. Padovan /* duplicated tx_seq */ 35400a708f8fSGustavo F. Padovan if (tx_seq_offset < expected_tx_seq_offset) 35410a708f8fSGustavo F. Padovan goto drop; 35420a708f8fSGustavo F. Padovan 3543525cd185SGustavo F. Padovan chan->conn_state |= L2CAP_CONN_SREJ_SENT; 35440a708f8fSGustavo F. Padovan 354549208c9cSGustavo F. Padovan BT_DBG("chan %p, Enter SREJ", chan); 35460a708f8fSGustavo F. Padovan 354739d5a3eeSGustavo F. Padovan INIT_LIST_HEAD(&chan->srej_l); 354842e5c802SGustavo F. Padovan chan->buffer_seq_srej = chan->buffer_seq; 35490a708f8fSGustavo F. Padovan 3550f1c6775bSGustavo F. Padovan __skb_queue_head_init(&chan->srej_q); 3551f1c6775bSGustavo F. Padovan __skb_queue_head_init(&chan->busy_q); 355242e5c802SGustavo F. Padovan l2cap_add_to_srej_queue(chan, skb, tx_seq, sar); 35530a708f8fSGustavo F. Padovan 3554525cd185SGustavo F. Padovan chan->conn_state |= L2CAP_CONN_SEND_PBIT; 35550a708f8fSGustavo F. Padovan 3556525cd185SGustavo F. Padovan l2cap_send_srejframe(chan, tx_seq); 35570a708f8fSGustavo F. Padovan 3558e92c8e70SGustavo F. Padovan del_timer(&chan->ack_timer); 35590a708f8fSGustavo F. Padovan } 35600a708f8fSGustavo F. Padovan return 0; 35610a708f8fSGustavo F. Padovan 35620a708f8fSGustavo F. Padovan expected: 356342e5c802SGustavo F. Padovan chan->expected_tx_seq = (chan->expected_tx_seq + 1) % 64; 35640a708f8fSGustavo F. Padovan 3565525cd185SGustavo F. Padovan if (chan->conn_state & L2CAP_CONN_SREJ_SENT) { 35660a708f8fSGustavo F. Padovan bt_cb(skb)->tx_seq = tx_seq; 35670a708f8fSGustavo F. Padovan bt_cb(skb)->sar = sar; 3568f1c6775bSGustavo F. Padovan __skb_queue_tail(&chan->srej_q, skb); 35690a708f8fSGustavo F. Padovan return 0; 35700a708f8fSGustavo F. Padovan } 35710a708f8fSGustavo F. Padovan 3572525cd185SGustavo F. Padovan err = l2cap_push_rx_skb(chan, skb, rx_control); 35730a708f8fSGustavo F. Padovan if (err < 0) 35740a708f8fSGustavo F. Padovan return 0; 35750a708f8fSGustavo F. Padovan 35760a708f8fSGustavo F. Padovan if (rx_control & L2CAP_CTRL_FINAL) { 3577525cd185SGustavo F. Padovan if (chan->conn_state & L2CAP_CONN_REJ_ACT) 3578525cd185SGustavo F. Padovan chan->conn_state &= ~L2CAP_CONN_REJ_ACT; 35790a708f8fSGustavo F. Padovan else 3580525cd185SGustavo F. Padovan l2cap_retransmit_frames(chan); 35810a708f8fSGustavo F. Padovan } 35820a708f8fSGustavo F. Padovan 35830a708f8fSGustavo F. Padovan __mod_ack_timer(); 35840a708f8fSGustavo F. Padovan 35856a026610SGustavo F. Padovan chan->num_acked = (chan->num_acked + 1) % num_to_ack; 35866a026610SGustavo F. Padovan if (chan->num_acked == num_to_ack - 1) 3587525cd185SGustavo F. Padovan l2cap_send_ack(chan); 35880a708f8fSGustavo F. Padovan 35890a708f8fSGustavo F. Padovan return 0; 35900a708f8fSGustavo F. Padovan 35910a708f8fSGustavo F. Padovan drop: 35920a708f8fSGustavo F. Padovan kfree_skb(skb); 35930a708f8fSGustavo F. Padovan return 0; 35940a708f8fSGustavo F. Padovan } 35950a708f8fSGustavo F. Padovan 3596525cd185SGustavo F. Padovan static inline void l2cap_data_channel_rrframe(struct l2cap_chan *chan, u16 rx_control) 35970a708f8fSGustavo F. Padovan { 359849208c9cSGustavo F. Padovan BT_DBG("chan %p, req_seq %d ctrl 0x%4.4x", chan, __get_reqseq(rx_control), 35990a708f8fSGustavo F. Padovan rx_control); 36000a708f8fSGustavo F. Padovan 360142e5c802SGustavo F. Padovan chan->expected_ack_seq = __get_reqseq(rx_control); 360242e5c802SGustavo F. Padovan l2cap_drop_acked_frames(chan); 36030a708f8fSGustavo F. Padovan 36040a708f8fSGustavo F. Padovan if (rx_control & L2CAP_CTRL_POLL) { 3605525cd185SGustavo F. Padovan chan->conn_state |= L2CAP_CONN_SEND_FBIT; 3606525cd185SGustavo F. Padovan if (chan->conn_state & L2CAP_CONN_SREJ_SENT) { 3607525cd185SGustavo F. Padovan if ((chan->conn_state & L2CAP_CONN_REMOTE_BUSY) && 36086a026610SGustavo F. Padovan (chan->unacked_frames > 0)) 36090a708f8fSGustavo F. Padovan __mod_retrans_timer(); 36100a708f8fSGustavo F. Padovan 3611525cd185SGustavo F. Padovan chan->conn_state &= ~L2CAP_CONN_REMOTE_BUSY; 3612525cd185SGustavo F. Padovan l2cap_send_srejtail(chan); 36130a708f8fSGustavo F. Padovan } else { 3614525cd185SGustavo F. Padovan l2cap_send_i_or_rr_or_rnr(chan); 36150a708f8fSGustavo F. Padovan } 36160a708f8fSGustavo F. Padovan 36170a708f8fSGustavo F. Padovan } else if (rx_control & L2CAP_CTRL_FINAL) { 3618525cd185SGustavo F. Padovan chan->conn_state &= ~L2CAP_CONN_REMOTE_BUSY; 36190a708f8fSGustavo F. Padovan 3620525cd185SGustavo F. Padovan if (chan->conn_state & L2CAP_CONN_REJ_ACT) 3621525cd185SGustavo F. Padovan chan->conn_state &= ~L2CAP_CONN_REJ_ACT; 36220a708f8fSGustavo F. Padovan else 3623525cd185SGustavo F. Padovan l2cap_retransmit_frames(chan); 36240a708f8fSGustavo F. Padovan 36250a708f8fSGustavo F. Padovan } else { 3626525cd185SGustavo F. Padovan if ((chan->conn_state & L2CAP_CONN_REMOTE_BUSY) && 36276a026610SGustavo F. Padovan (chan->unacked_frames > 0)) 36280a708f8fSGustavo F. Padovan __mod_retrans_timer(); 36290a708f8fSGustavo F. Padovan 3630525cd185SGustavo F. Padovan chan->conn_state &= ~L2CAP_CONN_REMOTE_BUSY; 3631525cd185SGustavo F. Padovan if (chan->conn_state & L2CAP_CONN_SREJ_SENT) 3632525cd185SGustavo F. Padovan l2cap_send_ack(chan); 36330a708f8fSGustavo F. Padovan else 3634525cd185SGustavo F. Padovan l2cap_ertm_send(chan); 36350a708f8fSGustavo F. Padovan } 36360a708f8fSGustavo F. Padovan } 36370a708f8fSGustavo F. Padovan 3638525cd185SGustavo F. Padovan static inline void l2cap_data_channel_rejframe(struct l2cap_chan *chan, u16 rx_control) 36390a708f8fSGustavo F. Padovan { 36400a708f8fSGustavo F. Padovan u8 tx_seq = __get_reqseq(rx_control); 36410a708f8fSGustavo F. Padovan 3642525cd185SGustavo F. Padovan BT_DBG("chan %p, req_seq %d ctrl 0x%4.4x", chan, tx_seq, rx_control); 36430a708f8fSGustavo F. Padovan 3644525cd185SGustavo F. Padovan chan->conn_state &= ~L2CAP_CONN_REMOTE_BUSY; 36450a708f8fSGustavo F. Padovan 364642e5c802SGustavo F. Padovan chan->expected_ack_seq = tx_seq; 364742e5c802SGustavo F. Padovan l2cap_drop_acked_frames(chan); 36480a708f8fSGustavo F. Padovan 36490a708f8fSGustavo F. Padovan if (rx_control & L2CAP_CTRL_FINAL) { 3650525cd185SGustavo F. Padovan if (chan->conn_state & L2CAP_CONN_REJ_ACT) 3651525cd185SGustavo F. Padovan chan->conn_state &= ~L2CAP_CONN_REJ_ACT; 36520a708f8fSGustavo F. Padovan else 3653525cd185SGustavo F. Padovan l2cap_retransmit_frames(chan); 36540a708f8fSGustavo F. Padovan } else { 3655525cd185SGustavo F. Padovan l2cap_retransmit_frames(chan); 36560a708f8fSGustavo F. Padovan 3657525cd185SGustavo F. Padovan if (chan->conn_state & L2CAP_CONN_WAIT_F) 3658525cd185SGustavo F. Padovan chan->conn_state |= L2CAP_CONN_REJ_ACT; 36590a708f8fSGustavo F. Padovan } 36600a708f8fSGustavo F. Padovan } 3661525cd185SGustavo F. Padovan static inline void l2cap_data_channel_srejframe(struct l2cap_chan *chan, u16 rx_control) 36620a708f8fSGustavo F. Padovan { 36630a708f8fSGustavo F. Padovan u8 tx_seq = __get_reqseq(rx_control); 36640a708f8fSGustavo F. Padovan 3665525cd185SGustavo F. Padovan BT_DBG("chan %p, req_seq %d ctrl 0x%4.4x", chan, tx_seq, rx_control); 36660a708f8fSGustavo F. Padovan 3667525cd185SGustavo F. Padovan chan->conn_state &= ~L2CAP_CONN_REMOTE_BUSY; 36680a708f8fSGustavo F. Padovan 36690a708f8fSGustavo F. Padovan if (rx_control & L2CAP_CTRL_POLL) { 367042e5c802SGustavo F. Padovan chan->expected_ack_seq = tx_seq; 367142e5c802SGustavo F. Padovan l2cap_drop_acked_frames(chan); 36720a708f8fSGustavo F. Padovan 3673525cd185SGustavo F. Padovan chan->conn_state |= L2CAP_CONN_SEND_FBIT; 3674525cd185SGustavo F. Padovan l2cap_retransmit_one_frame(chan, tx_seq); 36750a708f8fSGustavo F. Padovan 3676525cd185SGustavo F. Padovan l2cap_ertm_send(chan); 36770a708f8fSGustavo F. Padovan 3678525cd185SGustavo F. Padovan if (chan->conn_state & L2CAP_CONN_WAIT_F) { 36796a026610SGustavo F. Padovan chan->srej_save_reqseq = tx_seq; 3680525cd185SGustavo F. Padovan chan->conn_state |= L2CAP_CONN_SREJ_ACT; 36810a708f8fSGustavo F. Padovan } 36820a708f8fSGustavo F. Padovan } else if (rx_control & L2CAP_CTRL_FINAL) { 3683525cd185SGustavo F. Padovan if ((chan->conn_state & L2CAP_CONN_SREJ_ACT) && 36846a026610SGustavo F. Padovan chan->srej_save_reqseq == tx_seq) 3685525cd185SGustavo F. Padovan chan->conn_state &= ~L2CAP_CONN_SREJ_ACT; 36860a708f8fSGustavo F. Padovan else 3687525cd185SGustavo F. Padovan l2cap_retransmit_one_frame(chan, tx_seq); 36880a708f8fSGustavo F. Padovan } else { 3689525cd185SGustavo F. Padovan l2cap_retransmit_one_frame(chan, tx_seq); 3690525cd185SGustavo F. Padovan if (chan->conn_state & L2CAP_CONN_WAIT_F) { 36916a026610SGustavo F. Padovan chan->srej_save_reqseq = tx_seq; 3692525cd185SGustavo F. Padovan chan->conn_state |= L2CAP_CONN_SREJ_ACT; 36930a708f8fSGustavo F. Padovan } 36940a708f8fSGustavo F. Padovan } 36950a708f8fSGustavo F. Padovan } 36960a708f8fSGustavo F. Padovan 3697525cd185SGustavo F. Padovan static inline void l2cap_data_channel_rnrframe(struct l2cap_chan *chan, u16 rx_control) 36980a708f8fSGustavo F. Padovan { 36990a708f8fSGustavo F. Padovan u8 tx_seq = __get_reqseq(rx_control); 37000a708f8fSGustavo F. Padovan 3701525cd185SGustavo F. Padovan BT_DBG("chan %p, req_seq %d ctrl 0x%4.4x", chan, tx_seq, rx_control); 37020a708f8fSGustavo F. Padovan 3703525cd185SGustavo F. Padovan chan->conn_state |= L2CAP_CONN_REMOTE_BUSY; 370442e5c802SGustavo F. Padovan chan->expected_ack_seq = tx_seq; 370542e5c802SGustavo F. Padovan l2cap_drop_acked_frames(chan); 37060a708f8fSGustavo F. Padovan 37070a708f8fSGustavo F. Padovan if (rx_control & L2CAP_CTRL_POLL) 3708525cd185SGustavo F. Padovan chan->conn_state |= L2CAP_CONN_SEND_FBIT; 37090a708f8fSGustavo F. Padovan 3710525cd185SGustavo F. Padovan if (!(chan->conn_state & L2CAP_CONN_SREJ_SENT)) { 3711e92c8e70SGustavo F. Padovan del_timer(&chan->retrans_timer); 37120a708f8fSGustavo F. Padovan if (rx_control & L2CAP_CTRL_POLL) 3713525cd185SGustavo F. Padovan l2cap_send_rr_or_rnr(chan, L2CAP_CTRL_FINAL); 37140a708f8fSGustavo F. Padovan return; 37150a708f8fSGustavo F. Padovan } 37160a708f8fSGustavo F. Padovan 37170a708f8fSGustavo F. Padovan if (rx_control & L2CAP_CTRL_POLL) 3718525cd185SGustavo F. Padovan l2cap_send_srejtail(chan); 37190a708f8fSGustavo F. Padovan else 3720525cd185SGustavo F. Padovan l2cap_send_sframe(chan, L2CAP_SUPER_RCV_READY); 37210a708f8fSGustavo F. Padovan } 37220a708f8fSGustavo F. Padovan 3723525cd185SGustavo F. Padovan static inline int l2cap_data_channel_sframe(struct l2cap_chan *chan, u16 rx_control, struct sk_buff *skb) 37240a708f8fSGustavo F. Padovan { 3725525cd185SGustavo F. Padovan BT_DBG("chan %p rx_control 0x%4.4x len %d", chan, rx_control, skb->len); 37260a708f8fSGustavo F. Padovan 37270a708f8fSGustavo F. Padovan if (L2CAP_CTRL_FINAL & rx_control && 3728525cd185SGustavo F. Padovan chan->conn_state & L2CAP_CONN_WAIT_F) { 3729e92c8e70SGustavo F. Padovan del_timer(&chan->monitor_timer); 37306a026610SGustavo F. Padovan if (chan->unacked_frames > 0) 37310a708f8fSGustavo F. Padovan __mod_retrans_timer(); 3732525cd185SGustavo F. Padovan chan->conn_state &= ~L2CAP_CONN_WAIT_F; 37330a708f8fSGustavo F. Padovan } 37340a708f8fSGustavo F. Padovan 37350a708f8fSGustavo F. Padovan switch (rx_control & L2CAP_CTRL_SUPERVISE) { 37360a708f8fSGustavo F. Padovan case L2CAP_SUPER_RCV_READY: 3737525cd185SGustavo F. Padovan l2cap_data_channel_rrframe(chan, rx_control); 37380a708f8fSGustavo F. Padovan break; 37390a708f8fSGustavo F. Padovan 37400a708f8fSGustavo F. Padovan case L2CAP_SUPER_REJECT: 3741525cd185SGustavo F. Padovan l2cap_data_channel_rejframe(chan, rx_control); 37420a708f8fSGustavo F. Padovan break; 37430a708f8fSGustavo F. Padovan 37440a708f8fSGustavo F. Padovan case L2CAP_SUPER_SELECT_REJECT: 3745525cd185SGustavo F. Padovan l2cap_data_channel_srejframe(chan, rx_control); 37460a708f8fSGustavo F. Padovan break; 37470a708f8fSGustavo F. Padovan 37480a708f8fSGustavo F. Padovan case L2CAP_SUPER_RCV_NOT_READY: 3749525cd185SGustavo F. Padovan l2cap_data_channel_rnrframe(chan, rx_control); 37500a708f8fSGustavo F. Padovan break; 37510a708f8fSGustavo F. Padovan } 37520a708f8fSGustavo F. Padovan 37530a708f8fSGustavo F. Padovan kfree_skb(skb); 37540a708f8fSGustavo F. Padovan return 0; 37550a708f8fSGustavo F. Padovan } 37560a708f8fSGustavo F. Padovan 37570a708f8fSGustavo F. Padovan static int l2cap_ertm_data_rcv(struct sock *sk, struct sk_buff *skb) 37580a708f8fSGustavo F. Padovan { 3759525cd185SGustavo F. Padovan struct l2cap_chan *chan = l2cap_pi(sk)->chan; 37600a708f8fSGustavo F. Padovan u16 control; 37610a708f8fSGustavo F. Padovan u8 req_seq; 37620a708f8fSGustavo F. Padovan int len, next_tx_seq_offset, req_seq_offset; 37630a708f8fSGustavo F. Padovan 37640a708f8fSGustavo F. Padovan control = get_unaligned_le16(skb->data); 37650a708f8fSGustavo F. Padovan skb_pull(skb, 2); 37660a708f8fSGustavo F. Padovan len = skb->len; 37670a708f8fSGustavo F. Padovan 37680a708f8fSGustavo F. Padovan /* 37690a708f8fSGustavo F. Padovan * We can just drop the corrupted I-frame here. 37700a708f8fSGustavo F. Padovan * Receiver will miss it and start proper recovery 37710a708f8fSGustavo F. Padovan * procedures and ask retransmission. 37720a708f8fSGustavo F. Padovan */ 377347d1ec61SGustavo F. Padovan if (l2cap_check_fcs(chan, skb)) 37740a708f8fSGustavo F. Padovan goto drop; 37750a708f8fSGustavo F. Padovan 37760a708f8fSGustavo F. Padovan if (__is_sar_start(control) && __is_iframe(control)) 37770a708f8fSGustavo F. Padovan len -= 2; 37780a708f8fSGustavo F. Padovan 377947d1ec61SGustavo F. Padovan if (chan->fcs == L2CAP_FCS_CRC16) 37800a708f8fSGustavo F. Padovan len -= 2; 37810a708f8fSGustavo F. Padovan 378247d1ec61SGustavo F. Padovan if (len > chan->mps) { 37838c1d787bSGustavo F. Padovan l2cap_send_disconn_req(chan->conn, chan, ECONNRESET); 37840a708f8fSGustavo F. Padovan goto drop; 37850a708f8fSGustavo F. Padovan } 37860a708f8fSGustavo F. Padovan 37870a708f8fSGustavo F. Padovan req_seq = __get_reqseq(control); 378842e5c802SGustavo F. Padovan req_seq_offset = (req_seq - chan->expected_ack_seq) % 64; 37890a708f8fSGustavo F. Padovan if (req_seq_offset < 0) 37900a708f8fSGustavo F. Padovan req_seq_offset += 64; 37910a708f8fSGustavo F. Padovan 37920a708f8fSGustavo F. Padovan next_tx_seq_offset = 379342e5c802SGustavo F. Padovan (chan->next_tx_seq - chan->expected_ack_seq) % 64; 37940a708f8fSGustavo F. Padovan if (next_tx_seq_offset < 0) 37950a708f8fSGustavo F. Padovan next_tx_seq_offset += 64; 37960a708f8fSGustavo F. Padovan 37970a708f8fSGustavo F. Padovan /* check for invalid req-seq */ 37980a708f8fSGustavo F. Padovan if (req_seq_offset > next_tx_seq_offset) { 37998c1d787bSGustavo F. Padovan l2cap_send_disconn_req(chan->conn, chan, ECONNRESET); 38000a708f8fSGustavo F. Padovan goto drop; 38010a708f8fSGustavo F. Padovan } 38020a708f8fSGustavo F. Padovan 38030a708f8fSGustavo F. Padovan if (__is_iframe(control)) { 38040a708f8fSGustavo F. Padovan if (len < 0) { 38058c1d787bSGustavo F. Padovan l2cap_send_disconn_req(chan->conn, chan, ECONNRESET); 38060a708f8fSGustavo F. Padovan goto drop; 38070a708f8fSGustavo F. Padovan } 38080a708f8fSGustavo F. Padovan 3809525cd185SGustavo F. Padovan l2cap_data_channel_iframe(chan, control, skb); 38100a708f8fSGustavo F. Padovan } else { 38110a708f8fSGustavo F. Padovan if (len != 0) { 38120a708f8fSGustavo F. Padovan BT_ERR("%d", len); 38138c1d787bSGustavo F. Padovan l2cap_send_disconn_req(chan->conn, chan, ECONNRESET); 38140a708f8fSGustavo F. Padovan goto drop; 38150a708f8fSGustavo F. Padovan } 38160a708f8fSGustavo F. Padovan 3817525cd185SGustavo F. Padovan l2cap_data_channel_sframe(chan, control, skb); 38180a708f8fSGustavo F. Padovan } 38190a708f8fSGustavo F. Padovan 38200a708f8fSGustavo F. Padovan return 0; 38210a708f8fSGustavo F. Padovan 38220a708f8fSGustavo F. Padovan drop: 38230a708f8fSGustavo F. Padovan kfree_skb(skb); 38240a708f8fSGustavo F. Padovan return 0; 38250a708f8fSGustavo F. Padovan } 38260a708f8fSGustavo F. Padovan 38270a708f8fSGustavo F. Padovan static inline int l2cap_data_channel(struct l2cap_conn *conn, u16 cid, struct sk_buff *skb) 38280a708f8fSGustavo F. Padovan { 382948454079SGustavo F. Padovan struct l2cap_chan *chan; 3830bf734843SDavid S. Miller struct sock *sk = NULL; 38310a708f8fSGustavo F. Padovan u16 control; 38320a708f8fSGustavo F. Padovan u8 tx_seq; 38330a708f8fSGustavo F. Padovan int len; 38340a708f8fSGustavo F. Padovan 3835baa7e1faSGustavo F. Padovan chan = l2cap_get_chan_by_scid(conn, cid); 383648454079SGustavo F. Padovan if (!chan) { 38370a708f8fSGustavo F. Padovan BT_DBG("unknown cid 0x%4.4x", cid); 38380a708f8fSGustavo F. Padovan goto drop; 38390a708f8fSGustavo F. Padovan } 38400a708f8fSGustavo F. Padovan 384148454079SGustavo F. Padovan sk = chan->sk; 38420a708f8fSGustavo F. Padovan 384349208c9cSGustavo F. Padovan BT_DBG("chan %p, len %d", chan, skb->len); 38440a708f8fSGustavo F. Padovan 38450a708f8fSGustavo F. Padovan if (sk->sk_state != BT_CONNECTED) 38460a708f8fSGustavo F. Padovan goto drop; 38470a708f8fSGustavo F. Padovan 38480c1bc5c6SGustavo F. Padovan switch (chan->mode) { 38490a708f8fSGustavo F. Padovan case L2CAP_MODE_BASIC: 38500a708f8fSGustavo F. Padovan /* If socket recv buffers overflows we drop data here 38510a708f8fSGustavo F. Padovan * which is *bad* because L2CAP has to be reliable. 38520a708f8fSGustavo F. Padovan * But we don't have any other choice. L2CAP doesn't 38530a708f8fSGustavo F. Padovan * provide flow control mechanism. */ 38540a708f8fSGustavo F. Padovan 38550c1bc5c6SGustavo F. Padovan if (chan->imtu < skb->len) 38560a708f8fSGustavo F. Padovan goto drop; 38570a708f8fSGustavo F. Padovan 38580a708f8fSGustavo F. Padovan if (!sock_queue_rcv_skb(sk, skb)) 38590a708f8fSGustavo F. Padovan goto done; 38600a708f8fSGustavo F. Padovan break; 38610a708f8fSGustavo F. Padovan 38620a708f8fSGustavo F. Padovan case L2CAP_MODE_ERTM: 38630a708f8fSGustavo F. Padovan if (!sock_owned_by_user(sk)) { 38640a708f8fSGustavo F. Padovan l2cap_ertm_data_rcv(sk, skb); 38650a708f8fSGustavo F. Padovan } else { 38660a708f8fSGustavo F. Padovan if (sk_add_backlog(sk, skb)) 38670a708f8fSGustavo F. Padovan goto drop; 38680a708f8fSGustavo F. Padovan } 38690a708f8fSGustavo F. Padovan 38700a708f8fSGustavo F. Padovan goto done; 38710a708f8fSGustavo F. Padovan 38720a708f8fSGustavo F. Padovan case L2CAP_MODE_STREAMING: 38730a708f8fSGustavo F. Padovan control = get_unaligned_le16(skb->data); 38740a708f8fSGustavo F. Padovan skb_pull(skb, 2); 38750a708f8fSGustavo F. Padovan len = skb->len; 38760a708f8fSGustavo F. Padovan 387747d1ec61SGustavo F. Padovan if (l2cap_check_fcs(chan, skb)) 38780a708f8fSGustavo F. Padovan goto drop; 38790a708f8fSGustavo F. Padovan 38800a708f8fSGustavo F. Padovan if (__is_sar_start(control)) 38810a708f8fSGustavo F. Padovan len -= 2; 38820a708f8fSGustavo F. Padovan 388347d1ec61SGustavo F. Padovan if (chan->fcs == L2CAP_FCS_CRC16) 38840a708f8fSGustavo F. Padovan len -= 2; 38850a708f8fSGustavo F. Padovan 388647d1ec61SGustavo F. Padovan if (len > chan->mps || len < 0 || __is_sframe(control)) 38870a708f8fSGustavo F. Padovan goto drop; 38880a708f8fSGustavo F. Padovan 38890a708f8fSGustavo F. Padovan tx_seq = __get_txseq(control); 38900a708f8fSGustavo F. Padovan 389142e5c802SGustavo F. Padovan if (chan->expected_tx_seq == tx_seq) 389242e5c802SGustavo F. Padovan chan->expected_tx_seq = (chan->expected_tx_seq + 1) % 64; 38930a708f8fSGustavo F. Padovan else 389442e5c802SGustavo F. Padovan chan->expected_tx_seq = (tx_seq + 1) % 64; 38950a708f8fSGustavo F. Padovan 3896525cd185SGustavo F. Padovan l2cap_streaming_reassembly_sdu(chan, skb, control); 38970a708f8fSGustavo F. Padovan 38980a708f8fSGustavo F. Padovan goto done; 38990a708f8fSGustavo F. Padovan 39000a708f8fSGustavo F. Padovan default: 39010c1bc5c6SGustavo F. Padovan BT_DBG("chan %p: bad mode 0x%2.2x", chan, chan->mode); 39020a708f8fSGustavo F. Padovan break; 39030a708f8fSGustavo F. Padovan } 39040a708f8fSGustavo F. Padovan 39050a708f8fSGustavo F. Padovan drop: 39060a708f8fSGustavo F. Padovan kfree_skb(skb); 39070a708f8fSGustavo F. Padovan 39080a708f8fSGustavo F. Padovan done: 39090a708f8fSGustavo F. Padovan if (sk) 39100a708f8fSGustavo F. Padovan bh_unlock_sock(sk); 39110a708f8fSGustavo F. Padovan 39120a708f8fSGustavo F. Padovan return 0; 39130a708f8fSGustavo F. Padovan } 39140a708f8fSGustavo F. Padovan 39150a708f8fSGustavo F. Padovan static inline int l2cap_conless_channel(struct l2cap_conn *conn, __le16 psm, struct sk_buff *skb) 39160a708f8fSGustavo F. Padovan { 39176dcae1eaSDavid S. Miller struct sock *sk = NULL; 391823691d75SGustavo F. Padovan struct l2cap_chan *chan; 39190a708f8fSGustavo F. Padovan 392023691d75SGustavo F. Padovan chan = l2cap_global_chan_by_psm(0, psm, conn->src); 392123691d75SGustavo F. Padovan if (!chan) 39220a708f8fSGustavo F. Padovan goto drop; 39230a708f8fSGustavo F. Padovan 392423691d75SGustavo F. Padovan sk = chan->sk; 392523691d75SGustavo F. Padovan 39260a708f8fSGustavo F. Padovan bh_lock_sock(sk); 39270a708f8fSGustavo F. Padovan 39280a708f8fSGustavo F. Padovan BT_DBG("sk %p, len %d", sk, skb->len); 39290a708f8fSGustavo F. Padovan 39300a708f8fSGustavo F. Padovan if (sk->sk_state != BT_BOUND && sk->sk_state != BT_CONNECTED) 39310a708f8fSGustavo F. Padovan goto drop; 39320a708f8fSGustavo F. Padovan 39330c1bc5c6SGustavo F. Padovan if (l2cap_pi(sk)->chan->imtu < skb->len) 39340a708f8fSGustavo F. Padovan goto drop; 39350a708f8fSGustavo F. Padovan 39360a708f8fSGustavo F. Padovan if (!sock_queue_rcv_skb(sk, skb)) 39370a708f8fSGustavo F. Padovan goto done; 39380a708f8fSGustavo F. Padovan 39390a708f8fSGustavo F. Padovan drop: 39400a708f8fSGustavo F. Padovan kfree_skb(skb); 39410a708f8fSGustavo F. Padovan 39420a708f8fSGustavo F. Padovan done: 39430a708f8fSGustavo F. Padovan if (sk) 39440a708f8fSGustavo F. Padovan bh_unlock_sock(sk); 39450a708f8fSGustavo F. Padovan return 0; 39460a708f8fSGustavo F. Padovan } 39470a708f8fSGustavo F. Padovan 39489f69bda6SGustavo F. Padovan static inline int l2cap_att_channel(struct l2cap_conn *conn, __le16 cid, struct sk_buff *skb) 39499f69bda6SGustavo F. Padovan { 39506dcae1eaSDavid S. Miller struct sock *sk = NULL; 395123691d75SGustavo F. Padovan struct l2cap_chan *chan; 39529f69bda6SGustavo F. Padovan 395323691d75SGustavo F. Padovan chan = l2cap_global_chan_by_scid(0, cid, conn->src); 395423691d75SGustavo F. Padovan if (!chan) 39559f69bda6SGustavo F. Padovan goto drop; 39569f69bda6SGustavo F. Padovan 395723691d75SGustavo F. Padovan sk = chan->sk; 395823691d75SGustavo F. Padovan 39599f69bda6SGustavo F. Padovan bh_lock_sock(sk); 39609f69bda6SGustavo F. Padovan 39619f69bda6SGustavo F. Padovan BT_DBG("sk %p, len %d", sk, skb->len); 39629f69bda6SGustavo F. Padovan 39639f69bda6SGustavo F. Padovan if (sk->sk_state != BT_BOUND && sk->sk_state != BT_CONNECTED) 39649f69bda6SGustavo F. Padovan goto drop; 39659f69bda6SGustavo F. Padovan 39660c1bc5c6SGustavo F. Padovan if (l2cap_pi(sk)->chan->imtu < skb->len) 39679f69bda6SGustavo F. Padovan goto drop; 39689f69bda6SGustavo F. Padovan 39699f69bda6SGustavo F. Padovan if (!sock_queue_rcv_skb(sk, skb)) 39709f69bda6SGustavo F. Padovan goto done; 39719f69bda6SGustavo F. Padovan 39729f69bda6SGustavo F. Padovan drop: 39739f69bda6SGustavo F. Padovan kfree_skb(skb); 39749f69bda6SGustavo F. Padovan 39759f69bda6SGustavo F. Padovan done: 39769f69bda6SGustavo F. Padovan if (sk) 39779f69bda6SGustavo F. Padovan bh_unlock_sock(sk); 39789f69bda6SGustavo F. Padovan return 0; 39799f69bda6SGustavo F. Padovan } 39809f69bda6SGustavo F. Padovan 39810a708f8fSGustavo F. Padovan static void l2cap_recv_frame(struct l2cap_conn *conn, struct sk_buff *skb) 39820a708f8fSGustavo F. Padovan { 39830a708f8fSGustavo F. Padovan struct l2cap_hdr *lh = (void *) skb->data; 39840a708f8fSGustavo F. Padovan u16 cid, len; 39850a708f8fSGustavo F. Padovan __le16 psm; 39860a708f8fSGustavo F. Padovan 39870a708f8fSGustavo F. Padovan skb_pull(skb, L2CAP_HDR_SIZE); 39880a708f8fSGustavo F. Padovan cid = __le16_to_cpu(lh->cid); 39890a708f8fSGustavo F. Padovan len = __le16_to_cpu(lh->len); 39900a708f8fSGustavo F. Padovan 39910a708f8fSGustavo F. Padovan if (len != skb->len) { 39920a708f8fSGustavo F. Padovan kfree_skb(skb); 39930a708f8fSGustavo F. Padovan return; 39940a708f8fSGustavo F. Padovan } 39950a708f8fSGustavo F. Padovan 39960a708f8fSGustavo F. Padovan BT_DBG("len %d, cid 0x%4.4x", len, cid); 39970a708f8fSGustavo F. Padovan 39980a708f8fSGustavo F. Padovan switch (cid) { 39993300d9a9SClaudio Takahasi case L2CAP_CID_LE_SIGNALING: 40000a708f8fSGustavo F. Padovan case L2CAP_CID_SIGNALING: 40010a708f8fSGustavo F. Padovan l2cap_sig_channel(conn, skb); 40020a708f8fSGustavo F. Padovan break; 40030a708f8fSGustavo F. Padovan 40040a708f8fSGustavo F. Padovan case L2CAP_CID_CONN_LESS: 40050a708f8fSGustavo F. Padovan psm = get_unaligned_le16(skb->data); 40060a708f8fSGustavo F. Padovan skb_pull(skb, 2); 40070a708f8fSGustavo F. Padovan l2cap_conless_channel(conn, psm, skb); 40080a708f8fSGustavo F. Padovan break; 40090a708f8fSGustavo F. Padovan 40109f69bda6SGustavo F. Padovan case L2CAP_CID_LE_DATA: 40119f69bda6SGustavo F. Padovan l2cap_att_channel(conn, cid, skb); 40129f69bda6SGustavo F. Padovan break; 40139f69bda6SGustavo F. Padovan 40140a708f8fSGustavo F. Padovan default: 40150a708f8fSGustavo F. Padovan l2cap_data_channel(conn, cid, skb); 40160a708f8fSGustavo F. Padovan break; 40170a708f8fSGustavo F. Padovan } 40180a708f8fSGustavo F. Padovan } 40190a708f8fSGustavo F. Padovan 40200a708f8fSGustavo F. Padovan /* ---- L2CAP interface with lower layer (HCI) ---- */ 40210a708f8fSGustavo F. Padovan 40220a708f8fSGustavo F. Padovan static int l2cap_connect_ind(struct hci_dev *hdev, bdaddr_t *bdaddr, u8 type) 40230a708f8fSGustavo F. Padovan { 40240a708f8fSGustavo F. Padovan int exact = 0, lm1 = 0, lm2 = 0; 402523691d75SGustavo F. Padovan struct l2cap_chan *c; 40260a708f8fSGustavo F. Padovan 40270a708f8fSGustavo F. Padovan if (type != ACL_LINK) 40280a708f8fSGustavo F. Padovan return -EINVAL; 40290a708f8fSGustavo F. Padovan 40300a708f8fSGustavo F. Padovan BT_DBG("hdev %s, bdaddr %s", hdev->name, batostr(bdaddr)); 40310a708f8fSGustavo F. Padovan 40320a708f8fSGustavo F. Padovan /* Find listening sockets and check their link_mode */ 403323691d75SGustavo F. Padovan read_lock(&chan_list_lock); 403423691d75SGustavo F. Padovan list_for_each_entry(c, &chan_list, global_l) { 403523691d75SGustavo F. Padovan struct sock *sk = c->sk; 40364343478fSGustavo F. Padovan 40370a708f8fSGustavo F. Padovan if (sk->sk_state != BT_LISTEN) 40380a708f8fSGustavo F. Padovan continue; 40390a708f8fSGustavo F. Padovan 40400a708f8fSGustavo F. Padovan if (!bacmp(&bt_sk(sk)->src, &hdev->bdaddr)) { 40410a708f8fSGustavo F. Padovan lm1 |= HCI_LM_ACCEPT; 404223691d75SGustavo F. Padovan if (c->role_switch) 40430a708f8fSGustavo F. Padovan lm1 |= HCI_LM_MASTER; 40440a708f8fSGustavo F. Padovan exact++; 40450a708f8fSGustavo F. Padovan } else if (!bacmp(&bt_sk(sk)->src, BDADDR_ANY)) { 40460a708f8fSGustavo F. Padovan lm2 |= HCI_LM_ACCEPT; 404723691d75SGustavo F. Padovan if (c->role_switch) 40480a708f8fSGustavo F. Padovan lm2 |= HCI_LM_MASTER; 40490a708f8fSGustavo F. Padovan } 40500a708f8fSGustavo F. Padovan } 405123691d75SGustavo F. Padovan read_unlock(&chan_list_lock); 40520a708f8fSGustavo F. Padovan 40530a708f8fSGustavo F. Padovan return exact ? lm1 : lm2; 40540a708f8fSGustavo F. Padovan } 40550a708f8fSGustavo F. Padovan 40560a708f8fSGustavo F. Padovan static int l2cap_connect_cfm(struct hci_conn *hcon, u8 status) 40570a708f8fSGustavo F. Padovan { 40580a708f8fSGustavo F. Padovan struct l2cap_conn *conn; 40590a708f8fSGustavo F. Padovan 40600a708f8fSGustavo F. Padovan BT_DBG("hcon %p bdaddr %s status %d", hcon, batostr(&hcon->dst), status); 40610a708f8fSGustavo F. Padovan 4062acd7d370SVille Tervo if (!(hcon->type == ACL_LINK || hcon->type == LE_LINK)) 40630a708f8fSGustavo F. Padovan return -EINVAL; 40640a708f8fSGustavo F. Padovan 40650a708f8fSGustavo F. Padovan if (!status) { 40660a708f8fSGustavo F. Padovan conn = l2cap_conn_add(hcon, status); 40670a708f8fSGustavo F. Padovan if (conn) 40680a708f8fSGustavo F. Padovan l2cap_conn_ready(conn); 40690a708f8fSGustavo F. Padovan } else 40700a708f8fSGustavo F. Padovan l2cap_conn_del(hcon, bt_err(status)); 40710a708f8fSGustavo F. Padovan 40720a708f8fSGustavo F. Padovan return 0; 40730a708f8fSGustavo F. Padovan } 40740a708f8fSGustavo F. Padovan 40750a708f8fSGustavo F. Padovan static int l2cap_disconn_ind(struct hci_conn *hcon) 40760a708f8fSGustavo F. Padovan { 40770a708f8fSGustavo F. Padovan struct l2cap_conn *conn = hcon->l2cap_data; 40780a708f8fSGustavo F. Padovan 40790a708f8fSGustavo F. Padovan BT_DBG("hcon %p", hcon); 40800a708f8fSGustavo F. Padovan 40810a708f8fSGustavo F. Padovan if (hcon->type != ACL_LINK || !conn) 40820a708f8fSGustavo F. Padovan return 0x13; 40830a708f8fSGustavo F. Padovan 40840a708f8fSGustavo F. Padovan return conn->disc_reason; 40850a708f8fSGustavo F. Padovan } 40860a708f8fSGustavo F. Padovan 40870a708f8fSGustavo F. Padovan static int l2cap_disconn_cfm(struct hci_conn *hcon, u8 reason) 40880a708f8fSGustavo F. Padovan { 40890a708f8fSGustavo F. Padovan BT_DBG("hcon %p reason %d", hcon, reason); 40900a708f8fSGustavo F. Padovan 4091acd7d370SVille Tervo if (!(hcon->type == ACL_LINK || hcon->type == LE_LINK)) 40920a708f8fSGustavo F. Padovan return -EINVAL; 40930a708f8fSGustavo F. Padovan 40940a708f8fSGustavo F. Padovan l2cap_conn_del(hcon, bt_err(reason)); 40950a708f8fSGustavo F. Padovan 40960a708f8fSGustavo F. Padovan return 0; 40970a708f8fSGustavo F. Padovan } 40980a708f8fSGustavo F. Padovan 40994343478fSGustavo F. Padovan static inline void l2cap_check_encryption(struct l2cap_chan *chan, u8 encrypt) 41000a708f8fSGustavo F. Padovan { 41014343478fSGustavo F. Padovan struct sock *sk = chan->sk; 41024343478fSGustavo F. Padovan 41030a708f8fSGustavo F. Padovan if (sk->sk_type != SOCK_SEQPACKET && sk->sk_type != SOCK_STREAM) 41040a708f8fSGustavo F. Padovan return; 41050a708f8fSGustavo F. Padovan 41060a708f8fSGustavo F. Padovan if (encrypt == 0x00) { 41074343478fSGustavo F. Padovan if (chan->sec_level == BT_SECURITY_MEDIUM) { 41080a708f8fSGustavo F. Padovan l2cap_sock_clear_timer(sk); 41090a708f8fSGustavo F. Padovan l2cap_sock_set_timer(sk, HZ * 5); 41104343478fSGustavo F. Padovan } else if (chan->sec_level == BT_SECURITY_HIGH) 41114519de9aSGustavo F. Padovan __l2cap_chan_close(chan, ECONNREFUSED); 41120a708f8fSGustavo F. Padovan } else { 41134343478fSGustavo F. Padovan if (chan->sec_level == BT_SECURITY_MEDIUM) 41140a708f8fSGustavo F. Padovan l2cap_sock_clear_timer(sk); 41150a708f8fSGustavo F. Padovan } 41160a708f8fSGustavo F. Padovan } 41170a708f8fSGustavo F. Padovan 41180a708f8fSGustavo F. Padovan static int l2cap_security_cfm(struct hci_conn *hcon, u8 status, u8 encrypt) 41190a708f8fSGustavo F. Padovan { 41200a708f8fSGustavo F. Padovan struct l2cap_conn *conn = hcon->l2cap_data; 412148454079SGustavo F. Padovan struct l2cap_chan *chan; 41220a708f8fSGustavo F. Padovan 41230a708f8fSGustavo F. Padovan if (!conn) 41240a708f8fSGustavo F. Padovan return 0; 41250a708f8fSGustavo F. Padovan 41260a708f8fSGustavo F. Padovan BT_DBG("conn %p", conn); 41270a708f8fSGustavo F. Padovan 4128baa7e1faSGustavo F. Padovan read_lock(&conn->chan_lock); 41290a708f8fSGustavo F. Padovan 4130baa7e1faSGustavo F. Padovan list_for_each_entry(chan, &conn->chan_l, list) { 413148454079SGustavo F. Padovan struct sock *sk = chan->sk; 4132baa7e1faSGustavo F. Padovan 41330a708f8fSGustavo F. Padovan bh_lock_sock(sk); 41340a708f8fSGustavo F. Padovan 4135b4450035SGustavo F. Padovan if (chan->conf_state & L2CAP_CONF_CONNECT_PEND) { 41360a708f8fSGustavo F. Padovan bh_unlock_sock(sk); 41370a708f8fSGustavo F. Padovan continue; 41380a708f8fSGustavo F. Padovan } 41390a708f8fSGustavo F. Padovan 41400a708f8fSGustavo F. Padovan if (!status && (sk->sk_state == BT_CONNECTED || 41410a708f8fSGustavo F. Padovan sk->sk_state == BT_CONFIG)) { 41424343478fSGustavo F. Padovan l2cap_check_encryption(chan, encrypt); 41430a708f8fSGustavo F. Padovan bh_unlock_sock(sk); 41440a708f8fSGustavo F. Padovan continue; 41450a708f8fSGustavo F. Padovan } 41460a708f8fSGustavo F. Padovan 41470a708f8fSGustavo F. Padovan if (sk->sk_state == BT_CONNECT) { 41480a708f8fSGustavo F. Padovan if (!status) { 41490a708f8fSGustavo F. Padovan struct l2cap_conn_req req; 4150fe4128e0SGustavo F. Padovan req.scid = cpu_to_le16(chan->scid); 4151fe4128e0SGustavo F. Padovan req.psm = chan->psm; 41520a708f8fSGustavo F. Padovan 4153fc7f8a7eSGustavo F. Padovan chan->ident = l2cap_get_ident(conn); 4154b4450035SGustavo F. Padovan chan->conf_state |= L2CAP_CONF_CONNECT_PEND; 41550a708f8fSGustavo F. Padovan 4156fc7f8a7eSGustavo F. Padovan l2cap_send_cmd(conn, chan->ident, 41570a708f8fSGustavo F. Padovan L2CAP_CONN_REQ, sizeof(req), &req); 41580a708f8fSGustavo F. Padovan } else { 41590a708f8fSGustavo F. Padovan l2cap_sock_clear_timer(sk); 41600a708f8fSGustavo F. Padovan l2cap_sock_set_timer(sk, HZ / 10); 41610a708f8fSGustavo F. Padovan } 41620a708f8fSGustavo F. Padovan } else if (sk->sk_state == BT_CONNECT2) { 41630a708f8fSGustavo F. Padovan struct l2cap_conn_rsp rsp; 41640a708f8fSGustavo F. Padovan __u16 result; 41650a708f8fSGustavo F. Padovan 41660a708f8fSGustavo F. Padovan if (!status) { 41670a708f8fSGustavo F. Padovan sk->sk_state = BT_CONFIG; 41680a708f8fSGustavo F. Padovan result = L2CAP_CR_SUCCESS; 41690a708f8fSGustavo F. Padovan } else { 41700a708f8fSGustavo F. Padovan sk->sk_state = BT_DISCONN; 41710a708f8fSGustavo F. Padovan l2cap_sock_set_timer(sk, HZ / 10); 41720a708f8fSGustavo F. Padovan result = L2CAP_CR_SEC_BLOCK; 41730a708f8fSGustavo F. Padovan } 41740a708f8fSGustavo F. Padovan 4175fe4128e0SGustavo F. Padovan rsp.scid = cpu_to_le16(chan->dcid); 4176fe4128e0SGustavo F. Padovan rsp.dcid = cpu_to_le16(chan->scid); 41770a708f8fSGustavo F. Padovan rsp.result = cpu_to_le16(result); 41780a708f8fSGustavo F. Padovan rsp.status = cpu_to_le16(L2CAP_CS_NO_INFO); 4179fc7f8a7eSGustavo F. Padovan l2cap_send_cmd(conn, chan->ident, L2CAP_CONN_RSP, 4180fc7f8a7eSGustavo F. Padovan sizeof(rsp), &rsp); 41810a708f8fSGustavo F. Padovan } 41820a708f8fSGustavo F. Padovan 41830a708f8fSGustavo F. Padovan bh_unlock_sock(sk); 41840a708f8fSGustavo F. Padovan } 41850a708f8fSGustavo F. Padovan 4186baa7e1faSGustavo F. Padovan read_unlock(&conn->chan_lock); 41870a708f8fSGustavo F. Padovan 41880a708f8fSGustavo F. Padovan return 0; 41890a708f8fSGustavo F. Padovan } 41900a708f8fSGustavo F. Padovan 41910a708f8fSGustavo F. Padovan static int l2cap_recv_acldata(struct hci_conn *hcon, struct sk_buff *skb, u16 flags) 41920a708f8fSGustavo F. Padovan { 41930a708f8fSGustavo F. Padovan struct l2cap_conn *conn = hcon->l2cap_data; 41940a708f8fSGustavo F. Padovan 41950a708f8fSGustavo F. Padovan if (!conn) 41960a708f8fSGustavo F. Padovan conn = l2cap_conn_add(hcon, 0); 41970a708f8fSGustavo F. Padovan 41980a708f8fSGustavo F. Padovan if (!conn) 41990a708f8fSGustavo F. Padovan goto drop; 42000a708f8fSGustavo F. Padovan 42010a708f8fSGustavo F. Padovan BT_DBG("conn %p len %d flags 0x%x", conn, skb->len, flags); 42020a708f8fSGustavo F. Padovan 42030a708f8fSGustavo F. Padovan if (!(flags & ACL_CONT)) { 42040a708f8fSGustavo F. Padovan struct l2cap_hdr *hdr; 420548454079SGustavo F. Padovan struct l2cap_chan *chan; 42060a708f8fSGustavo F. Padovan u16 cid; 42070a708f8fSGustavo F. Padovan int len; 42080a708f8fSGustavo F. Padovan 42090a708f8fSGustavo F. Padovan if (conn->rx_len) { 42100a708f8fSGustavo F. Padovan BT_ERR("Unexpected start frame (len %d)", skb->len); 42110a708f8fSGustavo F. Padovan kfree_skb(conn->rx_skb); 42120a708f8fSGustavo F. Padovan conn->rx_skb = NULL; 42130a708f8fSGustavo F. Padovan conn->rx_len = 0; 42140a708f8fSGustavo F. Padovan l2cap_conn_unreliable(conn, ECOMM); 42150a708f8fSGustavo F. Padovan } 42160a708f8fSGustavo F. Padovan 42170a708f8fSGustavo F. Padovan /* Start fragment always begin with Basic L2CAP header */ 42180a708f8fSGustavo F. Padovan if (skb->len < L2CAP_HDR_SIZE) { 42190a708f8fSGustavo F. Padovan BT_ERR("Frame is too short (len %d)", skb->len); 42200a708f8fSGustavo F. Padovan l2cap_conn_unreliable(conn, ECOMM); 42210a708f8fSGustavo F. Padovan goto drop; 42220a708f8fSGustavo F. Padovan } 42230a708f8fSGustavo F. Padovan 42240a708f8fSGustavo F. Padovan hdr = (struct l2cap_hdr *) skb->data; 42250a708f8fSGustavo F. Padovan len = __le16_to_cpu(hdr->len) + L2CAP_HDR_SIZE; 42260a708f8fSGustavo F. Padovan cid = __le16_to_cpu(hdr->cid); 42270a708f8fSGustavo F. Padovan 42280a708f8fSGustavo F. Padovan if (len == skb->len) { 42290a708f8fSGustavo F. Padovan /* Complete frame received */ 42300a708f8fSGustavo F. Padovan l2cap_recv_frame(conn, skb); 42310a708f8fSGustavo F. Padovan return 0; 42320a708f8fSGustavo F. Padovan } 42330a708f8fSGustavo F. Padovan 42340a708f8fSGustavo F. Padovan BT_DBG("Start: total len %d, frag len %d", len, skb->len); 42350a708f8fSGustavo F. Padovan 42360a708f8fSGustavo F. Padovan if (skb->len > len) { 42370a708f8fSGustavo F. Padovan BT_ERR("Frame is too long (len %d, expected len %d)", 42380a708f8fSGustavo F. Padovan skb->len, len); 42390a708f8fSGustavo F. Padovan l2cap_conn_unreliable(conn, ECOMM); 42400a708f8fSGustavo F. Padovan goto drop; 42410a708f8fSGustavo F. Padovan } 42420a708f8fSGustavo F. Padovan 4243baa7e1faSGustavo F. Padovan chan = l2cap_get_chan_by_scid(conn, cid); 42440a708f8fSGustavo F. Padovan 424548454079SGustavo F. Padovan if (chan && chan->sk) { 424648454079SGustavo F. Padovan struct sock *sk = chan->sk; 424748454079SGustavo F. Padovan 42480c1bc5c6SGustavo F. Padovan if (chan->imtu < len - L2CAP_HDR_SIZE) { 424948454079SGustavo F. Padovan BT_ERR("Frame exceeding recv MTU (len %d, " 425048454079SGustavo F. Padovan "MTU %d)", len, 42510c1bc5c6SGustavo F. Padovan chan->imtu); 42520a708f8fSGustavo F. Padovan bh_unlock_sock(sk); 42530a708f8fSGustavo F. Padovan l2cap_conn_unreliable(conn, ECOMM); 42540a708f8fSGustavo F. Padovan goto drop; 42550a708f8fSGustavo F. Padovan } 42560a708f8fSGustavo F. Padovan bh_unlock_sock(sk); 425748454079SGustavo F. Padovan } 42580a708f8fSGustavo F. Padovan 42590a708f8fSGustavo F. Padovan /* Allocate skb for the complete frame (with header) */ 42600a708f8fSGustavo F. Padovan conn->rx_skb = bt_skb_alloc(len, GFP_ATOMIC); 42610a708f8fSGustavo F. Padovan if (!conn->rx_skb) 42620a708f8fSGustavo F. Padovan goto drop; 42630a708f8fSGustavo F. Padovan 42640a708f8fSGustavo F. Padovan skb_copy_from_linear_data(skb, skb_put(conn->rx_skb, skb->len), 42650a708f8fSGustavo F. Padovan skb->len); 42660a708f8fSGustavo F. Padovan conn->rx_len = len - skb->len; 42670a708f8fSGustavo F. Padovan } else { 42680a708f8fSGustavo F. Padovan BT_DBG("Cont: frag len %d (expecting %d)", skb->len, conn->rx_len); 42690a708f8fSGustavo F. Padovan 42700a708f8fSGustavo F. Padovan if (!conn->rx_len) { 42710a708f8fSGustavo F. Padovan BT_ERR("Unexpected continuation frame (len %d)", skb->len); 42720a708f8fSGustavo F. Padovan l2cap_conn_unreliable(conn, ECOMM); 42730a708f8fSGustavo F. Padovan goto drop; 42740a708f8fSGustavo F. Padovan } 42750a708f8fSGustavo F. Padovan 42760a708f8fSGustavo F. Padovan if (skb->len > conn->rx_len) { 42770a708f8fSGustavo F. Padovan BT_ERR("Fragment is too long (len %d, expected %d)", 42780a708f8fSGustavo F. Padovan skb->len, conn->rx_len); 42790a708f8fSGustavo F. Padovan kfree_skb(conn->rx_skb); 42800a708f8fSGustavo F. Padovan conn->rx_skb = NULL; 42810a708f8fSGustavo F. Padovan conn->rx_len = 0; 42820a708f8fSGustavo F. Padovan l2cap_conn_unreliable(conn, ECOMM); 42830a708f8fSGustavo F. Padovan goto drop; 42840a708f8fSGustavo F. Padovan } 42850a708f8fSGustavo F. Padovan 42860a708f8fSGustavo F. Padovan skb_copy_from_linear_data(skb, skb_put(conn->rx_skb, skb->len), 42870a708f8fSGustavo F. Padovan skb->len); 42880a708f8fSGustavo F. Padovan conn->rx_len -= skb->len; 42890a708f8fSGustavo F. Padovan 42900a708f8fSGustavo F. Padovan if (!conn->rx_len) { 42910a708f8fSGustavo F. Padovan /* Complete frame received */ 42920a708f8fSGustavo F. Padovan l2cap_recv_frame(conn, conn->rx_skb); 42930a708f8fSGustavo F. Padovan conn->rx_skb = NULL; 42940a708f8fSGustavo F. Padovan } 42950a708f8fSGustavo F. Padovan } 42960a708f8fSGustavo F. Padovan 42970a708f8fSGustavo F. Padovan drop: 42980a708f8fSGustavo F. Padovan kfree_skb(skb); 42990a708f8fSGustavo F. Padovan return 0; 43000a708f8fSGustavo F. Padovan } 43010a708f8fSGustavo F. Padovan 43020a708f8fSGustavo F. Padovan static int l2cap_debugfs_show(struct seq_file *f, void *p) 43030a708f8fSGustavo F. Padovan { 430423691d75SGustavo F. Padovan struct l2cap_chan *c; 43050a708f8fSGustavo F. Padovan 430623691d75SGustavo F. Padovan read_lock_bh(&chan_list_lock); 43070a708f8fSGustavo F. Padovan 430823691d75SGustavo F. Padovan list_for_each_entry(c, &chan_list, global_l) { 430923691d75SGustavo F. Padovan struct sock *sk = c->sk; 43100a708f8fSGustavo F. Padovan 4311903d343eSGustavo F. Padovan seq_printf(f, "%s %s %d %d 0x%4.4x 0x%4.4x %d %d %d %d\n", 43120a708f8fSGustavo F. Padovan batostr(&bt_sk(sk)->src), 43130a708f8fSGustavo F. Padovan batostr(&bt_sk(sk)->dst), 431423691d75SGustavo F. Padovan sk->sk_state, __le16_to_cpu(c->psm), 431523691d75SGustavo F. Padovan c->scid, c->dcid, c->imtu, c->omtu, 431623691d75SGustavo F. Padovan c->sec_level, c->mode); 43170a708f8fSGustavo F. Padovan } 43180a708f8fSGustavo F. Padovan 431923691d75SGustavo F. Padovan read_unlock_bh(&chan_list_lock); 43200a708f8fSGustavo F. Padovan 43210a708f8fSGustavo F. Padovan return 0; 43220a708f8fSGustavo F. Padovan } 43230a708f8fSGustavo F. Padovan 43240a708f8fSGustavo F. Padovan static int l2cap_debugfs_open(struct inode *inode, struct file *file) 43250a708f8fSGustavo F. Padovan { 43260a708f8fSGustavo F. Padovan return single_open(file, l2cap_debugfs_show, inode->i_private); 43270a708f8fSGustavo F. Padovan } 43280a708f8fSGustavo F. Padovan 43290a708f8fSGustavo F. Padovan static const struct file_operations l2cap_debugfs_fops = { 43300a708f8fSGustavo F. Padovan .open = l2cap_debugfs_open, 43310a708f8fSGustavo F. Padovan .read = seq_read, 43320a708f8fSGustavo F. Padovan .llseek = seq_lseek, 43330a708f8fSGustavo F. Padovan .release = single_release, 43340a708f8fSGustavo F. Padovan }; 43350a708f8fSGustavo F. Padovan 43360a708f8fSGustavo F. Padovan static struct dentry *l2cap_debugfs; 43370a708f8fSGustavo F. Padovan 43380a708f8fSGustavo F. Padovan static struct hci_proto l2cap_hci_proto = { 43390a708f8fSGustavo F. Padovan .name = "L2CAP", 43400a708f8fSGustavo F. Padovan .id = HCI_PROTO_L2CAP, 43410a708f8fSGustavo F. Padovan .connect_ind = l2cap_connect_ind, 43420a708f8fSGustavo F. Padovan .connect_cfm = l2cap_connect_cfm, 43430a708f8fSGustavo F. Padovan .disconn_ind = l2cap_disconn_ind, 43440a708f8fSGustavo F. Padovan .disconn_cfm = l2cap_disconn_cfm, 43450a708f8fSGustavo F. Padovan .security_cfm = l2cap_security_cfm, 43460a708f8fSGustavo F. Padovan .recv_acldata = l2cap_recv_acldata 43470a708f8fSGustavo F. Padovan }; 43480a708f8fSGustavo F. Padovan 434964274518SGustavo F. Padovan int __init l2cap_init(void) 43500a708f8fSGustavo F. Padovan { 43510a708f8fSGustavo F. Padovan int err; 43520a708f8fSGustavo F. Padovan 4353bb58f747SGustavo F. Padovan err = l2cap_init_sockets(); 43540a708f8fSGustavo F. Padovan if (err < 0) 43550a708f8fSGustavo F. Padovan return err; 43560a708f8fSGustavo F. Padovan 43570a708f8fSGustavo F. Padovan _busy_wq = create_singlethread_workqueue("l2cap"); 43580a708f8fSGustavo F. Padovan if (!_busy_wq) { 4359bb58f747SGustavo F. Padovan err = -ENOMEM; 43600a708f8fSGustavo F. Padovan goto error; 43610a708f8fSGustavo F. Padovan } 43620a708f8fSGustavo F. Padovan 43630a708f8fSGustavo F. Padovan err = hci_register_proto(&l2cap_hci_proto); 43640a708f8fSGustavo F. Padovan if (err < 0) { 43650a708f8fSGustavo F. Padovan BT_ERR("L2CAP protocol registration failed"); 43660a708f8fSGustavo F. Padovan bt_sock_unregister(BTPROTO_L2CAP); 43670a708f8fSGustavo F. Padovan goto error; 43680a708f8fSGustavo F. Padovan } 43690a708f8fSGustavo F. Padovan 43700a708f8fSGustavo F. Padovan if (bt_debugfs) { 43710a708f8fSGustavo F. Padovan l2cap_debugfs = debugfs_create_file("l2cap", 0444, 43720a708f8fSGustavo F. Padovan bt_debugfs, NULL, &l2cap_debugfs_fops); 43730a708f8fSGustavo F. Padovan if (!l2cap_debugfs) 43740a708f8fSGustavo F. Padovan BT_ERR("Failed to create L2CAP debug file"); 43750a708f8fSGustavo F. Padovan } 43760a708f8fSGustavo F. Padovan 43770a708f8fSGustavo F. Padovan return 0; 43780a708f8fSGustavo F. Padovan 43790a708f8fSGustavo F. Padovan error: 43800a708f8fSGustavo F. Padovan destroy_workqueue(_busy_wq); 4381bb58f747SGustavo F. Padovan l2cap_cleanup_sockets(); 43820a708f8fSGustavo F. Padovan return err; 43830a708f8fSGustavo F. Padovan } 43840a708f8fSGustavo F. Padovan 438564274518SGustavo F. Padovan void l2cap_exit(void) 43860a708f8fSGustavo F. Padovan { 43870a708f8fSGustavo F. Padovan debugfs_remove(l2cap_debugfs); 43880a708f8fSGustavo F. Padovan 43890a708f8fSGustavo F. Padovan flush_workqueue(_busy_wq); 43900a708f8fSGustavo F. Padovan destroy_workqueue(_busy_wq); 43910a708f8fSGustavo F. Padovan 43920a708f8fSGustavo F. Padovan if (hci_unregister_proto(&l2cap_hci_proto) < 0) 43930a708f8fSGustavo F. Padovan BT_ERR("L2CAP protocol unregistration failed"); 43940a708f8fSGustavo F. Padovan 4395bb58f747SGustavo F. Padovan l2cap_cleanup_sockets(); 43960a708f8fSGustavo F. Padovan } 43970a708f8fSGustavo F. Padovan 43980a708f8fSGustavo F. Padovan module_param(disable_ertm, bool, 0644); 43990a708f8fSGustavo F. Padovan MODULE_PARM_DESC(disable_ertm, "Disable enhanced retransmission mode"); 4400