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 65bb58f747SGustavo F. Padovan struct bt_sock_list l2cap_sk_list = { 660a708f8fSGustavo F. Padovan .lock = __RW_LOCK_UNLOCKED(l2cap_sk_list.lock) 670a708f8fSGustavo F. Padovan }; 680a708f8fSGustavo F. Padovan 690a708f8fSGustavo F. Padovan static void l2cap_busy_work(struct work_struct *work); 700a708f8fSGustavo F. Padovan 710a708f8fSGustavo F. Padovan static struct sk_buff *l2cap_build_cmd(struct l2cap_conn *conn, 720a708f8fSGustavo F. Padovan u8 code, u8 ident, u16 dlen, void *data); 73710f9b0aSGustavo F. Padovan static int l2cap_build_conf_req(struct l2cap_chan *chan, void *data); 740a708f8fSGustavo F. Padovan 750a708f8fSGustavo F. Padovan static int l2cap_ertm_data_rcv(struct sock *sk, struct sk_buff *skb); 760a708f8fSGustavo F. Padovan 770a708f8fSGustavo F. Padovan /* ---- L2CAP channels ---- */ 78baa7e1faSGustavo F. Padovan static struct l2cap_chan *__l2cap_get_chan_by_dcid(struct l2cap_conn *conn, u16 cid) 790a708f8fSGustavo F. Padovan { 8048454079SGustavo F. Padovan struct l2cap_chan *c; 81baa7e1faSGustavo F. Padovan 82baa7e1faSGustavo F. Padovan list_for_each_entry(c, &conn->chan_l, list) { 83baa7e1faSGustavo F. Padovan struct sock *s = c->sk; 84baa7e1faSGustavo F. Padovan if (l2cap_pi(s)->dcid == cid) 8548454079SGustavo F. Padovan return c; 860a708f8fSGustavo F. Padovan } 87baa7e1faSGustavo F. Padovan return NULL; 88baa7e1faSGustavo F. Padovan 89baa7e1faSGustavo F. Padovan } 900a708f8fSGustavo F. Padovan 91baa7e1faSGustavo F. Padovan static struct l2cap_chan *__l2cap_get_chan_by_scid(struct l2cap_conn *conn, u16 cid) 920a708f8fSGustavo F. Padovan { 9348454079SGustavo F. Padovan struct l2cap_chan *c; 94baa7e1faSGustavo F. Padovan 95baa7e1faSGustavo F. Padovan list_for_each_entry(c, &conn->chan_l, list) { 96baa7e1faSGustavo F. Padovan struct sock *s = c->sk; 97baa7e1faSGustavo F. Padovan if (l2cap_pi(s)->scid == cid) 9848454079SGustavo F. Padovan return c; 990a708f8fSGustavo F. Padovan } 100baa7e1faSGustavo F. Padovan return NULL; 101baa7e1faSGustavo F. Padovan } 1020a708f8fSGustavo F. Padovan 1030a708f8fSGustavo F. Padovan /* Find channel with given SCID. 1040a708f8fSGustavo F. Padovan * Returns locked socket */ 105baa7e1faSGustavo F. Padovan static struct l2cap_chan *l2cap_get_chan_by_scid(struct l2cap_conn *conn, u16 cid) 1060a708f8fSGustavo F. Padovan { 10748454079SGustavo F. Padovan struct l2cap_chan *c; 108baa7e1faSGustavo F. Padovan 109baa7e1faSGustavo F. Padovan read_lock(&conn->chan_lock); 110baa7e1faSGustavo F. Padovan c = __l2cap_get_chan_by_scid(conn, cid); 11148454079SGustavo F. Padovan if (c) 11248454079SGustavo F. Padovan bh_lock_sock(c->sk); 113baa7e1faSGustavo F. Padovan read_unlock(&conn->chan_lock); 11448454079SGustavo F. Padovan return c; 1150a708f8fSGustavo F. Padovan } 1160a708f8fSGustavo F. Padovan 117baa7e1faSGustavo F. Padovan static struct l2cap_chan *__l2cap_get_chan_by_ident(struct l2cap_conn *conn, u8 ident) 1180a708f8fSGustavo F. Padovan { 11948454079SGustavo F. Padovan struct l2cap_chan *c; 120baa7e1faSGustavo F. Padovan 121baa7e1faSGustavo F. Padovan list_for_each_entry(c, &conn->chan_l, list) { 122fc7f8a7eSGustavo F. Padovan if (c->ident == ident) 12348454079SGustavo F. Padovan return c; 1240a708f8fSGustavo F. Padovan } 125baa7e1faSGustavo F. Padovan return NULL; 126baa7e1faSGustavo F. Padovan } 1270a708f8fSGustavo F. Padovan 128baa7e1faSGustavo F. Padovan static inline struct l2cap_chan *l2cap_get_chan_by_ident(struct l2cap_conn *conn, u8 ident) 1290a708f8fSGustavo F. Padovan { 13048454079SGustavo F. Padovan struct l2cap_chan *c; 131baa7e1faSGustavo F. Padovan 132baa7e1faSGustavo F. Padovan read_lock(&conn->chan_lock); 133baa7e1faSGustavo F. Padovan c = __l2cap_get_chan_by_ident(conn, ident); 13448454079SGustavo F. Padovan if (c) 13548454079SGustavo F. Padovan bh_lock_sock(c->sk); 136baa7e1faSGustavo F. Padovan read_unlock(&conn->chan_lock); 13748454079SGustavo F. Padovan return c; 1380a708f8fSGustavo F. Padovan } 1390a708f8fSGustavo F. Padovan 140baa7e1faSGustavo F. Padovan static u16 l2cap_alloc_cid(struct l2cap_conn *conn) 1410a708f8fSGustavo F. Padovan { 1420a708f8fSGustavo F. Padovan u16 cid = L2CAP_CID_DYN_START; 1430a708f8fSGustavo F. Padovan 1440a708f8fSGustavo F. Padovan for (; cid < L2CAP_CID_DYN_END; cid++) { 145baa7e1faSGustavo F. Padovan if (!__l2cap_get_chan_by_scid(conn, cid)) 1460a708f8fSGustavo F. Padovan return cid; 1470a708f8fSGustavo F. Padovan } 1480a708f8fSGustavo F. Padovan 1490a708f8fSGustavo F. Padovan return 0; 1500a708f8fSGustavo F. Padovan } 1510a708f8fSGustavo F. Padovan 1525d41ce1dSGustavo F. Padovan struct l2cap_chan *l2cap_chan_alloc(struct sock *sk) 1530a708f8fSGustavo F. Padovan { 15448454079SGustavo F. Padovan struct l2cap_chan *chan; 1550a708f8fSGustavo F. Padovan 15648454079SGustavo F. Padovan chan = kzalloc(sizeof(*chan), GFP_ATOMIC); 15748454079SGustavo F. Padovan if (!chan) 15848454079SGustavo F. Padovan return NULL; 1590a708f8fSGustavo F. Padovan 16048454079SGustavo F. Padovan chan->sk = sk; 16148454079SGustavo F. Padovan 16248454079SGustavo F. Padovan return chan; 1630a708f8fSGustavo F. Padovan } 1640a708f8fSGustavo F. Padovan 16548454079SGustavo F. Padovan static void __l2cap_chan_add(struct l2cap_conn *conn, struct l2cap_chan *chan) 1660a708f8fSGustavo F. Padovan { 16748454079SGustavo F. Padovan struct sock *sk = chan->sk; 1680a708f8fSGustavo F. Padovan 1690a708f8fSGustavo F. Padovan BT_DBG("conn %p, psm 0x%2.2x, dcid 0x%4.4x", conn, 1700a708f8fSGustavo F. Padovan l2cap_pi(sk)->psm, l2cap_pi(sk)->dcid); 1710a708f8fSGustavo F. Padovan 1720a708f8fSGustavo F. Padovan conn->disc_reason = 0x13; 1730a708f8fSGustavo F. Padovan 1740a708f8fSGustavo F. Padovan l2cap_pi(sk)->conn = conn; 1750a708f8fSGustavo F. Padovan 1760a708f8fSGustavo F. Padovan if (sk->sk_type == SOCK_SEQPACKET || sk->sk_type == SOCK_STREAM) { 177b62f328bSVille Tervo if (conn->hcon->type == LE_LINK) { 178b62f328bSVille Tervo /* LE connection */ 179b62f328bSVille Tervo l2cap_pi(sk)->omtu = L2CAP_LE_DEFAULT_MTU; 180b62f328bSVille Tervo l2cap_pi(sk)->scid = L2CAP_CID_LE_DATA; 181b62f328bSVille Tervo l2cap_pi(sk)->dcid = L2CAP_CID_LE_DATA; 182b62f328bSVille Tervo } else { 1830a708f8fSGustavo F. Padovan /* Alloc CID for connection-oriented socket */ 184baa7e1faSGustavo F. Padovan l2cap_pi(sk)->scid = l2cap_alloc_cid(conn); 185b62f328bSVille Tervo l2cap_pi(sk)->omtu = L2CAP_DEFAULT_MTU; 186b62f328bSVille Tervo } 1870a708f8fSGustavo F. Padovan } else if (sk->sk_type == SOCK_DGRAM) { 1880a708f8fSGustavo F. Padovan /* Connectionless socket */ 1890a708f8fSGustavo F. Padovan l2cap_pi(sk)->scid = L2CAP_CID_CONN_LESS; 1900a708f8fSGustavo F. Padovan l2cap_pi(sk)->dcid = L2CAP_CID_CONN_LESS; 1910a708f8fSGustavo F. Padovan l2cap_pi(sk)->omtu = L2CAP_DEFAULT_MTU; 1920a708f8fSGustavo F. Padovan } else { 1930a708f8fSGustavo F. Padovan /* Raw socket can send/recv signalling messages only */ 1940a708f8fSGustavo F. Padovan l2cap_pi(sk)->scid = L2CAP_CID_SIGNALING; 1950a708f8fSGustavo F. Padovan l2cap_pi(sk)->dcid = L2CAP_CID_SIGNALING; 1960a708f8fSGustavo F. Padovan l2cap_pi(sk)->omtu = L2CAP_DEFAULT_MTU; 1970a708f8fSGustavo F. Padovan } 1980a708f8fSGustavo F. Padovan 199baa7e1faSGustavo F. Padovan sock_hold(sk); 200baa7e1faSGustavo F. Padovan 201baa7e1faSGustavo F. Padovan list_add(&chan->list, &conn->chan_l); 2020a708f8fSGustavo F. Padovan } 2030a708f8fSGustavo F. Padovan 2040a708f8fSGustavo F. Padovan /* Delete channel. 2050a708f8fSGustavo F. Padovan * Must be called on the locked socket. */ 20648454079SGustavo F. Padovan void l2cap_chan_del(struct l2cap_chan *chan, int err) 2070a708f8fSGustavo F. Padovan { 20848454079SGustavo F. Padovan struct sock *sk = chan->sk; 2090a708f8fSGustavo F. Padovan struct l2cap_conn *conn = l2cap_pi(sk)->conn; 2100a708f8fSGustavo F. Padovan struct sock *parent = bt_sk(sk)->parent; 2110a708f8fSGustavo F. Padovan 2120a708f8fSGustavo F. Padovan l2cap_sock_clear_timer(sk); 2130a708f8fSGustavo F. Padovan 21449208c9cSGustavo F. Padovan BT_DBG("chan %p, conn %p, err %d", chan, conn, err); 2150a708f8fSGustavo F. Padovan 2160a708f8fSGustavo F. Padovan if (conn) { 217baa7e1faSGustavo F. Padovan /* Delete from channel list */ 218baa7e1faSGustavo F. Padovan write_lock_bh(&conn->chan_lock); 219baa7e1faSGustavo F. Padovan list_del(&chan->list); 220baa7e1faSGustavo F. Padovan write_unlock_bh(&conn->chan_lock); 221baa7e1faSGustavo F. Padovan __sock_put(sk); 222baa7e1faSGustavo F. Padovan 2230a708f8fSGustavo F. Padovan l2cap_pi(sk)->conn = NULL; 2240a708f8fSGustavo F. Padovan hci_conn_put(conn->hcon); 2250a708f8fSGustavo F. Padovan } 2260a708f8fSGustavo F. Padovan 2270a708f8fSGustavo F. Padovan sk->sk_state = BT_CLOSED; 2280a708f8fSGustavo F. Padovan sock_set_flag(sk, SOCK_ZAPPED); 2290a708f8fSGustavo F. Padovan 2300a708f8fSGustavo F. Padovan if (err) 2310a708f8fSGustavo F. Padovan sk->sk_err = err; 2320a708f8fSGustavo F. Padovan 2330a708f8fSGustavo F. Padovan if (parent) { 2340a708f8fSGustavo F. Padovan bt_accept_unlink(sk); 2350a708f8fSGustavo F. Padovan parent->sk_data_ready(parent, 0); 2360a708f8fSGustavo F. Padovan } else 2370a708f8fSGustavo F. Padovan sk->sk_state_change(sk); 2380a708f8fSGustavo F. Padovan 239b4450035SGustavo F. Padovan if (!(chan->conf_state & L2CAP_CONF_OUTPUT_DONE && 240b4450035SGustavo F. Padovan chan->conf_state & L2CAP_CONF_INPUT_DONE)) 2412ead70b8SGustavo F. Padovan goto free; 2422ead70b8SGustavo F. Padovan 24358d35f87SGustavo F. Padovan skb_queue_purge(&chan->tx_q); 2440a708f8fSGustavo F. Padovan 2450a708f8fSGustavo F. Padovan if (l2cap_pi(sk)->mode == L2CAP_MODE_ERTM) { 2460a708f8fSGustavo F. Padovan struct srej_list *l, *tmp; 2470a708f8fSGustavo F. Padovan 248e92c8e70SGustavo F. Padovan del_timer(&chan->retrans_timer); 249e92c8e70SGustavo F. Padovan del_timer(&chan->monitor_timer); 250e92c8e70SGustavo F. Padovan del_timer(&chan->ack_timer); 2510a708f8fSGustavo F. Padovan 252f1c6775bSGustavo F. Padovan skb_queue_purge(&chan->srej_q); 253f1c6775bSGustavo F. Padovan skb_queue_purge(&chan->busy_q); 2540a708f8fSGustavo F. Padovan 25539d5a3eeSGustavo F. Padovan list_for_each_entry_safe(l, tmp, &chan->srej_l, list) { 2560a708f8fSGustavo F. Padovan list_del(&l->list); 2570a708f8fSGustavo F. Padovan kfree(l); 2580a708f8fSGustavo F. Padovan } 2590a708f8fSGustavo F. Padovan } 26048454079SGustavo F. Padovan 2612ead70b8SGustavo F. Padovan free: 26248454079SGustavo F. Padovan kfree(chan); 2630a708f8fSGustavo F. Padovan } 2640a708f8fSGustavo F. Padovan 265*4343478fSGustavo F. Padovan static inline u8 l2cap_get_auth_type(struct l2cap_chan *chan) 2660a708f8fSGustavo F. Padovan { 267*4343478fSGustavo F. Padovan struct sock *sk = chan->sk; 268*4343478fSGustavo F. Padovan 2690a708f8fSGustavo F. Padovan if (sk->sk_type == SOCK_RAW) { 270*4343478fSGustavo F. Padovan switch (chan->sec_level) { 2710a708f8fSGustavo F. Padovan case BT_SECURITY_HIGH: 2720a708f8fSGustavo F. Padovan return HCI_AT_DEDICATED_BONDING_MITM; 2730a708f8fSGustavo F. Padovan case BT_SECURITY_MEDIUM: 2740a708f8fSGustavo F. Padovan return HCI_AT_DEDICATED_BONDING; 2750a708f8fSGustavo F. Padovan default: 2760a708f8fSGustavo F. Padovan return HCI_AT_NO_BONDING; 2770a708f8fSGustavo F. Padovan } 2780a708f8fSGustavo F. Padovan } else if (l2cap_pi(sk)->psm == cpu_to_le16(0x0001)) { 279*4343478fSGustavo F. Padovan if (chan->sec_level == BT_SECURITY_LOW) 280*4343478fSGustavo F. Padovan chan->sec_level = BT_SECURITY_SDP; 2810a708f8fSGustavo F. Padovan 282*4343478fSGustavo F. Padovan if (chan->sec_level == BT_SECURITY_HIGH) 2830a708f8fSGustavo F. Padovan return HCI_AT_NO_BONDING_MITM; 2840a708f8fSGustavo F. Padovan else 2850a708f8fSGustavo F. Padovan return HCI_AT_NO_BONDING; 2860a708f8fSGustavo F. Padovan } else { 287*4343478fSGustavo F. Padovan switch (chan->sec_level) { 2880a708f8fSGustavo F. Padovan case BT_SECURITY_HIGH: 2890a708f8fSGustavo F. Padovan return HCI_AT_GENERAL_BONDING_MITM; 2900a708f8fSGustavo F. Padovan case BT_SECURITY_MEDIUM: 2910a708f8fSGustavo F. Padovan return HCI_AT_GENERAL_BONDING; 2920a708f8fSGustavo F. Padovan default: 2930a708f8fSGustavo F. Padovan return HCI_AT_NO_BONDING; 2940a708f8fSGustavo F. Padovan } 2950a708f8fSGustavo F. Padovan } 2960a708f8fSGustavo F. Padovan } 2970a708f8fSGustavo F. Padovan 2980a708f8fSGustavo F. Padovan /* Service level security */ 299*4343478fSGustavo F. Padovan static inline int l2cap_check_security(struct l2cap_chan *chan) 3000a708f8fSGustavo F. Padovan { 301*4343478fSGustavo F. Padovan struct l2cap_conn *conn = l2cap_pi(chan->sk)->conn; 3020a708f8fSGustavo F. Padovan __u8 auth_type; 3030a708f8fSGustavo F. Padovan 304*4343478fSGustavo F. Padovan auth_type = l2cap_get_auth_type(chan); 3050a708f8fSGustavo F. Padovan 306*4343478fSGustavo F. Padovan return hci_conn_security(conn->hcon, chan->sec_level, auth_type); 3070a708f8fSGustavo F. Padovan } 3080a708f8fSGustavo F. Padovan 30968983259SGustavo F. Padovan u8 l2cap_get_ident(struct l2cap_conn *conn) 3100a708f8fSGustavo F. Padovan { 3110a708f8fSGustavo F. Padovan u8 id; 3120a708f8fSGustavo F. Padovan 3130a708f8fSGustavo F. Padovan /* Get next available identificator. 3140a708f8fSGustavo F. Padovan * 1 - 128 are used by kernel. 3150a708f8fSGustavo F. Padovan * 129 - 199 are reserved. 3160a708f8fSGustavo F. Padovan * 200 - 254 are used by utilities like l2ping, etc. 3170a708f8fSGustavo F. Padovan */ 3180a708f8fSGustavo F. Padovan 3190a708f8fSGustavo F. Padovan spin_lock_bh(&conn->lock); 3200a708f8fSGustavo F. Padovan 3210a708f8fSGustavo F. Padovan if (++conn->tx_ident > 128) 3220a708f8fSGustavo F. Padovan conn->tx_ident = 1; 3230a708f8fSGustavo F. Padovan 3240a708f8fSGustavo F. Padovan id = conn->tx_ident; 3250a708f8fSGustavo F. Padovan 3260a708f8fSGustavo F. Padovan spin_unlock_bh(&conn->lock); 3270a708f8fSGustavo F. Padovan 3280a708f8fSGustavo F. Padovan return id; 3290a708f8fSGustavo F. Padovan } 3300a708f8fSGustavo F. Padovan 33168983259SGustavo F. Padovan void l2cap_send_cmd(struct l2cap_conn *conn, u8 ident, u8 code, u16 len, void *data) 3320a708f8fSGustavo F. Padovan { 3330a708f8fSGustavo F. Padovan struct sk_buff *skb = l2cap_build_cmd(conn, code, ident, len, data); 3340a708f8fSGustavo F. Padovan u8 flags; 3350a708f8fSGustavo F. Padovan 3360a708f8fSGustavo F. Padovan BT_DBG("code 0x%2.2x", code); 3370a708f8fSGustavo F. Padovan 3380a708f8fSGustavo F. Padovan if (!skb) 3390a708f8fSGustavo F. Padovan return; 3400a708f8fSGustavo F. Padovan 3410a708f8fSGustavo F. Padovan if (lmp_no_flush_capable(conn->hcon->hdev)) 3420a708f8fSGustavo F. Padovan flags = ACL_START_NO_FLUSH; 3430a708f8fSGustavo F. Padovan else 3440a708f8fSGustavo F. Padovan flags = ACL_START; 3450a708f8fSGustavo F. Padovan 3460a708f8fSGustavo F. Padovan hci_send_acl(conn->hcon, skb, flags); 3470a708f8fSGustavo F. Padovan } 3480a708f8fSGustavo F. Padovan 349525cd185SGustavo F. Padovan static inline void l2cap_send_sframe(struct l2cap_chan *chan, u16 control) 3500a708f8fSGustavo F. Padovan { 3510a708f8fSGustavo F. Padovan struct sk_buff *skb; 3520a708f8fSGustavo F. Padovan struct l2cap_hdr *lh; 353525cd185SGustavo F. Padovan struct l2cap_pinfo *pi = l2cap_pi(chan->sk); 3540a708f8fSGustavo F. Padovan struct l2cap_conn *conn = pi->conn; 3550a708f8fSGustavo F. Padovan struct sock *sk = (struct sock *)pi; 3560a708f8fSGustavo F. Padovan int count, hlen = L2CAP_HDR_SIZE + 2; 3570a708f8fSGustavo F. Padovan u8 flags; 3580a708f8fSGustavo F. Padovan 3590a708f8fSGustavo F. Padovan if (sk->sk_state != BT_CONNECTED) 3600a708f8fSGustavo F. Padovan return; 3610a708f8fSGustavo F. Padovan 3620a708f8fSGustavo F. Padovan if (pi->fcs == L2CAP_FCS_CRC16) 3630a708f8fSGustavo F. Padovan hlen += 2; 3640a708f8fSGustavo F. Padovan 36549208c9cSGustavo F. Padovan BT_DBG("chan %p, control 0x%2.2x", chan, control); 3660a708f8fSGustavo F. Padovan 3670a708f8fSGustavo F. Padovan count = min_t(unsigned int, conn->mtu, hlen); 3680a708f8fSGustavo F. Padovan control |= L2CAP_CTRL_FRAME_TYPE; 3690a708f8fSGustavo F. Padovan 370525cd185SGustavo F. Padovan if (chan->conn_state & L2CAP_CONN_SEND_FBIT) { 3710a708f8fSGustavo F. Padovan control |= L2CAP_CTRL_FINAL; 372525cd185SGustavo F. Padovan chan->conn_state &= ~L2CAP_CONN_SEND_FBIT; 3730a708f8fSGustavo F. Padovan } 3740a708f8fSGustavo F. Padovan 375525cd185SGustavo F. Padovan if (chan->conn_state & L2CAP_CONN_SEND_PBIT) { 3760a708f8fSGustavo F. Padovan control |= L2CAP_CTRL_POLL; 377525cd185SGustavo F. Padovan chan->conn_state &= ~L2CAP_CONN_SEND_PBIT; 3780a708f8fSGustavo F. Padovan } 3790a708f8fSGustavo F. Padovan 3800a708f8fSGustavo F. Padovan skb = bt_skb_alloc(count, GFP_ATOMIC); 3810a708f8fSGustavo F. Padovan if (!skb) 3820a708f8fSGustavo F. Padovan return; 3830a708f8fSGustavo F. Padovan 3840a708f8fSGustavo F. Padovan lh = (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE); 3850a708f8fSGustavo F. Padovan lh->len = cpu_to_le16(hlen - L2CAP_HDR_SIZE); 3860a708f8fSGustavo F. Padovan lh->cid = cpu_to_le16(pi->dcid); 3870a708f8fSGustavo F. Padovan put_unaligned_le16(control, skb_put(skb, 2)); 3880a708f8fSGustavo F. Padovan 3890a708f8fSGustavo F. Padovan if (pi->fcs == L2CAP_FCS_CRC16) { 3900a708f8fSGustavo F. Padovan u16 fcs = crc16(0, (u8 *)lh, count - 2); 3910a708f8fSGustavo F. Padovan put_unaligned_le16(fcs, skb_put(skb, 2)); 3920a708f8fSGustavo F. Padovan } 3930a708f8fSGustavo F. Padovan 3940a708f8fSGustavo F. Padovan if (lmp_no_flush_capable(conn->hcon->hdev)) 3950a708f8fSGustavo F. Padovan flags = ACL_START_NO_FLUSH; 3960a708f8fSGustavo F. Padovan else 3970a708f8fSGustavo F. Padovan flags = ACL_START; 3980a708f8fSGustavo F. Padovan 3990a708f8fSGustavo F. Padovan hci_send_acl(pi->conn->hcon, skb, flags); 4000a708f8fSGustavo F. Padovan } 4010a708f8fSGustavo F. Padovan 402525cd185SGustavo F. Padovan static inline void l2cap_send_rr_or_rnr(struct l2cap_chan *chan, u16 control) 4030a708f8fSGustavo F. Padovan { 404525cd185SGustavo F. Padovan if (chan->conn_state & L2CAP_CONN_LOCAL_BUSY) { 4050a708f8fSGustavo F. Padovan control |= L2CAP_SUPER_RCV_NOT_READY; 406525cd185SGustavo F. Padovan chan->conn_state |= L2CAP_CONN_RNR_SENT; 4070a708f8fSGustavo F. Padovan } else 4080a708f8fSGustavo F. Padovan control |= L2CAP_SUPER_RCV_READY; 4090a708f8fSGustavo F. Padovan 41042e5c802SGustavo F. Padovan control |= chan->buffer_seq << L2CAP_CTRL_REQSEQ_SHIFT; 4110a708f8fSGustavo F. Padovan 412525cd185SGustavo F. Padovan l2cap_send_sframe(chan, control); 4130a708f8fSGustavo F. Padovan } 4140a708f8fSGustavo F. Padovan 415b4450035SGustavo F. Padovan static inline int __l2cap_no_conn_pending(struct l2cap_chan *chan) 4160a708f8fSGustavo F. Padovan { 417b4450035SGustavo F. Padovan return !(chan->conf_state & L2CAP_CONF_CONNECT_PEND); 4180a708f8fSGustavo F. Padovan } 4190a708f8fSGustavo F. Padovan 420fc7f8a7eSGustavo F. Padovan static void l2cap_do_start(struct l2cap_chan *chan) 4210a708f8fSGustavo F. Padovan { 422fc7f8a7eSGustavo F. Padovan struct sock *sk = chan->sk; 4230a708f8fSGustavo F. Padovan struct l2cap_conn *conn = l2cap_pi(sk)->conn; 4240a708f8fSGustavo F. Padovan 4250a708f8fSGustavo F. Padovan if (conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_SENT) { 4260a708f8fSGustavo F. Padovan if (!(conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_DONE)) 4270a708f8fSGustavo F. Padovan return; 4280a708f8fSGustavo F. Padovan 429*4343478fSGustavo F. Padovan if (l2cap_check_security(chan) && 430*4343478fSGustavo F. Padovan __l2cap_no_conn_pending(chan)) { 4310a708f8fSGustavo F. Padovan struct l2cap_conn_req req; 4320a708f8fSGustavo F. Padovan req.scid = cpu_to_le16(l2cap_pi(sk)->scid); 4330a708f8fSGustavo F. Padovan req.psm = l2cap_pi(sk)->psm; 4340a708f8fSGustavo F. Padovan 435fc7f8a7eSGustavo F. Padovan chan->ident = l2cap_get_ident(conn); 436b4450035SGustavo F. Padovan chan->conf_state |= L2CAP_CONF_CONNECT_PEND; 4370a708f8fSGustavo F. Padovan 438fc7f8a7eSGustavo F. Padovan l2cap_send_cmd(conn, chan->ident, L2CAP_CONN_REQ, 439fc7f8a7eSGustavo F. Padovan sizeof(req), &req); 4400a708f8fSGustavo F. Padovan } 4410a708f8fSGustavo F. Padovan } else { 4420a708f8fSGustavo F. Padovan struct l2cap_info_req req; 4430a708f8fSGustavo F. Padovan req.type = cpu_to_le16(L2CAP_IT_FEAT_MASK); 4440a708f8fSGustavo F. Padovan 4450a708f8fSGustavo F. Padovan conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_SENT; 4460a708f8fSGustavo F. Padovan conn->info_ident = l2cap_get_ident(conn); 4470a708f8fSGustavo F. Padovan 4480a708f8fSGustavo F. Padovan mod_timer(&conn->info_timer, jiffies + 4490a708f8fSGustavo F. Padovan msecs_to_jiffies(L2CAP_INFO_TIMEOUT)); 4500a708f8fSGustavo F. Padovan 4510a708f8fSGustavo F. Padovan l2cap_send_cmd(conn, conn->info_ident, 4520a708f8fSGustavo F. Padovan L2CAP_INFO_REQ, sizeof(req), &req); 4530a708f8fSGustavo F. Padovan } 4540a708f8fSGustavo F. Padovan } 4550a708f8fSGustavo F. Padovan 4560a708f8fSGustavo F. Padovan static inline int l2cap_mode_supported(__u8 mode, __u32 feat_mask) 4570a708f8fSGustavo F. Padovan { 4580a708f8fSGustavo F. Padovan u32 local_feat_mask = l2cap_feat_mask; 4590a708f8fSGustavo F. Padovan if (!disable_ertm) 4600a708f8fSGustavo F. Padovan local_feat_mask |= L2CAP_FEAT_ERTM | L2CAP_FEAT_STREAMING; 4610a708f8fSGustavo F. Padovan 4620a708f8fSGustavo F. Padovan switch (mode) { 4630a708f8fSGustavo F. Padovan case L2CAP_MODE_ERTM: 4640a708f8fSGustavo F. Padovan return L2CAP_FEAT_ERTM & feat_mask & local_feat_mask; 4650a708f8fSGustavo F. Padovan case L2CAP_MODE_STREAMING: 4660a708f8fSGustavo F. Padovan return L2CAP_FEAT_STREAMING & feat_mask & local_feat_mask; 4670a708f8fSGustavo F. Padovan default: 4680a708f8fSGustavo F. Padovan return 0x00; 4690a708f8fSGustavo F. Padovan } 4700a708f8fSGustavo F. Padovan } 4710a708f8fSGustavo F. Padovan 472e92c8e70SGustavo F. Padovan void l2cap_send_disconn_req(struct l2cap_conn *conn, struct l2cap_chan *chan, int err) 4730a708f8fSGustavo F. Padovan { 474e92c8e70SGustavo F. Padovan struct sock *sk; 4750a708f8fSGustavo F. Padovan struct l2cap_disconn_req req; 4760a708f8fSGustavo F. Padovan 4770a708f8fSGustavo F. Padovan if (!conn) 4780a708f8fSGustavo F. Padovan return; 4790a708f8fSGustavo F. Padovan 480e92c8e70SGustavo F. Padovan sk = chan->sk; 481e92c8e70SGustavo F. Padovan 4820a708f8fSGustavo F. Padovan if (l2cap_pi(sk)->mode == L2CAP_MODE_ERTM) { 483e92c8e70SGustavo F. Padovan del_timer(&chan->retrans_timer); 484e92c8e70SGustavo F. Padovan del_timer(&chan->monitor_timer); 485e92c8e70SGustavo F. Padovan del_timer(&chan->ack_timer); 4860a708f8fSGustavo F. Padovan } 4870a708f8fSGustavo F. Padovan 4880a708f8fSGustavo F. Padovan req.dcid = cpu_to_le16(l2cap_pi(sk)->dcid); 4890a708f8fSGustavo F. Padovan req.scid = cpu_to_le16(l2cap_pi(sk)->scid); 4900a708f8fSGustavo F. Padovan l2cap_send_cmd(conn, l2cap_get_ident(conn), 4910a708f8fSGustavo F. Padovan L2CAP_DISCONN_REQ, sizeof(req), &req); 4920a708f8fSGustavo F. Padovan 4930a708f8fSGustavo F. Padovan sk->sk_state = BT_DISCONN; 4940a708f8fSGustavo F. Padovan sk->sk_err = err; 4950a708f8fSGustavo F. Padovan } 4960a708f8fSGustavo F. Padovan 4970a708f8fSGustavo F. Padovan /* ---- L2CAP connections ---- */ 4980a708f8fSGustavo F. Padovan static void l2cap_conn_start(struct l2cap_conn *conn) 4990a708f8fSGustavo F. Padovan { 500820ffdb3SGustavo F. Padovan struct l2cap_chan *chan, *tmp; 5010a708f8fSGustavo F. Padovan 5020a708f8fSGustavo F. Padovan BT_DBG("conn %p", conn); 5030a708f8fSGustavo F. Padovan 504baa7e1faSGustavo F. Padovan read_lock(&conn->chan_lock); 5050a708f8fSGustavo F. Padovan 506820ffdb3SGustavo F. Padovan list_for_each_entry_safe(chan, tmp, &conn->chan_l, list) { 50748454079SGustavo F. Padovan struct sock *sk = chan->sk; 508baa7e1faSGustavo F. Padovan 5090a708f8fSGustavo F. Padovan bh_lock_sock(sk); 5100a708f8fSGustavo F. Padovan 5110a708f8fSGustavo F. Padovan if (sk->sk_type != SOCK_SEQPACKET && 5120a708f8fSGustavo F. Padovan sk->sk_type != SOCK_STREAM) { 5130a708f8fSGustavo F. Padovan bh_unlock_sock(sk); 5140a708f8fSGustavo F. Padovan continue; 5150a708f8fSGustavo F. Padovan } 5160a708f8fSGustavo F. Padovan 5170a708f8fSGustavo F. Padovan if (sk->sk_state == BT_CONNECT) { 5180a708f8fSGustavo F. Padovan struct l2cap_conn_req req; 5190a708f8fSGustavo F. Padovan 520*4343478fSGustavo F. Padovan if (!l2cap_check_security(chan) || 521b4450035SGustavo F. Padovan !__l2cap_no_conn_pending(chan)) { 5220a708f8fSGustavo F. Padovan bh_unlock_sock(sk); 5230a708f8fSGustavo F. Padovan continue; 5240a708f8fSGustavo F. Padovan } 5250a708f8fSGustavo F. Padovan 5260a708f8fSGustavo F. Padovan if (!l2cap_mode_supported(l2cap_pi(sk)->mode, 5270a708f8fSGustavo F. Padovan conn->feat_mask) 528b4450035SGustavo F. Padovan && chan->conf_state & 5290a708f8fSGustavo F. Padovan L2CAP_CONF_STATE2_DEVICE) { 530820ffdb3SGustavo F. Padovan /* __l2cap_sock_close() calls list_del(chan) 531820ffdb3SGustavo F. Padovan * so release the lock */ 532820ffdb3SGustavo F. Padovan read_unlock_bh(&conn->chan_lock); 533820ffdb3SGustavo F. Padovan __l2cap_sock_close(sk, ECONNRESET); 534820ffdb3SGustavo F. Padovan read_lock_bh(&conn->chan_lock); 5350a708f8fSGustavo F. Padovan bh_unlock_sock(sk); 5360a708f8fSGustavo F. Padovan continue; 5370a708f8fSGustavo F. Padovan } 5380a708f8fSGustavo F. Padovan 5390a708f8fSGustavo F. Padovan req.scid = cpu_to_le16(l2cap_pi(sk)->scid); 5400a708f8fSGustavo F. Padovan req.psm = l2cap_pi(sk)->psm; 5410a708f8fSGustavo F. Padovan 542fc7f8a7eSGustavo F. Padovan chan->ident = l2cap_get_ident(conn); 543b4450035SGustavo F. Padovan chan->conf_state |= L2CAP_CONF_CONNECT_PEND; 5440a708f8fSGustavo F. Padovan 545fc7f8a7eSGustavo F. Padovan l2cap_send_cmd(conn, chan->ident, L2CAP_CONN_REQ, 546fc7f8a7eSGustavo F. Padovan sizeof(req), &req); 5470a708f8fSGustavo F. Padovan 5480a708f8fSGustavo F. Padovan } else if (sk->sk_state == BT_CONNECT2) { 5490a708f8fSGustavo F. Padovan struct l2cap_conn_rsp rsp; 5500a708f8fSGustavo F. Padovan char buf[128]; 5510a708f8fSGustavo F. Padovan rsp.scid = cpu_to_le16(l2cap_pi(sk)->dcid); 5520a708f8fSGustavo F. Padovan rsp.dcid = cpu_to_le16(l2cap_pi(sk)->scid); 5530a708f8fSGustavo F. Padovan 554*4343478fSGustavo F. Padovan if (l2cap_check_security(chan)) { 5550a708f8fSGustavo F. Padovan if (bt_sk(sk)->defer_setup) { 5560a708f8fSGustavo F. Padovan struct sock *parent = bt_sk(sk)->parent; 5570a708f8fSGustavo F. Padovan rsp.result = cpu_to_le16(L2CAP_CR_PEND); 5580a708f8fSGustavo F. Padovan rsp.status = cpu_to_le16(L2CAP_CS_AUTHOR_PEND); 5590a708f8fSGustavo F. Padovan parent->sk_data_ready(parent, 0); 5600a708f8fSGustavo F. Padovan 5610a708f8fSGustavo F. Padovan } else { 5620a708f8fSGustavo F. Padovan sk->sk_state = BT_CONFIG; 5630a708f8fSGustavo F. Padovan rsp.result = cpu_to_le16(L2CAP_CR_SUCCESS); 5640a708f8fSGustavo F. Padovan rsp.status = cpu_to_le16(L2CAP_CS_NO_INFO); 5650a708f8fSGustavo F. Padovan } 5660a708f8fSGustavo F. Padovan } else { 5670a708f8fSGustavo F. Padovan rsp.result = cpu_to_le16(L2CAP_CR_PEND); 5680a708f8fSGustavo F. Padovan rsp.status = cpu_to_le16(L2CAP_CS_AUTHEN_PEND); 5690a708f8fSGustavo F. Padovan } 5700a708f8fSGustavo F. Padovan 571fc7f8a7eSGustavo F. Padovan l2cap_send_cmd(conn, chan->ident, L2CAP_CONN_RSP, 572fc7f8a7eSGustavo F. Padovan sizeof(rsp), &rsp); 5730a708f8fSGustavo F. Padovan 574b4450035SGustavo F. Padovan if (chan->conf_state & L2CAP_CONF_REQ_SENT || 5750a708f8fSGustavo F. Padovan rsp.result != L2CAP_CR_SUCCESS) { 5760a708f8fSGustavo F. Padovan bh_unlock_sock(sk); 5770a708f8fSGustavo F. Padovan continue; 5780a708f8fSGustavo F. Padovan } 5790a708f8fSGustavo F. Padovan 580b4450035SGustavo F. Padovan chan->conf_state |= L2CAP_CONF_REQ_SENT; 5810a708f8fSGustavo F. Padovan l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ, 58273ffa904SGustavo F. Padovan l2cap_build_conf_req(chan, buf), buf); 58373ffa904SGustavo F. Padovan chan->num_conf_req++; 5840a708f8fSGustavo F. Padovan } 5850a708f8fSGustavo F. Padovan 5860a708f8fSGustavo F. Padovan bh_unlock_sock(sk); 5870a708f8fSGustavo F. Padovan } 5880a708f8fSGustavo F. Padovan 589baa7e1faSGustavo F. Padovan read_unlock(&conn->chan_lock); 5900a708f8fSGustavo F. Padovan } 5910a708f8fSGustavo F. Padovan 592b62f328bSVille Tervo /* Find socket with cid and source bdaddr. 593b62f328bSVille Tervo * Returns closest match, locked. 594b62f328bSVille Tervo */ 595b62f328bSVille Tervo static struct sock *l2cap_get_sock_by_scid(int state, __le16 cid, bdaddr_t *src) 596b62f328bSVille Tervo { 597280f294fSGustavo F. Padovan struct sock *sk = NULL, *sk1 = NULL; 598b62f328bSVille Tervo struct hlist_node *node; 599b62f328bSVille Tervo 600b62f328bSVille Tervo read_lock(&l2cap_sk_list.lock); 601b62f328bSVille Tervo 602b62f328bSVille Tervo sk_for_each(sk, node, &l2cap_sk_list.head) { 603b62f328bSVille Tervo if (state && sk->sk_state != state) 604b62f328bSVille Tervo continue; 605b62f328bSVille Tervo 606b62f328bSVille Tervo if (l2cap_pi(sk)->scid == cid) { 607b62f328bSVille Tervo /* Exact match. */ 608b62f328bSVille Tervo if (!bacmp(&bt_sk(sk)->src, src)) 609b62f328bSVille Tervo break; 610b62f328bSVille Tervo 611b62f328bSVille Tervo /* Closest match */ 612b62f328bSVille Tervo if (!bacmp(&bt_sk(sk)->src, BDADDR_ANY)) 613b62f328bSVille Tervo sk1 = sk; 614b62f328bSVille Tervo } 615b62f328bSVille Tervo } 616280f294fSGustavo F. Padovan 617b62f328bSVille Tervo read_unlock(&l2cap_sk_list.lock); 618b62f328bSVille Tervo 619280f294fSGustavo F. Padovan return node ? sk : sk1; 620b62f328bSVille Tervo } 621b62f328bSVille Tervo 622b62f328bSVille Tervo static void l2cap_le_conn_ready(struct l2cap_conn *conn) 623b62f328bSVille Tervo { 624c916fbe4SGustavo F. Padovan struct sock *parent, *sk; 62548454079SGustavo F. Padovan struct l2cap_chan *chan; 626b62f328bSVille Tervo 627b62f328bSVille Tervo BT_DBG(""); 628b62f328bSVille Tervo 629b62f328bSVille Tervo /* Check if we have socket listening on cid */ 630b62f328bSVille Tervo parent = l2cap_get_sock_by_scid(BT_LISTEN, L2CAP_CID_LE_DATA, 631b62f328bSVille Tervo conn->src); 632b62f328bSVille Tervo if (!parent) 633b62f328bSVille Tervo return; 634b62f328bSVille Tervo 63562f3a2cfSGustavo F. Padovan bh_lock_sock(parent); 63662f3a2cfSGustavo F. Padovan 637b62f328bSVille Tervo /* Check for backlog size */ 638b62f328bSVille Tervo if (sk_acceptq_is_full(parent)) { 639b62f328bSVille Tervo BT_DBG("backlog full %d", parent->sk_ack_backlog); 640b62f328bSVille Tervo goto clean; 641b62f328bSVille Tervo } 642b62f328bSVille Tervo 643b62f328bSVille Tervo sk = l2cap_sock_alloc(sock_net(parent), NULL, BTPROTO_L2CAP, GFP_ATOMIC); 644b62f328bSVille Tervo if (!sk) 645b62f328bSVille Tervo goto clean; 646b62f328bSVille Tervo 64748454079SGustavo F. Padovan chan = l2cap_chan_alloc(sk); 64848454079SGustavo F. Padovan if (!chan) { 64948454079SGustavo F. Padovan l2cap_sock_kill(sk); 65048454079SGustavo F. Padovan goto clean; 65148454079SGustavo F. Padovan } 65248454079SGustavo F. Padovan 6535d41ce1dSGustavo F. Padovan l2cap_pi(sk)->chan = chan; 6545d41ce1dSGustavo F. Padovan 655baa7e1faSGustavo F. Padovan write_lock_bh(&conn->chan_lock); 656b62f328bSVille Tervo 657b62f328bSVille Tervo hci_conn_hold(conn->hcon); 658b62f328bSVille Tervo 659b62f328bSVille Tervo l2cap_sock_init(sk, parent); 660baa7e1faSGustavo F. Padovan 661b62f328bSVille Tervo bacpy(&bt_sk(sk)->src, conn->src); 662b62f328bSVille Tervo bacpy(&bt_sk(sk)->dst, conn->dst); 663b62f328bSVille Tervo 664d1010240SGustavo F. Padovan bt_accept_enqueue(parent, sk); 665d1010240SGustavo F. Padovan 66648454079SGustavo F. Padovan __l2cap_chan_add(conn, chan); 66748454079SGustavo F. Padovan 668b62f328bSVille Tervo l2cap_sock_set_timer(sk, sk->sk_sndtimeo); 669b62f328bSVille Tervo 670b62f328bSVille Tervo sk->sk_state = BT_CONNECTED; 671b62f328bSVille Tervo parent->sk_data_ready(parent, 0); 672b62f328bSVille Tervo 673baa7e1faSGustavo F. Padovan write_unlock_bh(&conn->chan_lock); 674b62f328bSVille Tervo 675b62f328bSVille Tervo clean: 676b62f328bSVille Tervo bh_unlock_sock(parent); 677b62f328bSVille Tervo } 678b62f328bSVille Tervo 6790a708f8fSGustavo F. Padovan static void l2cap_conn_ready(struct l2cap_conn *conn) 6800a708f8fSGustavo F. Padovan { 68148454079SGustavo F. Padovan struct l2cap_chan *chan; 6820a708f8fSGustavo F. Padovan 6830a708f8fSGustavo F. Padovan BT_DBG("conn %p", conn); 6840a708f8fSGustavo F. Padovan 685b62f328bSVille Tervo if (!conn->hcon->out && conn->hcon->type == LE_LINK) 686b62f328bSVille Tervo l2cap_le_conn_ready(conn); 687b62f328bSVille Tervo 688baa7e1faSGustavo F. Padovan read_lock(&conn->chan_lock); 6890a708f8fSGustavo F. Padovan 690baa7e1faSGustavo F. Padovan list_for_each_entry(chan, &conn->chan_l, list) { 69148454079SGustavo F. Padovan struct sock *sk = chan->sk; 692baa7e1faSGustavo F. Padovan 6930a708f8fSGustavo F. Padovan bh_lock_sock(sk); 6940a708f8fSGustavo F. Padovan 695acd7d370SVille Tervo if (conn->hcon->type == LE_LINK) { 696acd7d370SVille Tervo l2cap_sock_clear_timer(sk); 697acd7d370SVille Tervo sk->sk_state = BT_CONNECTED; 698acd7d370SVille Tervo sk->sk_state_change(sk); 699acd7d370SVille Tervo } 700acd7d370SVille Tervo 7010a708f8fSGustavo F. Padovan if (sk->sk_type != SOCK_SEQPACKET && 7020a708f8fSGustavo F. Padovan sk->sk_type != SOCK_STREAM) { 7030a708f8fSGustavo F. Padovan l2cap_sock_clear_timer(sk); 7040a708f8fSGustavo F. Padovan sk->sk_state = BT_CONNECTED; 7050a708f8fSGustavo F. Padovan sk->sk_state_change(sk); 7060a708f8fSGustavo F. Padovan } else if (sk->sk_state == BT_CONNECT) 707fc7f8a7eSGustavo F. Padovan l2cap_do_start(chan); 7080a708f8fSGustavo F. Padovan 7090a708f8fSGustavo F. Padovan bh_unlock_sock(sk); 7100a708f8fSGustavo F. Padovan } 7110a708f8fSGustavo F. Padovan 712baa7e1faSGustavo F. Padovan read_unlock(&conn->chan_lock); 7130a708f8fSGustavo F. Padovan } 7140a708f8fSGustavo F. Padovan 7150a708f8fSGustavo F. Padovan /* Notify sockets that we cannot guaranty reliability anymore */ 7160a708f8fSGustavo F. Padovan static void l2cap_conn_unreliable(struct l2cap_conn *conn, int err) 7170a708f8fSGustavo F. Padovan { 71848454079SGustavo F. Padovan struct l2cap_chan *chan; 7190a708f8fSGustavo F. Padovan 7200a708f8fSGustavo F. Padovan BT_DBG("conn %p", conn); 7210a708f8fSGustavo F. Padovan 722baa7e1faSGustavo F. Padovan read_lock(&conn->chan_lock); 7230a708f8fSGustavo F. Padovan 724baa7e1faSGustavo F. Padovan list_for_each_entry(chan, &conn->chan_l, list) { 72548454079SGustavo F. Padovan struct sock *sk = chan->sk; 726baa7e1faSGustavo F. Padovan 727*4343478fSGustavo F. Padovan if (chan->force_reliable) 7280a708f8fSGustavo F. Padovan sk->sk_err = err; 7290a708f8fSGustavo F. Padovan } 7300a708f8fSGustavo F. Padovan 731baa7e1faSGustavo F. Padovan read_unlock(&conn->chan_lock); 7320a708f8fSGustavo F. Padovan } 7330a708f8fSGustavo F. Padovan 7340a708f8fSGustavo F. Padovan static void l2cap_info_timeout(unsigned long arg) 7350a708f8fSGustavo F. Padovan { 7360a708f8fSGustavo F. Padovan struct l2cap_conn *conn = (void *) arg; 7370a708f8fSGustavo F. Padovan 7380a708f8fSGustavo F. Padovan conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_DONE; 7390a708f8fSGustavo F. Padovan conn->info_ident = 0; 7400a708f8fSGustavo F. Padovan 7410a708f8fSGustavo F. Padovan l2cap_conn_start(conn); 7420a708f8fSGustavo F. Padovan } 7430a708f8fSGustavo F. Padovan 7440a708f8fSGustavo F. Padovan static struct l2cap_conn *l2cap_conn_add(struct hci_conn *hcon, u8 status) 7450a708f8fSGustavo F. Padovan { 7460a708f8fSGustavo F. Padovan struct l2cap_conn *conn = hcon->l2cap_data; 7470a708f8fSGustavo F. Padovan 7480a708f8fSGustavo F. Padovan if (conn || status) 7490a708f8fSGustavo F. Padovan return conn; 7500a708f8fSGustavo F. Padovan 7510a708f8fSGustavo F. Padovan conn = kzalloc(sizeof(struct l2cap_conn), GFP_ATOMIC); 7520a708f8fSGustavo F. Padovan if (!conn) 7530a708f8fSGustavo F. Padovan return NULL; 7540a708f8fSGustavo F. Padovan 7550a708f8fSGustavo F. Padovan hcon->l2cap_data = conn; 7560a708f8fSGustavo F. Padovan conn->hcon = hcon; 7570a708f8fSGustavo F. Padovan 7580a708f8fSGustavo F. Padovan BT_DBG("hcon %p conn %p", hcon, conn); 7590a708f8fSGustavo F. Padovan 760acd7d370SVille Tervo if (hcon->hdev->le_mtu && hcon->type == LE_LINK) 761acd7d370SVille Tervo conn->mtu = hcon->hdev->le_mtu; 762acd7d370SVille Tervo else 7630a708f8fSGustavo F. Padovan conn->mtu = hcon->hdev->acl_mtu; 764acd7d370SVille Tervo 7650a708f8fSGustavo F. Padovan conn->src = &hcon->hdev->bdaddr; 7660a708f8fSGustavo F. Padovan conn->dst = &hcon->dst; 7670a708f8fSGustavo F. Padovan 7680a708f8fSGustavo F. Padovan conn->feat_mask = 0; 7690a708f8fSGustavo F. Padovan 7700a708f8fSGustavo F. Padovan spin_lock_init(&conn->lock); 771baa7e1faSGustavo F. Padovan rwlock_init(&conn->chan_lock); 772baa7e1faSGustavo F. Padovan 773baa7e1faSGustavo F. Padovan INIT_LIST_HEAD(&conn->chan_l); 7740a708f8fSGustavo F. Padovan 775b62f328bSVille Tervo if (hcon->type != LE_LINK) 7760a708f8fSGustavo F. Padovan setup_timer(&conn->info_timer, l2cap_info_timeout, 7770a708f8fSGustavo F. Padovan (unsigned long) conn); 7780a708f8fSGustavo F. Padovan 7790a708f8fSGustavo F. Padovan conn->disc_reason = 0x13; 7800a708f8fSGustavo F. Padovan 7810a708f8fSGustavo F. Padovan return conn; 7820a708f8fSGustavo F. Padovan } 7830a708f8fSGustavo F. Padovan 7840a708f8fSGustavo F. Padovan static void l2cap_conn_del(struct hci_conn *hcon, int err) 7850a708f8fSGustavo F. Padovan { 7860a708f8fSGustavo F. Padovan struct l2cap_conn *conn = hcon->l2cap_data; 787baa7e1faSGustavo F. Padovan struct l2cap_chan *chan, *l; 7880a708f8fSGustavo F. Padovan struct sock *sk; 7890a708f8fSGustavo F. Padovan 7900a708f8fSGustavo F. Padovan if (!conn) 7910a708f8fSGustavo F. Padovan return; 7920a708f8fSGustavo F. Padovan 7930a708f8fSGustavo F. Padovan BT_DBG("hcon %p conn %p, err %d", hcon, conn, err); 7940a708f8fSGustavo F. Padovan 7950a708f8fSGustavo F. Padovan kfree_skb(conn->rx_skb); 7960a708f8fSGustavo F. Padovan 7970a708f8fSGustavo F. Padovan /* Kill channels */ 798baa7e1faSGustavo F. Padovan list_for_each_entry_safe(chan, l, &conn->chan_l, list) { 79948454079SGustavo F. Padovan sk = chan->sk; 8000a708f8fSGustavo F. Padovan bh_lock_sock(sk); 80148454079SGustavo F. Padovan l2cap_chan_del(chan, err); 8020a708f8fSGustavo F. Padovan bh_unlock_sock(sk); 8030a708f8fSGustavo F. Padovan l2cap_sock_kill(sk); 8040a708f8fSGustavo F. Padovan } 8050a708f8fSGustavo F. Padovan 8060a708f8fSGustavo F. Padovan if (conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_SENT) 8070a708f8fSGustavo F. Padovan del_timer_sync(&conn->info_timer); 8080a708f8fSGustavo F. Padovan 8090a708f8fSGustavo F. Padovan hcon->l2cap_data = NULL; 8100a708f8fSGustavo F. Padovan kfree(conn); 8110a708f8fSGustavo F. Padovan } 8120a708f8fSGustavo F. Padovan 81348454079SGustavo F. Padovan static inline void l2cap_chan_add(struct l2cap_conn *conn, struct l2cap_chan *chan) 8140a708f8fSGustavo F. Padovan { 815baa7e1faSGustavo F. Padovan write_lock_bh(&conn->chan_lock); 81648454079SGustavo F. Padovan __l2cap_chan_add(conn, chan); 817baa7e1faSGustavo F. Padovan write_unlock_bh(&conn->chan_lock); 8180a708f8fSGustavo F. Padovan } 8190a708f8fSGustavo F. Padovan 8200a708f8fSGustavo F. Padovan /* ---- Socket interface ---- */ 8210a708f8fSGustavo F. Padovan 8220a708f8fSGustavo F. Padovan /* Find socket with psm and source bdaddr. 8230a708f8fSGustavo F. Padovan * Returns closest match. 8240a708f8fSGustavo F. Padovan */ 8250a708f8fSGustavo F. Padovan static struct sock *l2cap_get_sock_by_psm(int state, __le16 psm, bdaddr_t *src) 8260a708f8fSGustavo F. Padovan { 8270a708f8fSGustavo F. Padovan struct sock *sk = NULL, *sk1 = NULL; 8280a708f8fSGustavo F. Padovan struct hlist_node *node; 8290a708f8fSGustavo F. Padovan 8300a708f8fSGustavo F. Padovan read_lock(&l2cap_sk_list.lock); 8310a708f8fSGustavo F. Padovan 8320a708f8fSGustavo F. Padovan sk_for_each(sk, node, &l2cap_sk_list.head) { 8330a708f8fSGustavo F. Padovan if (state && sk->sk_state != state) 8340a708f8fSGustavo F. Padovan continue; 8350a708f8fSGustavo F. Padovan 8360a708f8fSGustavo F. Padovan if (l2cap_pi(sk)->psm == psm) { 8370a708f8fSGustavo F. Padovan /* Exact match. */ 8380a708f8fSGustavo F. Padovan if (!bacmp(&bt_sk(sk)->src, src)) 8390a708f8fSGustavo F. Padovan break; 8400a708f8fSGustavo F. Padovan 8410a708f8fSGustavo F. Padovan /* Closest match */ 8420a708f8fSGustavo F. Padovan if (!bacmp(&bt_sk(sk)->src, BDADDR_ANY)) 8430a708f8fSGustavo F. Padovan sk1 = sk; 8440a708f8fSGustavo F. Padovan } 8450a708f8fSGustavo F. Padovan } 8460a708f8fSGustavo F. Padovan 8470a708f8fSGustavo F. Padovan read_unlock(&l2cap_sk_list.lock); 8480a708f8fSGustavo F. Padovan 8490a708f8fSGustavo F. Padovan return node ? sk : sk1; 8500a708f8fSGustavo F. Padovan } 8510a708f8fSGustavo F. Padovan 85277a74c7eSGustavo F. Padovan int l2cap_chan_connect(struct l2cap_chan *chan) 8530a708f8fSGustavo F. Padovan { 8545d41ce1dSGustavo F. Padovan struct sock *sk = chan->sk; 8550a708f8fSGustavo F. Padovan bdaddr_t *src = &bt_sk(sk)->src; 8560a708f8fSGustavo F. Padovan bdaddr_t *dst = &bt_sk(sk)->dst; 8570a708f8fSGustavo F. Padovan struct l2cap_conn *conn; 8580a708f8fSGustavo F. Padovan struct hci_conn *hcon; 8590a708f8fSGustavo F. Padovan struct hci_dev *hdev; 8600a708f8fSGustavo F. Padovan __u8 auth_type; 8610a708f8fSGustavo F. Padovan int err; 8620a708f8fSGustavo F. Padovan 8630a708f8fSGustavo F. Padovan BT_DBG("%s -> %s psm 0x%2.2x", batostr(src), batostr(dst), 8640a708f8fSGustavo F. Padovan l2cap_pi(sk)->psm); 8650a708f8fSGustavo F. Padovan 8660a708f8fSGustavo F. Padovan hdev = hci_get_route(dst, src); 8670a708f8fSGustavo F. Padovan if (!hdev) 8680a708f8fSGustavo F. Padovan return -EHOSTUNREACH; 8690a708f8fSGustavo F. Padovan 8700a708f8fSGustavo F. Padovan hci_dev_lock_bh(hdev); 8710a708f8fSGustavo F. Padovan 872*4343478fSGustavo F. Padovan auth_type = l2cap_get_auth_type(chan); 8730a708f8fSGustavo F. Padovan 874acd7d370SVille Tervo if (l2cap_pi(sk)->dcid == L2CAP_CID_LE_DATA) 875acd7d370SVille Tervo hcon = hci_connect(hdev, LE_LINK, dst, 876*4343478fSGustavo F. Padovan chan->sec_level, auth_type); 877acd7d370SVille Tervo else 8780a708f8fSGustavo F. Padovan hcon = hci_connect(hdev, ACL_LINK, dst, 879*4343478fSGustavo F. Padovan chan->sec_level, auth_type); 880acd7d370SVille Tervo 88130e76272SVille Tervo if (IS_ERR(hcon)) { 88230e76272SVille Tervo err = PTR_ERR(hcon); 8830a708f8fSGustavo F. Padovan goto done; 88430e76272SVille Tervo } 8850a708f8fSGustavo F. Padovan 8860a708f8fSGustavo F. Padovan conn = l2cap_conn_add(hcon, 0); 8870a708f8fSGustavo F. Padovan if (!conn) { 8880a708f8fSGustavo F. Padovan hci_conn_put(hcon); 88930e76272SVille Tervo err = -ENOMEM; 8900a708f8fSGustavo F. Padovan goto done; 8910a708f8fSGustavo F. Padovan } 8920a708f8fSGustavo F. Padovan 8930a708f8fSGustavo F. Padovan /* Update source addr of the socket */ 8940a708f8fSGustavo F. Padovan bacpy(src, conn->src); 8950a708f8fSGustavo F. Padovan 89648454079SGustavo F. Padovan l2cap_chan_add(conn, chan); 89748454079SGustavo F. Padovan 8980a708f8fSGustavo F. Padovan sk->sk_state = BT_CONNECT; 8990a708f8fSGustavo F. Padovan l2cap_sock_set_timer(sk, sk->sk_sndtimeo); 9000a708f8fSGustavo F. Padovan 9010a708f8fSGustavo F. Padovan if (hcon->state == BT_CONNECTED) { 9020a708f8fSGustavo F. Padovan if (sk->sk_type != SOCK_SEQPACKET && 9030a708f8fSGustavo F. Padovan sk->sk_type != SOCK_STREAM) { 9040a708f8fSGustavo F. Padovan l2cap_sock_clear_timer(sk); 905*4343478fSGustavo F. Padovan if (l2cap_check_security(chan)) 9060a708f8fSGustavo F. Padovan sk->sk_state = BT_CONNECTED; 9070a708f8fSGustavo F. Padovan } else 908fc7f8a7eSGustavo F. Padovan l2cap_do_start(chan); 9090a708f8fSGustavo F. Padovan } 9100a708f8fSGustavo F. Padovan 91130e76272SVille Tervo err = 0; 91230e76272SVille Tervo 9130a708f8fSGustavo F. Padovan done: 9140a708f8fSGustavo F. Padovan hci_dev_unlock_bh(hdev); 9150a708f8fSGustavo F. Padovan hci_dev_put(hdev); 9160a708f8fSGustavo F. Padovan return err; 9170a708f8fSGustavo F. Padovan } 9180a708f8fSGustavo F. Padovan 919dcba0dbaSGustavo F. Padovan int __l2cap_wait_ack(struct sock *sk) 9200a708f8fSGustavo F. Padovan { 9210a708f8fSGustavo F. Padovan DECLARE_WAITQUEUE(wait, current); 9220a708f8fSGustavo F. Padovan int err = 0; 9230a708f8fSGustavo F. Padovan int timeo = HZ/5; 9240a708f8fSGustavo F. Padovan 9250a708f8fSGustavo F. Padovan add_wait_queue(sk_sleep(sk), &wait); 9266a026610SGustavo F. Padovan while ((l2cap_pi(sk)->chan->unacked_frames > 0 && l2cap_pi(sk)->conn)) { 9270a708f8fSGustavo F. Padovan set_current_state(TASK_INTERRUPTIBLE); 9280a708f8fSGustavo F. Padovan 9290a708f8fSGustavo F. Padovan if (!timeo) 9300a708f8fSGustavo F. Padovan timeo = HZ/5; 9310a708f8fSGustavo F. Padovan 9320a708f8fSGustavo F. Padovan if (signal_pending(current)) { 9330a708f8fSGustavo F. Padovan err = sock_intr_errno(timeo); 9340a708f8fSGustavo F. Padovan break; 9350a708f8fSGustavo F. Padovan } 9360a708f8fSGustavo F. Padovan 9370a708f8fSGustavo F. Padovan release_sock(sk); 9380a708f8fSGustavo F. Padovan timeo = schedule_timeout(timeo); 9390a708f8fSGustavo F. Padovan lock_sock(sk); 9400a708f8fSGustavo F. Padovan 9410a708f8fSGustavo F. Padovan err = sock_error(sk); 9420a708f8fSGustavo F. Padovan if (err) 9430a708f8fSGustavo F. Padovan break; 9440a708f8fSGustavo F. Padovan } 9450a708f8fSGustavo F. Padovan set_current_state(TASK_RUNNING); 9460a708f8fSGustavo F. Padovan remove_wait_queue(sk_sleep(sk), &wait); 9470a708f8fSGustavo F. Padovan return err; 9480a708f8fSGustavo F. Padovan } 9490a708f8fSGustavo F. Padovan 9500a708f8fSGustavo F. Padovan static void l2cap_monitor_timeout(unsigned long arg) 9510a708f8fSGustavo F. Padovan { 952525cd185SGustavo F. Padovan struct l2cap_chan *chan = (void *) arg; 953525cd185SGustavo F. Padovan struct sock *sk = chan->sk; 9540a708f8fSGustavo F. Padovan 955525cd185SGustavo F. Padovan BT_DBG("chan %p", chan); 9560a708f8fSGustavo F. Padovan 9570a708f8fSGustavo F. Padovan bh_lock_sock(sk); 9582c03a7a4SGustavo F. Padovan if (chan->retry_count >= chan->remote_max_tx) { 959e92c8e70SGustavo F. Padovan l2cap_send_disconn_req(l2cap_pi(sk)->conn, chan, ECONNABORTED); 9600a708f8fSGustavo F. Padovan bh_unlock_sock(sk); 9610a708f8fSGustavo F. Padovan return; 9620a708f8fSGustavo F. Padovan } 9630a708f8fSGustavo F. Padovan 9646a026610SGustavo F. Padovan chan->retry_count++; 9650a708f8fSGustavo F. Padovan __mod_monitor_timer(); 9660a708f8fSGustavo F. Padovan 967525cd185SGustavo F. Padovan l2cap_send_rr_or_rnr(chan, L2CAP_CTRL_POLL); 9680a708f8fSGustavo F. Padovan bh_unlock_sock(sk); 9690a708f8fSGustavo F. Padovan } 9700a708f8fSGustavo F. Padovan 9710a708f8fSGustavo F. Padovan static void l2cap_retrans_timeout(unsigned long arg) 9720a708f8fSGustavo F. Padovan { 973525cd185SGustavo F. Padovan struct l2cap_chan *chan = (void *) arg; 974525cd185SGustavo F. Padovan struct sock *sk = chan->sk; 9750a708f8fSGustavo F. Padovan 97649208c9cSGustavo F. Padovan BT_DBG("chan %p", chan); 9770a708f8fSGustavo F. Padovan 9780a708f8fSGustavo F. Padovan bh_lock_sock(sk); 9796a026610SGustavo F. Padovan chan->retry_count = 1; 9800a708f8fSGustavo F. Padovan __mod_monitor_timer(); 9810a708f8fSGustavo F. Padovan 982525cd185SGustavo F. Padovan chan->conn_state |= L2CAP_CONN_WAIT_F; 9830a708f8fSGustavo F. Padovan 984525cd185SGustavo F. Padovan l2cap_send_rr_or_rnr(chan, L2CAP_CTRL_POLL); 9850a708f8fSGustavo F. Padovan bh_unlock_sock(sk); 9860a708f8fSGustavo F. Padovan } 9870a708f8fSGustavo F. Padovan 98842e5c802SGustavo F. Padovan static void l2cap_drop_acked_frames(struct l2cap_chan *chan) 9890a708f8fSGustavo F. Padovan { 9900a708f8fSGustavo F. Padovan struct sk_buff *skb; 9910a708f8fSGustavo F. Padovan 99258d35f87SGustavo F. Padovan while ((skb = skb_peek(&chan->tx_q)) && 9936a026610SGustavo F. Padovan chan->unacked_frames) { 99442e5c802SGustavo F. Padovan if (bt_cb(skb)->tx_seq == chan->expected_ack_seq) 9950a708f8fSGustavo F. Padovan break; 9960a708f8fSGustavo F. Padovan 99758d35f87SGustavo F. Padovan skb = skb_dequeue(&chan->tx_q); 9980a708f8fSGustavo F. Padovan kfree_skb(skb); 9990a708f8fSGustavo F. Padovan 10006a026610SGustavo F. Padovan chan->unacked_frames--; 10010a708f8fSGustavo F. Padovan } 10020a708f8fSGustavo F. Padovan 10036a026610SGustavo F. Padovan if (!chan->unacked_frames) 1004e92c8e70SGustavo F. Padovan del_timer(&chan->retrans_timer); 10050a708f8fSGustavo F. Padovan } 10060a708f8fSGustavo F. Padovan 1007*4343478fSGustavo F. Padovan void l2cap_do_send(struct l2cap_chan *chan, struct sk_buff *skb) 10080a708f8fSGustavo F. Padovan { 1009*4343478fSGustavo F. Padovan struct sock *sk = chan->sk; 1010*4343478fSGustavo F. Padovan struct hci_conn *hcon = l2cap_pi(sk)->conn->hcon; 10110a708f8fSGustavo F. Padovan u16 flags; 10120a708f8fSGustavo F. Padovan 1013*4343478fSGustavo F. Padovan BT_DBG("chan %p, skb %p len %d", chan, skb, skb->len); 10140a708f8fSGustavo F. Padovan 1015*4343478fSGustavo F. Padovan if (!chan->flushable && lmp_no_flush_capable(hcon->hdev)) 10160a708f8fSGustavo F. Padovan flags = ACL_START_NO_FLUSH; 10170a708f8fSGustavo F. Padovan else 10180a708f8fSGustavo F. Padovan flags = ACL_START; 10190a708f8fSGustavo F. Padovan 10200a708f8fSGustavo F. Padovan hci_send_acl(hcon, skb, flags); 10210a708f8fSGustavo F. Padovan } 10220a708f8fSGustavo F. Padovan 102342e5c802SGustavo F. Padovan void l2cap_streaming_send(struct l2cap_chan *chan) 10240a708f8fSGustavo F. Padovan { 102542e5c802SGustavo F. Padovan struct sock *sk = chan->sk; 10260a708f8fSGustavo F. Padovan struct sk_buff *skb; 10270a708f8fSGustavo F. Padovan struct l2cap_pinfo *pi = l2cap_pi(sk); 10280a708f8fSGustavo F. Padovan u16 control, fcs; 10290a708f8fSGustavo F. Padovan 103058d35f87SGustavo F. Padovan while ((skb = skb_dequeue(&chan->tx_q))) { 10310a708f8fSGustavo F. Padovan control = get_unaligned_le16(skb->data + L2CAP_HDR_SIZE); 103242e5c802SGustavo F. Padovan control |= chan->next_tx_seq << L2CAP_CTRL_TXSEQ_SHIFT; 10330a708f8fSGustavo F. Padovan put_unaligned_le16(control, skb->data + L2CAP_HDR_SIZE); 10340a708f8fSGustavo F. Padovan 10350a708f8fSGustavo F. Padovan if (pi->fcs == L2CAP_FCS_CRC16) { 10360a708f8fSGustavo F. Padovan fcs = crc16(0, (u8 *)skb->data, skb->len - 2); 10370a708f8fSGustavo F. Padovan put_unaligned_le16(fcs, skb->data + skb->len - 2); 10380a708f8fSGustavo F. Padovan } 10390a708f8fSGustavo F. Padovan 1040*4343478fSGustavo F. Padovan l2cap_do_send(chan, skb); 10410a708f8fSGustavo F. Padovan 104242e5c802SGustavo F. Padovan chan->next_tx_seq = (chan->next_tx_seq + 1) % 64; 10430a708f8fSGustavo F. Padovan } 10440a708f8fSGustavo F. Padovan } 10450a708f8fSGustavo F. Padovan 1046525cd185SGustavo F. Padovan static void l2cap_retransmit_one_frame(struct l2cap_chan *chan, u8 tx_seq) 10470a708f8fSGustavo F. Padovan { 1048525cd185SGustavo F. Padovan struct sock *sk = chan->sk; 10490a708f8fSGustavo F. Padovan struct l2cap_pinfo *pi = l2cap_pi(sk); 10500a708f8fSGustavo F. Padovan struct sk_buff *skb, *tx_skb; 10510a708f8fSGustavo F. Padovan u16 control, fcs; 10520a708f8fSGustavo F. Padovan 105358d35f87SGustavo F. Padovan skb = skb_peek(&chan->tx_q); 10540a708f8fSGustavo F. Padovan if (!skb) 10550a708f8fSGustavo F. Padovan return; 10560a708f8fSGustavo F. Padovan 10570a708f8fSGustavo F. Padovan do { 10580a708f8fSGustavo F. Padovan if (bt_cb(skb)->tx_seq == tx_seq) 10590a708f8fSGustavo F. Padovan break; 10600a708f8fSGustavo F. Padovan 106158d35f87SGustavo F. Padovan if (skb_queue_is_last(&chan->tx_q, skb)) 10620a708f8fSGustavo F. Padovan return; 10630a708f8fSGustavo F. Padovan 106458d35f87SGustavo F. Padovan } while ((skb = skb_queue_next(&chan->tx_q, skb))); 10650a708f8fSGustavo F. Padovan 10662c03a7a4SGustavo F. Padovan if (chan->remote_max_tx && 10672c03a7a4SGustavo F. Padovan bt_cb(skb)->retries == chan->remote_max_tx) { 1068e92c8e70SGustavo F. Padovan l2cap_send_disconn_req(pi->conn, chan, ECONNABORTED); 10690a708f8fSGustavo F. Padovan return; 10700a708f8fSGustavo F. Padovan } 10710a708f8fSGustavo F. Padovan 10720a708f8fSGustavo F. Padovan tx_skb = skb_clone(skb, GFP_ATOMIC); 10730a708f8fSGustavo F. Padovan bt_cb(skb)->retries++; 10740a708f8fSGustavo F. Padovan control = get_unaligned_le16(tx_skb->data + L2CAP_HDR_SIZE); 1075a429b519SRuiyi Zhang control &= L2CAP_CTRL_SAR; 10760a708f8fSGustavo F. Padovan 1077525cd185SGustavo F. Padovan if (chan->conn_state & L2CAP_CONN_SEND_FBIT) { 10780a708f8fSGustavo F. Padovan control |= L2CAP_CTRL_FINAL; 1079525cd185SGustavo F. Padovan chan->conn_state &= ~L2CAP_CONN_SEND_FBIT; 10800a708f8fSGustavo F. Padovan } 10810a708f8fSGustavo F. Padovan 108242e5c802SGustavo F. Padovan control |= (chan->buffer_seq << L2CAP_CTRL_REQSEQ_SHIFT) 10830a708f8fSGustavo F. Padovan | (tx_seq << L2CAP_CTRL_TXSEQ_SHIFT); 10840a708f8fSGustavo F. Padovan 10850a708f8fSGustavo F. Padovan put_unaligned_le16(control, tx_skb->data + L2CAP_HDR_SIZE); 10860a708f8fSGustavo F. Padovan 10870a708f8fSGustavo F. Padovan if (pi->fcs == L2CAP_FCS_CRC16) { 10880a708f8fSGustavo F. Padovan fcs = crc16(0, (u8 *)tx_skb->data, tx_skb->len - 2); 10890a708f8fSGustavo F. Padovan put_unaligned_le16(fcs, tx_skb->data + tx_skb->len - 2); 10900a708f8fSGustavo F. Padovan } 10910a708f8fSGustavo F. Padovan 1092*4343478fSGustavo F. Padovan l2cap_do_send(chan, tx_skb); 10930a708f8fSGustavo F. Padovan } 10940a708f8fSGustavo F. Padovan 1095525cd185SGustavo F. Padovan int l2cap_ertm_send(struct l2cap_chan *chan) 10960a708f8fSGustavo F. Padovan { 10970a708f8fSGustavo F. Padovan struct sk_buff *skb, *tx_skb; 1098525cd185SGustavo F. Padovan struct sock *sk = chan->sk; 10990a708f8fSGustavo F. Padovan struct l2cap_pinfo *pi = l2cap_pi(sk); 11000a708f8fSGustavo F. Padovan u16 control, fcs; 11010a708f8fSGustavo F. Padovan int nsent = 0; 11020a708f8fSGustavo F. Padovan 11030a708f8fSGustavo F. Padovan if (sk->sk_state != BT_CONNECTED) 11040a708f8fSGustavo F. Padovan return -ENOTCONN; 11050a708f8fSGustavo F. Padovan 110658d35f87SGustavo F. Padovan while ((skb = chan->tx_send_head) && (!l2cap_tx_window_full(chan))) { 11070a708f8fSGustavo F. Padovan 11082c03a7a4SGustavo F. Padovan if (chan->remote_max_tx && 11092c03a7a4SGustavo F. Padovan bt_cb(skb)->retries == chan->remote_max_tx) { 1110e92c8e70SGustavo F. Padovan l2cap_send_disconn_req(pi->conn, chan, ECONNABORTED); 11110a708f8fSGustavo F. Padovan break; 11120a708f8fSGustavo F. Padovan } 11130a708f8fSGustavo F. Padovan 11140a708f8fSGustavo F. Padovan tx_skb = skb_clone(skb, GFP_ATOMIC); 11150a708f8fSGustavo F. Padovan 11160a708f8fSGustavo F. Padovan bt_cb(skb)->retries++; 11170a708f8fSGustavo F. Padovan 11180a708f8fSGustavo F. Padovan control = get_unaligned_le16(tx_skb->data + L2CAP_HDR_SIZE); 11190a708f8fSGustavo F. Padovan control &= L2CAP_CTRL_SAR; 11200a708f8fSGustavo F. Padovan 1121525cd185SGustavo F. Padovan if (chan->conn_state & L2CAP_CONN_SEND_FBIT) { 11220a708f8fSGustavo F. Padovan control |= L2CAP_CTRL_FINAL; 1123525cd185SGustavo F. Padovan chan->conn_state &= ~L2CAP_CONN_SEND_FBIT; 11240a708f8fSGustavo F. Padovan } 112542e5c802SGustavo F. Padovan control |= (chan->buffer_seq << L2CAP_CTRL_REQSEQ_SHIFT) 112642e5c802SGustavo F. Padovan | (chan->next_tx_seq << L2CAP_CTRL_TXSEQ_SHIFT); 11270a708f8fSGustavo F. Padovan put_unaligned_le16(control, tx_skb->data + L2CAP_HDR_SIZE); 11280a708f8fSGustavo F. Padovan 11290a708f8fSGustavo F. Padovan 11300a708f8fSGustavo F. Padovan if (pi->fcs == L2CAP_FCS_CRC16) { 11310a708f8fSGustavo F. Padovan fcs = crc16(0, (u8 *)skb->data, tx_skb->len - 2); 11320a708f8fSGustavo F. Padovan put_unaligned_le16(fcs, skb->data + tx_skb->len - 2); 11330a708f8fSGustavo F. Padovan } 11340a708f8fSGustavo F. Padovan 1135*4343478fSGustavo F. Padovan l2cap_do_send(chan, tx_skb); 11360a708f8fSGustavo F. Padovan 11370a708f8fSGustavo F. Padovan __mod_retrans_timer(); 11380a708f8fSGustavo F. Padovan 113942e5c802SGustavo F. Padovan bt_cb(skb)->tx_seq = chan->next_tx_seq; 114042e5c802SGustavo F. Padovan chan->next_tx_seq = (chan->next_tx_seq + 1) % 64; 11410a708f8fSGustavo F. Padovan 114223e9fde2SSuraj Sumangala if (bt_cb(skb)->retries == 1) 11436a026610SGustavo F. Padovan chan->unacked_frames++; 114423e9fde2SSuraj Sumangala 11456a026610SGustavo F. Padovan chan->frames_sent++; 11460a708f8fSGustavo F. Padovan 114758d35f87SGustavo F. Padovan if (skb_queue_is_last(&chan->tx_q, skb)) 114858d35f87SGustavo F. Padovan chan->tx_send_head = NULL; 11490a708f8fSGustavo F. Padovan else 115058d35f87SGustavo F. Padovan chan->tx_send_head = skb_queue_next(&chan->tx_q, skb); 11510a708f8fSGustavo F. Padovan 11520a708f8fSGustavo F. Padovan nsent++; 11530a708f8fSGustavo F. Padovan } 11540a708f8fSGustavo F. Padovan 11550a708f8fSGustavo F. Padovan return nsent; 11560a708f8fSGustavo F. Padovan } 11570a708f8fSGustavo F. Padovan 1158525cd185SGustavo F. Padovan static int l2cap_retransmit_frames(struct l2cap_chan *chan) 11590a708f8fSGustavo F. Padovan { 11600a708f8fSGustavo F. Padovan int ret; 11610a708f8fSGustavo F. Padovan 116258d35f87SGustavo F. Padovan if (!skb_queue_empty(&chan->tx_q)) 116358d35f87SGustavo F. Padovan chan->tx_send_head = chan->tx_q.next; 11640a708f8fSGustavo F. Padovan 116542e5c802SGustavo F. Padovan chan->next_tx_seq = chan->expected_ack_seq; 1166525cd185SGustavo F. Padovan ret = l2cap_ertm_send(chan); 11670a708f8fSGustavo F. Padovan return ret; 11680a708f8fSGustavo F. Padovan } 11690a708f8fSGustavo F. Padovan 1170525cd185SGustavo F. Padovan static void l2cap_send_ack(struct l2cap_chan *chan) 11710a708f8fSGustavo F. Padovan { 11720a708f8fSGustavo F. Padovan u16 control = 0; 11730a708f8fSGustavo F. Padovan 117442e5c802SGustavo F. Padovan control |= chan->buffer_seq << L2CAP_CTRL_REQSEQ_SHIFT; 11750a708f8fSGustavo F. Padovan 1176525cd185SGustavo F. Padovan if (chan->conn_state & L2CAP_CONN_LOCAL_BUSY) { 11770a708f8fSGustavo F. Padovan control |= L2CAP_SUPER_RCV_NOT_READY; 1178525cd185SGustavo F. Padovan chan->conn_state |= L2CAP_CONN_RNR_SENT; 1179525cd185SGustavo F. Padovan l2cap_send_sframe(chan, control); 11800a708f8fSGustavo F. Padovan return; 11810a708f8fSGustavo F. Padovan } 11820a708f8fSGustavo F. Padovan 1183525cd185SGustavo F. Padovan if (l2cap_ertm_send(chan) > 0) 11840a708f8fSGustavo F. Padovan return; 11850a708f8fSGustavo F. Padovan 11860a708f8fSGustavo F. Padovan control |= L2CAP_SUPER_RCV_READY; 1187525cd185SGustavo F. Padovan l2cap_send_sframe(chan, control); 11880a708f8fSGustavo F. Padovan } 11890a708f8fSGustavo F. Padovan 1190525cd185SGustavo F. Padovan static void l2cap_send_srejtail(struct l2cap_chan *chan) 11910a708f8fSGustavo F. Padovan { 11920a708f8fSGustavo F. Padovan struct srej_list *tail; 11930a708f8fSGustavo F. Padovan u16 control; 11940a708f8fSGustavo F. Padovan 11950a708f8fSGustavo F. Padovan control = L2CAP_SUPER_SELECT_REJECT; 11960a708f8fSGustavo F. Padovan control |= L2CAP_CTRL_FINAL; 11970a708f8fSGustavo F. Padovan 119839d5a3eeSGustavo F. Padovan tail = list_entry((&chan->srej_l)->prev, struct srej_list, list); 11990a708f8fSGustavo F. Padovan control |= tail->tx_seq << L2CAP_CTRL_REQSEQ_SHIFT; 12000a708f8fSGustavo F. Padovan 1201525cd185SGustavo F. Padovan l2cap_send_sframe(chan, control); 12020a708f8fSGustavo F. Padovan } 12030a708f8fSGustavo F. Padovan 12040a708f8fSGustavo F. Padovan static inline int l2cap_skbuff_fromiovec(struct sock *sk, struct msghdr *msg, int len, int count, struct sk_buff *skb) 12050a708f8fSGustavo F. Padovan { 12060a708f8fSGustavo F. Padovan struct l2cap_conn *conn = l2cap_pi(sk)->conn; 12070a708f8fSGustavo F. Padovan struct sk_buff **frag; 12080a708f8fSGustavo F. Padovan int err, sent = 0; 12090a708f8fSGustavo F. Padovan 12100a708f8fSGustavo F. Padovan if (memcpy_fromiovec(skb_put(skb, count), msg->msg_iov, count)) 12110a708f8fSGustavo F. Padovan return -EFAULT; 12120a708f8fSGustavo F. Padovan 12130a708f8fSGustavo F. Padovan sent += count; 12140a708f8fSGustavo F. Padovan len -= count; 12150a708f8fSGustavo F. Padovan 12160a708f8fSGustavo F. Padovan /* Continuation fragments (no L2CAP header) */ 12170a708f8fSGustavo F. Padovan frag = &skb_shinfo(skb)->frag_list; 12180a708f8fSGustavo F. Padovan while (len) { 12190a708f8fSGustavo F. Padovan count = min_t(unsigned int, conn->mtu, len); 12200a708f8fSGustavo F. Padovan 12210a708f8fSGustavo F. Padovan *frag = bt_skb_send_alloc(sk, count, msg->msg_flags & MSG_DONTWAIT, &err); 12220a708f8fSGustavo F. Padovan if (!*frag) 12230a708f8fSGustavo F. Padovan return err; 12240a708f8fSGustavo F. Padovan if (memcpy_fromiovec(skb_put(*frag, count), msg->msg_iov, count)) 12250a708f8fSGustavo F. Padovan return -EFAULT; 12260a708f8fSGustavo F. Padovan 12270a708f8fSGustavo F. Padovan sent += count; 12280a708f8fSGustavo F. Padovan len -= count; 12290a708f8fSGustavo F. Padovan 12300a708f8fSGustavo F. Padovan frag = &(*frag)->next; 12310a708f8fSGustavo F. Padovan } 12320a708f8fSGustavo F. Padovan 12330a708f8fSGustavo F. Padovan return sent; 12340a708f8fSGustavo F. Padovan } 12350a708f8fSGustavo F. Padovan 1236fd83ccdbSGustavo F. Padovan struct sk_buff *l2cap_create_connless_pdu(struct sock *sk, struct msghdr *msg, size_t len) 12370a708f8fSGustavo F. Padovan { 12380a708f8fSGustavo F. Padovan struct l2cap_conn *conn = l2cap_pi(sk)->conn; 12390a708f8fSGustavo F. Padovan struct sk_buff *skb; 12400a708f8fSGustavo F. Padovan int err, count, hlen = L2CAP_HDR_SIZE + 2; 12410a708f8fSGustavo F. Padovan struct l2cap_hdr *lh; 12420a708f8fSGustavo F. Padovan 12430a708f8fSGustavo F. Padovan BT_DBG("sk %p len %d", sk, (int)len); 12440a708f8fSGustavo F. Padovan 12450a708f8fSGustavo F. Padovan count = min_t(unsigned int, (conn->mtu - hlen), len); 12460a708f8fSGustavo F. Padovan skb = bt_skb_send_alloc(sk, count + hlen, 12470a708f8fSGustavo F. Padovan msg->msg_flags & MSG_DONTWAIT, &err); 12480a708f8fSGustavo F. Padovan if (!skb) 12490a708f8fSGustavo F. Padovan return ERR_PTR(err); 12500a708f8fSGustavo F. Padovan 12510a708f8fSGustavo F. Padovan /* Create L2CAP header */ 12520a708f8fSGustavo F. Padovan lh = (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE); 12530a708f8fSGustavo F. Padovan lh->cid = cpu_to_le16(l2cap_pi(sk)->dcid); 12540a708f8fSGustavo F. Padovan lh->len = cpu_to_le16(len + (hlen - L2CAP_HDR_SIZE)); 12550a708f8fSGustavo F. Padovan put_unaligned_le16(l2cap_pi(sk)->psm, skb_put(skb, 2)); 12560a708f8fSGustavo F. Padovan 12570a708f8fSGustavo F. Padovan err = l2cap_skbuff_fromiovec(sk, msg, len, count, skb); 12580a708f8fSGustavo F. Padovan if (unlikely(err < 0)) { 12590a708f8fSGustavo F. Padovan kfree_skb(skb); 12600a708f8fSGustavo F. Padovan return ERR_PTR(err); 12610a708f8fSGustavo F. Padovan } 12620a708f8fSGustavo F. Padovan return skb; 12630a708f8fSGustavo F. Padovan } 12640a708f8fSGustavo F. Padovan 1265fd83ccdbSGustavo F. Padovan struct sk_buff *l2cap_create_basic_pdu(struct sock *sk, struct msghdr *msg, size_t len) 12660a708f8fSGustavo F. Padovan { 12670a708f8fSGustavo F. Padovan struct l2cap_conn *conn = l2cap_pi(sk)->conn; 12680a708f8fSGustavo F. Padovan struct sk_buff *skb; 12690a708f8fSGustavo F. Padovan int err, count, hlen = L2CAP_HDR_SIZE; 12700a708f8fSGustavo F. Padovan struct l2cap_hdr *lh; 12710a708f8fSGustavo F. Padovan 12720a708f8fSGustavo F. Padovan BT_DBG("sk %p len %d", sk, (int)len); 12730a708f8fSGustavo F. Padovan 12740a708f8fSGustavo F. Padovan count = min_t(unsigned int, (conn->mtu - hlen), len); 12750a708f8fSGustavo F. Padovan skb = bt_skb_send_alloc(sk, count + hlen, 12760a708f8fSGustavo F. Padovan msg->msg_flags & MSG_DONTWAIT, &err); 12770a708f8fSGustavo F. Padovan if (!skb) 12780a708f8fSGustavo F. Padovan return ERR_PTR(err); 12790a708f8fSGustavo F. Padovan 12800a708f8fSGustavo F. Padovan /* Create L2CAP header */ 12810a708f8fSGustavo F. Padovan lh = (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE); 12820a708f8fSGustavo F. Padovan lh->cid = cpu_to_le16(l2cap_pi(sk)->dcid); 12830a708f8fSGustavo F. Padovan lh->len = cpu_to_le16(len + (hlen - L2CAP_HDR_SIZE)); 12840a708f8fSGustavo F. Padovan 12850a708f8fSGustavo F. Padovan err = l2cap_skbuff_fromiovec(sk, msg, len, count, skb); 12860a708f8fSGustavo F. Padovan if (unlikely(err < 0)) { 12870a708f8fSGustavo F. Padovan kfree_skb(skb); 12880a708f8fSGustavo F. Padovan return ERR_PTR(err); 12890a708f8fSGustavo F. Padovan } 12900a708f8fSGustavo F. Padovan return skb; 12910a708f8fSGustavo F. Padovan } 12920a708f8fSGustavo F. Padovan 1293fd83ccdbSGustavo F. Padovan struct sk_buff *l2cap_create_iframe_pdu(struct sock *sk, struct msghdr *msg, size_t len, u16 control, u16 sdulen) 12940a708f8fSGustavo F. Padovan { 12950a708f8fSGustavo F. Padovan struct l2cap_conn *conn = l2cap_pi(sk)->conn; 12960a708f8fSGustavo F. Padovan struct sk_buff *skb; 12970a708f8fSGustavo F. Padovan int err, count, hlen = L2CAP_HDR_SIZE + 2; 12980a708f8fSGustavo F. Padovan struct l2cap_hdr *lh; 12990a708f8fSGustavo F. Padovan 13000a708f8fSGustavo F. Padovan BT_DBG("sk %p len %d", sk, (int)len); 13010a708f8fSGustavo F. Padovan 13020a708f8fSGustavo F. Padovan if (!conn) 13030a708f8fSGustavo F. Padovan return ERR_PTR(-ENOTCONN); 13040a708f8fSGustavo F. Padovan 13050a708f8fSGustavo F. Padovan if (sdulen) 13060a708f8fSGustavo F. Padovan hlen += 2; 13070a708f8fSGustavo F. Padovan 13080a708f8fSGustavo F. Padovan if (l2cap_pi(sk)->fcs == L2CAP_FCS_CRC16) 13090a708f8fSGustavo F. Padovan hlen += 2; 13100a708f8fSGustavo F. Padovan 13110a708f8fSGustavo F. Padovan count = min_t(unsigned int, (conn->mtu - hlen), len); 13120a708f8fSGustavo F. Padovan skb = bt_skb_send_alloc(sk, count + hlen, 13130a708f8fSGustavo F. Padovan msg->msg_flags & MSG_DONTWAIT, &err); 13140a708f8fSGustavo F. Padovan if (!skb) 13150a708f8fSGustavo F. Padovan return ERR_PTR(err); 13160a708f8fSGustavo F. Padovan 13170a708f8fSGustavo F. Padovan /* Create L2CAP header */ 13180a708f8fSGustavo F. Padovan lh = (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE); 13190a708f8fSGustavo F. Padovan lh->cid = cpu_to_le16(l2cap_pi(sk)->dcid); 13200a708f8fSGustavo F. Padovan lh->len = cpu_to_le16(len + (hlen - L2CAP_HDR_SIZE)); 13210a708f8fSGustavo F. Padovan put_unaligned_le16(control, skb_put(skb, 2)); 13220a708f8fSGustavo F. Padovan if (sdulen) 13230a708f8fSGustavo F. Padovan put_unaligned_le16(sdulen, skb_put(skb, 2)); 13240a708f8fSGustavo F. Padovan 13250a708f8fSGustavo F. Padovan err = l2cap_skbuff_fromiovec(sk, msg, len, count, skb); 13260a708f8fSGustavo F. Padovan if (unlikely(err < 0)) { 13270a708f8fSGustavo F. Padovan kfree_skb(skb); 13280a708f8fSGustavo F. Padovan return ERR_PTR(err); 13290a708f8fSGustavo F. Padovan } 13300a708f8fSGustavo F. Padovan 13310a708f8fSGustavo F. Padovan if (l2cap_pi(sk)->fcs == L2CAP_FCS_CRC16) 13320a708f8fSGustavo F. Padovan put_unaligned_le16(0, skb_put(skb, 2)); 13330a708f8fSGustavo F. Padovan 13340a708f8fSGustavo F. Padovan bt_cb(skb)->retries = 0; 13350a708f8fSGustavo F. Padovan return skb; 13360a708f8fSGustavo F. Padovan } 13370a708f8fSGustavo F. Padovan 13382c03a7a4SGustavo F. Padovan int l2cap_sar_segment_sdu(struct l2cap_chan *chan, struct msghdr *msg, size_t len) 13390a708f8fSGustavo F. Padovan { 13402c03a7a4SGustavo F. Padovan struct sock *sk = chan->sk; 13410a708f8fSGustavo F. Padovan struct sk_buff *skb; 13420a708f8fSGustavo F. Padovan struct sk_buff_head sar_queue; 13430a708f8fSGustavo F. Padovan u16 control; 13440a708f8fSGustavo F. Padovan size_t size = 0; 13450a708f8fSGustavo F. Padovan 13460a708f8fSGustavo F. Padovan skb_queue_head_init(&sar_queue); 13470a708f8fSGustavo F. Padovan control = L2CAP_SDU_START; 13482c03a7a4SGustavo F. Padovan skb = l2cap_create_iframe_pdu(sk, msg, chan->remote_mps, control, len); 13490a708f8fSGustavo F. Padovan if (IS_ERR(skb)) 13500a708f8fSGustavo F. Padovan return PTR_ERR(skb); 13510a708f8fSGustavo F. Padovan 13520a708f8fSGustavo F. Padovan __skb_queue_tail(&sar_queue, skb); 13532c03a7a4SGustavo F. Padovan len -= chan->remote_mps; 13542c03a7a4SGustavo F. Padovan size += chan->remote_mps; 13550a708f8fSGustavo F. Padovan 13560a708f8fSGustavo F. Padovan while (len > 0) { 13570a708f8fSGustavo F. Padovan size_t buflen; 13580a708f8fSGustavo F. Padovan 13592c03a7a4SGustavo F. Padovan if (len > chan->remote_mps) { 13600a708f8fSGustavo F. Padovan control = L2CAP_SDU_CONTINUE; 13612c03a7a4SGustavo F. Padovan buflen = chan->remote_mps; 13620a708f8fSGustavo F. Padovan } else { 13630a708f8fSGustavo F. Padovan control = L2CAP_SDU_END; 13640a708f8fSGustavo F. Padovan buflen = len; 13650a708f8fSGustavo F. Padovan } 13660a708f8fSGustavo F. Padovan 13670a708f8fSGustavo F. Padovan skb = l2cap_create_iframe_pdu(sk, msg, buflen, control, 0); 13680a708f8fSGustavo F. Padovan if (IS_ERR(skb)) { 13690a708f8fSGustavo F. Padovan skb_queue_purge(&sar_queue); 13700a708f8fSGustavo F. Padovan return PTR_ERR(skb); 13710a708f8fSGustavo F. Padovan } 13720a708f8fSGustavo F. Padovan 13730a708f8fSGustavo F. Padovan __skb_queue_tail(&sar_queue, skb); 13740a708f8fSGustavo F. Padovan len -= buflen; 13750a708f8fSGustavo F. Padovan size += buflen; 13760a708f8fSGustavo F. Padovan } 137758d35f87SGustavo F. Padovan skb_queue_splice_tail(&sar_queue, &chan->tx_q); 137858d35f87SGustavo F. Padovan if (chan->tx_send_head == NULL) 137958d35f87SGustavo F. Padovan chan->tx_send_head = sar_queue.next; 13800a708f8fSGustavo F. Padovan 13810a708f8fSGustavo F. Padovan return size; 13820a708f8fSGustavo F. Padovan } 13830a708f8fSGustavo F. Padovan 13840a708f8fSGustavo F. Padovan static void l2cap_chan_ready(struct sock *sk) 13850a708f8fSGustavo F. Padovan { 13860a708f8fSGustavo F. Padovan struct sock *parent = bt_sk(sk)->parent; 1387b4450035SGustavo F. Padovan struct l2cap_chan *chan = l2cap_pi(sk)->chan; 13880a708f8fSGustavo F. Padovan 13890a708f8fSGustavo F. Padovan BT_DBG("sk %p, parent %p", sk, parent); 13900a708f8fSGustavo F. Padovan 1391b4450035SGustavo F. Padovan chan->conf_state = 0; 13920a708f8fSGustavo F. Padovan l2cap_sock_clear_timer(sk); 13930a708f8fSGustavo F. Padovan 13940a708f8fSGustavo F. Padovan if (!parent) { 13950a708f8fSGustavo F. Padovan /* Outgoing channel. 13960a708f8fSGustavo F. Padovan * Wake up socket sleeping on connect. 13970a708f8fSGustavo F. Padovan */ 13980a708f8fSGustavo F. Padovan sk->sk_state = BT_CONNECTED; 13990a708f8fSGustavo F. Padovan sk->sk_state_change(sk); 14000a708f8fSGustavo F. Padovan } else { 14010a708f8fSGustavo F. Padovan /* Incoming channel. 14020a708f8fSGustavo F. Padovan * Wake up socket sleeping on accept. 14030a708f8fSGustavo F. Padovan */ 14040a708f8fSGustavo F. Padovan parent->sk_data_ready(parent, 0); 14050a708f8fSGustavo F. Padovan } 14060a708f8fSGustavo F. Padovan } 14070a708f8fSGustavo F. Padovan 14080a708f8fSGustavo F. Padovan /* Copy frame to all raw sockets on that connection */ 14090a708f8fSGustavo F. Padovan static void l2cap_raw_recv(struct l2cap_conn *conn, struct sk_buff *skb) 14100a708f8fSGustavo F. Padovan { 14110a708f8fSGustavo F. Padovan struct sk_buff *nskb; 141248454079SGustavo F. Padovan struct l2cap_chan *chan; 14130a708f8fSGustavo F. Padovan 14140a708f8fSGustavo F. Padovan BT_DBG("conn %p", conn); 14150a708f8fSGustavo F. Padovan 1416baa7e1faSGustavo F. Padovan read_lock(&conn->chan_lock); 1417baa7e1faSGustavo F. Padovan list_for_each_entry(chan, &conn->chan_l, list) { 141848454079SGustavo F. Padovan struct sock *sk = chan->sk; 14190a708f8fSGustavo F. Padovan if (sk->sk_type != SOCK_RAW) 14200a708f8fSGustavo F. Padovan continue; 14210a708f8fSGustavo F. Padovan 14220a708f8fSGustavo F. Padovan /* Don't send frame to the socket it came from */ 14230a708f8fSGustavo F. Padovan if (skb->sk == sk) 14240a708f8fSGustavo F. Padovan continue; 14250a708f8fSGustavo F. Padovan nskb = skb_clone(skb, GFP_ATOMIC); 14260a708f8fSGustavo F. Padovan if (!nskb) 14270a708f8fSGustavo F. Padovan continue; 14280a708f8fSGustavo F. Padovan 14290a708f8fSGustavo F. Padovan if (sock_queue_rcv_skb(sk, nskb)) 14300a708f8fSGustavo F. Padovan kfree_skb(nskb); 14310a708f8fSGustavo F. Padovan } 1432baa7e1faSGustavo F. Padovan read_unlock(&conn->chan_lock); 14330a708f8fSGustavo F. Padovan } 14340a708f8fSGustavo F. Padovan 14350a708f8fSGustavo F. Padovan /* ---- L2CAP signalling commands ---- */ 14360a708f8fSGustavo F. Padovan static struct sk_buff *l2cap_build_cmd(struct l2cap_conn *conn, 14370a708f8fSGustavo F. Padovan u8 code, u8 ident, u16 dlen, void *data) 14380a708f8fSGustavo F. Padovan { 14390a708f8fSGustavo F. Padovan struct sk_buff *skb, **frag; 14400a708f8fSGustavo F. Padovan struct l2cap_cmd_hdr *cmd; 14410a708f8fSGustavo F. Padovan struct l2cap_hdr *lh; 14420a708f8fSGustavo F. Padovan int len, count; 14430a708f8fSGustavo F. Padovan 14440a708f8fSGustavo F. Padovan BT_DBG("conn %p, code 0x%2.2x, ident 0x%2.2x, len %d", 14450a708f8fSGustavo F. Padovan conn, code, ident, dlen); 14460a708f8fSGustavo F. Padovan 14470a708f8fSGustavo F. Padovan len = L2CAP_HDR_SIZE + L2CAP_CMD_HDR_SIZE + dlen; 14480a708f8fSGustavo F. Padovan count = min_t(unsigned int, conn->mtu, len); 14490a708f8fSGustavo F. Padovan 14500a708f8fSGustavo F. Padovan skb = bt_skb_alloc(count, GFP_ATOMIC); 14510a708f8fSGustavo F. Padovan if (!skb) 14520a708f8fSGustavo F. Padovan return NULL; 14530a708f8fSGustavo F. Padovan 14540a708f8fSGustavo F. Padovan lh = (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE); 14550a708f8fSGustavo F. Padovan lh->len = cpu_to_le16(L2CAP_CMD_HDR_SIZE + dlen); 14563300d9a9SClaudio Takahasi 14573300d9a9SClaudio Takahasi if (conn->hcon->type == LE_LINK) 14583300d9a9SClaudio Takahasi lh->cid = cpu_to_le16(L2CAP_CID_LE_SIGNALING); 14593300d9a9SClaudio Takahasi else 14600a708f8fSGustavo F. Padovan lh->cid = cpu_to_le16(L2CAP_CID_SIGNALING); 14610a708f8fSGustavo F. Padovan 14620a708f8fSGustavo F. Padovan cmd = (struct l2cap_cmd_hdr *) skb_put(skb, L2CAP_CMD_HDR_SIZE); 14630a708f8fSGustavo F. Padovan cmd->code = code; 14640a708f8fSGustavo F. Padovan cmd->ident = ident; 14650a708f8fSGustavo F. Padovan cmd->len = cpu_to_le16(dlen); 14660a708f8fSGustavo F. Padovan 14670a708f8fSGustavo F. Padovan if (dlen) { 14680a708f8fSGustavo F. Padovan count -= L2CAP_HDR_SIZE + L2CAP_CMD_HDR_SIZE; 14690a708f8fSGustavo F. Padovan memcpy(skb_put(skb, count), data, count); 14700a708f8fSGustavo F. Padovan data += count; 14710a708f8fSGustavo F. Padovan } 14720a708f8fSGustavo F. Padovan 14730a708f8fSGustavo F. Padovan len -= skb->len; 14740a708f8fSGustavo F. Padovan 14750a708f8fSGustavo F. Padovan /* Continuation fragments (no L2CAP header) */ 14760a708f8fSGustavo F. Padovan frag = &skb_shinfo(skb)->frag_list; 14770a708f8fSGustavo F. Padovan while (len) { 14780a708f8fSGustavo F. Padovan count = min_t(unsigned int, conn->mtu, len); 14790a708f8fSGustavo F. Padovan 14800a708f8fSGustavo F. Padovan *frag = bt_skb_alloc(count, GFP_ATOMIC); 14810a708f8fSGustavo F. Padovan if (!*frag) 14820a708f8fSGustavo F. Padovan goto fail; 14830a708f8fSGustavo F. Padovan 14840a708f8fSGustavo F. Padovan memcpy(skb_put(*frag, count), data, count); 14850a708f8fSGustavo F. Padovan 14860a708f8fSGustavo F. Padovan len -= count; 14870a708f8fSGustavo F. Padovan data += count; 14880a708f8fSGustavo F. Padovan 14890a708f8fSGustavo F. Padovan frag = &(*frag)->next; 14900a708f8fSGustavo F. Padovan } 14910a708f8fSGustavo F. Padovan 14920a708f8fSGustavo F. Padovan return skb; 14930a708f8fSGustavo F. Padovan 14940a708f8fSGustavo F. Padovan fail: 14950a708f8fSGustavo F. Padovan kfree_skb(skb); 14960a708f8fSGustavo F. Padovan return NULL; 14970a708f8fSGustavo F. Padovan } 14980a708f8fSGustavo F. Padovan 14990a708f8fSGustavo F. Padovan static inline int l2cap_get_conf_opt(void **ptr, int *type, int *olen, unsigned long *val) 15000a708f8fSGustavo F. Padovan { 15010a708f8fSGustavo F. Padovan struct l2cap_conf_opt *opt = *ptr; 15020a708f8fSGustavo F. Padovan int len; 15030a708f8fSGustavo F. Padovan 15040a708f8fSGustavo F. Padovan len = L2CAP_CONF_OPT_SIZE + opt->len; 15050a708f8fSGustavo F. Padovan *ptr += len; 15060a708f8fSGustavo F. Padovan 15070a708f8fSGustavo F. Padovan *type = opt->type; 15080a708f8fSGustavo F. Padovan *olen = opt->len; 15090a708f8fSGustavo F. Padovan 15100a708f8fSGustavo F. Padovan switch (opt->len) { 15110a708f8fSGustavo F. Padovan case 1: 15120a708f8fSGustavo F. Padovan *val = *((u8 *) opt->val); 15130a708f8fSGustavo F. Padovan break; 15140a708f8fSGustavo F. Padovan 15150a708f8fSGustavo F. Padovan case 2: 15160a708f8fSGustavo F. Padovan *val = get_unaligned_le16(opt->val); 15170a708f8fSGustavo F. Padovan break; 15180a708f8fSGustavo F. Padovan 15190a708f8fSGustavo F. Padovan case 4: 15200a708f8fSGustavo F. Padovan *val = get_unaligned_le32(opt->val); 15210a708f8fSGustavo F. Padovan break; 15220a708f8fSGustavo F. Padovan 15230a708f8fSGustavo F. Padovan default: 15240a708f8fSGustavo F. Padovan *val = (unsigned long) opt->val; 15250a708f8fSGustavo F. Padovan break; 15260a708f8fSGustavo F. Padovan } 15270a708f8fSGustavo F. Padovan 15280a708f8fSGustavo F. Padovan BT_DBG("type 0x%2.2x len %d val 0x%lx", *type, opt->len, *val); 15290a708f8fSGustavo F. Padovan return len; 15300a708f8fSGustavo F. Padovan } 15310a708f8fSGustavo F. Padovan 15320a708f8fSGustavo F. Padovan static void l2cap_add_conf_opt(void **ptr, u8 type, u8 len, unsigned long val) 15330a708f8fSGustavo F. Padovan { 15340a708f8fSGustavo F. Padovan struct l2cap_conf_opt *opt = *ptr; 15350a708f8fSGustavo F. Padovan 15360a708f8fSGustavo F. Padovan BT_DBG("type 0x%2.2x len %d val 0x%lx", type, len, val); 15370a708f8fSGustavo F. Padovan 15380a708f8fSGustavo F. Padovan opt->type = type; 15390a708f8fSGustavo F. Padovan opt->len = len; 15400a708f8fSGustavo F. Padovan 15410a708f8fSGustavo F. Padovan switch (len) { 15420a708f8fSGustavo F. Padovan case 1: 15430a708f8fSGustavo F. Padovan *((u8 *) opt->val) = val; 15440a708f8fSGustavo F. Padovan break; 15450a708f8fSGustavo F. Padovan 15460a708f8fSGustavo F. Padovan case 2: 15470a708f8fSGustavo F. Padovan put_unaligned_le16(val, opt->val); 15480a708f8fSGustavo F. Padovan break; 15490a708f8fSGustavo F. Padovan 15500a708f8fSGustavo F. Padovan case 4: 15510a708f8fSGustavo F. Padovan put_unaligned_le32(val, opt->val); 15520a708f8fSGustavo F. Padovan break; 15530a708f8fSGustavo F. Padovan 15540a708f8fSGustavo F. Padovan default: 15550a708f8fSGustavo F. Padovan memcpy(opt->val, (void *) val, len); 15560a708f8fSGustavo F. Padovan break; 15570a708f8fSGustavo F. Padovan } 15580a708f8fSGustavo F. Padovan 15590a708f8fSGustavo F. Padovan *ptr += L2CAP_CONF_OPT_SIZE + len; 15600a708f8fSGustavo F. Padovan } 15610a708f8fSGustavo F. Padovan 15620a708f8fSGustavo F. Padovan static void l2cap_ack_timeout(unsigned long arg) 15630a708f8fSGustavo F. Padovan { 1564525cd185SGustavo F. Padovan struct l2cap_chan *chan = (void *) arg; 15650a708f8fSGustavo F. Padovan 1566525cd185SGustavo F. Padovan bh_lock_sock(chan->sk); 1567525cd185SGustavo F. Padovan l2cap_send_ack(chan); 1568525cd185SGustavo F. Padovan bh_unlock_sock(chan->sk); 15690a708f8fSGustavo F. Padovan } 15700a708f8fSGustavo F. Padovan 1571525cd185SGustavo F. Padovan static inline void l2cap_ertm_init(struct l2cap_chan *chan) 15720a708f8fSGustavo F. Padovan { 1573525cd185SGustavo F. Padovan struct sock *sk = chan->sk; 1574525cd185SGustavo F. Padovan 157542e5c802SGustavo F. Padovan chan->expected_ack_seq = 0; 15766a026610SGustavo F. Padovan chan->unacked_frames = 0; 157742e5c802SGustavo F. Padovan chan->buffer_seq = 0; 15786a026610SGustavo F. Padovan chan->num_acked = 0; 15796a026610SGustavo F. Padovan chan->frames_sent = 0; 15800a708f8fSGustavo F. Padovan 1581e92c8e70SGustavo F. Padovan setup_timer(&chan->retrans_timer, l2cap_retrans_timeout, 1582e92c8e70SGustavo F. Padovan (unsigned long) chan); 1583e92c8e70SGustavo F. Padovan setup_timer(&chan->monitor_timer, l2cap_monitor_timeout, 1584e92c8e70SGustavo F. Padovan (unsigned long) chan); 1585e92c8e70SGustavo F. Padovan setup_timer(&chan->ack_timer, l2cap_ack_timeout, (unsigned long) chan); 15860a708f8fSGustavo F. Padovan 1587f1c6775bSGustavo F. Padovan skb_queue_head_init(&chan->srej_q); 1588f1c6775bSGustavo F. Padovan skb_queue_head_init(&chan->busy_q); 15890a708f8fSGustavo F. Padovan 159039d5a3eeSGustavo F. Padovan INIT_LIST_HEAD(&chan->srej_l); 159139d5a3eeSGustavo F. Padovan 1592311bb895SGustavo F. Padovan INIT_WORK(&chan->busy_work, l2cap_busy_work); 15930a708f8fSGustavo F. Padovan 15940a708f8fSGustavo F. Padovan sk->sk_backlog_rcv = l2cap_ertm_data_rcv; 15950a708f8fSGustavo F. Padovan } 15960a708f8fSGustavo F. Padovan 15970a708f8fSGustavo F. Padovan static inline __u8 l2cap_select_mode(__u8 mode, __u16 remote_feat_mask) 15980a708f8fSGustavo F. Padovan { 15990a708f8fSGustavo F. Padovan switch (mode) { 16000a708f8fSGustavo F. Padovan case L2CAP_MODE_STREAMING: 16010a708f8fSGustavo F. Padovan case L2CAP_MODE_ERTM: 16020a708f8fSGustavo F. Padovan if (l2cap_mode_supported(mode, remote_feat_mask)) 16030a708f8fSGustavo F. Padovan return mode; 16040a708f8fSGustavo F. Padovan /* fall through */ 16050a708f8fSGustavo F. Padovan default: 16060a708f8fSGustavo F. Padovan return L2CAP_MODE_BASIC; 16070a708f8fSGustavo F. Padovan } 16080a708f8fSGustavo F. Padovan } 16090a708f8fSGustavo F. Padovan 1610710f9b0aSGustavo F. Padovan static int l2cap_build_conf_req(struct l2cap_chan *chan, void *data) 16110a708f8fSGustavo F. Padovan { 161249208c9cSGustavo F. Padovan struct l2cap_pinfo *pi = l2cap_pi(chan->sk); 16130a708f8fSGustavo F. Padovan struct l2cap_conf_req *req = data; 16140a708f8fSGustavo F. Padovan struct l2cap_conf_rfc rfc = { .mode = pi->mode }; 16150a708f8fSGustavo F. Padovan void *ptr = req->data; 16160a708f8fSGustavo F. Padovan 161749208c9cSGustavo F. Padovan BT_DBG("chan %p", chan); 16180a708f8fSGustavo F. Padovan 161973ffa904SGustavo F. Padovan if (chan->num_conf_req || chan->num_conf_rsp) 16200a708f8fSGustavo F. Padovan goto done; 16210a708f8fSGustavo F. Padovan 16220a708f8fSGustavo F. Padovan switch (pi->mode) { 16230a708f8fSGustavo F. Padovan case L2CAP_MODE_STREAMING: 16240a708f8fSGustavo F. Padovan case L2CAP_MODE_ERTM: 1625b4450035SGustavo F. Padovan if (chan->conf_state & L2CAP_CONF_STATE2_DEVICE) 16260a708f8fSGustavo F. Padovan break; 16270a708f8fSGustavo F. Padovan 16280a708f8fSGustavo F. Padovan /* fall through */ 16290a708f8fSGustavo F. Padovan default: 16300a708f8fSGustavo F. Padovan pi->mode = l2cap_select_mode(rfc.mode, pi->conn->feat_mask); 16310a708f8fSGustavo F. Padovan break; 16320a708f8fSGustavo F. Padovan } 16330a708f8fSGustavo F. Padovan 16340a708f8fSGustavo F. Padovan done: 16350a708f8fSGustavo F. Padovan if (pi->imtu != L2CAP_DEFAULT_MTU) 16360a708f8fSGustavo F. Padovan l2cap_add_conf_opt(&ptr, L2CAP_CONF_MTU, 2, pi->imtu); 16370a708f8fSGustavo F. Padovan 16380a708f8fSGustavo F. Padovan switch (pi->mode) { 16390a708f8fSGustavo F. Padovan case L2CAP_MODE_BASIC: 16400a708f8fSGustavo F. Padovan if (!(pi->conn->feat_mask & L2CAP_FEAT_ERTM) && 16410a708f8fSGustavo F. Padovan !(pi->conn->feat_mask & L2CAP_FEAT_STREAMING)) 16420a708f8fSGustavo F. Padovan break; 16430a708f8fSGustavo F. Padovan 16440a708f8fSGustavo F. Padovan rfc.mode = L2CAP_MODE_BASIC; 16450a708f8fSGustavo F. Padovan rfc.txwin_size = 0; 16460a708f8fSGustavo F. Padovan rfc.max_transmit = 0; 16470a708f8fSGustavo F. Padovan rfc.retrans_timeout = 0; 16480a708f8fSGustavo F. Padovan rfc.monitor_timeout = 0; 16490a708f8fSGustavo F. Padovan rfc.max_pdu_size = 0; 16500a708f8fSGustavo F. Padovan 16510a708f8fSGustavo F. Padovan l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC, sizeof(rfc), 16520a708f8fSGustavo F. Padovan (unsigned long) &rfc); 16530a708f8fSGustavo F. Padovan break; 16540a708f8fSGustavo F. Padovan 16550a708f8fSGustavo F. Padovan case L2CAP_MODE_ERTM: 16560a708f8fSGustavo F. Padovan rfc.mode = L2CAP_MODE_ERTM; 16570a708f8fSGustavo F. Padovan rfc.txwin_size = pi->tx_win; 16580a708f8fSGustavo F. Padovan rfc.max_transmit = pi->max_tx; 16590a708f8fSGustavo F. Padovan rfc.retrans_timeout = 0; 16600a708f8fSGustavo F. Padovan rfc.monitor_timeout = 0; 16610a708f8fSGustavo F. Padovan rfc.max_pdu_size = cpu_to_le16(L2CAP_DEFAULT_MAX_PDU_SIZE); 16620a708f8fSGustavo F. Padovan if (L2CAP_DEFAULT_MAX_PDU_SIZE > pi->conn->mtu - 10) 16630a708f8fSGustavo F. Padovan rfc.max_pdu_size = cpu_to_le16(pi->conn->mtu - 10); 16640a708f8fSGustavo F. Padovan 16650a708f8fSGustavo F. Padovan l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC, sizeof(rfc), 16660a708f8fSGustavo F. Padovan (unsigned long) &rfc); 16670a708f8fSGustavo F. Padovan 16680a708f8fSGustavo F. Padovan if (!(pi->conn->feat_mask & L2CAP_FEAT_FCS)) 16690a708f8fSGustavo F. Padovan break; 16700a708f8fSGustavo F. Padovan 16710a708f8fSGustavo F. Padovan if (pi->fcs == L2CAP_FCS_NONE || 1672b4450035SGustavo F. Padovan chan->conf_state & L2CAP_CONF_NO_FCS_RECV) { 16730a708f8fSGustavo F. Padovan pi->fcs = L2CAP_FCS_NONE; 16740a708f8fSGustavo F. Padovan l2cap_add_conf_opt(&ptr, L2CAP_CONF_FCS, 1, pi->fcs); 16750a708f8fSGustavo F. Padovan } 16760a708f8fSGustavo F. Padovan break; 16770a708f8fSGustavo F. Padovan 16780a708f8fSGustavo F. Padovan case L2CAP_MODE_STREAMING: 16790a708f8fSGustavo F. Padovan rfc.mode = L2CAP_MODE_STREAMING; 16800a708f8fSGustavo F. Padovan rfc.txwin_size = 0; 16810a708f8fSGustavo F. Padovan rfc.max_transmit = 0; 16820a708f8fSGustavo F. Padovan rfc.retrans_timeout = 0; 16830a708f8fSGustavo F. Padovan rfc.monitor_timeout = 0; 16840a708f8fSGustavo F. Padovan rfc.max_pdu_size = cpu_to_le16(L2CAP_DEFAULT_MAX_PDU_SIZE); 16850a708f8fSGustavo F. Padovan if (L2CAP_DEFAULT_MAX_PDU_SIZE > pi->conn->mtu - 10) 16860a708f8fSGustavo F. Padovan rfc.max_pdu_size = cpu_to_le16(pi->conn->mtu - 10); 16870a708f8fSGustavo F. Padovan 16880a708f8fSGustavo F. Padovan l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC, sizeof(rfc), 16890a708f8fSGustavo F. Padovan (unsigned long) &rfc); 16900a708f8fSGustavo F. Padovan 16910a708f8fSGustavo F. Padovan if (!(pi->conn->feat_mask & L2CAP_FEAT_FCS)) 16920a708f8fSGustavo F. Padovan break; 16930a708f8fSGustavo F. Padovan 16940a708f8fSGustavo F. Padovan if (pi->fcs == L2CAP_FCS_NONE || 1695b4450035SGustavo F. Padovan chan->conf_state & L2CAP_CONF_NO_FCS_RECV) { 16960a708f8fSGustavo F. Padovan pi->fcs = L2CAP_FCS_NONE; 16970a708f8fSGustavo F. Padovan l2cap_add_conf_opt(&ptr, L2CAP_CONF_FCS, 1, pi->fcs); 16980a708f8fSGustavo F. Padovan } 16990a708f8fSGustavo F. Padovan break; 17000a708f8fSGustavo F. Padovan } 17010a708f8fSGustavo F. Padovan 17020a708f8fSGustavo F. Padovan req->dcid = cpu_to_le16(pi->dcid); 17030a708f8fSGustavo F. Padovan req->flags = cpu_to_le16(0); 17040a708f8fSGustavo F. Padovan 17050a708f8fSGustavo F. Padovan return ptr - data; 17060a708f8fSGustavo F. Padovan } 17070a708f8fSGustavo F. Padovan 170873ffa904SGustavo F. Padovan static int l2cap_parse_conf_req(struct l2cap_chan *chan, void *data) 17090a708f8fSGustavo F. Padovan { 171073ffa904SGustavo F. Padovan struct l2cap_pinfo *pi = l2cap_pi(chan->sk); 17110a708f8fSGustavo F. Padovan struct l2cap_conf_rsp *rsp = data; 17120a708f8fSGustavo F. Padovan void *ptr = rsp->data; 171373ffa904SGustavo F. Padovan void *req = chan->conf_req; 171473ffa904SGustavo F. Padovan int len = chan->conf_len; 17150a708f8fSGustavo F. Padovan int type, hint, olen; 17160a708f8fSGustavo F. Padovan unsigned long val; 17170a708f8fSGustavo F. Padovan struct l2cap_conf_rfc rfc = { .mode = L2CAP_MODE_BASIC }; 17180a708f8fSGustavo F. Padovan u16 mtu = L2CAP_DEFAULT_MTU; 17190a708f8fSGustavo F. Padovan u16 result = L2CAP_CONF_SUCCESS; 17200a708f8fSGustavo F. Padovan 172173ffa904SGustavo F. Padovan BT_DBG("chan %p", chan); 17220a708f8fSGustavo F. Padovan 17230a708f8fSGustavo F. Padovan while (len >= L2CAP_CONF_OPT_SIZE) { 17240a708f8fSGustavo F. Padovan len -= l2cap_get_conf_opt(&req, &type, &olen, &val); 17250a708f8fSGustavo F. Padovan 17260a708f8fSGustavo F. Padovan hint = type & L2CAP_CONF_HINT; 17270a708f8fSGustavo F. Padovan type &= L2CAP_CONF_MASK; 17280a708f8fSGustavo F. Padovan 17290a708f8fSGustavo F. Padovan switch (type) { 17300a708f8fSGustavo F. Padovan case L2CAP_CONF_MTU: 17310a708f8fSGustavo F. Padovan mtu = val; 17320a708f8fSGustavo F. Padovan break; 17330a708f8fSGustavo F. Padovan 17340a708f8fSGustavo F. Padovan case L2CAP_CONF_FLUSH_TO: 17350a708f8fSGustavo F. Padovan pi->flush_to = val; 17360a708f8fSGustavo F. Padovan break; 17370a708f8fSGustavo F. Padovan 17380a708f8fSGustavo F. Padovan case L2CAP_CONF_QOS: 17390a708f8fSGustavo F. Padovan break; 17400a708f8fSGustavo F. Padovan 17410a708f8fSGustavo F. Padovan case L2CAP_CONF_RFC: 17420a708f8fSGustavo F. Padovan if (olen == sizeof(rfc)) 17430a708f8fSGustavo F. Padovan memcpy(&rfc, (void *) val, olen); 17440a708f8fSGustavo F. Padovan break; 17450a708f8fSGustavo F. Padovan 17460a708f8fSGustavo F. Padovan case L2CAP_CONF_FCS: 17470a708f8fSGustavo F. Padovan if (val == L2CAP_FCS_NONE) 1748b4450035SGustavo F. Padovan chan->conf_state |= L2CAP_CONF_NO_FCS_RECV; 17490a708f8fSGustavo F. Padovan 17500a708f8fSGustavo F. Padovan break; 17510a708f8fSGustavo F. Padovan 17520a708f8fSGustavo F. Padovan default: 17530a708f8fSGustavo F. Padovan if (hint) 17540a708f8fSGustavo F. Padovan break; 17550a708f8fSGustavo F. Padovan 17560a708f8fSGustavo F. Padovan result = L2CAP_CONF_UNKNOWN; 17570a708f8fSGustavo F. Padovan *((u8 *) ptr++) = type; 17580a708f8fSGustavo F. Padovan break; 17590a708f8fSGustavo F. Padovan } 17600a708f8fSGustavo F. Padovan } 17610a708f8fSGustavo F. Padovan 176273ffa904SGustavo F. Padovan if (chan->num_conf_rsp || chan->num_conf_req > 1) 17630a708f8fSGustavo F. Padovan goto done; 17640a708f8fSGustavo F. Padovan 17650a708f8fSGustavo F. Padovan switch (pi->mode) { 17660a708f8fSGustavo F. Padovan case L2CAP_MODE_STREAMING: 17670a708f8fSGustavo F. Padovan case L2CAP_MODE_ERTM: 1768b4450035SGustavo F. Padovan if (!(chan->conf_state & L2CAP_CONF_STATE2_DEVICE)) { 17690a708f8fSGustavo F. Padovan pi->mode = l2cap_select_mode(rfc.mode, 17700a708f8fSGustavo F. Padovan pi->conn->feat_mask); 17710a708f8fSGustavo F. Padovan break; 17720a708f8fSGustavo F. Padovan } 17730a708f8fSGustavo F. Padovan 17740a708f8fSGustavo F. Padovan if (pi->mode != rfc.mode) 17750a708f8fSGustavo F. Padovan return -ECONNREFUSED; 17760a708f8fSGustavo F. Padovan 17770a708f8fSGustavo F. Padovan break; 17780a708f8fSGustavo F. Padovan } 17790a708f8fSGustavo F. Padovan 17800a708f8fSGustavo F. Padovan done: 17810a708f8fSGustavo F. Padovan if (pi->mode != rfc.mode) { 17820a708f8fSGustavo F. Padovan result = L2CAP_CONF_UNACCEPT; 17830a708f8fSGustavo F. Padovan rfc.mode = pi->mode; 17840a708f8fSGustavo F. Padovan 178573ffa904SGustavo F. Padovan if (chan->num_conf_rsp == 1) 17860a708f8fSGustavo F. Padovan return -ECONNREFUSED; 17870a708f8fSGustavo F. Padovan 17880a708f8fSGustavo F. Padovan l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC, 17890a708f8fSGustavo F. Padovan sizeof(rfc), (unsigned long) &rfc); 17900a708f8fSGustavo F. Padovan } 17910a708f8fSGustavo F. Padovan 17920a708f8fSGustavo F. Padovan 17930a708f8fSGustavo F. Padovan if (result == L2CAP_CONF_SUCCESS) { 17940a708f8fSGustavo F. Padovan /* Configure output options and let the other side know 17950a708f8fSGustavo F. Padovan * which ones we don't like. */ 17960a708f8fSGustavo F. Padovan 17970a708f8fSGustavo F. Padovan if (mtu < L2CAP_DEFAULT_MIN_MTU) 17980a708f8fSGustavo F. Padovan result = L2CAP_CONF_UNACCEPT; 17990a708f8fSGustavo F. Padovan else { 18000a708f8fSGustavo F. Padovan pi->omtu = mtu; 1801b4450035SGustavo F. Padovan chan->conf_state |= L2CAP_CONF_MTU_DONE; 18020a708f8fSGustavo F. Padovan } 18030a708f8fSGustavo F. Padovan l2cap_add_conf_opt(&ptr, L2CAP_CONF_MTU, 2, pi->omtu); 18040a708f8fSGustavo F. Padovan 18050a708f8fSGustavo F. Padovan switch (rfc.mode) { 18060a708f8fSGustavo F. Padovan case L2CAP_MODE_BASIC: 18070a708f8fSGustavo F. Padovan pi->fcs = L2CAP_FCS_NONE; 1808b4450035SGustavo F. Padovan chan->conf_state |= L2CAP_CONF_MODE_DONE; 18090a708f8fSGustavo F. Padovan break; 18100a708f8fSGustavo F. Padovan 18110a708f8fSGustavo F. Padovan case L2CAP_MODE_ERTM: 18122c03a7a4SGustavo F. Padovan chan->remote_tx_win = rfc.txwin_size; 18132c03a7a4SGustavo F. Padovan chan->remote_max_tx = rfc.max_transmit; 18140a708f8fSGustavo F. Padovan 18150a708f8fSGustavo F. Padovan if (le16_to_cpu(rfc.max_pdu_size) > pi->conn->mtu - 10) 18160a708f8fSGustavo F. Padovan rfc.max_pdu_size = cpu_to_le16(pi->conn->mtu - 10); 18170a708f8fSGustavo F. Padovan 18182c03a7a4SGustavo F. Padovan chan->remote_mps = le16_to_cpu(rfc.max_pdu_size); 18190a708f8fSGustavo F. Padovan 18200a708f8fSGustavo F. Padovan rfc.retrans_timeout = 18210a708f8fSGustavo F. Padovan le16_to_cpu(L2CAP_DEFAULT_RETRANS_TO); 18220a708f8fSGustavo F. Padovan rfc.monitor_timeout = 18230a708f8fSGustavo F. Padovan le16_to_cpu(L2CAP_DEFAULT_MONITOR_TO); 18240a708f8fSGustavo F. Padovan 1825b4450035SGustavo F. Padovan chan->conf_state |= L2CAP_CONF_MODE_DONE; 18260a708f8fSGustavo F. Padovan 18270a708f8fSGustavo F. Padovan l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC, 18280a708f8fSGustavo F. Padovan sizeof(rfc), (unsigned long) &rfc); 18290a708f8fSGustavo F. Padovan 18300a708f8fSGustavo F. Padovan break; 18310a708f8fSGustavo F. Padovan 18320a708f8fSGustavo F. Padovan case L2CAP_MODE_STREAMING: 18330a708f8fSGustavo F. Padovan if (le16_to_cpu(rfc.max_pdu_size) > pi->conn->mtu - 10) 18340a708f8fSGustavo F. Padovan rfc.max_pdu_size = cpu_to_le16(pi->conn->mtu - 10); 18350a708f8fSGustavo F. Padovan 18362c03a7a4SGustavo F. Padovan chan->remote_mps = le16_to_cpu(rfc.max_pdu_size); 18370a708f8fSGustavo F. Padovan 1838b4450035SGustavo F. Padovan chan->conf_state |= L2CAP_CONF_MODE_DONE; 18390a708f8fSGustavo F. Padovan 18400a708f8fSGustavo F. Padovan l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC, 18410a708f8fSGustavo F. Padovan sizeof(rfc), (unsigned long) &rfc); 18420a708f8fSGustavo F. Padovan 18430a708f8fSGustavo F. Padovan break; 18440a708f8fSGustavo F. Padovan 18450a708f8fSGustavo F. Padovan default: 18460a708f8fSGustavo F. Padovan result = L2CAP_CONF_UNACCEPT; 18470a708f8fSGustavo F. Padovan 18480a708f8fSGustavo F. Padovan memset(&rfc, 0, sizeof(rfc)); 18490a708f8fSGustavo F. Padovan rfc.mode = pi->mode; 18500a708f8fSGustavo F. Padovan } 18510a708f8fSGustavo F. Padovan 18520a708f8fSGustavo F. Padovan if (result == L2CAP_CONF_SUCCESS) 1853b4450035SGustavo F. Padovan chan->conf_state |= L2CAP_CONF_OUTPUT_DONE; 18540a708f8fSGustavo F. Padovan } 18550a708f8fSGustavo F. Padovan rsp->scid = cpu_to_le16(pi->dcid); 18560a708f8fSGustavo F. Padovan rsp->result = cpu_to_le16(result); 18570a708f8fSGustavo F. Padovan rsp->flags = cpu_to_le16(0x0000); 18580a708f8fSGustavo F. Padovan 18590a708f8fSGustavo F. Padovan return ptr - data; 18600a708f8fSGustavo F. Padovan } 18610a708f8fSGustavo F. Padovan 1862b4450035SGustavo F. Padovan static int l2cap_parse_conf_rsp(struct l2cap_chan *chan, void *rsp, int len, void *data, u16 *result) 18630a708f8fSGustavo F. Padovan { 1864b4450035SGustavo F. Padovan struct sock *sk = chan->sk; 18650a708f8fSGustavo F. Padovan struct l2cap_pinfo *pi = l2cap_pi(sk); 18660a708f8fSGustavo F. Padovan struct l2cap_conf_req *req = data; 18670a708f8fSGustavo F. Padovan void *ptr = req->data; 18680a708f8fSGustavo F. Padovan int type, olen; 18690a708f8fSGustavo F. Padovan unsigned long val; 18700a708f8fSGustavo F. Padovan struct l2cap_conf_rfc rfc; 18710a708f8fSGustavo F. Padovan 18720a708f8fSGustavo F. Padovan BT_DBG("sk %p, rsp %p, len %d, req %p", sk, rsp, len, data); 18730a708f8fSGustavo F. Padovan 18740a708f8fSGustavo F. Padovan while (len >= L2CAP_CONF_OPT_SIZE) { 18750a708f8fSGustavo F. Padovan len -= l2cap_get_conf_opt(&rsp, &type, &olen, &val); 18760a708f8fSGustavo F. Padovan 18770a708f8fSGustavo F. Padovan switch (type) { 18780a708f8fSGustavo F. Padovan case L2CAP_CONF_MTU: 18790a708f8fSGustavo F. Padovan if (val < L2CAP_DEFAULT_MIN_MTU) { 18800a708f8fSGustavo F. Padovan *result = L2CAP_CONF_UNACCEPT; 18810a708f8fSGustavo F. Padovan pi->imtu = L2CAP_DEFAULT_MIN_MTU; 18820a708f8fSGustavo F. Padovan } else 18830a708f8fSGustavo F. Padovan pi->imtu = val; 18840a708f8fSGustavo F. Padovan l2cap_add_conf_opt(&ptr, L2CAP_CONF_MTU, 2, pi->imtu); 18850a708f8fSGustavo F. Padovan break; 18860a708f8fSGustavo F. Padovan 18870a708f8fSGustavo F. Padovan case L2CAP_CONF_FLUSH_TO: 18880a708f8fSGustavo F. Padovan pi->flush_to = val; 18890a708f8fSGustavo F. Padovan l2cap_add_conf_opt(&ptr, L2CAP_CONF_FLUSH_TO, 18900a708f8fSGustavo F. Padovan 2, pi->flush_to); 18910a708f8fSGustavo F. Padovan break; 18920a708f8fSGustavo F. Padovan 18930a708f8fSGustavo F. Padovan case L2CAP_CONF_RFC: 18940a708f8fSGustavo F. Padovan if (olen == sizeof(rfc)) 18950a708f8fSGustavo F. Padovan memcpy(&rfc, (void *)val, olen); 18960a708f8fSGustavo F. Padovan 1897b4450035SGustavo F. Padovan if ((chan->conf_state & L2CAP_CONF_STATE2_DEVICE) && 18980a708f8fSGustavo F. Padovan rfc.mode != pi->mode) 18990a708f8fSGustavo F. Padovan return -ECONNREFUSED; 19000a708f8fSGustavo F. Padovan 19010a708f8fSGustavo F. Padovan pi->fcs = 0; 19020a708f8fSGustavo F. Padovan 19030a708f8fSGustavo F. Padovan l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC, 19040a708f8fSGustavo F. Padovan sizeof(rfc), (unsigned long) &rfc); 19050a708f8fSGustavo F. Padovan break; 19060a708f8fSGustavo F. Padovan } 19070a708f8fSGustavo F. Padovan } 19080a708f8fSGustavo F. Padovan 19090a708f8fSGustavo F. Padovan if (pi->mode == L2CAP_MODE_BASIC && pi->mode != rfc.mode) 19100a708f8fSGustavo F. Padovan return -ECONNREFUSED; 19110a708f8fSGustavo F. Padovan 19120a708f8fSGustavo F. Padovan pi->mode = rfc.mode; 19130a708f8fSGustavo F. Padovan 19140a708f8fSGustavo F. Padovan if (*result == L2CAP_CONF_SUCCESS) { 19150a708f8fSGustavo F. Padovan switch (rfc.mode) { 19160a708f8fSGustavo F. Padovan case L2CAP_MODE_ERTM: 19170a708f8fSGustavo F. Padovan pi->retrans_timeout = le16_to_cpu(rfc.retrans_timeout); 19180a708f8fSGustavo F. Padovan pi->monitor_timeout = le16_to_cpu(rfc.monitor_timeout); 19190a708f8fSGustavo F. Padovan pi->mps = le16_to_cpu(rfc.max_pdu_size); 19200a708f8fSGustavo F. Padovan break; 19210a708f8fSGustavo F. Padovan case L2CAP_MODE_STREAMING: 19220a708f8fSGustavo F. Padovan pi->mps = le16_to_cpu(rfc.max_pdu_size); 19230a708f8fSGustavo F. Padovan } 19240a708f8fSGustavo F. Padovan } 19250a708f8fSGustavo F. Padovan 19260a708f8fSGustavo F. Padovan req->dcid = cpu_to_le16(pi->dcid); 19270a708f8fSGustavo F. Padovan req->flags = cpu_to_le16(0x0000); 19280a708f8fSGustavo F. Padovan 19290a708f8fSGustavo F. Padovan return ptr - data; 19300a708f8fSGustavo F. Padovan } 19310a708f8fSGustavo F. Padovan 19320a708f8fSGustavo F. Padovan static int l2cap_build_conf_rsp(struct sock *sk, void *data, u16 result, u16 flags) 19330a708f8fSGustavo F. Padovan { 19340a708f8fSGustavo F. Padovan struct l2cap_conf_rsp *rsp = data; 19350a708f8fSGustavo F. Padovan void *ptr = rsp->data; 19360a708f8fSGustavo F. Padovan 19370a708f8fSGustavo F. Padovan BT_DBG("sk %p", sk); 19380a708f8fSGustavo F. Padovan 19390a708f8fSGustavo F. Padovan rsp->scid = cpu_to_le16(l2cap_pi(sk)->dcid); 19400a708f8fSGustavo F. Padovan rsp->result = cpu_to_le16(result); 19410a708f8fSGustavo F. Padovan rsp->flags = cpu_to_le16(flags); 19420a708f8fSGustavo F. Padovan 19430a708f8fSGustavo F. Padovan return ptr - data; 19440a708f8fSGustavo F. Padovan } 19450a708f8fSGustavo F. Padovan 1946710f9b0aSGustavo F. Padovan void __l2cap_connect_rsp_defer(struct sock *sk) 1947710f9b0aSGustavo F. Padovan { 1948710f9b0aSGustavo F. Padovan struct l2cap_conn_rsp rsp; 1949710f9b0aSGustavo F. Padovan struct l2cap_conn *conn = l2cap_pi(sk)->conn; 1950710f9b0aSGustavo F. Padovan struct l2cap_chan *chan = l2cap_pi(sk)->chan; 1951710f9b0aSGustavo F. Padovan u8 buf[128]; 1952710f9b0aSGustavo F. Padovan 1953710f9b0aSGustavo F. Padovan sk->sk_state = BT_CONFIG; 1954710f9b0aSGustavo F. Padovan 1955710f9b0aSGustavo F. Padovan rsp.scid = cpu_to_le16(l2cap_pi(sk)->dcid); 1956710f9b0aSGustavo F. Padovan rsp.dcid = cpu_to_le16(l2cap_pi(sk)->scid); 1957710f9b0aSGustavo F. Padovan rsp.result = cpu_to_le16(L2CAP_CR_SUCCESS); 1958710f9b0aSGustavo F. Padovan rsp.status = cpu_to_le16(L2CAP_CS_NO_INFO); 1959710f9b0aSGustavo F. Padovan l2cap_send_cmd(conn, chan->ident, 1960710f9b0aSGustavo F. Padovan L2CAP_CONN_RSP, sizeof(rsp), &rsp); 1961710f9b0aSGustavo F. Padovan 1962b4450035SGustavo F. Padovan if (chan->conf_state & L2CAP_CONF_REQ_SENT) 1963710f9b0aSGustavo F. Padovan return; 1964710f9b0aSGustavo F. Padovan 1965b4450035SGustavo F. Padovan chan->conf_state |= L2CAP_CONF_REQ_SENT; 1966710f9b0aSGustavo F. Padovan l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ, 1967710f9b0aSGustavo F. Padovan l2cap_build_conf_req(chan, buf), buf); 1968710f9b0aSGustavo F. Padovan chan->num_conf_req++; 1969710f9b0aSGustavo F. Padovan } 1970710f9b0aSGustavo F. Padovan 19710a708f8fSGustavo F. Padovan static void l2cap_conf_rfc_get(struct sock *sk, void *rsp, int len) 19720a708f8fSGustavo F. Padovan { 19730a708f8fSGustavo F. Padovan struct l2cap_pinfo *pi = l2cap_pi(sk); 19740a708f8fSGustavo F. Padovan int type, olen; 19750a708f8fSGustavo F. Padovan unsigned long val; 19760a708f8fSGustavo F. Padovan struct l2cap_conf_rfc rfc; 19770a708f8fSGustavo F. Padovan 19780a708f8fSGustavo F. Padovan BT_DBG("sk %p, rsp %p, len %d", sk, rsp, len); 19790a708f8fSGustavo F. Padovan 19800a708f8fSGustavo F. Padovan if ((pi->mode != L2CAP_MODE_ERTM) && (pi->mode != L2CAP_MODE_STREAMING)) 19810a708f8fSGustavo F. Padovan return; 19820a708f8fSGustavo F. Padovan 19830a708f8fSGustavo F. Padovan while (len >= L2CAP_CONF_OPT_SIZE) { 19840a708f8fSGustavo F. Padovan len -= l2cap_get_conf_opt(&rsp, &type, &olen, &val); 19850a708f8fSGustavo F. Padovan 19860a708f8fSGustavo F. Padovan switch (type) { 19870a708f8fSGustavo F. Padovan case L2CAP_CONF_RFC: 19880a708f8fSGustavo F. Padovan if (olen == sizeof(rfc)) 19890a708f8fSGustavo F. Padovan memcpy(&rfc, (void *)val, olen); 19900a708f8fSGustavo F. Padovan goto done; 19910a708f8fSGustavo F. Padovan } 19920a708f8fSGustavo F. Padovan } 19930a708f8fSGustavo F. Padovan 19940a708f8fSGustavo F. Padovan done: 19950a708f8fSGustavo F. Padovan switch (rfc.mode) { 19960a708f8fSGustavo F. Padovan case L2CAP_MODE_ERTM: 19970a708f8fSGustavo F. Padovan pi->retrans_timeout = le16_to_cpu(rfc.retrans_timeout); 19980a708f8fSGustavo F. Padovan pi->monitor_timeout = le16_to_cpu(rfc.monitor_timeout); 19990a708f8fSGustavo F. Padovan pi->mps = le16_to_cpu(rfc.max_pdu_size); 20000a708f8fSGustavo F. Padovan break; 20010a708f8fSGustavo F. Padovan case L2CAP_MODE_STREAMING: 20020a708f8fSGustavo F. Padovan pi->mps = le16_to_cpu(rfc.max_pdu_size); 20030a708f8fSGustavo F. Padovan } 20040a708f8fSGustavo F. Padovan } 20050a708f8fSGustavo F. Padovan 20060a708f8fSGustavo F. Padovan static inline int l2cap_command_rej(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data) 20070a708f8fSGustavo F. Padovan { 20080a708f8fSGustavo F. Padovan struct l2cap_cmd_rej *rej = (struct l2cap_cmd_rej *) data; 20090a708f8fSGustavo F. Padovan 20100a708f8fSGustavo F. Padovan if (rej->reason != 0x0000) 20110a708f8fSGustavo F. Padovan return 0; 20120a708f8fSGustavo F. Padovan 20130a708f8fSGustavo F. Padovan if ((conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_SENT) && 20140a708f8fSGustavo F. Padovan cmd->ident == conn->info_ident) { 20150a708f8fSGustavo F. Padovan del_timer(&conn->info_timer); 20160a708f8fSGustavo F. Padovan 20170a708f8fSGustavo F. Padovan conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_DONE; 20180a708f8fSGustavo F. Padovan conn->info_ident = 0; 20190a708f8fSGustavo F. Padovan 20200a708f8fSGustavo F. Padovan l2cap_conn_start(conn); 20210a708f8fSGustavo F. Padovan } 20220a708f8fSGustavo F. Padovan 20230a708f8fSGustavo F. Padovan return 0; 20240a708f8fSGustavo F. Padovan } 20250a708f8fSGustavo F. Padovan 20260a708f8fSGustavo F. Padovan static inline int l2cap_connect_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data) 20270a708f8fSGustavo F. Padovan { 20280a708f8fSGustavo F. Padovan struct l2cap_conn_req *req = (struct l2cap_conn_req *) data; 20290a708f8fSGustavo F. Padovan struct l2cap_conn_rsp rsp; 203073ffa904SGustavo F. Padovan struct l2cap_chan *chan = NULL; 20310a708f8fSGustavo F. Padovan struct sock *parent, *sk = NULL; 20320a708f8fSGustavo F. Padovan int result, status = L2CAP_CS_NO_INFO; 20330a708f8fSGustavo F. Padovan 20340a708f8fSGustavo F. Padovan u16 dcid = 0, scid = __le16_to_cpu(req->scid); 20350a708f8fSGustavo F. Padovan __le16 psm = req->psm; 20360a708f8fSGustavo F. Padovan 20370a708f8fSGustavo F. Padovan BT_DBG("psm 0x%2.2x scid 0x%4.4x", psm, scid); 20380a708f8fSGustavo F. Padovan 20390a708f8fSGustavo F. Padovan /* Check if we have socket listening on psm */ 20400a708f8fSGustavo F. Padovan parent = l2cap_get_sock_by_psm(BT_LISTEN, psm, conn->src); 20410a708f8fSGustavo F. Padovan if (!parent) { 20420a708f8fSGustavo F. Padovan result = L2CAP_CR_BAD_PSM; 20430a708f8fSGustavo F. Padovan goto sendresp; 20440a708f8fSGustavo F. Padovan } 20450a708f8fSGustavo F. Padovan 20460a708f8fSGustavo F. Padovan bh_lock_sock(parent); 20470a708f8fSGustavo F. Padovan 20480a708f8fSGustavo F. Padovan /* Check if the ACL is secure enough (if not SDP) */ 20490a708f8fSGustavo F. Padovan if (psm != cpu_to_le16(0x0001) && 20500a708f8fSGustavo F. Padovan !hci_conn_check_link_mode(conn->hcon)) { 20510a708f8fSGustavo F. Padovan conn->disc_reason = 0x05; 20520a708f8fSGustavo F. Padovan result = L2CAP_CR_SEC_BLOCK; 20530a708f8fSGustavo F. Padovan goto response; 20540a708f8fSGustavo F. Padovan } 20550a708f8fSGustavo F. Padovan 20560a708f8fSGustavo F. Padovan result = L2CAP_CR_NO_MEM; 20570a708f8fSGustavo F. Padovan 20580a708f8fSGustavo F. Padovan /* Check for backlog size */ 20590a708f8fSGustavo F. Padovan if (sk_acceptq_is_full(parent)) { 20600a708f8fSGustavo F. Padovan BT_DBG("backlog full %d", parent->sk_ack_backlog); 20610a708f8fSGustavo F. Padovan goto response; 20620a708f8fSGustavo F. Padovan } 20630a708f8fSGustavo F. Padovan 20640a708f8fSGustavo F. Padovan sk = l2cap_sock_alloc(sock_net(parent), NULL, BTPROTO_L2CAP, GFP_ATOMIC); 20650a708f8fSGustavo F. Padovan if (!sk) 20660a708f8fSGustavo F. Padovan goto response; 20670a708f8fSGustavo F. Padovan 206848454079SGustavo F. Padovan chan = l2cap_chan_alloc(sk); 206948454079SGustavo F. Padovan if (!chan) { 207048454079SGustavo F. Padovan l2cap_sock_kill(sk); 207148454079SGustavo F. Padovan goto response; 207248454079SGustavo F. Padovan } 207348454079SGustavo F. Padovan 20745d41ce1dSGustavo F. Padovan l2cap_pi(sk)->chan = chan; 20755d41ce1dSGustavo F. Padovan 2076baa7e1faSGustavo F. Padovan write_lock_bh(&conn->chan_lock); 20770a708f8fSGustavo F. Padovan 20780a708f8fSGustavo F. Padovan /* Check if we already have channel with that dcid */ 2079baa7e1faSGustavo F. Padovan if (__l2cap_get_chan_by_dcid(conn, scid)) { 2080baa7e1faSGustavo F. Padovan write_unlock_bh(&conn->chan_lock); 20810a708f8fSGustavo F. Padovan sock_set_flag(sk, SOCK_ZAPPED); 20820a708f8fSGustavo F. Padovan l2cap_sock_kill(sk); 20830a708f8fSGustavo F. Padovan goto response; 20840a708f8fSGustavo F. Padovan } 20850a708f8fSGustavo F. Padovan 20860a708f8fSGustavo F. Padovan hci_conn_hold(conn->hcon); 20870a708f8fSGustavo F. Padovan 20880a708f8fSGustavo F. Padovan l2cap_sock_init(sk, parent); 20890a708f8fSGustavo F. Padovan bacpy(&bt_sk(sk)->src, conn->src); 20900a708f8fSGustavo F. Padovan bacpy(&bt_sk(sk)->dst, conn->dst); 20910a708f8fSGustavo F. Padovan l2cap_pi(sk)->psm = psm; 20920a708f8fSGustavo F. Padovan l2cap_pi(sk)->dcid = scid; 20930a708f8fSGustavo F. Padovan 2094d1010240SGustavo F. Padovan bt_accept_enqueue(parent, sk); 2095d1010240SGustavo F. Padovan 209648454079SGustavo F. Padovan __l2cap_chan_add(conn, chan); 209748454079SGustavo F. Padovan 20980a708f8fSGustavo F. Padovan dcid = l2cap_pi(sk)->scid; 20990a708f8fSGustavo F. Padovan 21000a708f8fSGustavo F. Padovan l2cap_sock_set_timer(sk, sk->sk_sndtimeo); 21010a708f8fSGustavo F. Padovan 2102fc7f8a7eSGustavo F. Padovan chan->ident = cmd->ident; 21030a708f8fSGustavo F. Padovan 21040a708f8fSGustavo F. Padovan if (conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_DONE) { 2105*4343478fSGustavo F. Padovan if (l2cap_check_security(chan)) { 21060a708f8fSGustavo F. Padovan if (bt_sk(sk)->defer_setup) { 21070a708f8fSGustavo F. Padovan sk->sk_state = BT_CONNECT2; 21080a708f8fSGustavo F. Padovan result = L2CAP_CR_PEND; 21090a708f8fSGustavo F. Padovan status = L2CAP_CS_AUTHOR_PEND; 21100a708f8fSGustavo F. Padovan parent->sk_data_ready(parent, 0); 21110a708f8fSGustavo F. Padovan } else { 21120a708f8fSGustavo F. Padovan sk->sk_state = BT_CONFIG; 21130a708f8fSGustavo F. Padovan result = L2CAP_CR_SUCCESS; 21140a708f8fSGustavo F. Padovan status = L2CAP_CS_NO_INFO; 21150a708f8fSGustavo F. Padovan } 21160a708f8fSGustavo F. Padovan } else { 21170a708f8fSGustavo F. Padovan sk->sk_state = BT_CONNECT2; 21180a708f8fSGustavo F. Padovan result = L2CAP_CR_PEND; 21190a708f8fSGustavo F. Padovan status = L2CAP_CS_AUTHEN_PEND; 21200a708f8fSGustavo F. Padovan } 21210a708f8fSGustavo F. Padovan } else { 21220a708f8fSGustavo F. Padovan sk->sk_state = BT_CONNECT2; 21230a708f8fSGustavo F. Padovan result = L2CAP_CR_PEND; 21240a708f8fSGustavo F. Padovan status = L2CAP_CS_NO_INFO; 21250a708f8fSGustavo F. Padovan } 21260a708f8fSGustavo F. Padovan 2127baa7e1faSGustavo F. Padovan write_unlock_bh(&conn->chan_lock); 21280a708f8fSGustavo F. Padovan 21290a708f8fSGustavo F. Padovan response: 21300a708f8fSGustavo F. Padovan bh_unlock_sock(parent); 21310a708f8fSGustavo F. Padovan 21320a708f8fSGustavo F. Padovan sendresp: 21330a708f8fSGustavo F. Padovan rsp.scid = cpu_to_le16(scid); 21340a708f8fSGustavo F. Padovan rsp.dcid = cpu_to_le16(dcid); 21350a708f8fSGustavo F. Padovan rsp.result = cpu_to_le16(result); 21360a708f8fSGustavo F. Padovan rsp.status = cpu_to_le16(status); 21370a708f8fSGustavo F. Padovan l2cap_send_cmd(conn, cmd->ident, L2CAP_CONN_RSP, sizeof(rsp), &rsp); 21380a708f8fSGustavo F. Padovan 21390a708f8fSGustavo F. Padovan if (result == L2CAP_CR_PEND && status == L2CAP_CS_NO_INFO) { 21400a708f8fSGustavo F. Padovan struct l2cap_info_req info; 21410a708f8fSGustavo F. Padovan info.type = cpu_to_le16(L2CAP_IT_FEAT_MASK); 21420a708f8fSGustavo F. Padovan 21430a708f8fSGustavo F. Padovan conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_SENT; 21440a708f8fSGustavo F. Padovan conn->info_ident = l2cap_get_ident(conn); 21450a708f8fSGustavo F. Padovan 21460a708f8fSGustavo F. Padovan mod_timer(&conn->info_timer, jiffies + 21470a708f8fSGustavo F. Padovan msecs_to_jiffies(L2CAP_INFO_TIMEOUT)); 21480a708f8fSGustavo F. Padovan 21490a708f8fSGustavo F. Padovan l2cap_send_cmd(conn, conn->info_ident, 21500a708f8fSGustavo F. Padovan L2CAP_INFO_REQ, sizeof(info), &info); 21510a708f8fSGustavo F. Padovan } 21520a708f8fSGustavo F. Padovan 2153b4450035SGustavo F. Padovan if (chan && !(chan->conf_state & L2CAP_CONF_REQ_SENT) && 21540a708f8fSGustavo F. Padovan result == L2CAP_CR_SUCCESS) { 21550a708f8fSGustavo F. Padovan u8 buf[128]; 2156b4450035SGustavo F. Padovan chan->conf_state |= L2CAP_CONF_REQ_SENT; 21570a708f8fSGustavo F. Padovan l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ, 215873ffa904SGustavo F. Padovan l2cap_build_conf_req(chan, buf), buf); 215973ffa904SGustavo F. Padovan chan->num_conf_req++; 21600a708f8fSGustavo F. Padovan } 21610a708f8fSGustavo F. Padovan 21620a708f8fSGustavo F. Padovan return 0; 21630a708f8fSGustavo F. Padovan } 21640a708f8fSGustavo F. Padovan 21650a708f8fSGustavo F. Padovan static inline int l2cap_connect_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data) 21660a708f8fSGustavo F. Padovan { 21670a708f8fSGustavo F. Padovan struct l2cap_conn_rsp *rsp = (struct l2cap_conn_rsp *) data; 21680a708f8fSGustavo F. Padovan u16 scid, dcid, result, status; 216948454079SGustavo F. Padovan struct l2cap_chan *chan; 21700a708f8fSGustavo F. Padovan struct sock *sk; 21710a708f8fSGustavo F. Padovan u8 req[128]; 21720a708f8fSGustavo F. Padovan 21730a708f8fSGustavo F. Padovan scid = __le16_to_cpu(rsp->scid); 21740a708f8fSGustavo F. Padovan dcid = __le16_to_cpu(rsp->dcid); 21750a708f8fSGustavo F. Padovan result = __le16_to_cpu(rsp->result); 21760a708f8fSGustavo F. Padovan status = __le16_to_cpu(rsp->status); 21770a708f8fSGustavo F. Padovan 21780a708f8fSGustavo F. Padovan BT_DBG("dcid 0x%4.4x scid 0x%4.4x result 0x%2.2x status 0x%2.2x", dcid, scid, result, status); 21790a708f8fSGustavo F. Padovan 21800a708f8fSGustavo F. Padovan if (scid) { 2181baa7e1faSGustavo F. Padovan chan = l2cap_get_chan_by_scid(conn, scid); 218248454079SGustavo F. Padovan if (!chan) 21830a708f8fSGustavo F. Padovan return -EFAULT; 21840a708f8fSGustavo F. Padovan } else { 2185baa7e1faSGustavo F. Padovan chan = l2cap_get_chan_by_ident(conn, cmd->ident); 218648454079SGustavo F. Padovan if (!chan) 21870a708f8fSGustavo F. Padovan return -EFAULT; 21880a708f8fSGustavo F. Padovan } 21890a708f8fSGustavo F. Padovan 219048454079SGustavo F. Padovan sk = chan->sk; 219148454079SGustavo F. Padovan 21920a708f8fSGustavo F. Padovan switch (result) { 21930a708f8fSGustavo F. Padovan case L2CAP_CR_SUCCESS: 21940a708f8fSGustavo F. Padovan sk->sk_state = BT_CONFIG; 2195fc7f8a7eSGustavo F. Padovan chan->ident = 0; 21960a708f8fSGustavo F. Padovan l2cap_pi(sk)->dcid = dcid; 2197b4450035SGustavo F. Padovan chan->conf_state &= ~L2CAP_CONF_CONNECT_PEND; 21980a708f8fSGustavo F. Padovan 2199b4450035SGustavo F. Padovan if (chan->conf_state & L2CAP_CONF_REQ_SENT) 22000a708f8fSGustavo F. Padovan break; 22010a708f8fSGustavo F. Padovan 2202b4450035SGustavo F. Padovan chan->conf_state |= L2CAP_CONF_REQ_SENT; 22030a708f8fSGustavo F. Padovan 22040a708f8fSGustavo F. Padovan l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ, 220573ffa904SGustavo F. Padovan l2cap_build_conf_req(chan, req), req); 220673ffa904SGustavo F. Padovan chan->num_conf_req++; 22070a708f8fSGustavo F. Padovan break; 22080a708f8fSGustavo F. Padovan 22090a708f8fSGustavo F. Padovan case L2CAP_CR_PEND: 2210b4450035SGustavo F. Padovan chan->conf_state |= L2CAP_CONF_CONNECT_PEND; 22110a708f8fSGustavo F. Padovan break; 22120a708f8fSGustavo F. Padovan 22130a708f8fSGustavo F. Padovan default: 22140a708f8fSGustavo F. Padovan /* don't delete l2cap channel if sk is owned by user */ 22150a708f8fSGustavo F. Padovan if (sock_owned_by_user(sk)) { 22160a708f8fSGustavo F. Padovan sk->sk_state = BT_DISCONN; 22170a708f8fSGustavo F. Padovan l2cap_sock_clear_timer(sk); 22180a708f8fSGustavo F. Padovan l2cap_sock_set_timer(sk, HZ / 5); 22190a708f8fSGustavo F. Padovan break; 22200a708f8fSGustavo F. Padovan } 22210a708f8fSGustavo F. Padovan 222248454079SGustavo F. Padovan l2cap_chan_del(chan, ECONNREFUSED); 22230a708f8fSGustavo F. Padovan break; 22240a708f8fSGustavo F. Padovan } 22250a708f8fSGustavo F. Padovan 22260a708f8fSGustavo F. Padovan bh_unlock_sock(sk); 22270a708f8fSGustavo F. Padovan return 0; 22280a708f8fSGustavo F. Padovan } 22290a708f8fSGustavo F. Padovan 22300a708f8fSGustavo F. Padovan static inline void set_default_fcs(struct l2cap_pinfo *pi) 22310a708f8fSGustavo F. Padovan { 22320a708f8fSGustavo F. Padovan /* FCS is enabled only in ERTM or streaming mode, if one or both 22330a708f8fSGustavo F. Padovan * sides request it. 22340a708f8fSGustavo F. Padovan */ 22350a708f8fSGustavo F. Padovan if (pi->mode != L2CAP_MODE_ERTM && pi->mode != L2CAP_MODE_STREAMING) 22360a708f8fSGustavo F. Padovan pi->fcs = L2CAP_FCS_NONE; 2237b4450035SGustavo F. Padovan else if (!(pi->chan->conf_state & L2CAP_CONF_NO_FCS_RECV)) 22380a708f8fSGustavo F. Padovan pi->fcs = L2CAP_FCS_CRC16; 22390a708f8fSGustavo F. Padovan } 22400a708f8fSGustavo F. Padovan 22410a708f8fSGustavo F. Padovan static inline int l2cap_config_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u16 cmd_len, u8 *data) 22420a708f8fSGustavo F. Padovan { 22430a708f8fSGustavo F. Padovan struct l2cap_conf_req *req = (struct l2cap_conf_req *) data; 22440a708f8fSGustavo F. Padovan u16 dcid, flags; 22450a708f8fSGustavo F. Padovan u8 rsp[64]; 224648454079SGustavo F. Padovan struct l2cap_chan *chan; 22470a708f8fSGustavo F. Padovan struct sock *sk; 22480a708f8fSGustavo F. Padovan int len; 22490a708f8fSGustavo F. Padovan 22500a708f8fSGustavo F. Padovan dcid = __le16_to_cpu(req->dcid); 22510a708f8fSGustavo F. Padovan flags = __le16_to_cpu(req->flags); 22520a708f8fSGustavo F. Padovan 22530a708f8fSGustavo F. Padovan BT_DBG("dcid 0x%4.4x flags 0x%2.2x", dcid, flags); 22540a708f8fSGustavo F. Padovan 2255baa7e1faSGustavo F. Padovan chan = l2cap_get_chan_by_scid(conn, dcid); 225648454079SGustavo F. Padovan if (!chan) 22570a708f8fSGustavo F. Padovan return -ENOENT; 22580a708f8fSGustavo F. Padovan 225948454079SGustavo F. Padovan sk = chan->sk; 226048454079SGustavo F. Padovan 22610a708f8fSGustavo F. Padovan if (sk->sk_state != BT_CONFIG) { 22620a708f8fSGustavo F. Padovan struct l2cap_cmd_rej rej; 22630a708f8fSGustavo F. Padovan 22640a708f8fSGustavo F. Padovan rej.reason = cpu_to_le16(0x0002); 22650a708f8fSGustavo F. Padovan l2cap_send_cmd(conn, cmd->ident, L2CAP_COMMAND_REJ, 22660a708f8fSGustavo F. Padovan sizeof(rej), &rej); 22670a708f8fSGustavo F. Padovan goto unlock; 22680a708f8fSGustavo F. Padovan } 22690a708f8fSGustavo F. Padovan 22700a708f8fSGustavo F. Padovan /* Reject if config buffer is too small. */ 22710a708f8fSGustavo F. Padovan len = cmd_len - sizeof(*req); 227273ffa904SGustavo F. Padovan if (chan->conf_len + len > sizeof(chan->conf_req)) { 22730a708f8fSGustavo F. Padovan l2cap_send_cmd(conn, cmd->ident, L2CAP_CONF_RSP, 22740a708f8fSGustavo F. Padovan l2cap_build_conf_rsp(sk, rsp, 22750a708f8fSGustavo F. Padovan L2CAP_CONF_REJECT, flags), rsp); 22760a708f8fSGustavo F. Padovan goto unlock; 22770a708f8fSGustavo F. Padovan } 22780a708f8fSGustavo F. Padovan 22790a708f8fSGustavo F. Padovan /* Store config. */ 228073ffa904SGustavo F. Padovan memcpy(chan->conf_req + chan->conf_len, req->data, len); 228173ffa904SGustavo F. Padovan chan->conf_len += len; 22820a708f8fSGustavo F. Padovan 22830a708f8fSGustavo F. Padovan if (flags & 0x0001) { 22840a708f8fSGustavo F. Padovan /* Incomplete config. Send empty response. */ 22850a708f8fSGustavo F. Padovan l2cap_send_cmd(conn, cmd->ident, L2CAP_CONF_RSP, 22860a708f8fSGustavo F. Padovan l2cap_build_conf_rsp(sk, rsp, 22870a708f8fSGustavo F. Padovan L2CAP_CONF_SUCCESS, 0x0001), rsp); 22880a708f8fSGustavo F. Padovan goto unlock; 22890a708f8fSGustavo F. Padovan } 22900a708f8fSGustavo F. Padovan 22910a708f8fSGustavo F. Padovan /* Complete config. */ 229273ffa904SGustavo F. Padovan len = l2cap_parse_conf_req(chan, rsp); 22930a708f8fSGustavo F. Padovan if (len < 0) { 2294e92c8e70SGustavo F. Padovan l2cap_send_disconn_req(conn, chan, ECONNRESET); 22950a708f8fSGustavo F. Padovan goto unlock; 22960a708f8fSGustavo F. Padovan } 22970a708f8fSGustavo F. Padovan 22980a708f8fSGustavo F. Padovan l2cap_send_cmd(conn, cmd->ident, L2CAP_CONF_RSP, len, rsp); 229973ffa904SGustavo F. Padovan chan->num_conf_rsp++; 23000a708f8fSGustavo F. Padovan 23010a708f8fSGustavo F. Padovan /* Reset config buffer. */ 230273ffa904SGustavo F. Padovan chan->conf_len = 0; 23030a708f8fSGustavo F. Padovan 2304b4450035SGustavo F. Padovan if (!(chan->conf_state & L2CAP_CONF_OUTPUT_DONE)) 23050a708f8fSGustavo F. Padovan goto unlock; 23060a708f8fSGustavo F. Padovan 2307b4450035SGustavo F. Padovan if (chan->conf_state & L2CAP_CONF_INPUT_DONE) { 23080a708f8fSGustavo F. Padovan set_default_fcs(l2cap_pi(sk)); 23090a708f8fSGustavo F. Padovan 23100a708f8fSGustavo F. Padovan sk->sk_state = BT_CONNECTED; 23110a708f8fSGustavo F. Padovan 231242e5c802SGustavo F. Padovan chan->next_tx_seq = 0; 231342e5c802SGustavo F. Padovan chan->expected_tx_seq = 0; 231458d35f87SGustavo F. Padovan skb_queue_head_init(&chan->tx_q); 23150a708f8fSGustavo F. Padovan if (l2cap_pi(sk)->mode == L2CAP_MODE_ERTM) 2316525cd185SGustavo F. Padovan l2cap_ertm_init(chan); 23170a708f8fSGustavo F. Padovan 23180a708f8fSGustavo F. Padovan l2cap_chan_ready(sk); 23190a708f8fSGustavo F. Padovan goto unlock; 23200a708f8fSGustavo F. Padovan } 23210a708f8fSGustavo F. Padovan 2322b4450035SGustavo F. Padovan if (!(chan->conf_state & L2CAP_CONF_REQ_SENT)) { 23230a708f8fSGustavo F. Padovan u8 buf[64]; 2324b4450035SGustavo F. Padovan chan->conf_state |= L2CAP_CONF_REQ_SENT; 23250a708f8fSGustavo F. Padovan l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ, 232673ffa904SGustavo F. Padovan l2cap_build_conf_req(chan, buf), buf); 232773ffa904SGustavo F. Padovan chan->num_conf_req++; 23280a708f8fSGustavo F. Padovan } 23290a708f8fSGustavo F. Padovan 23300a708f8fSGustavo F. Padovan unlock: 23310a708f8fSGustavo F. Padovan bh_unlock_sock(sk); 23320a708f8fSGustavo F. Padovan return 0; 23330a708f8fSGustavo F. Padovan } 23340a708f8fSGustavo F. Padovan 23350a708f8fSGustavo F. Padovan static inline int l2cap_config_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data) 23360a708f8fSGustavo F. Padovan { 23370a708f8fSGustavo F. Padovan struct l2cap_conf_rsp *rsp = (struct l2cap_conf_rsp *)data; 23380a708f8fSGustavo F. Padovan u16 scid, flags, result; 233948454079SGustavo F. Padovan struct l2cap_chan *chan; 23400a708f8fSGustavo F. Padovan struct sock *sk; 23410a708f8fSGustavo F. Padovan int len = cmd->len - sizeof(*rsp); 23420a708f8fSGustavo F. Padovan 23430a708f8fSGustavo F. Padovan scid = __le16_to_cpu(rsp->scid); 23440a708f8fSGustavo F. Padovan flags = __le16_to_cpu(rsp->flags); 23450a708f8fSGustavo F. Padovan result = __le16_to_cpu(rsp->result); 23460a708f8fSGustavo F. Padovan 23470a708f8fSGustavo F. Padovan BT_DBG("scid 0x%4.4x flags 0x%2.2x result 0x%2.2x", 23480a708f8fSGustavo F. Padovan scid, flags, result); 23490a708f8fSGustavo F. Padovan 2350baa7e1faSGustavo F. Padovan chan = l2cap_get_chan_by_scid(conn, scid); 235148454079SGustavo F. Padovan if (!chan) 23520a708f8fSGustavo F. Padovan return 0; 23530a708f8fSGustavo F. Padovan 235448454079SGustavo F. Padovan sk = chan->sk; 235548454079SGustavo F. Padovan 23560a708f8fSGustavo F. Padovan switch (result) { 23570a708f8fSGustavo F. Padovan case L2CAP_CONF_SUCCESS: 23580a708f8fSGustavo F. Padovan l2cap_conf_rfc_get(sk, rsp->data, len); 23590a708f8fSGustavo F. Padovan break; 23600a708f8fSGustavo F. Padovan 23610a708f8fSGustavo F. Padovan case L2CAP_CONF_UNACCEPT: 236273ffa904SGustavo F. Padovan if (chan->num_conf_rsp <= L2CAP_CONF_MAX_CONF_RSP) { 23630a708f8fSGustavo F. Padovan char req[64]; 23640a708f8fSGustavo F. Padovan 23650a708f8fSGustavo F. Padovan if (len > sizeof(req) - sizeof(struct l2cap_conf_req)) { 2366e92c8e70SGustavo F. Padovan l2cap_send_disconn_req(conn, chan, ECONNRESET); 23670a708f8fSGustavo F. Padovan goto done; 23680a708f8fSGustavo F. Padovan } 23690a708f8fSGustavo F. Padovan 23700a708f8fSGustavo F. Padovan /* throw out any old stored conf requests */ 23710a708f8fSGustavo F. Padovan result = L2CAP_CONF_SUCCESS; 2372b4450035SGustavo F. Padovan len = l2cap_parse_conf_rsp(chan, rsp->data, len, 2373b4450035SGustavo F. Padovan req, &result); 23740a708f8fSGustavo F. Padovan if (len < 0) { 2375e92c8e70SGustavo F. Padovan l2cap_send_disconn_req(conn, chan, ECONNRESET); 23760a708f8fSGustavo F. Padovan goto done; 23770a708f8fSGustavo F. Padovan } 23780a708f8fSGustavo F. Padovan 23790a708f8fSGustavo F. Padovan l2cap_send_cmd(conn, l2cap_get_ident(conn), 23800a708f8fSGustavo F. Padovan L2CAP_CONF_REQ, len, req); 238173ffa904SGustavo F. Padovan chan->num_conf_req++; 23820a708f8fSGustavo F. Padovan if (result != L2CAP_CONF_SUCCESS) 23830a708f8fSGustavo F. Padovan goto done; 23840a708f8fSGustavo F. Padovan break; 23850a708f8fSGustavo F. Padovan } 23860a708f8fSGustavo F. Padovan 23870a708f8fSGustavo F. Padovan default: 23880a708f8fSGustavo F. Padovan sk->sk_err = ECONNRESET; 23890a708f8fSGustavo F. Padovan l2cap_sock_set_timer(sk, HZ * 5); 2390e92c8e70SGustavo F. Padovan l2cap_send_disconn_req(conn, chan, ECONNRESET); 23910a708f8fSGustavo F. Padovan goto done; 23920a708f8fSGustavo F. Padovan } 23930a708f8fSGustavo F. Padovan 23940a708f8fSGustavo F. Padovan if (flags & 0x01) 23950a708f8fSGustavo F. Padovan goto done; 23960a708f8fSGustavo F. Padovan 2397b4450035SGustavo F. Padovan chan->conf_state |= L2CAP_CONF_INPUT_DONE; 23980a708f8fSGustavo F. Padovan 2399b4450035SGustavo F. Padovan if (chan->conf_state & L2CAP_CONF_OUTPUT_DONE) { 24000a708f8fSGustavo F. Padovan set_default_fcs(l2cap_pi(sk)); 24010a708f8fSGustavo F. Padovan 24020a708f8fSGustavo F. Padovan sk->sk_state = BT_CONNECTED; 240342e5c802SGustavo F. Padovan chan->next_tx_seq = 0; 240442e5c802SGustavo F. Padovan chan->expected_tx_seq = 0; 240558d35f87SGustavo F. Padovan skb_queue_head_init(&chan->tx_q); 24060a708f8fSGustavo F. Padovan if (l2cap_pi(sk)->mode == L2CAP_MODE_ERTM) 2407525cd185SGustavo F. Padovan l2cap_ertm_init(chan); 24080a708f8fSGustavo F. Padovan 24090a708f8fSGustavo F. Padovan l2cap_chan_ready(sk); 24100a708f8fSGustavo F. Padovan } 24110a708f8fSGustavo F. Padovan 24120a708f8fSGustavo F. Padovan done: 24130a708f8fSGustavo F. Padovan bh_unlock_sock(sk); 24140a708f8fSGustavo F. Padovan return 0; 24150a708f8fSGustavo F. Padovan } 24160a708f8fSGustavo F. Padovan 24170a708f8fSGustavo F. Padovan static inline int l2cap_disconnect_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data) 24180a708f8fSGustavo F. Padovan { 24190a708f8fSGustavo F. Padovan struct l2cap_disconn_req *req = (struct l2cap_disconn_req *) data; 24200a708f8fSGustavo F. Padovan struct l2cap_disconn_rsp rsp; 24210a708f8fSGustavo F. Padovan u16 dcid, scid; 242248454079SGustavo F. Padovan struct l2cap_chan *chan; 24230a708f8fSGustavo F. Padovan struct sock *sk; 24240a708f8fSGustavo F. Padovan 24250a708f8fSGustavo F. Padovan scid = __le16_to_cpu(req->scid); 24260a708f8fSGustavo F. Padovan dcid = __le16_to_cpu(req->dcid); 24270a708f8fSGustavo F. Padovan 24280a708f8fSGustavo F. Padovan BT_DBG("scid 0x%4.4x dcid 0x%4.4x", scid, dcid); 24290a708f8fSGustavo F. Padovan 2430baa7e1faSGustavo F. Padovan chan = l2cap_get_chan_by_scid(conn, dcid); 243148454079SGustavo F. Padovan if (!chan) 24320a708f8fSGustavo F. Padovan return 0; 24330a708f8fSGustavo F. Padovan 243448454079SGustavo F. Padovan sk = chan->sk; 243548454079SGustavo F. Padovan 24360a708f8fSGustavo F. Padovan rsp.dcid = cpu_to_le16(l2cap_pi(sk)->scid); 24370a708f8fSGustavo F. Padovan rsp.scid = cpu_to_le16(l2cap_pi(sk)->dcid); 24380a708f8fSGustavo F. Padovan l2cap_send_cmd(conn, cmd->ident, L2CAP_DISCONN_RSP, sizeof(rsp), &rsp); 24390a708f8fSGustavo F. Padovan 24400a708f8fSGustavo F. Padovan sk->sk_shutdown = SHUTDOWN_MASK; 24410a708f8fSGustavo F. Padovan 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 bh_unlock_sock(sk); 24480a708f8fSGustavo F. Padovan return 0; 24490a708f8fSGustavo F. Padovan } 24500a708f8fSGustavo F. Padovan 245148454079SGustavo F. Padovan l2cap_chan_del(chan, ECONNRESET); 24520a708f8fSGustavo F. Padovan bh_unlock_sock(sk); 24530a708f8fSGustavo F. Padovan 24540a708f8fSGustavo F. Padovan l2cap_sock_kill(sk); 24550a708f8fSGustavo F. Padovan return 0; 24560a708f8fSGustavo F. Padovan } 24570a708f8fSGustavo F. Padovan 24580a708f8fSGustavo F. Padovan static inline int l2cap_disconnect_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data) 24590a708f8fSGustavo F. Padovan { 24600a708f8fSGustavo F. Padovan struct l2cap_disconn_rsp *rsp = (struct l2cap_disconn_rsp *) data; 24610a708f8fSGustavo F. Padovan u16 dcid, scid; 246248454079SGustavo F. Padovan struct l2cap_chan *chan; 24630a708f8fSGustavo F. Padovan struct sock *sk; 24640a708f8fSGustavo F. Padovan 24650a708f8fSGustavo F. Padovan scid = __le16_to_cpu(rsp->scid); 24660a708f8fSGustavo F. Padovan dcid = __le16_to_cpu(rsp->dcid); 24670a708f8fSGustavo F. Padovan 24680a708f8fSGustavo F. Padovan BT_DBG("dcid 0x%4.4x scid 0x%4.4x", dcid, scid); 24690a708f8fSGustavo F. Padovan 2470baa7e1faSGustavo F. Padovan chan = l2cap_get_chan_by_scid(conn, scid); 247148454079SGustavo F. Padovan if (!chan) 24720a708f8fSGustavo F. Padovan return 0; 24730a708f8fSGustavo F. Padovan 247448454079SGustavo F. Padovan sk = chan->sk; 247548454079SGustavo F. Padovan 24760a708f8fSGustavo F. Padovan /* don't delete l2cap channel if sk is owned by user */ 24770a708f8fSGustavo F. Padovan if (sock_owned_by_user(sk)) { 24780a708f8fSGustavo F. Padovan sk->sk_state = BT_DISCONN; 24790a708f8fSGustavo F. Padovan l2cap_sock_clear_timer(sk); 24800a708f8fSGustavo F. Padovan l2cap_sock_set_timer(sk, HZ / 5); 24810a708f8fSGustavo F. Padovan bh_unlock_sock(sk); 24820a708f8fSGustavo F. Padovan return 0; 24830a708f8fSGustavo F. Padovan } 24840a708f8fSGustavo F. Padovan 248548454079SGustavo F. Padovan l2cap_chan_del(chan, 0); 24860a708f8fSGustavo F. Padovan bh_unlock_sock(sk); 24870a708f8fSGustavo F. Padovan 24880a708f8fSGustavo F. Padovan l2cap_sock_kill(sk); 24890a708f8fSGustavo F. Padovan return 0; 24900a708f8fSGustavo F. Padovan } 24910a708f8fSGustavo F. Padovan 24920a708f8fSGustavo F. Padovan static inline int l2cap_information_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data) 24930a708f8fSGustavo F. Padovan { 24940a708f8fSGustavo F. Padovan struct l2cap_info_req *req = (struct l2cap_info_req *) data; 24950a708f8fSGustavo F. Padovan u16 type; 24960a708f8fSGustavo F. Padovan 24970a708f8fSGustavo F. Padovan type = __le16_to_cpu(req->type); 24980a708f8fSGustavo F. Padovan 24990a708f8fSGustavo F. Padovan BT_DBG("type 0x%4.4x", type); 25000a708f8fSGustavo F. Padovan 25010a708f8fSGustavo F. Padovan if (type == L2CAP_IT_FEAT_MASK) { 25020a708f8fSGustavo F. Padovan u8 buf[8]; 25030a708f8fSGustavo F. Padovan u32 feat_mask = l2cap_feat_mask; 25040a708f8fSGustavo F. Padovan struct l2cap_info_rsp *rsp = (struct l2cap_info_rsp *) buf; 25050a708f8fSGustavo F. Padovan rsp->type = cpu_to_le16(L2CAP_IT_FEAT_MASK); 25060a708f8fSGustavo F. Padovan rsp->result = cpu_to_le16(L2CAP_IR_SUCCESS); 25070a708f8fSGustavo F. Padovan if (!disable_ertm) 25080a708f8fSGustavo F. Padovan feat_mask |= L2CAP_FEAT_ERTM | L2CAP_FEAT_STREAMING 25090a708f8fSGustavo F. Padovan | L2CAP_FEAT_FCS; 25100a708f8fSGustavo F. Padovan put_unaligned_le32(feat_mask, rsp->data); 25110a708f8fSGustavo F. Padovan l2cap_send_cmd(conn, cmd->ident, 25120a708f8fSGustavo F. Padovan L2CAP_INFO_RSP, sizeof(buf), buf); 25130a708f8fSGustavo F. Padovan } else if (type == L2CAP_IT_FIXED_CHAN) { 25140a708f8fSGustavo F. Padovan u8 buf[12]; 25150a708f8fSGustavo F. Padovan struct l2cap_info_rsp *rsp = (struct l2cap_info_rsp *) buf; 25160a708f8fSGustavo F. Padovan rsp->type = cpu_to_le16(L2CAP_IT_FIXED_CHAN); 25170a708f8fSGustavo F. Padovan rsp->result = cpu_to_le16(L2CAP_IR_SUCCESS); 25180a708f8fSGustavo F. Padovan memcpy(buf + 4, l2cap_fixed_chan, 8); 25190a708f8fSGustavo F. Padovan l2cap_send_cmd(conn, cmd->ident, 25200a708f8fSGustavo F. Padovan L2CAP_INFO_RSP, sizeof(buf), buf); 25210a708f8fSGustavo F. Padovan } else { 25220a708f8fSGustavo F. Padovan struct l2cap_info_rsp rsp; 25230a708f8fSGustavo F. Padovan rsp.type = cpu_to_le16(type); 25240a708f8fSGustavo F. Padovan rsp.result = cpu_to_le16(L2CAP_IR_NOTSUPP); 25250a708f8fSGustavo F. Padovan l2cap_send_cmd(conn, cmd->ident, 25260a708f8fSGustavo F. Padovan L2CAP_INFO_RSP, sizeof(rsp), &rsp); 25270a708f8fSGustavo F. Padovan } 25280a708f8fSGustavo F. Padovan 25290a708f8fSGustavo F. Padovan return 0; 25300a708f8fSGustavo F. Padovan } 25310a708f8fSGustavo F. Padovan 25320a708f8fSGustavo F. Padovan static inline int l2cap_information_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data) 25330a708f8fSGustavo F. Padovan { 25340a708f8fSGustavo F. Padovan struct l2cap_info_rsp *rsp = (struct l2cap_info_rsp *) data; 25350a708f8fSGustavo F. Padovan u16 type, result; 25360a708f8fSGustavo F. Padovan 25370a708f8fSGustavo F. Padovan type = __le16_to_cpu(rsp->type); 25380a708f8fSGustavo F. Padovan result = __le16_to_cpu(rsp->result); 25390a708f8fSGustavo F. Padovan 25400a708f8fSGustavo F. Padovan BT_DBG("type 0x%4.4x result 0x%2.2x", type, result); 25410a708f8fSGustavo F. Padovan 2542e90165beSAndrei Emeltchenko /* L2CAP Info req/rsp are unbound to channels, add extra checks */ 2543e90165beSAndrei Emeltchenko if (cmd->ident != conn->info_ident || 2544e90165beSAndrei Emeltchenko conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_DONE) 2545e90165beSAndrei Emeltchenko return 0; 2546e90165beSAndrei Emeltchenko 25470a708f8fSGustavo F. Padovan del_timer(&conn->info_timer); 25480a708f8fSGustavo F. Padovan 25490a708f8fSGustavo F. Padovan if (result != L2CAP_IR_SUCCESS) { 25500a708f8fSGustavo F. Padovan conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_DONE; 25510a708f8fSGustavo F. Padovan conn->info_ident = 0; 25520a708f8fSGustavo F. Padovan 25530a708f8fSGustavo F. Padovan l2cap_conn_start(conn); 25540a708f8fSGustavo F. Padovan 25550a708f8fSGustavo F. Padovan return 0; 25560a708f8fSGustavo F. Padovan } 25570a708f8fSGustavo F. Padovan 25580a708f8fSGustavo F. Padovan if (type == L2CAP_IT_FEAT_MASK) { 25590a708f8fSGustavo F. Padovan conn->feat_mask = get_unaligned_le32(rsp->data); 25600a708f8fSGustavo F. Padovan 25610a708f8fSGustavo F. Padovan if (conn->feat_mask & L2CAP_FEAT_FIXED_CHAN) { 25620a708f8fSGustavo F. Padovan struct l2cap_info_req req; 25630a708f8fSGustavo F. Padovan req.type = cpu_to_le16(L2CAP_IT_FIXED_CHAN); 25640a708f8fSGustavo F. Padovan 25650a708f8fSGustavo F. Padovan conn->info_ident = l2cap_get_ident(conn); 25660a708f8fSGustavo F. Padovan 25670a708f8fSGustavo F. Padovan l2cap_send_cmd(conn, conn->info_ident, 25680a708f8fSGustavo F. Padovan L2CAP_INFO_REQ, sizeof(req), &req); 25690a708f8fSGustavo F. Padovan } else { 25700a708f8fSGustavo F. Padovan conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_DONE; 25710a708f8fSGustavo F. Padovan conn->info_ident = 0; 25720a708f8fSGustavo F. Padovan 25730a708f8fSGustavo F. Padovan l2cap_conn_start(conn); 25740a708f8fSGustavo F. Padovan } 25750a708f8fSGustavo F. Padovan } else if (type == L2CAP_IT_FIXED_CHAN) { 25760a708f8fSGustavo F. Padovan conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_DONE; 25770a708f8fSGustavo F. Padovan conn->info_ident = 0; 25780a708f8fSGustavo F. Padovan 25790a708f8fSGustavo F. Padovan l2cap_conn_start(conn); 25800a708f8fSGustavo F. Padovan } 25810a708f8fSGustavo F. Padovan 25820a708f8fSGustavo F. Padovan return 0; 25830a708f8fSGustavo F. Padovan } 25840a708f8fSGustavo F. Padovan 2585e2174ca4SGustavo F. Padovan static inline int l2cap_check_conn_param(u16 min, u16 max, u16 latency, 2586de73115aSClaudio Takahasi u16 to_multiplier) 2587de73115aSClaudio Takahasi { 2588de73115aSClaudio Takahasi u16 max_latency; 2589de73115aSClaudio Takahasi 2590de73115aSClaudio Takahasi if (min > max || min < 6 || max > 3200) 2591de73115aSClaudio Takahasi return -EINVAL; 2592de73115aSClaudio Takahasi 2593de73115aSClaudio Takahasi if (to_multiplier < 10 || to_multiplier > 3200) 2594de73115aSClaudio Takahasi return -EINVAL; 2595de73115aSClaudio Takahasi 2596de73115aSClaudio Takahasi if (max >= to_multiplier * 8) 2597de73115aSClaudio Takahasi return -EINVAL; 2598de73115aSClaudio Takahasi 2599de73115aSClaudio Takahasi max_latency = (to_multiplier * 8 / max) - 1; 2600de73115aSClaudio Takahasi if (latency > 499 || latency > max_latency) 2601de73115aSClaudio Takahasi return -EINVAL; 2602de73115aSClaudio Takahasi 2603de73115aSClaudio Takahasi return 0; 2604de73115aSClaudio Takahasi } 2605de73115aSClaudio Takahasi 2606de73115aSClaudio Takahasi static inline int l2cap_conn_param_update_req(struct l2cap_conn *conn, 2607de73115aSClaudio Takahasi struct l2cap_cmd_hdr *cmd, u8 *data) 2608de73115aSClaudio Takahasi { 2609de73115aSClaudio Takahasi struct hci_conn *hcon = conn->hcon; 2610de73115aSClaudio Takahasi struct l2cap_conn_param_update_req *req; 2611de73115aSClaudio Takahasi struct l2cap_conn_param_update_rsp rsp; 2612de73115aSClaudio Takahasi u16 min, max, latency, to_multiplier, cmd_len; 26132ce603ebSClaudio Takahasi int err; 2614de73115aSClaudio Takahasi 2615de73115aSClaudio Takahasi if (!(hcon->link_mode & HCI_LM_MASTER)) 2616de73115aSClaudio Takahasi return -EINVAL; 2617de73115aSClaudio Takahasi 2618de73115aSClaudio Takahasi cmd_len = __le16_to_cpu(cmd->len); 2619de73115aSClaudio Takahasi if (cmd_len != sizeof(struct l2cap_conn_param_update_req)) 2620de73115aSClaudio Takahasi return -EPROTO; 2621de73115aSClaudio Takahasi 2622de73115aSClaudio Takahasi req = (struct l2cap_conn_param_update_req *) data; 2623de73115aSClaudio Takahasi min = __le16_to_cpu(req->min); 2624de73115aSClaudio Takahasi max = __le16_to_cpu(req->max); 2625de73115aSClaudio Takahasi latency = __le16_to_cpu(req->latency); 2626de73115aSClaudio Takahasi to_multiplier = __le16_to_cpu(req->to_multiplier); 2627de73115aSClaudio Takahasi 2628de73115aSClaudio Takahasi BT_DBG("min 0x%4.4x max 0x%4.4x latency: 0x%4.4x Timeout: 0x%4.4x", 2629de73115aSClaudio Takahasi min, max, latency, to_multiplier); 2630de73115aSClaudio Takahasi 2631de73115aSClaudio Takahasi memset(&rsp, 0, sizeof(rsp)); 26322ce603ebSClaudio Takahasi 26332ce603ebSClaudio Takahasi err = l2cap_check_conn_param(min, max, latency, to_multiplier); 26342ce603ebSClaudio Takahasi if (err) 2635de73115aSClaudio Takahasi rsp.result = cpu_to_le16(L2CAP_CONN_PARAM_REJECTED); 2636de73115aSClaudio Takahasi else 2637de73115aSClaudio Takahasi rsp.result = cpu_to_le16(L2CAP_CONN_PARAM_ACCEPTED); 2638de73115aSClaudio Takahasi 2639de73115aSClaudio Takahasi l2cap_send_cmd(conn, cmd->ident, L2CAP_CONN_PARAM_UPDATE_RSP, 2640de73115aSClaudio Takahasi sizeof(rsp), &rsp); 2641de73115aSClaudio Takahasi 26422ce603ebSClaudio Takahasi if (!err) 26432ce603ebSClaudio Takahasi hci_le_conn_update(hcon, min, max, latency, to_multiplier); 26442ce603ebSClaudio Takahasi 2645de73115aSClaudio Takahasi return 0; 2646de73115aSClaudio Takahasi } 2647de73115aSClaudio Takahasi 26483300d9a9SClaudio Takahasi static inline int l2cap_bredr_sig_cmd(struct l2cap_conn *conn, 26493300d9a9SClaudio Takahasi struct l2cap_cmd_hdr *cmd, u16 cmd_len, u8 *data) 26503300d9a9SClaudio Takahasi { 26513300d9a9SClaudio Takahasi int err = 0; 26523300d9a9SClaudio Takahasi 26533300d9a9SClaudio Takahasi switch (cmd->code) { 26543300d9a9SClaudio Takahasi case L2CAP_COMMAND_REJ: 26553300d9a9SClaudio Takahasi l2cap_command_rej(conn, cmd, data); 26563300d9a9SClaudio Takahasi break; 26573300d9a9SClaudio Takahasi 26583300d9a9SClaudio Takahasi case L2CAP_CONN_REQ: 26593300d9a9SClaudio Takahasi err = l2cap_connect_req(conn, cmd, data); 26603300d9a9SClaudio Takahasi break; 26613300d9a9SClaudio Takahasi 26623300d9a9SClaudio Takahasi case L2CAP_CONN_RSP: 26633300d9a9SClaudio Takahasi err = l2cap_connect_rsp(conn, cmd, data); 26643300d9a9SClaudio Takahasi break; 26653300d9a9SClaudio Takahasi 26663300d9a9SClaudio Takahasi case L2CAP_CONF_REQ: 26673300d9a9SClaudio Takahasi err = l2cap_config_req(conn, cmd, cmd_len, data); 26683300d9a9SClaudio Takahasi break; 26693300d9a9SClaudio Takahasi 26703300d9a9SClaudio Takahasi case L2CAP_CONF_RSP: 26713300d9a9SClaudio Takahasi err = l2cap_config_rsp(conn, cmd, data); 26723300d9a9SClaudio Takahasi break; 26733300d9a9SClaudio Takahasi 26743300d9a9SClaudio Takahasi case L2CAP_DISCONN_REQ: 26753300d9a9SClaudio Takahasi err = l2cap_disconnect_req(conn, cmd, data); 26763300d9a9SClaudio Takahasi break; 26773300d9a9SClaudio Takahasi 26783300d9a9SClaudio Takahasi case L2CAP_DISCONN_RSP: 26793300d9a9SClaudio Takahasi err = l2cap_disconnect_rsp(conn, cmd, data); 26803300d9a9SClaudio Takahasi break; 26813300d9a9SClaudio Takahasi 26823300d9a9SClaudio Takahasi case L2CAP_ECHO_REQ: 26833300d9a9SClaudio Takahasi l2cap_send_cmd(conn, cmd->ident, L2CAP_ECHO_RSP, cmd_len, data); 26843300d9a9SClaudio Takahasi break; 26853300d9a9SClaudio Takahasi 26863300d9a9SClaudio Takahasi case L2CAP_ECHO_RSP: 26873300d9a9SClaudio Takahasi break; 26883300d9a9SClaudio Takahasi 26893300d9a9SClaudio Takahasi case L2CAP_INFO_REQ: 26903300d9a9SClaudio Takahasi err = l2cap_information_req(conn, cmd, data); 26913300d9a9SClaudio Takahasi break; 26923300d9a9SClaudio Takahasi 26933300d9a9SClaudio Takahasi case L2CAP_INFO_RSP: 26943300d9a9SClaudio Takahasi err = l2cap_information_rsp(conn, cmd, data); 26953300d9a9SClaudio Takahasi break; 26963300d9a9SClaudio Takahasi 26973300d9a9SClaudio Takahasi default: 26983300d9a9SClaudio Takahasi BT_ERR("Unknown BR/EDR signaling command 0x%2.2x", cmd->code); 26993300d9a9SClaudio Takahasi err = -EINVAL; 27003300d9a9SClaudio Takahasi break; 27013300d9a9SClaudio Takahasi } 27023300d9a9SClaudio Takahasi 27033300d9a9SClaudio Takahasi return err; 27043300d9a9SClaudio Takahasi } 27053300d9a9SClaudio Takahasi 27063300d9a9SClaudio Takahasi static inline int l2cap_le_sig_cmd(struct l2cap_conn *conn, 27073300d9a9SClaudio Takahasi struct l2cap_cmd_hdr *cmd, u8 *data) 27083300d9a9SClaudio Takahasi { 27093300d9a9SClaudio Takahasi switch (cmd->code) { 27103300d9a9SClaudio Takahasi case L2CAP_COMMAND_REJ: 27113300d9a9SClaudio Takahasi return 0; 27123300d9a9SClaudio Takahasi 27133300d9a9SClaudio Takahasi case L2CAP_CONN_PARAM_UPDATE_REQ: 2714de73115aSClaudio Takahasi return l2cap_conn_param_update_req(conn, cmd, data); 27153300d9a9SClaudio Takahasi 27163300d9a9SClaudio Takahasi case L2CAP_CONN_PARAM_UPDATE_RSP: 27173300d9a9SClaudio Takahasi return 0; 27183300d9a9SClaudio Takahasi 27193300d9a9SClaudio Takahasi default: 27203300d9a9SClaudio Takahasi BT_ERR("Unknown LE signaling command 0x%2.2x", cmd->code); 27213300d9a9SClaudio Takahasi return -EINVAL; 27223300d9a9SClaudio Takahasi } 27233300d9a9SClaudio Takahasi } 27243300d9a9SClaudio Takahasi 27253300d9a9SClaudio Takahasi static inline void l2cap_sig_channel(struct l2cap_conn *conn, 27263300d9a9SClaudio Takahasi struct sk_buff *skb) 27270a708f8fSGustavo F. Padovan { 27280a708f8fSGustavo F. Padovan u8 *data = skb->data; 27290a708f8fSGustavo F. Padovan int len = skb->len; 27300a708f8fSGustavo F. Padovan struct l2cap_cmd_hdr cmd; 27313300d9a9SClaudio Takahasi int err; 27320a708f8fSGustavo F. Padovan 27330a708f8fSGustavo F. Padovan l2cap_raw_recv(conn, skb); 27340a708f8fSGustavo F. Padovan 27350a708f8fSGustavo F. Padovan while (len >= L2CAP_CMD_HDR_SIZE) { 27360a708f8fSGustavo F. Padovan u16 cmd_len; 27370a708f8fSGustavo F. Padovan memcpy(&cmd, data, L2CAP_CMD_HDR_SIZE); 27380a708f8fSGustavo F. Padovan data += L2CAP_CMD_HDR_SIZE; 27390a708f8fSGustavo F. Padovan len -= L2CAP_CMD_HDR_SIZE; 27400a708f8fSGustavo F. Padovan 27410a708f8fSGustavo F. Padovan cmd_len = le16_to_cpu(cmd.len); 27420a708f8fSGustavo F. Padovan 27430a708f8fSGustavo F. Padovan BT_DBG("code 0x%2.2x len %d id 0x%2.2x", cmd.code, cmd_len, cmd.ident); 27440a708f8fSGustavo F. Padovan 27450a708f8fSGustavo F. Padovan if (cmd_len > len || !cmd.ident) { 27460a708f8fSGustavo F. Padovan BT_DBG("corrupted command"); 27470a708f8fSGustavo F. Padovan break; 27480a708f8fSGustavo F. Padovan } 27490a708f8fSGustavo F. Padovan 27503300d9a9SClaudio Takahasi if (conn->hcon->type == LE_LINK) 27513300d9a9SClaudio Takahasi err = l2cap_le_sig_cmd(conn, &cmd, data); 27523300d9a9SClaudio Takahasi else 27533300d9a9SClaudio Takahasi err = l2cap_bredr_sig_cmd(conn, &cmd, cmd_len, data); 27540a708f8fSGustavo F. Padovan 27550a708f8fSGustavo F. Padovan if (err) { 27560a708f8fSGustavo F. Padovan struct l2cap_cmd_rej rej; 27572c6d1a2eSGustavo F. Padovan 27582c6d1a2eSGustavo F. Padovan BT_ERR("Wrong link type (%d)", err); 27590a708f8fSGustavo F. Padovan 27600a708f8fSGustavo F. Padovan /* FIXME: Map err to a valid reason */ 27610a708f8fSGustavo F. Padovan rej.reason = cpu_to_le16(0); 27620a708f8fSGustavo F. Padovan l2cap_send_cmd(conn, cmd.ident, L2CAP_COMMAND_REJ, sizeof(rej), &rej); 27630a708f8fSGustavo F. Padovan } 27640a708f8fSGustavo F. Padovan 27650a708f8fSGustavo F. Padovan data += cmd_len; 27660a708f8fSGustavo F. Padovan len -= cmd_len; 27670a708f8fSGustavo F. Padovan } 27680a708f8fSGustavo F. Padovan 27690a708f8fSGustavo F. Padovan kfree_skb(skb); 27700a708f8fSGustavo F. Padovan } 27710a708f8fSGustavo F. Padovan 27720a708f8fSGustavo F. Padovan static int l2cap_check_fcs(struct l2cap_pinfo *pi, struct sk_buff *skb) 27730a708f8fSGustavo F. Padovan { 27740a708f8fSGustavo F. Padovan u16 our_fcs, rcv_fcs; 27750a708f8fSGustavo F. Padovan int hdr_size = L2CAP_HDR_SIZE + 2; 27760a708f8fSGustavo F. Padovan 27770a708f8fSGustavo F. Padovan if (pi->fcs == L2CAP_FCS_CRC16) { 27780a708f8fSGustavo F. Padovan skb_trim(skb, skb->len - 2); 27790a708f8fSGustavo F. Padovan rcv_fcs = get_unaligned_le16(skb->data + skb->len); 27800a708f8fSGustavo F. Padovan our_fcs = crc16(0, skb->data - hdr_size, skb->len + hdr_size); 27810a708f8fSGustavo F. Padovan 27820a708f8fSGustavo F. Padovan if (our_fcs != rcv_fcs) 27830a708f8fSGustavo F. Padovan return -EBADMSG; 27840a708f8fSGustavo F. Padovan } 27850a708f8fSGustavo F. Padovan return 0; 27860a708f8fSGustavo F. Padovan } 27870a708f8fSGustavo F. Padovan 2788525cd185SGustavo F. Padovan static inline void l2cap_send_i_or_rr_or_rnr(struct l2cap_chan *chan) 27890a708f8fSGustavo F. Padovan { 27900a708f8fSGustavo F. Padovan u16 control = 0; 27910a708f8fSGustavo F. Padovan 27926a026610SGustavo F. Padovan chan->frames_sent = 0; 27930a708f8fSGustavo F. Padovan 279442e5c802SGustavo F. Padovan control |= chan->buffer_seq << L2CAP_CTRL_REQSEQ_SHIFT; 27950a708f8fSGustavo F. Padovan 2796525cd185SGustavo F. Padovan if (chan->conn_state & L2CAP_CONN_LOCAL_BUSY) { 27970a708f8fSGustavo F. Padovan control |= L2CAP_SUPER_RCV_NOT_READY; 2798525cd185SGustavo F. Padovan l2cap_send_sframe(chan, control); 2799525cd185SGustavo F. Padovan chan->conn_state |= L2CAP_CONN_RNR_SENT; 28000a708f8fSGustavo F. Padovan } 28010a708f8fSGustavo F. Padovan 2802525cd185SGustavo F. Padovan if (chan->conn_state & L2CAP_CONN_REMOTE_BUSY) 2803525cd185SGustavo F. Padovan l2cap_retransmit_frames(chan); 28040a708f8fSGustavo F. Padovan 2805525cd185SGustavo F. Padovan l2cap_ertm_send(chan); 28060a708f8fSGustavo F. Padovan 2807525cd185SGustavo F. Padovan if (!(chan->conn_state & L2CAP_CONN_LOCAL_BUSY) && 28086a026610SGustavo F. Padovan chan->frames_sent == 0) { 28090a708f8fSGustavo F. Padovan control |= L2CAP_SUPER_RCV_READY; 2810525cd185SGustavo F. Padovan l2cap_send_sframe(chan, control); 28110a708f8fSGustavo F. Padovan } 28120a708f8fSGustavo F. Padovan } 28130a708f8fSGustavo F. Padovan 281442e5c802SGustavo F. Padovan static int l2cap_add_to_srej_queue(struct l2cap_chan *chan, struct sk_buff *skb, u8 tx_seq, u8 sar) 28150a708f8fSGustavo F. Padovan { 28160a708f8fSGustavo F. Padovan struct sk_buff *next_skb; 28170a708f8fSGustavo F. Padovan int tx_seq_offset, next_tx_seq_offset; 28180a708f8fSGustavo F. Padovan 28190a708f8fSGustavo F. Padovan bt_cb(skb)->tx_seq = tx_seq; 28200a708f8fSGustavo F. Padovan bt_cb(skb)->sar = sar; 28210a708f8fSGustavo F. Padovan 2822f1c6775bSGustavo F. Padovan next_skb = skb_peek(&chan->srej_q); 28230a708f8fSGustavo F. Padovan if (!next_skb) { 2824f1c6775bSGustavo F. Padovan __skb_queue_tail(&chan->srej_q, skb); 28250a708f8fSGustavo F. Padovan return 0; 28260a708f8fSGustavo F. Padovan } 28270a708f8fSGustavo F. Padovan 282842e5c802SGustavo F. Padovan tx_seq_offset = (tx_seq - chan->buffer_seq) % 64; 28290a708f8fSGustavo F. Padovan if (tx_seq_offset < 0) 28300a708f8fSGustavo F. Padovan tx_seq_offset += 64; 28310a708f8fSGustavo F. Padovan 28320a708f8fSGustavo F. Padovan do { 28330a708f8fSGustavo F. Padovan if (bt_cb(next_skb)->tx_seq == tx_seq) 28340a708f8fSGustavo F. Padovan return -EINVAL; 28350a708f8fSGustavo F. Padovan 28360a708f8fSGustavo F. Padovan next_tx_seq_offset = (bt_cb(next_skb)->tx_seq - 283742e5c802SGustavo F. Padovan chan->buffer_seq) % 64; 28380a708f8fSGustavo F. Padovan if (next_tx_seq_offset < 0) 28390a708f8fSGustavo F. Padovan next_tx_seq_offset += 64; 28400a708f8fSGustavo F. Padovan 28410a708f8fSGustavo F. Padovan if (next_tx_seq_offset > tx_seq_offset) { 2842f1c6775bSGustavo F. Padovan __skb_queue_before(&chan->srej_q, next_skb, skb); 28430a708f8fSGustavo F. Padovan return 0; 28440a708f8fSGustavo F. Padovan } 28450a708f8fSGustavo F. Padovan 2846f1c6775bSGustavo F. Padovan if (skb_queue_is_last(&chan->srej_q, next_skb)) 28470a708f8fSGustavo F. Padovan break; 28480a708f8fSGustavo F. Padovan 2849f1c6775bSGustavo F. Padovan } while ((next_skb = skb_queue_next(&chan->srej_q, next_skb))); 28500a708f8fSGustavo F. Padovan 2851f1c6775bSGustavo F. Padovan __skb_queue_tail(&chan->srej_q, skb); 28520a708f8fSGustavo F. Padovan 28530a708f8fSGustavo F. Padovan return 0; 28540a708f8fSGustavo F. Padovan } 28550a708f8fSGustavo F. Padovan 2856525cd185SGustavo F. Padovan static int l2cap_ertm_reassembly_sdu(struct l2cap_chan *chan, struct sk_buff *skb, u16 control) 28570a708f8fSGustavo F. Padovan { 2858525cd185SGustavo F. Padovan struct l2cap_pinfo *pi = l2cap_pi(chan->sk); 28590a708f8fSGustavo F. Padovan struct sk_buff *_skb; 28600a708f8fSGustavo F. Padovan int err; 28610a708f8fSGustavo F. Padovan 28620a708f8fSGustavo F. Padovan switch (control & L2CAP_CTRL_SAR) { 28630a708f8fSGustavo F. Padovan case L2CAP_SDU_UNSEGMENTED: 2864525cd185SGustavo F. Padovan if (chan->conn_state & L2CAP_CONN_SAR_SDU) 28650a708f8fSGustavo F. Padovan goto drop; 28660a708f8fSGustavo F. Padovan 2867525cd185SGustavo F. Padovan err = sock_queue_rcv_skb(chan->sk, skb); 28680a708f8fSGustavo F. Padovan if (!err) 28690a708f8fSGustavo F. Padovan return err; 28700a708f8fSGustavo F. Padovan 28710a708f8fSGustavo F. Padovan break; 28720a708f8fSGustavo F. Padovan 28730a708f8fSGustavo F. Padovan case L2CAP_SDU_START: 2874525cd185SGustavo F. Padovan if (chan->conn_state & L2CAP_CONN_SAR_SDU) 28750a708f8fSGustavo F. Padovan goto drop; 28760a708f8fSGustavo F. Padovan 28776f61fd47SGustavo F. Padovan chan->sdu_len = get_unaligned_le16(skb->data); 28780a708f8fSGustavo F. Padovan 28796f61fd47SGustavo F. Padovan if (chan->sdu_len > pi->imtu) 28800a708f8fSGustavo F. Padovan goto disconnect; 28810a708f8fSGustavo F. Padovan 28826f61fd47SGustavo F. Padovan chan->sdu = bt_skb_alloc(chan->sdu_len, GFP_ATOMIC); 28836f61fd47SGustavo F. Padovan if (!chan->sdu) 28840a708f8fSGustavo F. Padovan return -ENOMEM; 28850a708f8fSGustavo F. Padovan 28860a708f8fSGustavo F. Padovan /* pull sdu_len bytes only after alloc, because of Local Busy 28870a708f8fSGustavo F. Padovan * condition we have to be sure that this will be executed 28880a708f8fSGustavo F. Padovan * only once, i.e., when alloc does not fail */ 28890a708f8fSGustavo F. Padovan skb_pull(skb, 2); 28900a708f8fSGustavo F. Padovan 28916f61fd47SGustavo F. Padovan memcpy(skb_put(chan->sdu, skb->len), skb->data, skb->len); 28920a708f8fSGustavo F. Padovan 2893525cd185SGustavo F. Padovan chan->conn_state |= L2CAP_CONN_SAR_SDU; 28946f61fd47SGustavo F. Padovan chan->partial_sdu_len = skb->len; 28950a708f8fSGustavo F. Padovan break; 28960a708f8fSGustavo F. Padovan 28970a708f8fSGustavo F. Padovan case L2CAP_SDU_CONTINUE: 2898525cd185SGustavo F. Padovan if (!(chan->conn_state & L2CAP_CONN_SAR_SDU)) 28990a708f8fSGustavo F. Padovan goto disconnect; 29000a708f8fSGustavo F. Padovan 29016f61fd47SGustavo F. Padovan if (!chan->sdu) 29020a708f8fSGustavo F. Padovan goto disconnect; 29030a708f8fSGustavo F. Padovan 29046f61fd47SGustavo F. Padovan chan->partial_sdu_len += skb->len; 29056f61fd47SGustavo F. Padovan if (chan->partial_sdu_len > chan->sdu_len) 29060a708f8fSGustavo F. Padovan goto drop; 29070a708f8fSGustavo F. Padovan 29086f61fd47SGustavo F. Padovan memcpy(skb_put(chan->sdu, skb->len), skb->data, skb->len); 29090a708f8fSGustavo F. Padovan 29100a708f8fSGustavo F. Padovan break; 29110a708f8fSGustavo F. Padovan 29120a708f8fSGustavo F. Padovan case L2CAP_SDU_END: 2913525cd185SGustavo F. Padovan if (!(chan->conn_state & L2CAP_CONN_SAR_SDU)) 29140a708f8fSGustavo F. Padovan goto disconnect; 29150a708f8fSGustavo F. Padovan 29166f61fd47SGustavo F. Padovan if (!chan->sdu) 29170a708f8fSGustavo F. Padovan goto disconnect; 29180a708f8fSGustavo F. Padovan 2919525cd185SGustavo F. Padovan if (!(chan->conn_state & L2CAP_CONN_SAR_RETRY)) { 29206f61fd47SGustavo F. Padovan chan->partial_sdu_len += skb->len; 29210a708f8fSGustavo F. Padovan 29226f61fd47SGustavo F. Padovan if (chan->partial_sdu_len > pi->imtu) 29230a708f8fSGustavo F. Padovan goto drop; 29240a708f8fSGustavo F. Padovan 29256f61fd47SGustavo F. Padovan if (chan->partial_sdu_len != chan->sdu_len) 29260a708f8fSGustavo F. Padovan goto drop; 29270a708f8fSGustavo F. Padovan 29286f61fd47SGustavo F. Padovan memcpy(skb_put(chan->sdu, skb->len), skb->data, skb->len); 29290a708f8fSGustavo F. Padovan } 29300a708f8fSGustavo F. Padovan 29316f61fd47SGustavo F. Padovan _skb = skb_clone(chan->sdu, GFP_ATOMIC); 29320a708f8fSGustavo F. Padovan if (!_skb) { 2933525cd185SGustavo F. Padovan chan->conn_state |= L2CAP_CONN_SAR_RETRY; 29340a708f8fSGustavo F. Padovan return -ENOMEM; 29350a708f8fSGustavo F. Padovan } 29360a708f8fSGustavo F. Padovan 2937525cd185SGustavo F. Padovan err = sock_queue_rcv_skb(chan->sk, _skb); 29380a708f8fSGustavo F. Padovan if (err < 0) { 29390a708f8fSGustavo F. Padovan kfree_skb(_skb); 2940525cd185SGustavo F. Padovan chan->conn_state |= L2CAP_CONN_SAR_RETRY; 29410a708f8fSGustavo F. Padovan return err; 29420a708f8fSGustavo F. Padovan } 29430a708f8fSGustavo F. Padovan 2944525cd185SGustavo F. Padovan chan->conn_state &= ~L2CAP_CONN_SAR_RETRY; 2945525cd185SGustavo F. Padovan chan->conn_state &= ~L2CAP_CONN_SAR_SDU; 29460a708f8fSGustavo F. Padovan 29476f61fd47SGustavo F. Padovan kfree_skb(chan->sdu); 29480a708f8fSGustavo F. Padovan break; 29490a708f8fSGustavo F. Padovan } 29500a708f8fSGustavo F. Padovan 29510a708f8fSGustavo F. Padovan kfree_skb(skb); 29520a708f8fSGustavo F. Padovan return 0; 29530a708f8fSGustavo F. Padovan 29540a708f8fSGustavo F. Padovan drop: 29556f61fd47SGustavo F. Padovan kfree_skb(chan->sdu); 29566f61fd47SGustavo F. Padovan chan->sdu = NULL; 29570a708f8fSGustavo F. Padovan 29580a708f8fSGustavo F. Padovan disconnect: 2959e92c8e70SGustavo F. Padovan l2cap_send_disconn_req(pi->conn, chan, ECONNRESET); 29600a708f8fSGustavo F. Padovan kfree_skb(skb); 29610a708f8fSGustavo F. Padovan return 0; 29620a708f8fSGustavo F. Padovan } 29630a708f8fSGustavo F. Padovan 2964525cd185SGustavo F. Padovan static int l2cap_try_push_rx_skb(struct l2cap_chan *chan) 29650a708f8fSGustavo F. Padovan { 29660a708f8fSGustavo F. Padovan struct sk_buff *skb; 29670a708f8fSGustavo F. Padovan u16 control; 29680a708f8fSGustavo F. Padovan int err; 29690a708f8fSGustavo F. Padovan 2970f1c6775bSGustavo F. Padovan while ((skb = skb_dequeue(&chan->busy_q))) { 29710a708f8fSGustavo F. Padovan control = bt_cb(skb)->sar << L2CAP_CTRL_SAR_SHIFT; 2972525cd185SGustavo F. Padovan err = l2cap_ertm_reassembly_sdu(chan, skb, control); 29730a708f8fSGustavo F. Padovan if (err < 0) { 2974f1c6775bSGustavo F. Padovan skb_queue_head(&chan->busy_q, skb); 29750a708f8fSGustavo F. Padovan return -EBUSY; 29760a708f8fSGustavo F. Padovan } 29770a708f8fSGustavo F. Padovan 297842e5c802SGustavo F. Padovan chan->buffer_seq = (chan->buffer_seq + 1) % 64; 29790a708f8fSGustavo F. Padovan } 29800a708f8fSGustavo F. Padovan 2981525cd185SGustavo F. Padovan if (!(chan->conn_state & L2CAP_CONN_RNR_SENT)) 29820a708f8fSGustavo F. Padovan goto done; 29830a708f8fSGustavo F. Padovan 298442e5c802SGustavo F. Padovan control = chan->buffer_seq << L2CAP_CTRL_REQSEQ_SHIFT; 29850a708f8fSGustavo F. Padovan control |= L2CAP_SUPER_RCV_READY | L2CAP_CTRL_POLL; 2986525cd185SGustavo F. Padovan l2cap_send_sframe(chan, control); 29876a026610SGustavo F. Padovan chan->retry_count = 1; 29880a708f8fSGustavo F. Padovan 2989e92c8e70SGustavo F. Padovan del_timer(&chan->retrans_timer); 29900a708f8fSGustavo F. Padovan __mod_monitor_timer(); 29910a708f8fSGustavo F. Padovan 2992525cd185SGustavo F. Padovan chan->conn_state |= L2CAP_CONN_WAIT_F; 29930a708f8fSGustavo F. Padovan 29940a708f8fSGustavo F. Padovan done: 2995525cd185SGustavo F. Padovan chan->conn_state &= ~L2CAP_CONN_LOCAL_BUSY; 2996525cd185SGustavo F. Padovan chan->conn_state &= ~L2CAP_CONN_RNR_SENT; 29970a708f8fSGustavo F. Padovan 299849208c9cSGustavo F. Padovan BT_DBG("chan %p, Exit local busy", chan); 29990a708f8fSGustavo F. Padovan 30000a708f8fSGustavo F. Padovan return 0; 30010a708f8fSGustavo F. Padovan } 30020a708f8fSGustavo F. Padovan 30030a708f8fSGustavo F. Padovan static void l2cap_busy_work(struct work_struct *work) 30040a708f8fSGustavo F. Padovan { 30050a708f8fSGustavo F. Padovan DECLARE_WAITQUEUE(wait, current); 3006311bb895SGustavo F. Padovan struct l2cap_chan *chan = 3007311bb895SGustavo F. Padovan container_of(work, struct l2cap_chan, busy_work); 3008311bb895SGustavo F. Padovan struct sock *sk = chan->sk; 30090a708f8fSGustavo F. Padovan int n_tries = 0, timeo = HZ/5, err; 30100a708f8fSGustavo F. Padovan struct sk_buff *skb; 30110a708f8fSGustavo F. Padovan 30120a708f8fSGustavo F. Padovan lock_sock(sk); 30130a708f8fSGustavo F. Padovan 30140a708f8fSGustavo F. Padovan add_wait_queue(sk_sleep(sk), &wait); 3015311bb895SGustavo F. Padovan while ((skb = skb_peek(&chan->busy_q))) { 30160a708f8fSGustavo F. Padovan set_current_state(TASK_INTERRUPTIBLE); 30170a708f8fSGustavo F. Padovan 30180a708f8fSGustavo F. Padovan if (n_tries++ > L2CAP_LOCAL_BUSY_TRIES) { 30190a708f8fSGustavo F. Padovan err = -EBUSY; 3020311bb895SGustavo F. Padovan l2cap_send_disconn_req(l2cap_pi(sk)->conn, chan, EBUSY); 30210a708f8fSGustavo F. Padovan break; 30220a708f8fSGustavo F. Padovan } 30230a708f8fSGustavo F. Padovan 30240a708f8fSGustavo F. Padovan if (!timeo) 30250a708f8fSGustavo F. Padovan timeo = HZ/5; 30260a708f8fSGustavo F. Padovan 30270a708f8fSGustavo F. Padovan if (signal_pending(current)) { 30280a708f8fSGustavo F. Padovan err = sock_intr_errno(timeo); 30290a708f8fSGustavo F. Padovan break; 30300a708f8fSGustavo F. Padovan } 30310a708f8fSGustavo F. Padovan 30320a708f8fSGustavo F. Padovan release_sock(sk); 30330a708f8fSGustavo F. Padovan timeo = schedule_timeout(timeo); 30340a708f8fSGustavo F. Padovan lock_sock(sk); 30350a708f8fSGustavo F. Padovan 30360a708f8fSGustavo F. Padovan err = sock_error(sk); 30370a708f8fSGustavo F. Padovan if (err) 30380a708f8fSGustavo F. Padovan break; 30390a708f8fSGustavo F. Padovan 3040311bb895SGustavo F. Padovan if (l2cap_try_push_rx_skb(chan) == 0) 30410a708f8fSGustavo F. Padovan break; 30420a708f8fSGustavo F. Padovan } 30430a708f8fSGustavo F. Padovan 30440a708f8fSGustavo F. Padovan set_current_state(TASK_RUNNING); 30450a708f8fSGustavo F. Padovan remove_wait_queue(sk_sleep(sk), &wait); 30460a708f8fSGustavo F. Padovan 30470a708f8fSGustavo F. Padovan release_sock(sk); 30480a708f8fSGustavo F. Padovan } 30490a708f8fSGustavo F. Padovan 3050525cd185SGustavo F. Padovan static int l2cap_push_rx_skb(struct l2cap_chan *chan, struct sk_buff *skb, u16 control) 30510a708f8fSGustavo F. Padovan { 30520a708f8fSGustavo F. Padovan int sctrl, err; 30530a708f8fSGustavo F. Padovan 3054525cd185SGustavo F. Padovan if (chan->conn_state & L2CAP_CONN_LOCAL_BUSY) { 30550a708f8fSGustavo F. Padovan bt_cb(skb)->sar = control >> L2CAP_CTRL_SAR_SHIFT; 3056f1c6775bSGustavo F. Padovan __skb_queue_tail(&chan->busy_q, skb); 3057525cd185SGustavo F. Padovan return l2cap_try_push_rx_skb(chan); 30580a708f8fSGustavo F. Padovan 30590a708f8fSGustavo F. Padovan 30600a708f8fSGustavo F. Padovan } 30610a708f8fSGustavo F. Padovan 3062525cd185SGustavo F. Padovan err = l2cap_ertm_reassembly_sdu(chan, skb, control); 30630a708f8fSGustavo F. Padovan if (err >= 0) { 306442e5c802SGustavo F. Padovan chan->buffer_seq = (chan->buffer_seq + 1) % 64; 30650a708f8fSGustavo F. Padovan return err; 30660a708f8fSGustavo F. Padovan } 30670a708f8fSGustavo F. Padovan 30680a708f8fSGustavo F. Padovan /* Busy Condition */ 3069311bb895SGustavo F. Padovan BT_DBG("chan %p, Enter local busy", chan); 30700a708f8fSGustavo F. Padovan 3071525cd185SGustavo F. Padovan chan->conn_state |= L2CAP_CONN_LOCAL_BUSY; 30720a708f8fSGustavo F. Padovan bt_cb(skb)->sar = control >> L2CAP_CTRL_SAR_SHIFT; 3073f1c6775bSGustavo F. Padovan __skb_queue_tail(&chan->busy_q, skb); 30740a708f8fSGustavo F. Padovan 307542e5c802SGustavo F. Padovan sctrl = chan->buffer_seq << L2CAP_CTRL_REQSEQ_SHIFT; 30760a708f8fSGustavo F. Padovan sctrl |= L2CAP_SUPER_RCV_NOT_READY; 3077525cd185SGustavo F. Padovan l2cap_send_sframe(chan, sctrl); 30780a708f8fSGustavo F. Padovan 3079525cd185SGustavo F. Padovan chan->conn_state |= L2CAP_CONN_RNR_SENT; 30800a708f8fSGustavo F. Padovan 3081e92c8e70SGustavo F. Padovan del_timer(&chan->ack_timer); 30820a708f8fSGustavo F. Padovan 3083311bb895SGustavo F. Padovan queue_work(_busy_wq, &chan->busy_work); 30840a708f8fSGustavo F. Padovan 30850a708f8fSGustavo F. Padovan return err; 30860a708f8fSGustavo F. Padovan } 30870a708f8fSGustavo F. Padovan 3088525cd185SGustavo F. Padovan static int l2cap_streaming_reassembly_sdu(struct l2cap_chan *chan, struct sk_buff *skb, u16 control) 30890a708f8fSGustavo F. Padovan { 3090525cd185SGustavo F. Padovan struct l2cap_pinfo *pi = l2cap_pi(chan->sk); 30910a708f8fSGustavo F. Padovan struct sk_buff *_skb; 30920a708f8fSGustavo F. Padovan int err = -EINVAL; 30930a708f8fSGustavo F. Padovan 30940a708f8fSGustavo F. Padovan /* 30950a708f8fSGustavo F. Padovan * TODO: We have to notify the userland if some data is lost with the 30960a708f8fSGustavo F. Padovan * Streaming Mode. 30970a708f8fSGustavo F. Padovan */ 30980a708f8fSGustavo F. Padovan 30990a708f8fSGustavo F. Padovan switch (control & L2CAP_CTRL_SAR) { 31000a708f8fSGustavo F. Padovan case L2CAP_SDU_UNSEGMENTED: 3101525cd185SGustavo F. Padovan if (chan->conn_state & L2CAP_CONN_SAR_SDU) { 31026f61fd47SGustavo F. Padovan kfree_skb(chan->sdu); 31030a708f8fSGustavo F. Padovan break; 31040a708f8fSGustavo F. Padovan } 31050a708f8fSGustavo F. Padovan 3106525cd185SGustavo F. Padovan err = sock_queue_rcv_skb(chan->sk, skb); 31070a708f8fSGustavo F. Padovan if (!err) 31080a708f8fSGustavo F. Padovan return 0; 31090a708f8fSGustavo F. Padovan 31100a708f8fSGustavo F. Padovan break; 31110a708f8fSGustavo F. Padovan 31120a708f8fSGustavo F. Padovan case L2CAP_SDU_START: 3113525cd185SGustavo F. Padovan if (chan->conn_state & L2CAP_CONN_SAR_SDU) { 31146f61fd47SGustavo F. Padovan kfree_skb(chan->sdu); 31150a708f8fSGustavo F. Padovan break; 31160a708f8fSGustavo F. Padovan } 31170a708f8fSGustavo F. Padovan 31186f61fd47SGustavo F. Padovan chan->sdu_len = get_unaligned_le16(skb->data); 31190a708f8fSGustavo F. Padovan skb_pull(skb, 2); 31200a708f8fSGustavo F. Padovan 31216f61fd47SGustavo F. Padovan if (chan->sdu_len > pi->imtu) { 31220a708f8fSGustavo F. Padovan err = -EMSGSIZE; 31230a708f8fSGustavo F. Padovan break; 31240a708f8fSGustavo F. Padovan } 31250a708f8fSGustavo F. Padovan 31266f61fd47SGustavo F. Padovan chan->sdu = bt_skb_alloc(chan->sdu_len, GFP_ATOMIC); 31276f61fd47SGustavo F. Padovan if (!chan->sdu) { 31280a708f8fSGustavo F. Padovan err = -ENOMEM; 31290a708f8fSGustavo F. Padovan break; 31300a708f8fSGustavo F. Padovan } 31310a708f8fSGustavo F. Padovan 31326f61fd47SGustavo F. Padovan memcpy(skb_put(chan->sdu, skb->len), skb->data, skb->len); 31330a708f8fSGustavo F. Padovan 3134525cd185SGustavo F. Padovan chan->conn_state |= L2CAP_CONN_SAR_SDU; 31356f61fd47SGustavo F. Padovan chan->partial_sdu_len = skb->len; 31360a708f8fSGustavo F. Padovan err = 0; 31370a708f8fSGustavo F. Padovan break; 31380a708f8fSGustavo F. Padovan 31390a708f8fSGustavo F. Padovan case L2CAP_SDU_CONTINUE: 3140525cd185SGustavo F. Padovan if (!(chan->conn_state & L2CAP_CONN_SAR_SDU)) 31410a708f8fSGustavo F. Padovan break; 31420a708f8fSGustavo F. Padovan 31436f61fd47SGustavo F. Padovan memcpy(skb_put(chan->sdu, skb->len), skb->data, skb->len); 31440a708f8fSGustavo F. Padovan 31456f61fd47SGustavo F. Padovan chan->partial_sdu_len += skb->len; 31466f61fd47SGustavo F. Padovan if (chan->partial_sdu_len > chan->sdu_len) 31476f61fd47SGustavo F. Padovan kfree_skb(chan->sdu); 31480a708f8fSGustavo F. Padovan else 31490a708f8fSGustavo F. Padovan err = 0; 31500a708f8fSGustavo F. Padovan 31510a708f8fSGustavo F. Padovan break; 31520a708f8fSGustavo F. Padovan 31530a708f8fSGustavo F. Padovan case L2CAP_SDU_END: 3154525cd185SGustavo F. Padovan if (!(chan->conn_state & L2CAP_CONN_SAR_SDU)) 31550a708f8fSGustavo F. Padovan break; 31560a708f8fSGustavo F. Padovan 31576f61fd47SGustavo F. Padovan memcpy(skb_put(chan->sdu, skb->len), skb->data, skb->len); 31580a708f8fSGustavo F. Padovan 3159525cd185SGustavo F. Padovan chan->conn_state &= ~L2CAP_CONN_SAR_SDU; 31606f61fd47SGustavo F. Padovan chan->partial_sdu_len += skb->len; 31610a708f8fSGustavo F. Padovan 31626f61fd47SGustavo F. Padovan if (chan->partial_sdu_len > pi->imtu) 31630a708f8fSGustavo F. Padovan goto drop; 31640a708f8fSGustavo F. Padovan 31656f61fd47SGustavo F. Padovan if (chan->partial_sdu_len == chan->sdu_len) { 31666f61fd47SGustavo F. Padovan _skb = skb_clone(chan->sdu, GFP_ATOMIC); 3167525cd185SGustavo F. Padovan err = sock_queue_rcv_skb(chan->sk, _skb); 31680a708f8fSGustavo F. Padovan if (err < 0) 31690a708f8fSGustavo F. Padovan kfree_skb(_skb); 31700a708f8fSGustavo F. Padovan } 31710a708f8fSGustavo F. Padovan err = 0; 31720a708f8fSGustavo F. Padovan 31730a708f8fSGustavo F. Padovan drop: 31746f61fd47SGustavo F. Padovan kfree_skb(chan->sdu); 31750a708f8fSGustavo F. Padovan break; 31760a708f8fSGustavo F. Padovan } 31770a708f8fSGustavo F. Padovan 31780a708f8fSGustavo F. Padovan kfree_skb(skb); 31790a708f8fSGustavo F. Padovan return err; 31800a708f8fSGustavo F. Padovan } 31810a708f8fSGustavo F. Padovan 3182525cd185SGustavo F. Padovan static void l2cap_check_srej_gap(struct l2cap_chan *chan, u8 tx_seq) 31830a708f8fSGustavo F. Padovan { 31840a708f8fSGustavo F. Padovan struct sk_buff *skb; 31850a708f8fSGustavo F. Padovan u16 control; 31860a708f8fSGustavo F. Padovan 3187f1c6775bSGustavo F. Padovan while ((skb = skb_peek(&chan->srej_q))) { 31880a708f8fSGustavo F. Padovan if (bt_cb(skb)->tx_seq != tx_seq) 31890a708f8fSGustavo F. Padovan break; 31900a708f8fSGustavo F. Padovan 3191f1c6775bSGustavo F. Padovan skb = skb_dequeue(&chan->srej_q); 31920a708f8fSGustavo F. Padovan control = bt_cb(skb)->sar << L2CAP_CTRL_SAR_SHIFT; 3193525cd185SGustavo F. Padovan l2cap_ertm_reassembly_sdu(chan, skb, control); 319442e5c802SGustavo F. Padovan chan->buffer_seq_srej = 319542e5c802SGustavo F. Padovan (chan->buffer_seq_srej + 1) % 64; 31960a708f8fSGustavo F. Padovan tx_seq = (tx_seq + 1) % 64; 31970a708f8fSGustavo F. Padovan } 31980a708f8fSGustavo F. Padovan } 31990a708f8fSGustavo F. Padovan 3200525cd185SGustavo F. Padovan static void l2cap_resend_srejframe(struct l2cap_chan *chan, u8 tx_seq) 32010a708f8fSGustavo F. Padovan { 32020a708f8fSGustavo F. Padovan struct srej_list *l, *tmp; 32030a708f8fSGustavo F. Padovan u16 control; 32040a708f8fSGustavo F. Padovan 320539d5a3eeSGustavo F. Padovan list_for_each_entry_safe(l, tmp, &chan->srej_l, list) { 32060a708f8fSGustavo F. Padovan if (l->tx_seq == tx_seq) { 32070a708f8fSGustavo F. Padovan list_del(&l->list); 32080a708f8fSGustavo F. Padovan kfree(l); 32090a708f8fSGustavo F. Padovan return; 32100a708f8fSGustavo F. Padovan } 32110a708f8fSGustavo F. Padovan control = L2CAP_SUPER_SELECT_REJECT; 32120a708f8fSGustavo F. Padovan control |= l->tx_seq << L2CAP_CTRL_REQSEQ_SHIFT; 3213525cd185SGustavo F. Padovan l2cap_send_sframe(chan, control); 32140a708f8fSGustavo F. Padovan list_del(&l->list); 321539d5a3eeSGustavo F. Padovan list_add_tail(&l->list, &chan->srej_l); 32160a708f8fSGustavo F. Padovan } 32170a708f8fSGustavo F. Padovan } 32180a708f8fSGustavo F. Padovan 3219525cd185SGustavo F. Padovan static void l2cap_send_srejframe(struct l2cap_chan *chan, u8 tx_seq) 32200a708f8fSGustavo F. Padovan { 32210a708f8fSGustavo F. Padovan struct srej_list *new; 32220a708f8fSGustavo F. Padovan u16 control; 32230a708f8fSGustavo F. Padovan 322442e5c802SGustavo F. Padovan while (tx_seq != chan->expected_tx_seq) { 32250a708f8fSGustavo F. Padovan control = L2CAP_SUPER_SELECT_REJECT; 322642e5c802SGustavo F. Padovan control |= chan->expected_tx_seq << L2CAP_CTRL_REQSEQ_SHIFT; 3227525cd185SGustavo F. Padovan l2cap_send_sframe(chan, control); 32280a708f8fSGustavo F. Padovan 32290a708f8fSGustavo F. Padovan new = kzalloc(sizeof(struct srej_list), GFP_ATOMIC); 323042e5c802SGustavo F. Padovan new->tx_seq = chan->expected_tx_seq; 323142e5c802SGustavo F. Padovan chan->expected_tx_seq = (chan->expected_tx_seq + 1) % 64; 323239d5a3eeSGustavo F. Padovan list_add_tail(&new->list, &chan->srej_l); 32330a708f8fSGustavo F. Padovan } 323442e5c802SGustavo F. Padovan chan->expected_tx_seq = (chan->expected_tx_seq + 1) % 64; 32350a708f8fSGustavo F. Padovan } 32360a708f8fSGustavo F. Padovan 3237525cd185SGustavo F. Padovan static inline int l2cap_data_channel_iframe(struct l2cap_chan *chan, u16 rx_control, struct sk_buff *skb) 32380a708f8fSGustavo F. Padovan { 323949208c9cSGustavo F. Padovan struct l2cap_pinfo *pi = l2cap_pi(chan->sk); 32400a708f8fSGustavo F. Padovan u8 tx_seq = __get_txseq(rx_control); 32410a708f8fSGustavo F. Padovan u8 req_seq = __get_reqseq(rx_control); 32420a708f8fSGustavo F. Padovan u8 sar = rx_control >> L2CAP_CTRL_SAR_SHIFT; 32430a708f8fSGustavo F. Padovan int tx_seq_offset, expected_tx_seq_offset; 32440a708f8fSGustavo F. Padovan int num_to_ack = (pi->tx_win/6) + 1; 32450a708f8fSGustavo F. Padovan int err = 0; 32460a708f8fSGustavo F. Padovan 3247525cd185SGustavo F. Padovan BT_DBG("chan %p len %d tx_seq %d rx_control 0x%4.4x", chan, skb->len, 3248525cd185SGustavo F. Padovan tx_seq, rx_control); 32490a708f8fSGustavo F. Padovan 32500a708f8fSGustavo F. Padovan if (L2CAP_CTRL_FINAL & rx_control && 3251525cd185SGustavo F. Padovan chan->conn_state & L2CAP_CONN_WAIT_F) { 3252e92c8e70SGustavo F. Padovan del_timer(&chan->monitor_timer); 32536a026610SGustavo F. Padovan if (chan->unacked_frames > 0) 32540a708f8fSGustavo F. Padovan __mod_retrans_timer(); 3255525cd185SGustavo F. Padovan chan->conn_state &= ~L2CAP_CONN_WAIT_F; 32560a708f8fSGustavo F. Padovan } 32570a708f8fSGustavo F. Padovan 325842e5c802SGustavo F. Padovan chan->expected_ack_seq = req_seq; 325942e5c802SGustavo F. Padovan l2cap_drop_acked_frames(chan); 32600a708f8fSGustavo F. Padovan 326142e5c802SGustavo F. Padovan if (tx_seq == chan->expected_tx_seq) 32620a708f8fSGustavo F. Padovan goto expected; 32630a708f8fSGustavo F. Padovan 326442e5c802SGustavo F. Padovan tx_seq_offset = (tx_seq - chan->buffer_seq) % 64; 32650a708f8fSGustavo F. Padovan if (tx_seq_offset < 0) 32660a708f8fSGustavo F. Padovan tx_seq_offset += 64; 32670a708f8fSGustavo F. Padovan 32680a708f8fSGustavo F. Padovan /* invalid tx_seq */ 32690a708f8fSGustavo F. Padovan if (tx_seq_offset >= pi->tx_win) { 3270e92c8e70SGustavo F. Padovan l2cap_send_disconn_req(pi->conn, chan, ECONNRESET); 32710a708f8fSGustavo F. Padovan goto drop; 32720a708f8fSGustavo F. Padovan } 32730a708f8fSGustavo F. Padovan 3274525cd185SGustavo F. Padovan if (chan->conn_state == L2CAP_CONN_LOCAL_BUSY) 32750a708f8fSGustavo F. Padovan goto drop; 32760a708f8fSGustavo F. Padovan 3277525cd185SGustavo F. Padovan if (chan->conn_state & L2CAP_CONN_SREJ_SENT) { 32780a708f8fSGustavo F. Padovan struct srej_list *first; 32790a708f8fSGustavo F. Padovan 328039d5a3eeSGustavo F. Padovan first = list_first_entry(&chan->srej_l, 32810a708f8fSGustavo F. Padovan struct srej_list, list); 32820a708f8fSGustavo F. Padovan if (tx_seq == first->tx_seq) { 328342e5c802SGustavo F. Padovan l2cap_add_to_srej_queue(chan, skb, tx_seq, sar); 3284525cd185SGustavo F. Padovan l2cap_check_srej_gap(chan, tx_seq); 32850a708f8fSGustavo F. Padovan 32860a708f8fSGustavo F. Padovan list_del(&first->list); 32870a708f8fSGustavo F. Padovan kfree(first); 32880a708f8fSGustavo F. Padovan 328939d5a3eeSGustavo F. Padovan if (list_empty(&chan->srej_l)) { 329042e5c802SGustavo F. Padovan chan->buffer_seq = chan->buffer_seq_srej; 3291525cd185SGustavo F. Padovan chan->conn_state &= ~L2CAP_CONN_SREJ_SENT; 3292525cd185SGustavo F. Padovan l2cap_send_ack(chan); 329349208c9cSGustavo F. Padovan BT_DBG("chan %p, Exit SREJ_SENT", chan); 32940a708f8fSGustavo F. Padovan } 32950a708f8fSGustavo F. Padovan } else { 32960a708f8fSGustavo F. Padovan struct srej_list *l; 32970a708f8fSGustavo F. Padovan 32980a708f8fSGustavo F. Padovan /* duplicated tx_seq */ 329942e5c802SGustavo F. Padovan if (l2cap_add_to_srej_queue(chan, skb, tx_seq, sar) < 0) 33000a708f8fSGustavo F. Padovan goto drop; 33010a708f8fSGustavo F. Padovan 330239d5a3eeSGustavo F. Padovan list_for_each_entry(l, &chan->srej_l, list) { 33030a708f8fSGustavo F. Padovan if (l->tx_seq == tx_seq) { 3304525cd185SGustavo F. Padovan l2cap_resend_srejframe(chan, tx_seq); 33050a708f8fSGustavo F. Padovan return 0; 33060a708f8fSGustavo F. Padovan } 33070a708f8fSGustavo F. Padovan } 3308525cd185SGustavo F. Padovan l2cap_send_srejframe(chan, tx_seq); 33090a708f8fSGustavo F. Padovan } 33100a708f8fSGustavo F. Padovan } else { 33110a708f8fSGustavo F. Padovan expected_tx_seq_offset = 331242e5c802SGustavo F. Padovan (chan->expected_tx_seq - chan->buffer_seq) % 64; 33130a708f8fSGustavo F. Padovan if (expected_tx_seq_offset < 0) 33140a708f8fSGustavo F. Padovan expected_tx_seq_offset += 64; 33150a708f8fSGustavo F. Padovan 33160a708f8fSGustavo F. Padovan /* duplicated tx_seq */ 33170a708f8fSGustavo F. Padovan if (tx_seq_offset < expected_tx_seq_offset) 33180a708f8fSGustavo F. Padovan goto drop; 33190a708f8fSGustavo F. Padovan 3320525cd185SGustavo F. Padovan chan->conn_state |= L2CAP_CONN_SREJ_SENT; 33210a708f8fSGustavo F. Padovan 332249208c9cSGustavo F. Padovan BT_DBG("chan %p, Enter SREJ", chan); 33230a708f8fSGustavo F. Padovan 332439d5a3eeSGustavo F. Padovan INIT_LIST_HEAD(&chan->srej_l); 332542e5c802SGustavo F. Padovan chan->buffer_seq_srej = chan->buffer_seq; 33260a708f8fSGustavo F. Padovan 3327f1c6775bSGustavo F. Padovan __skb_queue_head_init(&chan->srej_q); 3328f1c6775bSGustavo F. Padovan __skb_queue_head_init(&chan->busy_q); 332942e5c802SGustavo F. Padovan l2cap_add_to_srej_queue(chan, skb, tx_seq, sar); 33300a708f8fSGustavo F. Padovan 3331525cd185SGustavo F. Padovan chan->conn_state |= L2CAP_CONN_SEND_PBIT; 33320a708f8fSGustavo F. Padovan 3333525cd185SGustavo F. Padovan l2cap_send_srejframe(chan, tx_seq); 33340a708f8fSGustavo F. Padovan 3335e92c8e70SGustavo F. Padovan del_timer(&chan->ack_timer); 33360a708f8fSGustavo F. Padovan } 33370a708f8fSGustavo F. Padovan return 0; 33380a708f8fSGustavo F. Padovan 33390a708f8fSGustavo F. Padovan expected: 334042e5c802SGustavo F. Padovan chan->expected_tx_seq = (chan->expected_tx_seq + 1) % 64; 33410a708f8fSGustavo F. Padovan 3342525cd185SGustavo F. Padovan if (chan->conn_state & L2CAP_CONN_SREJ_SENT) { 33430a708f8fSGustavo F. Padovan bt_cb(skb)->tx_seq = tx_seq; 33440a708f8fSGustavo F. Padovan bt_cb(skb)->sar = sar; 3345f1c6775bSGustavo F. Padovan __skb_queue_tail(&chan->srej_q, skb); 33460a708f8fSGustavo F. Padovan return 0; 33470a708f8fSGustavo F. Padovan } 33480a708f8fSGustavo F. Padovan 3349525cd185SGustavo F. Padovan err = l2cap_push_rx_skb(chan, skb, rx_control); 33500a708f8fSGustavo F. Padovan if (err < 0) 33510a708f8fSGustavo F. Padovan return 0; 33520a708f8fSGustavo F. Padovan 33530a708f8fSGustavo F. Padovan if (rx_control & L2CAP_CTRL_FINAL) { 3354525cd185SGustavo F. Padovan if (chan->conn_state & L2CAP_CONN_REJ_ACT) 3355525cd185SGustavo F. Padovan chan->conn_state &= ~L2CAP_CONN_REJ_ACT; 33560a708f8fSGustavo F. Padovan else 3357525cd185SGustavo F. Padovan l2cap_retransmit_frames(chan); 33580a708f8fSGustavo F. Padovan } 33590a708f8fSGustavo F. Padovan 33600a708f8fSGustavo F. Padovan __mod_ack_timer(); 33610a708f8fSGustavo F. Padovan 33626a026610SGustavo F. Padovan chan->num_acked = (chan->num_acked + 1) % num_to_ack; 33636a026610SGustavo F. Padovan if (chan->num_acked == num_to_ack - 1) 3364525cd185SGustavo F. Padovan l2cap_send_ack(chan); 33650a708f8fSGustavo F. Padovan 33660a708f8fSGustavo F. Padovan return 0; 33670a708f8fSGustavo F. Padovan 33680a708f8fSGustavo F. Padovan drop: 33690a708f8fSGustavo F. Padovan kfree_skb(skb); 33700a708f8fSGustavo F. Padovan return 0; 33710a708f8fSGustavo F. Padovan } 33720a708f8fSGustavo F. Padovan 3373525cd185SGustavo F. Padovan static inline void l2cap_data_channel_rrframe(struct l2cap_chan *chan, u16 rx_control) 33740a708f8fSGustavo F. Padovan { 337549208c9cSGustavo F. Padovan BT_DBG("chan %p, req_seq %d ctrl 0x%4.4x", chan, __get_reqseq(rx_control), 33760a708f8fSGustavo F. Padovan rx_control); 33770a708f8fSGustavo F. Padovan 337842e5c802SGustavo F. Padovan chan->expected_ack_seq = __get_reqseq(rx_control); 337942e5c802SGustavo F. Padovan l2cap_drop_acked_frames(chan); 33800a708f8fSGustavo F. Padovan 33810a708f8fSGustavo F. Padovan if (rx_control & L2CAP_CTRL_POLL) { 3382525cd185SGustavo F. Padovan chan->conn_state |= L2CAP_CONN_SEND_FBIT; 3383525cd185SGustavo F. Padovan if (chan->conn_state & L2CAP_CONN_SREJ_SENT) { 3384525cd185SGustavo F. Padovan if ((chan->conn_state & L2CAP_CONN_REMOTE_BUSY) && 33856a026610SGustavo F. Padovan (chan->unacked_frames > 0)) 33860a708f8fSGustavo F. Padovan __mod_retrans_timer(); 33870a708f8fSGustavo F. Padovan 3388525cd185SGustavo F. Padovan chan->conn_state &= ~L2CAP_CONN_REMOTE_BUSY; 3389525cd185SGustavo F. Padovan l2cap_send_srejtail(chan); 33900a708f8fSGustavo F. Padovan } else { 3391525cd185SGustavo F. Padovan l2cap_send_i_or_rr_or_rnr(chan); 33920a708f8fSGustavo F. Padovan } 33930a708f8fSGustavo F. Padovan 33940a708f8fSGustavo F. Padovan } else if (rx_control & L2CAP_CTRL_FINAL) { 3395525cd185SGustavo F. Padovan chan->conn_state &= ~L2CAP_CONN_REMOTE_BUSY; 33960a708f8fSGustavo F. Padovan 3397525cd185SGustavo F. Padovan if (chan->conn_state & L2CAP_CONN_REJ_ACT) 3398525cd185SGustavo F. Padovan chan->conn_state &= ~L2CAP_CONN_REJ_ACT; 33990a708f8fSGustavo F. Padovan else 3400525cd185SGustavo F. Padovan l2cap_retransmit_frames(chan); 34010a708f8fSGustavo F. Padovan 34020a708f8fSGustavo F. Padovan } else { 3403525cd185SGustavo F. Padovan if ((chan->conn_state & L2CAP_CONN_REMOTE_BUSY) && 34046a026610SGustavo F. Padovan (chan->unacked_frames > 0)) 34050a708f8fSGustavo F. Padovan __mod_retrans_timer(); 34060a708f8fSGustavo F. Padovan 3407525cd185SGustavo F. Padovan chan->conn_state &= ~L2CAP_CONN_REMOTE_BUSY; 3408525cd185SGustavo F. Padovan if (chan->conn_state & L2CAP_CONN_SREJ_SENT) 3409525cd185SGustavo F. Padovan l2cap_send_ack(chan); 34100a708f8fSGustavo F. Padovan else 3411525cd185SGustavo F. Padovan l2cap_ertm_send(chan); 34120a708f8fSGustavo F. Padovan } 34130a708f8fSGustavo F. Padovan } 34140a708f8fSGustavo F. Padovan 3415525cd185SGustavo F. Padovan static inline void l2cap_data_channel_rejframe(struct l2cap_chan *chan, u16 rx_control) 34160a708f8fSGustavo F. Padovan { 34170a708f8fSGustavo F. Padovan u8 tx_seq = __get_reqseq(rx_control); 34180a708f8fSGustavo F. Padovan 3419525cd185SGustavo F. Padovan BT_DBG("chan %p, req_seq %d ctrl 0x%4.4x", chan, tx_seq, rx_control); 34200a708f8fSGustavo F. Padovan 3421525cd185SGustavo F. Padovan chan->conn_state &= ~L2CAP_CONN_REMOTE_BUSY; 34220a708f8fSGustavo F. Padovan 342342e5c802SGustavo F. Padovan chan->expected_ack_seq = tx_seq; 342442e5c802SGustavo F. Padovan l2cap_drop_acked_frames(chan); 34250a708f8fSGustavo F. Padovan 34260a708f8fSGustavo F. Padovan if (rx_control & L2CAP_CTRL_FINAL) { 3427525cd185SGustavo F. Padovan if (chan->conn_state & L2CAP_CONN_REJ_ACT) 3428525cd185SGustavo F. Padovan chan->conn_state &= ~L2CAP_CONN_REJ_ACT; 34290a708f8fSGustavo F. Padovan else 3430525cd185SGustavo F. Padovan l2cap_retransmit_frames(chan); 34310a708f8fSGustavo F. Padovan } else { 3432525cd185SGustavo F. Padovan l2cap_retransmit_frames(chan); 34330a708f8fSGustavo F. Padovan 3434525cd185SGustavo F. Padovan if (chan->conn_state & L2CAP_CONN_WAIT_F) 3435525cd185SGustavo F. Padovan chan->conn_state |= L2CAP_CONN_REJ_ACT; 34360a708f8fSGustavo F. Padovan } 34370a708f8fSGustavo F. Padovan } 3438525cd185SGustavo F. Padovan static inline void l2cap_data_channel_srejframe(struct l2cap_chan *chan, u16 rx_control) 34390a708f8fSGustavo F. Padovan { 34400a708f8fSGustavo F. Padovan u8 tx_seq = __get_reqseq(rx_control); 34410a708f8fSGustavo F. Padovan 3442525cd185SGustavo F. Padovan BT_DBG("chan %p, req_seq %d ctrl 0x%4.4x", chan, tx_seq, rx_control); 34430a708f8fSGustavo F. Padovan 3444525cd185SGustavo F. Padovan chan->conn_state &= ~L2CAP_CONN_REMOTE_BUSY; 34450a708f8fSGustavo F. Padovan 34460a708f8fSGustavo F. Padovan if (rx_control & L2CAP_CTRL_POLL) { 344742e5c802SGustavo F. Padovan chan->expected_ack_seq = tx_seq; 344842e5c802SGustavo F. Padovan l2cap_drop_acked_frames(chan); 34490a708f8fSGustavo F. Padovan 3450525cd185SGustavo F. Padovan chan->conn_state |= L2CAP_CONN_SEND_FBIT; 3451525cd185SGustavo F. Padovan l2cap_retransmit_one_frame(chan, tx_seq); 34520a708f8fSGustavo F. Padovan 3453525cd185SGustavo F. Padovan l2cap_ertm_send(chan); 34540a708f8fSGustavo F. Padovan 3455525cd185SGustavo F. Padovan if (chan->conn_state & L2CAP_CONN_WAIT_F) { 34566a026610SGustavo F. Padovan chan->srej_save_reqseq = tx_seq; 3457525cd185SGustavo F. Padovan chan->conn_state |= L2CAP_CONN_SREJ_ACT; 34580a708f8fSGustavo F. Padovan } 34590a708f8fSGustavo F. Padovan } else if (rx_control & L2CAP_CTRL_FINAL) { 3460525cd185SGustavo F. Padovan if ((chan->conn_state & L2CAP_CONN_SREJ_ACT) && 34616a026610SGustavo F. Padovan chan->srej_save_reqseq == tx_seq) 3462525cd185SGustavo F. Padovan chan->conn_state &= ~L2CAP_CONN_SREJ_ACT; 34630a708f8fSGustavo F. Padovan else 3464525cd185SGustavo F. Padovan l2cap_retransmit_one_frame(chan, tx_seq); 34650a708f8fSGustavo F. Padovan } else { 3466525cd185SGustavo F. Padovan l2cap_retransmit_one_frame(chan, tx_seq); 3467525cd185SGustavo F. Padovan if (chan->conn_state & L2CAP_CONN_WAIT_F) { 34686a026610SGustavo F. Padovan chan->srej_save_reqseq = tx_seq; 3469525cd185SGustavo F. Padovan chan->conn_state |= L2CAP_CONN_SREJ_ACT; 34700a708f8fSGustavo F. Padovan } 34710a708f8fSGustavo F. Padovan } 34720a708f8fSGustavo F. Padovan } 34730a708f8fSGustavo F. Padovan 3474525cd185SGustavo F. Padovan static inline void l2cap_data_channel_rnrframe(struct l2cap_chan *chan, u16 rx_control) 34750a708f8fSGustavo F. Padovan { 34760a708f8fSGustavo F. Padovan u8 tx_seq = __get_reqseq(rx_control); 34770a708f8fSGustavo F. Padovan 3478525cd185SGustavo F. Padovan BT_DBG("chan %p, req_seq %d ctrl 0x%4.4x", chan, tx_seq, rx_control); 34790a708f8fSGustavo F. Padovan 3480525cd185SGustavo F. Padovan chan->conn_state |= L2CAP_CONN_REMOTE_BUSY; 348142e5c802SGustavo F. Padovan chan->expected_ack_seq = tx_seq; 348242e5c802SGustavo F. Padovan l2cap_drop_acked_frames(chan); 34830a708f8fSGustavo F. Padovan 34840a708f8fSGustavo F. Padovan if (rx_control & L2CAP_CTRL_POLL) 3485525cd185SGustavo F. Padovan chan->conn_state |= L2CAP_CONN_SEND_FBIT; 34860a708f8fSGustavo F. Padovan 3487525cd185SGustavo F. Padovan if (!(chan->conn_state & L2CAP_CONN_SREJ_SENT)) { 3488e92c8e70SGustavo F. Padovan del_timer(&chan->retrans_timer); 34890a708f8fSGustavo F. Padovan if (rx_control & L2CAP_CTRL_POLL) 3490525cd185SGustavo F. Padovan l2cap_send_rr_or_rnr(chan, L2CAP_CTRL_FINAL); 34910a708f8fSGustavo F. Padovan return; 34920a708f8fSGustavo F. Padovan } 34930a708f8fSGustavo F. Padovan 34940a708f8fSGustavo F. Padovan if (rx_control & L2CAP_CTRL_POLL) 3495525cd185SGustavo F. Padovan l2cap_send_srejtail(chan); 34960a708f8fSGustavo F. Padovan else 3497525cd185SGustavo F. Padovan l2cap_send_sframe(chan, L2CAP_SUPER_RCV_READY); 34980a708f8fSGustavo F. Padovan } 34990a708f8fSGustavo F. Padovan 3500525cd185SGustavo F. Padovan static inline int l2cap_data_channel_sframe(struct l2cap_chan *chan, u16 rx_control, struct sk_buff *skb) 35010a708f8fSGustavo F. Padovan { 3502525cd185SGustavo F. Padovan BT_DBG("chan %p rx_control 0x%4.4x len %d", chan, rx_control, skb->len); 35030a708f8fSGustavo F. Padovan 35040a708f8fSGustavo F. Padovan if (L2CAP_CTRL_FINAL & rx_control && 3505525cd185SGustavo F. Padovan chan->conn_state & L2CAP_CONN_WAIT_F) { 3506e92c8e70SGustavo F. Padovan del_timer(&chan->monitor_timer); 35076a026610SGustavo F. Padovan if (chan->unacked_frames > 0) 35080a708f8fSGustavo F. Padovan __mod_retrans_timer(); 3509525cd185SGustavo F. Padovan chan->conn_state &= ~L2CAP_CONN_WAIT_F; 35100a708f8fSGustavo F. Padovan } 35110a708f8fSGustavo F. Padovan 35120a708f8fSGustavo F. Padovan switch (rx_control & L2CAP_CTRL_SUPERVISE) { 35130a708f8fSGustavo F. Padovan case L2CAP_SUPER_RCV_READY: 3514525cd185SGustavo F. Padovan l2cap_data_channel_rrframe(chan, rx_control); 35150a708f8fSGustavo F. Padovan break; 35160a708f8fSGustavo F. Padovan 35170a708f8fSGustavo F. Padovan case L2CAP_SUPER_REJECT: 3518525cd185SGustavo F. Padovan l2cap_data_channel_rejframe(chan, rx_control); 35190a708f8fSGustavo F. Padovan break; 35200a708f8fSGustavo F. Padovan 35210a708f8fSGustavo F. Padovan case L2CAP_SUPER_SELECT_REJECT: 3522525cd185SGustavo F. Padovan l2cap_data_channel_srejframe(chan, rx_control); 35230a708f8fSGustavo F. Padovan break; 35240a708f8fSGustavo F. Padovan 35250a708f8fSGustavo F. Padovan case L2CAP_SUPER_RCV_NOT_READY: 3526525cd185SGustavo F. Padovan l2cap_data_channel_rnrframe(chan, rx_control); 35270a708f8fSGustavo F. Padovan break; 35280a708f8fSGustavo F. Padovan } 35290a708f8fSGustavo F. Padovan 35300a708f8fSGustavo F. Padovan kfree_skb(skb); 35310a708f8fSGustavo F. Padovan return 0; 35320a708f8fSGustavo F. Padovan } 35330a708f8fSGustavo F. Padovan 35340a708f8fSGustavo F. Padovan static int l2cap_ertm_data_rcv(struct sock *sk, struct sk_buff *skb) 35350a708f8fSGustavo F. Padovan { 3536525cd185SGustavo F. Padovan struct l2cap_chan *chan = l2cap_pi(sk)->chan; 35370a708f8fSGustavo F. Padovan struct l2cap_pinfo *pi = l2cap_pi(sk); 35380a708f8fSGustavo F. Padovan u16 control; 35390a708f8fSGustavo F. Padovan u8 req_seq; 35400a708f8fSGustavo F. Padovan int len, next_tx_seq_offset, req_seq_offset; 35410a708f8fSGustavo F. Padovan 35420a708f8fSGustavo F. Padovan control = get_unaligned_le16(skb->data); 35430a708f8fSGustavo F. Padovan skb_pull(skb, 2); 35440a708f8fSGustavo F. Padovan len = skb->len; 35450a708f8fSGustavo F. Padovan 35460a708f8fSGustavo F. Padovan /* 35470a708f8fSGustavo F. Padovan * We can just drop the corrupted I-frame here. 35480a708f8fSGustavo F. Padovan * Receiver will miss it and start proper recovery 35490a708f8fSGustavo F. Padovan * procedures and ask retransmission. 35500a708f8fSGustavo F. Padovan */ 35510a708f8fSGustavo F. Padovan if (l2cap_check_fcs(pi, skb)) 35520a708f8fSGustavo F. Padovan goto drop; 35530a708f8fSGustavo F. Padovan 35540a708f8fSGustavo F. Padovan if (__is_sar_start(control) && __is_iframe(control)) 35550a708f8fSGustavo F. Padovan len -= 2; 35560a708f8fSGustavo F. Padovan 35570a708f8fSGustavo F. Padovan if (pi->fcs == L2CAP_FCS_CRC16) 35580a708f8fSGustavo F. Padovan len -= 2; 35590a708f8fSGustavo F. Padovan 35600a708f8fSGustavo F. Padovan if (len > pi->mps) { 3561e92c8e70SGustavo F. Padovan l2cap_send_disconn_req(pi->conn, chan, ECONNRESET); 35620a708f8fSGustavo F. Padovan goto drop; 35630a708f8fSGustavo F. Padovan } 35640a708f8fSGustavo F. Padovan 35650a708f8fSGustavo F. Padovan req_seq = __get_reqseq(control); 356642e5c802SGustavo F. Padovan req_seq_offset = (req_seq - chan->expected_ack_seq) % 64; 35670a708f8fSGustavo F. Padovan if (req_seq_offset < 0) 35680a708f8fSGustavo F. Padovan req_seq_offset += 64; 35690a708f8fSGustavo F. Padovan 35700a708f8fSGustavo F. Padovan next_tx_seq_offset = 357142e5c802SGustavo F. Padovan (chan->next_tx_seq - chan->expected_ack_seq) % 64; 35720a708f8fSGustavo F. Padovan if (next_tx_seq_offset < 0) 35730a708f8fSGustavo F. Padovan next_tx_seq_offset += 64; 35740a708f8fSGustavo F. Padovan 35750a708f8fSGustavo F. Padovan /* check for invalid req-seq */ 35760a708f8fSGustavo F. Padovan if (req_seq_offset > next_tx_seq_offset) { 3577e92c8e70SGustavo F. Padovan l2cap_send_disconn_req(pi->conn, chan, ECONNRESET); 35780a708f8fSGustavo F. Padovan goto drop; 35790a708f8fSGustavo F. Padovan } 35800a708f8fSGustavo F. Padovan 35810a708f8fSGustavo F. Padovan if (__is_iframe(control)) { 35820a708f8fSGustavo F. Padovan if (len < 0) { 3583e92c8e70SGustavo F. Padovan l2cap_send_disconn_req(pi->conn, chan, ECONNRESET); 35840a708f8fSGustavo F. Padovan goto drop; 35850a708f8fSGustavo F. Padovan } 35860a708f8fSGustavo F. Padovan 3587525cd185SGustavo F. Padovan l2cap_data_channel_iframe(chan, control, skb); 35880a708f8fSGustavo F. Padovan } else { 35890a708f8fSGustavo F. Padovan if (len != 0) { 35900a708f8fSGustavo F. Padovan BT_ERR("%d", len); 3591e92c8e70SGustavo F. Padovan l2cap_send_disconn_req(pi->conn, chan, ECONNRESET); 35920a708f8fSGustavo F. Padovan goto drop; 35930a708f8fSGustavo F. Padovan } 35940a708f8fSGustavo F. Padovan 3595525cd185SGustavo F. Padovan l2cap_data_channel_sframe(chan, control, skb); 35960a708f8fSGustavo F. Padovan } 35970a708f8fSGustavo F. Padovan 35980a708f8fSGustavo F. Padovan return 0; 35990a708f8fSGustavo F. Padovan 36000a708f8fSGustavo F. Padovan drop: 36010a708f8fSGustavo F. Padovan kfree_skb(skb); 36020a708f8fSGustavo F. Padovan return 0; 36030a708f8fSGustavo F. Padovan } 36040a708f8fSGustavo F. Padovan 36050a708f8fSGustavo F. Padovan static inline int l2cap_data_channel(struct l2cap_conn *conn, u16 cid, struct sk_buff *skb) 36060a708f8fSGustavo F. Padovan { 360748454079SGustavo F. Padovan struct l2cap_chan *chan; 36080a708f8fSGustavo F. Padovan struct sock *sk; 36090a708f8fSGustavo F. Padovan struct l2cap_pinfo *pi; 36100a708f8fSGustavo F. Padovan u16 control; 36110a708f8fSGustavo F. Padovan u8 tx_seq; 36120a708f8fSGustavo F. Padovan int len; 36130a708f8fSGustavo F. Padovan 3614baa7e1faSGustavo F. Padovan chan = l2cap_get_chan_by_scid(conn, cid); 361548454079SGustavo F. Padovan if (!chan) { 36160a708f8fSGustavo F. Padovan BT_DBG("unknown cid 0x%4.4x", cid); 36170a708f8fSGustavo F. Padovan goto drop; 36180a708f8fSGustavo F. Padovan } 36190a708f8fSGustavo F. Padovan 362048454079SGustavo F. Padovan sk = chan->sk; 36210a708f8fSGustavo F. Padovan pi = l2cap_pi(sk); 36220a708f8fSGustavo F. Padovan 362349208c9cSGustavo F. Padovan BT_DBG("chan %p, len %d", chan, skb->len); 36240a708f8fSGustavo F. Padovan 36250a708f8fSGustavo F. Padovan if (sk->sk_state != BT_CONNECTED) 36260a708f8fSGustavo F. Padovan goto drop; 36270a708f8fSGustavo F. Padovan 36280a708f8fSGustavo F. Padovan switch (pi->mode) { 36290a708f8fSGustavo F. Padovan case L2CAP_MODE_BASIC: 36300a708f8fSGustavo F. Padovan /* If socket recv buffers overflows we drop data here 36310a708f8fSGustavo F. Padovan * which is *bad* because L2CAP has to be reliable. 36320a708f8fSGustavo F. Padovan * But we don't have any other choice. L2CAP doesn't 36330a708f8fSGustavo F. Padovan * provide flow control mechanism. */ 36340a708f8fSGustavo F. Padovan 36350a708f8fSGustavo F. Padovan if (pi->imtu < skb->len) 36360a708f8fSGustavo F. Padovan goto drop; 36370a708f8fSGustavo F. Padovan 36380a708f8fSGustavo F. Padovan if (!sock_queue_rcv_skb(sk, skb)) 36390a708f8fSGustavo F. Padovan goto done; 36400a708f8fSGustavo F. Padovan break; 36410a708f8fSGustavo F. Padovan 36420a708f8fSGustavo F. Padovan case L2CAP_MODE_ERTM: 36430a708f8fSGustavo F. Padovan if (!sock_owned_by_user(sk)) { 36440a708f8fSGustavo F. Padovan l2cap_ertm_data_rcv(sk, skb); 36450a708f8fSGustavo F. Padovan } else { 36460a708f8fSGustavo F. Padovan if (sk_add_backlog(sk, skb)) 36470a708f8fSGustavo F. Padovan goto drop; 36480a708f8fSGustavo F. Padovan } 36490a708f8fSGustavo F. Padovan 36500a708f8fSGustavo F. Padovan goto done; 36510a708f8fSGustavo F. Padovan 36520a708f8fSGustavo F. Padovan case L2CAP_MODE_STREAMING: 36530a708f8fSGustavo F. Padovan control = get_unaligned_le16(skb->data); 36540a708f8fSGustavo F. Padovan skb_pull(skb, 2); 36550a708f8fSGustavo F. Padovan len = skb->len; 36560a708f8fSGustavo F. Padovan 36570a708f8fSGustavo F. Padovan if (l2cap_check_fcs(pi, skb)) 36580a708f8fSGustavo F. Padovan goto drop; 36590a708f8fSGustavo F. Padovan 36600a708f8fSGustavo F. Padovan if (__is_sar_start(control)) 36610a708f8fSGustavo F. Padovan len -= 2; 36620a708f8fSGustavo F. Padovan 36630a708f8fSGustavo F. Padovan if (pi->fcs == L2CAP_FCS_CRC16) 36640a708f8fSGustavo F. Padovan len -= 2; 36650a708f8fSGustavo F. Padovan 36660a708f8fSGustavo F. Padovan if (len > pi->mps || len < 0 || __is_sframe(control)) 36670a708f8fSGustavo F. Padovan goto drop; 36680a708f8fSGustavo F. Padovan 36690a708f8fSGustavo F. Padovan tx_seq = __get_txseq(control); 36700a708f8fSGustavo F. Padovan 367142e5c802SGustavo F. Padovan if (chan->expected_tx_seq == tx_seq) 367242e5c802SGustavo F. Padovan chan->expected_tx_seq = (chan->expected_tx_seq + 1) % 64; 36730a708f8fSGustavo F. Padovan else 367442e5c802SGustavo F. Padovan chan->expected_tx_seq = (tx_seq + 1) % 64; 36750a708f8fSGustavo F. Padovan 3676525cd185SGustavo F. Padovan l2cap_streaming_reassembly_sdu(chan, skb, control); 36770a708f8fSGustavo F. Padovan 36780a708f8fSGustavo F. Padovan goto done; 36790a708f8fSGustavo F. Padovan 36800a708f8fSGustavo F. Padovan default: 368149208c9cSGustavo F. Padovan BT_DBG("chan %p: bad mode 0x%2.2x", chan, pi->mode); 36820a708f8fSGustavo F. Padovan break; 36830a708f8fSGustavo F. Padovan } 36840a708f8fSGustavo F. Padovan 36850a708f8fSGustavo F. Padovan drop: 36860a708f8fSGustavo F. Padovan kfree_skb(skb); 36870a708f8fSGustavo F. Padovan 36880a708f8fSGustavo F. Padovan done: 36890a708f8fSGustavo F. Padovan if (sk) 36900a708f8fSGustavo F. Padovan bh_unlock_sock(sk); 36910a708f8fSGustavo F. Padovan 36920a708f8fSGustavo F. Padovan return 0; 36930a708f8fSGustavo F. Padovan } 36940a708f8fSGustavo F. Padovan 36950a708f8fSGustavo F. Padovan static inline int l2cap_conless_channel(struct l2cap_conn *conn, __le16 psm, struct sk_buff *skb) 36960a708f8fSGustavo F. Padovan { 36970a708f8fSGustavo F. Padovan struct sock *sk; 36980a708f8fSGustavo F. Padovan 36990a708f8fSGustavo F. Padovan sk = l2cap_get_sock_by_psm(0, psm, conn->src); 37000a708f8fSGustavo F. Padovan if (!sk) 37010a708f8fSGustavo F. Padovan goto drop; 37020a708f8fSGustavo F. Padovan 37030a708f8fSGustavo F. Padovan bh_lock_sock(sk); 37040a708f8fSGustavo F. Padovan 37050a708f8fSGustavo F. Padovan BT_DBG("sk %p, len %d", sk, skb->len); 37060a708f8fSGustavo F. Padovan 37070a708f8fSGustavo F. Padovan if (sk->sk_state != BT_BOUND && sk->sk_state != BT_CONNECTED) 37080a708f8fSGustavo F. Padovan goto drop; 37090a708f8fSGustavo F. Padovan 37100a708f8fSGustavo F. Padovan if (l2cap_pi(sk)->imtu < skb->len) 37110a708f8fSGustavo F. Padovan goto drop; 37120a708f8fSGustavo F. Padovan 37130a708f8fSGustavo F. Padovan if (!sock_queue_rcv_skb(sk, skb)) 37140a708f8fSGustavo F. Padovan goto done; 37150a708f8fSGustavo F. Padovan 37160a708f8fSGustavo F. Padovan drop: 37170a708f8fSGustavo F. Padovan kfree_skb(skb); 37180a708f8fSGustavo F. Padovan 37190a708f8fSGustavo F. Padovan done: 37200a708f8fSGustavo F. Padovan if (sk) 37210a708f8fSGustavo F. Padovan bh_unlock_sock(sk); 37220a708f8fSGustavo F. Padovan return 0; 37230a708f8fSGustavo F. Padovan } 37240a708f8fSGustavo F. Padovan 37259f69bda6SGustavo F. Padovan static inline int l2cap_att_channel(struct l2cap_conn *conn, __le16 cid, struct sk_buff *skb) 37269f69bda6SGustavo F. Padovan { 37279f69bda6SGustavo F. Padovan struct sock *sk; 37289f69bda6SGustavo F. Padovan 37299f69bda6SGustavo F. Padovan sk = l2cap_get_sock_by_scid(0, cid, conn->src); 37309f69bda6SGustavo F. Padovan if (!sk) 37319f69bda6SGustavo F. Padovan goto drop; 37329f69bda6SGustavo F. Padovan 37339f69bda6SGustavo F. Padovan bh_lock_sock(sk); 37349f69bda6SGustavo F. Padovan 37359f69bda6SGustavo F. Padovan BT_DBG("sk %p, len %d", sk, skb->len); 37369f69bda6SGustavo F. Padovan 37379f69bda6SGustavo F. Padovan if (sk->sk_state != BT_BOUND && sk->sk_state != BT_CONNECTED) 37389f69bda6SGustavo F. Padovan goto drop; 37399f69bda6SGustavo F. Padovan 37409f69bda6SGustavo F. Padovan if (l2cap_pi(sk)->imtu < skb->len) 37419f69bda6SGustavo F. Padovan goto drop; 37429f69bda6SGustavo F. Padovan 37439f69bda6SGustavo F. Padovan if (!sock_queue_rcv_skb(sk, skb)) 37449f69bda6SGustavo F. Padovan goto done; 37459f69bda6SGustavo F. Padovan 37469f69bda6SGustavo F. Padovan drop: 37479f69bda6SGustavo F. Padovan kfree_skb(skb); 37489f69bda6SGustavo F. Padovan 37499f69bda6SGustavo F. Padovan done: 37509f69bda6SGustavo F. Padovan if (sk) 37519f69bda6SGustavo F. Padovan bh_unlock_sock(sk); 37529f69bda6SGustavo F. Padovan return 0; 37539f69bda6SGustavo F. Padovan } 37549f69bda6SGustavo F. Padovan 37550a708f8fSGustavo F. Padovan static void l2cap_recv_frame(struct l2cap_conn *conn, struct sk_buff *skb) 37560a708f8fSGustavo F. Padovan { 37570a708f8fSGustavo F. Padovan struct l2cap_hdr *lh = (void *) skb->data; 37580a708f8fSGustavo F. Padovan u16 cid, len; 37590a708f8fSGustavo F. Padovan __le16 psm; 37600a708f8fSGustavo F. Padovan 37610a708f8fSGustavo F. Padovan skb_pull(skb, L2CAP_HDR_SIZE); 37620a708f8fSGustavo F. Padovan cid = __le16_to_cpu(lh->cid); 37630a708f8fSGustavo F. Padovan len = __le16_to_cpu(lh->len); 37640a708f8fSGustavo F. Padovan 37650a708f8fSGustavo F. Padovan if (len != skb->len) { 37660a708f8fSGustavo F. Padovan kfree_skb(skb); 37670a708f8fSGustavo F. Padovan return; 37680a708f8fSGustavo F. Padovan } 37690a708f8fSGustavo F. Padovan 37700a708f8fSGustavo F. Padovan BT_DBG("len %d, cid 0x%4.4x", len, cid); 37710a708f8fSGustavo F. Padovan 37720a708f8fSGustavo F. Padovan switch (cid) { 37733300d9a9SClaudio Takahasi case L2CAP_CID_LE_SIGNALING: 37740a708f8fSGustavo F. Padovan case L2CAP_CID_SIGNALING: 37750a708f8fSGustavo F. Padovan l2cap_sig_channel(conn, skb); 37760a708f8fSGustavo F. Padovan break; 37770a708f8fSGustavo F. Padovan 37780a708f8fSGustavo F. Padovan case L2CAP_CID_CONN_LESS: 37790a708f8fSGustavo F. Padovan psm = get_unaligned_le16(skb->data); 37800a708f8fSGustavo F. Padovan skb_pull(skb, 2); 37810a708f8fSGustavo F. Padovan l2cap_conless_channel(conn, psm, skb); 37820a708f8fSGustavo F. Padovan break; 37830a708f8fSGustavo F. Padovan 37849f69bda6SGustavo F. Padovan case L2CAP_CID_LE_DATA: 37859f69bda6SGustavo F. Padovan l2cap_att_channel(conn, cid, skb); 37869f69bda6SGustavo F. Padovan break; 37879f69bda6SGustavo F. Padovan 37880a708f8fSGustavo F. Padovan default: 37890a708f8fSGustavo F. Padovan l2cap_data_channel(conn, cid, skb); 37900a708f8fSGustavo F. Padovan break; 37910a708f8fSGustavo F. Padovan } 37920a708f8fSGustavo F. Padovan } 37930a708f8fSGustavo F. Padovan 37940a708f8fSGustavo F. Padovan /* ---- L2CAP interface with lower layer (HCI) ---- */ 37950a708f8fSGustavo F. Padovan 37960a708f8fSGustavo F. Padovan static int l2cap_connect_ind(struct hci_dev *hdev, bdaddr_t *bdaddr, u8 type) 37970a708f8fSGustavo F. Padovan { 37980a708f8fSGustavo F. Padovan int exact = 0, lm1 = 0, lm2 = 0; 37990a708f8fSGustavo F. Padovan register struct sock *sk; 38000a708f8fSGustavo F. Padovan struct hlist_node *node; 38010a708f8fSGustavo F. Padovan 38020a708f8fSGustavo F. Padovan if (type != ACL_LINK) 38030a708f8fSGustavo F. Padovan return -EINVAL; 38040a708f8fSGustavo F. Padovan 38050a708f8fSGustavo F. Padovan BT_DBG("hdev %s, bdaddr %s", hdev->name, batostr(bdaddr)); 38060a708f8fSGustavo F. Padovan 38070a708f8fSGustavo F. Padovan /* Find listening sockets and check their link_mode */ 38080a708f8fSGustavo F. Padovan read_lock(&l2cap_sk_list.lock); 38090a708f8fSGustavo F. Padovan sk_for_each(sk, node, &l2cap_sk_list.head) { 3810*4343478fSGustavo F. Padovan struct l2cap_chan *chan = l2cap_pi(sk)->chan; 3811*4343478fSGustavo F. Padovan 38120a708f8fSGustavo F. Padovan if (sk->sk_state != BT_LISTEN) 38130a708f8fSGustavo F. Padovan continue; 38140a708f8fSGustavo F. Padovan 38150a708f8fSGustavo F. Padovan if (!bacmp(&bt_sk(sk)->src, &hdev->bdaddr)) { 38160a708f8fSGustavo F. Padovan lm1 |= HCI_LM_ACCEPT; 3817*4343478fSGustavo F. Padovan if (chan->role_switch) 38180a708f8fSGustavo F. Padovan lm1 |= HCI_LM_MASTER; 38190a708f8fSGustavo F. Padovan exact++; 38200a708f8fSGustavo F. Padovan } else if (!bacmp(&bt_sk(sk)->src, BDADDR_ANY)) { 38210a708f8fSGustavo F. Padovan lm2 |= HCI_LM_ACCEPT; 3822*4343478fSGustavo F. Padovan if (chan->role_switch) 38230a708f8fSGustavo F. Padovan lm2 |= HCI_LM_MASTER; 38240a708f8fSGustavo F. Padovan } 38250a708f8fSGustavo F. Padovan } 38260a708f8fSGustavo F. Padovan read_unlock(&l2cap_sk_list.lock); 38270a708f8fSGustavo F. Padovan 38280a708f8fSGustavo F. Padovan return exact ? lm1 : lm2; 38290a708f8fSGustavo F. Padovan } 38300a708f8fSGustavo F. Padovan 38310a708f8fSGustavo F. Padovan static int l2cap_connect_cfm(struct hci_conn *hcon, u8 status) 38320a708f8fSGustavo F. Padovan { 38330a708f8fSGustavo F. Padovan struct l2cap_conn *conn; 38340a708f8fSGustavo F. Padovan 38350a708f8fSGustavo F. Padovan BT_DBG("hcon %p bdaddr %s status %d", hcon, batostr(&hcon->dst), status); 38360a708f8fSGustavo F. Padovan 3837acd7d370SVille Tervo if (!(hcon->type == ACL_LINK || hcon->type == LE_LINK)) 38380a708f8fSGustavo F. Padovan return -EINVAL; 38390a708f8fSGustavo F. Padovan 38400a708f8fSGustavo F. Padovan if (!status) { 38410a708f8fSGustavo F. Padovan conn = l2cap_conn_add(hcon, status); 38420a708f8fSGustavo F. Padovan if (conn) 38430a708f8fSGustavo F. Padovan l2cap_conn_ready(conn); 38440a708f8fSGustavo F. Padovan } else 38450a708f8fSGustavo F. Padovan l2cap_conn_del(hcon, bt_err(status)); 38460a708f8fSGustavo F. Padovan 38470a708f8fSGustavo F. Padovan return 0; 38480a708f8fSGustavo F. Padovan } 38490a708f8fSGustavo F. Padovan 38500a708f8fSGustavo F. Padovan static int l2cap_disconn_ind(struct hci_conn *hcon) 38510a708f8fSGustavo F. Padovan { 38520a708f8fSGustavo F. Padovan struct l2cap_conn *conn = hcon->l2cap_data; 38530a708f8fSGustavo F. Padovan 38540a708f8fSGustavo F. Padovan BT_DBG("hcon %p", hcon); 38550a708f8fSGustavo F. Padovan 38560a708f8fSGustavo F. Padovan if (hcon->type != ACL_LINK || !conn) 38570a708f8fSGustavo F. Padovan return 0x13; 38580a708f8fSGustavo F. Padovan 38590a708f8fSGustavo F. Padovan return conn->disc_reason; 38600a708f8fSGustavo F. Padovan } 38610a708f8fSGustavo F. Padovan 38620a708f8fSGustavo F. Padovan static int l2cap_disconn_cfm(struct hci_conn *hcon, u8 reason) 38630a708f8fSGustavo F. Padovan { 38640a708f8fSGustavo F. Padovan BT_DBG("hcon %p reason %d", hcon, reason); 38650a708f8fSGustavo F. Padovan 3866acd7d370SVille Tervo if (!(hcon->type == ACL_LINK || hcon->type == LE_LINK)) 38670a708f8fSGustavo F. Padovan return -EINVAL; 38680a708f8fSGustavo F. Padovan 38690a708f8fSGustavo F. Padovan l2cap_conn_del(hcon, bt_err(reason)); 38700a708f8fSGustavo F. Padovan 38710a708f8fSGustavo F. Padovan return 0; 38720a708f8fSGustavo F. Padovan } 38730a708f8fSGustavo F. Padovan 3874*4343478fSGustavo F. Padovan static inline void l2cap_check_encryption(struct l2cap_chan *chan, u8 encrypt) 38750a708f8fSGustavo F. Padovan { 3876*4343478fSGustavo F. Padovan struct sock *sk = chan->sk; 3877*4343478fSGustavo F. Padovan 38780a708f8fSGustavo F. Padovan if (sk->sk_type != SOCK_SEQPACKET && sk->sk_type != SOCK_STREAM) 38790a708f8fSGustavo F. Padovan return; 38800a708f8fSGustavo F. Padovan 38810a708f8fSGustavo F. Padovan if (encrypt == 0x00) { 3882*4343478fSGustavo F. Padovan if (chan->sec_level == BT_SECURITY_MEDIUM) { 38830a708f8fSGustavo F. Padovan l2cap_sock_clear_timer(sk); 38840a708f8fSGustavo F. Padovan l2cap_sock_set_timer(sk, HZ * 5); 3885*4343478fSGustavo F. Padovan } else if (chan->sec_level == BT_SECURITY_HIGH) 38860a708f8fSGustavo F. Padovan __l2cap_sock_close(sk, ECONNREFUSED); 38870a708f8fSGustavo F. Padovan } else { 3888*4343478fSGustavo F. Padovan if (chan->sec_level == BT_SECURITY_MEDIUM) 38890a708f8fSGustavo F. Padovan l2cap_sock_clear_timer(sk); 38900a708f8fSGustavo F. Padovan } 38910a708f8fSGustavo F. Padovan } 38920a708f8fSGustavo F. Padovan 38930a708f8fSGustavo F. Padovan static int l2cap_security_cfm(struct hci_conn *hcon, u8 status, u8 encrypt) 38940a708f8fSGustavo F. Padovan { 38950a708f8fSGustavo F. Padovan struct l2cap_conn *conn = hcon->l2cap_data; 389648454079SGustavo F. Padovan struct l2cap_chan *chan; 38970a708f8fSGustavo F. Padovan 38980a708f8fSGustavo F. Padovan if (!conn) 38990a708f8fSGustavo F. Padovan return 0; 39000a708f8fSGustavo F. Padovan 39010a708f8fSGustavo F. Padovan BT_DBG("conn %p", conn); 39020a708f8fSGustavo F. Padovan 3903baa7e1faSGustavo F. Padovan read_lock(&conn->chan_lock); 39040a708f8fSGustavo F. Padovan 3905baa7e1faSGustavo F. Padovan list_for_each_entry(chan, &conn->chan_l, list) { 390648454079SGustavo F. Padovan struct sock *sk = chan->sk; 3907baa7e1faSGustavo F. Padovan 39080a708f8fSGustavo F. Padovan bh_lock_sock(sk); 39090a708f8fSGustavo F. Padovan 3910b4450035SGustavo F. Padovan if (chan->conf_state & L2CAP_CONF_CONNECT_PEND) { 39110a708f8fSGustavo F. Padovan bh_unlock_sock(sk); 39120a708f8fSGustavo F. Padovan continue; 39130a708f8fSGustavo F. Padovan } 39140a708f8fSGustavo F. Padovan 39150a708f8fSGustavo F. Padovan if (!status && (sk->sk_state == BT_CONNECTED || 39160a708f8fSGustavo F. Padovan sk->sk_state == BT_CONFIG)) { 3917*4343478fSGustavo F. Padovan l2cap_check_encryption(chan, encrypt); 39180a708f8fSGustavo F. Padovan bh_unlock_sock(sk); 39190a708f8fSGustavo F. Padovan continue; 39200a708f8fSGustavo F. Padovan } 39210a708f8fSGustavo F. Padovan 39220a708f8fSGustavo F. Padovan if (sk->sk_state == BT_CONNECT) { 39230a708f8fSGustavo F. Padovan if (!status) { 39240a708f8fSGustavo F. Padovan struct l2cap_conn_req req; 39250a708f8fSGustavo F. Padovan req.scid = cpu_to_le16(l2cap_pi(sk)->scid); 39260a708f8fSGustavo F. Padovan req.psm = l2cap_pi(sk)->psm; 39270a708f8fSGustavo F. Padovan 3928fc7f8a7eSGustavo F. Padovan chan->ident = l2cap_get_ident(conn); 3929b4450035SGustavo F. Padovan chan->conf_state |= L2CAP_CONF_CONNECT_PEND; 39300a708f8fSGustavo F. Padovan 3931fc7f8a7eSGustavo F. Padovan l2cap_send_cmd(conn, chan->ident, 39320a708f8fSGustavo F. Padovan L2CAP_CONN_REQ, sizeof(req), &req); 39330a708f8fSGustavo F. Padovan } else { 39340a708f8fSGustavo F. Padovan l2cap_sock_clear_timer(sk); 39350a708f8fSGustavo F. Padovan l2cap_sock_set_timer(sk, HZ / 10); 39360a708f8fSGustavo F. Padovan } 39370a708f8fSGustavo F. Padovan } else if (sk->sk_state == BT_CONNECT2) { 39380a708f8fSGustavo F. Padovan struct l2cap_conn_rsp rsp; 39390a708f8fSGustavo F. Padovan __u16 result; 39400a708f8fSGustavo F. Padovan 39410a708f8fSGustavo F. Padovan if (!status) { 39420a708f8fSGustavo F. Padovan sk->sk_state = BT_CONFIG; 39430a708f8fSGustavo F. Padovan result = L2CAP_CR_SUCCESS; 39440a708f8fSGustavo F. Padovan } else { 39450a708f8fSGustavo F. Padovan sk->sk_state = BT_DISCONN; 39460a708f8fSGustavo F. Padovan l2cap_sock_set_timer(sk, HZ / 10); 39470a708f8fSGustavo F. Padovan result = L2CAP_CR_SEC_BLOCK; 39480a708f8fSGustavo F. Padovan } 39490a708f8fSGustavo F. Padovan 39500a708f8fSGustavo F. Padovan rsp.scid = cpu_to_le16(l2cap_pi(sk)->dcid); 39510a708f8fSGustavo F. Padovan rsp.dcid = cpu_to_le16(l2cap_pi(sk)->scid); 39520a708f8fSGustavo F. Padovan rsp.result = cpu_to_le16(result); 39530a708f8fSGustavo F. Padovan rsp.status = cpu_to_le16(L2CAP_CS_NO_INFO); 3954fc7f8a7eSGustavo F. Padovan l2cap_send_cmd(conn, chan->ident, L2CAP_CONN_RSP, 3955fc7f8a7eSGustavo F. Padovan sizeof(rsp), &rsp); 39560a708f8fSGustavo F. Padovan } 39570a708f8fSGustavo F. Padovan 39580a708f8fSGustavo F. Padovan bh_unlock_sock(sk); 39590a708f8fSGustavo F. Padovan } 39600a708f8fSGustavo F. Padovan 3961baa7e1faSGustavo F. Padovan read_unlock(&conn->chan_lock); 39620a708f8fSGustavo F. Padovan 39630a708f8fSGustavo F. Padovan return 0; 39640a708f8fSGustavo F. Padovan } 39650a708f8fSGustavo F. Padovan 39660a708f8fSGustavo F. Padovan static int l2cap_recv_acldata(struct hci_conn *hcon, struct sk_buff *skb, u16 flags) 39670a708f8fSGustavo F. Padovan { 39680a708f8fSGustavo F. Padovan struct l2cap_conn *conn = hcon->l2cap_data; 39690a708f8fSGustavo F. Padovan 39700a708f8fSGustavo F. Padovan if (!conn) 39710a708f8fSGustavo F. Padovan conn = l2cap_conn_add(hcon, 0); 39720a708f8fSGustavo F. Padovan 39730a708f8fSGustavo F. Padovan if (!conn) 39740a708f8fSGustavo F. Padovan goto drop; 39750a708f8fSGustavo F. Padovan 39760a708f8fSGustavo F. Padovan BT_DBG("conn %p len %d flags 0x%x", conn, skb->len, flags); 39770a708f8fSGustavo F. Padovan 39780a708f8fSGustavo F. Padovan if (!(flags & ACL_CONT)) { 39790a708f8fSGustavo F. Padovan struct l2cap_hdr *hdr; 398048454079SGustavo F. Padovan struct l2cap_chan *chan; 39810a708f8fSGustavo F. Padovan u16 cid; 39820a708f8fSGustavo F. Padovan int len; 39830a708f8fSGustavo F. Padovan 39840a708f8fSGustavo F. Padovan if (conn->rx_len) { 39850a708f8fSGustavo F. Padovan BT_ERR("Unexpected start frame (len %d)", skb->len); 39860a708f8fSGustavo F. Padovan kfree_skb(conn->rx_skb); 39870a708f8fSGustavo F. Padovan conn->rx_skb = NULL; 39880a708f8fSGustavo F. Padovan conn->rx_len = 0; 39890a708f8fSGustavo F. Padovan l2cap_conn_unreliable(conn, ECOMM); 39900a708f8fSGustavo F. Padovan } 39910a708f8fSGustavo F. Padovan 39920a708f8fSGustavo F. Padovan /* Start fragment always begin with Basic L2CAP header */ 39930a708f8fSGustavo F. Padovan if (skb->len < L2CAP_HDR_SIZE) { 39940a708f8fSGustavo F. Padovan BT_ERR("Frame is too short (len %d)", skb->len); 39950a708f8fSGustavo F. Padovan l2cap_conn_unreliable(conn, ECOMM); 39960a708f8fSGustavo F. Padovan goto drop; 39970a708f8fSGustavo F. Padovan } 39980a708f8fSGustavo F. Padovan 39990a708f8fSGustavo F. Padovan hdr = (struct l2cap_hdr *) skb->data; 40000a708f8fSGustavo F. Padovan len = __le16_to_cpu(hdr->len) + L2CAP_HDR_SIZE; 40010a708f8fSGustavo F. Padovan cid = __le16_to_cpu(hdr->cid); 40020a708f8fSGustavo F. Padovan 40030a708f8fSGustavo F. Padovan if (len == skb->len) { 40040a708f8fSGustavo F. Padovan /* Complete frame received */ 40050a708f8fSGustavo F. Padovan l2cap_recv_frame(conn, skb); 40060a708f8fSGustavo F. Padovan return 0; 40070a708f8fSGustavo F. Padovan } 40080a708f8fSGustavo F. Padovan 40090a708f8fSGustavo F. Padovan BT_DBG("Start: total len %d, frag len %d", len, skb->len); 40100a708f8fSGustavo F. Padovan 40110a708f8fSGustavo F. Padovan if (skb->len > len) { 40120a708f8fSGustavo F. Padovan BT_ERR("Frame is too long (len %d, expected len %d)", 40130a708f8fSGustavo F. Padovan skb->len, len); 40140a708f8fSGustavo F. Padovan l2cap_conn_unreliable(conn, ECOMM); 40150a708f8fSGustavo F. Padovan goto drop; 40160a708f8fSGustavo F. Padovan } 40170a708f8fSGustavo F. Padovan 4018baa7e1faSGustavo F. Padovan chan = l2cap_get_chan_by_scid(conn, cid); 40190a708f8fSGustavo F. Padovan 402048454079SGustavo F. Padovan if (chan && chan->sk) { 402148454079SGustavo F. Padovan struct sock *sk = chan->sk; 402248454079SGustavo F. Padovan 402348454079SGustavo F. Padovan if (l2cap_pi(sk)->imtu < len - L2CAP_HDR_SIZE) { 402448454079SGustavo F. Padovan BT_ERR("Frame exceeding recv MTU (len %d, " 402548454079SGustavo F. Padovan "MTU %d)", len, 402648454079SGustavo F. Padovan l2cap_pi(sk)->imtu); 40270a708f8fSGustavo F. Padovan bh_unlock_sock(sk); 40280a708f8fSGustavo F. Padovan l2cap_conn_unreliable(conn, ECOMM); 40290a708f8fSGustavo F. Padovan goto drop; 40300a708f8fSGustavo F. Padovan } 40310a708f8fSGustavo F. Padovan bh_unlock_sock(sk); 403248454079SGustavo F. Padovan } 40330a708f8fSGustavo F. Padovan 40340a708f8fSGustavo F. Padovan /* Allocate skb for the complete frame (with header) */ 40350a708f8fSGustavo F. Padovan conn->rx_skb = bt_skb_alloc(len, GFP_ATOMIC); 40360a708f8fSGustavo F. Padovan if (!conn->rx_skb) 40370a708f8fSGustavo F. Padovan goto drop; 40380a708f8fSGustavo F. Padovan 40390a708f8fSGustavo F. Padovan skb_copy_from_linear_data(skb, skb_put(conn->rx_skb, skb->len), 40400a708f8fSGustavo F. Padovan skb->len); 40410a708f8fSGustavo F. Padovan conn->rx_len = len - skb->len; 40420a708f8fSGustavo F. Padovan } else { 40430a708f8fSGustavo F. Padovan BT_DBG("Cont: frag len %d (expecting %d)", skb->len, conn->rx_len); 40440a708f8fSGustavo F. Padovan 40450a708f8fSGustavo F. Padovan if (!conn->rx_len) { 40460a708f8fSGustavo F. Padovan BT_ERR("Unexpected continuation frame (len %d)", skb->len); 40470a708f8fSGustavo F. Padovan l2cap_conn_unreliable(conn, ECOMM); 40480a708f8fSGustavo F. Padovan goto drop; 40490a708f8fSGustavo F. Padovan } 40500a708f8fSGustavo F. Padovan 40510a708f8fSGustavo F. Padovan if (skb->len > conn->rx_len) { 40520a708f8fSGustavo F. Padovan BT_ERR("Fragment is too long (len %d, expected %d)", 40530a708f8fSGustavo F. Padovan skb->len, conn->rx_len); 40540a708f8fSGustavo F. Padovan kfree_skb(conn->rx_skb); 40550a708f8fSGustavo F. Padovan conn->rx_skb = NULL; 40560a708f8fSGustavo F. Padovan conn->rx_len = 0; 40570a708f8fSGustavo F. Padovan l2cap_conn_unreliable(conn, ECOMM); 40580a708f8fSGustavo F. Padovan goto drop; 40590a708f8fSGustavo F. Padovan } 40600a708f8fSGustavo F. Padovan 40610a708f8fSGustavo F. Padovan skb_copy_from_linear_data(skb, skb_put(conn->rx_skb, skb->len), 40620a708f8fSGustavo F. Padovan skb->len); 40630a708f8fSGustavo F. Padovan conn->rx_len -= skb->len; 40640a708f8fSGustavo F. Padovan 40650a708f8fSGustavo F. Padovan if (!conn->rx_len) { 40660a708f8fSGustavo F. Padovan /* Complete frame received */ 40670a708f8fSGustavo F. Padovan l2cap_recv_frame(conn, conn->rx_skb); 40680a708f8fSGustavo F. Padovan conn->rx_skb = NULL; 40690a708f8fSGustavo F. Padovan } 40700a708f8fSGustavo F. Padovan } 40710a708f8fSGustavo F. Padovan 40720a708f8fSGustavo F. Padovan drop: 40730a708f8fSGustavo F. Padovan kfree_skb(skb); 40740a708f8fSGustavo F. Padovan return 0; 40750a708f8fSGustavo F. Padovan } 40760a708f8fSGustavo F. Padovan 40770a708f8fSGustavo F. Padovan static int l2cap_debugfs_show(struct seq_file *f, void *p) 40780a708f8fSGustavo F. Padovan { 40790a708f8fSGustavo F. Padovan struct sock *sk; 40800a708f8fSGustavo F. Padovan struct hlist_node *node; 40810a708f8fSGustavo F. Padovan 40820a708f8fSGustavo F. Padovan read_lock_bh(&l2cap_sk_list.lock); 40830a708f8fSGustavo F. Padovan 40840a708f8fSGustavo F. Padovan sk_for_each(sk, node, &l2cap_sk_list.head) { 40850a708f8fSGustavo F. Padovan struct l2cap_pinfo *pi = l2cap_pi(sk); 40860a708f8fSGustavo F. Padovan 4087903d343eSGustavo F. Padovan seq_printf(f, "%s %s %d %d 0x%4.4x 0x%4.4x %d %d %d %d\n", 40880a708f8fSGustavo F. Padovan batostr(&bt_sk(sk)->src), 40890a708f8fSGustavo F. Padovan batostr(&bt_sk(sk)->dst), 40900a708f8fSGustavo F. Padovan sk->sk_state, __le16_to_cpu(pi->psm), 40910a708f8fSGustavo F. Padovan pi->scid, pi->dcid, 4092*4343478fSGustavo F. Padovan pi->imtu, pi->omtu, pi->chan->sec_level, 4093903d343eSGustavo F. Padovan pi->mode); 40940a708f8fSGustavo F. Padovan } 40950a708f8fSGustavo F. Padovan 40960a708f8fSGustavo F. Padovan read_unlock_bh(&l2cap_sk_list.lock); 40970a708f8fSGustavo F. Padovan 40980a708f8fSGustavo F. Padovan return 0; 40990a708f8fSGustavo F. Padovan } 41000a708f8fSGustavo F. Padovan 41010a708f8fSGustavo F. Padovan static int l2cap_debugfs_open(struct inode *inode, struct file *file) 41020a708f8fSGustavo F. Padovan { 41030a708f8fSGustavo F. Padovan return single_open(file, l2cap_debugfs_show, inode->i_private); 41040a708f8fSGustavo F. Padovan } 41050a708f8fSGustavo F. Padovan 41060a708f8fSGustavo F. Padovan static const struct file_operations l2cap_debugfs_fops = { 41070a708f8fSGustavo F. Padovan .open = l2cap_debugfs_open, 41080a708f8fSGustavo F. Padovan .read = seq_read, 41090a708f8fSGustavo F. Padovan .llseek = seq_lseek, 41100a708f8fSGustavo F. Padovan .release = single_release, 41110a708f8fSGustavo F. Padovan }; 41120a708f8fSGustavo F. Padovan 41130a708f8fSGustavo F. Padovan static struct dentry *l2cap_debugfs; 41140a708f8fSGustavo F. Padovan 41150a708f8fSGustavo F. Padovan static struct hci_proto l2cap_hci_proto = { 41160a708f8fSGustavo F. Padovan .name = "L2CAP", 41170a708f8fSGustavo F. Padovan .id = HCI_PROTO_L2CAP, 41180a708f8fSGustavo F. Padovan .connect_ind = l2cap_connect_ind, 41190a708f8fSGustavo F. Padovan .connect_cfm = l2cap_connect_cfm, 41200a708f8fSGustavo F. Padovan .disconn_ind = l2cap_disconn_ind, 41210a708f8fSGustavo F. Padovan .disconn_cfm = l2cap_disconn_cfm, 41220a708f8fSGustavo F. Padovan .security_cfm = l2cap_security_cfm, 41230a708f8fSGustavo F. Padovan .recv_acldata = l2cap_recv_acldata 41240a708f8fSGustavo F. Padovan }; 41250a708f8fSGustavo F. Padovan 412664274518SGustavo F. Padovan int __init l2cap_init(void) 41270a708f8fSGustavo F. Padovan { 41280a708f8fSGustavo F. Padovan int err; 41290a708f8fSGustavo F. Padovan 4130bb58f747SGustavo F. Padovan err = l2cap_init_sockets(); 41310a708f8fSGustavo F. Padovan if (err < 0) 41320a708f8fSGustavo F. Padovan return err; 41330a708f8fSGustavo F. Padovan 41340a708f8fSGustavo F. Padovan _busy_wq = create_singlethread_workqueue("l2cap"); 41350a708f8fSGustavo F. Padovan if (!_busy_wq) { 4136bb58f747SGustavo F. Padovan err = -ENOMEM; 41370a708f8fSGustavo F. Padovan goto error; 41380a708f8fSGustavo F. Padovan } 41390a708f8fSGustavo F. Padovan 41400a708f8fSGustavo F. Padovan err = hci_register_proto(&l2cap_hci_proto); 41410a708f8fSGustavo F. Padovan if (err < 0) { 41420a708f8fSGustavo F. Padovan BT_ERR("L2CAP protocol registration failed"); 41430a708f8fSGustavo F. Padovan bt_sock_unregister(BTPROTO_L2CAP); 41440a708f8fSGustavo F. Padovan goto error; 41450a708f8fSGustavo F. Padovan } 41460a708f8fSGustavo F. Padovan 41470a708f8fSGustavo F. Padovan if (bt_debugfs) { 41480a708f8fSGustavo F. Padovan l2cap_debugfs = debugfs_create_file("l2cap", 0444, 41490a708f8fSGustavo F. Padovan bt_debugfs, NULL, &l2cap_debugfs_fops); 41500a708f8fSGustavo F. Padovan if (!l2cap_debugfs) 41510a708f8fSGustavo F. Padovan BT_ERR("Failed to create L2CAP debug file"); 41520a708f8fSGustavo F. Padovan } 41530a708f8fSGustavo F. Padovan 41540a708f8fSGustavo F. Padovan return 0; 41550a708f8fSGustavo F. Padovan 41560a708f8fSGustavo F. Padovan error: 41570a708f8fSGustavo F. Padovan destroy_workqueue(_busy_wq); 4158bb58f747SGustavo F. Padovan l2cap_cleanup_sockets(); 41590a708f8fSGustavo F. Padovan return err; 41600a708f8fSGustavo F. Padovan } 41610a708f8fSGustavo F. Padovan 416264274518SGustavo F. Padovan void l2cap_exit(void) 41630a708f8fSGustavo F. Padovan { 41640a708f8fSGustavo F. Padovan debugfs_remove(l2cap_debugfs); 41650a708f8fSGustavo F. Padovan 41660a708f8fSGustavo F. Padovan flush_workqueue(_busy_wq); 41670a708f8fSGustavo F. Padovan destroy_workqueue(_busy_wq); 41680a708f8fSGustavo F. Padovan 41690a708f8fSGustavo F. Padovan if (hci_unregister_proto(&l2cap_hci_proto) < 0) 41700a708f8fSGustavo F. Padovan BT_ERR("L2CAP protocol unregistration failed"); 41710a708f8fSGustavo F. Padovan 4172bb58f747SGustavo F. Padovan l2cap_cleanup_sockets(); 41730a708f8fSGustavo F. Padovan } 41740a708f8fSGustavo F. Padovan 41750a708f8fSGustavo F. Padovan module_param(disable_ertm, bool, 0644); 41760a708f8fSGustavo F. Padovan MODULE_PARM_DESC(disable_ertm, "Disable enhanced retransmission mode"); 4177