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); 730a708f8fSGustavo F. Padovan 740a708f8fSGustavo F. Padovan static int l2cap_ertm_data_rcv(struct sock *sk, struct sk_buff *skb); 750a708f8fSGustavo F. Padovan 760a708f8fSGustavo F. Padovan /* ---- L2CAP channels ---- */ 770a708f8fSGustavo F. Padovan static struct sock *__l2cap_get_chan_by_dcid(struct l2cap_chan_list *l, u16 cid) 780a708f8fSGustavo F. Padovan { 790a708f8fSGustavo F. Padovan struct sock *s; 800a708f8fSGustavo F. Padovan for (s = l->head; s; s = l2cap_pi(s)->next_c) { 810a708f8fSGustavo F. Padovan if (l2cap_pi(s)->dcid == cid) 820a708f8fSGustavo F. Padovan break; 830a708f8fSGustavo F. Padovan } 840a708f8fSGustavo F. Padovan return s; 850a708f8fSGustavo F. Padovan } 860a708f8fSGustavo F. Padovan 870a708f8fSGustavo F. Padovan static struct sock *__l2cap_get_chan_by_scid(struct l2cap_chan_list *l, u16 cid) 880a708f8fSGustavo F. Padovan { 890a708f8fSGustavo F. Padovan struct sock *s; 900a708f8fSGustavo F. Padovan for (s = l->head; s; s = l2cap_pi(s)->next_c) { 910a708f8fSGustavo F. Padovan if (l2cap_pi(s)->scid == cid) 920a708f8fSGustavo F. Padovan break; 930a708f8fSGustavo F. Padovan } 940a708f8fSGustavo F. Padovan return s; 950a708f8fSGustavo F. Padovan } 960a708f8fSGustavo F. Padovan 970a708f8fSGustavo F. Padovan /* Find channel with given SCID. 980a708f8fSGustavo F. Padovan * Returns locked socket */ 990a708f8fSGustavo F. Padovan static inline struct sock *l2cap_get_chan_by_scid(struct l2cap_chan_list *l, u16 cid) 1000a708f8fSGustavo F. Padovan { 1010a708f8fSGustavo F. Padovan struct sock *s; 1020a708f8fSGustavo F. Padovan read_lock(&l->lock); 1030a708f8fSGustavo F. Padovan s = __l2cap_get_chan_by_scid(l, cid); 1040a708f8fSGustavo F. Padovan if (s) 1050a708f8fSGustavo F. Padovan bh_lock_sock(s); 1060a708f8fSGustavo F. Padovan read_unlock(&l->lock); 1070a708f8fSGustavo F. Padovan return s; 1080a708f8fSGustavo F. Padovan } 1090a708f8fSGustavo F. Padovan 1100a708f8fSGustavo F. Padovan static struct sock *__l2cap_get_chan_by_ident(struct l2cap_chan_list *l, u8 ident) 1110a708f8fSGustavo F. Padovan { 1120a708f8fSGustavo F. Padovan struct sock *s; 1130a708f8fSGustavo F. Padovan for (s = l->head; s; s = l2cap_pi(s)->next_c) { 1140a708f8fSGustavo F. Padovan if (l2cap_pi(s)->ident == ident) 1150a708f8fSGustavo F. Padovan break; 1160a708f8fSGustavo F. Padovan } 1170a708f8fSGustavo F. Padovan return s; 1180a708f8fSGustavo F. Padovan } 1190a708f8fSGustavo F. Padovan 1200a708f8fSGustavo F. Padovan static inline struct sock *l2cap_get_chan_by_ident(struct l2cap_chan_list *l, u8 ident) 1210a708f8fSGustavo F. Padovan { 1220a708f8fSGustavo F. Padovan struct sock *s; 1230a708f8fSGustavo F. Padovan read_lock(&l->lock); 1240a708f8fSGustavo F. Padovan s = __l2cap_get_chan_by_ident(l, ident); 1250a708f8fSGustavo F. Padovan if (s) 1260a708f8fSGustavo F. Padovan bh_lock_sock(s); 1270a708f8fSGustavo F. Padovan read_unlock(&l->lock); 1280a708f8fSGustavo F. Padovan return s; 1290a708f8fSGustavo F. Padovan } 1300a708f8fSGustavo F. Padovan 1310a708f8fSGustavo F. Padovan static u16 l2cap_alloc_cid(struct l2cap_chan_list *l) 1320a708f8fSGustavo F. Padovan { 1330a708f8fSGustavo F. Padovan u16 cid = L2CAP_CID_DYN_START; 1340a708f8fSGustavo F. Padovan 1350a708f8fSGustavo F. Padovan for (; cid < L2CAP_CID_DYN_END; cid++) { 1360a708f8fSGustavo F. Padovan if (!__l2cap_get_chan_by_scid(l, cid)) 1370a708f8fSGustavo F. Padovan return cid; 1380a708f8fSGustavo F. Padovan } 1390a708f8fSGustavo F. Padovan 1400a708f8fSGustavo F. Padovan return 0; 1410a708f8fSGustavo F. Padovan } 1420a708f8fSGustavo F. Padovan 1430a708f8fSGustavo F. Padovan static inline void __l2cap_chan_link(struct l2cap_chan_list *l, struct sock *sk) 1440a708f8fSGustavo F. Padovan { 1450a708f8fSGustavo F. Padovan sock_hold(sk); 1460a708f8fSGustavo F. Padovan 1470a708f8fSGustavo F. Padovan if (l->head) 1480a708f8fSGustavo F. Padovan l2cap_pi(l->head)->prev_c = sk; 1490a708f8fSGustavo F. Padovan 1500a708f8fSGustavo F. Padovan l2cap_pi(sk)->next_c = l->head; 1510a708f8fSGustavo F. Padovan l2cap_pi(sk)->prev_c = NULL; 1520a708f8fSGustavo F. Padovan l->head = sk; 1530a708f8fSGustavo F. Padovan } 1540a708f8fSGustavo F. Padovan 1550a708f8fSGustavo F. Padovan static inline void l2cap_chan_unlink(struct l2cap_chan_list *l, struct sock *sk) 1560a708f8fSGustavo F. Padovan { 1570a708f8fSGustavo F. Padovan struct sock *next = l2cap_pi(sk)->next_c, *prev = l2cap_pi(sk)->prev_c; 1580a708f8fSGustavo F. Padovan 1590a708f8fSGustavo F. Padovan write_lock_bh(&l->lock); 1600a708f8fSGustavo F. Padovan if (sk == l->head) 1610a708f8fSGustavo F. Padovan l->head = next; 1620a708f8fSGustavo F. Padovan 1630a708f8fSGustavo F. Padovan if (next) 1640a708f8fSGustavo F. Padovan l2cap_pi(next)->prev_c = prev; 1650a708f8fSGustavo F. Padovan if (prev) 1660a708f8fSGustavo F. Padovan l2cap_pi(prev)->next_c = next; 1670a708f8fSGustavo F. Padovan write_unlock_bh(&l->lock); 1680a708f8fSGustavo F. Padovan 1690a708f8fSGustavo F. Padovan __sock_put(sk); 1700a708f8fSGustavo F. Padovan } 1710a708f8fSGustavo F. Padovan 1720a708f8fSGustavo F. Padovan static void __l2cap_chan_add(struct l2cap_conn *conn, struct sock *sk, struct sock *parent) 1730a708f8fSGustavo F. Padovan { 1740a708f8fSGustavo F. Padovan struct l2cap_chan_list *l = &conn->chan_list; 1750a708f8fSGustavo F. Padovan 1760a708f8fSGustavo F. Padovan BT_DBG("conn %p, psm 0x%2.2x, dcid 0x%4.4x", conn, 1770a708f8fSGustavo F. Padovan l2cap_pi(sk)->psm, l2cap_pi(sk)->dcid); 1780a708f8fSGustavo F. Padovan 1790a708f8fSGustavo F. Padovan conn->disc_reason = 0x13; 1800a708f8fSGustavo F. Padovan 1810a708f8fSGustavo F. Padovan l2cap_pi(sk)->conn = conn; 1820a708f8fSGustavo F. Padovan 1830a708f8fSGustavo F. Padovan if (sk->sk_type == SOCK_SEQPACKET || sk->sk_type == SOCK_STREAM) { 184b62f328bSVille Tervo if (conn->hcon->type == LE_LINK) { 185b62f328bSVille Tervo /* LE connection */ 186b62f328bSVille Tervo l2cap_pi(sk)->omtu = L2CAP_LE_DEFAULT_MTU; 187b62f328bSVille Tervo l2cap_pi(sk)->scid = L2CAP_CID_LE_DATA; 188b62f328bSVille Tervo l2cap_pi(sk)->dcid = L2CAP_CID_LE_DATA; 189b62f328bSVille Tervo } else { 1900a708f8fSGustavo F. Padovan /* Alloc CID for connection-oriented socket */ 1910a708f8fSGustavo F. Padovan l2cap_pi(sk)->scid = l2cap_alloc_cid(l); 192b62f328bSVille Tervo l2cap_pi(sk)->omtu = L2CAP_DEFAULT_MTU; 193b62f328bSVille Tervo } 1940a708f8fSGustavo F. Padovan } else if (sk->sk_type == SOCK_DGRAM) { 1950a708f8fSGustavo F. Padovan /* Connectionless socket */ 1960a708f8fSGustavo F. Padovan l2cap_pi(sk)->scid = L2CAP_CID_CONN_LESS; 1970a708f8fSGustavo F. Padovan l2cap_pi(sk)->dcid = L2CAP_CID_CONN_LESS; 1980a708f8fSGustavo F. Padovan l2cap_pi(sk)->omtu = L2CAP_DEFAULT_MTU; 1990a708f8fSGustavo F. Padovan } else { 2000a708f8fSGustavo F. Padovan /* Raw socket can send/recv signalling messages only */ 2010a708f8fSGustavo F. Padovan l2cap_pi(sk)->scid = L2CAP_CID_SIGNALING; 2020a708f8fSGustavo F. Padovan l2cap_pi(sk)->dcid = L2CAP_CID_SIGNALING; 2030a708f8fSGustavo F. Padovan l2cap_pi(sk)->omtu = L2CAP_DEFAULT_MTU; 2040a708f8fSGustavo F. Padovan } 2050a708f8fSGustavo F. Padovan 2060a708f8fSGustavo F. Padovan __l2cap_chan_link(l, sk); 2070a708f8fSGustavo F. Padovan 2080a708f8fSGustavo F. Padovan if (parent) 2090a708f8fSGustavo F. Padovan bt_accept_enqueue(parent, sk); 2100a708f8fSGustavo F. Padovan } 2110a708f8fSGustavo F. Padovan 2120a708f8fSGustavo F. Padovan /* Delete channel. 2130a708f8fSGustavo F. Padovan * Must be called on the locked socket. */ 2146de0702bSGustavo F. Padovan void l2cap_chan_del(struct sock *sk, int err) 2150a708f8fSGustavo F. Padovan { 2160a708f8fSGustavo F. Padovan struct l2cap_conn *conn = l2cap_pi(sk)->conn; 2170a708f8fSGustavo F. Padovan struct sock *parent = bt_sk(sk)->parent; 2180a708f8fSGustavo F. Padovan 2190a708f8fSGustavo F. Padovan l2cap_sock_clear_timer(sk); 2200a708f8fSGustavo F. Padovan 2210a708f8fSGustavo F. Padovan BT_DBG("sk %p, conn %p, err %d", sk, conn, err); 2220a708f8fSGustavo F. Padovan 2230a708f8fSGustavo F. Padovan if (conn) { 2240a708f8fSGustavo F. Padovan /* Unlink from channel list */ 2250a708f8fSGustavo F. Padovan l2cap_chan_unlink(&conn->chan_list, sk); 2260a708f8fSGustavo F. Padovan l2cap_pi(sk)->conn = NULL; 2270a708f8fSGustavo F. Padovan hci_conn_put(conn->hcon); 2280a708f8fSGustavo F. Padovan } 2290a708f8fSGustavo F. Padovan 2300a708f8fSGustavo F. Padovan sk->sk_state = BT_CLOSED; 2310a708f8fSGustavo F. Padovan sock_set_flag(sk, SOCK_ZAPPED); 2320a708f8fSGustavo F. Padovan 2330a708f8fSGustavo F. Padovan if (err) 2340a708f8fSGustavo F. Padovan sk->sk_err = err; 2350a708f8fSGustavo F. Padovan 2360a708f8fSGustavo F. Padovan if (parent) { 2370a708f8fSGustavo F. Padovan bt_accept_unlink(sk); 2380a708f8fSGustavo F. Padovan parent->sk_data_ready(parent, 0); 2390a708f8fSGustavo F. Padovan } else 2400a708f8fSGustavo F. Padovan sk->sk_state_change(sk); 2410a708f8fSGustavo F. Padovan 2420a708f8fSGustavo F. Padovan skb_queue_purge(TX_QUEUE(sk)); 2430a708f8fSGustavo F. Padovan 2440a708f8fSGustavo F. Padovan if (l2cap_pi(sk)->mode == L2CAP_MODE_ERTM) { 2450a708f8fSGustavo F. Padovan struct srej_list *l, *tmp; 2460a708f8fSGustavo F. Padovan 2470a708f8fSGustavo F. Padovan del_timer(&l2cap_pi(sk)->retrans_timer); 2480a708f8fSGustavo F. Padovan del_timer(&l2cap_pi(sk)->monitor_timer); 2490a708f8fSGustavo F. Padovan del_timer(&l2cap_pi(sk)->ack_timer); 2500a708f8fSGustavo F. Padovan 2510a708f8fSGustavo F. Padovan skb_queue_purge(SREJ_QUEUE(sk)); 2520a708f8fSGustavo F. Padovan skb_queue_purge(BUSY_QUEUE(sk)); 2530a708f8fSGustavo F. Padovan 2540a708f8fSGustavo F. Padovan list_for_each_entry_safe(l, tmp, SREJ_LIST(sk), list) { 2550a708f8fSGustavo F. Padovan list_del(&l->list); 2560a708f8fSGustavo F. Padovan kfree(l); 2570a708f8fSGustavo F. Padovan } 2580a708f8fSGustavo F. Padovan } 2590a708f8fSGustavo F. Padovan } 2600a708f8fSGustavo F. Padovan 2610a708f8fSGustavo F. Padovan static inline u8 l2cap_get_auth_type(struct sock *sk) 2620a708f8fSGustavo F. Padovan { 2630a708f8fSGustavo F. Padovan if (sk->sk_type == SOCK_RAW) { 2640a708f8fSGustavo F. Padovan switch (l2cap_pi(sk)->sec_level) { 2650a708f8fSGustavo F. Padovan case BT_SECURITY_HIGH: 2660a708f8fSGustavo F. Padovan return HCI_AT_DEDICATED_BONDING_MITM; 2670a708f8fSGustavo F. Padovan case BT_SECURITY_MEDIUM: 2680a708f8fSGustavo F. Padovan return HCI_AT_DEDICATED_BONDING; 2690a708f8fSGustavo F. Padovan default: 2700a708f8fSGustavo F. Padovan return HCI_AT_NO_BONDING; 2710a708f8fSGustavo F. Padovan } 2720a708f8fSGustavo F. Padovan } else if (l2cap_pi(sk)->psm == cpu_to_le16(0x0001)) { 2730a708f8fSGustavo F. Padovan if (l2cap_pi(sk)->sec_level == BT_SECURITY_LOW) 2740a708f8fSGustavo F. Padovan l2cap_pi(sk)->sec_level = BT_SECURITY_SDP; 2750a708f8fSGustavo F. Padovan 2760a708f8fSGustavo F. Padovan if (l2cap_pi(sk)->sec_level == BT_SECURITY_HIGH) 2770a708f8fSGustavo F. Padovan return HCI_AT_NO_BONDING_MITM; 2780a708f8fSGustavo F. Padovan else 2790a708f8fSGustavo F. Padovan return HCI_AT_NO_BONDING; 2800a708f8fSGustavo F. Padovan } else { 2810a708f8fSGustavo F. Padovan switch (l2cap_pi(sk)->sec_level) { 2820a708f8fSGustavo F. Padovan case BT_SECURITY_HIGH: 2830a708f8fSGustavo F. Padovan return HCI_AT_GENERAL_BONDING_MITM; 2840a708f8fSGustavo F. Padovan case BT_SECURITY_MEDIUM: 2850a708f8fSGustavo F. Padovan return HCI_AT_GENERAL_BONDING; 2860a708f8fSGustavo F. Padovan default: 2870a708f8fSGustavo F. Padovan return HCI_AT_NO_BONDING; 2880a708f8fSGustavo F. Padovan } 2890a708f8fSGustavo F. Padovan } 2900a708f8fSGustavo F. Padovan } 2910a708f8fSGustavo F. Padovan 2920a708f8fSGustavo F. Padovan /* Service level security */ 2930a708f8fSGustavo F. Padovan static inline int l2cap_check_security(struct sock *sk) 2940a708f8fSGustavo F. Padovan { 2950a708f8fSGustavo F. Padovan struct l2cap_conn *conn = l2cap_pi(sk)->conn; 2960a708f8fSGustavo F. Padovan __u8 auth_type; 2970a708f8fSGustavo F. Padovan 2980a708f8fSGustavo F. Padovan auth_type = l2cap_get_auth_type(sk); 2990a708f8fSGustavo F. Padovan 3000a708f8fSGustavo F. Padovan return hci_conn_security(conn->hcon, l2cap_pi(sk)->sec_level, 3010a708f8fSGustavo F. Padovan auth_type); 3020a708f8fSGustavo F. Padovan } 3030a708f8fSGustavo F. Padovan 30468983259SGustavo F. Padovan u8 l2cap_get_ident(struct l2cap_conn *conn) 3050a708f8fSGustavo F. Padovan { 3060a708f8fSGustavo F. Padovan u8 id; 3070a708f8fSGustavo F. Padovan 3080a708f8fSGustavo F. Padovan /* Get next available identificator. 3090a708f8fSGustavo F. Padovan * 1 - 128 are used by kernel. 3100a708f8fSGustavo F. Padovan * 129 - 199 are reserved. 3110a708f8fSGustavo F. Padovan * 200 - 254 are used by utilities like l2ping, etc. 3120a708f8fSGustavo F. Padovan */ 3130a708f8fSGustavo F. Padovan 3140a708f8fSGustavo F. Padovan spin_lock_bh(&conn->lock); 3150a708f8fSGustavo F. Padovan 3160a708f8fSGustavo F. Padovan if (++conn->tx_ident > 128) 3170a708f8fSGustavo F. Padovan conn->tx_ident = 1; 3180a708f8fSGustavo F. Padovan 3190a708f8fSGustavo F. Padovan id = conn->tx_ident; 3200a708f8fSGustavo F. Padovan 3210a708f8fSGustavo F. Padovan spin_unlock_bh(&conn->lock); 3220a708f8fSGustavo F. Padovan 3230a708f8fSGustavo F. Padovan return id; 3240a708f8fSGustavo F. Padovan } 3250a708f8fSGustavo F. Padovan 32668983259SGustavo F. Padovan void l2cap_send_cmd(struct l2cap_conn *conn, u8 ident, u8 code, u16 len, void *data) 3270a708f8fSGustavo F. Padovan { 3280a708f8fSGustavo F. Padovan struct sk_buff *skb = l2cap_build_cmd(conn, code, ident, len, data); 3290a708f8fSGustavo F. Padovan u8 flags; 3300a708f8fSGustavo F. Padovan 3310a708f8fSGustavo F. Padovan BT_DBG("code 0x%2.2x", code); 3320a708f8fSGustavo F. Padovan 3330a708f8fSGustavo F. Padovan if (!skb) 3340a708f8fSGustavo F. Padovan return; 3350a708f8fSGustavo F. Padovan 3360a708f8fSGustavo F. Padovan if (lmp_no_flush_capable(conn->hcon->hdev)) 3370a708f8fSGustavo F. Padovan flags = ACL_START_NO_FLUSH; 3380a708f8fSGustavo F. Padovan else 3390a708f8fSGustavo F. Padovan flags = ACL_START; 3400a708f8fSGustavo F. Padovan 3410a708f8fSGustavo F. Padovan hci_send_acl(conn->hcon, skb, flags); 3420a708f8fSGustavo F. Padovan } 3430a708f8fSGustavo F. Padovan 3440a708f8fSGustavo F. Padovan static inline void l2cap_send_sframe(struct l2cap_pinfo *pi, u16 control) 3450a708f8fSGustavo F. Padovan { 3460a708f8fSGustavo F. Padovan struct sk_buff *skb; 3470a708f8fSGustavo F. Padovan struct l2cap_hdr *lh; 3480a708f8fSGustavo F. Padovan struct l2cap_conn *conn = pi->conn; 3490a708f8fSGustavo F. Padovan struct sock *sk = (struct sock *)pi; 3500a708f8fSGustavo F. Padovan int count, hlen = L2CAP_HDR_SIZE + 2; 3510a708f8fSGustavo F. Padovan u8 flags; 3520a708f8fSGustavo F. Padovan 3530a708f8fSGustavo F. Padovan if (sk->sk_state != BT_CONNECTED) 3540a708f8fSGustavo F. Padovan return; 3550a708f8fSGustavo F. Padovan 3560a708f8fSGustavo F. Padovan if (pi->fcs == L2CAP_FCS_CRC16) 3570a708f8fSGustavo F. Padovan hlen += 2; 3580a708f8fSGustavo F. Padovan 3590a708f8fSGustavo F. Padovan BT_DBG("pi %p, control 0x%2.2x", pi, control); 3600a708f8fSGustavo F. Padovan 3610a708f8fSGustavo F. Padovan count = min_t(unsigned int, conn->mtu, hlen); 3620a708f8fSGustavo F. Padovan control |= L2CAP_CTRL_FRAME_TYPE; 3630a708f8fSGustavo F. Padovan 3640a708f8fSGustavo F. Padovan if (pi->conn_state & L2CAP_CONN_SEND_FBIT) { 3650a708f8fSGustavo F. Padovan control |= L2CAP_CTRL_FINAL; 3660a708f8fSGustavo F. Padovan pi->conn_state &= ~L2CAP_CONN_SEND_FBIT; 3670a708f8fSGustavo F. Padovan } 3680a708f8fSGustavo F. Padovan 3690a708f8fSGustavo F. Padovan if (pi->conn_state & L2CAP_CONN_SEND_PBIT) { 3700a708f8fSGustavo F. Padovan control |= L2CAP_CTRL_POLL; 3710a708f8fSGustavo F. Padovan pi->conn_state &= ~L2CAP_CONN_SEND_PBIT; 3720a708f8fSGustavo F. Padovan } 3730a708f8fSGustavo F. Padovan 3740a708f8fSGustavo F. Padovan skb = bt_skb_alloc(count, GFP_ATOMIC); 3750a708f8fSGustavo F. Padovan if (!skb) 3760a708f8fSGustavo F. Padovan return; 3770a708f8fSGustavo F. Padovan 3780a708f8fSGustavo F. Padovan lh = (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE); 3790a708f8fSGustavo F. Padovan lh->len = cpu_to_le16(hlen - L2CAP_HDR_SIZE); 3800a708f8fSGustavo F. Padovan lh->cid = cpu_to_le16(pi->dcid); 3810a708f8fSGustavo F. Padovan put_unaligned_le16(control, skb_put(skb, 2)); 3820a708f8fSGustavo F. Padovan 3830a708f8fSGustavo F. Padovan if (pi->fcs == L2CAP_FCS_CRC16) { 3840a708f8fSGustavo F. Padovan u16 fcs = crc16(0, (u8 *)lh, count - 2); 3850a708f8fSGustavo F. Padovan put_unaligned_le16(fcs, skb_put(skb, 2)); 3860a708f8fSGustavo F. Padovan } 3870a708f8fSGustavo F. Padovan 3880a708f8fSGustavo F. Padovan if (lmp_no_flush_capable(conn->hcon->hdev)) 3890a708f8fSGustavo F. Padovan flags = ACL_START_NO_FLUSH; 3900a708f8fSGustavo F. Padovan else 3910a708f8fSGustavo F. Padovan flags = ACL_START; 3920a708f8fSGustavo F. Padovan 3930a708f8fSGustavo F. Padovan hci_send_acl(pi->conn->hcon, skb, flags); 3940a708f8fSGustavo F. Padovan } 3950a708f8fSGustavo F. Padovan 3960a708f8fSGustavo F. Padovan static inline void l2cap_send_rr_or_rnr(struct l2cap_pinfo *pi, u16 control) 3970a708f8fSGustavo F. Padovan { 3980a708f8fSGustavo F. Padovan if (pi->conn_state & L2CAP_CONN_LOCAL_BUSY) { 3990a708f8fSGustavo F. Padovan control |= L2CAP_SUPER_RCV_NOT_READY; 4000a708f8fSGustavo F. Padovan pi->conn_state |= L2CAP_CONN_RNR_SENT; 4010a708f8fSGustavo F. Padovan } else 4020a708f8fSGustavo F. Padovan control |= L2CAP_SUPER_RCV_READY; 4030a708f8fSGustavo F. Padovan 4040a708f8fSGustavo F. Padovan control |= pi->buffer_seq << L2CAP_CTRL_REQSEQ_SHIFT; 4050a708f8fSGustavo F. Padovan 4060a708f8fSGustavo F. Padovan l2cap_send_sframe(pi, control); 4070a708f8fSGustavo F. Padovan } 4080a708f8fSGustavo F. Padovan 4090a708f8fSGustavo F. Padovan static inline int __l2cap_no_conn_pending(struct sock *sk) 4100a708f8fSGustavo F. Padovan { 4110a708f8fSGustavo F. Padovan return !(l2cap_pi(sk)->conf_state & L2CAP_CONF_CONNECT_PEND); 4120a708f8fSGustavo F. Padovan } 4130a708f8fSGustavo F. Padovan 4140a708f8fSGustavo F. Padovan static void l2cap_do_start(struct sock *sk) 4150a708f8fSGustavo F. Padovan { 4160a708f8fSGustavo F. Padovan struct l2cap_conn *conn = l2cap_pi(sk)->conn; 4170a708f8fSGustavo F. Padovan 4180a708f8fSGustavo F. Padovan if (conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_SENT) { 4190a708f8fSGustavo F. Padovan if (!(conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_DONE)) 4200a708f8fSGustavo F. Padovan return; 4210a708f8fSGustavo F. Padovan 4220a708f8fSGustavo F. Padovan if (l2cap_check_security(sk) && __l2cap_no_conn_pending(sk)) { 4230a708f8fSGustavo F. Padovan struct l2cap_conn_req req; 4240a708f8fSGustavo F. Padovan req.scid = cpu_to_le16(l2cap_pi(sk)->scid); 4250a708f8fSGustavo F. Padovan req.psm = l2cap_pi(sk)->psm; 4260a708f8fSGustavo F. Padovan 4270a708f8fSGustavo F. Padovan l2cap_pi(sk)->ident = l2cap_get_ident(conn); 4280a708f8fSGustavo F. Padovan l2cap_pi(sk)->conf_state |= L2CAP_CONF_CONNECT_PEND; 4290a708f8fSGustavo F. Padovan 4300a708f8fSGustavo F. Padovan l2cap_send_cmd(conn, l2cap_pi(sk)->ident, 4310a708f8fSGustavo F. Padovan L2CAP_CONN_REQ, sizeof(req), &req); 4320a708f8fSGustavo F. Padovan } 4330a708f8fSGustavo F. Padovan } else { 4340a708f8fSGustavo F. Padovan struct l2cap_info_req req; 4350a708f8fSGustavo F. Padovan req.type = cpu_to_le16(L2CAP_IT_FEAT_MASK); 4360a708f8fSGustavo F. Padovan 4370a708f8fSGustavo F. Padovan conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_SENT; 4380a708f8fSGustavo F. Padovan conn->info_ident = l2cap_get_ident(conn); 4390a708f8fSGustavo F. Padovan 4400a708f8fSGustavo F. Padovan mod_timer(&conn->info_timer, jiffies + 4410a708f8fSGustavo F. Padovan msecs_to_jiffies(L2CAP_INFO_TIMEOUT)); 4420a708f8fSGustavo F. Padovan 4430a708f8fSGustavo F. Padovan l2cap_send_cmd(conn, conn->info_ident, 4440a708f8fSGustavo F. Padovan L2CAP_INFO_REQ, sizeof(req), &req); 4450a708f8fSGustavo F. Padovan } 4460a708f8fSGustavo F. Padovan } 4470a708f8fSGustavo F. Padovan 4480a708f8fSGustavo F. Padovan static inline int l2cap_mode_supported(__u8 mode, __u32 feat_mask) 4490a708f8fSGustavo F. Padovan { 4500a708f8fSGustavo F. Padovan u32 local_feat_mask = l2cap_feat_mask; 4510a708f8fSGustavo F. Padovan if (!disable_ertm) 4520a708f8fSGustavo F. Padovan local_feat_mask |= L2CAP_FEAT_ERTM | L2CAP_FEAT_STREAMING; 4530a708f8fSGustavo F. Padovan 4540a708f8fSGustavo F. Padovan switch (mode) { 4550a708f8fSGustavo F. Padovan case L2CAP_MODE_ERTM: 4560a708f8fSGustavo F. Padovan return L2CAP_FEAT_ERTM & feat_mask & local_feat_mask; 4570a708f8fSGustavo F. Padovan case L2CAP_MODE_STREAMING: 4580a708f8fSGustavo F. Padovan return L2CAP_FEAT_STREAMING & feat_mask & local_feat_mask; 4590a708f8fSGustavo F. Padovan default: 4600a708f8fSGustavo F. Padovan return 0x00; 4610a708f8fSGustavo F. Padovan } 4620a708f8fSGustavo F. Padovan } 4630a708f8fSGustavo F. Padovan 4646de0702bSGustavo F. Padovan void l2cap_send_disconn_req(struct l2cap_conn *conn, struct sock *sk, int err) 4650a708f8fSGustavo F. Padovan { 4660a708f8fSGustavo F. Padovan struct l2cap_disconn_req req; 4670a708f8fSGustavo F. Padovan 4680a708f8fSGustavo F. Padovan if (!conn) 4690a708f8fSGustavo F. Padovan return; 4700a708f8fSGustavo F. Padovan 4710a708f8fSGustavo F. Padovan skb_queue_purge(TX_QUEUE(sk)); 4720a708f8fSGustavo F. Padovan 4730a708f8fSGustavo F. Padovan if (l2cap_pi(sk)->mode == L2CAP_MODE_ERTM) { 4740a708f8fSGustavo F. Padovan del_timer(&l2cap_pi(sk)->retrans_timer); 4750a708f8fSGustavo F. Padovan del_timer(&l2cap_pi(sk)->monitor_timer); 4760a708f8fSGustavo F. Padovan del_timer(&l2cap_pi(sk)->ack_timer); 4770a708f8fSGustavo F. Padovan } 4780a708f8fSGustavo F. Padovan 4790a708f8fSGustavo F. Padovan req.dcid = cpu_to_le16(l2cap_pi(sk)->dcid); 4800a708f8fSGustavo F. Padovan req.scid = cpu_to_le16(l2cap_pi(sk)->scid); 4810a708f8fSGustavo F. Padovan l2cap_send_cmd(conn, l2cap_get_ident(conn), 4820a708f8fSGustavo F. Padovan L2CAP_DISCONN_REQ, sizeof(req), &req); 4830a708f8fSGustavo F. Padovan 4840a708f8fSGustavo F. Padovan sk->sk_state = BT_DISCONN; 4850a708f8fSGustavo F. Padovan sk->sk_err = err; 4860a708f8fSGustavo F. Padovan } 4870a708f8fSGustavo F. Padovan 4880a708f8fSGustavo F. Padovan /* ---- L2CAP connections ---- */ 4890a708f8fSGustavo F. Padovan static void l2cap_conn_start(struct l2cap_conn *conn) 4900a708f8fSGustavo F. Padovan { 4910a708f8fSGustavo F. Padovan struct l2cap_chan_list *l = &conn->chan_list; 4920a708f8fSGustavo F. Padovan struct sock_del_list del, *tmp1, *tmp2; 4930a708f8fSGustavo F. Padovan struct sock *sk; 4940a708f8fSGustavo F. Padovan 4950a708f8fSGustavo F. Padovan BT_DBG("conn %p", conn); 4960a708f8fSGustavo F. Padovan 4970a708f8fSGustavo F. Padovan INIT_LIST_HEAD(&del.list); 4980a708f8fSGustavo F. Padovan 4990a708f8fSGustavo F. Padovan read_lock(&l->lock); 5000a708f8fSGustavo F. Padovan 5010a708f8fSGustavo F. Padovan for (sk = l->head; sk; sk = l2cap_pi(sk)->next_c) { 5020a708f8fSGustavo F. Padovan bh_lock_sock(sk); 5030a708f8fSGustavo F. Padovan 5040a708f8fSGustavo F. Padovan if (sk->sk_type != SOCK_SEQPACKET && 5050a708f8fSGustavo F. Padovan sk->sk_type != SOCK_STREAM) { 5060a708f8fSGustavo F. Padovan bh_unlock_sock(sk); 5070a708f8fSGustavo F. Padovan continue; 5080a708f8fSGustavo F. Padovan } 5090a708f8fSGustavo F. Padovan 5100a708f8fSGustavo F. Padovan if (sk->sk_state == BT_CONNECT) { 5110a708f8fSGustavo F. Padovan struct l2cap_conn_req req; 5120a708f8fSGustavo F. Padovan 5130a708f8fSGustavo F. Padovan if (!l2cap_check_security(sk) || 5140a708f8fSGustavo F. Padovan !__l2cap_no_conn_pending(sk)) { 5150a708f8fSGustavo F. Padovan bh_unlock_sock(sk); 5160a708f8fSGustavo F. Padovan continue; 5170a708f8fSGustavo F. Padovan } 5180a708f8fSGustavo F. Padovan 5190a708f8fSGustavo F. Padovan if (!l2cap_mode_supported(l2cap_pi(sk)->mode, 5200a708f8fSGustavo F. Padovan conn->feat_mask) 5210a708f8fSGustavo F. Padovan && l2cap_pi(sk)->conf_state & 5220a708f8fSGustavo F. Padovan L2CAP_CONF_STATE2_DEVICE) { 5230a708f8fSGustavo F. Padovan tmp1 = kzalloc(sizeof(struct sock_del_list), 5240a708f8fSGustavo F. Padovan GFP_ATOMIC); 5250a708f8fSGustavo F. Padovan tmp1->sk = sk; 5260a708f8fSGustavo F. Padovan list_add_tail(&tmp1->list, &del.list); 5270a708f8fSGustavo F. Padovan bh_unlock_sock(sk); 5280a708f8fSGustavo F. Padovan continue; 5290a708f8fSGustavo F. Padovan } 5300a708f8fSGustavo F. Padovan 5310a708f8fSGustavo F. Padovan req.scid = cpu_to_le16(l2cap_pi(sk)->scid); 5320a708f8fSGustavo F. Padovan req.psm = l2cap_pi(sk)->psm; 5330a708f8fSGustavo F. Padovan 5340a708f8fSGustavo F. Padovan l2cap_pi(sk)->ident = l2cap_get_ident(conn); 5350a708f8fSGustavo F. Padovan l2cap_pi(sk)->conf_state |= L2CAP_CONF_CONNECT_PEND; 5360a708f8fSGustavo F. Padovan 5370a708f8fSGustavo F. Padovan l2cap_send_cmd(conn, l2cap_pi(sk)->ident, 5380a708f8fSGustavo F. Padovan L2CAP_CONN_REQ, sizeof(req), &req); 5390a708f8fSGustavo F. Padovan 5400a708f8fSGustavo F. Padovan } else if (sk->sk_state == BT_CONNECT2) { 5410a708f8fSGustavo F. Padovan struct l2cap_conn_rsp rsp; 5420a708f8fSGustavo F. Padovan char buf[128]; 5430a708f8fSGustavo F. Padovan rsp.scid = cpu_to_le16(l2cap_pi(sk)->dcid); 5440a708f8fSGustavo F. Padovan rsp.dcid = cpu_to_le16(l2cap_pi(sk)->scid); 5450a708f8fSGustavo F. Padovan 5460a708f8fSGustavo F. Padovan if (l2cap_check_security(sk)) { 5470a708f8fSGustavo F. Padovan if (bt_sk(sk)->defer_setup) { 5480a708f8fSGustavo F. Padovan struct sock *parent = bt_sk(sk)->parent; 5490a708f8fSGustavo F. Padovan rsp.result = cpu_to_le16(L2CAP_CR_PEND); 5500a708f8fSGustavo F. Padovan rsp.status = cpu_to_le16(L2CAP_CS_AUTHOR_PEND); 5510a708f8fSGustavo F. Padovan parent->sk_data_ready(parent, 0); 5520a708f8fSGustavo F. Padovan 5530a708f8fSGustavo F. Padovan } else { 5540a708f8fSGustavo F. Padovan sk->sk_state = BT_CONFIG; 5550a708f8fSGustavo F. Padovan rsp.result = cpu_to_le16(L2CAP_CR_SUCCESS); 5560a708f8fSGustavo F. Padovan rsp.status = cpu_to_le16(L2CAP_CS_NO_INFO); 5570a708f8fSGustavo F. Padovan } 5580a708f8fSGustavo F. Padovan } else { 5590a708f8fSGustavo F. Padovan rsp.result = cpu_to_le16(L2CAP_CR_PEND); 5600a708f8fSGustavo F. Padovan rsp.status = cpu_to_le16(L2CAP_CS_AUTHEN_PEND); 5610a708f8fSGustavo F. Padovan } 5620a708f8fSGustavo F. Padovan 5630a708f8fSGustavo F. Padovan l2cap_send_cmd(conn, l2cap_pi(sk)->ident, 5640a708f8fSGustavo F. Padovan L2CAP_CONN_RSP, sizeof(rsp), &rsp); 5650a708f8fSGustavo F. Padovan 5660a708f8fSGustavo F. Padovan if (l2cap_pi(sk)->conf_state & L2CAP_CONF_REQ_SENT || 5670a708f8fSGustavo F. Padovan rsp.result != L2CAP_CR_SUCCESS) { 5680a708f8fSGustavo F. Padovan bh_unlock_sock(sk); 5690a708f8fSGustavo F. Padovan continue; 5700a708f8fSGustavo F. Padovan } 5710a708f8fSGustavo F. Padovan 5720a708f8fSGustavo F. Padovan l2cap_pi(sk)->conf_state |= L2CAP_CONF_REQ_SENT; 5730a708f8fSGustavo F. Padovan l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ, 5740a708f8fSGustavo F. Padovan l2cap_build_conf_req(sk, buf), buf); 5750a708f8fSGustavo F. Padovan l2cap_pi(sk)->num_conf_req++; 5760a708f8fSGustavo F. Padovan } 5770a708f8fSGustavo F. Padovan 5780a708f8fSGustavo F. Padovan bh_unlock_sock(sk); 5790a708f8fSGustavo F. Padovan } 5800a708f8fSGustavo F. Padovan 5810a708f8fSGustavo F. Padovan read_unlock(&l->lock); 5820a708f8fSGustavo F. Padovan 5830a708f8fSGustavo F. Padovan list_for_each_entry_safe(tmp1, tmp2, &del.list, list) { 5840a708f8fSGustavo F. Padovan bh_lock_sock(tmp1->sk); 5850a708f8fSGustavo F. Padovan __l2cap_sock_close(tmp1->sk, ECONNRESET); 5860a708f8fSGustavo F. Padovan bh_unlock_sock(tmp1->sk); 5870a708f8fSGustavo F. Padovan list_del(&tmp1->list); 5880a708f8fSGustavo F. Padovan kfree(tmp1); 5890a708f8fSGustavo F. Padovan } 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 { 597b62f328bSVille Tervo struct sock *s, *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 } 616b62f328bSVille Tervo s = node ? sk : sk1; 617b62f328bSVille Tervo if (s) 618b62f328bSVille Tervo bh_lock_sock(s); 619b62f328bSVille Tervo read_unlock(&l2cap_sk_list.lock); 620b62f328bSVille Tervo 621b62f328bSVille Tervo return s; 622b62f328bSVille Tervo } 623b62f328bSVille Tervo 624b62f328bSVille Tervo static void l2cap_le_conn_ready(struct l2cap_conn *conn) 625b62f328bSVille Tervo { 626b62f328bSVille Tervo struct l2cap_chan_list *list = &conn->chan_list; 627b62f328bSVille Tervo struct sock *parent, *uninitialized_var(sk); 628b62f328bSVille Tervo 629b62f328bSVille Tervo BT_DBG(""); 630b62f328bSVille Tervo 631b62f328bSVille Tervo /* Check if we have socket listening on cid */ 632b62f328bSVille Tervo parent = l2cap_get_sock_by_scid(BT_LISTEN, L2CAP_CID_LE_DATA, 633b62f328bSVille Tervo conn->src); 634b62f328bSVille Tervo if (!parent) 635b62f328bSVille Tervo return; 636b62f328bSVille Tervo 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 647b62f328bSVille Tervo write_lock_bh(&list->lock); 648b62f328bSVille Tervo 649b62f328bSVille Tervo hci_conn_hold(conn->hcon); 650b62f328bSVille Tervo 651b62f328bSVille Tervo l2cap_sock_init(sk, parent); 652b62f328bSVille Tervo bacpy(&bt_sk(sk)->src, conn->src); 653b62f328bSVille Tervo bacpy(&bt_sk(sk)->dst, conn->dst); 654b62f328bSVille Tervo 655b62f328bSVille Tervo __l2cap_chan_add(conn, sk, parent); 656b62f328bSVille Tervo 657b62f328bSVille Tervo l2cap_sock_set_timer(sk, sk->sk_sndtimeo); 658b62f328bSVille Tervo 659b62f328bSVille Tervo sk->sk_state = BT_CONNECTED; 660b62f328bSVille Tervo parent->sk_data_ready(parent, 0); 661b62f328bSVille Tervo 662b62f328bSVille Tervo write_unlock_bh(&list->lock); 663b62f328bSVille Tervo 664b62f328bSVille Tervo clean: 665b62f328bSVille Tervo bh_unlock_sock(parent); 666b62f328bSVille Tervo } 667b62f328bSVille Tervo 6680a708f8fSGustavo F. Padovan static void l2cap_conn_ready(struct l2cap_conn *conn) 6690a708f8fSGustavo F. Padovan { 6700a708f8fSGustavo F. Padovan struct l2cap_chan_list *l = &conn->chan_list; 6710a708f8fSGustavo F. Padovan struct sock *sk; 6720a708f8fSGustavo F. Padovan 6730a708f8fSGustavo F. Padovan BT_DBG("conn %p", conn); 6740a708f8fSGustavo F. Padovan 675b62f328bSVille Tervo if (!conn->hcon->out && conn->hcon->type == LE_LINK) 676b62f328bSVille Tervo l2cap_le_conn_ready(conn); 677b62f328bSVille Tervo 6780a708f8fSGustavo F. Padovan read_lock(&l->lock); 6790a708f8fSGustavo F. Padovan 6800a708f8fSGustavo F. Padovan for (sk = l->head; sk; sk = l2cap_pi(sk)->next_c) { 6810a708f8fSGustavo F. Padovan bh_lock_sock(sk); 6820a708f8fSGustavo F. Padovan 683acd7d370SVille Tervo if (conn->hcon->type == LE_LINK) { 684acd7d370SVille Tervo l2cap_sock_clear_timer(sk); 685acd7d370SVille Tervo sk->sk_state = BT_CONNECTED; 686acd7d370SVille Tervo sk->sk_state_change(sk); 687acd7d370SVille Tervo } 688acd7d370SVille Tervo 6890a708f8fSGustavo F. Padovan if (sk->sk_type != SOCK_SEQPACKET && 6900a708f8fSGustavo F. Padovan sk->sk_type != SOCK_STREAM) { 6910a708f8fSGustavo F. Padovan l2cap_sock_clear_timer(sk); 6920a708f8fSGustavo F. Padovan sk->sk_state = BT_CONNECTED; 6930a708f8fSGustavo F. Padovan sk->sk_state_change(sk); 6940a708f8fSGustavo F. Padovan } else if (sk->sk_state == BT_CONNECT) 6950a708f8fSGustavo F. Padovan l2cap_do_start(sk); 6960a708f8fSGustavo F. Padovan 6970a708f8fSGustavo F. Padovan bh_unlock_sock(sk); 6980a708f8fSGustavo F. Padovan } 6990a708f8fSGustavo F. Padovan 7000a708f8fSGustavo F. Padovan read_unlock(&l->lock); 7010a708f8fSGustavo F. Padovan } 7020a708f8fSGustavo F. Padovan 7030a708f8fSGustavo F. Padovan /* Notify sockets that we cannot guaranty reliability anymore */ 7040a708f8fSGustavo F. Padovan static void l2cap_conn_unreliable(struct l2cap_conn *conn, int err) 7050a708f8fSGustavo F. Padovan { 7060a708f8fSGustavo F. Padovan struct l2cap_chan_list *l = &conn->chan_list; 7070a708f8fSGustavo F. Padovan struct sock *sk; 7080a708f8fSGustavo F. Padovan 7090a708f8fSGustavo F. Padovan BT_DBG("conn %p", conn); 7100a708f8fSGustavo F. Padovan 7110a708f8fSGustavo F. Padovan read_lock(&l->lock); 7120a708f8fSGustavo F. Padovan 7130a708f8fSGustavo F. Padovan for (sk = l->head; sk; sk = l2cap_pi(sk)->next_c) { 7140a708f8fSGustavo F. Padovan if (l2cap_pi(sk)->force_reliable) 7150a708f8fSGustavo F. Padovan sk->sk_err = err; 7160a708f8fSGustavo F. Padovan } 7170a708f8fSGustavo F. Padovan 7180a708f8fSGustavo F. Padovan read_unlock(&l->lock); 7190a708f8fSGustavo F. Padovan } 7200a708f8fSGustavo F. Padovan 7210a708f8fSGustavo F. Padovan static void l2cap_info_timeout(unsigned long arg) 7220a708f8fSGustavo F. Padovan { 7230a708f8fSGustavo F. Padovan struct l2cap_conn *conn = (void *) arg; 7240a708f8fSGustavo F. Padovan 7250a708f8fSGustavo F. Padovan conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_DONE; 7260a708f8fSGustavo F. Padovan conn->info_ident = 0; 7270a708f8fSGustavo F. Padovan 7280a708f8fSGustavo F. Padovan l2cap_conn_start(conn); 7290a708f8fSGustavo F. Padovan } 7300a708f8fSGustavo F. Padovan 7310a708f8fSGustavo F. Padovan static struct l2cap_conn *l2cap_conn_add(struct hci_conn *hcon, u8 status) 7320a708f8fSGustavo F. Padovan { 7330a708f8fSGustavo F. Padovan struct l2cap_conn *conn = hcon->l2cap_data; 7340a708f8fSGustavo F. Padovan 7350a708f8fSGustavo F. Padovan if (conn || status) 7360a708f8fSGustavo F. Padovan return conn; 7370a708f8fSGustavo F. Padovan 7380a708f8fSGustavo F. Padovan conn = kzalloc(sizeof(struct l2cap_conn), GFP_ATOMIC); 7390a708f8fSGustavo F. Padovan if (!conn) 7400a708f8fSGustavo F. Padovan return NULL; 7410a708f8fSGustavo F. Padovan 7420a708f8fSGustavo F. Padovan hcon->l2cap_data = conn; 7430a708f8fSGustavo F. Padovan conn->hcon = hcon; 7440a708f8fSGustavo F. Padovan 7450a708f8fSGustavo F. Padovan BT_DBG("hcon %p conn %p", hcon, conn); 7460a708f8fSGustavo F. Padovan 747acd7d370SVille Tervo if (hcon->hdev->le_mtu && hcon->type == LE_LINK) 748acd7d370SVille Tervo conn->mtu = hcon->hdev->le_mtu; 749acd7d370SVille Tervo else 7500a708f8fSGustavo F. Padovan conn->mtu = hcon->hdev->acl_mtu; 751acd7d370SVille Tervo 7520a708f8fSGustavo F. Padovan conn->src = &hcon->hdev->bdaddr; 7530a708f8fSGustavo F. Padovan conn->dst = &hcon->dst; 7540a708f8fSGustavo F. Padovan 7550a708f8fSGustavo F. Padovan conn->feat_mask = 0; 7560a708f8fSGustavo F. Padovan 7570a708f8fSGustavo F. Padovan spin_lock_init(&conn->lock); 7580a708f8fSGustavo F. Padovan rwlock_init(&conn->chan_list.lock); 7590a708f8fSGustavo F. Padovan 760b62f328bSVille Tervo if (hcon->type != LE_LINK) 7610a708f8fSGustavo F. Padovan setup_timer(&conn->info_timer, l2cap_info_timeout, 7620a708f8fSGustavo F. Padovan (unsigned long) conn); 7630a708f8fSGustavo F. Padovan 7640a708f8fSGustavo F. Padovan conn->disc_reason = 0x13; 7650a708f8fSGustavo F. Padovan 7660a708f8fSGustavo F. Padovan return conn; 7670a708f8fSGustavo F. Padovan } 7680a708f8fSGustavo F. Padovan 7690a708f8fSGustavo F. Padovan static void l2cap_conn_del(struct hci_conn *hcon, int err) 7700a708f8fSGustavo F. Padovan { 7710a708f8fSGustavo F. Padovan struct l2cap_conn *conn = hcon->l2cap_data; 7720a708f8fSGustavo F. Padovan struct sock *sk; 7730a708f8fSGustavo F. Padovan 7740a708f8fSGustavo F. Padovan if (!conn) 7750a708f8fSGustavo F. Padovan return; 7760a708f8fSGustavo F. Padovan 7770a708f8fSGustavo F. Padovan BT_DBG("hcon %p conn %p, err %d", hcon, conn, err); 7780a708f8fSGustavo F. Padovan 7790a708f8fSGustavo F. Padovan kfree_skb(conn->rx_skb); 7800a708f8fSGustavo F. Padovan 7810a708f8fSGustavo F. Padovan /* Kill channels */ 7820a708f8fSGustavo F. Padovan while ((sk = conn->chan_list.head)) { 7830a708f8fSGustavo F. Padovan bh_lock_sock(sk); 7840a708f8fSGustavo F. Padovan l2cap_chan_del(sk, err); 7850a708f8fSGustavo F. Padovan bh_unlock_sock(sk); 7860a708f8fSGustavo F. Padovan l2cap_sock_kill(sk); 7870a708f8fSGustavo F. Padovan } 7880a708f8fSGustavo F. Padovan 7890a708f8fSGustavo F. Padovan if (conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_SENT) 7900a708f8fSGustavo F. Padovan del_timer_sync(&conn->info_timer); 7910a708f8fSGustavo F. Padovan 7920a708f8fSGustavo F. Padovan hcon->l2cap_data = NULL; 7930a708f8fSGustavo F. Padovan kfree(conn); 7940a708f8fSGustavo F. Padovan } 7950a708f8fSGustavo F. Padovan 7960a708f8fSGustavo F. Padovan static inline void l2cap_chan_add(struct l2cap_conn *conn, struct sock *sk, struct sock *parent) 7970a708f8fSGustavo F. Padovan { 7980a708f8fSGustavo F. Padovan struct l2cap_chan_list *l = &conn->chan_list; 7990a708f8fSGustavo F. Padovan write_lock_bh(&l->lock); 8000a708f8fSGustavo F. Padovan __l2cap_chan_add(conn, sk, parent); 8010a708f8fSGustavo F. Padovan write_unlock_bh(&l->lock); 8020a708f8fSGustavo F. Padovan } 8030a708f8fSGustavo F. Padovan 8040a708f8fSGustavo F. Padovan /* ---- Socket interface ---- */ 8050a708f8fSGustavo F. Padovan 8060a708f8fSGustavo F. Padovan /* Find socket with psm and source bdaddr. 8070a708f8fSGustavo F. Padovan * Returns closest match. 8080a708f8fSGustavo F. Padovan */ 8090a708f8fSGustavo F. Padovan static struct sock *l2cap_get_sock_by_psm(int state, __le16 psm, bdaddr_t *src) 8100a708f8fSGustavo F. Padovan { 8110a708f8fSGustavo F. Padovan struct sock *sk = NULL, *sk1 = NULL; 8120a708f8fSGustavo F. Padovan struct hlist_node *node; 8130a708f8fSGustavo F. Padovan 8140a708f8fSGustavo F. Padovan read_lock(&l2cap_sk_list.lock); 8150a708f8fSGustavo F. Padovan 8160a708f8fSGustavo F. Padovan sk_for_each(sk, node, &l2cap_sk_list.head) { 8170a708f8fSGustavo F. Padovan if (state && sk->sk_state != state) 8180a708f8fSGustavo F. Padovan continue; 8190a708f8fSGustavo F. Padovan 8200a708f8fSGustavo F. Padovan if (l2cap_pi(sk)->psm == psm) { 8210a708f8fSGustavo F. Padovan /* Exact match. */ 8220a708f8fSGustavo F. Padovan if (!bacmp(&bt_sk(sk)->src, src)) 8230a708f8fSGustavo F. Padovan break; 8240a708f8fSGustavo F. Padovan 8250a708f8fSGustavo F. Padovan /* Closest match */ 8260a708f8fSGustavo F. Padovan if (!bacmp(&bt_sk(sk)->src, BDADDR_ANY)) 8270a708f8fSGustavo F. Padovan sk1 = sk; 8280a708f8fSGustavo F. Padovan } 8290a708f8fSGustavo F. Padovan } 8300a708f8fSGustavo F. Padovan 8310a708f8fSGustavo F. Padovan read_unlock(&l2cap_sk_list.lock); 8320a708f8fSGustavo F. Padovan 8330a708f8fSGustavo F. Padovan return node ? sk : sk1; 8340a708f8fSGustavo F. Padovan } 8350a708f8fSGustavo F. Padovan 8364e34c50bSGustavo F. Padovan int l2cap_do_connect(struct sock *sk) 8370a708f8fSGustavo F. Padovan { 8380a708f8fSGustavo F. Padovan bdaddr_t *src = &bt_sk(sk)->src; 8390a708f8fSGustavo F. Padovan bdaddr_t *dst = &bt_sk(sk)->dst; 8400a708f8fSGustavo F. Padovan struct l2cap_conn *conn; 8410a708f8fSGustavo F. Padovan struct hci_conn *hcon; 8420a708f8fSGustavo F. Padovan struct hci_dev *hdev; 8430a708f8fSGustavo F. Padovan __u8 auth_type; 8440a708f8fSGustavo F. Padovan int err; 8450a708f8fSGustavo F. Padovan 8460a708f8fSGustavo F. Padovan BT_DBG("%s -> %s psm 0x%2.2x", batostr(src), batostr(dst), 8470a708f8fSGustavo F. Padovan l2cap_pi(sk)->psm); 8480a708f8fSGustavo F. Padovan 8490a708f8fSGustavo F. Padovan hdev = hci_get_route(dst, src); 8500a708f8fSGustavo F. Padovan if (!hdev) 8510a708f8fSGustavo F. Padovan return -EHOSTUNREACH; 8520a708f8fSGustavo F. Padovan 8530a708f8fSGustavo F. Padovan hci_dev_lock_bh(hdev); 8540a708f8fSGustavo F. Padovan 8550a708f8fSGustavo F. Padovan err = -ENOMEM; 8560a708f8fSGustavo F. Padovan 8570a708f8fSGustavo F. Padovan auth_type = l2cap_get_auth_type(sk); 8580a708f8fSGustavo F. Padovan 859acd7d370SVille Tervo if (l2cap_pi(sk)->dcid == L2CAP_CID_LE_DATA) 860acd7d370SVille Tervo hcon = hci_connect(hdev, LE_LINK, dst, 861acd7d370SVille Tervo l2cap_pi(sk)->sec_level, auth_type); 862acd7d370SVille Tervo else 8630a708f8fSGustavo F. Padovan hcon = hci_connect(hdev, ACL_LINK, dst, 8640a708f8fSGustavo F. Padovan l2cap_pi(sk)->sec_level, auth_type); 865acd7d370SVille Tervo 8660a708f8fSGustavo F. Padovan if (!hcon) 8670a708f8fSGustavo F. Padovan goto done; 8680a708f8fSGustavo F. Padovan 8690a708f8fSGustavo F. Padovan conn = l2cap_conn_add(hcon, 0); 8700a708f8fSGustavo F. Padovan if (!conn) { 8710a708f8fSGustavo F. Padovan hci_conn_put(hcon); 8720a708f8fSGustavo F. Padovan goto done; 8730a708f8fSGustavo F. Padovan } 8740a708f8fSGustavo F. Padovan 8750a708f8fSGustavo F. Padovan err = 0; 8760a708f8fSGustavo F. Padovan 8770a708f8fSGustavo F. Padovan /* Update source addr of the socket */ 8780a708f8fSGustavo F. Padovan bacpy(src, conn->src); 8790a708f8fSGustavo F. Padovan 8800a708f8fSGustavo F. Padovan l2cap_chan_add(conn, sk, NULL); 8810a708f8fSGustavo F. Padovan 8820a708f8fSGustavo F. Padovan sk->sk_state = BT_CONNECT; 8830a708f8fSGustavo F. Padovan l2cap_sock_set_timer(sk, sk->sk_sndtimeo); 8840a708f8fSGustavo F. Padovan 8850a708f8fSGustavo F. Padovan if (hcon->state == BT_CONNECTED) { 8860a708f8fSGustavo F. Padovan if (sk->sk_type != SOCK_SEQPACKET && 8870a708f8fSGustavo F. Padovan sk->sk_type != SOCK_STREAM) { 8880a708f8fSGustavo F. Padovan l2cap_sock_clear_timer(sk); 8890a708f8fSGustavo F. Padovan if (l2cap_check_security(sk)) 8900a708f8fSGustavo F. Padovan sk->sk_state = BT_CONNECTED; 8910a708f8fSGustavo F. Padovan } else 8920a708f8fSGustavo F. Padovan l2cap_do_start(sk); 8930a708f8fSGustavo F. Padovan } 8940a708f8fSGustavo F. Padovan 8950a708f8fSGustavo F. Padovan done: 8960a708f8fSGustavo F. Padovan hci_dev_unlock_bh(hdev); 8970a708f8fSGustavo F. Padovan hci_dev_put(hdev); 8980a708f8fSGustavo F. Padovan return err; 8990a708f8fSGustavo F. Padovan } 9000a708f8fSGustavo F. Padovan 901dcba0dbaSGustavo F. Padovan int __l2cap_wait_ack(struct sock *sk) 9020a708f8fSGustavo F. Padovan { 9030a708f8fSGustavo F. Padovan DECLARE_WAITQUEUE(wait, current); 9040a708f8fSGustavo F. Padovan int err = 0; 9050a708f8fSGustavo F. Padovan int timeo = HZ/5; 9060a708f8fSGustavo F. Padovan 9070a708f8fSGustavo F. Padovan add_wait_queue(sk_sleep(sk), &wait); 9080a708f8fSGustavo F. Padovan while ((l2cap_pi(sk)->unacked_frames > 0 && l2cap_pi(sk)->conn)) { 9090a708f8fSGustavo F. Padovan set_current_state(TASK_INTERRUPTIBLE); 9100a708f8fSGustavo F. Padovan 9110a708f8fSGustavo F. Padovan if (!timeo) 9120a708f8fSGustavo F. Padovan timeo = HZ/5; 9130a708f8fSGustavo F. Padovan 9140a708f8fSGustavo F. Padovan if (signal_pending(current)) { 9150a708f8fSGustavo F. Padovan err = sock_intr_errno(timeo); 9160a708f8fSGustavo F. Padovan break; 9170a708f8fSGustavo F. Padovan } 9180a708f8fSGustavo F. Padovan 9190a708f8fSGustavo F. Padovan release_sock(sk); 9200a708f8fSGustavo F. Padovan timeo = schedule_timeout(timeo); 9210a708f8fSGustavo F. Padovan lock_sock(sk); 9220a708f8fSGustavo F. Padovan 9230a708f8fSGustavo F. Padovan err = sock_error(sk); 9240a708f8fSGustavo F. Padovan if (err) 9250a708f8fSGustavo F. Padovan break; 9260a708f8fSGustavo F. Padovan } 9270a708f8fSGustavo F. Padovan set_current_state(TASK_RUNNING); 9280a708f8fSGustavo F. Padovan remove_wait_queue(sk_sleep(sk), &wait); 9290a708f8fSGustavo F. Padovan return err; 9300a708f8fSGustavo F. Padovan } 9310a708f8fSGustavo F. Padovan 9320a708f8fSGustavo F. Padovan static void l2cap_monitor_timeout(unsigned long arg) 9330a708f8fSGustavo F. Padovan { 9340a708f8fSGustavo F. Padovan struct sock *sk = (void *) arg; 9350a708f8fSGustavo F. Padovan 9360a708f8fSGustavo F. Padovan BT_DBG("sk %p", sk); 9370a708f8fSGustavo F. Padovan 9380a708f8fSGustavo F. Padovan bh_lock_sock(sk); 9390a708f8fSGustavo F. Padovan if (l2cap_pi(sk)->retry_count >= l2cap_pi(sk)->remote_max_tx) { 9400a708f8fSGustavo F. Padovan l2cap_send_disconn_req(l2cap_pi(sk)->conn, sk, ECONNABORTED); 9410a708f8fSGustavo F. Padovan bh_unlock_sock(sk); 9420a708f8fSGustavo F. Padovan return; 9430a708f8fSGustavo F. Padovan } 9440a708f8fSGustavo F. Padovan 9450a708f8fSGustavo F. Padovan l2cap_pi(sk)->retry_count++; 9460a708f8fSGustavo F. Padovan __mod_monitor_timer(); 9470a708f8fSGustavo F. Padovan 9480a708f8fSGustavo F. Padovan l2cap_send_rr_or_rnr(l2cap_pi(sk), L2CAP_CTRL_POLL); 9490a708f8fSGustavo F. Padovan bh_unlock_sock(sk); 9500a708f8fSGustavo F. Padovan } 9510a708f8fSGustavo F. Padovan 9520a708f8fSGustavo F. Padovan static void l2cap_retrans_timeout(unsigned long arg) 9530a708f8fSGustavo F. Padovan { 9540a708f8fSGustavo F. Padovan struct sock *sk = (void *) arg; 9550a708f8fSGustavo F. Padovan 9560a708f8fSGustavo F. Padovan BT_DBG("sk %p", sk); 9570a708f8fSGustavo F. Padovan 9580a708f8fSGustavo F. Padovan bh_lock_sock(sk); 9590a708f8fSGustavo F. Padovan l2cap_pi(sk)->retry_count = 1; 9600a708f8fSGustavo F. Padovan __mod_monitor_timer(); 9610a708f8fSGustavo F. Padovan 9620a708f8fSGustavo F. Padovan l2cap_pi(sk)->conn_state |= L2CAP_CONN_WAIT_F; 9630a708f8fSGustavo F. Padovan 9640a708f8fSGustavo F. Padovan l2cap_send_rr_or_rnr(l2cap_pi(sk), L2CAP_CTRL_POLL); 9650a708f8fSGustavo F. Padovan bh_unlock_sock(sk); 9660a708f8fSGustavo F. Padovan } 9670a708f8fSGustavo F. Padovan 9680a708f8fSGustavo F. Padovan static void l2cap_drop_acked_frames(struct sock *sk) 9690a708f8fSGustavo F. Padovan { 9700a708f8fSGustavo F. Padovan struct sk_buff *skb; 9710a708f8fSGustavo F. Padovan 9720a708f8fSGustavo F. Padovan while ((skb = skb_peek(TX_QUEUE(sk))) && 9730a708f8fSGustavo F. Padovan l2cap_pi(sk)->unacked_frames) { 9740a708f8fSGustavo F. Padovan if (bt_cb(skb)->tx_seq == l2cap_pi(sk)->expected_ack_seq) 9750a708f8fSGustavo F. Padovan break; 9760a708f8fSGustavo F. Padovan 9770a708f8fSGustavo F. Padovan skb = skb_dequeue(TX_QUEUE(sk)); 9780a708f8fSGustavo F. Padovan kfree_skb(skb); 9790a708f8fSGustavo F. Padovan 9800a708f8fSGustavo F. Padovan l2cap_pi(sk)->unacked_frames--; 9810a708f8fSGustavo F. Padovan } 9820a708f8fSGustavo F. Padovan 9830a708f8fSGustavo F. Padovan if (!l2cap_pi(sk)->unacked_frames) 9840a708f8fSGustavo F. Padovan del_timer(&l2cap_pi(sk)->retrans_timer); 9850a708f8fSGustavo F. Padovan } 9860a708f8fSGustavo F. Padovan 987fd83ccdbSGustavo F. Padovan void l2cap_do_send(struct sock *sk, struct sk_buff *skb) 9880a708f8fSGustavo F. Padovan { 9890a708f8fSGustavo F. Padovan struct l2cap_pinfo *pi = l2cap_pi(sk); 9900a708f8fSGustavo F. Padovan struct hci_conn *hcon = pi->conn->hcon; 9910a708f8fSGustavo F. Padovan u16 flags; 9920a708f8fSGustavo F. Padovan 9930a708f8fSGustavo F. Padovan BT_DBG("sk %p, skb %p len %d", sk, skb, skb->len); 9940a708f8fSGustavo F. Padovan 9950a708f8fSGustavo F. Padovan if (!pi->flushable && lmp_no_flush_capable(hcon->hdev)) 9960a708f8fSGustavo F. Padovan flags = ACL_START_NO_FLUSH; 9970a708f8fSGustavo F. Padovan else 9980a708f8fSGustavo F. Padovan flags = ACL_START; 9990a708f8fSGustavo F. Padovan 10000a708f8fSGustavo F. Padovan hci_send_acl(hcon, skb, flags); 10010a708f8fSGustavo F. Padovan } 10020a708f8fSGustavo F. Padovan 1003fd83ccdbSGustavo F. Padovan void l2cap_streaming_send(struct sock *sk) 10040a708f8fSGustavo F. Padovan { 10050a708f8fSGustavo F. Padovan struct sk_buff *skb; 10060a708f8fSGustavo F. Padovan struct l2cap_pinfo *pi = l2cap_pi(sk); 10070a708f8fSGustavo F. Padovan u16 control, fcs; 10080a708f8fSGustavo F. Padovan 10090a708f8fSGustavo F. Padovan while ((skb = skb_dequeue(TX_QUEUE(sk)))) { 10100a708f8fSGustavo F. Padovan control = get_unaligned_le16(skb->data + L2CAP_HDR_SIZE); 10110a708f8fSGustavo F. Padovan control |= pi->next_tx_seq << L2CAP_CTRL_TXSEQ_SHIFT; 10120a708f8fSGustavo F. Padovan put_unaligned_le16(control, skb->data + L2CAP_HDR_SIZE); 10130a708f8fSGustavo F. Padovan 10140a708f8fSGustavo F. Padovan if (pi->fcs == L2CAP_FCS_CRC16) { 10150a708f8fSGustavo F. Padovan fcs = crc16(0, (u8 *)skb->data, skb->len - 2); 10160a708f8fSGustavo F. Padovan put_unaligned_le16(fcs, skb->data + skb->len - 2); 10170a708f8fSGustavo F. Padovan } 10180a708f8fSGustavo F. Padovan 10190a708f8fSGustavo F. Padovan l2cap_do_send(sk, skb); 10200a708f8fSGustavo F. Padovan 10210a708f8fSGustavo F. Padovan pi->next_tx_seq = (pi->next_tx_seq + 1) % 64; 10220a708f8fSGustavo F. Padovan } 10230a708f8fSGustavo F. Padovan } 10240a708f8fSGustavo F. Padovan 10250a708f8fSGustavo F. Padovan static void l2cap_retransmit_one_frame(struct sock *sk, u8 tx_seq) 10260a708f8fSGustavo F. Padovan { 10270a708f8fSGustavo F. Padovan struct l2cap_pinfo *pi = l2cap_pi(sk); 10280a708f8fSGustavo F. Padovan struct sk_buff *skb, *tx_skb; 10290a708f8fSGustavo F. Padovan u16 control, fcs; 10300a708f8fSGustavo F. Padovan 10310a708f8fSGustavo F. Padovan skb = skb_peek(TX_QUEUE(sk)); 10320a708f8fSGustavo F. Padovan if (!skb) 10330a708f8fSGustavo F. Padovan return; 10340a708f8fSGustavo F. Padovan 10350a708f8fSGustavo F. Padovan do { 10360a708f8fSGustavo F. Padovan if (bt_cb(skb)->tx_seq == tx_seq) 10370a708f8fSGustavo F. Padovan break; 10380a708f8fSGustavo F. Padovan 10390a708f8fSGustavo F. Padovan if (skb_queue_is_last(TX_QUEUE(sk), skb)) 10400a708f8fSGustavo F. Padovan return; 10410a708f8fSGustavo F. Padovan 10420a708f8fSGustavo F. Padovan } while ((skb = skb_queue_next(TX_QUEUE(sk), skb))); 10430a708f8fSGustavo F. Padovan 10440a708f8fSGustavo F. Padovan if (pi->remote_max_tx && 10450a708f8fSGustavo F. Padovan bt_cb(skb)->retries == pi->remote_max_tx) { 10460a708f8fSGustavo F. Padovan l2cap_send_disconn_req(pi->conn, sk, ECONNABORTED); 10470a708f8fSGustavo F. Padovan return; 10480a708f8fSGustavo F. Padovan } 10490a708f8fSGustavo F. Padovan 10500a708f8fSGustavo F. Padovan tx_skb = skb_clone(skb, GFP_ATOMIC); 10510a708f8fSGustavo F. Padovan bt_cb(skb)->retries++; 10520a708f8fSGustavo F. Padovan control = get_unaligned_le16(tx_skb->data + L2CAP_HDR_SIZE); 10530a708f8fSGustavo F. Padovan 10540a708f8fSGustavo F. Padovan if (pi->conn_state & L2CAP_CONN_SEND_FBIT) { 10550a708f8fSGustavo F. Padovan control |= L2CAP_CTRL_FINAL; 10560a708f8fSGustavo F. Padovan pi->conn_state &= ~L2CAP_CONN_SEND_FBIT; 10570a708f8fSGustavo F. Padovan } 10580a708f8fSGustavo F. Padovan 10590a708f8fSGustavo F. Padovan control |= (pi->buffer_seq << L2CAP_CTRL_REQSEQ_SHIFT) 10600a708f8fSGustavo F. Padovan | (tx_seq << L2CAP_CTRL_TXSEQ_SHIFT); 10610a708f8fSGustavo F. Padovan 10620a708f8fSGustavo F. Padovan put_unaligned_le16(control, tx_skb->data + L2CAP_HDR_SIZE); 10630a708f8fSGustavo F. Padovan 10640a708f8fSGustavo F. Padovan if (pi->fcs == L2CAP_FCS_CRC16) { 10650a708f8fSGustavo F. Padovan fcs = crc16(0, (u8 *)tx_skb->data, tx_skb->len - 2); 10660a708f8fSGustavo F. Padovan put_unaligned_le16(fcs, tx_skb->data + tx_skb->len - 2); 10670a708f8fSGustavo F. Padovan } 10680a708f8fSGustavo F. Padovan 10690a708f8fSGustavo F. Padovan l2cap_do_send(sk, tx_skb); 10700a708f8fSGustavo F. Padovan } 10710a708f8fSGustavo F. Padovan 1072fd83ccdbSGustavo F. Padovan int l2cap_ertm_send(struct sock *sk) 10730a708f8fSGustavo F. Padovan { 10740a708f8fSGustavo F. Padovan struct sk_buff *skb, *tx_skb; 10750a708f8fSGustavo F. Padovan struct l2cap_pinfo *pi = l2cap_pi(sk); 10760a708f8fSGustavo F. Padovan u16 control, fcs; 10770a708f8fSGustavo F. Padovan int nsent = 0; 10780a708f8fSGustavo F. Padovan 10790a708f8fSGustavo F. Padovan if (sk->sk_state != BT_CONNECTED) 10800a708f8fSGustavo F. Padovan return -ENOTCONN; 10810a708f8fSGustavo F. Padovan 10820a708f8fSGustavo F. Padovan while ((skb = sk->sk_send_head) && (!l2cap_tx_window_full(sk))) { 10830a708f8fSGustavo F. Padovan 10840a708f8fSGustavo F. Padovan if (pi->remote_max_tx && 10850a708f8fSGustavo F. Padovan bt_cb(skb)->retries == pi->remote_max_tx) { 10860a708f8fSGustavo F. Padovan l2cap_send_disconn_req(pi->conn, sk, ECONNABORTED); 10870a708f8fSGustavo F. Padovan break; 10880a708f8fSGustavo F. Padovan } 10890a708f8fSGustavo F. Padovan 10900a708f8fSGustavo F. Padovan tx_skb = skb_clone(skb, GFP_ATOMIC); 10910a708f8fSGustavo F. Padovan 10920a708f8fSGustavo F. Padovan bt_cb(skb)->retries++; 10930a708f8fSGustavo F. Padovan 10940a708f8fSGustavo F. Padovan control = get_unaligned_le16(tx_skb->data + L2CAP_HDR_SIZE); 10950a708f8fSGustavo F. Padovan control &= L2CAP_CTRL_SAR; 10960a708f8fSGustavo F. Padovan 10970a708f8fSGustavo F. Padovan if (pi->conn_state & L2CAP_CONN_SEND_FBIT) { 10980a708f8fSGustavo F. Padovan control |= L2CAP_CTRL_FINAL; 10990a708f8fSGustavo F. Padovan pi->conn_state &= ~L2CAP_CONN_SEND_FBIT; 11000a708f8fSGustavo F. Padovan } 11010a708f8fSGustavo F. Padovan control |= (pi->buffer_seq << L2CAP_CTRL_REQSEQ_SHIFT) 11020a708f8fSGustavo F. Padovan | (pi->next_tx_seq << L2CAP_CTRL_TXSEQ_SHIFT); 11030a708f8fSGustavo F. Padovan put_unaligned_le16(control, tx_skb->data + L2CAP_HDR_SIZE); 11040a708f8fSGustavo F. Padovan 11050a708f8fSGustavo F. Padovan 11060a708f8fSGustavo F. Padovan if (pi->fcs == L2CAP_FCS_CRC16) { 11070a708f8fSGustavo F. Padovan fcs = crc16(0, (u8 *)skb->data, tx_skb->len - 2); 11080a708f8fSGustavo F. Padovan put_unaligned_le16(fcs, skb->data + tx_skb->len - 2); 11090a708f8fSGustavo F. Padovan } 11100a708f8fSGustavo F. Padovan 11110a708f8fSGustavo F. Padovan l2cap_do_send(sk, tx_skb); 11120a708f8fSGustavo F. Padovan 11130a708f8fSGustavo F. Padovan __mod_retrans_timer(); 11140a708f8fSGustavo F. Padovan 11150a708f8fSGustavo F. Padovan bt_cb(skb)->tx_seq = pi->next_tx_seq; 11160a708f8fSGustavo F. Padovan pi->next_tx_seq = (pi->next_tx_seq + 1) % 64; 11170a708f8fSGustavo F. Padovan 11180a708f8fSGustavo F. Padovan pi->unacked_frames++; 11190a708f8fSGustavo F. Padovan pi->frames_sent++; 11200a708f8fSGustavo F. Padovan 11210a708f8fSGustavo F. Padovan if (skb_queue_is_last(TX_QUEUE(sk), skb)) 11220a708f8fSGustavo F. Padovan sk->sk_send_head = NULL; 11230a708f8fSGustavo F. Padovan else 11240a708f8fSGustavo F. Padovan sk->sk_send_head = skb_queue_next(TX_QUEUE(sk), skb); 11250a708f8fSGustavo F. Padovan 11260a708f8fSGustavo F. Padovan nsent++; 11270a708f8fSGustavo F. Padovan } 11280a708f8fSGustavo F. Padovan 11290a708f8fSGustavo F. Padovan return nsent; 11300a708f8fSGustavo F. Padovan } 11310a708f8fSGustavo F. Padovan 11320a708f8fSGustavo F. Padovan static int l2cap_retransmit_frames(struct sock *sk) 11330a708f8fSGustavo F. Padovan { 11340a708f8fSGustavo F. Padovan struct l2cap_pinfo *pi = l2cap_pi(sk); 11350a708f8fSGustavo F. Padovan int ret; 11360a708f8fSGustavo F. Padovan 11370a708f8fSGustavo F. Padovan if (!skb_queue_empty(TX_QUEUE(sk))) 11380a708f8fSGustavo F. Padovan sk->sk_send_head = TX_QUEUE(sk)->next; 11390a708f8fSGustavo F. Padovan 11400a708f8fSGustavo F. Padovan pi->next_tx_seq = pi->expected_ack_seq; 11410a708f8fSGustavo F. Padovan ret = l2cap_ertm_send(sk); 11420a708f8fSGustavo F. Padovan return ret; 11430a708f8fSGustavo F. Padovan } 11440a708f8fSGustavo F. Padovan 11450a708f8fSGustavo F. Padovan static void l2cap_send_ack(struct l2cap_pinfo *pi) 11460a708f8fSGustavo F. Padovan { 11470a708f8fSGustavo F. Padovan struct sock *sk = (struct sock *)pi; 11480a708f8fSGustavo F. Padovan u16 control = 0; 11490a708f8fSGustavo F. Padovan 11500a708f8fSGustavo F. Padovan control |= pi->buffer_seq << L2CAP_CTRL_REQSEQ_SHIFT; 11510a708f8fSGustavo F. Padovan 11520a708f8fSGustavo F. Padovan if (pi->conn_state & L2CAP_CONN_LOCAL_BUSY) { 11530a708f8fSGustavo F. Padovan control |= L2CAP_SUPER_RCV_NOT_READY; 11540a708f8fSGustavo F. Padovan pi->conn_state |= L2CAP_CONN_RNR_SENT; 11550a708f8fSGustavo F. Padovan l2cap_send_sframe(pi, control); 11560a708f8fSGustavo F. Padovan return; 11570a708f8fSGustavo F. Padovan } 11580a708f8fSGustavo F. Padovan 11590a708f8fSGustavo F. Padovan if (l2cap_ertm_send(sk) > 0) 11600a708f8fSGustavo F. Padovan return; 11610a708f8fSGustavo F. Padovan 11620a708f8fSGustavo F. Padovan control |= L2CAP_SUPER_RCV_READY; 11630a708f8fSGustavo F. Padovan l2cap_send_sframe(pi, control); 11640a708f8fSGustavo F. Padovan } 11650a708f8fSGustavo F. Padovan 11660a708f8fSGustavo F. Padovan static void l2cap_send_srejtail(struct sock *sk) 11670a708f8fSGustavo F. Padovan { 11680a708f8fSGustavo F. Padovan struct srej_list *tail; 11690a708f8fSGustavo F. Padovan u16 control; 11700a708f8fSGustavo F. Padovan 11710a708f8fSGustavo F. Padovan control = L2CAP_SUPER_SELECT_REJECT; 11720a708f8fSGustavo F. Padovan control |= L2CAP_CTRL_FINAL; 11730a708f8fSGustavo F. Padovan 11740a708f8fSGustavo F. Padovan tail = list_entry(SREJ_LIST(sk)->prev, struct srej_list, list); 11750a708f8fSGustavo F. Padovan control |= tail->tx_seq << L2CAP_CTRL_REQSEQ_SHIFT; 11760a708f8fSGustavo F. Padovan 11770a708f8fSGustavo F. Padovan l2cap_send_sframe(l2cap_pi(sk), control); 11780a708f8fSGustavo F. Padovan } 11790a708f8fSGustavo F. Padovan 11800a708f8fSGustavo F. Padovan static inline int l2cap_skbuff_fromiovec(struct sock *sk, struct msghdr *msg, int len, int count, struct sk_buff *skb) 11810a708f8fSGustavo F. Padovan { 11820a708f8fSGustavo F. Padovan struct l2cap_conn *conn = l2cap_pi(sk)->conn; 11830a708f8fSGustavo F. Padovan struct sk_buff **frag; 11840a708f8fSGustavo F. Padovan int err, sent = 0; 11850a708f8fSGustavo F. Padovan 11860a708f8fSGustavo F. Padovan if (memcpy_fromiovec(skb_put(skb, count), msg->msg_iov, count)) 11870a708f8fSGustavo F. Padovan return -EFAULT; 11880a708f8fSGustavo F. Padovan 11890a708f8fSGustavo F. Padovan sent += count; 11900a708f8fSGustavo F. Padovan len -= count; 11910a708f8fSGustavo F. Padovan 11920a708f8fSGustavo F. Padovan /* Continuation fragments (no L2CAP header) */ 11930a708f8fSGustavo F. Padovan frag = &skb_shinfo(skb)->frag_list; 11940a708f8fSGustavo F. Padovan while (len) { 11950a708f8fSGustavo F. Padovan count = min_t(unsigned int, conn->mtu, len); 11960a708f8fSGustavo F. Padovan 11970a708f8fSGustavo F. Padovan *frag = bt_skb_send_alloc(sk, count, msg->msg_flags & MSG_DONTWAIT, &err); 11980a708f8fSGustavo F. Padovan if (!*frag) 11990a708f8fSGustavo F. Padovan return err; 12000a708f8fSGustavo F. Padovan if (memcpy_fromiovec(skb_put(*frag, count), msg->msg_iov, count)) 12010a708f8fSGustavo F. Padovan return -EFAULT; 12020a708f8fSGustavo F. Padovan 12030a708f8fSGustavo F. Padovan sent += count; 12040a708f8fSGustavo F. Padovan len -= count; 12050a708f8fSGustavo F. Padovan 12060a708f8fSGustavo F. Padovan frag = &(*frag)->next; 12070a708f8fSGustavo F. Padovan } 12080a708f8fSGustavo F. Padovan 12090a708f8fSGustavo F. Padovan return sent; 12100a708f8fSGustavo F. Padovan } 12110a708f8fSGustavo F. Padovan 1212fd83ccdbSGustavo F. Padovan struct sk_buff *l2cap_create_connless_pdu(struct sock *sk, struct msghdr *msg, size_t len) 12130a708f8fSGustavo F. Padovan { 12140a708f8fSGustavo F. Padovan struct l2cap_conn *conn = l2cap_pi(sk)->conn; 12150a708f8fSGustavo F. Padovan struct sk_buff *skb; 12160a708f8fSGustavo F. Padovan int err, count, hlen = L2CAP_HDR_SIZE + 2; 12170a708f8fSGustavo F. Padovan struct l2cap_hdr *lh; 12180a708f8fSGustavo F. Padovan 12190a708f8fSGustavo F. Padovan BT_DBG("sk %p len %d", sk, (int)len); 12200a708f8fSGustavo F. Padovan 12210a708f8fSGustavo F. Padovan count = min_t(unsigned int, (conn->mtu - hlen), len); 12220a708f8fSGustavo F. Padovan skb = bt_skb_send_alloc(sk, count + hlen, 12230a708f8fSGustavo F. Padovan msg->msg_flags & MSG_DONTWAIT, &err); 12240a708f8fSGustavo F. Padovan if (!skb) 12250a708f8fSGustavo F. Padovan return ERR_PTR(err); 12260a708f8fSGustavo F. Padovan 12270a708f8fSGustavo F. Padovan /* Create L2CAP header */ 12280a708f8fSGustavo F. Padovan lh = (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE); 12290a708f8fSGustavo F. Padovan lh->cid = cpu_to_le16(l2cap_pi(sk)->dcid); 12300a708f8fSGustavo F. Padovan lh->len = cpu_to_le16(len + (hlen - L2CAP_HDR_SIZE)); 12310a708f8fSGustavo F. Padovan put_unaligned_le16(l2cap_pi(sk)->psm, skb_put(skb, 2)); 12320a708f8fSGustavo F. Padovan 12330a708f8fSGustavo F. Padovan err = l2cap_skbuff_fromiovec(sk, msg, len, count, skb); 12340a708f8fSGustavo F. Padovan if (unlikely(err < 0)) { 12350a708f8fSGustavo F. Padovan kfree_skb(skb); 12360a708f8fSGustavo F. Padovan return ERR_PTR(err); 12370a708f8fSGustavo F. Padovan } 12380a708f8fSGustavo F. Padovan return skb; 12390a708f8fSGustavo F. Padovan } 12400a708f8fSGustavo F. Padovan 1241fd83ccdbSGustavo F. Padovan struct sk_buff *l2cap_create_basic_pdu(struct sock *sk, struct msghdr *msg, size_t len) 12420a708f8fSGustavo F. Padovan { 12430a708f8fSGustavo F. Padovan struct l2cap_conn *conn = l2cap_pi(sk)->conn; 12440a708f8fSGustavo F. Padovan struct sk_buff *skb; 12450a708f8fSGustavo F. Padovan int err, count, hlen = L2CAP_HDR_SIZE; 12460a708f8fSGustavo F. Padovan struct l2cap_hdr *lh; 12470a708f8fSGustavo F. Padovan 12480a708f8fSGustavo F. Padovan BT_DBG("sk %p len %d", sk, (int)len); 12490a708f8fSGustavo F. Padovan 12500a708f8fSGustavo F. Padovan count = min_t(unsigned int, (conn->mtu - hlen), len); 12510a708f8fSGustavo F. Padovan skb = bt_skb_send_alloc(sk, count + hlen, 12520a708f8fSGustavo F. Padovan msg->msg_flags & MSG_DONTWAIT, &err); 12530a708f8fSGustavo F. Padovan if (!skb) 12540a708f8fSGustavo F. Padovan return ERR_PTR(err); 12550a708f8fSGustavo F. Padovan 12560a708f8fSGustavo F. Padovan /* Create L2CAP header */ 12570a708f8fSGustavo F. Padovan lh = (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE); 12580a708f8fSGustavo F. Padovan lh->cid = cpu_to_le16(l2cap_pi(sk)->dcid); 12590a708f8fSGustavo F. Padovan lh->len = cpu_to_le16(len + (hlen - L2CAP_HDR_SIZE)); 12600a708f8fSGustavo F. Padovan 12610a708f8fSGustavo F. Padovan err = l2cap_skbuff_fromiovec(sk, msg, len, count, skb); 12620a708f8fSGustavo F. Padovan if (unlikely(err < 0)) { 12630a708f8fSGustavo F. Padovan kfree_skb(skb); 12640a708f8fSGustavo F. Padovan return ERR_PTR(err); 12650a708f8fSGustavo F. Padovan } 12660a708f8fSGustavo F. Padovan return skb; 12670a708f8fSGustavo F. Padovan } 12680a708f8fSGustavo F. Padovan 1269fd83ccdbSGustavo F. Padovan struct sk_buff *l2cap_create_iframe_pdu(struct sock *sk, struct msghdr *msg, size_t len, u16 control, u16 sdulen) 12700a708f8fSGustavo F. Padovan { 12710a708f8fSGustavo F. Padovan struct l2cap_conn *conn = l2cap_pi(sk)->conn; 12720a708f8fSGustavo F. Padovan struct sk_buff *skb; 12730a708f8fSGustavo F. Padovan int err, count, hlen = L2CAP_HDR_SIZE + 2; 12740a708f8fSGustavo F. Padovan struct l2cap_hdr *lh; 12750a708f8fSGustavo F. Padovan 12760a708f8fSGustavo F. Padovan BT_DBG("sk %p len %d", sk, (int)len); 12770a708f8fSGustavo F. Padovan 12780a708f8fSGustavo F. Padovan if (!conn) 12790a708f8fSGustavo F. Padovan return ERR_PTR(-ENOTCONN); 12800a708f8fSGustavo F. Padovan 12810a708f8fSGustavo F. Padovan if (sdulen) 12820a708f8fSGustavo F. Padovan hlen += 2; 12830a708f8fSGustavo F. Padovan 12840a708f8fSGustavo F. Padovan if (l2cap_pi(sk)->fcs == L2CAP_FCS_CRC16) 12850a708f8fSGustavo F. Padovan hlen += 2; 12860a708f8fSGustavo F. Padovan 12870a708f8fSGustavo F. Padovan count = min_t(unsigned int, (conn->mtu - hlen), len); 12880a708f8fSGustavo F. Padovan skb = bt_skb_send_alloc(sk, count + hlen, 12890a708f8fSGustavo F. Padovan msg->msg_flags & MSG_DONTWAIT, &err); 12900a708f8fSGustavo F. Padovan if (!skb) 12910a708f8fSGustavo F. Padovan return ERR_PTR(err); 12920a708f8fSGustavo F. Padovan 12930a708f8fSGustavo F. Padovan /* Create L2CAP header */ 12940a708f8fSGustavo F. Padovan lh = (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE); 12950a708f8fSGustavo F. Padovan lh->cid = cpu_to_le16(l2cap_pi(sk)->dcid); 12960a708f8fSGustavo F. Padovan lh->len = cpu_to_le16(len + (hlen - L2CAP_HDR_SIZE)); 12970a708f8fSGustavo F. Padovan put_unaligned_le16(control, skb_put(skb, 2)); 12980a708f8fSGustavo F. Padovan if (sdulen) 12990a708f8fSGustavo F. Padovan put_unaligned_le16(sdulen, skb_put(skb, 2)); 13000a708f8fSGustavo F. Padovan 13010a708f8fSGustavo F. Padovan err = l2cap_skbuff_fromiovec(sk, msg, len, count, skb); 13020a708f8fSGustavo F. Padovan if (unlikely(err < 0)) { 13030a708f8fSGustavo F. Padovan kfree_skb(skb); 13040a708f8fSGustavo F. Padovan return ERR_PTR(err); 13050a708f8fSGustavo F. Padovan } 13060a708f8fSGustavo F. Padovan 13070a708f8fSGustavo F. Padovan if (l2cap_pi(sk)->fcs == L2CAP_FCS_CRC16) 13080a708f8fSGustavo F. Padovan put_unaligned_le16(0, skb_put(skb, 2)); 13090a708f8fSGustavo F. Padovan 13100a708f8fSGustavo F. Padovan bt_cb(skb)->retries = 0; 13110a708f8fSGustavo F. Padovan return skb; 13120a708f8fSGustavo F. Padovan } 13130a708f8fSGustavo F. Padovan 1314fd83ccdbSGustavo F. Padovan int l2cap_sar_segment_sdu(struct sock *sk, struct msghdr *msg, size_t len) 13150a708f8fSGustavo F. Padovan { 13160a708f8fSGustavo F. Padovan struct l2cap_pinfo *pi = l2cap_pi(sk); 13170a708f8fSGustavo F. Padovan struct sk_buff *skb; 13180a708f8fSGustavo F. Padovan struct sk_buff_head sar_queue; 13190a708f8fSGustavo F. Padovan u16 control; 13200a708f8fSGustavo F. Padovan size_t size = 0; 13210a708f8fSGustavo F. Padovan 13220a708f8fSGustavo F. Padovan skb_queue_head_init(&sar_queue); 13230a708f8fSGustavo F. Padovan control = L2CAP_SDU_START; 13240a708f8fSGustavo F. Padovan skb = l2cap_create_iframe_pdu(sk, msg, pi->remote_mps, control, len); 13250a708f8fSGustavo F. Padovan if (IS_ERR(skb)) 13260a708f8fSGustavo F. Padovan return PTR_ERR(skb); 13270a708f8fSGustavo F. Padovan 13280a708f8fSGustavo F. Padovan __skb_queue_tail(&sar_queue, skb); 13290a708f8fSGustavo F. Padovan len -= pi->remote_mps; 13300a708f8fSGustavo F. Padovan size += pi->remote_mps; 13310a708f8fSGustavo F. Padovan 13320a708f8fSGustavo F. Padovan while (len > 0) { 13330a708f8fSGustavo F. Padovan size_t buflen; 13340a708f8fSGustavo F. Padovan 13350a708f8fSGustavo F. Padovan if (len > pi->remote_mps) { 13360a708f8fSGustavo F. Padovan control = L2CAP_SDU_CONTINUE; 13370a708f8fSGustavo F. Padovan buflen = pi->remote_mps; 13380a708f8fSGustavo F. Padovan } else { 13390a708f8fSGustavo F. Padovan control = L2CAP_SDU_END; 13400a708f8fSGustavo F. Padovan buflen = len; 13410a708f8fSGustavo F. Padovan } 13420a708f8fSGustavo F. Padovan 13430a708f8fSGustavo F. Padovan skb = l2cap_create_iframe_pdu(sk, msg, buflen, control, 0); 13440a708f8fSGustavo F. Padovan if (IS_ERR(skb)) { 13450a708f8fSGustavo F. Padovan skb_queue_purge(&sar_queue); 13460a708f8fSGustavo F. Padovan return PTR_ERR(skb); 13470a708f8fSGustavo F. Padovan } 13480a708f8fSGustavo F. Padovan 13490a708f8fSGustavo F. Padovan __skb_queue_tail(&sar_queue, skb); 13500a708f8fSGustavo F. Padovan len -= buflen; 13510a708f8fSGustavo F. Padovan size += buflen; 13520a708f8fSGustavo F. Padovan } 13530a708f8fSGustavo F. Padovan skb_queue_splice_tail(&sar_queue, TX_QUEUE(sk)); 13540a708f8fSGustavo F. Padovan if (sk->sk_send_head == NULL) 13550a708f8fSGustavo F. Padovan sk->sk_send_head = sar_queue.next; 13560a708f8fSGustavo F. Padovan 13570a708f8fSGustavo F. Padovan return size; 13580a708f8fSGustavo F. Padovan } 13590a708f8fSGustavo F. Padovan 13600a708f8fSGustavo F. Padovan static void l2cap_chan_ready(struct sock *sk) 13610a708f8fSGustavo F. Padovan { 13620a708f8fSGustavo F. Padovan struct sock *parent = bt_sk(sk)->parent; 13630a708f8fSGustavo F. Padovan 13640a708f8fSGustavo F. Padovan BT_DBG("sk %p, parent %p", sk, parent); 13650a708f8fSGustavo F. Padovan 13660a708f8fSGustavo F. Padovan l2cap_pi(sk)->conf_state = 0; 13670a708f8fSGustavo F. Padovan l2cap_sock_clear_timer(sk); 13680a708f8fSGustavo F. Padovan 13690a708f8fSGustavo F. Padovan if (!parent) { 13700a708f8fSGustavo F. Padovan /* Outgoing channel. 13710a708f8fSGustavo F. Padovan * Wake up socket sleeping on connect. 13720a708f8fSGustavo F. Padovan */ 13730a708f8fSGustavo F. Padovan sk->sk_state = BT_CONNECTED; 13740a708f8fSGustavo F. Padovan sk->sk_state_change(sk); 13750a708f8fSGustavo F. Padovan } else { 13760a708f8fSGustavo F. Padovan /* Incoming channel. 13770a708f8fSGustavo F. Padovan * Wake up socket sleeping on accept. 13780a708f8fSGustavo F. Padovan */ 13790a708f8fSGustavo F. Padovan parent->sk_data_ready(parent, 0); 13800a708f8fSGustavo F. Padovan } 13810a708f8fSGustavo F. Padovan } 13820a708f8fSGustavo F. Padovan 13830a708f8fSGustavo F. Padovan /* Copy frame to all raw sockets on that connection */ 13840a708f8fSGustavo F. Padovan static void l2cap_raw_recv(struct l2cap_conn *conn, struct sk_buff *skb) 13850a708f8fSGustavo F. Padovan { 13860a708f8fSGustavo F. Padovan struct l2cap_chan_list *l = &conn->chan_list; 13870a708f8fSGustavo F. Padovan struct sk_buff *nskb; 13880a708f8fSGustavo F. Padovan struct sock *sk; 13890a708f8fSGustavo F. Padovan 13900a708f8fSGustavo F. Padovan BT_DBG("conn %p", conn); 13910a708f8fSGustavo F. Padovan 13920a708f8fSGustavo F. Padovan read_lock(&l->lock); 13930a708f8fSGustavo F. Padovan for (sk = l->head; sk; sk = l2cap_pi(sk)->next_c) { 13940a708f8fSGustavo F. Padovan if (sk->sk_type != SOCK_RAW) 13950a708f8fSGustavo F. Padovan continue; 13960a708f8fSGustavo F. Padovan 13970a708f8fSGustavo F. Padovan /* Don't send frame to the socket it came from */ 13980a708f8fSGustavo F. Padovan if (skb->sk == sk) 13990a708f8fSGustavo F. Padovan continue; 14000a708f8fSGustavo F. Padovan nskb = skb_clone(skb, GFP_ATOMIC); 14010a708f8fSGustavo F. Padovan if (!nskb) 14020a708f8fSGustavo F. Padovan continue; 14030a708f8fSGustavo F. Padovan 14040a708f8fSGustavo F. Padovan if (sock_queue_rcv_skb(sk, nskb)) 14050a708f8fSGustavo F. Padovan kfree_skb(nskb); 14060a708f8fSGustavo F. Padovan } 14070a708f8fSGustavo F. Padovan read_unlock(&l->lock); 14080a708f8fSGustavo F. Padovan } 14090a708f8fSGustavo F. Padovan 14100a708f8fSGustavo F. Padovan /* ---- L2CAP signalling commands ---- */ 14110a708f8fSGustavo F. Padovan static struct sk_buff *l2cap_build_cmd(struct l2cap_conn *conn, 14120a708f8fSGustavo F. Padovan u8 code, u8 ident, u16 dlen, void *data) 14130a708f8fSGustavo F. Padovan { 14140a708f8fSGustavo F. Padovan struct sk_buff *skb, **frag; 14150a708f8fSGustavo F. Padovan struct l2cap_cmd_hdr *cmd; 14160a708f8fSGustavo F. Padovan struct l2cap_hdr *lh; 14170a708f8fSGustavo F. Padovan int len, count; 14180a708f8fSGustavo F. Padovan 14190a708f8fSGustavo F. Padovan BT_DBG("conn %p, code 0x%2.2x, ident 0x%2.2x, len %d", 14200a708f8fSGustavo F. Padovan conn, code, ident, dlen); 14210a708f8fSGustavo F. Padovan 14220a708f8fSGustavo F. Padovan len = L2CAP_HDR_SIZE + L2CAP_CMD_HDR_SIZE + dlen; 14230a708f8fSGustavo F. Padovan count = min_t(unsigned int, conn->mtu, len); 14240a708f8fSGustavo F. Padovan 14250a708f8fSGustavo F. Padovan skb = bt_skb_alloc(count, GFP_ATOMIC); 14260a708f8fSGustavo F. Padovan if (!skb) 14270a708f8fSGustavo F. Padovan return NULL; 14280a708f8fSGustavo F. Padovan 14290a708f8fSGustavo F. Padovan lh = (struct l2cap_hdr *) skb_put(skb, L2CAP_HDR_SIZE); 14300a708f8fSGustavo F. Padovan lh->len = cpu_to_le16(L2CAP_CMD_HDR_SIZE + dlen); 1431*3300d9a9SClaudio Takahasi 1432*3300d9a9SClaudio Takahasi if (conn->hcon->type == LE_LINK) 1433*3300d9a9SClaudio Takahasi lh->cid = cpu_to_le16(L2CAP_CID_LE_SIGNALING); 1434*3300d9a9SClaudio Takahasi else 14350a708f8fSGustavo F. Padovan lh->cid = cpu_to_le16(L2CAP_CID_SIGNALING); 14360a708f8fSGustavo F. Padovan 14370a708f8fSGustavo F. Padovan cmd = (struct l2cap_cmd_hdr *) skb_put(skb, L2CAP_CMD_HDR_SIZE); 14380a708f8fSGustavo F. Padovan cmd->code = code; 14390a708f8fSGustavo F. Padovan cmd->ident = ident; 14400a708f8fSGustavo F. Padovan cmd->len = cpu_to_le16(dlen); 14410a708f8fSGustavo F. Padovan 14420a708f8fSGustavo F. Padovan if (dlen) { 14430a708f8fSGustavo F. Padovan count -= L2CAP_HDR_SIZE + L2CAP_CMD_HDR_SIZE; 14440a708f8fSGustavo F. Padovan memcpy(skb_put(skb, count), data, count); 14450a708f8fSGustavo F. Padovan data += count; 14460a708f8fSGustavo F. Padovan } 14470a708f8fSGustavo F. Padovan 14480a708f8fSGustavo F. Padovan len -= skb->len; 14490a708f8fSGustavo F. Padovan 14500a708f8fSGustavo F. Padovan /* Continuation fragments (no L2CAP header) */ 14510a708f8fSGustavo F. Padovan frag = &skb_shinfo(skb)->frag_list; 14520a708f8fSGustavo F. Padovan while (len) { 14530a708f8fSGustavo F. Padovan count = min_t(unsigned int, conn->mtu, len); 14540a708f8fSGustavo F. Padovan 14550a708f8fSGustavo F. Padovan *frag = bt_skb_alloc(count, GFP_ATOMIC); 14560a708f8fSGustavo F. Padovan if (!*frag) 14570a708f8fSGustavo F. Padovan goto fail; 14580a708f8fSGustavo F. Padovan 14590a708f8fSGustavo F. Padovan memcpy(skb_put(*frag, count), data, count); 14600a708f8fSGustavo F. Padovan 14610a708f8fSGustavo F. Padovan len -= count; 14620a708f8fSGustavo F. Padovan data += count; 14630a708f8fSGustavo F. Padovan 14640a708f8fSGustavo F. Padovan frag = &(*frag)->next; 14650a708f8fSGustavo F. Padovan } 14660a708f8fSGustavo F. Padovan 14670a708f8fSGustavo F. Padovan return skb; 14680a708f8fSGustavo F. Padovan 14690a708f8fSGustavo F. Padovan fail: 14700a708f8fSGustavo F. Padovan kfree_skb(skb); 14710a708f8fSGustavo F. Padovan return NULL; 14720a708f8fSGustavo F. Padovan } 14730a708f8fSGustavo F. Padovan 14740a708f8fSGustavo F. Padovan static inline int l2cap_get_conf_opt(void **ptr, int *type, int *olen, unsigned long *val) 14750a708f8fSGustavo F. Padovan { 14760a708f8fSGustavo F. Padovan struct l2cap_conf_opt *opt = *ptr; 14770a708f8fSGustavo F. Padovan int len; 14780a708f8fSGustavo F. Padovan 14790a708f8fSGustavo F. Padovan len = L2CAP_CONF_OPT_SIZE + opt->len; 14800a708f8fSGustavo F. Padovan *ptr += len; 14810a708f8fSGustavo F. Padovan 14820a708f8fSGustavo F. Padovan *type = opt->type; 14830a708f8fSGustavo F. Padovan *olen = opt->len; 14840a708f8fSGustavo F. Padovan 14850a708f8fSGustavo F. Padovan switch (opt->len) { 14860a708f8fSGustavo F. Padovan case 1: 14870a708f8fSGustavo F. Padovan *val = *((u8 *) opt->val); 14880a708f8fSGustavo F. Padovan break; 14890a708f8fSGustavo F. Padovan 14900a708f8fSGustavo F. Padovan case 2: 14910a708f8fSGustavo F. Padovan *val = get_unaligned_le16(opt->val); 14920a708f8fSGustavo F. Padovan break; 14930a708f8fSGustavo F. Padovan 14940a708f8fSGustavo F. Padovan case 4: 14950a708f8fSGustavo F. Padovan *val = get_unaligned_le32(opt->val); 14960a708f8fSGustavo F. Padovan break; 14970a708f8fSGustavo F. Padovan 14980a708f8fSGustavo F. Padovan default: 14990a708f8fSGustavo F. Padovan *val = (unsigned long) opt->val; 15000a708f8fSGustavo F. Padovan break; 15010a708f8fSGustavo F. Padovan } 15020a708f8fSGustavo F. Padovan 15030a708f8fSGustavo F. Padovan BT_DBG("type 0x%2.2x len %d val 0x%lx", *type, opt->len, *val); 15040a708f8fSGustavo F. Padovan return len; 15050a708f8fSGustavo F. Padovan } 15060a708f8fSGustavo F. Padovan 15070a708f8fSGustavo F. Padovan static void l2cap_add_conf_opt(void **ptr, u8 type, u8 len, unsigned long val) 15080a708f8fSGustavo F. Padovan { 15090a708f8fSGustavo F. Padovan struct l2cap_conf_opt *opt = *ptr; 15100a708f8fSGustavo F. Padovan 15110a708f8fSGustavo F. Padovan BT_DBG("type 0x%2.2x len %d val 0x%lx", type, len, val); 15120a708f8fSGustavo F. Padovan 15130a708f8fSGustavo F. Padovan opt->type = type; 15140a708f8fSGustavo F. Padovan opt->len = len; 15150a708f8fSGustavo F. Padovan 15160a708f8fSGustavo F. Padovan switch (len) { 15170a708f8fSGustavo F. Padovan case 1: 15180a708f8fSGustavo F. Padovan *((u8 *) opt->val) = val; 15190a708f8fSGustavo F. Padovan break; 15200a708f8fSGustavo F. Padovan 15210a708f8fSGustavo F. Padovan case 2: 15220a708f8fSGustavo F. Padovan put_unaligned_le16(val, opt->val); 15230a708f8fSGustavo F. Padovan break; 15240a708f8fSGustavo F. Padovan 15250a708f8fSGustavo F. Padovan case 4: 15260a708f8fSGustavo F. Padovan put_unaligned_le32(val, opt->val); 15270a708f8fSGustavo F. Padovan break; 15280a708f8fSGustavo F. Padovan 15290a708f8fSGustavo F. Padovan default: 15300a708f8fSGustavo F. Padovan memcpy(opt->val, (void *) val, len); 15310a708f8fSGustavo F. Padovan break; 15320a708f8fSGustavo F. Padovan } 15330a708f8fSGustavo F. Padovan 15340a708f8fSGustavo F. Padovan *ptr += L2CAP_CONF_OPT_SIZE + len; 15350a708f8fSGustavo F. Padovan } 15360a708f8fSGustavo F. Padovan 15370a708f8fSGustavo F. Padovan static void l2cap_ack_timeout(unsigned long arg) 15380a708f8fSGustavo F. Padovan { 15390a708f8fSGustavo F. Padovan struct sock *sk = (void *) arg; 15400a708f8fSGustavo F. Padovan 15410a708f8fSGustavo F. Padovan bh_lock_sock(sk); 15420a708f8fSGustavo F. Padovan l2cap_send_ack(l2cap_pi(sk)); 15430a708f8fSGustavo F. Padovan bh_unlock_sock(sk); 15440a708f8fSGustavo F. Padovan } 15450a708f8fSGustavo F. Padovan 15460a708f8fSGustavo F. Padovan static inline void l2cap_ertm_init(struct sock *sk) 15470a708f8fSGustavo F. Padovan { 15480a708f8fSGustavo F. Padovan l2cap_pi(sk)->expected_ack_seq = 0; 15490a708f8fSGustavo F. Padovan l2cap_pi(sk)->unacked_frames = 0; 15500a708f8fSGustavo F. Padovan l2cap_pi(sk)->buffer_seq = 0; 15510a708f8fSGustavo F. Padovan l2cap_pi(sk)->num_acked = 0; 15520a708f8fSGustavo F. Padovan l2cap_pi(sk)->frames_sent = 0; 15530a708f8fSGustavo F. Padovan 15540a708f8fSGustavo F. Padovan setup_timer(&l2cap_pi(sk)->retrans_timer, 15550a708f8fSGustavo F. Padovan l2cap_retrans_timeout, (unsigned long) sk); 15560a708f8fSGustavo F. Padovan setup_timer(&l2cap_pi(sk)->monitor_timer, 15570a708f8fSGustavo F. Padovan l2cap_monitor_timeout, (unsigned long) sk); 15580a708f8fSGustavo F. Padovan setup_timer(&l2cap_pi(sk)->ack_timer, 15590a708f8fSGustavo F. Padovan l2cap_ack_timeout, (unsigned long) sk); 15600a708f8fSGustavo F. Padovan 15610a708f8fSGustavo F. Padovan __skb_queue_head_init(SREJ_QUEUE(sk)); 15620a708f8fSGustavo F. Padovan __skb_queue_head_init(BUSY_QUEUE(sk)); 15630a708f8fSGustavo F. Padovan 15640a708f8fSGustavo F. Padovan INIT_WORK(&l2cap_pi(sk)->busy_work, l2cap_busy_work); 15650a708f8fSGustavo F. Padovan 15660a708f8fSGustavo F. Padovan sk->sk_backlog_rcv = l2cap_ertm_data_rcv; 15670a708f8fSGustavo F. Padovan } 15680a708f8fSGustavo F. Padovan 15690a708f8fSGustavo F. Padovan static inline __u8 l2cap_select_mode(__u8 mode, __u16 remote_feat_mask) 15700a708f8fSGustavo F. Padovan { 15710a708f8fSGustavo F. Padovan switch (mode) { 15720a708f8fSGustavo F. Padovan case L2CAP_MODE_STREAMING: 15730a708f8fSGustavo F. Padovan case L2CAP_MODE_ERTM: 15740a708f8fSGustavo F. Padovan if (l2cap_mode_supported(mode, remote_feat_mask)) 15750a708f8fSGustavo F. Padovan return mode; 15760a708f8fSGustavo F. Padovan /* fall through */ 15770a708f8fSGustavo F. Padovan default: 15780a708f8fSGustavo F. Padovan return L2CAP_MODE_BASIC; 15790a708f8fSGustavo F. Padovan } 15800a708f8fSGustavo F. Padovan } 15810a708f8fSGustavo F. Padovan 158268983259SGustavo F. Padovan int l2cap_build_conf_req(struct sock *sk, void *data) 15830a708f8fSGustavo F. Padovan { 15840a708f8fSGustavo F. Padovan struct l2cap_pinfo *pi = l2cap_pi(sk); 15850a708f8fSGustavo F. Padovan struct l2cap_conf_req *req = data; 15860a708f8fSGustavo F. Padovan struct l2cap_conf_rfc rfc = { .mode = pi->mode }; 15870a708f8fSGustavo F. Padovan void *ptr = req->data; 15880a708f8fSGustavo F. Padovan 15890a708f8fSGustavo F. Padovan BT_DBG("sk %p", sk); 15900a708f8fSGustavo F. Padovan 15910a708f8fSGustavo F. Padovan if (pi->num_conf_req || pi->num_conf_rsp) 15920a708f8fSGustavo F. Padovan goto done; 15930a708f8fSGustavo F. Padovan 15940a708f8fSGustavo F. Padovan switch (pi->mode) { 15950a708f8fSGustavo F. Padovan case L2CAP_MODE_STREAMING: 15960a708f8fSGustavo F. Padovan case L2CAP_MODE_ERTM: 15970a708f8fSGustavo F. Padovan if (pi->conf_state & L2CAP_CONF_STATE2_DEVICE) 15980a708f8fSGustavo F. Padovan break; 15990a708f8fSGustavo F. Padovan 16000a708f8fSGustavo F. Padovan /* fall through */ 16010a708f8fSGustavo F. Padovan default: 16020a708f8fSGustavo F. Padovan pi->mode = l2cap_select_mode(rfc.mode, pi->conn->feat_mask); 16030a708f8fSGustavo F. Padovan break; 16040a708f8fSGustavo F. Padovan } 16050a708f8fSGustavo F. Padovan 16060a708f8fSGustavo F. Padovan done: 16070a708f8fSGustavo F. Padovan if (pi->imtu != L2CAP_DEFAULT_MTU) 16080a708f8fSGustavo F. Padovan l2cap_add_conf_opt(&ptr, L2CAP_CONF_MTU, 2, pi->imtu); 16090a708f8fSGustavo F. Padovan 16100a708f8fSGustavo F. Padovan switch (pi->mode) { 16110a708f8fSGustavo F. Padovan case L2CAP_MODE_BASIC: 16120a708f8fSGustavo F. Padovan if (!(pi->conn->feat_mask & L2CAP_FEAT_ERTM) && 16130a708f8fSGustavo F. Padovan !(pi->conn->feat_mask & L2CAP_FEAT_STREAMING)) 16140a708f8fSGustavo F. Padovan break; 16150a708f8fSGustavo F. Padovan 16160a708f8fSGustavo F. Padovan rfc.mode = L2CAP_MODE_BASIC; 16170a708f8fSGustavo F. Padovan rfc.txwin_size = 0; 16180a708f8fSGustavo F. Padovan rfc.max_transmit = 0; 16190a708f8fSGustavo F. Padovan rfc.retrans_timeout = 0; 16200a708f8fSGustavo F. Padovan rfc.monitor_timeout = 0; 16210a708f8fSGustavo F. Padovan rfc.max_pdu_size = 0; 16220a708f8fSGustavo F. Padovan 16230a708f8fSGustavo F. Padovan l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC, sizeof(rfc), 16240a708f8fSGustavo F. Padovan (unsigned long) &rfc); 16250a708f8fSGustavo F. Padovan break; 16260a708f8fSGustavo F. Padovan 16270a708f8fSGustavo F. Padovan case L2CAP_MODE_ERTM: 16280a708f8fSGustavo F. Padovan rfc.mode = L2CAP_MODE_ERTM; 16290a708f8fSGustavo F. Padovan rfc.txwin_size = pi->tx_win; 16300a708f8fSGustavo F. Padovan rfc.max_transmit = pi->max_tx; 16310a708f8fSGustavo F. Padovan rfc.retrans_timeout = 0; 16320a708f8fSGustavo F. Padovan rfc.monitor_timeout = 0; 16330a708f8fSGustavo F. Padovan rfc.max_pdu_size = cpu_to_le16(L2CAP_DEFAULT_MAX_PDU_SIZE); 16340a708f8fSGustavo F. Padovan if (L2CAP_DEFAULT_MAX_PDU_SIZE > pi->conn->mtu - 10) 16350a708f8fSGustavo F. Padovan rfc.max_pdu_size = cpu_to_le16(pi->conn->mtu - 10); 16360a708f8fSGustavo F. Padovan 16370a708f8fSGustavo F. Padovan l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC, sizeof(rfc), 16380a708f8fSGustavo F. Padovan (unsigned long) &rfc); 16390a708f8fSGustavo F. Padovan 16400a708f8fSGustavo F. Padovan if (!(pi->conn->feat_mask & L2CAP_FEAT_FCS)) 16410a708f8fSGustavo F. Padovan break; 16420a708f8fSGustavo F. Padovan 16430a708f8fSGustavo F. Padovan if (pi->fcs == L2CAP_FCS_NONE || 16440a708f8fSGustavo F. Padovan pi->conf_state & L2CAP_CONF_NO_FCS_RECV) { 16450a708f8fSGustavo F. Padovan pi->fcs = L2CAP_FCS_NONE; 16460a708f8fSGustavo F. Padovan l2cap_add_conf_opt(&ptr, L2CAP_CONF_FCS, 1, pi->fcs); 16470a708f8fSGustavo F. Padovan } 16480a708f8fSGustavo F. Padovan break; 16490a708f8fSGustavo F. Padovan 16500a708f8fSGustavo F. Padovan case L2CAP_MODE_STREAMING: 16510a708f8fSGustavo F. Padovan rfc.mode = L2CAP_MODE_STREAMING; 16520a708f8fSGustavo F. Padovan rfc.txwin_size = 0; 16530a708f8fSGustavo F. Padovan rfc.max_transmit = 0; 16540a708f8fSGustavo F. Padovan rfc.retrans_timeout = 0; 16550a708f8fSGustavo F. Padovan rfc.monitor_timeout = 0; 16560a708f8fSGustavo F. Padovan rfc.max_pdu_size = cpu_to_le16(L2CAP_DEFAULT_MAX_PDU_SIZE); 16570a708f8fSGustavo F. Padovan if (L2CAP_DEFAULT_MAX_PDU_SIZE > pi->conn->mtu - 10) 16580a708f8fSGustavo F. Padovan rfc.max_pdu_size = cpu_to_le16(pi->conn->mtu - 10); 16590a708f8fSGustavo F. Padovan 16600a708f8fSGustavo F. Padovan l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC, sizeof(rfc), 16610a708f8fSGustavo F. Padovan (unsigned long) &rfc); 16620a708f8fSGustavo F. Padovan 16630a708f8fSGustavo F. Padovan if (!(pi->conn->feat_mask & L2CAP_FEAT_FCS)) 16640a708f8fSGustavo F. Padovan break; 16650a708f8fSGustavo F. Padovan 16660a708f8fSGustavo F. Padovan if (pi->fcs == L2CAP_FCS_NONE || 16670a708f8fSGustavo F. Padovan pi->conf_state & L2CAP_CONF_NO_FCS_RECV) { 16680a708f8fSGustavo F. Padovan pi->fcs = L2CAP_FCS_NONE; 16690a708f8fSGustavo F. Padovan l2cap_add_conf_opt(&ptr, L2CAP_CONF_FCS, 1, pi->fcs); 16700a708f8fSGustavo F. Padovan } 16710a708f8fSGustavo F. Padovan break; 16720a708f8fSGustavo F. Padovan } 16730a708f8fSGustavo F. Padovan 16740a708f8fSGustavo F. Padovan /* FIXME: Need actual value of the flush timeout */ 16750a708f8fSGustavo F. Padovan //if (flush_to != L2CAP_DEFAULT_FLUSH_TO) 16760a708f8fSGustavo F. Padovan // l2cap_add_conf_opt(&ptr, L2CAP_CONF_FLUSH_TO, 2, pi->flush_to); 16770a708f8fSGustavo F. Padovan 16780a708f8fSGustavo F. Padovan req->dcid = cpu_to_le16(pi->dcid); 16790a708f8fSGustavo F. Padovan req->flags = cpu_to_le16(0); 16800a708f8fSGustavo F. Padovan 16810a708f8fSGustavo F. Padovan return ptr - data; 16820a708f8fSGustavo F. Padovan } 16830a708f8fSGustavo F. Padovan 16840a708f8fSGustavo F. Padovan static int l2cap_parse_conf_req(struct sock *sk, void *data) 16850a708f8fSGustavo F. Padovan { 16860a708f8fSGustavo F. Padovan struct l2cap_pinfo *pi = l2cap_pi(sk); 16870a708f8fSGustavo F. Padovan struct l2cap_conf_rsp *rsp = data; 16880a708f8fSGustavo F. Padovan void *ptr = rsp->data; 16890a708f8fSGustavo F. Padovan void *req = pi->conf_req; 16900a708f8fSGustavo F. Padovan int len = pi->conf_len; 16910a708f8fSGustavo F. Padovan int type, hint, olen; 16920a708f8fSGustavo F. Padovan unsigned long val; 16930a708f8fSGustavo F. Padovan struct l2cap_conf_rfc rfc = { .mode = L2CAP_MODE_BASIC }; 16940a708f8fSGustavo F. Padovan u16 mtu = L2CAP_DEFAULT_MTU; 16950a708f8fSGustavo F. Padovan u16 result = L2CAP_CONF_SUCCESS; 16960a708f8fSGustavo F. Padovan 16970a708f8fSGustavo F. Padovan BT_DBG("sk %p", sk); 16980a708f8fSGustavo F. Padovan 16990a708f8fSGustavo F. Padovan while (len >= L2CAP_CONF_OPT_SIZE) { 17000a708f8fSGustavo F. Padovan len -= l2cap_get_conf_opt(&req, &type, &olen, &val); 17010a708f8fSGustavo F. Padovan 17020a708f8fSGustavo F. Padovan hint = type & L2CAP_CONF_HINT; 17030a708f8fSGustavo F. Padovan type &= L2CAP_CONF_MASK; 17040a708f8fSGustavo F. Padovan 17050a708f8fSGustavo F. Padovan switch (type) { 17060a708f8fSGustavo F. Padovan case L2CAP_CONF_MTU: 17070a708f8fSGustavo F. Padovan mtu = val; 17080a708f8fSGustavo F. Padovan break; 17090a708f8fSGustavo F. Padovan 17100a708f8fSGustavo F. Padovan case L2CAP_CONF_FLUSH_TO: 17110a708f8fSGustavo F. Padovan pi->flush_to = val; 17120a708f8fSGustavo F. Padovan break; 17130a708f8fSGustavo F. Padovan 17140a708f8fSGustavo F. Padovan case L2CAP_CONF_QOS: 17150a708f8fSGustavo F. Padovan break; 17160a708f8fSGustavo F. Padovan 17170a708f8fSGustavo F. Padovan case L2CAP_CONF_RFC: 17180a708f8fSGustavo F. Padovan if (olen == sizeof(rfc)) 17190a708f8fSGustavo F. Padovan memcpy(&rfc, (void *) val, olen); 17200a708f8fSGustavo F. Padovan break; 17210a708f8fSGustavo F. Padovan 17220a708f8fSGustavo F. Padovan case L2CAP_CONF_FCS: 17230a708f8fSGustavo F. Padovan if (val == L2CAP_FCS_NONE) 17240a708f8fSGustavo F. Padovan pi->conf_state |= L2CAP_CONF_NO_FCS_RECV; 17250a708f8fSGustavo F. Padovan 17260a708f8fSGustavo F. Padovan break; 17270a708f8fSGustavo F. Padovan 17280a708f8fSGustavo F. Padovan default: 17290a708f8fSGustavo F. Padovan if (hint) 17300a708f8fSGustavo F. Padovan break; 17310a708f8fSGustavo F. Padovan 17320a708f8fSGustavo F. Padovan result = L2CAP_CONF_UNKNOWN; 17330a708f8fSGustavo F. Padovan *((u8 *) ptr++) = type; 17340a708f8fSGustavo F. Padovan break; 17350a708f8fSGustavo F. Padovan } 17360a708f8fSGustavo F. Padovan } 17370a708f8fSGustavo F. Padovan 17380a708f8fSGustavo F. Padovan if (pi->num_conf_rsp || pi->num_conf_req > 1) 17390a708f8fSGustavo F. Padovan goto done; 17400a708f8fSGustavo F. Padovan 17410a708f8fSGustavo F. Padovan switch (pi->mode) { 17420a708f8fSGustavo F. Padovan case L2CAP_MODE_STREAMING: 17430a708f8fSGustavo F. Padovan case L2CAP_MODE_ERTM: 17440a708f8fSGustavo F. Padovan if (!(pi->conf_state & L2CAP_CONF_STATE2_DEVICE)) { 17450a708f8fSGustavo F. Padovan pi->mode = l2cap_select_mode(rfc.mode, 17460a708f8fSGustavo F. Padovan pi->conn->feat_mask); 17470a708f8fSGustavo F. Padovan break; 17480a708f8fSGustavo F. Padovan } 17490a708f8fSGustavo F. Padovan 17500a708f8fSGustavo F. Padovan if (pi->mode != rfc.mode) 17510a708f8fSGustavo F. Padovan return -ECONNREFUSED; 17520a708f8fSGustavo F. Padovan 17530a708f8fSGustavo F. Padovan break; 17540a708f8fSGustavo F. Padovan } 17550a708f8fSGustavo F. Padovan 17560a708f8fSGustavo F. Padovan done: 17570a708f8fSGustavo F. Padovan if (pi->mode != rfc.mode) { 17580a708f8fSGustavo F. Padovan result = L2CAP_CONF_UNACCEPT; 17590a708f8fSGustavo F. Padovan rfc.mode = pi->mode; 17600a708f8fSGustavo F. Padovan 17610a708f8fSGustavo F. Padovan if (pi->num_conf_rsp == 1) 17620a708f8fSGustavo F. Padovan return -ECONNREFUSED; 17630a708f8fSGustavo F. Padovan 17640a708f8fSGustavo F. Padovan l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC, 17650a708f8fSGustavo F. Padovan sizeof(rfc), (unsigned long) &rfc); 17660a708f8fSGustavo F. Padovan } 17670a708f8fSGustavo F. Padovan 17680a708f8fSGustavo F. Padovan 17690a708f8fSGustavo F. Padovan if (result == L2CAP_CONF_SUCCESS) { 17700a708f8fSGustavo F. Padovan /* Configure output options and let the other side know 17710a708f8fSGustavo F. Padovan * which ones we don't like. */ 17720a708f8fSGustavo F. Padovan 17730a708f8fSGustavo F. Padovan if (mtu < L2CAP_DEFAULT_MIN_MTU) 17740a708f8fSGustavo F. Padovan result = L2CAP_CONF_UNACCEPT; 17750a708f8fSGustavo F. Padovan else { 17760a708f8fSGustavo F. Padovan pi->omtu = mtu; 17770a708f8fSGustavo F. Padovan pi->conf_state |= L2CAP_CONF_MTU_DONE; 17780a708f8fSGustavo F. Padovan } 17790a708f8fSGustavo F. Padovan l2cap_add_conf_opt(&ptr, L2CAP_CONF_MTU, 2, pi->omtu); 17800a708f8fSGustavo F. Padovan 17810a708f8fSGustavo F. Padovan switch (rfc.mode) { 17820a708f8fSGustavo F. Padovan case L2CAP_MODE_BASIC: 17830a708f8fSGustavo F. Padovan pi->fcs = L2CAP_FCS_NONE; 17840a708f8fSGustavo F. Padovan pi->conf_state |= L2CAP_CONF_MODE_DONE; 17850a708f8fSGustavo F. Padovan break; 17860a708f8fSGustavo F. Padovan 17870a708f8fSGustavo F. Padovan case L2CAP_MODE_ERTM: 17880a708f8fSGustavo F. Padovan pi->remote_tx_win = rfc.txwin_size; 17890a708f8fSGustavo F. Padovan pi->remote_max_tx = rfc.max_transmit; 17900a708f8fSGustavo F. Padovan 17910a708f8fSGustavo F. Padovan if (le16_to_cpu(rfc.max_pdu_size) > pi->conn->mtu - 10) 17920a708f8fSGustavo F. Padovan rfc.max_pdu_size = cpu_to_le16(pi->conn->mtu - 10); 17930a708f8fSGustavo F. Padovan 17940a708f8fSGustavo F. Padovan pi->remote_mps = le16_to_cpu(rfc.max_pdu_size); 17950a708f8fSGustavo F. Padovan 17960a708f8fSGustavo F. Padovan rfc.retrans_timeout = 17970a708f8fSGustavo F. Padovan le16_to_cpu(L2CAP_DEFAULT_RETRANS_TO); 17980a708f8fSGustavo F. Padovan rfc.monitor_timeout = 17990a708f8fSGustavo F. Padovan le16_to_cpu(L2CAP_DEFAULT_MONITOR_TO); 18000a708f8fSGustavo F. Padovan 18010a708f8fSGustavo F. Padovan pi->conf_state |= L2CAP_CONF_MODE_DONE; 18020a708f8fSGustavo F. Padovan 18030a708f8fSGustavo F. Padovan l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC, 18040a708f8fSGustavo F. Padovan sizeof(rfc), (unsigned long) &rfc); 18050a708f8fSGustavo F. Padovan 18060a708f8fSGustavo F. Padovan break; 18070a708f8fSGustavo F. Padovan 18080a708f8fSGustavo F. Padovan case L2CAP_MODE_STREAMING: 18090a708f8fSGustavo F. Padovan if (le16_to_cpu(rfc.max_pdu_size) > pi->conn->mtu - 10) 18100a708f8fSGustavo F. Padovan rfc.max_pdu_size = cpu_to_le16(pi->conn->mtu - 10); 18110a708f8fSGustavo F. Padovan 18120a708f8fSGustavo F. Padovan pi->remote_mps = le16_to_cpu(rfc.max_pdu_size); 18130a708f8fSGustavo F. Padovan 18140a708f8fSGustavo F. Padovan pi->conf_state |= L2CAP_CONF_MODE_DONE; 18150a708f8fSGustavo F. Padovan 18160a708f8fSGustavo F. Padovan l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC, 18170a708f8fSGustavo F. Padovan sizeof(rfc), (unsigned long) &rfc); 18180a708f8fSGustavo F. Padovan 18190a708f8fSGustavo F. Padovan break; 18200a708f8fSGustavo F. Padovan 18210a708f8fSGustavo F. Padovan default: 18220a708f8fSGustavo F. Padovan result = L2CAP_CONF_UNACCEPT; 18230a708f8fSGustavo F. Padovan 18240a708f8fSGustavo F. Padovan memset(&rfc, 0, sizeof(rfc)); 18250a708f8fSGustavo F. Padovan rfc.mode = pi->mode; 18260a708f8fSGustavo F. Padovan } 18270a708f8fSGustavo F. Padovan 18280a708f8fSGustavo F. Padovan if (result == L2CAP_CONF_SUCCESS) 18290a708f8fSGustavo F. Padovan pi->conf_state |= L2CAP_CONF_OUTPUT_DONE; 18300a708f8fSGustavo F. Padovan } 18310a708f8fSGustavo F. Padovan rsp->scid = cpu_to_le16(pi->dcid); 18320a708f8fSGustavo F. Padovan rsp->result = cpu_to_le16(result); 18330a708f8fSGustavo F. Padovan rsp->flags = cpu_to_le16(0x0000); 18340a708f8fSGustavo F. Padovan 18350a708f8fSGustavo F. Padovan return ptr - data; 18360a708f8fSGustavo F. Padovan } 18370a708f8fSGustavo F. Padovan 18380a708f8fSGustavo F. Padovan static int l2cap_parse_conf_rsp(struct sock *sk, void *rsp, int len, void *data, u16 *result) 18390a708f8fSGustavo F. Padovan { 18400a708f8fSGustavo F. Padovan struct l2cap_pinfo *pi = l2cap_pi(sk); 18410a708f8fSGustavo F. Padovan struct l2cap_conf_req *req = data; 18420a708f8fSGustavo F. Padovan void *ptr = req->data; 18430a708f8fSGustavo F. Padovan int type, olen; 18440a708f8fSGustavo F. Padovan unsigned long val; 18450a708f8fSGustavo F. Padovan struct l2cap_conf_rfc rfc; 18460a708f8fSGustavo F. Padovan 18470a708f8fSGustavo F. Padovan BT_DBG("sk %p, rsp %p, len %d, req %p", sk, rsp, len, data); 18480a708f8fSGustavo F. Padovan 18490a708f8fSGustavo F. Padovan while (len >= L2CAP_CONF_OPT_SIZE) { 18500a708f8fSGustavo F. Padovan len -= l2cap_get_conf_opt(&rsp, &type, &olen, &val); 18510a708f8fSGustavo F. Padovan 18520a708f8fSGustavo F. Padovan switch (type) { 18530a708f8fSGustavo F. Padovan case L2CAP_CONF_MTU: 18540a708f8fSGustavo F. Padovan if (val < L2CAP_DEFAULT_MIN_MTU) { 18550a708f8fSGustavo F. Padovan *result = L2CAP_CONF_UNACCEPT; 18560a708f8fSGustavo F. Padovan pi->imtu = L2CAP_DEFAULT_MIN_MTU; 18570a708f8fSGustavo F. Padovan } else 18580a708f8fSGustavo F. Padovan pi->imtu = val; 18590a708f8fSGustavo F. Padovan l2cap_add_conf_opt(&ptr, L2CAP_CONF_MTU, 2, pi->imtu); 18600a708f8fSGustavo F. Padovan break; 18610a708f8fSGustavo F. Padovan 18620a708f8fSGustavo F. Padovan case L2CAP_CONF_FLUSH_TO: 18630a708f8fSGustavo F. Padovan pi->flush_to = val; 18640a708f8fSGustavo F. Padovan l2cap_add_conf_opt(&ptr, L2CAP_CONF_FLUSH_TO, 18650a708f8fSGustavo F. Padovan 2, pi->flush_to); 18660a708f8fSGustavo F. Padovan break; 18670a708f8fSGustavo F. Padovan 18680a708f8fSGustavo F. Padovan case L2CAP_CONF_RFC: 18690a708f8fSGustavo F. Padovan if (olen == sizeof(rfc)) 18700a708f8fSGustavo F. Padovan memcpy(&rfc, (void *)val, olen); 18710a708f8fSGustavo F. Padovan 18720a708f8fSGustavo F. Padovan if ((pi->conf_state & L2CAP_CONF_STATE2_DEVICE) && 18730a708f8fSGustavo F. Padovan rfc.mode != pi->mode) 18740a708f8fSGustavo F. Padovan return -ECONNREFUSED; 18750a708f8fSGustavo F. Padovan 18760a708f8fSGustavo F. Padovan pi->fcs = 0; 18770a708f8fSGustavo F. Padovan 18780a708f8fSGustavo F. Padovan l2cap_add_conf_opt(&ptr, L2CAP_CONF_RFC, 18790a708f8fSGustavo F. Padovan sizeof(rfc), (unsigned long) &rfc); 18800a708f8fSGustavo F. Padovan break; 18810a708f8fSGustavo F. Padovan } 18820a708f8fSGustavo F. Padovan } 18830a708f8fSGustavo F. Padovan 18840a708f8fSGustavo F. Padovan if (pi->mode == L2CAP_MODE_BASIC && pi->mode != rfc.mode) 18850a708f8fSGustavo F. Padovan return -ECONNREFUSED; 18860a708f8fSGustavo F. Padovan 18870a708f8fSGustavo F. Padovan pi->mode = rfc.mode; 18880a708f8fSGustavo F. Padovan 18890a708f8fSGustavo F. Padovan if (*result == L2CAP_CONF_SUCCESS) { 18900a708f8fSGustavo F. Padovan switch (rfc.mode) { 18910a708f8fSGustavo F. Padovan case L2CAP_MODE_ERTM: 18920a708f8fSGustavo F. Padovan pi->retrans_timeout = le16_to_cpu(rfc.retrans_timeout); 18930a708f8fSGustavo F. Padovan pi->monitor_timeout = le16_to_cpu(rfc.monitor_timeout); 18940a708f8fSGustavo F. Padovan pi->mps = le16_to_cpu(rfc.max_pdu_size); 18950a708f8fSGustavo F. Padovan break; 18960a708f8fSGustavo F. Padovan case L2CAP_MODE_STREAMING: 18970a708f8fSGustavo F. Padovan pi->mps = le16_to_cpu(rfc.max_pdu_size); 18980a708f8fSGustavo F. Padovan } 18990a708f8fSGustavo F. Padovan } 19000a708f8fSGustavo F. Padovan 19010a708f8fSGustavo F. Padovan req->dcid = cpu_to_le16(pi->dcid); 19020a708f8fSGustavo F. Padovan req->flags = cpu_to_le16(0x0000); 19030a708f8fSGustavo F. Padovan 19040a708f8fSGustavo F. Padovan return ptr - data; 19050a708f8fSGustavo F. Padovan } 19060a708f8fSGustavo F. Padovan 19070a708f8fSGustavo F. Padovan static int l2cap_build_conf_rsp(struct sock *sk, void *data, u16 result, u16 flags) 19080a708f8fSGustavo F. Padovan { 19090a708f8fSGustavo F. Padovan struct l2cap_conf_rsp *rsp = data; 19100a708f8fSGustavo F. Padovan void *ptr = rsp->data; 19110a708f8fSGustavo F. Padovan 19120a708f8fSGustavo F. Padovan BT_DBG("sk %p", sk); 19130a708f8fSGustavo F. Padovan 19140a708f8fSGustavo F. Padovan rsp->scid = cpu_to_le16(l2cap_pi(sk)->dcid); 19150a708f8fSGustavo F. Padovan rsp->result = cpu_to_le16(result); 19160a708f8fSGustavo F. Padovan rsp->flags = cpu_to_le16(flags); 19170a708f8fSGustavo F. Padovan 19180a708f8fSGustavo F. Padovan return ptr - data; 19190a708f8fSGustavo F. Padovan } 19200a708f8fSGustavo F. Padovan 19210a708f8fSGustavo F. Padovan static void l2cap_conf_rfc_get(struct sock *sk, void *rsp, int len) 19220a708f8fSGustavo F. Padovan { 19230a708f8fSGustavo F. Padovan struct l2cap_pinfo *pi = l2cap_pi(sk); 19240a708f8fSGustavo F. Padovan int type, olen; 19250a708f8fSGustavo F. Padovan unsigned long val; 19260a708f8fSGustavo F. Padovan struct l2cap_conf_rfc rfc; 19270a708f8fSGustavo F. Padovan 19280a708f8fSGustavo F. Padovan BT_DBG("sk %p, rsp %p, len %d", sk, rsp, len); 19290a708f8fSGustavo F. Padovan 19300a708f8fSGustavo F. Padovan if ((pi->mode != L2CAP_MODE_ERTM) && (pi->mode != L2CAP_MODE_STREAMING)) 19310a708f8fSGustavo F. Padovan return; 19320a708f8fSGustavo F. Padovan 19330a708f8fSGustavo F. Padovan while (len >= L2CAP_CONF_OPT_SIZE) { 19340a708f8fSGustavo F. Padovan len -= l2cap_get_conf_opt(&rsp, &type, &olen, &val); 19350a708f8fSGustavo F. Padovan 19360a708f8fSGustavo F. Padovan switch (type) { 19370a708f8fSGustavo F. Padovan case L2CAP_CONF_RFC: 19380a708f8fSGustavo F. Padovan if (olen == sizeof(rfc)) 19390a708f8fSGustavo F. Padovan memcpy(&rfc, (void *)val, olen); 19400a708f8fSGustavo F. Padovan goto done; 19410a708f8fSGustavo F. Padovan } 19420a708f8fSGustavo F. Padovan } 19430a708f8fSGustavo F. Padovan 19440a708f8fSGustavo F. Padovan done: 19450a708f8fSGustavo F. Padovan switch (rfc.mode) { 19460a708f8fSGustavo F. Padovan case L2CAP_MODE_ERTM: 19470a708f8fSGustavo F. Padovan pi->retrans_timeout = le16_to_cpu(rfc.retrans_timeout); 19480a708f8fSGustavo F. Padovan pi->monitor_timeout = le16_to_cpu(rfc.monitor_timeout); 19490a708f8fSGustavo F. Padovan pi->mps = le16_to_cpu(rfc.max_pdu_size); 19500a708f8fSGustavo F. Padovan break; 19510a708f8fSGustavo F. Padovan case L2CAP_MODE_STREAMING: 19520a708f8fSGustavo F. Padovan pi->mps = le16_to_cpu(rfc.max_pdu_size); 19530a708f8fSGustavo F. Padovan } 19540a708f8fSGustavo F. Padovan } 19550a708f8fSGustavo F. Padovan 19560a708f8fSGustavo F. Padovan static inline int l2cap_command_rej(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data) 19570a708f8fSGustavo F. Padovan { 19580a708f8fSGustavo F. Padovan struct l2cap_cmd_rej *rej = (struct l2cap_cmd_rej *) data; 19590a708f8fSGustavo F. Padovan 19600a708f8fSGustavo F. Padovan if (rej->reason != 0x0000) 19610a708f8fSGustavo F. Padovan return 0; 19620a708f8fSGustavo F. Padovan 19630a708f8fSGustavo F. Padovan if ((conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_SENT) && 19640a708f8fSGustavo F. Padovan cmd->ident == conn->info_ident) { 19650a708f8fSGustavo F. Padovan del_timer(&conn->info_timer); 19660a708f8fSGustavo F. Padovan 19670a708f8fSGustavo F. Padovan conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_DONE; 19680a708f8fSGustavo F. Padovan conn->info_ident = 0; 19690a708f8fSGustavo F. Padovan 19700a708f8fSGustavo F. Padovan l2cap_conn_start(conn); 19710a708f8fSGustavo F. Padovan } 19720a708f8fSGustavo F. Padovan 19730a708f8fSGustavo F. Padovan return 0; 19740a708f8fSGustavo F. Padovan } 19750a708f8fSGustavo F. Padovan 19760a708f8fSGustavo F. Padovan static inline int l2cap_connect_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data) 19770a708f8fSGustavo F. Padovan { 19780a708f8fSGustavo F. Padovan struct l2cap_chan_list *list = &conn->chan_list; 19790a708f8fSGustavo F. Padovan struct l2cap_conn_req *req = (struct l2cap_conn_req *) data; 19800a708f8fSGustavo F. Padovan struct l2cap_conn_rsp rsp; 19810a708f8fSGustavo F. Padovan struct sock *parent, *sk = NULL; 19820a708f8fSGustavo F. Padovan int result, status = L2CAP_CS_NO_INFO; 19830a708f8fSGustavo F. Padovan 19840a708f8fSGustavo F. Padovan u16 dcid = 0, scid = __le16_to_cpu(req->scid); 19850a708f8fSGustavo F. Padovan __le16 psm = req->psm; 19860a708f8fSGustavo F. Padovan 19870a708f8fSGustavo F. Padovan BT_DBG("psm 0x%2.2x scid 0x%4.4x", psm, scid); 19880a708f8fSGustavo F. Padovan 19890a708f8fSGustavo F. Padovan /* Check if we have socket listening on psm */ 19900a708f8fSGustavo F. Padovan parent = l2cap_get_sock_by_psm(BT_LISTEN, psm, conn->src); 19910a708f8fSGustavo F. Padovan if (!parent) { 19920a708f8fSGustavo F. Padovan result = L2CAP_CR_BAD_PSM; 19930a708f8fSGustavo F. Padovan goto sendresp; 19940a708f8fSGustavo F. Padovan } 19950a708f8fSGustavo F. Padovan 19960a708f8fSGustavo F. Padovan bh_lock_sock(parent); 19970a708f8fSGustavo F. Padovan 19980a708f8fSGustavo F. Padovan /* Check if the ACL is secure enough (if not SDP) */ 19990a708f8fSGustavo F. Padovan if (psm != cpu_to_le16(0x0001) && 20000a708f8fSGustavo F. Padovan !hci_conn_check_link_mode(conn->hcon)) { 20010a708f8fSGustavo F. Padovan conn->disc_reason = 0x05; 20020a708f8fSGustavo F. Padovan result = L2CAP_CR_SEC_BLOCK; 20030a708f8fSGustavo F. Padovan goto response; 20040a708f8fSGustavo F. Padovan } 20050a708f8fSGustavo F. Padovan 20060a708f8fSGustavo F. Padovan result = L2CAP_CR_NO_MEM; 20070a708f8fSGustavo F. Padovan 20080a708f8fSGustavo F. Padovan /* Check for backlog size */ 20090a708f8fSGustavo F. Padovan if (sk_acceptq_is_full(parent)) { 20100a708f8fSGustavo F. Padovan BT_DBG("backlog full %d", parent->sk_ack_backlog); 20110a708f8fSGustavo F. Padovan goto response; 20120a708f8fSGustavo F. Padovan } 20130a708f8fSGustavo F. Padovan 20140a708f8fSGustavo F. Padovan sk = l2cap_sock_alloc(sock_net(parent), NULL, BTPROTO_L2CAP, GFP_ATOMIC); 20150a708f8fSGustavo F. Padovan if (!sk) 20160a708f8fSGustavo F. Padovan goto response; 20170a708f8fSGustavo F. Padovan 20180a708f8fSGustavo F. Padovan write_lock_bh(&list->lock); 20190a708f8fSGustavo F. Padovan 20200a708f8fSGustavo F. Padovan /* Check if we already have channel with that dcid */ 20210a708f8fSGustavo F. Padovan if (__l2cap_get_chan_by_dcid(list, scid)) { 20220a708f8fSGustavo F. Padovan write_unlock_bh(&list->lock); 20230a708f8fSGustavo F. Padovan sock_set_flag(sk, SOCK_ZAPPED); 20240a708f8fSGustavo F. Padovan l2cap_sock_kill(sk); 20250a708f8fSGustavo F. Padovan goto response; 20260a708f8fSGustavo F. Padovan } 20270a708f8fSGustavo F. Padovan 20280a708f8fSGustavo F. Padovan hci_conn_hold(conn->hcon); 20290a708f8fSGustavo F. Padovan 20300a708f8fSGustavo F. Padovan l2cap_sock_init(sk, parent); 20310a708f8fSGustavo F. Padovan bacpy(&bt_sk(sk)->src, conn->src); 20320a708f8fSGustavo F. Padovan bacpy(&bt_sk(sk)->dst, conn->dst); 20330a708f8fSGustavo F. Padovan l2cap_pi(sk)->psm = psm; 20340a708f8fSGustavo F. Padovan l2cap_pi(sk)->dcid = scid; 20350a708f8fSGustavo F. Padovan 20360a708f8fSGustavo F. Padovan __l2cap_chan_add(conn, sk, parent); 20370a708f8fSGustavo F. Padovan dcid = l2cap_pi(sk)->scid; 20380a708f8fSGustavo F. Padovan 20390a708f8fSGustavo F. Padovan l2cap_sock_set_timer(sk, sk->sk_sndtimeo); 20400a708f8fSGustavo F. Padovan 20410a708f8fSGustavo F. Padovan l2cap_pi(sk)->ident = cmd->ident; 20420a708f8fSGustavo F. Padovan 20430a708f8fSGustavo F. Padovan if (conn->info_state & L2CAP_INFO_FEAT_MASK_REQ_DONE) { 20440a708f8fSGustavo F. Padovan if (l2cap_check_security(sk)) { 20450a708f8fSGustavo F. Padovan if (bt_sk(sk)->defer_setup) { 20460a708f8fSGustavo F. Padovan sk->sk_state = BT_CONNECT2; 20470a708f8fSGustavo F. Padovan result = L2CAP_CR_PEND; 20480a708f8fSGustavo F. Padovan status = L2CAP_CS_AUTHOR_PEND; 20490a708f8fSGustavo F. Padovan parent->sk_data_ready(parent, 0); 20500a708f8fSGustavo F. Padovan } else { 20510a708f8fSGustavo F. Padovan sk->sk_state = BT_CONFIG; 20520a708f8fSGustavo F. Padovan result = L2CAP_CR_SUCCESS; 20530a708f8fSGustavo F. Padovan status = L2CAP_CS_NO_INFO; 20540a708f8fSGustavo F. Padovan } 20550a708f8fSGustavo F. Padovan } else { 20560a708f8fSGustavo F. Padovan sk->sk_state = BT_CONNECT2; 20570a708f8fSGustavo F. Padovan result = L2CAP_CR_PEND; 20580a708f8fSGustavo F. Padovan status = L2CAP_CS_AUTHEN_PEND; 20590a708f8fSGustavo F. Padovan } 20600a708f8fSGustavo F. Padovan } else { 20610a708f8fSGustavo F. Padovan sk->sk_state = BT_CONNECT2; 20620a708f8fSGustavo F. Padovan result = L2CAP_CR_PEND; 20630a708f8fSGustavo F. Padovan status = L2CAP_CS_NO_INFO; 20640a708f8fSGustavo F. Padovan } 20650a708f8fSGustavo F. Padovan 20660a708f8fSGustavo F. Padovan write_unlock_bh(&list->lock); 20670a708f8fSGustavo F. Padovan 20680a708f8fSGustavo F. Padovan response: 20690a708f8fSGustavo F. Padovan bh_unlock_sock(parent); 20700a708f8fSGustavo F. Padovan 20710a708f8fSGustavo F. Padovan sendresp: 20720a708f8fSGustavo F. Padovan rsp.scid = cpu_to_le16(scid); 20730a708f8fSGustavo F. Padovan rsp.dcid = cpu_to_le16(dcid); 20740a708f8fSGustavo F. Padovan rsp.result = cpu_to_le16(result); 20750a708f8fSGustavo F. Padovan rsp.status = cpu_to_le16(status); 20760a708f8fSGustavo F. Padovan l2cap_send_cmd(conn, cmd->ident, L2CAP_CONN_RSP, sizeof(rsp), &rsp); 20770a708f8fSGustavo F. Padovan 20780a708f8fSGustavo F. Padovan if (result == L2CAP_CR_PEND && status == L2CAP_CS_NO_INFO) { 20790a708f8fSGustavo F. Padovan struct l2cap_info_req info; 20800a708f8fSGustavo F. Padovan info.type = cpu_to_le16(L2CAP_IT_FEAT_MASK); 20810a708f8fSGustavo F. Padovan 20820a708f8fSGustavo F. Padovan conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_SENT; 20830a708f8fSGustavo F. Padovan conn->info_ident = l2cap_get_ident(conn); 20840a708f8fSGustavo F. Padovan 20850a708f8fSGustavo F. Padovan mod_timer(&conn->info_timer, jiffies + 20860a708f8fSGustavo F. Padovan msecs_to_jiffies(L2CAP_INFO_TIMEOUT)); 20870a708f8fSGustavo F. Padovan 20880a708f8fSGustavo F. Padovan l2cap_send_cmd(conn, conn->info_ident, 20890a708f8fSGustavo F. Padovan L2CAP_INFO_REQ, sizeof(info), &info); 20900a708f8fSGustavo F. Padovan } 20910a708f8fSGustavo F. Padovan 20920a708f8fSGustavo F. Padovan if (sk && !(l2cap_pi(sk)->conf_state & L2CAP_CONF_REQ_SENT) && 20930a708f8fSGustavo F. Padovan result == L2CAP_CR_SUCCESS) { 20940a708f8fSGustavo F. Padovan u8 buf[128]; 20950a708f8fSGustavo F. Padovan l2cap_pi(sk)->conf_state |= L2CAP_CONF_REQ_SENT; 20960a708f8fSGustavo F. Padovan l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ, 20970a708f8fSGustavo F. Padovan l2cap_build_conf_req(sk, buf), buf); 20980a708f8fSGustavo F. Padovan l2cap_pi(sk)->num_conf_req++; 20990a708f8fSGustavo F. Padovan } 21000a708f8fSGustavo F. Padovan 21010a708f8fSGustavo F. Padovan return 0; 21020a708f8fSGustavo F. Padovan } 21030a708f8fSGustavo F. Padovan 21040a708f8fSGustavo F. Padovan static inline int l2cap_connect_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data) 21050a708f8fSGustavo F. Padovan { 21060a708f8fSGustavo F. Padovan struct l2cap_conn_rsp *rsp = (struct l2cap_conn_rsp *) data; 21070a708f8fSGustavo F. Padovan u16 scid, dcid, result, status; 21080a708f8fSGustavo F. Padovan struct sock *sk; 21090a708f8fSGustavo F. Padovan u8 req[128]; 21100a708f8fSGustavo F. Padovan 21110a708f8fSGustavo F. Padovan scid = __le16_to_cpu(rsp->scid); 21120a708f8fSGustavo F. Padovan dcid = __le16_to_cpu(rsp->dcid); 21130a708f8fSGustavo F. Padovan result = __le16_to_cpu(rsp->result); 21140a708f8fSGustavo F. Padovan status = __le16_to_cpu(rsp->status); 21150a708f8fSGustavo F. Padovan 21160a708f8fSGustavo F. Padovan BT_DBG("dcid 0x%4.4x scid 0x%4.4x result 0x%2.2x status 0x%2.2x", dcid, scid, result, status); 21170a708f8fSGustavo F. Padovan 21180a708f8fSGustavo F. Padovan if (scid) { 21190a708f8fSGustavo F. Padovan sk = l2cap_get_chan_by_scid(&conn->chan_list, scid); 21200a708f8fSGustavo F. Padovan if (!sk) 21210a708f8fSGustavo F. Padovan return -EFAULT; 21220a708f8fSGustavo F. Padovan } else { 21230a708f8fSGustavo F. Padovan sk = l2cap_get_chan_by_ident(&conn->chan_list, cmd->ident); 21240a708f8fSGustavo F. Padovan if (!sk) 21250a708f8fSGustavo F. Padovan return -EFAULT; 21260a708f8fSGustavo F. Padovan } 21270a708f8fSGustavo F. Padovan 21280a708f8fSGustavo F. Padovan switch (result) { 21290a708f8fSGustavo F. Padovan case L2CAP_CR_SUCCESS: 21300a708f8fSGustavo F. Padovan sk->sk_state = BT_CONFIG; 21310a708f8fSGustavo F. Padovan l2cap_pi(sk)->ident = 0; 21320a708f8fSGustavo F. Padovan l2cap_pi(sk)->dcid = dcid; 21330a708f8fSGustavo F. Padovan l2cap_pi(sk)->conf_state &= ~L2CAP_CONF_CONNECT_PEND; 21340a708f8fSGustavo F. Padovan 21350a708f8fSGustavo F. Padovan if (l2cap_pi(sk)->conf_state & L2CAP_CONF_REQ_SENT) 21360a708f8fSGustavo F. Padovan break; 21370a708f8fSGustavo F. Padovan 21380a708f8fSGustavo F. Padovan l2cap_pi(sk)->conf_state |= L2CAP_CONF_REQ_SENT; 21390a708f8fSGustavo F. Padovan 21400a708f8fSGustavo F. Padovan l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ, 21410a708f8fSGustavo F. Padovan l2cap_build_conf_req(sk, req), req); 21420a708f8fSGustavo F. Padovan l2cap_pi(sk)->num_conf_req++; 21430a708f8fSGustavo F. Padovan break; 21440a708f8fSGustavo F. Padovan 21450a708f8fSGustavo F. Padovan case L2CAP_CR_PEND: 21460a708f8fSGustavo F. Padovan l2cap_pi(sk)->conf_state |= L2CAP_CONF_CONNECT_PEND; 21470a708f8fSGustavo F. Padovan break; 21480a708f8fSGustavo F. Padovan 21490a708f8fSGustavo F. Padovan default: 21500a708f8fSGustavo F. Padovan /* don't delete l2cap channel if sk is owned by user */ 21510a708f8fSGustavo F. Padovan if (sock_owned_by_user(sk)) { 21520a708f8fSGustavo F. Padovan sk->sk_state = BT_DISCONN; 21530a708f8fSGustavo F. Padovan l2cap_sock_clear_timer(sk); 21540a708f8fSGustavo F. Padovan l2cap_sock_set_timer(sk, HZ / 5); 21550a708f8fSGustavo F. Padovan break; 21560a708f8fSGustavo F. Padovan } 21570a708f8fSGustavo F. Padovan 21580a708f8fSGustavo F. Padovan l2cap_chan_del(sk, ECONNREFUSED); 21590a708f8fSGustavo F. Padovan break; 21600a708f8fSGustavo F. Padovan } 21610a708f8fSGustavo F. Padovan 21620a708f8fSGustavo F. Padovan bh_unlock_sock(sk); 21630a708f8fSGustavo F. Padovan return 0; 21640a708f8fSGustavo F. Padovan } 21650a708f8fSGustavo F. Padovan 21660a708f8fSGustavo F. Padovan static inline void set_default_fcs(struct l2cap_pinfo *pi) 21670a708f8fSGustavo F. Padovan { 21680a708f8fSGustavo F. Padovan /* FCS is enabled only in ERTM or streaming mode, if one or both 21690a708f8fSGustavo F. Padovan * sides request it. 21700a708f8fSGustavo F. Padovan */ 21710a708f8fSGustavo F. Padovan if (pi->mode != L2CAP_MODE_ERTM && pi->mode != L2CAP_MODE_STREAMING) 21720a708f8fSGustavo F. Padovan pi->fcs = L2CAP_FCS_NONE; 21730a708f8fSGustavo F. Padovan else if (!(pi->conf_state & L2CAP_CONF_NO_FCS_RECV)) 21740a708f8fSGustavo F. Padovan pi->fcs = L2CAP_FCS_CRC16; 21750a708f8fSGustavo F. Padovan } 21760a708f8fSGustavo F. Padovan 21770a708f8fSGustavo F. Padovan static inline int l2cap_config_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u16 cmd_len, u8 *data) 21780a708f8fSGustavo F. Padovan { 21790a708f8fSGustavo F. Padovan struct l2cap_conf_req *req = (struct l2cap_conf_req *) data; 21800a708f8fSGustavo F. Padovan u16 dcid, flags; 21810a708f8fSGustavo F. Padovan u8 rsp[64]; 21820a708f8fSGustavo F. Padovan struct sock *sk; 21830a708f8fSGustavo F. Padovan int len; 21840a708f8fSGustavo F. Padovan 21850a708f8fSGustavo F. Padovan dcid = __le16_to_cpu(req->dcid); 21860a708f8fSGustavo F. Padovan flags = __le16_to_cpu(req->flags); 21870a708f8fSGustavo F. Padovan 21880a708f8fSGustavo F. Padovan BT_DBG("dcid 0x%4.4x flags 0x%2.2x", dcid, flags); 21890a708f8fSGustavo F. Padovan 21900a708f8fSGustavo F. Padovan sk = l2cap_get_chan_by_scid(&conn->chan_list, dcid); 21910a708f8fSGustavo F. Padovan if (!sk) 21920a708f8fSGustavo F. Padovan return -ENOENT; 21930a708f8fSGustavo F. Padovan 21940a708f8fSGustavo F. Padovan if (sk->sk_state != BT_CONFIG) { 21950a708f8fSGustavo F. Padovan struct l2cap_cmd_rej rej; 21960a708f8fSGustavo F. Padovan 21970a708f8fSGustavo F. Padovan rej.reason = cpu_to_le16(0x0002); 21980a708f8fSGustavo F. Padovan l2cap_send_cmd(conn, cmd->ident, L2CAP_COMMAND_REJ, 21990a708f8fSGustavo F. Padovan sizeof(rej), &rej); 22000a708f8fSGustavo F. Padovan goto unlock; 22010a708f8fSGustavo F. Padovan } 22020a708f8fSGustavo F. Padovan 22030a708f8fSGustavo F. Padovan /* Reject if config buffer is too small. */ 22040a708f8fSGustavo F. Padovan len = cmd_len - sizeof(*req); 22050a708f8fSGustavo F. Padovan if (l2cap_pi(sk)->conf_len + len > sizeof(l2cap_pi(sk)->conf_req)) { 22060a708f8fSGustavo F. Padovan l2cap_send_cmd(conn, cmd->ident, L2CAP_CONF_RSP, 22070a708f8fSGustavo F. Padovan l2cap_build_conf_rsp(sk, rsp, 22080a708f8fSGustavo F. Padovan L2CAP_CONF_REJECT, flags), rsp); 22090a708f8fSGustavo F. Padovan goto unlock; 22100a708f8fSGustavo F. Padovan } 22110a708f8fSGustavo F. Padovan 22120a708f8fSGustavo F. Padovan /* Store config. */ 22130a708f8fSGustavo F. Padovan memcpy(l2cap_pi(sk)->conf_req + l2cap_pi(sk)->conf_len, req->data, len); 22140a708f8fSGustavo F. Padovan l2cap_pi(sk)->conf_len += len; 22150a708f8fSGustavo F. Padovan 22160a708f8fSGustavo F. Padovan if (flags & 0x0001) { 22170a708f8fSGustavo F. Padovan /* Incomplete config. Send empty response. */ 22180a708f8fSGustavo F. Padovan l2cap_send_cmd(conn, cmd->ident, L2CAP_CONF_RSP, 22190a708f8fSGustavo F. Padovan l2cap_build_conf_rsp(sk, rsp, 22200a708f8fSGustavo F. Padovan L2CAP_CONF_SUCCESS, 0x0001), rsp); 22210a708f8fSGustavo F. Padovan goto unlock; 22220a708f8fSGustavo F. Padovan } 22230a708f8fSGustavo F. Padovan 22240a708f8fSGustavo F. Padovan /* Complete config. */ 22250a708f8fSGustavo F. Padovan len = l2cap_parse_conf_req(sk, rsp); 22260a708f8fSGustavo F. Padovan if (len < 0) { 22270a708f8fSGustavo F. Padovan l2cap_send_disconn_req(conn, sk, ECONNRESET); 22280a708f8fSGustavo F. Padovan goto unlock; 22290a708f8fSGustavo F. Padovan } 22300a708f8fSGustavo F. Padovan 22310a708f8fSGustavo F. Padovan l2cap_send_cmd(conn, cmd->ident, L2CAP_CONF_RSP, len, rsp); 22320a708f8fSGustavo F. Padovan l2cap_pi(sk)->num_conf_rsp++; 22330a708f8fSGustavo F. Padovan 22340a708f8fSGustavo F. Padovan /* Reset config buffer. */ 22350a708f8fSGustavo F. Padovan l2cap_pi(sk)->conf_len = 0; 22360a708f8fSGustavo F. Padovan 22370a708f8fSGustavo F. Padovan if (!(l2cap_pi(sk)->conf_state & L2CAP_CONF_OUTPUT_DONE)) 22380a708f8fSGustavo F. Padovan goto unlock; 22390a708f8fSGustavo F. Padovan 22400a708f8fSGustavo F. Padovan if (l2cap_pi(sk)->conf_state & L2CAP_CONF_INPUT_DONE) { 22410a708f8fSGustavo F. Padovan set_default_fcs(l2cap_pi(sk)); 22420a708f8fSGustavo F. Padovan 22430a708f8fSGustavo F. Padovan sk->sk_state = BT_CONNECTED; 22440a708f8fSGustavo F. Padovan 22450a708f8fSGustavo F. Padovan l2cap_pi(sk)->next_tx_seq = 0; 22460a708f8fSGustavo F. Padovan l2cap_pi(sk)->expected_tx_seq = 0; 22470a708f8fSGustavo F. Padovan __skb_queue_head_init(TX_QUEUE(sk)); 22480a708f8fSGustavo F. Padovan if (l2cap_pi(sk)->mode == L2CAP_MODE_ERTM) 22490a708f8fSGustavo F. Padovan l2cap_ertm_init(sk); 22500a708f8fSGustavo F. Padovan 22510a708f8fSGustavo F. Padovan l2cap_chan_ready(sk); 22520a708f8fSGustavo F. Padovan goto unlock; 22530a708f8fSGustavo F. Padovan } 22540a708f8fSGustavo F. Padovan 22550a708f8fSGustavo F. Padovan if (!(l2cap_pi(sk)->conf_state & L2CAP_CONF_REQ_SENT)) { 22560a708f8fSGustavo F. Padovan u8 buf[64]; 22570a708f8fSGustavo F. Padovan l2cap_pi(sk)->conf_state |= L2CAP_CONF_REQ_SENT; 22580a708f8fSGustavo F. Padovan l2cap_send_cmd(conn, l2cap_get_ident(conn), L2CAP_CONF_REQ, 22590a708f8fSGustavo F. Padovan l2cap_build_conf_req(sk, buf), buf); 22600a708f8fSGustavo F. Padovan l2cap_pi(sk)->num_conf_req++; 22610a708f8fSGustavo F. Padovan } 22620a708f8fSGustavo F. Padovan 22630a708f8fSGustavo F. Padovan unlock: 22640a708f8fSGustavo F. Padovan bh_unlock_sock(sk); 22650a708f8fSGustavo F. Padovan return 0; 22660a708f8fSGustavo F. Padovan } 22670a708f8fSGustavo F. Padovan 22680a708f8fSGustavo F. Padovan static inline int l2cap_config_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data) 22690a708f8fSGustavo F. Padovan { 22700a708f8fSGustavo F. Padovan struct l2cap_conf_rsp *rsp = (struct l2cap_conf_rsp *)data; 22710a708f8fSGustavo F. Padovan u16 scid, flags, result; 22720a708f8fSGustavo F. Padovan struct sock *sk; 22730a708f8fSGustavo F. Padovan int len = cmd->len - sizeof(*rsp); 22740a708f8fSGustavo F. Padovan 22750a708f8fSGustavo F. Padovan scid = __le16_to_cpu(rsp->scid); 22760a708f8fSGustavo F. Padovan flags = __le16_to_cpu(rsp->flags); 22770a708f8fSGustavo F. Padovan result = __le16_to_cpu(rsp->result); 22780a708f8fSGustavo F. Padovan 22790a708f8fSGustavo F. Padovan BT_DBG("scid 0x%4.4x flags 0x%2.2x result 0x%2.2x", 22800a708f8fSGustavo F. Padovan scid, flags, result); 22810a708f8fSGustavo F. Padovan 22820a708f8fSGustavo F. Padovan sk = l2cap_get_chan_by_scid(&conn->chan_list, scid); 22830a708f8fSGustavo F. Padovan if (!sk) 22840a708f8fSGustavo F. Padovan return 0; 22850a708f8fSGustavo F. Padovan 22860a708f8fSGustavo F. Padovan switch (result) { 22870a708f8fSGustavo F. Padovan case L2CAP_CONF_SUCCESS: 22880a708f8fSGustavo F. Padovan l2cap_conf_rfc_get(sk, rsp->data, len); 22890a708f8fSGustavo F. Padovan break; 22900a708f8fSGustavo F. Padovan 22910a708f8fSGustavo F. Padovan case L2CAP_CONF_UNACCEPT: 22920a708f8fSGustavo F. Padovan if (l2cap_pi(sk)->num_conf_rsp <= L2CAP_CONF_MAX_CONF_RSP) { 22930a708f8fSGustavo F. Padovan char req[64]; 22940a708f8fSGustavo F. Padovan 22950a708f8fSGustavo F. Padovan if (len > sizeof(req) - sizeof(struct l2cap_conf_req)) { 22960a708f8fSGustavo F. Padovan l2cap_send_disconn_req(conn, sk, ECONNRESET); 22970a708f8fSGustavo F. Padovan goto done; 22980a708f8fSGustavo F. Padovan } 22990a708f8fSGustavo F. Padovan 23000a708f8fSGustavo F. Padovan /* throw out any old stored conf requests */ 23010a708f8fSGustavo F. Padovan result = L2CAP_CONF_SUCCESS; 23020a708f8fSGustavo F. Padovan len = l2cap_parse_conf_rsp(sk, rsp->data, 23030a708f8fSGustavo F. Padovan len, req, &result); 23040a708f8fSGustavo F. Padovan if (len < 0) { 23050a708f8fSGustavo F. Padovan l2cap_send_disconn_req(conn, sk, ECONNRESET); 23060a708f8fSGustavo F. Padovan goto done; 23070a708f8fSGustavo F. Padovan } 23080a708f8fSGustavo F. Padovan 23090a708f8fSGustavo F. Padovan l2cap_send_cmd(conn, l2cap_get_ident(conn), 23100a708f8fSGustavo F. Padovan L2CAP_CONF_REQ, len, req); 23110a708f8fSGustavo F. Padovan l2cap_pi(sk)->num_conf_req++; 23120a708f8fSGustavo F. Padovan if (result != L2CAP_CONF_SUCCESS) 23130a708f8fSGustavo F. Padovan goto done; 23140a708f8fSGustavo F. Padovan break; 23150a708f8fSGustavo F. Padovan } 23160a708f8fSGustavo F. Padovan 23170a708f8fSGustavo F. Padovan default: 23180a708f8fSGustavo F. Padovan sk->sk_err = ECONNRESET; 23190a708f8fSGustavo F. Padovan l2cap_sock_set_timer(sk, HZ * 5); 23200a708f8fSGustavo F. Padovan l2cap_send_disconn_req(conn, sk, ECONNRESET); 23210a708f8fSGustavo F. Padovan goto done; 23220a708f8fSGustavo F. Padovan } 23230a708f8fSGustavo F. Padovan 23240a708f8fSGustavo F. Padovan if (flags & 0x01) 23250a708f8fSGustavo F. Padovan goto done; 23260a708f8fSGustavo F. Padovan 23270a708f8fSGustavo F. Padovan l2cap_pi(sk)->conf_state |= L2CAP_CONF_INPUT_DONE; 23280a708f8fSGustavo F. Padovan 23290a708f8fSGustavo F. Padovan if (l2cap_pi(sk)->conf_state & L2CAP_CONF_OUTPUT_DONE) { 23300a708f8fSGustavo F. Padovan set_default_fcs(l2cap_pi(sk)); 23310a708f8fSGustavo F. Padovan 23320a708f8fSGustavo F. Padovan sk->sk_state = BT_CONNECTED; 23330a708f8fSGustavo F. Padovan l2cap_pi(sk)->next_tx_seq = 0; 23340a708f8fSGustavo F. Padovan l2cap_pi(sk)->expected_tx_seq = 0; 23350a708f8fSGustavo F. Padovan __skb_queue_head_init(TX_QUEUE(sk)); 23360a708f8fSGustavo F. Padovan if (l2cap_pi(sk)->mode == L2CAP_MODE_ERTM) 23370a708f8fSGustavo F. Padovan l2cap_ertm_init(sk); 23380a708f8fSGustavo F. Padovan 23390a708f8fSGustavo F. Padovan l2cap_chan_ready(sk); 23400a708f8fSGustavo F. Padovan } 23410a708f8fSGustavo F. Padovan 23420a708f8fSGustavo F. Padovan done: 23430a708f8fSGustavo F. Padovan bh_unlock_sock(sk); 23440a708f8fSGustavo F. Padovan return 0; 23450a708f8fSGustavo F. Padovan } 23460a708f8fSGustavo F. Padovan 23470a708f8fSGustavo F. Padovan static inline int l2cap_disconnect_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data) 23480a708f8fSGustavo F. Padovan { 23490a708f8fSGustavo F. Padovan struct l2cap_disconn_req *req = (struct l2cap_disconn_req *) data; 23500a708f8fSGustavo F. Padovan struct l2cap_disconn_rsp rsp; 23510a708f8fSGustavo F. Padovan u16 dcid, scid; 23520a708f8fSGustavo F. Padovan struct sock *sk; 23530a708f8fSGustavo F. Padovan 23540a708f8fSGustavo F. Padovan scid = __le16_to_cpu(req->scid); 23550a708f8fSGustavo F. Padovan dcid = __le16_to_cpu(req->dcid); 23560a708f8fSGustavo F. Padovan 23570a708f8fSGustavo F. Padovan BT_DBG("scid 0x%4.4x dcid 0x%4.4x", scid, dcid); 23580a708f8fSGustavo F. Padovan 23590a708f8fSGustavo F. Padovan sk = l2cap_get_chan_by_scid(&conn->chan_list, dcid); 23600a708f8fSGustavo F. Padovan if (!sk) 23610a708f8fSGustavo F. Padovan return 0; 23620a708f8fSGustavo F. Padovan 23630a708f8fSGustavo F. Padovan rsp.dcid = cpu_to_le16(l2cap_pi(sk)->scid); 23640a708f8fSGustavo F. Padovan rsp.scid = cpu_to_le16(l2cap_pi(sk)->dcid); 23650a708f8fSGustavo F. Padovan l2cap_send_cmd(conn, cmd->ident, L2CAP_DISCONN_RSP, sizeof(rsp), &rsp); 23660a708f8fSGustavo F. Padovan 23670a708f8fSGustavo F. Padovan sk->sk_shutdown = SHUTDOWN_MASK; 23680a708f8fSGustavo F. Padovan 23690a708f8fSGustavo F. Padovan /* don't delete l2cap channel if sk is owned by user */ 23700a708f8fSGustavo F. Padovan if (sock_owned_by_user(sk)) { 23710a708f8fSGustavo F. Padovan sk->sk_state = BT_DISCONN; 23720a708f8fSGustavo F. Padovan l2cap_sock_clear_timer(sk); 23730a708f8fSGustavo F. Padovan l2cap_sock_set_timer(sk, HZ / 5); 23740a708f8fSGustavo F. Padovan bh_unlock_sock(sk); 23750a708f8fSGustavo F. Padovan return 0; 23760a708f8fSGustavo F. Padovan } 23770a708f8fSGustavo F. Padovan 23780a708f8fSGustavo F. Padovan l2cap_chan_del(sk, ECONNRESET); 23790a708f8fSGustavo F. Padovan bh_unlock_sock(sk); 23800a708f8fSGustavo F. Padovan 23810a708f8fSGustavo F. Padovan l2cap_sock_kill(sk); 23820a708f8fSGustavo F. Padovan return 0; 23830a708f8fSGustavo F. Padovan } 23840a708f8fSGustavo F. Padovan 23850a708f8fSGustavo F. Padovan static inline int l2cap_disconnect_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data) 23860a708f8fSGustavo F. Padovan { 23870a708f8fSGustavo F. Padovan struct l2cap_disconn_rsp *rsp = (struct l2cap_disconn_rsp *) data; 23880a708f8fSGustavo F. Padovan u16 dcid, scid; 23890a708f8fSGustavo F. Padovan struct sock *sk; 23900a708f8fSGustavo F. Padovan 23910a708f8fSGustavo F. Padovan scid = __le16_to_cpu(rsp->scid); 23920a708f8fSGustavo F. Padovan dcid = __le16_to_cpu(rsp->dcid); 23930a708f8fSGustavo F. Padovan 23940a708f8fSGustavo F. Padovan BT_DBG("dcid 0x%4.4x scid 0x%4.4x", dcid, scid); 23950a708f8fSGustavo F. Padovan 23960a708f8fSGustavo F. Padovan sk = l2cap_get_chan_by_scid(&conn->chan_list, scid); 23970a708f8fSGustavo F. Padovan if (!sk) 23980a708f8fSGustavo F. Padovan return 0; 23990a708f8fSGustavo F. Padovan 24000a708f8fSGustavo F. Padovan /* don't delete l2cap channel if sk is owned by user */ 24010a708f8fSGustavo F. Padovan if (sock_owned_by_user(sk)) { 24020a708f8fSGustavo F. Padovan sk->sk_state = BT_DISCONN; 24030a708f8fSGustavo F. Padovan l2cap_sock_clear_timer(sk); 24040a708f8fSGustavo F. Padovan l2cap_sock_set_timer(sk, HZ / 5); 24050a708f8fSGustavo F. Padovan bh_unlock_sock(sk); 24060a708f8fSGustavo F. Padovan return 0; 24070a708f8fSGustavo F. Padovan } 24080a708f8fSGustavo F. Padovan 24090a708f8fSGustavo F. Padovan l2cap_chan_del(sk, 0); 24100a708f8fSGustavo F. Padovan bh_unlock_sock(sk); 24110a708f8fSGustavo F. Padovan 24120a708f8fSGustavo F. Padovan l2cap_sock_kill(sk); 24130a708f8fSGustavo F. Padovan return 0; 24140a708f8fSGustavo F. Padovan } 24150a708f8fSGustavo F. Padovan 24160a708f8fSGustavo F. Padovan static inline int l2cap_information_req(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data) 24170a708f8fSGustavo F. Padovan { 24180a708f8fSGustavo F. Padovan struct l2cap_info_req *req = (struct l2cap_info_req *) data; 24190a708f8fSGustavo F. Padovan u16 type; 24200a708f8fSGustavo F. Padovan 24210a708f8fSGustavo F. Padovan type = __le16_to_cpu(req->type); 24220a708f8fSGustavo F. Padovan 24230a708f8fSGustavo F. Padovan BT_DBG("type 0x%4.4x", type); 24240a708f8fSGustavo F. Padovan 24250a708f8fSGustavo F. Padovan if (type == L2CAP_IT_FEAT_MASK) { 24260a708f8fSGustavo F. Padovan u8 buf[8]; 24270a708f8fSGustavo F. Padovan u32 feat_mask = l2cap_feat_mask; 24280a708f8fSGustavo F. Padovan struct l2cap_info_rsp *rsp = (struct l2cap_info_rsp *) buf; 24290a708f8fSGustavo F. Padovan rsp->type = cpu_to_le16(L2CAP_IT_FEAT_MASK); 24300a708f8fSGustavo F. Padovan rsp->result = cpu_to_le16(L2CAP_IR_SUCCESS); 24310a708f8fSGustavo F. Padovan if (!disable_ertm) 24320a708f8fSGustavo F. Padovan feat_mask |= L2CAP_FEAT_ERTM | L2CAP_FEAT_STREAMING 24330a708f8fSGustavo F. Padovan | L2CAP_FEAT_FCS; 24340a708f8fSGustavo F. Padovan put_unaligned_le32(feat_mask, rsp->data); 24350a708f8fSGustavo F. Padovan l2cap_send_cmd(conn, cmd->ident, 24360a708f8fSGustavo F. Padovan L2CAP_INFO_RSP, sizeof(buf), buf); 24370a708f8fSGustavo F. Padovan } else if (type == L2CAP_IT_FIXED_CHAN) { 24380a708f8fSGustavo F. Padovan u8 buf[12]; 24390a708f8fSGustavo F. Padovan struct l2cap_info_rsp *rsp = (struct l2cap_info_rsp *) buf; 24400a708f8fSGustavo F. Padovan rsp->type = cpu_to_le16(L2CAP_IT_FIXED_CHAN); 24410a708f8fSGustavo F. Padovan rsp->result = cpu_to_le16(L2CAP_IR_SUCCESS); 24420a708f8fSGustavo F. Padovan memcpy(buf + 4, l2cap_fixed_chan, 8); 24430a708f8fSGustavo F. Padovan l2cap_send_cmd(conn, cmd->ident, 24440a708f8fSGustavo F. Padovan L2CAP_INFO_RSP, sizeof(buf), buf); 24450a708f8fSGustavo F. Padovan } else { 24460a708f8fSGustavo F. Padovan struct l2cap_info_rsp rsp; 24470a708f8fSGustavo F. Padovan rsp.type = cpu_to_le16(type); 24480a708f8fSGustavo F. Padovan rsp.result = cpu_to_le16(L2CAP_IR_NOTSUPP); 24490a708f8fSGustavo F. Padovan l2cap_send_cmd(conn, cmd->ident, 24500a708f8fSGustavo F. Padovan L2CAP_INFO_RSP, sizeof(rsp), &rsp); 24510a708f8fSGustavo F. Padovan } 24520a708f8fSGustavo F. Padovan 24530a708f8fSGustavo F. Padovan return 0; 24540a708f8fSGustavo F. Padovan } 24550a708f8fSGustavo F. Padovan 24560a708f8fSGustavo F. Padovan static inline int l2cap_information_rsp(struct l2cap_conn *conn, struct l2cap_cmd_hdr *cmd, u8 *data) 24570a708f8fSGustavo F. Padovan { 24580a708f8fSGustavo F. Padovan struct l2cap_info_rsp *rsp = (struct l2cap_info_rsp *) data; 24590a708f8fSGustavo F. Padovan u16 type, result; 24600a708f8fSGustavo F. Padovan 24610a708f8fSGustavo F. Padovan type = __le16_to_cpu(rsp->type); 24620a708f8fSGustavo F. Padovan result = __le16_to_cpu(rsp->result); 24630a708f8fSGustavo F. Padovan 24640a708f8fSGustavo F. Padovan BT_DBG("type 0x%4.4x result 0x%2.2x", type, result); 24650a708f8fSGustavo F. Padovan 24660a708f8fSGustavo F. Padovan del_timer(&conn->info_timer); 24670a708f8fSGustavo F. Padovan 24680a708f8fSGustavo F. Padovan if (result != L2CAP_IR_SUCCESS) { 24690a708f8fSGustavo F. Padovan conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_DONE; 24700a708f8fSGustavo F. Padovan conn->info_ident = 0; 24710a708f8fSGustavo F. Padovan 24720a708f8fSGustavo F. Padovan l2cap_conn_start(conn); 24730a708f8fSGustavo F. Padovan 24740a708f8fSGustavo F. Padovan return 0; 24750a708f8fSGustavo F. Padovan } 24760a708f8fSGustavo F. Padovan 24770a708f8fSGustavo F. Padovan if (type == L2CAP_IT_FEAT_MASK) { 24780a708f8fSGustavo F. Padovan conn->feat_mask = get_unaligned_le32(rsp->data); 24790a708f8fSGustavo F. Padovan 24800a708f8fSGustavo F. Padovan if (conn->feat_mask & L2CAP_FEAT_FIXED_CHAN) { 24810a708f8fSGustavo F. Padovan struct l2cap_info_req req; 24820a708f8fSGustavo F. Padovan req.type = cpu_to_le16(L2CAP_IT_FIXED_CHAN); 24830a708f8fSGustavo F. Padovan 24840a708f8fSGustavo F. Padovan conn->info_ident = l2cap_get_ident(conn); 24850a708f8fSGustavo F. Padovan 24860a708f8fSGustavo F. Padovan l2cap_send_cmd(conn, conn->info_ident, 24870a708f8fSGustavo F. Padovan L2CAP_INFO_REQ, sizeof(req), &req); 24880a708f8fSGustavo F. Padovan } else { 24890a708f8fSGustavo F. Padovan conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_DONE; 24900a708f8fSGustavo F. Padovan conn->info_ident = 0; 24910a708f8fSGustavo F. Padovan 24920a708f8fSGustavo F. Padovan l2cap_conn_start(conn); 24930a708f8fSGustavo F. Padovan } 24940a708f8fSGustavo F. Padovan } else if (type == L2CAP_IT_FIXED_CHAN) { 24950a708f8fSGustavo F. Padovan conn->info_state |= L2CAP_INFO_FEAT_MASK_REQ_DONE; 24960a708f8fSGustavo F. Padovan conn->info_ident = 0; 24970a708f8fSGustavo F. Padovan 24980a708f8fSGustavo F. Padovan l2cap_conn_start(conn); 24990a708f8fSGustavo F. Padovan } 25000a708f8fSGustavo F. Padovan 25010a708f8fSGustavo F. Padovan return 0; 25020a708f8fSGustavo F. Padovan } 25030a708f8fSGustavo F. Padovan 2504*3300d9a9SClaudio Takahasi static inline int l2cap_bredr_sig_cmd(struct l2cap_conn *conn, 2505*3300d9a9SClaudio Takahasi struct l2cap_cmd_hdr *cmd, u16 cmd_len, u8 *data) 2506*3300d9a9SClaudio Takahasi { 2507*3300d9a9SClaudio Takahasi int err = 0; 2508*3300d9a9SClaudio Takahasi 2509*3300d9a9SClaudio Takahasi switch (cmd->code) { 2510*3300d9a9SClaudio Takahasi case L2CAP_COMMAND_REJ: 2511*3300d9a9SClaudio Takahasi l2cap_command_rej(conn, cmd, data); 2512*3300d9a9SClaudio Takahasi break; 2513*3300d9a9SClaudio Takahasi 2514*3300d9a9SClaudio Takahasi case L2CAP_CONN_REQ: 2515*3300d9a9SClaudio Takahasi err = l2cap_connect_req(conn, cmd, data); 2516*3300d9a9SClaudio Takahasi break; 2517*3300d9a9SClaudio Takahasi 2518*3300d9a9SClaudio Takahasi case L2CAP_CONN_RSP: 2519*3300d9a9SClaudio Takahasi err = l2cap_connect_rsp(conn, cmd, data); 2520*3300d9a9SClaudio Takahasi break; 2521*3300d9a9SClaudio Takahasi 2522*3300d9a9SClaudio Takahasi case L2CAP_CONF_REQ: 2523*3300d9a9SClaudio Takahasi err = l2cap_config_req(conn, cmd, cmd_len, data); 2524*3300d9a9SClaudio Takahasi break; 2525*3300d9a9SClaudio Takahasi 2526*3300d9a9SClaudio Takahasi case L2CAP_CONF_RSP: 2527*3300d9a9SClaudio Takahasi err = l2cap_config_rsp(conn, cmd, data); 2528*3300d9a9SClaudio Takahasi break; 2529*3300d9a9SClaudio Takahasi 2530*3300d9a9SClaudio Takahasi case L2CAP_DISCONN_REQ: 2531*3300d9a9SClaudio Takahasi err = l2cap_disconnect_req(conn, cmd, data); 2532*3300d9a9SClaudio Takahasi break; 2533*3300d9a9SClaudio Takahasi 2534*3300d9a9SClaudio Takahasi case L2CAP_DISCONN_RSP: 2535*3300d9a9SClaudio Takahasi err = l2cap_disconnect_rsp(conn, cmd, data); 2536*3300d9a9SClaudio Takahasi break; 2537*3300d9a9SClaudio Takahasi 2538*3300d9a9SClaudio Takahasi case L2CAP_ECHO_REQ: 2539*3300d9a9SClaudio Takahasi l2cap_send_cmd(conn, cmd->ident, L2CAP_ECHO_RSP, cmd_len, data); 2540*3300d9a9SClaudio Takahasi break; 2541*3300d9a9SClaudio Takahasi 2542*3300d9a9SClaudio Takahasi case L2CAP_ECHO_RSP: 2543*3300d9a9SClaudio Takahasi break; 2544*3300d9a9SClaudio Takahasi 2545*3300d9a9SClaudio Takahasi case L2CAP_INFO_REQ: 2546*3300d9a9SClaudio Takahasi err = l2cap_information_req(conn, cmd, data); 2547*3300d9a9SClaudio Takahasi break; 2548*3300d9a9SClaudio Takahasi 2549*3300d9a9SClaudio Takahasi case L2CAP_INFO_RSP: 2550*3300d9a9SClaudio Takahasi err = l2cap_information_rsp(conn, cmd, data); 2551*3300d9a9SClaudio Takahasi break; 2552*3300d9a9SClaudio Takahasi 2553*3300d9a9SClaudio Takahasi default: 2554*3300d9a9SClaudio Takahasi BT_ERR("Unknown BR/EDR signaling command 0x%2.2x", cmd->code); 2555*3300d9a9SClaudio Takahasi err = -EINVAL; 2556*3300d9a9SClaudio Takahasi break; 2557*3300d9a9SClaudio Takahasi } 2558*3300d9a9SClaudio Takahasi 2559*3300d9a9SClaudio Takahasi return err; 2560*3300d9a9SClaudio Takahasi } 2561*3300d9a9SClaudio Takahasi 2562*3300d9a9SClaudio Takahasi static inline int l2cap_le_sig_cmd(struct l2cap_conn *conn, 2563*3300d9a9SClaudio Takahasi struct l2cap_cmd_hdr *cmd, u8 *data) 2564*3300d9a9SClaudio Takahasi { 2565*3300d9a9SClaudio Takahasi switch (cmd->code) { 2566*3300d9a9SClaudio Takahasi case L2CAP_COMMAND_REJ: 2567*3300d9a9SClaudio Takahasi return 0; 2568*3300d9a9SClaudio Takahasi 2569*3300d9a9SClaudio Takahasi case L2CAP_CONN_PARAM_UPDATE_REQ: 2570*3300d9a9SClaudio Takahasi return -EINVAL; 2571*3300d9a9SClaudio Takahasi 2572*3300d9a9SClaudio Takahasi case L2CAP_CONN_PARAM_UPDATE_RSP: 2573*3300d9a9SClaudio Takahasi return 0; 2574*3300d9a9SClaudio Takahasi 2575*3300d9a9SClaudio Takahasi default: 2576*3300d9a9SClaudio Takahasi BT_ERR("Unknown LE signaling command 0x%2.2x", cmd->code); 2577*3300d9a9SClaudio Takahasi return -EINVAL; 2578*3300d9a9SClaudio Takahasi } 2579*3300d9a9SClaudio Takahasi } 2580*3300d9a9SClaudio Takahasi 2581*3300d9a9SClaudio Takahasi static inline void l2cap_sig_channel(struct l2cap_conn *conn, 2582*3300d9a9SClaudio Takahasi struct sk_buff *skb) 25830a708f8fSGustavo F. Padovan { 25840a708f8fSGustavo F. Padovan u8 *data = skb->data; 25850a708f8fSGustavo F. Padovan int len = skb->len; 25860a708f8fSGustavo F. Padovan struct l2cap_cmd_hdr cmd; 2587*3300d9a9SClaudio Takahasi int err; 25880a708f8fSGustavo F. Padovan 25890a708f8fSGustavo F. Padovan l2cap_raw_recv(conn, skb); 25900a708f8fSGustavo F. Padovan 25910a708f8fSGustavo F. Padovan while (len >= L2CAP_CMD_HDR_SIZE) { 25920a708f8fSGustavo F. Padovan u16 cmd_len; 25930a708f8fSGustavo F. Padovan memcpy(&cmd, data, L2CAP_CMD_HDR_SIZE); 25940a708f8fSGustavo F. Padovan data += L2CAP_CMD_HDR_SIZE; 25950a708f8fSGustavo F. Padovan len -= L2CAP_CMD_HDR_SIZE; 25960a708f8fSGustavo F. Padovan 25970a708f8fSGustavo F. Padovan cmd_len = le16_to_cpu(cmd.len); 25980a708f8fSGustavo F. Padovan 25990a708f8fSGustavo F. Padovan BT_DBG("code 0x%2.2x len %d id 0x%2.2x", cmd.code, cmd_len, cmd.ident); 26000a708f8fSGustavo F. Padovan 26010a708f8fSGustavo F. Padovan if (cmd_len > len || !cmd.ident) { 26020a708f8fSGustavo F. Padovan BT_DBG("corrupted command"); 26030a708f8fSGustavo F. Padovan break; 26040a708f8fSGustavo F. Padovan } 26050a708f8fSGustavo F. Padovan 2606*3300d9a9SClaudio Takahasi if (conn->hcon->type == LE_LINK) 2607*3300d9a9SClaudio Takahasi err = l2cap_le_sig_cmd(conn, &cmd, data); 2608*3300d9a9SClaudio Takahasi else 2609*3300d9a9SClaudio Takahasi err = l2cap_bredr_sig_cmd(conn, &cmd, cmd_len, data); 26100a708f8fSGustavo F. Padovan 26110a708f8fSGustavo F. Padovan if (err) { 26120a708f8fSGustavo F. Padovan struct l2cap_cmd_rej rej; 26130a708f8fSGustavo F. Padovan BT_DBG("error %d", err); 26140a708f8fSGustavo F. Padovan 26150a708f8fSGustavo F. Padovan /* FIXME: Map err to a valid reason */ 26160a708f8fSGustavo F. Padovan rej.reason = cpu_to_le16(0); 26170a708f8fSGustavo F. Padovan l2cap_send_cmd(conn, cmd.ident, L2CAP_COMMAND_REJ, sizeof(rej), &rej); 26180a708f8fSGustavo F. Padovan } 26190a708f8fSGustavo F. Padovan 26200a708f8fSGustavo F. Padovan data += cmd_len; 26210a708f8fSGustavo F. Padovan len -= cmd_len; 26220a708f8fSGustavo F. Padovan } 26230a708f8fSGustavo F. Padovan 26240a708f8fSGustavo F. Padovan kfree_skb(skb); 26250a708f8fSGustavo F. Padovan } 26260a708f8fSGustavo F. Padovan 26270a708f8fSGustavo F. Padovan static int l2cap_check_fcs(struct l2cap_pinfo *pi, struct sk_buff *skb) 26280a708f8fSGustavo F. Padovan { 26290a708f8fSGustavo F. Padovan u16 our_fcs, rcv_fcs; 26300a708f8fSGustavo F. Padovan int hdr_size = L2CAP_HDR_SIZE + 2; 26310a708f8fSGustavo F. Padovan 26320a708f8fSGustavo F. Padovan if (pi->fcs == L2CAP_FCS_CRC16) { 26330a708f8fSGustavo F. Padovan skb_trim(skb, skb->len - 2); 26340a708f8fSGustavo F. Padovan rcv_fcs = get_unaligned_le16(skb->data + skb->len); 26350a708f8fSGustavo F. Padovan our_fcs = crc16(0, skb->data - hdr_size, skb->len + hdr_size); 26360a708f8fSGustavo F. Padovan 26370a708f8fSGustavo F. Padovan if (our_fcs != rcv_fcs) 26380a708f8fSGustavo F. Padovan return -EBADMSG; 26390a708f8fSGustavo F. Padovan } 26400a708f8fSGustavo F. Padovan return 0; 26410a708f8fSGustavo F. Padovan } 26420a708f8fSGustavo F. Padovan 26430a708f8fSGustavo F. Padovan static inline void l2cap_send_i_or_rr_or_rnr(struct sock *sk) 26440a708f8fSGustavo F. Padovan { 26450a708f8fSGustavo F. Padovan struct l2cap_pinfo *pi = l2cap_pi(sk); 26460a708f8fSGustavo F. Padovan u16 control = 0; 26470a708f8fSGustavo F. Padovan 26480a708f8fSGustavo F. Padovan pi->frames_sent = 0; 26490a708f8fSGustavo F. Padovan 26500a708f8fSGustavo F. Padovan control |= pi->buffer_seq << L2CAP_CTRL_REQSEQ_SHIFT; 26510a708f8fSGustavo F. Padovan 26520a708f8fSGustavo F. Padovan if (pi->conn_state & L2CAP_CONN_LOCAL_BUSY) { 26530a708f8fSGustavo F. Padovan control |= L2CAP_SUPER_RCV_NOT_READY; 26540a708f8fSGustavo F. Padovan l2cap_send_sframe(pi, control); 26550a708f8fSGustavo F. Padovan pi->conn_state |= L2CAP_CONN_RNR_SENT; 26560a708f8fSGustavo F. Padovan } 26570a708f8fSGustavo F. Padovan 26580a708f8fSGustavo F. Padovan if (pi->conn_state & L2CAP_CONN_REMOTE_BUSY) 26590a708f8fSGustavo F. Padovan l2cap_retransmit_frames(sk); 26600a708f8fSGustavo F. Padovan 26610a708f8fSGustavo F. Padovan l2cap_ertm_send(sk); 26620a708f8fSGustavo F. Padovan 26630a708f8fSGustavo F. Padovan if (!(pi->conn_state & L2CAP_CONN_LOCAL_BUSY) && 26640a708f8fSGustavo F. Padovan pi->frames_sent == 0) { 26650a708f8fSGustavo F. Padovan control |= L2CAP_SUPER_RCV_READY; 26660a708f8fSGustavo F. Padovan l2cap_send_sframe(pi, control); 26670a708f8fSGustavo F. Padovan } 26680a708f8fSGustavo F. Padovan } 26690a708f8fSGustavo F. Padovan 26700a708f8fSGustavo F. Padovan static int l2cap_add_to_srej_queue(struct sock *sk, struct sk_buff *skb, u8 tx_seq, u8 sar) 26710a708f8fSGustavo F. Padovan { 26720a708f8fSGustavo F. Padovan struct sk_buff *next_skb; 26730a708f8fSGustavo F. Padovan struct l2cap_pinfo *pi = l2cap_pi(sk); 26740a708f8fSGustavo F. Padovan int tx_seq_offset, next_tx_seq_offset; 26750a708f8fSGustavo F. Padovan 26760a708f8fSGustavo F. Padovan bt_cb(skb)->tx_seq = tx_seq; 26770a708f8fSGustavo F. Padovan bt_cb(skb)->sar = sar; 26780a708f8fSGustavo F. Padovan 26790a708f8fSGustavo F. Padovan next_skb = skb_peek(SREJ_QUEUE(sk)); 26800a708f8fSGustavo F. Padovan if (!next_skb) { 26810a708f8fSGustavo F. Padovan __skb_queue_tail(SREJ_QUEUE(sk), skb); 26820a708f8fSGustavo F. Padovan return 0; 26830a708f8fSGustavo F. Padovan } 26840a708f8fSGustavo F. Padovan 26850a708f8fSGustavo F. Padovan tx_seq_offset = (tx_seq - pi->buffer_seq) % 64; 26860a708f8fSGustavo F. Padovan if (tx_seq_offset < 0) 26870a708f8fSGustavo F. Padovan tx_seq_offset += 64; 26880a708f8fSGustavo F. Padovan 26890a708f8fSGustavo F. Padovan do { 26900a708f8fSGustavo F. Padovan if (bt_cb(next_skb)->tx_seq == tx_seq) 26910a708f8fSGustavo F. Padovan return -EINVAL; 26920a708f8fSGustavo F. Padovan 26930a708f8fSGustavo F. Padovan next_tx_seq_offset = (bt_cb(next_skb)->tx_seq - 26940a708f8fSGustavo F. Padovan pi->buffer_seq) % 64; 26950a708f8fSGustavo F. Padovan if (next_tx_seq_offset < 0) 26960a708f8fSGustavo F. Padovan next_tx_seq_offset += 64; 26970a708f8fSGustavo F. Padovan 26980a708f8fSGustavo F. Padovan if (next_tx_seq_offset > tx_seq_offset) { 26990a708f8fSGustavo F. Padovan __skb_queue_before(SREJ_QUEUE(sk), next_skb, skb); 27000a708f8fSGustavo F. Padovan return 0; 27010a708f8fSGustavo F. Padovan } 27020a708f8fSGustavo F. Padovan 27030a708f8fSGustavo F. Padovan if (skb_queue_is_last(SREJ_QUEUE(sk), next_skb)) 27040a708f8fSGustavo F. Padovan break; 27050a708f8fSGustavo F. Padovan 27060a708f8fSGustavo F. Padovan } while ((next_skb = skb_queue_next(SREJ_QUEUE(sk), next_skb))); 27070a708f8fSGustavo F. Padovan 27080a708f8fSGustavo F. Padovan __skb_queue_tail(SREJ_QUEUE(sk), skb); 27090a708f8fSGustavo F. Padovan 27100a708f8fSGustavo F. Padovan return 0; 27110a708f8fSGustavo F. Padovan } 27120a708f8fSGustavo F. Padovan 27130a708f8fSGustavo F. Padovan static int l2cap_ertm_reassembly_sdu(struct sock *sk, struct sk_buff *skb, u16 control) 27140a708f8fSGustavo F. Padovan { 27150a708f8fSGustavo F. Padovan struct l2cap_pinfo *pi = l2cap_pi(sk); 27160a708f8fSGustavo F. Padovan struct sk_buff *_skb; 27170a708f8fSGustavo F. Padovan int err; 27180a708f8fSGustavo F. Padovan 27190a708f8fSGustavo F. Padovan switch (control & L2CAP_CTRL_SAR) { 27200a708f8fSGustavo F. Padovan case L2CAP_SDU_UNSEGMENTED: 27210a708f8fSGustavo F. Padovan if (pi->conn_state & L2CAP_CONN_SAR_SDU) 27220a708f8fSGustavo F. Padovan goto drop; 27230a708f8fSGustavo F. Padovan 27240a708f8fSGustavo F. Padovan err = sock_queue_rcv_skb(sk, skb); 27250a708f8fSGustavo F. Padovan if (!err) 27260a708f8fSGustavo F. Padovan return err; 27270a708f8fSGustavo F. Padovan 27280a708f8fSGustavo F. Padovan break; 27290a708f8fSGustavo F. Padovan 27300a708f8fSGustavo F. Padovan case L2CAP_SDU_START: 27310a708f8fSGustavo F. Padovan if (pi->conn_state & L2CAP_CONN_SAR_SDU) 27320a708f8fSGustavo F. Padovan goto drop; 27330a708f8fSGustavo F. Padovan 27340a708f8fSGustavo F. Padovan pi->sdu_len = get_unaligned_le16(skb->data); 27350a708f8fSGustavo F. Padovan 27360a708f8fSGustavo F. Padovan if (pi->sdu_len > pi->imtu) 27370a708f8fSGustavo F. Padovan goto disconnect; 27380a708f8fSGustavo F. Padovan 27390a708f8fSGustavo F. Padovan pi->sdu = bt_skb_alloc(pi->sdu_len, GFP_ATOMIC); 27400a708f8fSGustavo F. Padovan if (!pi->sdu) 27410a708f8fSGustavo F. Padovan return -ENOMEM; 27420a708f8fSGustavo F. Padovan 27430a708f8fSGustavo F. Padovan /* pull sdu_len bytes only after alloc, because of Local Busy 27440a708f8fSGustavo F. Padovan * condition we have to be sure that this will be executed 27450a708f8fSGustavo F. Padovan * only once, i.e., when alloc does not fail */ 27460a708f8fSGustavo F. Padovan skb_pull(skb, 2); 27470a708f8fSGustavo F. Padovan 27480a708f8fSGustavo F. Padovan memcpy(skb_put(pi->sdu, skb->len), skb->data, skb->len); 27490a708f8fSGustavo F. Padovan 27500a708f8fSGustavo F. Padovan pi->conn_state |= L2CAP_CONN_SAR_SDU; 27510a708f8fSGustavo F. Padovan pi->partial_sdu_len = skb->len; 27520a708f8fSGustavo F. Padovan break; 27530a708f8fSGustavo F. Padovan 27540a708f8fSGustavo F. Padovan case L2CAP_SDU_CONTINUE: 27550a708f8fSGustavo F. Padovan if (!(pi->conn_state & L2CAP_CONN_SAR_SDU)) 27560a708f8fSGustavo F. Padovan goto disconnect; 27570a708f8fSGustavo F. Padovan 27580a708f8fSGustavo F. Padovan if (!pi->sdu) 27590a708f8fSGustavo F. Padovan goto disconnect; 27600a708f8fSGustavo F. Padovan 27610a708f8fSGustavo F. Padovan pi->partial_sdu_len += skb->len; 27620a708f8fSGustavo F. Padovan if (pi->partial_sdu_len > pi->sdu_len) 27630a708f8fSGustavo F. Padovan goto drop; 27640a708f8fSGustavo F. Padovan 27650a708f8fSGustavo F. Padovan memcpy(skb_put(pi->sdu, skb->len), skb->data, skb->len); 27660a708f8fSGustavo F. Padovan 27670a708f8fSGustavo F. Padovan break; 27680a708f8fSGustavo F. Padovan 27690a708f8fSGustavo F. Padovan case L2CAP_SDU_END: 27700a708f8fSGustavo F. Padovan if (!(pi->conn_state & L2CAP_CONN_SAR_SDU)) 27710a708f8fSGustavo F. Padovan goto disconnect; 27720a708f8fSGustavo F. Padovan 27730a708f8fSGustavo F. Padovan if (!pi->sdu) 27740a708f8fSGustavo F. Padovan goto disconnect; 27750a708f8fSGustavo F. Padovan 27760a708f8fSGustavo F. Padovan if (!(pi->conn_state & L2CAP_CONN_SAR_RETRY)) { 27770a708f8fSGustavo F. Padovan pi->partial_sdu_len += skb->len; 27780a708f8fSGustavo F. Padovan 27790a708f8fSGustavo F. Padovan if (pi->partial_sdu_len > pi->imtu) 27800a708f8fSGustavo F. Padovan goto drop; 27810a708f8fSGustavo F. Padovan 27820a708f8fSGustavo F. Padovan if (pi->partial_sdu_len != pi->sdu_len) 27830a708f8fSGustavo F. Padovan goto drop; 27840a708f8fSGustavo F. Padovan 27850a708f8fSGustavo F. Padovan memcpy(skb_put(pi->sdu, skb->len), skb->data, skb->len); 27860a708f8fSGustavo F. Padovan } 27870a708f8fSGustavo F. Padovan 27880a708f8fSGustavo F. Padovan _skb = skb_clone(pi->sdu, GFP_ATOMIC); 27890a708f8fSGustavo F. Padovan if (!_skb) { 27900a708f8fSGustavo F. Padovan pi->conn_state |= L2CAP_CONN_SAR_RETRY; 27910a708f8fSGustavo F. Padovan return -ENOMEM; 27920a708f8fSGustavo F. Padovan } 27930a708f8fSGustavo F. Padovan 27940a708f8fSGustavo F. Padovan err = sock_queue_rcv_skb(sk, _skb); 27950a708f8fSGustavo F. Padovan if (err < 0) { 27960a708f8fSGustavo F. Padovan kfree_skb(_skb); 27970a708f8fSGustavo F. Padovan pi->conn_state |= L2CAP_CONN_SAR_RETRY; 27980a708f8fSGustavo F. Padovan return err; 27990a708f8fSGustavo F. Padovan } 28000a708f8fSGustavo F. Padovan 28010a708f8fSGustavo F. Padovan pi->conn_state &= ~L2CAP_CONN_SAR_RETRY; 28020a708f8fSGustavo F. Padovan pi->conn_state &= ~L2CAP_CONN_SAR_SDU; 28030a708f8fSGustavo F. Padovan 28040a708f8fSGustavo F. Padovan kfree_skb(pi->sdu); 28050a708f8fSGustavo F. Padovan break; 28060a708f8fSGustavo F. Padovan } 28070a708f8fSGustavo F. Padovan 28080a708f8fSGustavo F. Padovan kfree_skb(skb); 28090a708f8fSGustavo F. Padovan return 0; 28100a708f8fSGustavo F. Padovan 28110a708f8fSGustavo F. Padovan drop: 28120a708f8fSGustavo F. Padovan kfree_skb(pi->sdu); 28130a708f8fSGustavo F. Padovan pi->sdu = NULL; 28140a708f8fSGustavo F. Padovan 28150a708f8fSGustavo F. Padovan disconnect: 28160a708f8fSGustavo F. Padovan l2cap_send_disconn_req(pi->conn, sk, ECONNRESET); 28170a708f8fSGustavo F. Padovan kfree_skb(skb); 28180a708f8fSGustavo F. Padovan return 0; 28190a708f8fSGustavo F. Padovan } 28200a708f8fSGustavo F. Padovan 28210a708f8fSGustavo F. Padovan static int l2cap_try_push_rx_skb(struct sock *sk) 28220a708f8fSGustavo F. Padovan { 28230a708f8fSGustavo F. Padovan struct l2cap_pinfo *pi = l2cap_pi(sk); 28240a708f8fSGustavo F. Padovan struct sk_buff *skb; 28250a708f8fSGustavo F. Padovan u16 control; 28260a708f8fSGustavo F. Padovan int err; 28270a708f8fSGustavo F. Padovan 28280a708f8fSGustavo F. Padovan while ((skb = skb_dequeue(BUSY_QUEUE(sk)))) { 28290a708f8fSGustavo F. Padovan control = bt_cb(skb)->sar << L2CAP_CTRL_SAR_SHIFT; 28300a708f8fSGustavo F. Padovan err = l2cap_ertm_reassembly_sdu(sk, skb, control); 28310a708f8fSGustavo F. Padovan if (err < 0) { 28320a708f8fSGustavo F. Padovan skb_queue_head(BUSY_QUEUE(sk), skb); 28330a708f8fSGustavo F. Padovan return -EBUSY; 28340a708f8fSGustavo F. Padovan } 28350a708f8fSGustavo F. Padovan 28360a708f8fSGustavo F. Padovan pi->buffer_seq = (pi->buffer_seq + 1) % 64; 28370a708f8fSGustavo F. Padovan } 28380a708f8fSGustavo F. Padovan 28390a708f8fSGustavo F. Padovan if (!(pi->conn_state & L2CAP_CONN_RNR_SENT)) 28400a708f8fSGustavo F. Padovan goto done; 28410a708f8fSGustavo F. Padovan 28420a708f8fSGustavo F. Padovan control = pi->buffer_seq << L2CAP_CTRL_REQSEQ_SHIFT; 28430a708f8fSGustavo F. Padovan control |= L2CAP_SUPER_RCV_READY | L2CAP_CTRL_POLL; 28440a708f8fSGustavo F. Padovan l2cap_send_sframe(pi, control); 28450a708f8fSGustavo F. Padovan l2cap_pi(sk)->retry_count = 1; 28460a708f8fSGustavo F. Padovan 28470a708f8fSGustavo F. Padovan del_timer(&pi->retrans_timer); 28480a708f8fSGustavo F. Padovan __mod_monitor_timer(); 28490a708f8fSGustavo F. Padovan 28500a708f8fSGustavo F. Padovan l2cap_pi(sk)->conn_state |= L2CAP_CONN_WAIT_F; 28510a708f8fSGustavo F. Padovan 28520a708f8fSGustavo F. Padovan done: 28530a708f8fSGustavo F. Padovan pi->conn_state &= ~L2CAP_CONN_LOCAL_BUSY; 28540a708f8fSGustavo F. Padovan pi->conn_state &= ~L2CAP_CONN_RNR_SENT; 28550a708f8fSGustavo F. Padovan 28560a708f8fSGustavo F. Padovan BT_DBG("sk %p, Exit local busy", sk); 28570a708f8fSGustavo F. Padovan 28580a708f8fSGustavo F. Padovan return 0; 28590a708f8fSGustavo F. Padovan } 28600a708f8fSGustavo F. Padovan 28610a708f8fSGustavo F. Padovan static void l2cap_busy_work(struct work_struct *work) 28620a708f8fSGustavo F. Padovan { 28630a708f8fSGustavo F. Padovan DECLARE_WAITQUEUE(wait, current); 28640a708f8fSGustavo F. Padovan struct l2cap_pinfo *pi = 28650a708f8fSGustavo F. Padovan container_of(work, struct l2cap_pinfo, busy_work); 28660a708f8fSGustavo F. Padovan struct sock *sk = (struct sock *)pi; 28670a708f8fSGustavo F. Padovan int n_tries = 0, timeo = HZ/5, err; 28680a708f8fSGustavo F. Padovan struct sk_buff *skb; 28690a708f8fSGustavo F. Padovan 28700a708f8fSGustavo F. Padovan lock_sock(sk); 28710a708f8fSGustavo F. Padovan 28720a708f8fSGustavo F. Padovan add_wait_queue(sk_sleep(sk), &wait); 28730a708f8fSGustavo F. Padovan while ((skb = skb_peek(BUSY_QUEUE(sk)))) { 28740a708f8fSGustavo F. Padovan set_current_state(TASK_INTERRUPTIBLE); 28750a708f8fSGustavo F. Padovan 28760a708f8fSGustavo F. Padovan if (n_tries++ > L2CAP_LOCAL_BUSY_TRIES) { 28770a708f8fSGustavo F. Padovan err = -EBUSY; 28780a708f8fSGustavo F. Padovan l2cap_send_disconn_req(pi->conn, sk, EBUSY); 28790a708f8fSGustavo F. Padovan break; 28800a708f8fSGustavo F. Padovan } 28810a708f8fSGustavo F. Padovan 28820a708f8fSGustavo F. Padovan if (!timeo) 28830a708f8fSGustavo F. Padovan timeo = HZ/5; 28840a708f8fSGustavo F. Padovan 28850a708f8fSGustavo F. Padovan if (signal_pending(current)) { 28860a708f8fSGustavo F. Padovan err = sock_intr_errno(timeo); 28870a708f8fSGustavo F. Padovan break; 28880a708f8fSGustavo F. Padovan } 28890a708f8fSGustavo F. Padovan 28900a708f8fSGustavo F. Padovan release_sock(sk); 28910a708f8fSGustavo F. Padovan timeo = schedule_timeout(timeo); 28920a708f8fSGustavo F. Padovan lock_sock(sk); 28930a708f8fSGustavo F. Padovan 28940a708f8fSGustavo F. Padovan err = sock_error(sk); 28950a708f8fSGustavo F. Padovan if (err) 28960a708f8fSGustavo F. Padovan break; 28970a708f8fSGustavo F. Padovan 28980a708f8fSGustavo F. Padovan if (l2cap_try_push_rx_skb(sk) == 0) 28990a708f8fSGustavo F. Padovan break; 29000a708f8fSGustavo F. Padovan } 29010a708f8fSGustavo F. Padovan 29020a708f8fSGustavo F. Padovan set_current_state(TASK_RUNNING); 29030a708f8fSGustavo F. Padovan remove_wait_queue(sk_sleep(sk), &wait); 29040a708f8fSGustavo F. Padovan 29050a708f8fSGustavo F. Padovan release_sock(sk); 29060a708f8fSGustavo F. Padovan } 29070a708f8fSGustavo F. Padovan 29080a708f8fSGustavo F. Padovan static int l2cap_push_rx_skb(struct sock *sk, struct sk_buff *skb, u16 control) 29090a708f8fSGustavo F. Padovan { 29100a708f8fSGustavo F. Padovan struct l2cap_pinfo *pi = l2cap_pi(sk); 29110a708f8fSGustavo F. Padovan int sctrl, err; 29120a708f8fSGustavo F. Padovan 29130a708f8fSGustavo F. Padovan if (pi->conn_state & L2CAP_CONN_LOCAL_BUSY) { 29140a708f8fSGustavo F. Padovan bt_cb(skb)->sar = control >> L2CAP_CTRL_SAR_SHIFT; 29150a708f8fSGustavo F. Padovan __skb_queue_tail(BUSY_QUEUE(sk), skb); 29160a708f8fSGustavo F. Padovan return l2cap_try_push_rx_skb(sk); 29170a708f8fSGustavo F. Padovan 29180a708f8fSGustavo F. Padovan 29190a708f8fSGustavo F. Padovan } 29200a708f8fSGustavo F. Padovan 29210a708f8fSGustavo F. Padovan err = l2cap_ertm_reassembly_sdu(sk, skb, control); 29220a708f8fSGustavo F. Padovan if (err >= 0) { 29230a708f8fSGustavo F. Padovan pi->buffer_seq = (pi->buffer_seq + 1) % 64; 29240a708f8fSGustavo F. Padovan return err; 29250a708f8fSGustavo F. Padovan } 29260a708f8fSGustavo F. Padovan 29270a708f8fSGustavo F. Padovan /* Busy Condition */ 29280a708f8fSGustavo F. Padovan BT_DBG("sk %p, Enter local busy", sk); 29290a708f8fSGustavo F. Padovan 29300a708f8fSGustavo F. Padovan pi->conn_state |= L2CAP_CONN_LOCAL_BUSY; 29310a708f8fSGustavo F. Padovan bt_cb(skb)->sar = control >> L2CAP_CTRL_SAR_SHIFT; 29320a708f8fSGustavo F. Padovan __skb_queue_tail(BUSY_QUEUE(sk), skb); 29330a708f8fSGustavo F. Padovan 29340a708f8fSGustavo F. Padovan sctrl = pi->buffer_seq << L2CAP_CTRL_REQSEQ_SHIFT; 29350a708f8fSGustavo F. Padovan sctrl |= L2CAP_SUPER_RCV_NOT_READY; 29360a708f8fSGustavo F. Padovan l2cap_send_sframe(pi, sctrl); 29370a708f8fSGustavo F. Padovan 29380a708f8fSGustavo F. Padovan pi->conn_state |= L2CAP_CONN_RNR_SENT; 29390a708f8fSGustavo F. Padovan 29400a708f8fSGustavo F. Padovan del_timer(&pi->ack_timer); 29410a708f8fSGustavo F. Padovan 29420a708f8fSGustavo F. Padovan queue_work(_busy_wq, &pi->busy_work); 29430a708f8fSGustavo F. Padovan 29440a708f8fSGustavo F. Padovan return err; 29450a708f8fSGustavo F. Padovan } 29460a708f8fSGustavo F. Padovan 29470a708f8fSGustavo F. Padovan static int l2cap_streaming_reassembly_sdu(struct sock *sk, struct sk_buff *skb, u16 control) 29480a708f8fSGustavo F. Padovan { 29490a708f8fSGustavo F. Padovan struct l2cap_pinfo *pi = l2cap_pi(sk); 29500a708f8fSGustavo F. Padovan struct sk_buff *_skb; 29510a708f8fSGustavo F. Padovan int err = -EINVAL; 29520a708f8fSGustavo F. Padovan 29530a708f8fSGustavo F. Padovan /* 29540a708f8fSGustavo F. Padovan * TODO: We have to notify the userland if some data is lost with the 29550a708f8fSGustavo F. Padovan * Streaming Mode. 29560a708f8fSGustavo F. Padovan */ 29570a708f8fSGustavo F. Padovan 29580a708f8fSGustavo F. Padovan switch (control & L2CAP_CTRL_SAR) { 29590a708f8fSGustavo F. Padovan case L2CAP_SDU_UNSEGMENTED: 29600a708f8fSGustavo F. Padovan if (pi->conn_state & L2CAP_CONN_SAR_SDU) { 29610a708f8fSGustavo F. Padovan kfree_skb(pi->sdu); 29620a708f8fSGustavo F. Padovan break; 29630a708f8fSGustavo F. Padovan } 29640a708f8fSGustavo F. Padovan 29650a708f8fSGustavo F. Padovan err = sock_queue_rcv_skb(sk, skb); 29660a708f8fSGustavo F. Padovan if (!err) 29670a708f8fSGustavo F. Padovan return 0; 29680a708f8fSGustavo F. Padovan 29690a708f8fSGustavo F. Padovan break; 29700a708f8fSGustavo F. Padovan 29710a708f8fSGustavo F. Padovan case L2CAP_SDU_START: 29720a708f8fSGustavo F. Padovan if (pi->conn_state & L2CAP_CONN_SAR_SDU) { 29730a708f8fSGustavo F. Padovan kfree_skb(pi->sdu); 29740a708f8fSGustavo F. Padovan break; 29750a708f8fSGustavo F. Padovan } 29760a708f8fSGustavo F. Padovan 29770a708f8fSGustavo F. Padovan pi->sdu_len = get_unaligned_le16(skb->data); 29780a708f8fSGustavo F. Padovan skb_pull(skb, 2); 29790a708f8fSGustavo F. Padovan 29800a708f8fSGustavo F. Padovan if (pi->sdu_len > pi->imtu) { 29810a708f8fSGustavo F. Padovan err = -EMSGSIZE; 29820a708f8fSGustavo F. Padovan break; 29830a708f8fSGustavo F. Padovan } 29840a708f8fSGustavo F. Padovan 29850a708f8fSGustavo F. Padovan pi->sdu = bt_skb_alloc(pi->sdu_len, GFP_ATOMIC); 29860a708f8fSGustavo F. Padovan if (!pi->sdu) { 29870a708f8fSGustavo F. Padovan err = -ENOMEM; 29880a708f8fSGustavo F. Padovan break; 29890a708f8fSGustavo F. Padovan } 29900a708f8fSGustavo F. Padovan 29910a708f8fSGustavo F. Padovan memcpy(skb_put(pi->sdu, skb->len), skb->data, skb->len); 29920a708f8fSGustavo F. Padovan 29930a708f8fSGustavo F. Padovan pi->conn_state |= L2CAP_CONN_SAR_SDU; 29940a708f8fSGustavo F. Padovan pi->partial_sdu_len = skb->len; 29950a708f8fSGustavo F. Padovan err = 0; 29960a708f8fSGustavo F. Padovan break; 29970a708f8fSGustavo F. Padovan 29980a708f8fSGustavo F. Padovan case L2CAP_SDU_CONTINUE: 29990a708f8fSGustavo F. Padovan if (!(pi->conn_state & L2CAP_CONN_SAR_SDU)) 30000a708f8fSGustavo F. Padovan break; 30010a708f8fSGustavo F. Padovan 30020a708f8fSGustavo F. Padovan memcpy(skb_put(pi->sdu, skb->len), skb->data, skb->len); 30030a708f8fSGustavo F. Padovan 30040a708f8fSGustavo F. Padovan pi->partial_sdu_len += skb->len; 30050a708f8fSGustavo F. Padovan if (pi->partial_sdu_len > pi->sdu_len) 30060a708f8fSGustavo F. Padovan kfree_skb(pi->sdu); 30070a708f8fSGustavo F. Padovan else 30080a708f8fSGustavo F. Padovan err = 0; 30090a708f8fSGustavo F. Padovan 30100a708f8fSGustavo F. Padovan break; 30110a708f8fSGustavo F. Padovan 30120a708f8fSGustavo F. Padovan case L2CAP_SDU_END: 30130a708f8fSGustavo F. Padovan if (!(pi->conn_state & L2CAP_CONN_SAR_SDU)) 30140a708f8fSGustavo F. Padovan break; 30150a708f8fSGustavo F. Padovan 30160a708f8fSGustavo F. Padovan memcpy(skb_put(pi->sdu, skb->len), skb->data, skb->len); 30170a708f8fSGustavo F. Padovan 30180a708f8fSGustavo F. Padovan pi->conn_state &= ~L2CAP_CONN_SAR_SDU; 30190a708f8fSGustavo F. Padovan pi->partial_sdu_len += skb->len; 30200a708f8fSGustavo F. Padovan 30210a708f8fSGustavo F. Padovan if (pi->partial_sdu_len > pi->imtu) 30220a708f8fSGustavo F. Padovan goto drop; 30230a708f8fSGustavo F. Padovan 30240a708f8fSGustavo F. Padovan if (pi->partial_sdu_len == pi->sdu_len) { 30250a708f8fSGustavo F. Padovan _skb = skb_clone(pi->sdu, GFP_ATOMIC); 30260a708f8fSGustavo F. Padovan err = sock_queue_rcv_skb(sk, _skb); 30270a708f8fSGustavo F. Padovan if (err < 0) 30280a708f8fSGustavo F. Padovan kfree_skb(_skb); 30290a708f8fSGustavo F. Padovan } 30300a708f8fSGustavo F. Padovan err = 0; 30310a708f8fSGustavo F. Padovan 30320a708f8fSGustavo F. Padovan drop: 30330a708f8fSGustavo F. Padovan kfree_skb(pi->sdu); 30340a708f8fSGustavo F. Padovan break; 30350a708f8fSGustavo F. Padovan } 30360a708f8fSGustavo F. Padovan 30370a708f8fSGustavo F. Padovan kfree_skb(skb); 30380a708f8fSGustavo F. Padovan return err; 30390a708f8fSGustavo F. Padovan } 30400a708f8fSGustavo F. Padovan 30410a708f8fSGustavo F. Padovan static void l2cap_check_srej_gap(struct sock *sk, u8 tx_seq) 30420a708f8fSGustavo F. Padovan { 30430a708f8fSGustavo F. Padovan struct sk_buff *skb; 30440a708f8fSGustavo F. Padovan u16 control; 30450a708f8fSGustavo F. Padovan 30460a708f8fSGustavo F. Padovan while ((skb = skb_peek(SREJ_QUEUE(sk)))) { 30470a708f8fSGustavo F. Padovan if (bt_cb(skb)->tx_seq != tx_seq) 30480a708f8fSGustavo F. Padovan break; 30490a708f8fSGustavo F. Padovan 30500a708f8fSGustavo F. Padovan skb = skb_dequeue(SREJ_QUEUE(sk)); 30510a708f8fSGustavo F. Padovan control = bt_cb(skb)->sar << L2CAP_CTRL_SAR_SHIFT; 30520a708f8fSGustavo F. Padovan l2cap_ertm_reassembly_sdu(sk, skb, control); 30530a708f8fSGustavo F. Padovan l2cap_pi(sk)->buffer_seq_srej = 30540a708f8fSGustavo F. Padovan (l2cap_pi(sk)->buffer_seq_srej + 1) % 64; 30550a708f8fSGustavo F. Padovan tx_seq = (tx_seq + 1) % 64; 30560a708f8fSGustavo F. Padovan } 30570a708f8fSGustavo F. Padovan } 30580a708f8fSGustavo F. Padovan 30590a708f8fSGustavo F. Padovan static void l2cap_resend_srejframe(struct sock *sk, u8 tx_seq) 30600a708f8fSGustavo F. Padovan { 30610a708f8fSGustavo F. Padovan struct l2cap_pinfo *pi = l2cap_pi(sk); 30620a708f8fSGustavo F. Padovan struct srej_list *l, *tmp; 30630a708f8fSGustavo F. Padovan u16 control; 30640a708f8fSGustavo F. Padovan 30650a708f8fSGustavo F. Padovan list_for_each_entry_safe(l, tmp, SREJ_LIST(sk), list) { 30660a708f8fSGustavo F. Padovan if (l->tx_seq == tx_seq) { 30670a708f8fSGustavo F. Padovan list_del(&l->list); 30680a708f8fSGustavo F. Padovan kfree(l); 30690a708f8fSGustavo F. Padovan return; 30700a708f8fSGustavo F. Padovan } 30710a708f8fSGustavo F. Padovan control = L2CAP_SUPER_SELECT_REJECT; 30720a708f8fSGustavo F. Padovan control |= l->tx_seq << L2CAP_CTRL_REQSEQ_SHIFT; 30730a708f8fSGustavo F. Padovan l2cap_send_sframe(pi, control); 30740a708f8fSGustavo F. Padovan list_del(&l->list); 30750a708f8fSGustavo F. Padovan list_add_tail(&l->list, SREJ_LIST(sk)); 30760a708f8fSGustavo F. Padovan } 30770a708f8fSGustavo F. Padovan } 30780a708f8fSGustavo F. Padovan 30790a708f8fSGustavo F. Padovan static void l2cap_send_srejframe(struct sock *sk, u8 tx_seq) 30800a708f8fSGustavo F. Padovan { 30810a708f8fSGustavo F. Padovan struct l2cap_pinfo *pi = l2cap_pi(sk); 30820a708f8fSGustavo F. Padovan struct srej_list *new; 30830a708f8fSGustavo F. Padovan u16 control; 30840a708f8fSGustavo F. Padovan 30850a708f8fSGustavo F. Padovan while (tx_seq != pi->expected_tx_seq) { 30860a708f8fSGustavo F. Padovan control = L2CAP_SUPER_SELECT_REJECT; 30870a708f8fSGustavo F. Padovan control |= pi->expected_tx_seq << L2CAP_CTRL_REQSEQ_SHIFT; 30880a708f8fSGustavo F. Padovan l2cap_send_sframe(pi, control); 30890a708f8fSGustavo F. Padovan 30900a708f8fSGustavo F. Padovan new = kzalloc(sizeof(struct srej_list), GFP_ATOMIC); 30910a708f8fSGustavo F. Padovan new->tx_seq = pi->expected_tx_seq; 30920a708f8fSGustavo F. Padovan pi->expected_tx_seq = (pi->expected_tx_seq + 1) % 64; 30930a708f8fSGustavo F. Padovan list_add_tail(&new->list, SREJ_LIST(sk)); 30940a708f8fSGustavo F. Padovan } 30950a708f8fSGustavo F. Padovan pi->expected_tx_seq = (pi->expected_tx_seq + 1) % 64; 30960a708f8fSGustavo F. Padovan } 30970a708f8fSGustavo F. Padovan 30980a708f8fSGustavo F. Padovan static inline int l2cap_data_channel_iframe(struct sock *sk, u16 rx_control, struct sk_buff *skb) 30990a708f8fSGustavo F. Padovan { 31000a708f8fSGustavo F. Padovan struct l2cap_pinfo *pi = l2cap_pi(sk); 31010a708f8fSGustavo F. Padovan u8 tx_seq = __get_txseq(rx_control); 31020a708f8fSGustavo F. Padovan u8 req_seq = __get_reqseq(rx_control); 31030a708f8fSGustavo F. Padovan u8 sar = rx_control >> L2CAP_CTRL_SAR_SHIFT; 31040a708f8fSGustavo F. Padovan int tx_seq_offset, expected_tx_seq_offset; 31050a708f8fSGustavo F. Padovan int num_to_ack = (pi->tx_win/6) + 1; 31060a708f8fSGustavo F. Padovan int err = 0; 31070a708f8fSGustavo F. Padovan 31080a708f8fSGustavo F. Padovan BT_DBG("sk %p len %d tx_seq %d rx_control 0x%4.4x", sk, skb->len, tx_seq, 31090a708f8fSGustavo F. Padovan rx_control); 31100a708f8fSGustavo F. Padovan 31110a708f8fSGustavo F. Padovan if (L2CAP_CTRL_FINAL & rx_control && 31120a708f8fSGustavo F. Padovan l2cap_pi(sk)->conn_state & L2CAP_CONN_WAIT_F) { 31130a708f8fSGustavo F. Padovan del_timer(&pi->monitor_timer); 31140a708f8fSGustavo F. Padovan if (pi->unacked_frames > 0) 31150a708f8fSGustavo F. Padovan __mod_retrans_timer(); 31160a708f8fSGustavo F. Padovan pi->conn_state &= ~L2CAP_CONN_WAIT_F; 31170a708f8fSGustavo F. Padovan } 31180a708f8fSGustavo F. Padovan 31190a708f8fSGustavo F. Padovan pi->expected_ack_seq = req_seq; 31200a708f8fSGustavo F. Padovan l2cap_drop_acked_frames(sk); 31210a708f8fSGustavo F. Padovan 31220a708f8fSGustavo F. Padovan if (tx_seq == pi->expected_tx_seq) 31230a708f8fSGustavo F. Padovan goto expected; 31240a708f8fSGustavo F. Padovan 31250a708f8fSGustavo F. Padovan tx_seq_offset = (tx_seq - pi->buffer_seq) % 64; 31260a708f8fSGustavo F. Padovan if (tx_seq_offset < 0) 31270a708f8fSGustavo F. Padovan tx_seq_offset += 64; 31280a708f8fSGustavo F. Padovan 31290a708f8fSGustavo F. Padovan /* invalid tx_seq */ 31300a708f8fSGustavo F. Padovan if (tx_seq_offset >= pi->tx_win) { 31310a708f8fSGustavo F. Padovan l2cap_send_disconn_req(pi->conn, sk, ECONNRESET); 31320a708f8fSGustavo F. Padovan goto drop; 31330a708f8fSGustavo F. Padovan } 31340a708f8fSGustavo F. Padovan 31350a708f8fSGustavo F. Padovan if (pi->conn_state == L2CAP_CONN_LOCAL_BUSY) 31360a708f8fSGustavo F. Padovan goto drop; 31370a708f8fSGustavo F. Padovan 31380a708f8fSGustavo F. Padovan if (pi->conn_state & L2CAP_CONN_SREJ_SENT) { 31390a708f8fSGustavo F. Padovan struct srej_list *first; 31400a708f8fSGustavo F. Padovan 31410a708f8fSGustavo F. Padovan first = list_first_entry(SREJ_LIST(sk), 31420a708f8fSGustavo F. Padovan struct srej_list, list); 31430a708f8fSGustavo F. Padovan if (tx_seq == first->tx_seq) { 31440a708f8fSGustavo F. Padovan l2cap_add_to_srej_queue(sk, skb, tx_seq, sar); 31450a708f8fSGustavo F. Padovan l2cap_check_srej_gap(sk, tx_seq); 31460a708f8fSGustavo F. Padovan 31470a708f8fSGustavo F. Padovan list_del(&first->list); 31480a708f8fSGustavo F. Padovan kfree(first); 31490a708f8fSGustavo F. Padovan 31500a708f8fSGustavo F. Padovan if (list_empty(SREJ_LIST(sk))) { 31510a708f8fSGustavo F. Padovan pi->buffer_seq = pi->buffer_seq_srej; 31520a708f8fSGustavo F. Padovan pi->conn_state &= ~L2CAP_CONN_SREJ_SENT; 31530a708f8fSGustavo F. Padovan l2cap_send_ack(pi); 31540a708f8fSGustavo F. Padovan BT_DBG("sk %p, Exit SREJ_SENT", sk); 31550a708f8fSGustavo F. Padovan } 31560a708f8fSGustavo F. Padovan } else { 31570a708f8fSGustavo F. Padovan struct srej_list *l; 31580a708f8fSGustavo F. Padovan 31590a708f8fSGustavo F. Padovan /* duplicated tx_seq */ 31600a708f8fSGustavo F. Padovan if (l2cap_add_to_srej_queue(sk, skb, tx_seq, sar) < 0) 31610a708f8fSGustavo F. Padovan goto drop; 31620a708f8fSGustavo F. Padovan 31630a708f8fSGustavo F. Padovan list_for_each_entry(l, SREJ_LIST(sk), list) { 31640a708f8fSGustavo F. Padovan if (l->tx_seq == tx_seq) { 31650a708f8fSGustavo F. Padovan l2cap_resend_srejframe(sk, tx_seq); 31660a708f8fSGustavo F. Padovan return 0; 31670a708f8fSGustavo F. Padovan } 31680a708f8fSGustavo F. Padovan } 31690a708f8fSGustavo F. Padovan l2cap_send_srejframe(sk, tx_seq); 31700a708f8fSGustavo F. Padovan } 31710a708f8fSGustavo F. Padovan } else { 31720a708f8fSGustavo F. Padovan expected_tx_seq_offset = 31730a708f8fSGustavo F. Padovan (pi->expected_tx_seq - pi->buffer_seq) % 64; 31740a708f8fSGustavo F. Padovan if (expected_tx_seq_offset < 0) 31750a708f8fSGustavo F. Padovan expected_tx_seq_offset += 64; 31760a708f8fSGustavo F. Padovan 31770a708f8fSGustavo F. Padovan /* duplicated tx_seq */ 31780a708f8fSGustavo F. Padovan if (tx_seq_offset < expected_tx_seq_offset) 31790a708f8fSGustavo F. Padovan goto drop; 31800a708f8fSGustavo F. Padovan 31810a708f8fSGustavo F. Padovan pi->conn_state |= L2CAP_CONN_SREJ_SENT; 31820a708f8fSGustavo F. Padovan 31830a708f8fSGustavo F. Padovan BT_DBG("sk %p, Enter SREJ", sk); 31840a708f8fSGustavo F. Padovan 31850a708f8fSGustavo F. Padovan INIT_LIST_HEAD(SREJ_LIST(sk)); 31860a708f8fSGustavo F. Padovan pi->buffer_seq_srej = pi->buffer_seq; 31870a708f8fSGustavo F. Padovan 31880a708f8fSGustavo F. Padovan __skb_queue_head_init(SREJ_QUEUE(sk)); 31890a708f8fSGustavo F. Padovan __skb_queue_head_init(BUSY_QUEUE(sk)); 31900a708f8fSGustavo F. Padovan l2cap_add_to_srej_queue(sk, skb, tx_seq, sar); 31910a708f8fSGustavo F. Padovan 31920a708f8fSGustavo F. Padovan pi->conn_state |= L2CAP_CONN_SEND_PBIT; 31930a708f8fSGustavo F. Padovan 31940a708f8fSGustavo F. Padovan l2cap_send_srejframe(sk, tx_seq); 31950a708f8fSGustavo F. Padovan 31960a708f8fSGustavo F. Padovan del_timer(&pi->ack_timer); 31970a708f8fSGustavo F. Padovan } 31980a708f8fSGustavo F. Padovan return 0; 31990a708f8fSGustavo F. Padovan 32000a708f8fSGustavo F. Padovan expected: 32010a708f8fSGustavo F. Padovan pi->expected_tx_seq = (pi->expected_tx_seq + 1) % 64; 32020a708f8fSGustavo F. Padovan 32030a708f8fSGustavo F. Padovan if (pi->conn_state & L2CAP_CONN_SREJ_SENT) { 32040a708f8fSGustavo F. Padovan bt_cb(skb)->tx_seq = tx_seq; 32050a708f8fSGustavo F. Padovan bt_cb(skb)->sar = sar; 32060a708f8fSGustavo F. Padovan __skb_queue_tail(SREJ_QUEUE(sk), skb); 32070a708f8fSGustavo F. Padovan return 0; 32080a708f8fSGustavo F. Padovan } 32090a708f8fSGustavo F. Padovan 32100a708f8fSGustavo F. Padovan err = l2cap_push_rx_skb(sk, skb, rx_control); 32110a708f8fSGustavo F. Padovan if (err < 0) 32120a708f8fSGustavo F. Padovan return 0; 32130a708f8fSGustavo F. Padovan 32140a708f8fSGustavo F. Padovan if (rx_control & L2CAP_CTRL_FINAL) { 32150a708f8fSGustavo F. Padovan if (pi->conn_state & L2CAP_CONN_REJ_ACT) 32160a708f8fSGustavo F. Padovan pi->conn_state &= ~L2CAP_CONN_REJ_ACT; 32170a708f8fSGustavo F. Padovan else 32180a708f8fSGustavo F. Padovan l2cap_retransmit_frames(sk); 32190a708f8fSGustavo F. Padovan } 32200a708f8fSGustavo F. Padovan 32210a708f8fSGustavo F. Padovan __mod_ack_timer(); 32220a708f8fSGustavo F. Padovan 32230a708f8fSGustavo F. Padovan pi->num_acked = (pi->num_acked + 1) % num_to_ack; 32240a708f8fSGustavo F. Padovan if (pi->num_acked == num_to_ack - 1) 32250a708f8fSGustavo F. Padovan l2cap_send_ack(pi); 32260a708f8fSGustavo F. Padovan 32270a708f8fSGustavo F. Padovan return 0; 32280a708f8fSGustavo F. Padovan 32290a708f8fSGustavo F. Padovan drop: 32300a708f8fSGustavo F. Padovan kfree_skb(skb); 32310a708f8fSGustavo F. Padovan return 0; 32320a708f8fSGustavo F. Padovan } 32330a708f8fSGustavo F. Padovan 32340a708f8fSGustavo F. Padovan static inline void l2cap_data_channel_rrframe(struct sock *sk, u16 rx_control) 32350a708f8fSGustavo F. Padovan { 32360a708f8fSGustavo F. Padovan struct l2cap_pinfo *pi = l2cap_pi(sk); 32370a708f8fSGustavo F. Padovan 32380a708f8fSGustavo F. Padovan BT_DBG("sk %p, req_seq %d ctrl 0x%4.4x", sk, __get_reqseq(rx_control), 32390a708f8fSGustavo F. Padovan rx_control); 32400a708f8fSGustavo F. Padovan 32410a708f8fSGustavo F. Padovan pi->expected_ack_seq = __get_reqseq(rx_control); 32420a708f8fSGustavo F. Padovan l2cap_drop_acked_frames(sk); 32430a708f8fSGustavo F. Padovan 32440a708f8fSGustavo F. Padovan if (rx_control & L2CAP_CTRL_POLL) { 32450a708f8fSGustavo F. Padovan pi->conn_state |= L2CAP_CONN_SEND_FBIT; 32460a708f8fSGustavo F. Padovan if (pi->conn_state & L2CAP_CONN_SREJ_SENT) { 32470a708f8fSGustavo F. Padovan if ((pi->conn_state & L2CAP_CONN_REMOTE_BUSY) && 32480a708f8fSGustavo F. Padovan (pi->unacked_frames > 0)) 32490a708f8fSGustavo F. Padovan __mod_retrans_timer(); 32500a708f8fSGustavo F. Padovan 32510a708f8fSGustavo F. Padovan pi->conn_state &= ~L2CAP_CONN_REMOTE_BUSY; 32520a708f8fSGustavo F. Padovan l2cap_send_srejtail(sk); 32530a708f8fSGustavo F. Padovan } else { 32540a708f8fSGustavo F. Padovan l2cap_send_i_or_rr_or_rnr(sk); 32550a708f8fSGustavo F. Padovan } 32560a708f8fSGustavo F. Padovan 32570a708f8fSGustavo F. Padovan } else if (rx_control & L2CAP_CTRL_FINAL) { 32580a708f8fSGustavo F. Padovan pi->conn_state &= ~L2CAP_CONN_REMOTE_BUSY; 32590a708f8fSGustavo F. Padovan 32600a708f8fSGustavo F. Padovan if (pi->conn_state & L2CAP_CONN_REJ_ACT) 32610a708f8fSGustavo F. Padovan pi->conn_state &= ~L2CAP_CONN_REJ_ACT; 32620a708f8fSGustavo F. Padovan else 32630a708f8fSGustavo F. Padovan l2cap_retransmit_frames(sk); 32640a708f8fSGustavo F. Padovan 32650a708f8fSGustavo F. Padovan } else { 32660a708f8fSGustavo F. Padovan if ((pi->conn_state & L2CAP_CONN_REMOTE_BUSY) && 32670a708f8fSGustavo F. Padovan (pi->unacked_frames > 0)) 32680a708f8fSGustavo F. Padovan __mod_retrans_timer(); 32690a708f8fSGustavo F. Padovan 32700a708f8fSGustavo F. Padovan pi->conn_state &= ~L2CAP_CONN_REMOTE_BUSY; 32710a708f8fSGustavo F. Padovan if (pi->conn_state & L2CAP_CONN_SREJ_SENT) 32720a708f8fSGustavo F. Padovan l2cap_send_ack(pi); 32730a708f8fSGustavo F. Padovan else 32740a708f8fSGustavo F. Padovan l2cap_ertm_send(sk); 32750a708f8fSGustavo F. Padovan } 32760a708f8fSGustavo F. Padovan } 32770a708f8fSGustavo F. Padovan 32780a708f8fSGustavo F. Padovan static inline void l2cap_data_channel_rejframe(struct sock *sk, u16 rx_control) 32790a708f8fSGustavo F. Padovan { 32800a708f8fSGustavo F. Padovan struct l2cap_pinfo *pi = l2cap_pi(sk); 32810a708f8fSGustavo F. Padovan u8 tx_seq = __get_reqseq(rx_control); 32820a708f8fSGustavo F. Padovan 32830a708f8fSGustavo F. Padovan BT_DBG("sk %p, req_seq %d ctrl 0x%4.4x", sk, tx_seq, rx_control); 32840a708f8fSGustavo F. Padovan 32850a708f8fSGustavo F. Padovan pi->conn_state &= ~L2CAP_CONN_REMOTE_BUSY; 32860a708f8fSGustavo F. Padovan 32870a708f8fSGustavo F. Padovan pi->expected_ack_seq = tx_seq; 32880a708f8fSGustavo F. Padovan l2cap_drop_acked_frames(sk); 32890a708f8fSGustavo F. Padovan 32900a708f8fSGustavo F. Padovan if (rx_control & L2CAP_CTRL_FINAL) { 32910a708f8fSGustavo F. Padovan if (pi->conn_state & L2CAP_CONN_REJ_ACT) 32920a708f8fSGustavo F. Padovan pi->conn_state &= ~L2CAP_CONN_REJ_ACT; 32930a708f8fSGustavo F. Padovan else 32940a708f8fSGustavo F. Padovan l2cap_retransmit_frames(sk); 32950a708f8fSGustavo F. Padovan } else { 32960a708f8fSGustavo F. Padovan l2cap_retransmit_frames(sk); 32970a708f8fSGustavo F. Padovan 32980a708f8fSGustavo F. Padovan if (pi->conn_state & L2CAP_CONN_WAIT_F) 32990a708f8fSGustavo F. Padovan pi->conn_state |= L2CAP_CONN_REJ_ACT; 33000a708f8fSGustavo F. Padovan } 33010a708f8fSGustavo F. Padovan } 33020a708f8fSGustavo F. Padovan static inline void l2cap_data_channel_srejframe(struct sock *sk, u16 rx_control) 33030a708f8fSGustavo F. Padovan { 33040a708f8fSGustavo F. Padovan struct l2cap_pinfo *pi = l2cap_pi(sk); 33050a708f8fSGustavo F. Padovan u8 tx_seq = __get_reqseq(rx_control); 33060a708f8fSGustavo F. Padovan 33070a708f8fSGustavo F. Padovan BT_DBG("sk %p, req_seq %d ctrl 0x%4.4x", sk, tx_seq, rx_control); 33080a708f8fSGustavo F. Padovan 33090a708f8fSGustavo F. Padovan pi->conn_state &= ~L2CAP_CONN_REMOTE_BUSY; 33100a708f8fSGustavo F. Padovan 33110a708f8fSGustavo F. Padovan if (rx_control & L2CAP_CTRL_POLL) { 33120a708f8fSGustavo F. Padovan pi->expected_ack_seq = tx_seq; 33130a708f8fSGustavo F. Padovan l2cap_drop_acked_frames(sk); 33140a708f8fSGustavo F. Padovan 33150a708f8fSGustavo F. Padovan pi->conn_state |= L2CAP_CONN_SEND_FBIT; 33160a708f8fSGustavo F. Padovan l2cap_retransmit_one_frame(sk, tx_seq); 33170a708f8fSGustavo F. Padovan 33180a708f8fSGustavo F. Padovan l2cap_ertm_send(sk); 33190a708f8fSGustavo F. Padovan 33200a708f8fSGustavo F. Padovan if (pi->conn_state & L2CAP_CONN_WAIT_F) { 33210a708f8fSGustavo F. Padovan pi->srej_save_reqseq = tx_seq; 33220a708f8fSGustavo F. Padovan pi->conn_state |= L2CAP_CONN_SREJ_ACT; 33230a708f8fSGustavo F. Padovan } 33240a708f8fSGustavo F. Padovan } else if (rx_control & L2CAP_CTRL_FINAL) { 33250a708f8fSGustavo F. Padovan if ((pi->conn_state & L2CAP_CONN_SREJ_ACT) && 33260a708f8fSGustavo F. Padovan pi->srej_save_reqseq == tx_seq) 33270a708f8fSGustavo F. Padovan pi->conn_state &= ~L2CAP_CONN_SREJ_ACT; 33280a708f8fSGustavo F. Padovan else 33290a708f8fSGustavo F. Padovan l2cap_retransmit_one_frame(sk, tx_seq); 33300a708f8fSGustavo F. Padovan } else { 33310a708f8fSGustavo F. Padovan l2cap_retransmit_one_frame(sk, tx_seq); 33320a708f8fSGustavo F. Padovan if (pi->conn_state & L2CAP_CONN_WAIT_F) { 33330a708f8fSGustavo F. Padovan pi->srej_save_reqseq = tx_seq; 33340a708f8fSGustavo F. Padovan pi->conn_state |= L2CAP_CONN_SREJ_ACT; 33350a708f8fSGustavo F. Padovan } 33360a708f8fSGustavo F. Padovan } 33370a708f8fSGustavo F. Padovan } 33380a708f8fSGustavo F. Padovan 33390a708f8fSGustavo F. Padovan static inline void l2cap_data_channel_rnrframe(struct sock *sk, u16 rx_control) 33400a708f8fSGustavo F. Padovan { 33410a708f8fSGustavo F. Padovan struct l2cap_pinfo *pi = l2cap_pi(sk); 33420a708f8fSGustavo F. Padovan u8 tx_seq = __get_reqseq(rx_control); 33430a708f8fSGustavo F. Padovan 33440a708f8fSGustavo F. Padovan BT_DBG("sk %p, req_seq %d ctrl 0x%4.4x", sk, tx_seq, rx_control); 33450a708f8fSGustavo F. Padovan 33460a708f8fSGustavo F. Padovan pi->conn_state |= L2CAP_CONN_REMOTE_BUSY; 33470a708f8fSGustavo F. Padovan pi->expected_ack_seq = tx_seq; 33480a708f8fSGustavo F. Padovan l2cap_drop_acked_frames(sk); 33490a708f8fSGustavo F. Padovan 33500a708f8fSGustavo F. Padovan if (rx_control & L2CAP_CTRL_POLL) 33510a708f8fSGustavo F. Padovan pi->conn_state |= L2CAP_CONN_SEND_FBIT; 33520a708f8fSGustavo F. Padovan 33530a708f8fSGustavo F. Padovan if (!(pi->conn_state & L2CAP_CONN_SREJ_SENT)) { 33540a708f8fSGustavo F. Padovan del_timer(&pi->retrans_timer); 33550a708f8fSGustavo F. Padovan if (rx_control & L2CAP_CTRL_POLL) 33560a708f8fSGustavo F. Padovan l2cap_send_rr_or_rnr(pi, L2CAP_CTRL_FINAL); 33570a708f8fSGustavo F. Padovan return; 33580a708f8fSGustavo F. Padovan } 33590a708f8fSGustavo F. Padovan 33600a708f8fSGustavo F. Padovan if (rx_control & L2CAP_CTRL_POLL) 33610a708f8fSGustavo F. Padovan l2cap_send_srejtail(sk); 33620a708f8fSGustavo F. Padovan else 33630a708f8fSGustavo F. Padovan l2cap_send_sframe(pi, L2CAP_SUPER_RCV_READY); 33640a708f8fSGustavo F. Padovan } 33650a708f8fSGustavo F. Padovan 33660a708f8fSGustavo F. Padovan static inline int l2cap_data_channel_sframe(struct sock *sk, u16 rx_control, struct sk_buff *skb) 33670a708f8fSGustavo F. Padovan { 33680a708f8fSGustavo F. Padovan BT_DBG("sk %p rx_control 0x%4.4x len %d", sk, rx_control, skb->len); 33690a708f8fSGustavo F. Padovan 33700a708f8fSGustavo F. Padovan if (L2CAP_CTRL_FINAL & rx_control && 33710a708f8fSGustavo F. Padovan l2cap_pi(sk)->conn_state & L2CAP_CONN_WAIT_F) { 33720a708f8fSGustavo F. Padovan del_timer(&l2cap_pi(sk)->monitor_timer); 33730a708f8fSGustavo F. Padovan if (l2cap_pi(sk)->unacked_frames > 0) 33740a708f8fSGustavo F. Padovan __mod_retrans_timer(); 33750a708f8fSGustavo F. Padovan l2cap_pi(sk)->conn_state &= ~L2CAP_CONN_WAIT_F; 33760a708f8fSGustavo F. Padovan } 33770a708f8fSGustavo F. Padovan 33780a708f8fSGustavo F. Padovan switch (rx_control & L2CAP_CTRL_SUPERVISE) { 33790a708f8fSGustavo F. Padovan case L2CAP_SUPER_RCV_READY: 33800a708f8fSGustavo F. Padovan l2cap_data_channel_rrframe(sk, rx_control); 33810a708f8fSGustavo F. Padovan break; 33820a708f8fSGustavo F. Padovan 33830a708f8fSGustavo F. Padovan case L2CAP_SUPER_REJECT: 33840a708f8fSGustavo F. Padovan l2cap_data_channel_rejframe(sk, rx_control); 33850a708f8fSGustavo F. Padovan break; 33860a708f8fSGustavo F. Padovan 33870a708f8fSGustavo F. Padovan case L2CAP_SUPER_SELECT_REJECT: 33880a708f8fSGustavo F. Padovan l2cap_data_channel_srejframe(sk, rx_control); 33890a708f8fSGustavo F. Padovan break; 33900a708f8fSGustavo F. Padovan 33910a708f8fSGustavo F. Padovan case L2CAP_SUPER_RCV_NOT_READY: 33920a708f8fSGustavo F. Padovan l2cap_data_channel_rnrframe(sk, rx_control); 33930a708f8fSGustavo F. Padovan break; 33940a708f8fSGustavo F. Padovan } 33950a708f8fSGustavo F. Padovan 33960a708f8fSGustavo F. Padovan kfree_skb(skb); 33970a708f8fSGustavo F. Padovan return 0; 33980a708f8fSGustavo F. Padovan } 33990a708f8fSGustavo F. Padovan 34000a708f8fSGustavo F. Padovan static int l2cap_ertm_data_rcv(struct sock *sk, struct sk_buff *skb) 34010a708f8fSGustavo F. Padovan { 34020a708f8fSGustavo F. Padovan struct l2cap_pinfo *pi = l2cap_pi(sk); 34030a708f8fSGustavo F. Padovan u16 control; 34040a708f8fSGustavo F. Padovan u8 req_seq; 34050a708f8fSGustavo F. Padovan int len, next_tx_seq_offset, req_seq_offset; 34060a708f8fSGustavo F. Padovan 34070a708f8fSGustavo F. Padovan control = get_unaligned_le16(skb->data); 34080a708f8fSGustavo F. Padovan skb_pull(skb, 2); 34090a708f8fSGustavo F. Padovan len = skb->len; 34100a708f8fSGustavo F. Padovan 34110a708f8fSGustavo F. Padovan /* 34120a708f8fSGustavo F. Padovan * We can just drop the corrupted I-frame here. 34130a708f8fSGustavo F. Padovan * Receiver will miss it and start proper recovery 34140a708f8fSGustavo F. Padovan * procedures and ask retransmission. 34150a708f8fSGustavo F. Padovan */ 34160a708f8fSGustavo F. Padovan if (l2cap_check_fcs(pi, skb)) 34170a708f8fSGustavo F. Padovan goto drop; 34180a708f8fSGustavo F. Padovan 34190a708f8fSGustavo F. Padovan if (__is_sar_start(control) && __is_iframe(control)) 34200a708f8fSGustavo F. Padovan len -= 2; 34210a708f8fSGustavo F. Padovan 34220a708f8fSGustavo F. Padovan if (pi->fcs == L2CAP_FCS_CRC16) 34230a708f8fSGustavo F. Padovan len -= 2; 34240a708f8fSGustavo F. Padovan 34250a708f8fSGustavo F. Padovan if (len > pi->mps) { 34260a708f8fSGustavo F. Padovan l2cap_send_disconn_req(pi->conn, sk, ECONNRESET); 34270a708f8fSGustavo F. Padovan goto drop; 34280a708f8fSGustavo F. Padovan } 34290a708f8fSGustavo F. Padovan 34300a708f8fSGustavo F. Padovan req_seq = __get_reqseq(control); 34310a708f8fSGustavo F. Padovan req_seq_offset = (req_seq - pi->expected_ack_seq) % 64; 34320a708f8fSGustavo F. Padovan if (req_seq_offset < 0) 34330a708f8fSGustavo F. Padovan req_seq_offset += 64; 34340a708f8fSGustavo F. Padovan 34350a708f8fSGustavo F. Padovan next_tx_seq_offset = 34360a708f8fSGustavo F. Padovan (pi->next_tx_seq - pi->expected_ack_seq) % 64; 34370a708f8fSGustavo F. Padovan if (next_tx_seq_offset < 0) 34380a708f8fSGustavo F. Padovan next_tx_seq_offset += 64; 34390a708f8fSGustavo F. Padovan 34400a708f8fSGustavo F. Padovan /* check for invalid req-seq */ 34410a708f8fSGustavo F. Padovan if (req_seq_offset > next_tx_seq_offset) { 34420a708f8fSGustavo F. Padovan l2cap_send_disconn_req(pi->conn, sk, ECONNRESET); 34430a708f8fSGustavo F. Padovan goto drop; 34440a708f8fSGustavo F. Padovan } 34450a708f8fSGustavo F. Padovan 34460a708f8fSGustavo F. Padovan if (__is_iframe(control)) { 34470a708f8fSGustavo F. Padovan if (len < 0) { 34480a708f8fSGustavo F. Padovan l2cap_send_disconn_req(pi->conn, sk, ECONNRESET); 34490a708f8fSGustavo F. Padovan goto drop; 34500a708f8fSGustavo F. Padovan } 34510a708f8fSGustavo F. Padovan 34520a708f8fSGustavo F. Padovan l2cap_data_channel_iframe(sk, control, skb); 34530a708f8fSGustavo F. Padovan } else { 34540a708f8fSGustavo F. Padovan if (len != 0) { 34550a708f8fSGustavo F. Padovan BT_ERR("%d", len); 34560a708f8fSGustavo F. Padovan l2cap_send_disconn_req(pi->conn, sk, ECONNRESET); 34570a708f8fSGustavo F. Padovan goto drop; 34580a708f8fSGustavo F. Padovan } 34590a708f8fSGustavo F. Padovan 34600a708f8fSGustavo F. Padovan l2cap_data_channel_sframe(sk, control, skb); 34610a708f8fSGustavo F. Padovan } 34620a708f8fSGustavo F. Padovan 34630a708f8fSGustavo F. Padovan return 0; 34640a708f8fSGustavo F. Padovan 34650a708f8fSGustavo F. Padovan drop: 34660a708f8fSGustavo F. Padovan kfree_skb(skb); 34670a708f8fSGustavo F. Padovan return 0; 34680a708f8fSGustavo F. Padovan } 34690a708f8fSGustavo F. Padovan 34700a708f8fSGustavo F. Padovan static inline int l2cap_data_channel(struct l2cap_conn *conn, u16 cid, struct sk_buff *skb) 34710a708f8fSGustavo F. Padovan { 34720a708f8fSGustavo F. Padovan struct sock *sk; 34730a708f8fSGustavo F. Padovan struct l2cap_pinfo *pi; 34740a708f8fSGustavo F. Padovan u16 control; 34750a708f8fSGustavo F. Padovan u8 tx_seq; 34760a708f8fSGustavo F. Padovan int len; 34770a708f8fSGustavo F. Padovan 34780a708f8fSGustavo F. Padovan sk = l2cap_get_chan_by_scid(&conn->chan_list, cid); 34790a708f8fSGustavo F. Padovan if (!sk) { 34800a708f8fSGustavo F. Padovan BT_DBG("unknown cid 0x%4.4x", cid); 34810a708f8fSGustavo F. Padovan goto drop; 34820a708f8fSGustavo F. Padovan } 34830a708f8fSGustavo F. Padovan 34840a708f8fSGustavo F. Padovan pi = l2cap_pi(sk); 34850a708f8fSGustavo F. Padovan 34860a708f8fSGustavo F. Padovan BT_DBG("sk %p, len %d", sk, skb->len); 34870a708f8fSGustavo F. Padovan 34880a708f8fSGustavo F. Padovan if (sk->sk_state != BT_CONNECTED) 34890a708f8fSGustavo F. Padovan goto drop; 34900a708f8fSGustavo F. Padovan 34910a708f8fSGustavo F. Padovan switch (pi->mode) { 34920a708f8fSGustavo F. Padovan case L2CAP_MODE_BASIC: 34930a708f8fSGustavo F. Padovan /* If socket recv buffers overflows we drop data here 34940a708f8fSGustavo F. Padovan * which is *bad* because L2CAP has to be reliable. 34950a708f8fSGustavo F. Padovan * But we don't have any other choice. L2CAP doesn't 34960a708f8fSGustavo F. Padovan * provide flow control mechanism. */ 34970a708f8fSGustavo F. Padovan 34980a708f8fSGustavo F. Padovan if (pi->imtu < skb->len) 34990a708f8fSGustavo F. Padovan goto drop; 35000a708f8fSGustavo F. Padovan 35010a708f8fSGustavo F. Padovan if (!sock_queue_rcv_skb(sk, skb)) 35020a708f8fSGustavo F. Padovan goto done; 35030a708f8fSGustavo F. Padovan break; 35040a708f8fSGustavo F. Padovan 35050a708f8fSGustavo F. Padovan case L2CAP_MODE_ERTM: 35060a708f8fSGustavo F. Padovan if (!sock_owned_by_user(sk)) { 35070a708f8fSGustavo F. Padovan l2cap_ertm_data_rcv(sk, skb); 35080a708f8fSGustavo F. Padovan } else { 35090a708f8fSGustavo F. Padovan if (sk_add_backlog(sk, skb)) 35100a708f8fSGustavo F. Padovan goto drop; 35110a708f8fSGustavo F. Padovan } 35120a708f8fSGustavo F. Padovan 35130a708f8fSGustavo F. Padovan goto done; 35140a708f8fSGustavo F. Padovan 35150a708f8fSGustavo F. Padovan case L2CAP_MODE_STREAMING: 35160a708f8fSGustavo F. Padovan control = get_unaligned_le16(skb->data); 35170a708f8fSGustavo F. Padovan skb_pull(skb, 2); 35180a708f8fSGustavo F. Padovan len = skb->len; 35190a708f8fSGustavo F. Padovan 35200a708f8fSGustavo F. Padovan if (l2cap_check_fcs(pi, skb)) 35210a708f8fSGustavo F. Padovan goto drop; 35220a708f8fSGustavo F. Padovan 35230a708f8fSGustavo F. Padovan if (__is_sar_start(control)) 35240a708f8fSGustavo F. Padovan len -= 2; 35250a708f8fSGustavo F. Padovan 35260a708f8fSGustavo F. Padovan if (pi->fcs == L2CAP_FCS_CRC16) 35270a708f8fSGustavo F. Padovan len -= 2; 35280a708f8fSGustavo F. Padovan 35290a708f8fSGustavo F. Padovan if (len > pi->mps || len < 0 || __is_sframe(control)) 35300a708f8fSGustavo F. Padovan goto drop; 35310a708f8fSGustavo F. Padovan 35320a708f8fSGustavo F. Padovan tx_seq = __get_txseq(control); 35330a708f8fSGustavo F. Padovan 35340a708f8fSGustavo F. Padovan if (pi->expected_tx_seq == tx_seq) 35350a708f8fSGustavo F. Padovan pi->expected_tx_seq = (pi->expected_tx_seq + 1) % 64; 35360a708f8fSGustavo F. Padovan else 35370a708f8fSGustavo F. Padovan pi->expected_tx_seq = (tx_seq + 1) % 64; 35380a708f8fSGustavo F. Padovan 35390a708f8fSGustavo F. Padovan l2cap_streaming_reassembly_sdu(sk, skb, control); 35400a708f8fSGustavo F. Padovan 35410a708f8fSGustavo F. Padovan goto done; 35420a708f8fSGustavo F. Padovan 35430a708f8fSGustavo F. Padovan default: 35440a708f8fSGustavo F. Padovan BT_DBG("sk %p: bad mode 0x%2.2x", sk, pi->mode); 35450a708f8fSGustavo F. Padovan break; 35460a708f8fSGustavo F. Padovan } 35470a708f8fSGustavo F. Padovan 35480a708f8fSGustavo F. Padovan drop: 35490a708f8fSGustavo F. Padovan kfree_skb(skb); 35500a708f8fSGustavo F. Padovan 35510a708f8fSGustavo F. Padovan done: 35520a708f8fSGustavo F. Padovan if (sk) 35530a708f8fSGustavo F. Padovan bh_unlock_sock(sk); 35540a708f8fSGustavo F. Padovan 35550a708f8fSGustavo F. Padovan return 0; 35560a708f8fSGustavo F. Padovan } 35570a708f8fSGustavo F. Padovan 35580a708f8fSGustavo F. Padovan static inline int l2cap_conless_channel(struct l2cap_conn *conn, __le16 psm, struct sk_buff *skb) 35590a708f8fSGustavo F. Padovan { 35600a708f8fSGustavo F. Padovan struct sock *sk; 35610a708f8fSGustavo F. Padovan 35620a708f8fSGustavo F. Padovan sk = l2cap_get_sock_by_psm(0, psm, conn->src); 35630a708f8fSGustavo F. Padovan if (!sk) 35640a708f8fSGustavo F. Padovan goto drop; 35650a708f8fSGustavo F. Padovan 35660a708f8fSGustavo F. Padovan bh_lock_sock(sk); 35670a708f8fSGustavo F. Padovan 35680a708f8fSGustavo F. Padovan BT_DBG("sk %p, len %d", sk, skb->len); 35690a708f8fSGustavo F. Padovan 35700a708f8fSGustavo F. Padovan if (sk->sk_state != BT_BOUND && sk->sk_state != BT_CONNECTED) 35710a708f8fSGustavo F. Padovan goto drop; 35720a708f8fSGustavo F. Padovan 35730a708f8fSGustavo F. Padovan if (l2cap_pi(sk)->imtu < skb->len) 35740a708f8fSGustavo F. Padovan goto drop; 35750a708f8fSGustavo F. Padovan 35760a708f8fSGustavo F. Padovan if (!sock_queue_rcv_skb(sk, skb)) 35770a708f8fSGustavo F. Padovan goto done; 35780a708f8fSGustavo F. Padovan 35790a708f8fSGustavo F. Padovan drop: 35800a708f8fSGustavo F. Padovan kfree_skb(skb); 35810a708f8fSGustavo F. Padovan 35820a708f8fSGustavo F. Padovan done: 35830a708f8fSGustavo F. Padovan if (sk) 35840a708f8fSGustavo F. Padovan bh_unlock_sock(sk); 35850a708f8fSGustavo F. Padovan return 0; 35860a708f8fSGustavo F. Padovan } 35870a708f8fSGustavo F. Padovan 35880a708f8fSGustavo F. Padovan static void l2cap_recv_frame(struct l2cap_conn *conn, struct sk_buff *skb) 35890a708f8fSGustavo F. Padovan { 35900a708f8fSGustavo F. Padovan struct l2cap_hdr *lh = (void *) skb->data; 35910a708f8fSGustavo F. Padovan u16 cid, len; 35920a708f8fSGustavo F. Padovan __le16 psm; 35930a708f8fSGustavo F. Padovan 35940a708f8fSGustavo F. Padovan skb_pull(skb, L2CAP_HDR_SIZE); 35950a708f8fSGustavo F. Padovan cid = __le16_to_cpu(lh->cid); 35960a708f8fSGustavo F. Padovan len = __le16_to_cpu(lh->len); 35970a708f8fSGustavo F. Padovan 35980a708f8fSGustavo F. Padovan if (len != skb->len) { 35990a708f8fSGustavo F. Padovan kfree_skb(skb); 36000a708f8fSGustavo F. Padovan return; 36010a708f8fSGustavo F. Padovan } 36020a708f8fSGustavo F. Padovan 36030a708f8fSGustavo F. Padovan BT_DBG("len %d, cid 0x%4.4x", len, cid); 36040a708f8fSGustavo F. Padovan 36050a708f8fSGustavo F. Padovan switch (cid) { 3606*3300d9a9SClaudio Takahasi case L2CAP_CID_LE_SIGNALING: 36070a708f8fSGustavo F. Padovan case L2CAP_CID_SIGNALING: 36080a708f8fSGustavo F. Padovan l2cap_sig_channel(conn, skb); 36090a708f8fSGustavo F. Padovan break; 36100a708f8fSGustavo F. Padovan 36110a708f8fSGustavo F. Padovan case L2CAP_CID_CONN_LESS: 36120a708f8fSGustavo F. Padovan psm = get_unaligned_le16(skb->data); 36130a708f8fSGustavo F. Padovan skb_pull(skb, 2); 36140a708f8fSGustavo F. Padovan l2cap_conless_channel(conn, psm, skb); 36150a708f8fSGustavo F. Padovan break; 36160a708f8fSGustavo F. Padovan 36170a708f8fSGustavo F. Padovan default: 36180a708f8fSGustavo F. Padovan l2cap_data_channel(conn, cid, skb); 36190a708f8fSGustavo F. Padovan break; 36200a708f8fSGustavo F. Padovan } 36210a708f8fSGustavo F. Padovan } 36220a708f8fSGustavo F. Padovan 36230a708f8fSGustavo F. Padovan /* ---- L2CAP interface with lower layer (HCI) ---- */ 36240a708f8fSGustavo F. Padovan 36250a708f8fSGustavo F. Padovan static int l2cap_connect_ind(struct hci_dev *hdev, bdaddr_t *bdaddr, u8 type) 36260a708f8fSGustavo F. Padovan { 36270a708f8fSGustavo F. Padovan int exact = 0, lm1 = 0, lm2 = 0; 36280a708f8fSGustavo F. Padovan register struct sock *sk; 36290a708f8fSGustavo F. Padovan struct hlist_node *node; 36300a708f8fSGustavo F. Padovan 36310a708f8fSGustavo F. Padovan if (type != ACL_LINK) 36320a708f8fSGustavo F. Padovan return -EINVAL; 36330a708f8fSGustavo F. Padovan 36340a708f8fSGustavo F. Padovan BT_DBG("hdev %s, bdaddr %s", hdev->name, batostr(bdaddr)); 36350a708f8fSGustavo F. Padovan 36360a708f8fSGustavo F. Padovan /* Find listening sockets and check their link_mode */ 36370a708f8fSGustavo F. Padovan read_lock(&l2cap_sk_list.lock); 36380a708f8fSGustavo F. Padovan sk_for_each(sk, node, &l2cap_sk_list.head) { 36390a708f8fSGustavo F. Padovan if (sk->sk_state != BT_LISTEN) 36400a708f8fSGustavo F. Padovan continue; 36410a708f8fSGustavo F. Padovan 36420a708f8fSGustavo F. Padovan if (!bacmp(&bt_sk(sk)->src, &hdev->bdaddr)) { 36430a708f8fSGustavo F. Padovan lm1 |= HCI_LM_ACCEPT; 36440a708f8fSGustavo F. Padovan if (l2cap_pi(sk)->role_switch) 36450a708f8fSGustavo F. Padovan lm1 |= HCI_LM_MASTER; 36460a708f8fSGustavo F. Padovan exact++; 36470a708f8fSGustavo F. Padovan } else if (!bacmp(&bt_sk(sk)->src, BDADDR_ANY)) { 36480a708f8fSGustavo F. Padovan lm2 |= HCI_LM_ACCEPT; 36490a708f8fSGustavo F. Padovan if (l2cap_pi(sk)->role_switch) 36500a708f8fSGustavo F. Padovan lm2 |= HCI_LM_MASTER; 36510a708f8fSGustavo F. Padovan } 36520a708f8fSGustavo F. Padovan } 36530a708f8fSGustavo F. Padovan read_unlock(&l2cap_sk_list.lock); 36540a708f8fSGustavo F. Padovan 36550a708f8fSGustavo F. Padovan return exact ? lm1 : lm2; 36560a708f8fSGustavo F. Padovan } 36570a708f8fSGustavo F. Padovan 36580a708f8fSGustavo F. Padovan static int l2cap_connect_cfm(struct hci_conn *hcon, u8 status) 36590a708f8fSGustavo F. Padovan { 36600a708f8fSGustavo F. Padovan struct l2cap_conn *conn; 36610a708f8fSGustavo F. Padovan 36620a708f8fSGustavo F. Padovan BT_DBG("hcon %p bdaddr %s status %d", hcon, batostr(&hcon->dst), status); 36630a708f8fSGustavo F. Padovan 3664acd7d370SVille Tervo if (!(hcon->type == ACL_LINK || hcon->type == LE_LINK)) 36650a708f8fSGustavo F. Padovan return -EINVAL; 36660a708f8fSGustavo F. Padovan 36670a708f8fSGustavo F. Padovan if (!status) { 36680a708f8fSGustavo F. Padovan conn = l2cap_conn_add(hcon, status); 36690a708f8fSGustavo F. Padovan if (conn) 36700a708f8fSGustavo F. Padovan l2cap_conn_ready(conn); 36710a708f8fSGustavo F. Padovan } else 36720a708f8fSGustavo F. Padovan l2cap_conn_del(hcon, bt_err(status)); 36730a708f8fSGustavo F. Padovan 36740a708f8fSGustavo F. Padovan return 0; 36750a708f8fSGustavo F. Padovan } 36760a708f8fSGustavo F. Padovan 36770a708f8fSGustavo F. Padovan static int l2cap_disconn_ind(struct hci_conn *hcon) 36780a708f8fSGustavo F. Padovan { 36790a708f8fSGustavo F. Padovan struct l2cap_conn *conn = hcon->l2cap_data; 36800a708f8fSGustavo F. Padovan 36810a708f8fSGustavo F. Padovan BT_DBG("hcon %p", hcon); 36820a708f8fSGustavo F. Padovan 36830a708f8fSGustavo F. Padovan if (hcon->type != ACL_LINK || !conn) 36840a708f8fSGustavo F. Padovan return 0x13; 36850a708f8fSGustavo F. Padovan 36860a708f8fSGustavo F. Padovan return conn->disc_reason; 36870a708f8fSGustavo F. Padovan } 36880a708f8fSGustavo F. Padovan 36890a708f8fSGustavo F. Padovan static int l2cap_disconn_cfm(struct hci_conn *hcon, u8 reason) 36900a708f8fSGustavo F. Padovan { 36910a708f8fSGustavo F. Padovan BT_DBG("hcon %p reason %d", hcon, reason); 36920a708f8fSGustavo F. Padovan 3693acd7d370SVille Tervo if (!(hcon->type == ACL_LINK || hcon->type == LE_LINK)) 36940a708f8fSGustavo F. Padovan return -EINVAL; 36950a708f8fSGustavo F. Padovan 36960a708f8fSGustavo F. Padovan l2cap_conn_del(hcon, bt_err(reason)); 36970a708f8fSGustavo F. Padovan 36980a708f8fSGustavo F. Padovan return 0; 36990a708f8fSGustavo F. Padovan } 37000a708f8fSGustavo F. Padovan 37010a708f8fSGustavo F. Padovan static inline void l2cap_check_encryption(struct sock *sk, u8 encrypt) 37020a708f8fSGustavo F. Padovan { 37030a708f8fSGustavo F. Padovan if (sk->sk_type != SOCK_SEQPACKET && sk->sk_type != SOCK_STREAM) 37040a708f8fSGustavo F. Padovan return; 37050a708f8fSGustavo F. Padovan 37060a708f8fSGustavo F. Padovan if (encrypt == 0x00) { 37070a708f8fSGustavo F. Padovan if (l2cap_pi(sk)->sec_level == BT_SECURITY_MEDIUM) { 37080a708f8fSGustavo F. Padovan l2cap_sock_clear_timer(sk); 37090a708f8fSGustavo F. Padovan l2cap_sock_set_timer(sk, HZ * 5); 37100a708f8fSGustavo F. Padovan } else if (l2cap_pi(sk)->sec_level == BT_SECURITY_HIGH) 37110a708f8fSGustavo F. Padovan __l2cap_sock_close(sk, ECONNREFUSED); 37120a708f8fSGustavo F. Padovan } else { 37130a708f8fSGustavo F. Padovan if (l2cap_pi(sk)->sec_level == BT_SECURITY_MEDIUM) 37140a708f8fSGustavo F. Padovan l2cap_sock_clear_timer(sk); 37150a708f8fSGustavo F. Padovan } 37160a708f8fSGustavo F. Padovan } 37170a708f8fSGustavo F. Padovan 37180a708f8fSGustavo F. Padovan static int l2cap_security_cfm(struct hci_conn *hcon, u8 status, u8 encrypt) 37190a708f8fSGustavo F. Padovan { 37200a708f8fSGustavo F. Padovan struct l2cap_chan_list *l; 37210a708f8fSGustavo F. Padovan struct l2cap_conn *conn = hcon->l2cap_data; 37220a708f8fSGustavo F. Padovan struct sock *sk; 37230a708f8fSGustavo F. Padovan 37240a708f8fSGustavo F. Padovan if (!conn) 37250a708f8fSGustavo F. Padovan return 0; 37260a708f8fSGustavo F. Padovan 37270a708f8fSGustavo F. Padovan l = &conn->chan_list; 37280a708f8fSGustavo F. Padovan 37290a708f8fSGustavo F. Padovan BT_DBG("conn %p", conn); 37300a708f8fSGustavo F. Padovan 37310a708f8fSGustavo F. Padovan read_lock(&l->lock); 37320a708f8fSGustavo F. Padovan 37330a708f8fSGustavo F. Padovan for (sk = l->head; sk; sk = l2cap_pi(sk)->next_c) { 37340a708f8fSGustavo F. Padovan bh_lock_sock(sk); 37350a708f8fSGustavo F. Padovan 37360a708f8fSGustavo F. Padovan if (l2cap_pi(sk)->conf_state & L2CAP_CONF_CONNECT_PEND) { 37370a708f8fSGustavo F. Padovan bh_unlock_sock(sk); 37380a708f8fSGustavo F. Padovan continue; 37390a708f8fSGustavo F. Padovan } 37400a708f8fSGustavo F. Padovan 37410a708f8fSGustavo F. Padovan if (!status && (sk->sk_state == BT_CONNECTED || 37420a708f8fSGustavo F. Padovan sk->sk_state == BT_CONFIG)) { 37430a708f8fSGustavo F. Padovan l2cap_check_encryption(sk, encrypt); 37440a708f8fSGustavo F. Padovan bh_unlock_sock(sk); 37450a708f8fSGustavo F. Padovan continue; 37460a708f8fSGustavo F. Padovan } 37470a708f8fSGustavo F. Padovan 37480a708f8fSGustavo F. Padovan if (sk->sk_state == BT_CONNECT) { 37490a708f8fSGustavo F. Padovan if (!status) { 37500a708f8fSGustavo F. Padovan struct l2cap_conn_req req; 37510a708f8fSGustavo F. Padovan req.scid = cpu_to_le16(l2cap_pi(sk)->scid); 37520a708f8fSGustavo F. Padovan req.psm = l2cap_pi(sk)->psm; 37530a708f8fSGustavo F. Padovan 37540a708f8fSGustavo F. Padovan l2cap_pi(sk)->ident = l2cap_get_ident(conn); 37550a708f8fSGustavo F. Padovan l2cap_pi(sk)->conf_state |= L2CAP_CONF_CONNECT_PEND; 37560a708f8fSGustavo F. Padovan 37570a708f8fSGustavo F. Padovan l2cap_send_cmd(conn, l2cap_pi(sk)->ident, 37580a708f8fSGustavo F. Padovan L2CAP_CONN_REQ, sizeof(req), &req); 37590a708f8fSGustavo F. Padovan } else { 37600a708f8fSGustavo F. Padovan l2cap_sock_clear_timer(sk); 37610a708f8fSGustavo F. Padovan l2cap_sock_set_timer(sk, HZ / 10); 37620a708f8fSGustavo F. Padovan } 37630a708f8fSGustavo F. Padovan } else if (sk->sk_state == BT_CONNECT2) { 37640a708f8fSGustavo F. Padovan struct l2cap_conn_rsp rsp; 37650a708f8fSGustavo F. Padovan __u16 result; 37660a708f8fSGustavo F. Padovan 37670a708f8fSGustavo F. Padovan if (!status) { 37680a708f8fSGustavo F. Padovan sk->sk_state = BT_CONFIG; 37690a708f8fSGustavo F. Padovan result = L2CAP_CR_SUCCESS; 37700a708f8fSGustavo F. Padovan } else { 37710a708f8fSGustavo F. Padovan sk->sk_state = BT_DISCONN; 37720a708f8fSGustavo F. Padovan l2cap_sock_set_timer(sk, HZ / 10); 37730a708f8fSGustavo F. Padovan result = L2CAP_CR_SEC_BLOCK; 37740a708f8fSGustavo F. Padovan } 37750a708f8fSGustavo F. Padovan 37760a708f8fSGustavo F. Padovan rsp.scid = cpu_to_le16(l2cap_pi(sk)->dcid); 37770a708f8fSGustavo F. Padovan rsp.dcid = cpu_to_le16(l2cap_pi(sk)->scid); 37780a708f8fSGustavo F. Padovan rsp.result = cpu_to_le16(result); 37790a708f8fSGustavo F. Padovan rsp.status = cpu_to_le16(L2CAP_CS_NO_INFO); 37800a708f8fSGustavo F. Padovan l2cap_send_cmd(conn, l2cap_pi(sk)->ident, 37810a708f8fSGustavo F. Padovan L2CAP_CONN_RSP, sizeof(rsp), &rsp); 37820a708f8fSGustavo F. Padovan } 37830a708f8fSGustavo F. Padovan 37840a708f8fSGustavo F. Padovan bh_unlock_sock(sk); 37850a708f8fSGustavo F. Padovan } 37860a708f8fSGustavo F. Padovan 37870a708f8fSGustavo F. Padovan read_unlock(&l->lock); 37880a708f8fSGustavo F. Padovan 37890a708f8fSGustavo F. Padovan return 0; 37900a708f8fSGustavo F. Padovan } 37910a708f8fSGustavo F. Padovan 37920a708f8fSGustavo F. Padovan static int l2cap_recv_acldata(struct hci_conn *hcon, struct sk_buff *skb, u16 flags) 37930a708f8fSGustavo F. Padovan { 37940a708f8fSGustavo F. Padovan struct l2cap_conn *conn = hcon->l2cap_data; 37950a708f8fSGustavo F. Padovan 37960a708f8fSGustavo F. Padovan if (!conn) 37970a708f8fSGustavo F. Padovan conn = l2cap_conn_add(hcon, 0); 37980a708f8fSGustavo F. Padovan 37990a708f8fSGustavo F. Padovan if (!conn) 38000a708f8fSGustavo F. Padovan goto drop; 38010a708f8fSGustavo F. Padovan 38020a708f8fSGustavo F. Padovan BT_DBG("conn %p len %d flags 0x%x", conn, skb->len, flags); 38030a708f8fSGustavo F. Padovan 38040a708f8fSGustavo F. Padovan if (!(flags & ACL_CONT)) { 38050a708f8fSGustavo F. Padovan struct l2cap_hdr *hdr; 38060a708f8fSGustavo F. Padovan struct sock *sk; 38070a708f8fSGustavo F. Padovan u16 cid; 38080a708f8fSGustavo F. Padovan int len; 38090a708f8fSGustavo F. Padovan 38100a708f8fSGustavo F. Padovan if (conn->rx_len) { 38110a708f8fSGustavo F. Padovan BT_ERR("Unexpected start frame (len %d)", skb->len); 38120a708f8fSGustavo F. Padovan kfree_skb(conn->rx_skb); 38130a708f8fSGustavo F. Padovan conn->rx_skb = NULL; 38140a708f8fSGustavo F. Padovan conn->rx_len = 0; 38150a708f8fSGustavo F. Padovan l2cap_conn_unreliable(conn, ECOMM); 38160a708f8fSGustavo F. Padovan } 38170a708f8fSGustavo F. Padovan 38180a708f8fSGustavo F. Padovan /* Start fragment always begin with Basic L2CAP header */ 38190a708f8fSGustavo F. Padovan if (skb->len < L2CAP_HDR_SIZE) { 38200a708f8fSGustavo F. Padovan BT_ERR("Frame is too short (len %d)", skb->len); 38210a708f8fSGustavo F. Padovan l2cap_conn_unreliable(conn, ECOMM); 38220a708f8fSGustavo F. Padovan goto drop; 38230a708f8fSGustavo F. Padovan } 38240a708f8fSGustavo F. Padovan 38250a708f8fSGustavo F. Padovan hdr = (struct l2cap_hdr *) skb->data; 38260a708f8fSGustavo F. Padovan len = __le16_to_cpu(hdr->len) + L2CAP_HDR_SIZE; 38270a708f8fSGustavo F. Padovan cid = __le16_to_cpu(hdr->cid); 38280a708f8fSGustavo F. Padovan 38290a708f8fSGustavo F. Padovan if (len == skb->len) { 38300a708f8fSGustavo F. Padovan /* Complete frame received */ 38310a708f8fSGustavo F. Padovan l2cap_recv_frame(conn, skb); 38320a708f8fSGustavo F. Padovan return 0; 38330a708f8fSGustavo F. Padovan } 38340a708f8fSGustavo F. Padovan 38350a708f8fSGustavo F. Padovan BT_DBG("Start: total len %d, frag len %d", len, skb->len); 38360a708f8fSGustavo F. Padovan 38370a708f8fSGustavo F. Padovan if (skb->len > len) { 38380a708f8fSGustavo F. Padovan BT_ERR("Frame is too long (len %d, expected len %d)", 38390a708f8fSGustavo F. Padovan skb->len, len); 38400a708f8fSGustavo F. Padovan l2cap_conn_unreliable(conn, ECOMM); 38410a708f8fSGustavo F. Padovan goto drop; 38420a708f8fSGustavo F. Padovan } 38430a708f8fSGustavo F. Padovan 38440a708f8fSGustavo F. Padovan sk = l2cap_get_chan_by_scid(&conn->chan_list, cid); 38450a708f8fSGustavo F. Padovan 38460a708f8fSGustavo F. Padovan if (sk && l2cap_pi(sk)->imtu < len - L2CAP_HDR_SIZE) { 38470a708f8fSGustavo F. Padovan BT_ERR("Frame exceeding recv MTU (len %d, MTU %d)", 38480a708f8fSGustavo F. Padovan len, l2cap_pi(sk)->imtu); 38490a708f8fSGustavo F. Padovan bh_unlock_sock(sk); 38500a708f8fSGustavo F. Padovan l2cap_conn_unreliable(conn, ECOMM); 38510a708f8fSGustavo F. Padovan goto drop; 38520a708f8fSGustavo F. Padovan } 38530a708f8fSGustavo F. Padovan 38540a708f8fSGustavo F. Padovan if (sk) 38550a708f8fSGustavo F. Padovan bh_unlock_sock(sk); 38560a708f8fSGustavo F. Padovan 38570a708f8fSGustavo F. Padovan /* Allocate skb for the complete frame (with header) */ 38580a708f8fSGustavo F. Padovan conn->rx_skb = bt_skb_alloc(len, GFP_ATOMIC); 38590a708f8fSGustavo F. Padovan if (!conn->rx_skb) 38600a708f8fSGustavo F. Padovan goto drop; 38610a708f8fSGustavo F. Padovan 38620a708f8fSGustavo F. Padovan skb_copy_from_linear_data(skb, skb_put(conn->rx_skb, skb->len), 38630a708f8fSGustavo F. Padovan skb->len); 38640a708f8fSGustavo F. Padovan conn->rx_len = len - skb->len; 38650a708f8fSGustavo F. Padovan } else { 38660a708f8fSGustavo F. Padovan BT_DBG("Cont: frag len %d (expecting %d)", skb->len, conn->rx_len); 38670a708f8fSGustavo F. Padovan 38680a708f8fSGustavo F. Padovan if (!conn->rx_len) { 38690a708f8fSGustavo F. Padovan BT_ERR("Unexpected continuation frame (len %d)", skb->len); 38700a708f8fSGustavo F. Padovan l2cap_conn_unreliable(conn, ECOMM); 38710a708f8fSGustavo F. Padovan goto drop; 38720a708f8fSGustavo F. Padovan } 38730a708f8fSGustavo F. Padovan 38740a708f8fSGustavo F. Padovan if (skb->len > conn->rx_len) { 38750a708f8fSGustavo F. Padovan BT_ERR("Fragment is too long (len %d, expected %d)", 38760a708f8fSGustavo F. Padovan skb->len, conn->rx_len); 38770a708f8fSGustavo F. Padovan kfree_skb(conn->rx_skb); 38780a708f8fSGustavo F. Padovan conn->rx_skb = NULL; 38790a708f8fSGustavo F. Padovan conn->rx_len = 0; 38800a708f8fSGustavo F. Padovan l2cap_conn_unreliable(conn, ECOMM); 38810a708f8fSGustavo F. Padovan goto drop; 38820a708f8fSGustavo F. Padovan } 38830a708f8fSGustavo F. Padovan 38840a708f8fSGustavo F. Padovan skb_copy_from_linear_data(skb, skb_put(conn->rx_skb, skb->len), 38850a708f8fSGustavo F. Padovan skb->len); 38860a708f8fSGustavo F. Padovan conn->rx_len -= skb->len; 38870a708f8fSGustavo F. Padovan 38880a708f8fSGustavo F. Padovan if (!conn->rx_len) { 38890a708f8fSGustavo F. Padovan /* Complete frame received */ 38900a708f8fSGustavo F. Padovan l2cap_recv_frame(conn, conn->rx_skb); 38910a708f8fSGustavo F. Padovan conn->rx_skb = NULL; 38920a708f8fSGustavo F. Padovan } 38930a708f8fSGustavo F. Padovan } 38940a708f8fSGustavo F. Padovan 38950a708f8fSGustavo F. Padovan drop: 38960a708f8fSGustavo F. Padovan kfree_skb(skb); 38970a708f8fSGustavo F. Padovan return 0; 38980a708f8fSGustavo F. Padovan } 38990a708f8fSGustavo F. Padovan 39000a708f8fSGustavo F. Padovan static int l2cap_debugfs_show(struct seq_file *f, void *p) 39010a708f8fSGustavo F. Padovan { 39020a708f8fSGustavo F. Padovan struct sock *sk; 39030a708f8fSGustavo F. Padovan struct hlist_node *node; 39040a708f8fSGustavo F. Padovan 39050a708f8fSGustavo F. Padovan read_lock_bh(&l2cap_sk_list.lock); 39060a708f8fSGustavo F. Padovan 39070a708f8fSGustavo F. Padovan sk_for_each(sk, node, &l2cap_sk_list.head) { 39080a708f8fSGustavo F. Padovan struct l2cap_pinfo *pi = l2cap_pi(sk); 39090a708f8fSGustavo F. Padovan 3910903d343eSGustavo F. Padovan seq_printf(f, "%s %s %d %d 0x%4.4x 0x%4.4x %d %d %d %d\n", 39110a708f8fSGustavo F. Padovan batostr(&bt_sk(sk)->src), 39120a708f8fSGustavo F. Padovan batostr(&bt_sk(sk)->dst), 39130a708f8fSGustavo F. Padovan sk->sk_state, __le16_to_cpu(pi->psm), 39140a708f8fSGustavo F. Padovan pi->scid, pi->dcid, 3915903d343eSGustavo F. Padovan pi->imtu, pi->omtu, pi->sec_level, 3916903d343eSGustavo F. Padovan pi->mode); 39170a708f8fSGustavo F. Padovan } 39180a708f8fSGustavo F. Padovan 39190a708f8fSGustavo F. Padovan read_unlock_bh(&l2cap_sk_list.lock); 39200a708f8fSGustavo F. Padovan 39210a708f8fSGustavo F. Padovan return 0; 39220a708f8fSGustavo F. Padovan } 39230a708f8fSGustavo F. Padovan 39240a708f8fSGustavo F. Padovan static int l2cap_debugfs_open(struct inode *inode, struct file *file) 39250a708f8fSGustavo F. Padovan { 39260a708f8fSGustavo F. Padovan return single_open(file, l2cap_debugfs_show, inode->i_private); 39270a708f8fSGustavo F. Padovan } 39280a708f8fSGustavo F. Padovan 39290a708f8fSGustavo F. Padovan static const struct file_operations l2cap_debugfs_fops = { 39300a708f8fSGustavo F. Padovan .open = l2cap_debugfs_open, 39310a708f8fSGustavo F. Padovan .read = seq_read, 39320a708f8fSGustavo F. Padovan .llseek = seq_lseek, 39330a708f8fSGustavo F. Padovan .release = single_release, 39340a708f8fSGustavo F. Padovan }; 39350a708f8fSGustavo F. Padovan 39360a708f8fSGustavo F. Padovan static struct dentry *l2cap_debugfs; 39370a708f8fSGustavo F. Padovan 39380a708f8fSGustavo F. Padovan static struct hci_proto l2cap_hci_proto = { 39390a708f8fSGustavo F. Padovan .name = "L2CAP", 39400a708f8fSGustavo F. Padovan .id = HCI_PROTO_L2CAP, 39410a708f8fSGustavo F. Padovan .connect_ind = l2cap_connect_ind, 39420a708f8fSGustavo F. Padovan .connect_cfm = l2cap_connect_cfm, 39430a708f8fSGustavo F. Padovan .disconn_ind = l2cap_disconn_ind, 39440a708f8fSGustavo F. Padovan .disconn_cfm = l2cap_disconn_cfm, 39450a708f8fSGustavo F. Padovan .security_cfm = l2cap_security_cfm, 39460a708f8fSGustavo F. Padovan .recv_acldata = l2cap_recv_acldata 39470a708f8fSGustavo F. Padovan }; 39480a708f8fSGustavo F. Padovan 394964274518SGustavo F. Padovan int __init l2cap_init(void) 39500a708f8fSGustavo F. Padovan { 39510a708f8fSGustavo F. Padovan int err; 39520a708f8fSGustavo F. Padovan 3953bb58f747SGustavo F. Padovan err = l2cap_init_sockets(); 39540a708f8fSGustavo F. Padovan if (err < 0) 39550a708f8fSGustavo F. Padovan return err; 39560a708f8fSGustavo F. Padovan 39570a708f8fSGustavo F. Padovan _busy_wq = create_singlethread_workqueue("l2cap"); 39580a708f8fSGustavo F. Padovan if (!_busy_wq) { 3959bb58f747SGustavo F. Padovan err = -ENOMEM; 39600a708f8fSGustavo F. Padovan goto error; 39610a708f8fSGustavo F. Padovan } 39620a708f8fSGustavo F. Padovan 39630a708f8fSGustavo F. Padovan err = hci_register_proto(&l2cap_hci_proto); 39640a708f8fSGustavo F. Padovan if (err < 0) { 39650a708f8fSGustavo F. Padovan BT_ERR("L2CAP protocol registration failed"); 39660a708f8fSGustavo F. Padovan bt_sock_unregister(BTPROTO_L2CAP); 39670a708f8fSGustavo F. Padovan goto error; 39680a708f8fSGustavo F. Padovan } 39690a708f8fSGustavo F. Padovan 39700a708f8fSGustavo F. Padovan if (bt_debugfs) { 39710a708f8fSGustavo F. Padovan l2cap_debugfs = debugfs_create_file("l2cap", 0444, 39720a708f8fSGustavo F. Padovan bt_debugfs, NULL, &l2cap_debugfs_fops); 39730a708f8fSGustavo F. Padovan if (!l2cap_debugfs) 39740a708f8fSGustavo F. Padovan BT_ERR("Failed to create L2CAP debug file"); 39750a708f8fSGustavo F. Padovan } 39760a708f8fSGustavo F. Padovan 39770a708f8fSGustavo F. Padovan BT_INFO("L2CAP socket layer initialized"); 39780a708f8fSGustavo F. Padovan 39790a708f8fSGustavo F. Padovan return 0; 39800a708f8fSGustavo F. Padovan 39810a708f8fSGustavo F. Padovan error: 39820a708f8fSGustavo F. Padovan destroy_workqueue(_busy_wq); 3983bb58f747SGustavo F. Padovan l2cap_cleanup_sockets(); 39840a708f8fSGustavo F. Padovan return err; 39850a708f8fSGustavo F. Padovan } 39860a708f8fSGustavo F. Padovan 398764274518SGustavo F. Padovan void l2cap_exit(void) 39880a708f8fSGustavo F. Padovan { 39890a708f8fSGustavo F. Padovan debugfs_remove(l2cap_debugfs); 39900a708f8fSGustavo F. Padovan 39910a708f8fSGustavo F. Padovan flush_workqueue(_busy_wq); 39920a708f8fSGustavo F. Padovan destroy_workqueue(_busy_wq); 39930a708f8fSGustavo F. Padovan 39940a708f8fSGustavo F. Padovan if (hci_unregister_proto(&l2cap_hci_proto) < 0) 39950a708f8fSGustavo F. Padovan BT_ERR("L2CAP protocol unregistration failed"); 39960a708f8fSGustavo F. Padovan 3997bb58f747SGustavo F. Padovan l2cap_cleanup_sockets(); 39980a708f8fSGustavo F. Padovan } 39990a708f8fSGustavo F. Padovan 40000a708f8fSGustavo F. Padovan module_param(disable_ertm, bool, 0644); 40010a708f8fSGustavo F. Padovan MODULE_PARM_DESC(disable_ertm, "Disable enhanced retransmission mode"); 4002